一种基于UWB和IMU融合的多机器人协同定位系统

一种基于uwb和imu融合的多机器人协同定位系统
技术领域
1.本发明涉及机器人定位技术领域，更具体地，涉及一种基于uwb和imu融合的多机器人协同定位系统。


背景技术：

2.随着传感器与机器人技术的发展，人们对室内自主移动机器人定位的方案需求越来越高。近年来，在室内定位中应用比较广泛的传感器有激光雷达、摄像头、imu(inertial measurement unit，惯性测量单元)、编码器、uwb(ultra wide band，超宽带技术)、超声波等。uwb集无线通信与实时感知定位于一体，与其他室内定位技术相比，其具有传输速率高、带宽极宽、功耗低、辐射小、抗干扰能力强、定位精度高等优点，而且其具有较高的性价比。
3.基于uwb的定位系统，通过安装固定的uwb基站测量uwb基站与机器人上的uwb标签之间的实时距离，进而对该机器人进行定位。
4.现有技术公开一种基于uwb定位的三维定位系统，包括uwb基站和移动uwb基站，包括以下步骤：(1)、搭建uwb基站，并分别计算两个uwb基站之间的距离；(2)、计算移动uwb基站到其他uwb基站的距离，计算出每个移动uwb基站的绝对位置；(3)、以移动uwb基站起始点，两个uwb基站分别为终止点，测量两个uwb基站与移动uwb基站的距离差，对移动uwb基站进行定位；(4)、选择其它uwb基站作为终止点，计算得到移动uwb基站的三维空间定位。本发明通过uwb定位达到三维空间实时定位，并且能够提高传感器的三维空间定位精度，形成多传感器的高效率监测；通过uwb基站与传感器在可移动装置上的配合使用，实现了实时监测数据传输和位置信息传输，提高了工程试验中的数据分析结果的准确性。
5.上述现有技术中，基于uwb的定位系统由于安装固定的uwb基站导致可定位范围非常局限，且由于nlos(non line of sight，非视距)误差，uwb标签会在受到遮挡的时候造成数据丢失而无法定位。


技术实现要素：

6.本发明提供一种基于uwb和imu融合的多机器人协同定位系统，解决现有技术中存在的nlos误差问题。
7.为解决上述技术问题，本发明的技术方案如下：
8.一种基于uwb和imu融合的多机器人协同定位系统，至少包括两个目标机器人，每一个目标机器人均包括定位模块、imu模块、数据融合模块以及多个uwb模块，其中：
9.所述imu模块获取目标机器人的目标加速度和目标角加速度；
10.每一个所述uwb模块用于测量该uwb模块与其它目标机器人的uwb模块之间的距离，并将该距离作为初始测距值；
11.所述定位模块根据imu模块获取的目标加速度和目标角加速度和每一个uwb模块获取的初始测距值，得到多个定位坐标；
12.所述融合模块根据所述多个定位坐标确定该目标机器人的目标位置信息。
13.优选地，所述imu模块包括三轴加速度计和三轴陀螺仪，所述三轴加速度计用于测量目标机器人的目标加速度；所述三轴陀螺仪用于测量目标机器人的目标角加速度。
14.优选地，每一个所述uwb模块为uwb收发器；当该uwb模块进行测距时，该uwb模块作为定位标签；当该uwb模块以外的任一其他uwb模块进行测距时，该uwb模块作为定位基站。
15.优选地，所述定位模块包括第一定位模块、校准与补偿模块和第二定位模块，其中，所述第一定位模块用于根据所述imu模块获取的目标加速度和目标角加速度，计算目标机器人的第一位置信息；所述校准与补偿模块用于根据所述第一位置信息对uwb模块得到的初始测距值进行校准，得到目标测距值；所述第二定位模块用于根据目标机器人的多个uwb模块的所述目标测距值，得到多个定位坐标。
16.优选地，所述第一定位模块用于根据所述imu模块获取的目标加速度和目标角加速度，计算目标机器人的第一位置信息，具体为：
17.模块获取的目标加速度a和目标角加速度ω，已知上一时刻的位置，利用以下公式计算当前位置：
18.ω＝ωb+bg+ng19.a＝q
bw
(aw+gw)+ba+na20.式中，上下标w表示世界坐标系，上下标b表示imu本体坐标系，q
bw
表示从w坐标系到b坐标系的旋转四元数；
21.又有p＝(x，y)表示位置，v表示速度，q表示姿态，其关于时间的导数有：
[0022][0023][0024][0025]
式中，下标t表示t时刻；
[0026]
则第k-1时刻时，积分得到第k时刻为：
[0027][0028][0029][0030]
由上可知，所以为其估计距离。
[0031]
优选地，所述校准与补偿模块用于根据所述第一位置信息对uwb模块得到的初始测距值进行校准，得到目标测距值，具体为：
[0032]
根据第一位置信息计算初始测距值的误差距离：
[0033]
e＝|z
k-xk|
[0034]
式中，e为误差距离，zk为uwb模块得到的初始测距值；
[0035]
针对每一初始测距值，若误差距离大于预设阈值，则表明当前该初始测距值为
nlos距离，为避免nlos误差，则将估计距离替换该初始测距值，以对初始测距值进行校准；针对每一初始测距值，若误差距离不大于预设阈值，则表明当前该初始测距值不为nlos距离，则不对初始测距值进行校准。
[0036]
优选地，若uwb模块向其它uwb模块发送的测距消息超过预设时间未收到回复，则确定该uwb模块与其它uwb模块为nlos距离，将估计距离作为目标测距值。
[0037]
优选地，所述校准与补偿模块还用于根据目标位置信息对预设航迹推演算法进行补偿。
[0038]
优选地，所述第二定位模块用于根据目标机器人的多个uwb模块的所述目标测距值，得到多个定位坐标，具体为：
[0039]
将当前目标机器人的侧足作为x轴，垂直于x轴为y轴，通过定义的初始位置和在当前目标机器人上的多个uwb模块的安装位置，得到当前目标机器人的多个uwb模块的坐标；
[0040]
目标测距值为当前目标机器人上的多个uwb模块测量到另一目标机器人上的其中一个uwb模块的距离，根据目标测距值与当前目标机器人的多个uwb模块的坐标，计算得到另一目标机器人的多个uwb模块的定位坐标。
[0041]
优选地，所述数据融合模块根据所述多个定位坐标确定该目标机器人的目标位置信息，具体为：
[0042]
根据另一目标机器人的多个uwb模块的定位坐标，求取平均值后得到另一目标机器人的定位坐标。
[0043]
与现有技术相比，本发明技术方案的有益效果是：
[0044]
本发明至少包括两个目标机器人，每一个目标机器人均包括定位模块、imu模块、数据融合模块以及多个uwb模块，其中：所述imu模块获取目标机器人的目标加速度和目标角加速度；每一个所述uwb模块用于测量该uwb模块与其它目标机器人的uwb模块之间的距离，并将该距离作为初始测距值；所述定位模块根据imu模块获取的目标加速度和目标角加速度和每一个uwb模块获取的初始测距值，得到多个定位坐标；所述融合模块根据所述多个定位坐标确定该目标机器人的目标位置信息。本发明通过imu模块和第一定位模块可以对目标机器人进行自定位得到第一位置信息，然后使用第一位置信息对uwb模块测得的初始测距值进行校准，进而确定目标机器人的目标位置信息，减少nlos对定位的影响。
附图说明
[0045]
图1为本发明提供的系统结构示意图。
[0046]
图2为本发明提供的系统中其中一个目标机器人结构示意图。
具体实施方式
[0047]
附图仅用于示例性说明，不能理解为对本专利的限制；
[0048]
为了更好说明本实施例，附图某些部件会有省略、放大或缩小，并不代表实际产品的尺寸；
[0049]
对于本领域技术人员来说，附图中某些公知结构及其说明可能省略是可以理解的。
[0050]
下面结合附图和实施例对本发明的技术方案做进一步的说明。
[0051]
实施例1
[0052]
一种基于uwb和imu融合的多机器人协同定位系统，如图1和图2所示，至少包括两个目标机器人，每一个目标机器人均包括定位模块、imu模块、数据融合模块以及多个uwb模块，其中：
[0053]
所述imu模块获取目标机器人的目标加速度和目标角加速度；
[0054]
每一个所述uwb模块用于测量该uwb模块与其它目标机器人的uwb模块之间的距离，并将该距离作为初始测距值；
[0055]
所述定位模块根据imu模块获取的目标加速度和目标角加速度和每一个uwb模块获取的初始测距值，得到多个定位坐标；
[0056]
所述融合模块根据所述多个定位坐标确定该目标机器人的目标位置信息。
[0057]
在本实施例中，目标机器人可以是足式机器人，例如，十二自由度的四足机器人，或者目标机器人可以是轮式移动机器人。
[0058]
实施例2
[0059]
本实施例在实施例1的基础上，继续公开以下内容：
[0060]
imu模块为imu传感器，部署与目标机器人的体内。所述imu模块包括三轴加速度计和三轴陀螺仪，所述三轴加速度计用于测量目标机器人的目标加速度；所述三轴陀螺仪用于测量目标机器人的目标角加速度，从目标机器人从初始位置开始运动，imu模块可以持续获取目标机器人当前的运动动态信息，即目标机器人的线速度和角速度。通过预设航位推演算法和目标机器人的线速度和角速度可以预测目标机器人相对于初始位置的第一位置信息。
[0061]
imu模块进行长时间的位置估计时，可能由于累积误差导致位置估计不准，目标机器人可以根据目标位置信息，对imu模块进行累积误差矫正，以保证imu模块位置估计的准确度。
[0062]
uwb模块为uwb收发器，可以部署于目标机器人的上表面便于uwb信号的发送接收；当该uwb模块进行测距时，该uwb模块作为定位标签；当该uwb模块以外的任一其他uwb模块进行测距时，该uwb模块作为定位基站。
[0063]
实施例3
[0064]
本实施例在实施例1和实施例2的基础上，继续公开以下内容：
[0065]
所述定位模块包括第一定位模块、校准与补偿模块和第二定位模块，其中，所述第一定位模块用于根据所述imu模块获取的目标加速度和目标角加速度，计算目标机器人的第一位置信息；所述校准与补偿模块用于根据所述第一位置信息对uwb模块得到的初始测距值进行校准，得到目标测距值；所述第二定位模块用于根据目标机器人的多个uwb模块的所述目标测距值，得到多个定位坐标。
[0066]
所述第一定位模块用于根据所述imu模块获取的目标加速度和目标角加速度，计算目标机器人的第一位置信息，具体为：
[0067]
根据预设航迹推演算法和imu模块获取的目标加速度和目标角加速度，计算目标机器人的第一预测坐标和第一预测航向，作为该目标机器人的第一位置信息：
[0068]
模块获取的目标加速度a和目标角加速度ω，已知上一时刻的位置，利用以下公式计算当前位置：
[0069]
ω＝ωb+bg+ng[0070]
a＝q
bw
(aw+gw)+ba+na[0071]
式中，上下标w表示世界坐标系，上下标b表示imu本体坐标系，q
bw
表示从w坐标系到b坐标系的旋转四元数；
[0072]
又有p＝(x，y)表示位置，v表示速度，q表示姿态，其关于时间的导数有：
[0073][0074][0075][0076]
式中，下标t表示t时刻；
[0077]
则第k-1时刻时，积分得到第k时刻为：
[0078][0079][0080][0081]
由上可知，所以为其估计距离。
[0082]
所述校准与补偿模块用于根据所述第一位置信息对uwb模块得到的初始测距值进行校准，得到目标测距值，具体为：
[0083]
根据第一位置信息计算初始测距值的误差距离：
[0084]
e＝|z
k-xk|
[0085]
式中，e为误差距离，zk为uwb模块得到的初始测距值；
[0086]
针对每一初始测距值，若误差距离大于预设阈值，则表明当前该初始测距值为nlos距离，为避免nlos误差，则将估计距离替换该初始测距值，以对初始测距值进行校准；针对每一初始测距值，若误差距离不大于预设阈值，则表明当前该初始测距值不为nlos距离，则不对初始测距值进行校准。
[0087]
若uwb模块向其它uwb模块发送的测距消息超过预设时间未收到回复，则确定该uwb模块与其它uwb模块为nlos距离，将估计距离作为目标测距值。
[0088]
所述校准与补偿模块还用于根据目标位置信息对预设航迹推演算法进行补偿。
[0089]
所述第二定位模块用于根据目标机器人的多个uwb模块的所述目标测距值，得到多个定位坐标，具体为：
[0090]
将当前目标机器人的侧足作为x轴，垂直于x轴为y轴，通过定义的初始位置和在当前目标机器人上的多个uwb模块的安装位置，得到当前目标机器人的多个uwb模块的坐标；
[0091]
目标测距值为当前目标机器人上的多个uwb模块测量到另一目标机器人上的其中一个uwb模块的距离，根据目标测距值与当前目标机器人的多个uwb模块的坐标，计算得到另一目标机器人的多个uwb模块的定位坐标。
[0092]
在本实施例中以两个目标机器人为例说明，分别为目标机器人a和目标机器人b。以目标机器人a为例，平行目标机器人a侧足为x轴，垂直于x轴为y轴。则通过定义的初始位置和三个uwb模块安装位置，可知目标机器人a上的三个模块的坐标分别为a0坐标为(x0，y0)，a1坐标为(x1，y1)，a2坐标为(x2，y2)。所以可得目标机器人的位置坐标为[(x0+x1+x2)/3，(y0+y1+y2)/3],设其为坐标原点。设目标机器人b的第一uwb模块b0坐标为(x3，y3)。r0、r1和r2为目标机器人a上面三个uwb模块测量到目标机器人b上的第一uwb模块b0上的距离。所以有：
[0093][0094]
所以求得目标机器人b第一uwb模块b0的坐标，同理也可以求得目标机器人b其余两个uwb模块的坐标。同理，两个以上的多目标机器人的定位坐标也是如此。
[0095]
所述数据融合模块根据所述多个定位坐标确定该目标机器人的目标位置信息，具体为：
[0096]
根据另一目标机器人的多个uwb模块的定位坐标，求取平均值后得到另一目标机器人的定位坐标。
[0097]
在本实施例中，按上述步骤求得目标机器人b的三个uwb模块的定位坐标b0(x3，y3)，b1(x4，y4)，b2(x5，y5)的坐标，则目标机器人b的定位坐标为[(x3+x4+x5)/3，(y3+y4+y5)/3]。反之，以目标机器人b的三个uwb模块去测距，得出目标机器人a的uwb多定位坐标。然后取均值处理得到目标位置信息。
[0098]
相同或相似的标号对应相同或相似的部件；
[0099]
附图中描述位置关系的用语仅用于示例性说明，不能理解为对本专利的限制；
[0100]
显然，本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。