[发明专利]基于机器视觉和惯导融合的移动机器人运动姿态估计方法

说明书

技术领域

本发明涉及一种基于机器视觉和惯导融合的移动机器人运动姿态估计方法，适用于自主机器人移动姿态估计及定位。

背景技术

精确的陆地定位系统在移动机器人的自主导航，路径规划，地形重建等方向都具有很重要的应用。传统的机器人定位方法包括GPS，惯性导航定位等。GPS广泛应用于车辆定位，但遮挡区域和室内无法应用；惯导(inertial measurement unit，IMU)通过积分角速度和线加速度实现航位推算，却易受到噪声影响导致定位结果“漂移”。此外，越来越多的研究将视觉定位方法作为传统定位方法的补充。例如视觉里程计(visual odometry，VO)，通过跟踪前后帧图像上的相同特征点，估计车体运动姿态的六个自由度。它适用于室内外、陆地及星际等多种环境，且定位精度高。但对图像序列质量要求高，图像中的移动物体、阴影都会影响其运动估计的准确性。

发明内容

本发明的目的是提供一种精度高、鲁棒，适用性广的基于机器视觉和惯导融合的移动机器人运动姿态估计方法。克服了传统航位推算由于累积误差造成的精度问题。

基于机器视觉和惯导融合的移动机器人运动姿态估计方法的步骤如下：

(1)同步采集移动机器人双目相机图像及三轴惯导数据；

(2)提取前后帧图像特征并匹配估计运动姿态；

(3)利用惯导计算俯仰角和横滚角；

(4)建立卡尔曼滤波器模型融合视觉和惯导姿态估计；

(5)根据估计方差自适应调整滤波器参数；

(6)姿态修正的累积航位推算。

所述步骤(1)为：机器人为通用轮式移动机器人，采用捷联式惯性导航仪，其输出为三维线加速度和角速度，采样频率100Hz，置于机器人几何中心位置，惯性导航仪坐标系以垂直地面向下为Z轴方向，机器人行进正前方为X轴方向，机器人正右侧且同时垂直于X轴方向和Z轴方向为Y轴方向，双目立体相机置于机器人正前方中心且无遮挡，俯仰角为30-45度，相机采样频率为1Hz。

所述步骤(2)为：对双目相机采集的左、右图，提取尺度不变性转换特征，对特征向量匹配后利用视差计算特征点的三维坐标，再通过匹配前后帧的尺度不变性转换特征得到同组特征点帧间的运动三维坐标变化，去除离群值，用最小二乘法求解这组特征点运动方程，得到旋转矩阵R和平移向量T。

所述步骤(3)为：利用惯性导航仪测量自身坐标系下3个轴向的加速度a_x，a_y，a_z，当无其他明显外力作用时，即机器人处于匀速运动或者静止等稳定状态时，得到

axayaz=-g-sinθsinφcosθcosφcosθ.]]>

其中g为重力加速度，根据惯导输出的加速度a_x，a_y，a_z，可计算机器人实时的俯仰角θ和侧倾角φ。

所述步骤(4)为：利用扩展卡尔曼滤波对视觉运动估计和惯导计算的姿态进行信息融合，定义状态向量x为机器人当前的四元数[e₀，e₁，e₂，e₃]表征的姿态：