[发明专利]一种基于UWB的室内机器人定位方法

权利要求书

1.一种基于UWB的室内机器人定位方法，其特征在于，包括如下步骤：

步骤1：构建室内定位系统，布置至少四个基站和一个依附于机器人上的移动标签，机器人以移动标签作为其自身的坐标点；

步骤2：将所有基站中的任一个基站作为时钟基准基站，当基站获取到机器人发送的第一个定位信号时，所有基站从零开始累加计时，记为基站工作时长，当基站接收到信号时，将当前基站工作时长作为该信号的本地到达时间；

步骤3：时钟基准基站向其他基站发射同步信号，其他基站基于同步信号本地达到时间、同步次数利用插值同步算法建立时钟模型，具体为：

其中，n为基站序号；k为同步次数；Δt_n,k为第n号基站，第k次同步时同步信号本地到达时间的总时钟偏移量；t_sn,k为第n号基站，第k次同步时同步信号本地到达时间；ε_n,k为同步信号发射k次的总周期，τ_n为第n号基站的通信传播延迟；ρ_sn,k为第n号基站，第k次同步时时钟偏移率；f_s为同步信号间隔频率；Δt_n,k-1为第n号基站，第k-1次同步时同步信号本地到达时间的总时钟偏移量；

机器人向所有基站发射定位信号，除时钟基准基站以外的基站基于定位信号本地到达时间、定位次数、时钟模型外推定位信号的时钟偏移，具体公式为：

ρ_n,i＝Δt_n,k+ρ_sn,k(t_n,i-t_sn,k)

其中，i为定位次数；ρ_n,i为第n号基站，第i次定位时定位信号本地到达时间的外推定位信号的时钟偏移；t_n,i为第n号基站，第i次定位时定位信号本地到达时间的总时钟偏移量；

除时钟基准基站以外的基站通过将定位信号本地到达时间减去定位信号的时钟偏移，对定位信号本地到达时间进行时钟偏移修正；

步骤4：基站将所述步骤3中修正后的定位信号本地到达时间发送给机器人，机器人逐一将除时间基准基站之外的基站发送的修正后的定位信号本地到达时间减去时间基准基站发送的定位信号本地到达时间，获得数个TDOA值；

步骤5：建立机器人的运动模型，基于数个TDOA值获取UKF滤波算法计算移动标签坐标所需的坐标解的距离差，UKF滤波算法根据坐标解的距离差获取移动标签坐标，其中，距离差的具体计算公式如下：

其中，Δd_n1,i为第n号基站和时钟基准基站在第i次定位时基站测量距离与时钟基准基站测量距离的差值；c为UWB信号传输速度；Δt_i为第i次定位的TDOA值；为第n号基站修正后的定位信号本地到达时间；t_1,i为时钟基准基站的定位信号本地到达时间；

步骤6：累加同步次数、定位次数，时钟基准基站以同步信号间隔周期为间隔、机器人以定位信号间隔周期为间隔重复执行步骤3～步骤5，实时获取移动标签坐标直至停止定位。