0 引言

定位问题是实现微小型移动机器人自主工作的基础^[1].从概率论的角度，机器人的定位问题也可以看作结合传感器信息实现状态估计的问题.经典的机器人自定位方法有基于贝叶斯理论的卡尔曼滤波^[2]、扩展卡尔曼滤波^[3]和粒子滤波^[4]等.其中粒子滤波由于能够解决非线性、非高斯系统的状态估计问题^[5]而受到广泛关注.受尺寸设计的限制，选用尺寸小、功耗低的红外传感器设计微小型移动机器人的定位测量系统.近年来，基于红外传感器的机器人定位系统研究取得了一定的进展.文献[6]基于Khepera Ⅲ本体上的红外传感器，在结构化规则的环境中(1.8 m范围内)，结合扩展卡尔曼滤波，对消除微小型机器人定位过程中的累计误差进行了研究，定位精度达到1.9 cm.文献[7]提出了一种基于改进IMM-UKF算法的红外路标室内定位方法，该系统在室内已知环境下配备单个大功率红外发射管作为路标，通过机器人本体上的红外摄像机作为接收传感器，定位精度可达4.5 cm，但该红外定位测量系统设计尺寸较大，无法应用到微小型机器人本体上.针对于此，文献[8]设计了一套基于六对红外收发传感器的小型定位系统，结合粒子滤波算法，实现了微小型机器人之间的相对定位，精度达到7.4 mm.

为进一步解决微小型移动机器人的定位问题，本文设计了一套小尺寸的红外定位系统，可根据红外接收信号强度得到机器人之间的相对位置信息，并在已知环境中通过将坐标确定的微小型机器人(指可根据周围环境进行自定位的机器人)作为地标，基于粒子滤波算法可对微小型移动机器人之间进行协作定位.

1 红外定位模型

机器人根据红外接收到的信号强度，换算出地标与当前机器人的相对距离和方位.假设红外接收信号的入射角为α，此时机器人相邻的n个接收端都接收到了信号，将这n个接收端定义为一个感应扇区，需要根据每个接收端的接收强度来近似入射信号强度r.假设n个接收端的接收信号强度沿顺时针方向依次为r′ _-m, r′ _-m+1, …r′ ₀, …, r′ _m-1, r′ _m，其中m=n/2.单个接收端的接收信号强度与入射信号强度的关系为r′=r cos(δ)，其中δ为该接收端与入射信号的夹角.则第i个接收端的接收信号强度为

$ {r'_i} = r\;{\rm{cos}}(\alpha - {\varphi _i}), $

(1)

式中：α为入射角，φ_i为第i个接收端与扇区中心接收端的夹角，且φ_i=-φ_-i.因此有

$ {{r'}_i} + {{r'}_{ - i}} = 2r\;{\rm{cos}}\alpha \;cos{\varphi _i};{{r'}_i} - {{r'}_{ - i}} = 2r\;{\rm{sin}}\alpha \;\sin {\varphi _i}. $

(2)

令

$ a = \frac{{\mathop \sum \limits_{i = 0/1}^m \left( {{{r'}_i} + {{r'}_{ - i}}} \right)}}{{\mathop \sum \limits_{i = 0}^m 2{\rm{cos}}{\varphi _i}}} = r\;{\rm{cos}}\alpha ;b = \frac{{\mathop \sum \limits_{i = 0/1}^m \left( {{{r'}_i} - {{r'}_{ - i}}} \right)}}{{\mathop \sum \limits_{i = 0}^m 2{\rm{sin}}{\varphi _i}}} = r\;{\rm{sin}}\alpha . $

(3)

利用三角函数变换公式，可得

$ \alpha = {\rm{arctan}}\left( {{\rm{ }}b{\rm{/}}a{\rm{ }}} \right);r = {\left( {{a^2} + {b^2}} \right)^{\frac{1}{2}}} $

(4)

当n=3时，机器人红外定位模型如图 1所示.此时，三个接收端的接收信号强度分别为r′₀=r cos α，r′₁=r cos(α-φ₁)，r′_-1=r cos(α-φ_-1).

令

$ a = \frac{{{{r'}_1} + {{r'}_{ - 1}} + 2{{r'}_0}}}{{2{\rm{cos}}{\varphi _1} + 2}} = r\;{\rm{cos}}\alpha ;b = \frac{{{{r'}_1} - {{r'}_{ - 1}}}}{{2\sin {\varphi _1}}} = r\;{\rm{sin}}\alpha $

(5)

将式(5)代入式(4)得到3个传感器的接收光强度，即可得到r和α.

2 粒子滤波定位 2.1 里程计运动模型

由于尺寸限制，机器人利用里程计作为内部传感器.图 2为机器人的里程计运动模型，假设机器人在很短的时间间隔内进行半径为R的圆弧运动，经过几何推算就可估计出机器人两时刻之间的位姿增量.

机器人相邻时刻的直线距离增量Δd可以近似表示为

$ \Delta d = \Delta S = \frac{{\Delta {S_{\rm{L}}} + \Delta {S_{\rm{R}}}}}{2}, $

(6)

式中：ΔS_L和ΔS_R分别为里程计测得的时间间隔内左、右轮平滑移动距离；ΔS为机器人中心的平滑移动距离.

根据机器人的差动运动学方程，机器人上一时刻与本时刻之间的位姿增量为

$ \Delta P = \left[ {\begin{array}{*{20}{c}} {\Delta \theta }\\ {\Delta x}\\ {\Delta y} \end{array}} \right] = \left[ {\begin{array}{*{20}{c}} {\frac{{\Delta {S_{\rm{R}}} - \Delta {S_{\rm{L}}}}}{{2R}}}\\ {\Delta d \cdot \cos \left( {\theta + \frac{{\Delta \theta }}{2}} \right)}\\ {\Delta d \cdot \sin \left( {\theta + \frac{{\Delta \theta }}{2}} \right)} \end{array}} \right]. $

(7)

机器人的里程计运动模型可表示为

$ \left\{ {\begin{array}{*{20}{c}} {\theta \left( k \right) = \theta \left( {k - 1} \right) + \Delta \theta + \varepsilon _{k - 1}^\theta }\\ {x\left( k \right) = x\left( {k - 1} \right) + \Delta x + \varepsilon _{k - 1}^x}\\ {y\left( k \right) = y\left( {k - 1} \right) + \Delta y{\rm{}} + {\rm{}}\varepsilon _{k - 1}^y} \end{array}.{\rm{}}} \right. $

(8)

粒子滤波中，可以根据式(8)的运动模型，由机器人k-1时刻的位姿信息，结合里程计的观测数据，来预测k时刻该机器人的位姿信息.

2.2 粒子滤波定位算法

粒子滤波定位算法的基本原理是对k时刻机器人的位姿状态采样N个粒子，建立带权值的粒子集$\left\{ {\tilde X_k^{\left( i \right)}, \tilde w_k^{\left( i \right)}} \right\}_{i = 1}^N $，利用粒子集近似机器人当前时刻的位姿状态.k时刻粒子集中的第i个粒子$\tilde X_k^{\left( i \right)} $是对k时刻机器人位姿状态的一种假设，它的位姿信息可表示为$\left( {x_k^{\left( i \right)}, y_k^{\left( i \right)}, \theta _k^{\left( i \right)}} \right), \tilde w_k^{\left( i \right)} $对应这个粒子的权值.利用粒子滤波算法实现机器人定位的具体步骤如下.

Step 1：初始化.对于k=0，从机器人初始位姿状态进行采样，得到初始粒子集$\left\{ {X_0^{\left( i \right)}, w_0^{\left( i \right)}} \right\}_{i = 1}^N $，其中w₀⁽ⁱ⁾=1/N.粒子滤波定位的初始状态是未知的，初始粒子均匀地分布在整个定位空间内.

Step 2：采样.对于k=1，2，…，从$p = \left( {X_k^{\left( i \right)}{\rm{|}}X_{k - 1}^{\left( i \right)}} \right) $采样得到新的粒子集$\left\{ {\tilde X_k^{\left( i \right)}} \right\}_{i = 1}^N $.实质上是根据里程计观测信息，利用里程计运动模型，根据上一时刻的位姿状态 $\left\{ {\mathit{X}_{\mathit{k} - 1}^{\left( \mathit{i} \right)}} \right\}_{\mathit{i} = 1}^\mathit{N}$预测本时刻的位姿状态$ \left\{ {\mathit{\tilde X}_\mathit{k}^{\left( \mathit{i} \right)}} \right\}_{\mathit{i = }{\rm{1}}}^\mathit{N}$.

Step 3：权值计算.根据$ \tilde w_k^{\left( i \right)} = \tilde w_{k - 1}^{\left( i \right)}p\left( {Z_k^{\left( i \right)}{\rm{|}}X_k^{\left( i \right)}} \right)$计算粒子$\tilde X_k^{\left( i \right)} $的权值$\tilde w_k^{\left( i \right)} $，并对粒子的权值进行归一化，$ w_k^{\left( i \right)} = \tilde w_k^{\left( i \right)}/\mathop \sum \nolimits_{i = 1}^N \tilde w_k^{\left( i \right)}$，其中$ p\left( {Z_k^{\left( i \right)}{\rm{|}}X_k^{\left( i \right)}} \right) $被称为感知模型.权值计算的过程，实际上可以看作是微小型移动机器人定位过程中传感器信息与地图进行匹配的过程.假设相对距离和相对方位观测噪声为0均值的高斯噪声方差分别为σ_d²、σ_β²，各观测相互独立，则权值可表示为

$ \tilde w_k^{\left( i \right)} = \tilde w_{k - 1}^{\left( i \right)}\frac{1}{{\sqrt {2{\rm{ \mathit{ π} }}} {\sigma _d}}}{{\rm{e}}^{\frac{{ - {{\left( {d - {d_i}} \right)}^2}}}{{2\sigma _d^2}}}}\frac{1}{{\sqrt {2{\rm{ \mathit{ π} }}} {\sigma _\beta }}}{{\rm{e}}^{\frac{{ - {{\left( {\beta - {\beta _i}} \right)}^2}}}{{2\sigma _\beta ^2}}}}, $

(9)

式中：d为k时刻观测距离；d_i为粒子X_k⁽ⁱ⁾到路标的预测距离；β为k时刻观测方位；β_i为粒子X_k⁽ⁱ⁾到路标的预测方位.若粒子位姿信息为$\left( {x_k^{\left( i \right)}, y_k^{\left( i \right)}, \theta _k^{\left( i \right)}} \right) $, 被观测路标的位置为$\left( {{{\hat x}_T}, {{\hat y}_T}} \right) $，则

$ {d_i} = \sqrt {{{\left( {{{\hat x}_T} - x_k^{\left( i \right)}} \right)}^2} + {{\left( {{{\hat y}_T} - y_k^{\left( i \right)}} \right)}^2}} , $

(10)

$ {\beta _i} = {\tan ^{ - 1}}\left( {\frac{{{{\hat y}_T} - y_k^{\left( i \right)}}}{{{{\hat x}_T} - x_k^{\left( i \right)}}}} \right) - \theta _k^{\left( i \right)}. $

(11)

若某一时刻同时观测到M个地标，则权值可表示为

$ \tilde w_k^{\left( i \right)} = \tilde w_{k - 1}^{\left( i \right)}\mathop \prod \limits_{j = 1}^M \frac{1}{{\sqrt {2{\rm{ \mathit{ π} }}} {\sigma _d}}}{{\rm{e}}^{\frac{{ - {{\left( {{d_j} - {d_i}} \right)}^2}}}{{2\sigma _d^2}}}}\frac{1}{{\sqrt {2{\rm{ \mathit{ π} }}} {\sigma _\beta }}}{{\rm{e}}^{\frac{{ - {{\left( {{\beta _j} - {\beta _i}} \right)}^2}}}{{2\sigma _\beta ^2}}}}{\rm{.}} $

(12)

式中：d_j、β_j(j=1, 2，…，M)为观测到相对第j个地标的距离和方位.

Step 4：状态估计输出.根据粒子集$ \left\{ {\tilde X_k^{\left( i \right)}, \tilde w_k^{\left( i \right)}} \right\}_{i = 1}^N$，k时刻系统的状态估计可以表示为

$ {\hat X_k} = E\left( {{X_k}} \right) = \mathop \sum \limits_{i = 1}^N w_k^{\left( i \right)}\tilde X_k^{\left( i \right)}. $

(13)

Step 5：重采样.根据粒子的权值$\tilde w_k^{\left( i \right)} $，从粒子集$\left\{ {\tilde X_k^{\left( i \right)}, \tilde w_k^{\left( i \right)}} \right\}_{i = 1}^N $中重新抽取N个粒子$\left\{ {X_k^{\left( i \right)}} \right\}_{i = 1}^N $，并令w_k⁽ⁱ⁾=1/N，重新建立粒子集$ \left\{ {X_k^{\left( i \right)}, 1/N} \right\}_{i = 1}^N$.重采样的目的是为了剔除一些权值小的粒子，减少不必要的推算，同时对权值大的粒子多次抽取，避免粒子退化.常用的重采样方法有简单随机重采样、分层重采样、系统重采样和残余重采样等.重采样过程是k时刻位姿信息处理的最后一步，重采样得到的粒子集将作为下一时刻位姿估计的初始粒子集.由此可见，粒子滤波实际上是一个增量式的递推过程，前一时刻位姿估计的误差将影响下一时刻状态估计的准确性.

3 实验部分与结果

为了验证本文的定位方法，对实际机器人系统进行了定位实验，整个实验过程如图 3所示.

在该实验中，选择4个机器人作为路标，1个待定位机器人按图 4中的理想轨迹移动，在移动的过程中，通过4个路标机器人进行定位和路径规划，定位过程共进行300次位置更新，时间间隔为1 s，粒子集为500.定位过程中不同时刻的距离误差如图 5(a)所示.可以看出，距离误差整体上集中在0.5~1.5 cm，随着时间增加变化幅度不大，基本满足定位需求，其中最大距离误差约为2.2 cm.不同时刻的航向角误差如图 5(b)所示.可以看出，航向角误差变化趋势比较平稳，整体上集中在0.02~0.10 rad，基本满足定位需求，其中最大航向角误差约为0.169 rad.从图 4中定位机器人的真实轨迹可以看出，定位机器人通过4个路标机器人可以较好地完成自身的定位.

4 结论

本文以微小型移动机器人为基础，设计了一套基于红外传感器的粒子滤波定位方法.该方法通过红外传感器定位模型确定机器人之间的相对位置，然后再将部分位置置信估计比较大的机器人(指可根据周围环境进行自定位的机器人)作为路标，根据里程计运动模型获取位姿状态预测信息，并将以上这些信息作为粒子滤波算法的预测和更新依据，通过更新粒子集来逼近机器人在全局坐标系中的位置.该方法降低了里程计累计误差对机器人定位的影响，可在相对定位的基础上进一步提高机器人的定位精度，比较适合复杂的非结构环境，机器人之间可以实现协作定位.