[发明专利]一种基于微惯性和地磁技术的自适应三维姿态定位方法

摘要

本发明公开一种基于微惯性和地磁技术的自适应三维姿态定位方法，包括：1)利用微惯性测量装置中的传感器感应载体的运动姿态；2)设置装置的初始姿态及全局坐标系下的加速度和地磁场信息；3)对装置的姿态值进行求解；4)同时利用三轴微陀螺传感器数据对装置姿态值进行预测；5)对三轴微加速度传感器和三轴磁场传感器数据进行置信判定，检测周围环境干扰，设置自适应参数；6)利用步骤5处理后的三轴微加速度传感器数据和三轴磁场传感器数据获得装置姿态误差值；7)对步骤4得到的姿态预测值以及步骤6得到的更正信息以及步骤5得到的自适应参数进行融合，获得装置姿态值；8)输出姿态信息。

主权项

1.一种基于微惯性和地磁技术的自适应三维姿态定位方法，其特征在于，包括以下步骤：步骤S1：利用集成了三轴微陀螺传感器、三轴微加速度传感器和三轴磁场传感器的微惯性组合测量装置来感应运动载体的运动姿态：三轴微陀螺传感器感应运动载体沿轴向的角速度信号，三轴微加速度传感器感应运动载体加速度信号，三轴磁场传感器感应地球磁场信号；步骤S2：设置微惯性组合测量装置的初始姿态值和全局坐标系下的三轴微加速度传感器的加速度数据和三轴磁场传感器的地球磁场数据：在确保三轴微陀螺传感器、三轴微加速度传感器和三轴磁场传感器坐标系对准的情况下，保持微惯性组合测量装置固定不动，采集三轴微加速度传感器的加速度数据和三轴磁场传感器感应的地球磁场数据；设置微惯性组合测量装置初始时刻即i＝0时的四元数形式姿态值q₀＝[1 0 0 0]^T、三轴微加速度传感器矢量ⁿY_A＝[a_0x a_0y a_0z]^T和三轴磁场传感器矢量ⁿY_M＝[m_0xm_0ym_0z]^T，对三轴微加速度传感器矢量ⁿY_A和三轴磁场传感器矢量ⁿY_M进行单位化，得到单位三轴微加速度传感器矢量ⁿS_A和单位三轴磁场传感器矢量ⁿS_M，所述单位化公式为：ⁿS_A＝ⁿY_A/||ⁿY_A||和ⁿS_M＝ⁿY_M/||ⁿY_M||，式中，a_0x、a_0y、a_0z表示初始时刻即i＝0时的全局坐标系下的三轴微加速度传感器的加速度数据，m_0x、m_0y、m_0z表示初始时刻即i＝0时的全局坐标系下的是三轴磁场传感器的地球磁场数据，下标0表示的是初始时刻，下标x、y、z表示的是每个传感器坐标系下的三个坐标轴；上标n表示的是全局坐标系下的传感器数据、下标A和M分别表示的是三轴微加速度传感器和三轴磁场传感器的数据；S表示的是单位化后相应传感器数据；上标T表示矢量的转置；步骤S3：对微惯性组合测量装置的姿态值进行求解：利用当前时刻即i＝t时的三轴微陀螺传感器的角速度数据、三轴微加速度传感器的加速度数据和三轴磁场传感器的地球磁场数据，当前时刻t的姿态值q_t以及步骤2获得的初始姿态值q₀、三轴微加速度传感器的加速度数据ⁿY_A、ⁿS_A和三轴磁场传感器的地球磁场数据ⁿY_M、ⁿS_M，来获取微惯性组合测量装置下一时刻即i＝t₊Δt时的姿态值q_t+Δt；步骤S4：利用三轴微陀螺传感器数据对i＝t₊Δt时刻微惯性组合测量装置的姿态值进行预测：将当前时刻t的三轴微陀螺传感器的角速度数据表示为当前姿态下的旋转角速度四元数Q_G＝[0 ω_tx ω_ty ω_tz]^T，当前时刻t的三轴微加速度传感器的加速度数据和三轴磁场传感器的磁场强度数据分别为^bY_A＝[a_tx a_ty a_tz]^T和^bY_M＝[m_tx m_ty m_tz]^T，其中G、b为标记号，ω_tx、ω_ty、ω_tz表示的是当前时刻t的三轴陀螺传感器的角速度数据，a_tx、a_ty、a_tz表示的是当前时刻t的三轴微加速度传感器的加速度数据，m_tx、m_ty、m_tz表示的是当前时刻t的三轴磁场传感器的地球磁场数据；利用微惯性组合测量装置当前时刻t的姿态值q_t和当前时刻t的三轴微陀螺传感器的旋转角速度四元数Q_G，对i＝t₊Δt时刻的微惯性组合测量装置的姿态值进行预测，由于两个时刻之间的间隔Δt较小，假设认为t和t₊Δt两个时刻之间三轴微陀螺传感器输出没有变化，从而获得姿态预测方程：q^t+Δt=qt+q·tΔt---(1)]]>式中q·t=12qt⊗QG---(2)]]>对预测后的四元数进行单位化：q^t+Δt′=q^t+Δt/||q^t+Δt||---(3)]]>式中，q_t为当前时刻t的微惯性组合测量装置的姿态值，为下一时刻预测姿态值，Δt为采样时间间隔，为四元数形式的当前时刻t全局姿态下的旋转角速度；步骤S5：对步骤S4获得的三轴微加速度传感器的加速度数据和三轴磁场传感器地球磁场数据进行置信判断，判定周围环境是否存在电磁场干扰或瞬时加速度干扰；若干扰过大，则放弃该时刻采集的三轴磁场传感器的地球磁场数据或三轴微加速度传感器的加速度数据，否则以测得的周围环境噪声的大小设置自适应因子，该自适应因子是表征不存在较大的电磁干扰或是瞬时加速度情况下的三轴微加速度传感器和三轴磁场传感器的噪声大小，从而基于该自适应因子对三轴微加速度传感器的加速度数据或是三轴磁场传感器的地球磁场数据的影响进行权重设置如下：SMb=YMb/||YMb||if(kM=1-|||YMb||-||qt-1⊗YMn⊗qt|||/qt-1⊗YMn⊗qt||<ϵM)qt-1⊗SMn⊗qtelsekM=1---(4)]]>SAb=YAb/||YAb||if(kA=1-|||YAb||-qt-1⊗YAn⊗qt|||/||qt-1⊗YAn⊗qt||<ϵA)qt-1⊗SAn⊗qtelsekA=1]]>式中，^bS_M为调整后的三轴磁场传感器矢量、^bS_A为调整后的三轴微加速度传感器矢量；k_A为三轴微加速度传感器自适应因子，k_M为三轴磁场传感器自适应因子；ε_A为三轴微加速度传感器噪声上界，ε_M为三轴磁场传感器噪声上界；所述干扰过大是超过公式(4)中的三轴微加速度传感器噪声上界ε_A或三轴磁场传感器噪声上界ε_M，ε_A、ε_M的取值范围是(0-1)，周围环境噪声越大，该噪声上界ε_A、ε_M值应取值越小，具体取值应依据在现场环境中的实验决定；步骤S6：利用步骤S5调整后的三轴微加速度传感器的加速度数据和三轴磁场传感器的地球磁场数据获得微惯性组合测量装置的姿态误差值该姿态误差值通过高斯-牛顿方法获得：q→e=-12Rnb(q^t+Δt′)Ξ-1Rnb(q^t+Δt′)T([Rnb(q^t+Δt′)SAn]×SAb+[Rnb(q^t+Δt′)nSM]×SMb)---(16)]]>式中：Ξ=2I-(SAnSATn+SMnSMTn)---(17)]]>Rnb(q^t+Δt′)=20.5-q22-q32q1q2+q0q3q1q3-q0q2q1q2-q0q30.5-q12-q32q2q3+q0q1q1q3+q0q2q2q3-q0q10.5-q12-q22---(18)]]>[λ]×=0-λzλyλz0-λx-λyλx0---(19)]]>[μ]x=0-μzμyμz0-μx-μyμx0---(20)]]>式中：λ表示为μ表示为λ_x、λ_y、λ_z为向量λ的三个分量，μ_x、μ_y、μ_z为向量μ的三个分量，q₀、q₁、q₂、q₃为四元数的四个分量，为四元数转换成的旋转矩阵；矩阵Ξ只与初始标定时刻的三轴微加速度传感器的加速度数据和三轴磁场传感器的地球磁场数据有关，所以在姿态方法中只要进行一次标定，获取Ξ矩阵及其逆矩阵，无需在其他时刻进行矩阵逆运算；步骤S7：基于步骤S4得到的姿态预测信息、步骤S6得到微惯性组合测量装置的姿态误差值和步骤S5得到的自适应因子进行融合，获得t+Δt时刻微惯性组合测量装置姿态值：qt+Δt=q^t+Δt+q^t+Δt⊗1q→ekAkMΔt---(21)]]>然后再对其单位化q_t+Δt＝q_t+Δt/||q_t+Δt||；其中表示的是四元数的乘法运算；步骤S8：输出姿态信息：根据四元数和欧拉角之间的转换关系，将四元数转换为具有直观意义的、自适应三维姿态的俯仰角α、横滚角β和航向角γ如下：α=arctg(2(q2q3-q0q1)q02-q12-q22+q32)α∈(-180,180]β=arcsin(-2(q1q3+q0q2))β∈(-90,90]γ=arctg(2(q1q2-q0q3)q02+q12-q22-q32)γ∈(-180,180]---(22).]]>