基于被动式移动架构的室内机器人二维定位方法

2021-05-06周奇勋刘全龙蔡紫薇

科学技术与工程 2021年5期

周奇勋, 王茜, 刘全龙, 蔡紫薇

(西安科技大学矿用电机驱动技术研究所，西安 710054)

机器人运动的能量来自于自身携带的电池。为了使机器人能够可持续工作，当机器人携带的电池电量低时，能够准确找到机器人充电桩位置完成自主充电这一过程就变得尤为重要。常见的室内定位技术有基准系统(reference point system，RPS)激光定位、视觉同步定位与建图(visual simultaneous localization and mapping，VSLAM)、WiFi定位[1]、射频识别(radio frequency idenfication，RFID)定位[2]、超声波定位[3]等。RPS激光定位中激光探头成本价格高，工作时激光探头不停地旋转导致寿命有限；VSLAM图像位移定位算法复杂且对硬件资源要求高；WiFi定位对环境依赖性较大，定位精度低，可达到米级别；RFID定位布局难度大，对环境依赖性也较大；而超声波定位具有抗干扰强、布局简单、定位精度高和成本低等优势[4-5]。

文献[6]将测量误差精度提高到不超过3 cm，但需要在定位区域分散安装4个定位基点，再结合三边测距原理得到移动节点的坐标(x,y)，文献[7]建立误差修正模型提高了定位精度，但定位算法采用了最小二乘法，需要系统中安装多个参考节点，还加入了汇聚节点，增加了系统实现难度。

现采用由两个发送节点和三个接收节点组成的被动式移动架构(2T3R passive mobile architecture，2T3R-PMA)定位系统，定位节点固定在机器人充电桩上，能够实现机器人的自主充电功能；通过测量模型除了得到机器人的位置坐标(x,y)，还可得到其相对于机器人充电桩的方向角。

1 2T3R-PMA定位原理与测量模型

采用的超声波测距方法为到达时间差(time difference of arrival，TDOA)方法[7-8]。TDOA方法的示意图如图1所示。机器人充电桩同时发送无线同步信号和超声波信号，由于无线信号传播速率(v1)远远大于超声波传播速率(v2)，可忽略不计无线信号的传播时间，即t1近似为0，Δt即为超声波的传播时间。

t1为无线信号的传播时间；t2为超声波信号的传播时间；Δt为两者差值图1 TDOA方法示意图Fig.1 Schematic diagram of TDOA method

2T3R-PMA的定位思想为机器人携带电池电量低时，通过无线模块向机器人充电桩定位基站发送“定位”无线信号，机器人充电桩接收到“定位”无线信号后，两个发送节点Ti(i=1,2)，周期性地轮流发送带有发送节点标识码的无线同步信号和超声波信号。机器人接收到带有标识码的无线同步信号开始计时；三个接收节点Rj(j=1,2,3)均接收到超声波信号后，停止计时，利用TDOA方法可得到超声波信号从发送节点Ti到接收节点Rj的传播时间，乘以当前声速，进而估算出Ti与Rj之间的距离。

2T3R-PMA定位系统示意图如图2所示。对机器人充电桩来说，以两个发送节点的中点Oc为坐标系原点，直线T1T2为xc轴；对机器人来说，以R2作为相对坐标系的原点Or，直线R1R2R3为xr轴。xr与xc的夹角θ为机器人的方位角。其中2a为机器人充电桩上两个发送节点之间的距离，2b为机器人上R1与R3接收节点之间的距离，取a=10 cm，b=30 cm。

图2 2T3R-PMA定位系统示意图Fig.2 Schematic diagram of 2T3R-PMA positioning system

假定发送节点和接收节点安装在同一高度上，在2T3R-PMA定位系统中，设机器人的位置信息为(x,y,θ)T，接收节点R1、R2、R3的坐标分别为(x1,y1)、(x2,y2)和(x3,y3)，发送节点到接收节点的距离公式可以表示为

(1)

式(1)中：(xj,yj)为接收节点的坐标；d1j为2#发射节点到j#接收节点的距离，cm；d2j为2#发射节点到j#接收节点的距离，cm。

任意两个接收节点xm、xn与两个发送节点ym、yn构成的测量模型可表示为

(2)

由式(1)和式(2)可得x和y的表达式为

(3)

(4)

(5)

机器人方向角θ的表达式为

(6)

(7)

(8)

接收节点与m，n，k的取值对应表如表1所示。

表1 接收节点与m，n，k的取值对应表

2 定位误差分析及修正

2.1 算法误差

算法误差产生的主要原因是d1j和d2j距离值的不准确。而导致距离值不准确的原因有两种：①由于超声波存在反射现象，机器人得到的d1j和d2j有可能为反射后的不准确数据;②当机器人相对于机器人充电桩的方向角|θ|过大或者机器人与其充电桩距离较近时，会导致两边接收节点R1或R3接收不到超声波信号。

当d1j和d2j距离值数据不准确时，直观地反映在测量模型公式上存在两种情况：①式(4)中ym或yn的根号下出现负值情况;②式(6)～式(8)中的sinθ、cosθ和tanθ的值出现大于1的情况。

以R1接收节点(即m=1)为例，假设d11=41.6 cm和d21=63.2 cm，各个中间变量的值如表2所示。

表2 d11=41.6 cm和d21=63.2 cm时各变量的值

此时y1的第一个根号下出现了负值，为-440.25，这时会计算出复数值，造成最终结果y的误差。误差修正的办法为在程序中设置滤值条件，舍弃不符合条件的距离值，保证不会出现根号下为负值的情况。假设每个接收节点都能接收到两个发送节点的超声波信号，以R1为例，T1、T2和R1三个节点一起组成△T1T2R1，三角形各个边长2a、d11和d21满足三角形构成条件|d11-d21|<2a。同理，R2和R3的约束条件分别为|d12-d22|<2a和|d13-d23|<2a。

以R1和R2节点(即m=1，n=2，k=b/2=15)为例，假设d11=348.86 cm，d21=347.44 cm，d12=361.42 cm和d22=362.35 cm，可得

此时得出的余弦值无法用于计算方位角θ。误差修正的方法为在程序中设置判断条件，若得到的正弦、余弦或者正切值出现大于1的情况，舍弃该值，用其他值计算θ。

当R1、R2和R3均收到超声波信号时，采用两两组合的方式均代入测量模型公式，最终结果取平均值。当R1节点接收不到超声波信号，就直接采用R2和R3两节点的数据进行计算。同理，当R3节点接收不到信号的情况下，用R1和R2两节点的数据计算。

2.2 测量误差

测量误差主要由硬件测量时间和程序执行时间等组成[7]。

TDOA测距方法默认无线信号与超声波信号同时发出，实际上无线信号通过串口通信直接发出，超声波信号需脉冲信号驱动发射电路才能发出，导致超声波发出时刻滞后于无线信号发出时刻。机器人充电桩上无线信号和超声波信号发出时序图如图3所示。

t3为无线信号的发出时刻；t4为超声波信号的发出时刻；Δt1为超声波信号滞后无线信号发出的时间图3 机器人充电桩上信号发出时序图Fig.3 Time sequence diagram of signal sending on robot charging pile

机器人是否接收到超声波信号的判断依据是接收芯片输出管脚是否产生下降沿。将接收芯片输出管脚设置成外部中断来监测是否有下降沿，输出管脚产生下降沿到外部中断响应有一定的时间延迟。

测量误差通过分段抛物线插值来补偿，分段抛物线插值的补偿效果比曲线拟合和线性插值效果更好、更精确。抛物线插值的原理为已知三点(x0,y0)、(x1,y1)和(x2,y2)，可构造出二次拉格朗日插值多项式为

(9)

2.3 温度漂移

超声波传播速度会受到空气中温度、湿度、压强等因素的影响，其中受温度的变化影响最大[6-12]。超声波在不同温度下的传播速度如表3所示，由表3中数据可以看出，超声波传播速度在-30 ℃时为313.1 m/s，而在30 ℃为349.0 m/s，假设在-30 ℃环境下测量却用30 ℃的传播速度进行计算，传播速度误差为(349.0-313.1)/313.1=11.47%。

表3 不同温度下对应的超声波传播速度

对表3中数据进行二次插值得到传播速度与温度表达式为

v=0.000 7t2+0.567 5t+v0

(10)

式(10)中：v为当前传播速度，m/s；t为当前环境温度，℃；v0=331.895 4 m/s。当t=0 ℃,代入式(10)得到v=331.895 4，与实际0 ℃对应传播速度的误差为|331.895 4-332.1|/332.1=0.62%，误差明显减少。所以对于温度漂移的修正办法为通过温度传感器检测温度，代入上述公式获得当前传播速度。