CN102538781B - 基于机器视觉和惯导融合的移动机器人运动姿态估计方法 - Google Patents

Abstract

本发明公开了一种基于机器视觉和惯导融合的移动机器人运动姿态估计方法，其步骤为：同步采集移动机器人双目相机图像及三轴惯导数据；提取前后帧图像特征并匹配估计运动姿态；利用惯导计算俯仰角和横滚角；建立卡尔曼滤波器模型融合视觉和惯导姿态估计；根据估计方差自适应调整滤波器参数；姿态修正的累积航位推算。本发明提出实时扩展卡尔曼滤波器姿态估计模型，利用惯导结合重力加速度方向作为补充，对视觉里程计三个方向姿态估计进行解耦，修正姿态估计的累积误差；根据运动状态采用模糊逻辑调整滤波器参数，实现自适应的滤波估计，降低加速度噪声的影响，有效的提高了视觉里程计的定位精度和鲁棒性。

Description

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

技术领域

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

背景技术

精确的陆地定位系统在移动机器人的自主导航，路径规划，地形重建等方向都具有很重要的应用。传统的机器人定位方法包括GPS，惯性导航定位等。GPS广泛应用于车辆定位，但遮挡区域和室内无法应用；惯导(inertial measurementunit，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，当无其他明显外力作用时，即机器人处于匀速运动或者静止等稳定状态时，得到

$\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]=-g\left[\begin{array}{c}-\sin \mathrm{\θ}\\ \sin \mathrm{\φ}\cos \mathrm{\θ}\\ \cos \mathrm{\φ}\cos \mathrm{\θ}\end{array}\right].$

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

所述步骤(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)表示预测过程的噪声，卡尔曼滤波观测模型根据惯导处于稳定状态时，加速度计输出计算实时的俯仰角θ和侧倾角φ，此外，对航向角的估计，考虑降低累积误差，仅在两图像帧间对惯性导航仪中陀螺输出的角速度积分，得到航向角变化Δψ，再利用姿态角和四元数的转换关系得到测量变量与过程状态的关系即观测模型方程

$z_k=\left[\begin{array}{c}\mathrm{\φ}\\ \mathrm{\θ}\\ \mathrm{\Δ\ψ}\end{array}\right]=h\left(x_k\right)=\left[\begin{array}{c}\arctan \left[\frac{2(e_0{e}_1+e_2{e}_3)}{e_0^2-e_1^2-e_2^2+e_3^2}\right]\\ \arcsin \left[2(e_0{e}_2-e_3{e}_1)\right]\\ \arctan \left[\frac{2(e_0{e}_3+e_1{e}_2)}{e_0^2+e_1^2-e_2^2-e_3^2}\right]-{\mathrm{\ψ}}_{k-1}\end{array}\right]+v\left(k\right).$

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

所述步骤(5)为：采用模糊自适应的卡尔曼滤波器参数是根据机器人运行的状态，有导向性的调整观测噪声的协方差矩阵，从而使滤波结果不发生偏移，判断机器人动态的标准为惯性导航仪输出的角速度和线加速度，定义归一化的加速度α和角速率β如下

α＝(‖a‖-g)/g，β＝‖ω‖.

其中g为重力加速度，a为惯性导航仪输出的线加速度向量，ω为输出的角速度向量，定义以下3个模糊规则，α和β的阈值确定一组模糊集，反映车体状态，考虑陀螺和加速度计的噪声影响，本方法中α、β的阈值分别为2mg、0.2°/s：

1.α≤Threshold(α)且β≤Threshold(β)，则z＝0

2.α＞Threshold(α)或β＞Threshold(β)，则z＝a₁α+a₂β

3.α＞Threshold(α)且β＞Threshold(β)，则z＝1

a₁，a₂为表征归一化的加速度和角速度的权重系数，用以上的模糊规则，定义如下观测噪声v(k)的协方差矩阵R

$R=\left(\begin{array}{ccc}(k_{1}z+k_2){\mathrm{\σ}}_{\mathrm{\φ}}^2& 0& 0\\ 0& (k_{1}z+k_2){\mathrm{\σ}}_{\mathrm{\θ}}^2& 0\\ 0& 0& {\mathrm{\σ}}_{\mathrm{\ψ}}^2\end{array}\right).$

σ_θ、σ_ψ为俯仰角θ、侧倾角φ和航向角变化Δψ的观测噪声方差，k₁和k₂为经验值，且k₁，k₂∈(0，1)，式中k₁＝0.7，k₂＝0.5，归一化的加速度和角速度的权重系数a₁＝3.5，a₂＝8，当车体加速度或者角速度超过阈值时，系统认为车体处于非平稳状态，从而调整观测值θ和φ的方差增加，卡尔曼增益随之降低，达到滤波器参数自适应的目的。

所述步骤(6)为：根据计算前后帧姿态变化的旋转矩阵R和平移矩阵T估计机器人在任意时刻的位置姿态信息，经过卡尔曼滤波用惯导数据融合更新的R矩阵解决了因航位推算中姿态累积误差问题，继而用下式对机器人当前位置进行推算：

$P_n^1=-\underset{k=1}{\overset{n-1}{\mathrm{\Σ}}}\left(\underset{m=1}{\overset{k-1}{\mathrm{\Π}}}{\left(R_m\right)}^{-1}\right)T_k.$

式中，若以相机初始坐标系为导航坐标系，则表示第n帧的相机坐标，R_m、T_m分别表示相机第m到第m+1帧的旋转和平移矩阵。

本发明通过利用惯导以重力加速度作为参考，建立卡尔曼滤波器模型对视觉运动姿态估计进行修正，降低了姿态估计的累积误差，提供了一种精确的机器人定位方法。能够广泛应用于机器人局部定位，尤其是GPS遮挡区以及室内环境。本发明与一些传统的惯性导航和里程计算技术相比，具有定位精度高，实时性强，实用和推广性高等特点。

附图说明

图1是基于机器视觉和惯导融合的移动机器人运动姿态估计方法的流程图；

图2是本发明实施例中用于室外机器人定位的一组轨迹图；

图3是本发明实施例中用于室外机器人定位的误差分析图。

具体实施方式

以下结合实施例进一步说明本发明方法。

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

(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，当无其他明显外力作用时，即机器人处于匀速运动或者静止等稳定状态时，得到

$\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]=-g\left[\begin{array}{c}-\sin \mathrm{\θ}\\ \sin \mathrm{\φ}\cos \mathrm{\θ}\\ \cos \mathrm{\φ}\cos \mathrm{\θ}\end{array}\right].$

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

所述步骤(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)表示预测过程的噪声，卡尔曼滤波观测模型根据惯导处于稳定状态时，加速度计输出计算实时的俯仰角θ和侧倾角φ，此外，对航向角的估计，考虑降低累积误差，仅在两图像帧间对惯性导航仪中陀螺输出的角速度积分，得到航向角变化Δψ，再利用姿态角和四元数的转换关系得到测量变量与过程状态的关系即观测模型方程

$z_k=\left[\begin{array}{c}\mathrm{\φ}\\ \mathrm{\θ}\\ \mathrm{\Δ\ψ}\end{array}\right]=h\left(x_k\right)=\left[\begin{array}{c}\arctan \left[\frac{2(e_0{e}_1+e_2{e}_3)}{e_0^2-e_1^2-e_2^2+e_3^2}\right]\\ \arcsin \left[2(e_0{e}_2-e_3{e}_1)\right]\\ \arctan \left[\frac{2(e_0{e}_3+e_1{e}_2)}{e_0^2+e_1^2-e_2^2-e_3^2}\right]-{\mathrm{\ψ}}_{k-1}\end{array}\right]+v\left(k\right).$

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

所述步骤(5)为：采用模糊自适应的卡尔曼滤波器参数是根据机器人运行的状态，有导向性的调整观测噪声的协方差矩阵，从而使滤波结果不发生偏移，判断机器人动态的标准为惯性导航仪输出的角速度和线加速度，定义归一化的加速度α和角速率β如下

α＝(‖a‖-g)/g，β＝‖ω‖.

其中g为重力加速度，a为惯性导航仪输出的线加速度向量，ω为输出的角速度向量，定义以下3个模糊规则，α和β的阈值确定一组模糊集，反映车体状态，考虑陀螺和加速度计的噪声影响，本方法中α、β的阈值分别为2mg、0.2°/s：

1.α≤Threshold(α)且β≤Threshold(β)，则z＝0

2.α＞Threshold(α)或β＞Threshold(β)，则z＝a₁α+a₂β

3.α＞Threshold(α)且β＞Threshold(β)，则z＝1

a₁，a₂为表征归一化的加速度和角速度的权重系数，用以上的模糊规则，定义如下观测噪声v(k)的协方差矩阵R

$R=\left(\begin{array}{ccc}(k_{1}z+k_2){\mathrm{\σ}}_{\mathrm{\φ}}^2& 0& 0\\ 0& (k_{1}z+k_2){\mathrm{\σ}}_{\mathrm{\θ}}^2& 0\\ 0& 0& {\mathrm{\σ}}_{\mathrm{\ψ}}^2\end{array}\right).$

σ_θ、σ_ψ为俯仰角θ、侧倾角φ和航向角变化Δψ的观测噪声方差，k₁和k₂为经验值，且k₁，k₂∈(0，1)，式中k₁＝0.7，k₂＝0.5，归一化的加速度和角速度的权重系数a₁＝3.5，a₂＝8，当车体加速度或者角速度超过阈值时，系统认为车体处于非平稳状态，从而调整观测值θ和φ的方差增加，卡尔曼增益随之降低，达到滤波器参数自适应的目的。

所述步骤(6)为：根据计算前后帧姿态变化的旋转矩阵R和平移矩阵T估计机器人在任意时刻的位置姿态信息，经过卡尔曼滤波用惯导数据融合更新的R矩阵解决了因航位推算中姿态累积误差问题，继而用下式对机器人当前位置进行推算：

$P_n^1=-\underset{k=1}{\overset{n-1}{\mathrm{\Σ}}}\left(\underset{m=1}{\overset{k-1}{\mathrm{\Π}}}{\left(R_m\right)}^{-1}\right)T_k.$

式中，若以相机初始坐标系为导航坐标系，则表示第n帧的相机坐标，R_m、T_m分别表示相机第m到第m+1帧的旋转和平移矩阵。

实施例

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

采用移动机器人先锋3(Pioneer 3)、耐威NV100型捷联式惯性导航仪和Bumblebee2双目立体相机。其中惯导采样频率为100Hz，置于机器人几何中心位置，以垂直地面向下为Z轴方向，机器人行进正前方为X轴方向，机器人正右侧且同时垂直于X、Z轴的方向为Y轴方向；双目立体相机置于机器人正前方，俯仰角为45度，相机采样频率为1Hz。

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

对双目相机采集的左、右图，提取尺度不变性转换(scale-invariant featuretransform，SIFT)特征，包括尺度空间求极值，确定特征点位置和尺度，计算特征点方向，生成特征点描述符。再根据提取出的描述符进行特征匹配，用特征向量的欧式距离来判定特征间的相似性，即对于一幅图像中的某个特征点，找出另一幅图像中与其欧氏距离最近的前两个特征点，如果最近距离与次近距离之比少于某个阈值，则接受这一对匹配点。降低这个比例阈值，匹配点数目会减少，但更加稳定。欧氏距离最近点的查找可以使用k-d树算法。匹配后利用视差原理计算特征点的三维坐标，再通过匹配前后帧的SIFT特征得到同组特征点帧间的运动三维坐标变化，用Ransac去除离群值，结合最小二乘法求解得到旋转和平移等运动参数。

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

由于三维惯导中陀螺估计的姿态角受到累积误差的影响，因此仅利用惯导测量自身坐标系下3个方向的加速度，当仅重力作用时，得到

$\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]=-g\left[\begin{array}{c}-\sin \mathrm{\θ}\\ \sin \mathrm{\φ}\cos \mathrm{\θ}\\ \cos \mathrm{\φ}\cos \mathrm{\θ}\end{array}\right].$

根据惯导输出的加速度a_x，a_y，a_z以重力加速度作为垂直方向参考，计算机器人实时的俯仰角θ和侧倾角φ。

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

利用扩展卡尔曼滤波对视觉运动估计和惯导计算的姿态进行信息融合。定义状态向量为机器人当前的姿态四元数：

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

$z_k=\left[\begin{array}{c}\mathrm{\φ}\\ \mathrm{\θ}\\ \mathrm{\Δ\ψ}\end{array}\right]=h\left(x_k\right)=\left[\begin{array}{c}\arctan \left[\frac{2(e_0{e}_1+e_2{e}_3)}{e_0^2-e_1^2-e_2^2+e_3^2}\right]\\ \arcsin \left[2(e_0{e}_2-e_3{e}_1)\right]\\ \arctan \left[\frac{2(e_0{e}_3+e_1{e}_2)}{e_0^2+e_1^2-e_2^2-e_3^2}\right]-{\mathrm{\ψ}}_{k-1}\end{array}\right]+v\left(k\right).$

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

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

采用模糊自适应的卡尔曼滤波器参数是根据机器人运行的状态，有导向性的调整观测噪声的协方差矩阵，从而使滤波结果不发生偏移。判断机器人动态的标准为IMU输出的角速度和线加速度。定义归一化的加速度α和角速率β如下

α＝(‖a‖-g)/g，β＝‖ω‖.

定义以下3个模糊规则(Fuzzy rules)，α和β的阈值确定一组模糊集，反映车体状态，考虑陀螺和加速度计的噪声影响，实验中α、β的阈值分别为2mg、0.2°/s：

1.α≤Threshold(α)且β≤Threshold(β)，则z＝0

2.α＞Threshold(α)或β＞Threshold(β)，则z＝a₁α+a₂β

3.α＞Threshold(α)且β＞Threshold(β)，则z＝1

a₁，a₂为表征归一化的加速度和角速度的权重系数，用以上的模糊规则，定义如下观测噪声协方差矩阵

$R=\left(\begin{array}{ccc}(k_{1}z+k_2){\mathrm{\σ}}_{\mathrm{\φ}}^2& 0& 0\\ 0& (k_{1}z+k_2){\mathrm{\σ}}_{\mathrm{\θ}}^2& 0\\ 0& 0& {\mathrm{\σ}}_{\mathrm{\ψ}}^2\end{array}\right).$

σ_θ、σ_ψ为对应的观测噪声方差，k₁和k₂为经验值，且k₁，k₂∈(0，1)，本方法中k₁＝0.7，k₂＝0.5，归一化的加速度和角速度的权重系数a₁＝3.5，a₂＝8。σ_ψ由于不受到加速度计测量的影响，无需调整。当移动机器人加速度或者角速度超过阈值时，系统认为车体处于非平稳状态，从而调整观测值θ和φ的方差增加，卡尔曼增益随之降低，达到滤波器参数自适应的目的。

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

根据计算前后帧姿态变化的旋转矩阵R和平移矩阵T估计机器人在任意时刻的位置姿态信息，经过卡尔曼滤波用惯导数据融合更新的R矩阵解决了因航位推算中姿态累积误差问题，继而用下式对机器人当前位置进行推算：

$P_n^1=-\underset{k=1}{\overset{n-1}{\mathrm{\Σ}}}\left(\underset{m=1}{\overset{k-1}{\mathrm{\Π}}}{\left(R_m\right)}^{-1}\right)T_k.$

式中，若以相机初始坐标系为导航坐标系，则表示第n帧的相机坐标，R_m、T_m分别表示相机第m到第m+1帧的旋转和平移矩阵。如图2，移动机器人移动一个环形区域，总共采集图像帧218帧，航位推算得到的轨迹图，包括仅利用视觉方法定位(VO)的结果以及本方法的定位轨迹图(VO+IMU)。并且实施例中用毫米级定位精度的全站仪(Total Station)作为定位真值参考验证本方法的精度，如图3的误差分析图。

Claims (4)

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

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

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

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

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

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

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

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

$z_k=\left[\begin{array}{c}\mathrm{\φ}\\ \mathrm{\θ}\\ \mathrm{\Δ\ψ}\end{array}\right]=h\left(x_k\right)=\left[\begin{array}{c}\arctan \left[\frac{2(e_0{e}_1+e_2{e}_3)}{e_0^2-e_1^2-e_2^2+e_3^1}\right]\\ \arcsin \left[2(e_0{e}_2-e_3{e}_1)\right]\\ \arctan \left[\frac{2(e_0{e}_3+e_1{e}_2)}{e_0^2+e_1^2-e_2^2-e_3^2}\right]-{\mathrm{\ψ}}_{k-1}\end{array}\right]+v\left(k\right)$

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

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

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

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

$\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]=-g\left[\begin{array}{c}-\sin \\ \sin \mathrm{\φ}\cos \mathrm{\θ}\\ \cos \mathrm{\φ}\cos \mathrm{\θ}\end{array}\right]$

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