CN108507572A - 一种基于mems惯性测量单元的姿态定位误差修正方法 - Google Patents

Abstract

一种基于MEMS惯性测量单元的姿态定位误差修正方法，涉及一种机器人曲面运动的姿态定位方法。该方法首先将MEMS惯性测量单元固定在待测载体上，采集MEMS惯性测量单元输出的三轴角速度和三轴加速度，然后控制载体在工作平面上运动2～10分钟后，保持静止状态30～180秒，并循环执行，进行运动全过程MEMS惯性测量单元陀螺仪漂移估计和补偿；采用余弦矩阵法计算姿态矩阵，在载体静止状态下利用三轴的加速度更新横滚角和俯仰角，实现机器人姿态定位。本发明的方法在不增加任何硬件成本并且不改变硬件安装结构的情况下，通过调整载体运动速度即可有效的提高姿态定位精度，特别适用于在室内、强磁干扰等作业环境下的机器人曲面运动姿态定位中的应用。

Description

一种基于MEMS惯性测量单元的姿态定位误差修正方法

技术领域

本发明属于惯性导航技术领域，特别涉及一种机器人曲面运动的姿态定位方法，适合于移动机器人在室内、强磁干扰等作业环境下利用MEMS惯性测量单元进行姿态定位。

背景技术

MEMS惯性测量单元以多轴方式组合陀螺仪、加速度计和磁力计等传感器，在动态环境下提供导航和姿态数据，被广泛应用在移动机器人、无人机、航天器等需要对运动物体进行姿态测量和控制的领域。

惯性导航法是一种常见的姿态测量方法。通过对MEMS惯性测量单元陀螺仪所测量的三轴运动角速度进行积分，能够计算得到姿态信息。MEMS惯性测量单元陀螺仪内部由微纳米尺寸下加工成的微机械元件组成，由于尺寸、体积非常小，极易受到环境因素的影响，特别是温度变化对其精度影响较大。MEMS陀螺仪的确定性误差主要包括常值零偏、刻度因素非线性误差、刻度因素温度漂移误差与三轴非正交误差。在去除常值漂移和噪声后，随机漂移对信号本身仍然会造成累积误差。实际中的随机漂移的均值和方差都会因温度等工作环境变化随时间缓慢地发生变化。长时间累积计算会产生较大误差，需要利用其它传感器对陀螺仪漂移进行估计和误差补偿。

已有的姿态估计方法，需要通过磁力计或GPS等外部辅助传感器对姿态定位误差进行修正和补偿。当作业环境中磁力计和GPS信号因强磁干扰、室内等因素而不可靠时，这些附加传感器以及相关的误差修正方法的应用受到限制。

通过旋转惯性测量单元能够实现不依赖外界传感器而估计陀螺仪漂移并减小姿态定位误差。已有的单轴或多轴旋转惯性器件的装置及方法，如专利“一种双轴旋转光纤捷联惯性导航装置”(CN102980578)，通过匀速旋转惯性测量单元将误差调制成周期变化的形式，在导航解算过程中利用积分运算将误差抵消来减小系统误差的积累。专利“基于惯性测量单元旋转的陀螺漂移估计补偿方法及装置”(CN103712622A)通过旋转陀螺仪获取不同姿态下加速度计输出的倾角计算陀螺仪漂移并实时补偿，这类方法需要加速度计测量值等于重力，系统匀速或低速运动。旋转惯性测量单元的方法需要设计对应的旋转结构和控制系统，对旋转装置的安装精度和控制有很高的要求，提高了运动姿态定位的成本。

综上所述，目前减小姿态定位中陀螺仪漂移引起的姿态定位误差主要依靠磁力计或GPS等外部传感器辅助的方法。当作业环境中磁力计和GPS信号不可靠时，旋转惯性测量单元的方法需要额外的硬件结构。因此，不依赖除惯性测量单元外的传感器和额外硬件结构的姿态定位方法具有工程应用价值。在一些复杂的应用环境如水轮机叶片表面修焊作业的机器人，无法使用有效的外部信息源(如GPS，磁场方向等等)。在不增加系统成本的情况下，充分利用惯性元器件信息提高机器人姿态定位精度，对很多应用场合具有重要意义。

发明内容

本发明的目的是针对已有技术的不足之处，提供一种基于MEMS惯性测量单元的姿态定位误差修正方法，以提高室内、磁干扰等环境下长时间的姿态估计精度。

为实现上述目的，本发明的技术方案如下：

本发明所述的一种基于MEMS惯性测量单元的姿态定位误差修正方法，该方法包括如下步骤：

1)定义γ为横滚角，θ为俯仰角，ψ为航向角，在载体运动的起始位置建立固定于地面的导航坐标系o_n-x_ny_nz_n；在MEMS惯性测量单元上建立惯性测量单元坐标系o_b-x_by_bz_b；将导航坐标系o_n-x_ny_nz_n绕其自身z_n轴转动-ψ角得到坐标系o-x₃y₃z₃，将坐标系o-x₃y₃z₃绕其自身x₃轴转动θ角得到坐标系o-x₂y₂z₂，将坐标系o-x₂y₂z₂再绕其自身y₂轴转γ角得到惯性测量单元坐标系o_b-x_by_bz_b；从导航坐标系o_n-x_ny_nz_n到惯性测量单元坐标系o_b-x_by_bz_b的转换矩阵C就是惯性测量单元的姿态矩阵，同时定义惯性测量单元初始姿态矩阵C₀为单位矩阵I＝diag(1,1,1)：

将MEMS惯性测量单元固连在待测载体上任意位置，使惯性测量单元坐标系y_b轴沿着载体正前方，z_b轴沿着竖直向上方向，保持载体静止t₀分钟，t₀∈(0.5,3]，记录静止过程中MEMS惯性测量单元输出的三轴角速度分别为三轴加速度分别为其中k代表角速度和加速度的采集序数，采用下式(2)估算t₀/2时刻MEMS惯性测量单元的常值漂移值

其中p是载体静止状态t₀分钟内记录的三轴角速度最大采集序数；

2)使待测载体沿着规划的路径运动，记录运动过程中MEMS惯性测量单元输出的三轴角速度分别为Ω_x,Ω_y,Ω_z，输出的三轴加速度分别为a_x,a_y,a_z，当载体连续运动t_spa分钟时，t_spa∈[2,10]，载体减速至静止状态；

3)继续保持载体静止状态t₀分钟，记录载体静止过程中MEMS惯性测量单元输出的三轴角速度和三轴加速度采用式(3)计算静止时MEMS惯性测量单元在t_M时刻输出的三轴角速度的常值漂移值其中M代表步骤3)执行的次数，取正整数且M<20，采用式(4)计算待测载体的横滚角γ和俯仰角θ；

式(4)中，A_x,A_y,A_z分别是MEMS惯性测量单元在载体静止时间内输出的三轴加速度均值，其中p是载体静止状态t₀分钟内记录的三轴加速度最大采集序数；

4)根据步骤3)获得的MEMS惯性测量单元在t_M时刻三轴角速度的常值漂移值采用式(5)的模型对载体运动全过程中MEMS惯性测量单元在任一时刻t输出的三轴角速度的常值漂移值ξ_x(t),ξ_y(t),ξ_z(t)进行直线拟合，采用式(6)计算MEMS惯性测量单元输出真实值Ω(t)，根据式(1)和式(7)计算姿态矩阵C_q，其中q表示姿态矩阵的迭代次数；

C_q＝exp(-Ω(t)·Δt)·C_q-1 (7)

式(5)中a_x,a_y,a_z,b_x,b_y,b_z是直线拟合参数，式(6)中Ω_x,Ω_y,Ω_z分别为MEMS惯性测量单元输出的三轴角速度，式(7)中Δt表示MEMS惯性测量单元采样时间间隔；

根据式(1)和式(8)计算出载体三个姿态角：

其中C₁₃代表姿态矩阵C_q的第1行第3列的元素，C₂₁代表姿态矩阵C_q的第2行第1列的元素，C₂₂代表姿态矩阵C_q的第2行第2列的元素，C₂₃代表姿态矩阵C_q的第2行第3列的元素，C₃₃代表姿态矩阵C_q的第3行第3列的元素；

5)重复步骤2)、步骤3)和步骤4)，直至载体工作过程结束；

6)当载体连续运动时间未满t_spa分钟因工作任务需要停车时，执行步骤2)，将MEMS惯性测量单元输出三轴角速度值置零，即Ω_x＝Ω_y＝Ω_z＝0。

上述技术方案中，其特征在于，步骤4)所述拟合方法可采用最小二乘法、拉格朗日插值法、牛顿插值法、牛顿迭代法、区间二分法、弦截法、雅克比迭代法或牛顿科特斯数值积分法。优选采用最小二乘法。

本发明与现有技术相比，具有以下优点及突出性的技术效果：

本发明利用载体运动中多次零角速度条件修正MEMS惯性测量单元角速度测量漂移的方法实现了减小运动姿态定位误差，尤其是航向角的定位误差，相比常用的以磁强计或GPS辅助修正姿态定位误差的方法，不受室内、磁干扰等环境的限制，特别适用于在室内、强磁干扰等作业环境下的机器人曲面运动姿态定位中的应用。相比旋转惯性测量单元的方法，本发明不增加任何硬件成本并且不改变硬件安装结构，通过调整载体运动速度即可有效的提高姿态定位精度。

附图说明

图1是本发明方法的流程示意图。

图2是本发明方法导航坐标系与惯性测量单元坐标系转换示意图。

图3是载体运动全过程MEMS惯性测量单元X轴角速度输出曲线。

图4是载体运动全过程MEMS惯性测量单元Y轴角速度输出曲线。

图5是载体运动全过程MEMS惯性测量单元Z轴角速度输出曲线。

图6是载体静止时MEMS惯性测量单元X轴角速度输出曲线。

图7是载体静止时MEMS惯性测量单元Y轴角速度输出曲线。

图8是载体静止时MEMS惯性测量单元Z轴角速度输出曲线。

图9是第60min时在不同连续运动时间t_spa下的姿态误差修正前后对比曲线。

图2中，1-MEMS惯性测量单元；2-载体运动的起始位置。

具体实施方式

下面结合附图和实施例对本发明做更详细地描述。

图1示意了本发明提供的一种基于MEMS惯性测量单元的姿态定位误差修正方法的流程图。该方法包括以下步骤：

1)定义γ为横滚角，θ为俯仰角，ψ为航向角，在载体运动的起始位置建立固定于地面的导航坐标系o_n-x_ny_nz_n；在MEMS惯性测量单元上建立惯性测量单元坐标系o_b-x_by_bz_b；将导航坐标系o_n-x_ny_nz_n绕其自身z_n轴转动-ψ角得到坐标系o-x₃y₃z₃，将坐标系o-x₃y₃z₃绕其自身x₃轴转动θ角得到坐标系o-x₂y₂z₂，将坐标系o-x₂y₂z₂再绕其自身y₂轴转γ角得到坐标系o-x_by_bz_b，通过平移坐标系o-x_by_bz_b可以得到惯性测量单元坐标系o_b-x_by_bz_b如图2所示；从导航坐标系o_n-x_ny_nz_n到惯性测量单元坐标系o_b-x_by_bz_b的转换矩阵C就是惯性测量单元的姿态矩阵，同时定义惯性测量单元初始姿态矩阵C₀为单位矩阵I＝diag(1,1,1)：

将MEMS惯性测量单元固连在待测载体上任意位置，使惯性测量单元坐标系y_b轴沿着载体正前方，z_b轴沿着竖直向上方向，保持载体静止t₀分钟，t₀∈(0.5,3]，本例中t₀＝1，记录静止过程中MEMS惯性测量单元输出的三轴角速度分别为三轴加速度分别为其中k代表角速度和加速度的采集序数，采用下式(2)估算t₀/2时刻MEMS惯性测量单元的常值漂移值

其中p是载体静止状态t₀分钟内记录的三轴角速度最大采集序数；

2)使载体沿着规划的路径运动，记录运动过程中MEMS惯性测量单元输出的三轴角速度分别为Ω_x,Ω_y,Ω_z，输出的三轴加速度分别为a_x,a_y,a_z；载体运动全过程中MEMS惯性测量单元的三轴角速度输出值如图3、图4和图5所示，图中三轴角速度输出值接近零的部分为载体静止过程中惯性测量单元输出值，其余部分为载体运动过程中惯性测量单元输出值；当载体连续运动t_spa分钟时，t_spa∈[2,10]，载体减速至静止状态；

3)继续保持载体静止状态t₀分钟，记录载体静止过程中MEMS惯性测量单元输出的三轴角速度和三轴加速度载体在静止时间段内MEMS惯性测量单元的三轴角速度输出值如图6、图7和图8所示，MEMS惯性测量单元的测量模型包含两类传感器误差，一类是波动激烈的测量白噪声，是AD转换器件引起的外部噪声，一类是变化缓慢的漂移，是传感器内部机械、温度等各种物理因素产生的传感器内部误差的综合参数。短时间内数据变化缓慢的漂移，可以用长时间内输出数据拟合的一次多项式或一阶傅里叶式表示；采用式(3)计算静止时MEMS惯性测量单元在t_M时刻输出的三轴角速度的常值漂移值其中M代表步骤3)执行的次数，取正整数且M<20，采用式(4)计算待测载体的横滚角γ和俯仰角θ；

式(4)中，A_x,A_y,A_z分别是MEMS惯性测量单元在载体静止时间内输出的三轴加速度均值，其中p是载体静止状态t₀分钟内记录的三轴加速度最大采集序数；

4)根据步骤3)获得的MEMS惯性测量单元在t_M时刻三轴角速度的常值漂移值i＝1,2,3...,M，采用式(5)的模型对载体运动全过程中MEMS惯性测量单元在任一时刻t输出的三轴角速度的常值漂移值ξ_x(t),ξ_y(t),ξ_z(t)进行直线拟合，所述直线拟合方法包括最小二乘法、拉格朗日插值法、牛顿插值法、牛顿迭代法、区间二分法、弦截法、雅克比迭代法或牛顿科特斯数值积分法，本例中采用最小二乘法；采用式(6)计算MEMS惯性测量单元输出真实值Ω(t)，根据式(1)和式(7)计算姿态矩阵C_q，其中q表示姿态矩阵的迭代次数；

C_q＝exp(-Ω(t)·Δt)·C_q-1 (7)

式(5)中a_x,a_y,a_z,b_x,b_y,b_z是直线拟合参数，式(6)中Ω_x,Ω_y,Ω_z分别为MEMS惯性测量单元输出的三轴角速度，式(7)中Δt表示MEMS惯性测量单元采样时间间隔；

根据式(1)和式(8)计算出载体三个姿态角：

其中C₁₃代表姿态矩阵C_q的第1行第3列的元素，C₂₁代表姿态矩阵C_q的第2行第1列的元素，C₂₂代表姿态矩阵C_q的第2行第2列的元素，C₂₃代表姿态矩阵C_q的第2行第3列的元素，C₃₃代表姿态矩阵C_q的第3行第3列的元素；

5)重复步骤2)、步骤3)和步骤4)，直至载体工作过程结束；本例中载体工作总时间为1小时，通过本发明所述的一种基于MEMS惯性测量单元姿态定位的零速修正方法修正后姿态定位误差得到了抑制，如图9所示，横坐标对应不同的t_spa，纵坐标是第60分钟时三轴姿态角定位误差均值，在固定载体静止时间t₀＝1的条件下，通过测试不同的载体连续运动时间t_spa，在载体工作总时间达到60分钟时，图9显示t_spa取值在3～8之间，本发明方法能够修正姿态定位误差，t_spa取值越小，姿态定位误差修正效果越好；

6)当载体连续运动时间未满t_spa分钟因工作任务需要停车时，执行步骤2)，将MEMS惯性测量单元输出三轴角速度值置零，即Ω_x＝Ω_y＝Ω_z＝0；

本实例中，所述的MEMS惯性测量单元采用ADI亚德诺半导体公司的MEMS惯性测量单元ADIS16488。

上述实施例仅用于说明本发明，根据MEMS惯性测量单元的漂移特性的不同，MEMS惯性测量单元的漂移拟合模型和姿态矩阵计算方法都是可以有所变化的。

Claims (3)

1.一种基于MEMS惯性测量单元的姿态定位误差修正方法，其特征在于该方法包括以下步骤：

1)定义γ为横滚角，θ为俯仰角，ψ为航向角；在载体运动的起始位置建立固定于地面的导航坐标系o_n-x_ny_nz_n；在MEMS惯性测量单元上建立惯性测量单元坐标系o_b-x_by_bz_b；将导航坐标系o_n-x_ny_nz_n绕其自身z_n轴转动-ψ角得到坐标系o-x₃y₃z₃，将坐标系o-x₃y₃z₃绕其自身x₃轴转动θ角得到坐标系o-x₂y₂z₂，将坐标系o-x₂y₂z₂再绕其自身y₂轴转γ角得到惯性测量单元坐标系o_b-x_by_bz_b；从导航坐标系o_n-x_ny_nz_n到惯性测量单元坐标系o_b-x_by_bz_b的转换矩阵C就是惯性测量单元的姿态矩阵，同时定义惯性测量单元初始姿态矩阵C₀为单位矩阵I＝diag(1,1,1)：

将MEMS惯性测量单元固连在待测载体上任意位置，使惯性测量单元坐标系y_b轴沿着载体正前方，z_b轴沿着竖直向上方向，保持载体静止t₀分钟，t₀∈(0.5,3]，记录静止过程中MEMS惯性测量单元输出的三轴角速度分别为三轴加速度分别为其中k代表角速度和加速度的采集序数，采用下式(2)估算t₀/2时刻MEMS惯性测量单元的常值漂移值

其中p是载体静止状态t₀分钟内记录的三轴角速度最大采集序数；

2)使待测载体沿着规划的路径运动，记录运动过程中MEMS惯性测量单元输出的三轴角速度分别为Ω_x,Ω_y,Ω_z，输出的三轴加速度分别为a_x,a_y,a_z，当载体连续运动t_spa分钟时，t_spa∈[2,10]，载体减速至静止状态；

3)继续保持载体静止状态t₀分钟，记录载体静止过程中MEMS惯性测量单元输出的三轴角速度和三轴加速度采用式(3)计算静止时MEMS惯性测量单元在t_M时刻输出的三轴角速度的常值漂移值其中M代表步骤3)执行的次数，取正整数且M<20，采用式(4)计算待测载体的横滚角γ和俯仰角θ；

式(4)中，A_x,A_y,A_z分别是MEMS惯性测量单元在载体静止时间内输出的三轴加速度均值，其中p是载体静止状态t₀分钟内记录的三轴加速度最大采集序数；

4)根据步骤3)获得的MEMS惯性测量单元在t_M时刻三轴角速度的常值漂移值采用式(5)的模型对载体运动全过程中MEMS惯性测量单元在任一时刻t输出的三轴角速度的常值漂移值ξ_x(t),ξ_y(t),ξ_z(t)进行直线拟合，采用式(6)计算MEMS惯性测量单元输出真实值Ω(t)，根据式(1)和式(7)计算姿态矩阵C_q，其中q表示姿态矩阵的迭代次数；

C_q＝exp(-Ω(t)·Δt)·C_q-1 (7)

式(5)中a_x,a_y,a_z,b_x,b_y,b_z是直线拟合参数，式(6)中Ω_x,Ω_y,Ω_z分别为MEMS惯性测量单元输出的三轴角速度，式(7)中Δt表示MEMS惯性测量单元采样时间间隔；

根据式(1)和式(8)计算出载体三个姿态角：

其中C₁₃代表姿态矩阵C_q的第1行第3列的元素，C₂₁代表姿态矩阵C_q的第2行第1列的元素，C₂₂代表姿态矩阵C_q的第2行第2列的元素，C₂₃代表姿态矩阵C_q的第2行第3列的元素，C₃₃代表姿态矩阵C_q的第3行第3列的元素；

5)重复步骤2)、步骤3)和步骤4)，直至载体工作过程结束；

6)当载体连续运动时间未满t_spa分钟因工作任务需要停车时，执行步骤2)，将MEMS惯性测量单元输出三轴角速度值置零，即Ω_x＝Ω_y＝Ω_z＝0。

2.如权利要求1所述的一种基于MEMS惯性测量单元的姿态定位误差修正方法，其特征在于，步骤4)所述拟合方法采用最小二乘法、拉格朗日插值法、牛顿插值法、牛顿迭代法、区间二分法、弦截法、雅克比迭代法或牛顿科特斯数值积分法。

3.如权利要求2所述的一种基于MEMS惯性测量单元的姿态定位误差修正方法，其特征在于：步骤4)所述拟合方法采用最小二乘法。