一种地图构建方法研究*

符秀辉 朱林楠 刘 然

（沈阳化工大学信息工程学院 沈阳 110142）

1 引言

机器人同步定位与地图构建（SLAM）是近年来的热点之一［1］，在构建地图时准确的实时姿态角获取是前提和关键。目前大多数惯性导航仪器存在漂移问题，在实际应用中，常见的姿态测量单元，主要由三轴陀螺仪、三轴加速度计和三轴磁力计组成［2］，一般采用三轴陀螺仪和三轴加速度计的配合使用来测算姿态角，进而得到机器人在地图中的位姿信息。运动载体的姿态可以由三轴陀螺仪通过积分获得［3］，短时间内解算的姿态精度高，而且具有良好的动态响应特性，但是由于陀螺仪本身存在漂移，而且在积分过程中会产生累积误差，因此长时间情况下，通过陀螺仪测量的数据解算得到的姿态精度较差。加速度计也可较精确测量运动载体的水平姿态角［4］，但是它对运动加速度十分敏感，当载体处于运动状态时，加速度计的测量会出现一定程度上的偏差，从而导致姿态精度下降［5］。考虑到三轴陀螺仪、三轴加速度计和三轴磁力计的动静态特性，拟采用数据融合方法对陀螺仪和加速度计测得的数据进行融合处理，采用卡尔曼滤波算法对角度进行误差修正［6］，以提高运动载体姿态信息的精度和系统的抗干扰能力，考虑到磁力计受到磁场强度的影响，参考航空飞行器对空中姿态角的测量方法，采用基于最小二乘法的椭球拟合对磁力计传感器数据进行校正。实验结果表明，对陀螺仪和加速度计的数据融合效果和对磁力计的数据校正会使误差更小，提高了系统的动态特性，从而实现对机器人在行进过程中的地图信息进行实时构建。

2 卡尔曼滤波

卡尔曼滤波在工程应用中较为成熟［7］，通过前一个时刻的估计值和现在的量测值来计算现在状态的估计值，具有实时性好、计算量小、易于工程实现的优点［8~9］。在实际导航系统中，陀螺仪存在温漂，短时间测量误差小，加速度计存在噪声，测量误差大，因此利用卡尔曼滤波融合陀螺仪与加速度计传感器的数据，减小数据偏差，获得相对准确的姿态角。卡尔曼滤波过程如下：

系统方程为

状态预测方程为

误差协方差方程为

卡尔曼增益方程为

修正估计方程为

更新误差协方差方程为

式中：Uk是输入的白噪声，A是状态转移矩阵，B是噪声驱动矩阵，H是观测矩阵，yk是k时刻的测量值，vk是观测噪声，X̂k-1是k-1 时刻的系统状态，X̂k|k-1是k-1 时刻系统最优状态，Pk|k-1是X̂k|k-1对应的协方差，Q是系统过程的协方差，K是卡尔曼增益，R是观测噪声协方差，I是系统单位矩阵。

3 椭球拟合

磁航向测量系统主要包括控制器、磁力计、加速度计和陀螺仪几大部分［10］。磁航向测量系统中的主要测量元件通常由于固定的原因无法和水平面处于同一平面内，与其存在两个夹角［11］：俯仰角和横滚角［12］，其大小由加速度计和陀螺仪共同测得。在同一环境中理想情况下，磁力计测得的x,y,z轴值，在空间中形成一个圆球面，由于磁力计会受到地磁场和周围磁性材料如电机等产生的磁场的影响，磁力计测得的数据在空间中形成一个椭球，但椭球的中心并不是传感器的中心，椭球的各个轴也不是传感器的各个轴［13］。如图1 所示，受到磁场强度影响的磁力计其椭球中心坐标为(x0,y0,z0)，三个轴的半周长分别是a,b,c。因此有必要对磁力计进行校准。过程如下：

图1 存在罗差的水平磁场分布

三维平面下椭球的方程为

为了求取理想情况下的磁力计数据，需要进行椭球拟合并通过最小二乘法求解椭球的六个元素：椭中心中心(x0,y0,z0)、x轴半轴长a、y轴半轴长为b，及z轴半轴长为c。任意椭球在空间坐标系下可表示为

将磁力计让其中心旋转360°得N(N≥5)个采样点(xi,yi,zi)，根据最小二乘法原理可得目标函数为

使得式（9）值最小来求解五个参数，则由式（10）：

可得方程组：

求解线性方程组得A，B,C,D,E,F的值。则椭圆五个参数可由A，B,C,D,E,F得到

4 实验与结果分析

移动机器人如图2 所示采用STM32F103ZET6控制，搭载激光雷达Rplider A1、惯性导航MPU6050 和三轴磁力计（数字罗盘）HMC5883L，在ROS 系统下运行。设加速度计的过程噪声协方差q_angle=0.001 ，陀螺仪的过程噪声协方差q_gyro=0.003 ，加速度计的测量噪声协方差r_angle=0.5 ，采样时间t=0.0002 ，陀螺仪漂移q_bias=0。横滚角卡尔曼滤波效果如图3所示，俯仰角卡尔曼滤波效果如图4 所示，偏航角卡尔曼滤波效果如图5所示。

图2 移动机器人

图3 横滚角卡尔曼滤波

图4 俯仰角卡尔曼滤波

图5 偏航角卡尔曼滤波

采用卡尔曼滤波后姿态角前后数据对比如表1所示。

表1 姿态角误差

采用磁力计测得数据时，理想情况下圆心坐标为(21,-20,20)，三个轴的半长轴分别为a=96，b=90，c=85。采用椭球拟合方法对磁力计进行校准并通过的最小二乘法对其拟合方程求解，得到椭球的六个参数对磁力计进行校正。在校准数据采样的过程中需要通过将磁力计进行旋转一周，通过加入陀螺仪辅助加速度计对磁力计的俯仰角和横滚角的进行测量，椭球拟合效果如图6 所示，前后数据对比如表2所示。

表2 椭球拟合前后数据对比

经过卡尔曼滤波和磁力计校正后，数据误差更小，可以使机器人在室内环境下建图效果更好。机器人在室内环境下地图创建前后效果对比如图7和图8 所示。图中白色区域为自由区，黑色区域为障碍区。由图7~8 对比可以看出校正后的建图效果更好，能清晰地识别出障碍物。

图7 未校正前的室内地图

图8 校正后的室内地图

5 结语

在地图创建过程中，采用卡尔曼滤波的惯性导航融合了姿态角的信号，与以往只通过加速度计来测量俯仰角和横滚角相比，可以有效地减小数据误差，更精确获得姿态角信息，并椭球拟合法校准磁力计，增加了系统的动态响应能力，提高了的环境适应能力，提高了磁航向的测量精度。通过以上方法可以使机器人创建的地图更加精确，对于后续的工作比如机器人定位和路径规划有更好的效果。