基于多传感器数据融合的机器人定位系统

乔志健　康永　许峰榕

1引言

目前，自主移动机器人在诸如家庭服务，养老辅助，教育等领域迅速发展，而机器人定位技术是可自主移动的机器人的技术关键点，也是近年来研究热点之一。可自主移动机器人的定位方式可分为相对定位和绝对定位两种方式。相对定位也称之为航迹推算（dead reckoning）。它通过装在轮子上的磁编码器来估算机器人的速度，通过自带的惯性元器件测量机器人的姿态信息，因此它并不依赖外部信号，就能估算出机器人相对于起始位置的位姿信息。近年来，mems技术快速发展，使得这种定位方法的成本逐渐降低，成为一种低成本，高效率，实时性强的机器人定位方式。

2 机械与电路设计

如图一所示，机械结构由可安放主电路板的凹槽和两个正交安放并装有磁编码器的从动轮组成。主电路板捷联陀螺仪芯片（icm20602-g）与三轴电子罗盘（LSM303AGR）可提供角速度，加速度，地磁信息。从动轮轴心装有随轮转动的磁铁，而磁编码器固定在机械结构上不随其转动，由此可获取从动轮转动圈数，结合轮子半径参数，可算出从动轮的里程。同时，两个从动轮的外侧安装了两个弹簧，这使其可以适应不平整的地面，并增大对地面的压力，增大摩擦力，减少打滑。

3 姿态解算

常用的姿态描述方法有欧拉角与四元数两种。欧拉角便于用户使用，但是欧拉角本身在运算中却具有极大的缺点： 一，欧拉角运算的时候会有大量的三角函数运算，计算量大；二，欧拉角存在万向节死锁。因此本文采用旋转四元数描述旋转过程，旋转四元数可以表示成如下形式：

（1）

上式中 描述旋转角度， 是一个单位向量，描述的是旋转轴的方向，所以上述四元数描述的是物体绕 旋转轴旋转了 。

通过查阅文献可以知道四元数微分如下：

其中Gyrox，Gyroy，Gyroz分别表示陀螺仪测量输出的三轴角速度。结合上式以及最新获取的角速度信息，使用四阶龙哥库塔可对四元数进行更新。

由于 是单位向量，所以满足：

从上面的式子就可以得到四元数描述的旋转对应的坐标变换公式，如下所示：

另外，可获取加速度的信息：x轴加速度分量measure.x，y轴加速度分量measure.y，z轴加速度分量measure.z。并通过以下公式计算出欧拉角信息：

最后，通过卡尔曼滤波算法对陀螺仪解算出的欧拉角与加速度计解算出的欧拉角进行融合，得出新姿态。

4 IMU冗余配置

陀螺仪的角度随机游走噪声是一段白噪声，通常使用ARMA时间序列分析法对陀螺仪静态输出进行建模，得到的模型可作为卡尔曼滤波器状态方程，对噪声进行过滤。本文使用的冗余配置常用于故障诊断中，但由于陀螺仪的角度随机游走噪声近似为高斯分布，因此陀螺仪的冗余配置同样可以减少噪声的强度，达到滤波的效果。本文使用三个IMU单元进行同时测量，并以三个IMU单元在静止状态下输出的方差的大小对三个IMU单元融合的权重进行确定。如下图所示为三个IMU输出姿态与三个IMU冗余配置输出结果（由上至下第三条）。结果表明，IMU冗余配置能有效减小陀螺仪角度随机游走带来的影响。

5 坐标计算

5.1 坐标解算过程

由航迹推算法可得，在很短的时间间隔内，可以将整个系统的运动状态看成是姿态不变的直线运动。由于采样时间和计算时间都很短（位姿计算周期5ms），本系统适用上述规律，因此可以采用下述算法，结果如下。

5.2 机械参数的矫正

如下图，矫正装置由一根直线导轨和一个可在滑轨上滑动的连接器构成。连接器可带动定位系统机加件在导轨上滑动，并且可以在水平面上进行转动。

通过磁编码器可间接计算出轮子转动的圈数num1和num2，并使定位装置在连接器的带动下，不改变角度，从导轨的起点走到终点。其中导轨的长度为729mm。根据机械误差模型可建立以下等式。

如图中所示，一个轮子的实际半径为 25.44mm， 另一个轮子的半径为 25.31mm， 并且机械角度误差为0.3度。

6 结论

航迹推算法是一种低成本，實时性强的机器人定位算法，但是累积误差一直是阻碍其性能的关键因素。本文通过陀螺仪和加速度计解算出机器人姿态，并用卡尔曼滤波对数据进行融合，同时，采用冗余配置减小陀螺仪输出噪声。并且，为了提高坐标计算精度，本文设计算法矫正机械加工误差。实验证明，本文提出的这些方法能有效提高传统航迹推算法的精度，为航迹推算法的研究提供参考。

（作者单位：东北大学信息科学与工程学院）