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

权利要求书

1.基于机器视觉和惯导融合的移动机器人运动姿态估计方法，其特征在于它的步骤如下：

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

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

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

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

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

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

2.根据权利要求1所述的一种基于机器视觉和惯导融合的移动机器人运动姿态估计方法，其特征在于，所述步骤(1)为：机器人为通用轮式移动机器人，采用捷联式惯性导航仪，其输出为三维线加速度和角速度，采样频率100Hz，置于机器人几何中心位置，惯性导航仪坐标系以垂直地面向下为Z轴方向，机器人行进正前方为X轴方向，机器人正右侧且同时垂直于X轴方向和Z轴方向为Y轴方向，双目立体相机置于机器人正前方中心且无遮挡，俯仰角为30-45度，相机采样频率为1Hz。

3.根据权利要求1所述的一种基于机器视觉和惯导融合的移动机器人运动姿态估计方法，其特征在于，所述步骤(2)为：对双目相机采集的左、右图，提取尺度不变性转换特征，对特征向量匹配后利用视差计算特征点的三维坐标，再通过匹配前后帧的尺度不变性转换特征得到同组特征点帧间的运动三维坐标变化，去除离群值，用最小二乘法求解这组特征点运动方程，得到旋转矩阵R和平移向量T。

4.根据权利要求1所述的一种基于机器视觉和惯导融合的移动机器人运动姿态估计方法，其特征在于，所述步骤(3)为：利用惯性导航仪测量自身坐标系下3个轴向的加速度a_x，a_y，a_z，当无其他明显外力作用时，即机器人处于匀速运动或者静止等稳定状态时，得到

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

5.根据权利要求1所述的一种基于机器视觉和惯导融合的移动机器人运动姿态估 计方法，其特征在于，所述步骤(4)为：利用扩展卡尔曼滤波对视觉运动估计和惯导计算的姿态进行信息融合，定义状态向量x为机器人当前的四元数[e₀，e₁，e₂，e₃]表征的姿态：

x＝[e₀，e₁，e₂，e₃]^T.

卡尔曼滤波预测模型是根据前后帧的图像计算的机器人姿态变化更新机器人运动姿态并用四元数表征

x_k＝q*x_k-1+w(k).

其中x_k、x_k-1分别表示机器人第k帧与第k-1帧的姿态，即对初始位置的旋转，q表示k-1帧到第k帧机器人的旋转，由图像特征点估计的两帧间的旋转矩阵R得到，*为四元数乘法，表示旋转的累积，w(k)表示预测过程的噪声，卡尔曼滤波观测模型根据惯导处于稳定状态时，加速度计输出计算实时的俯仰角θ和侧倾角φ，此外，对航向角的估计，考虑降低累积误差，仅在两图像帧间对惯性导航仪中陀螺输出的角速度积分，得到航向角变化Δψ，再利用姿态角和四元数的转换关系得到测量变量与过程状态的关系即观测模型方程

其中v(k)表示观测过程的高斯噪声。