[发明专利]一种基于助力车的惯性组合导航方向定位方法

权利要求书

1.一种基于助力车的惯性组合导航方向定位方法，其特征在于，包括以下步骤：

S1：选择载体和导航坐标系，定义姿态航向角后对传感器自检粗对准，根据姿态航向角对姿态四元数和姿态矩阵进行初始化；

S2：姿态更新，设置姿态更新周期，根据前一时刻的姿态四元数和当前时刻陀螺输出的角速度计算当前时刻的姿态四元数；通过当前时刻的姿态四元数得到载体当前时刻的姿态航向角；

S3：对描述系统动态特性的连续微分方程离散化后使用卡尔曼滤波对系统的状态向量进行估计；

S4：根据速度、卫星数和定位精度信息综合判断，剔除错误信息；进行陀螺仪温度漂移补偿后输出传感信息。

2.根据权利要求1所述的一种基于助力车的惯性组合导航方向定位方法，其特征在于，所述的传感器自检粗对准得到的俯仰角θ和横滚角γ如下：

其中，设加速度计输出为f^b＝[f_x f_y f_z]^T；

f_x、f_x和f_x分别为加速度计在X轴、Y轴和Z轴上的输出；

重力加速度大小为g；

航向角的初始值为0。

3.根据权利要求1或2所述的一种基于助力车的惯性组合导航方向定位方法，其特征在于，根据俯仰角θ、横滚角γ和航向角得到姿态四元数Q＝[q₀ q₁ q₂ q₃]^T，计算公式如下：

根据四元数Q＝[q₀ q₁ q₂ q₃]^T计算得到的姿态矩阵为：

其中，q₀、q₁、q₂和q₃分别为姿态四元数中的元素。

4.根据权利要求3所述的一种基于助力车的惯性组合导航方向定位方法，其特征在于，当前时刻的姿态四元数Q_k的计算过程为：

1)根据陀螺输出的角速率和姿态更新周期Δt计算角增量Δθ，公式如下：

2)计算角增量Δθ的模值|Δθ|；

3)根据角增量Δθ计算中间变量ΔΘ；

4)计算姿态四元数Q_k，计算公式如下：

其中，I为4维单位矩阵；

Q_k-1为前一时刻的姿态四元数。

5.根据权利要求4所述的一种基于助力车的惯性组合导航方向定位方法，其特征在于，姿态更新后，对四元数做规范化处理：

其中，i＝0，1，2，3；

在完成姿态四元数Q_k更新并规范化处理之后，计算当前时刻的姿态矩阵依据姿态矩阵与俯仰角θ、横滚角γ和航向角之间的关系，得到载体当前时刻的姿态航向角：

其中，T₀₀、T₀₁、T₀₂、T₁₀、T₁₁、T₁₂、T₂₀、T₂₁和T₂₂对应姿态矩阵中的元素。