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

权利要求书

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

S10，设置基站位置；

S20，定位系统构建；

S30，测距：测量各基站与标签之间的距离；

S40，筑构卡尔曼滤波，修正测量数据；

S50，定位解算，求出标签坐标；

S60，测试比较。

2.根据权利要求1所述的方法，其特征在于，所述S10设置基站位置，包括确定室内场地大小为a×b，并在其上设置四个UWB定位基站，分别放在(a1,b1),(a2,b2),(a3,b3),(a4,b4)四个位置，设置UWB定位基站高度为h，其中基站分为普通基站和通信基站，普通基站包括：第二基站(a2,b2,h)、第三基站(a3,b3,h)和第四基站(a4,b4,h)，通信基站包括第一基站(a1,b1,h)，在移动机器人顶端安装标签，通讯基站通过串口连接上位机。

3.根据权利要求2所述的方法，其特征在于，所述S20定位系统构建包括标签和基站均包括单片机和DWM1000通信模块，被配置为标签或者基站；基站被设置为主动端，标签被设置为被动端，第一基站作为核心，所有行为模式均集中于第一基站。

4.根据权利要求3所述的方法，其特征在于，所述S30测距，包括记录每一次消息的发送和接受信息的时间戳，标记不同的延时和往返时间，由此计算每个基站与标签间的信息飞行时间，从而计算距离。

5.根据权利要求3所述的方法，其特征在于，所述S30测距，包括以下步骤：

S31，初始化移动标签：通过移动标签向所有基站发出请求POLL请求帧，第一基站收到请求帧后启动计时，经过延时t_replyB1后向标签发送REPLY应答帧；标签收到应答帧后启动计时，并将发送和接收到信号的时间点写入FINAL终止帧，经延时t_replyA1后发送给各基站；第一基站收终止帧后表示第一基站与标签间的测距结束；

S32，根据设定的不同的应答时间，第一基站命令第二基站、第三基站和第四基站依次向标签发送应答帧，后续的步骤与第一基站相同，并将最后的测距信息通过UWB传入通信基站，即第一基站；

S33，第一基站将其信息通过串口传输给上位机。

6.根据权利要求5所述的方法，其特征在于，所述S40筑构卡尔曼滤波，修正测量数据，包括以下步骤：

S41，基于MATLAB筑构卡尔曼滤波器，对各项参数进行初始化，设置卡尔曼滤波器追踪点数、系统状态初值、状态转移矩阵初值、采样频率和迭代次数，输入各基站与标签之间的距离di(t)，(i＝1，2，3，4)；

S42，利用状态向量方程对距离信号进行卡尔曼滤波，估计出NLOS误差值Ni(t)；

S43，然后从最初的测距值di(t)中除去非视距误差，得到精确地距离值Di(t)。

7.根据权利要求6所述的方法，其特征在于，所述S50定位解算，求出标签坐标，包括将经过卡尔曼滤波后的数据Di(t)作为最小二乘法的输入，通过最小化误差的平方和，搜寻数据的最优函数匹配，最终求得标签坐标(xi,yi,zi)。

8.根据权利要求7所述的方法，其特征在于，所述S60测试比较，包括通过移动机器人的移动线路与计算得出的实时坐标路线比较，误差是否超出预设最大值，若超出则修改优化ADS-TWR的应答时间，减少标签测距的时间，继续实施S30-S60，直至误差处于预设范围内。