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

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

An error correction method for attitude determination based on MEMS inertial measurement unit

A method of attitude positioning error correction based on MEMS inertial measurement unit relates to a method of attitude positioning for robot surface motion. Firstly, the MEMS inertial measurement unit (IMU) is fixed on the carrier to be measured, and the output triaxial angular velocity and acceleration of the IMU are acquired. Then the IMU gyroscope is executed by controlling the carrier to remain at rest for 30-180 seconds after 2-10 minutes of motion on the working plane. The attitude matrix is calculated by cosine matrix method, and the roll angle and pitch angle are updated by the acceleration of three axes in the static state of the carrier to realize the attitude positioning of the robot. The method of the invention can effectively improve the attitude positioning accuracy by adjusting the moving speed of the carrier without increasing any hardware cost and changing the hardware installation structure, and is especially suitable for the application of the positioning of robot curved surface motion in indoor and strong magnetic interference environment.

【技术实现步骤摘要】
一种基于MEMS惯性测量单元的姿态定位误差修正方法
本专利技术属于惯性导航
，特别涉及一种机器人曲面运动的姿态定位方法，适合于移动机器人在室内、强磁干扰等作业环境下利用MEMS惯性测量单元进行姿态定位。
技术介绍
MEMS惯性测量单元以多轴方式组合陀螺仪、加速度计和磁力计等传感器，在动态环境下提供导航和姿态数据，被广泛应用在移动机器人、无人机、航天器等需要对运动物体进行姿态测量和控制的领域。惯性导航法是一种常见的姿态测量方法。通过对MEMS惯性测量单元陀螺仪所测量的三轴运动角速度进行积分，能够计算得到姿态信息。MEMS惯性测量单元陀螺仪内部由微纳米尺寸下加工成的微机械元件组成，由于尺寸、体积非常小，极易受到环境因素的影响，特别是温度变化对其精度影响较大。MEMS陀螺仪的确定性误差主要包括常值零偏、刻度因素非线性误差、刻度因素温度漂移误差与三轴非正交误差。在去除常值漂移和噪声后，随机漂移对信号本身仍然会造成累积误差。实际中的随机漂移的均值和方差都会因温度等工作环境变化随时间缓慢地发生变化。长时间累积计算会产生较大误差，需要利用其它传感器对陀螺仪漂移进行估计和误差补偿。已有的姿态估计方法，需要通过磁力计或GPS等外部辅助传感器对姿态定位误差进行修正和补偿。当作业环境中磁力计和GPS信号因强磁干扰、室内等因素而不可靠时，这些附加传感器以及相关的误差修正方法的应用受到限制。通过旋转惯性测量单元能够实现不依赖外界传感器而估计陀螺仪漂移并减小姿态定位误差。已有的单轴或多轴旋转惯性器件的装置及方法，如专利“一种双轴旋转光纤捷联惯性导航装置”(CN102980578)，通过匀速旋转惯性测量单元将误差调制成周期变化的形式，在导航解算过程中利用积分运算将误差抵消来减小系统误差的积累。专利“基于惯性测量单元旋转的陀螺漂移估计补偿方法及装置”(CN103712622A)通过旋转陀螺仪获取不同姿态下加速度计输出的倾角计算陀螺仪漂移并实时补偿，这类方法需要加速度计测量值等于重力，系统匀速或低速运动。旋转惯性测量单元的方法需要设计对应的旋转结构和控制系统，对旋转装置的安装精度和控制有很高的要求，提高了运动姿态定位的成本。综上所述，目前减小姿态定位中陀螺仪漂移引起的姿态定位误差主要依靠磁力计或GPS等外部传感器辅助的方法。当作业环境中磁力计和GPS信号不可靠时，旋转惯性测量单元的方法需要额外的硬件结构。因此，不依赖除惯性测量单元外的传感器和额外硬件结构的姿态定位方法具有工程应用价值。在一些复杂的应用环境如水轮机叶片表面修焊作业的机器人，无法使用有效的外部信息源(如GPS，磁场方向等等)。在不增加系统成本的情况下，充分利用惯性元器件信息提高机器人姿态定位精度，对很多应用场合具有重要意义。
技术实现思路
本专利技术的目的是针对已有技术的不足之处，提供一种基于MEMS惯性测量单元的姿态定位误差修正方法，以提高室内、磁干扰等环境下长时间的姿态估计精度。为实现上述目的，本专利技术的技术方案如下：本专利技术所述的一种基于MEMS惯性测量单元的姿态定位误差修正方法，该方法包括如下步骤：1)定义γ为横滚角，θ为俯仰角，ψ为航向角，在载体运动的起始位置建立固定于地面的导航坐标系on-xnynzn；在MEMS惯性测量单元上建立惯性测量单元坐标系ob-xbybzb；将导航坐标系on-xnynzn绕其自身zn轴转动-ψ角得到坐标系o-x3y3z3，将坐标系o-x3y3z3绕其自身x3轴转动θ角得到坐标系o-x2y2z2，将坐标系o-x2y2z2再绕其自身y2轴转γ角得到惯性测量单元坐标系ob-xbybzb；从导航坐标系on-xnynzn到惯性测量单元坐标系ob-xbybzb的转换矩阵C就是惯性测量单元的姿态矩阵，同时定义惯性测量单元初始姿态矩阵C0为单位矩阵I＝diag(1,1,1)：将MEMS惯性测量单元固连在待测载体上任意位置，使惯性测量单元坐标系yb轴沿着载体正前方，zb轴沿着竖直向上方向，保持载体静止t0分钟，t0∈(0.5,3]，记录静止过程中MEMS惯性测量单元输出的三轴角速度分别为三轴加速度分别为其中k代表角速度和加速度的采集序数，采用下式(2)估算t0/2时刻MEMS惯性测量单元的常值漂移值其中p是载体静止状态t0分钟内记录的三轴角速度最大采集序数；2)使待测载体沿着规划的路径运动，记录运动过程中MEMS惯性测量单元输出的三轴角速度分别为Ωx,Ωy,Ωz，输出的三轴加速度分别为ax,ay,az，当载体连续运动tspa分钟时，tspa∈[2,10]，载体减速至静止状态；3)继续保持载体静止状态t0分钟，记录载体静止过程中MEMS惯性测量单元输出的三轴角速度和三轴加速度采用式(3)计算静止时MEMS惯性测量单元在tM时刻输出的三轴角速度的常值漂移值其中M代表步骤3)执行的次数，取正整数且M<20，采用式(4)计算待测载体的横滚角γ和俯仰角θ；式(4)中，Ax,Ay,Az分别是MEMS惯性测量单元在载体静止时间内输出的三轴加速度均值，其中p是载体静止状态t0分钟内记录的三轴加速度最大采集序数；4)根据步骤3)获得的MEMS惯性测量单元在tM时刻三轴角速度的常值漂移值采用式(5)的模型对载体运动全过程中MEMS惯性测量单元在任一时刻t输出的三轴角速度的常值漂移值ξx(t),ξy(t),ξz(t)进行直线拟合，采用式(6)计算MEMS惯性测量单元输出真实值Ω(t)，根据式(1)和式(7)计算姿态矩阵Cq，其中q表示姿态矩阵的迭代次数；Cq＝exp(-Ω(t)·Δt)·Cq-1(7)式(5)中ax,ay,az,bx,by,bz是直线拟合参数，式(6)中Ωx,Ωy,Ωz分别为MEMS惯性测量单元输出的三轴角速度，式(7)中Δt表示MEMS惯性测量单元采样时间间隔；根据式(1)和式(8)计算出载体三个姿态角：其中C13代表姿态矩阵Cq的第1行第3列的元素，C21代表姿态矩阵Cq的第2行第1列的元素，C22代表姿态矩阵Cq的第2行第2列的元素，C23代表姿态矩阵Cq的第2行第3列的元素，C33代表姿态矩阵Cq的第3行第3列的元素；5)重复步骤2)、步骤3)和步骤4)，直至载体工作过程结束；6)当载体连续运动时间未满tspa分钟因工作任务需要停车时，执行步骤2)，将MEMS惯性测量单元输出三轴角速度值置零，即Ωx＝Ωy＝Ωz＝0。上述技术方案中，其特征在于，步骤4)所述拟合方法可采用最小二乘法、拉格朗日插值法、牛顿插值法、牛顿迭代法、区间二分法、弦截法、雅克比迭代法或牛顿科特斯数值积分法。优选采用最小二乘法。本专利技术与现有技术相比，具有以下优点及突出性的技术效果：本专利技术利用载体运动中多次零角速度条件修正MEMS惯性测量单元角速度测量漂移的方法实现了减小运动姿态定位误差，尤其是航向角的定位误差，相比常用的以磁强计或GPS辅助修正姿态定位误差的方法，不受室内、磁干扰等环境的限制，特别适用于在室内、强磁干扰等作业环境下的机器人曲面运动姿态定位中的应用。相比旋转惯性测量单元的方法，本专利技术不增加任何硬件成本并且不改变硬件安装结构，通过调整载体运动速度即可有效的提高姿态定位精度。附图说明图1是本专利技术方法的流程示意图。图2是本专利技术方法导航坐标系与惯性测量单元坐标本文档来自技高网...

【技术保护点】
1.一种基于MEMS惯性测量单元的姿态定位误差修正方法，其特征在于该方法包括以下步骤：1)定义γ为横滚角，θ为俯仰角，ψ为航向角；在载体运动的起始位置建立固定于地面的导航坐标系on‑xnynzn；在MEMS惯性测量单元上建立惯性测量单元坐标系ob‑xbybzb；将导航坐标系on‑xnynzn绕其自身zn轴转动‑ψ角得到坐标系o‑x3y3z3，将坐标系o‑x3y3z3绕其自身x3轴转动θ角得到坐标系o‑x2y2z2，将坐标系o‑x2y2z2再绕其自身y2轴转γ角得到惯性测量单元坐标系ob‑xbybzb；从导航坐标系on‑xnynzn到惯性测量单元坐标系ob‑xbybzb的转换矩阵C就是惯性测量单元的姿态矩阵，同时定义惯性测量单元初始姿态矩阵C0为单位矩阵I＝diag(1,1,1)：

【技术特征摘要】
1.一种基于MEMS惯性测量单元的姿态定位误差修正方法，其特征在于该方法包括以下步骤：1)定义γ为横滚角，θ为俯仰角，ψ为航向角；在载体运动的起始位置建立固定于地面的导航坐标系on-xnynzn；在MEMS惯性测量单元上建立惯性测量单元坐标系ob-xbybzb；将导航坐标系on-xnynzn绕其自身zn轴转动-ψ角得到坐标系o-x3y3z3，将坐标系o-x3y3z3绕其自身x3轴转动θ角得到坐标系o-x2y2z2，将坐标系o-x2y2z2再绕其自身y2轴转γ角得到惯性测量单元坐标系ob-xbybzb；从导航坐标系on-xnynzn到惯性测量单元坐标系ob-xbybzb的转换矩阵C就是惯性测量单元的姿态矩阵，同时定义惯性测量单元初始姿态矩阵C0为单位矩阵I＝diag(1,1,1)：将MEMS惯性测量单元固连在待测载体上任意位置，使惯性测量单元坐标系yb轴沿着载体正前方，zb轴沿着竖直向上方向，保持载体静止t0分钟，t0∈(0.5,3]，记录静止过程中MEMS惯性测量单元输出的三轴角速度分别为三轴加速度分别为其中k代表角速度和加速度的采集序数，采用下式(2)估算t0/2时刻MEMS惯性测量单元的常值漂移值其中p是载体静止状态t0分钟内记录的三轴角速度最大采集序数；2)使待测载体沿着规划的路径运动，记录运动过程中MEMS惯性测量单元输出的三轴角速度分别为Ωx,Ωy,Ωz，输出的三轴加速度分别为ax,ay,az，当载体连续运动tspa分钟时，tspa∈[2,10]，载体减速至静止状态；3)继续保持载体静止状态t0分钟，记录载体静止过程中MEMS惯性测量单元输出的三轴角速度和三轴加速度采用式(3)计算静止时MEMS惯性测量单元在tM时刻输出的三轴角速度的常值漂移值其中M代表步骤3)执行的次数，取正整数且M<20，采用式(4)计算待测...

【专利技术属性】
技术研发人员：张文，孙振国，张文东，陈强，
申请(专利权)人：清华大学，浙江清华长三角研究院，
类型：发明
国别省市：北京,11