车辆运动学约束辅助的回溯式行进间对准方法

摘要

本发明公开了一种车辆运动学约束辅助的回溯式行进间对准方法，用于解决现有车辆运动数学模型辅助的动基座对准方法对准精度差的技术问题。技术方案是在车辆行驶过程中进行实时粗对准，同时提取并存储关键传感器信息；获得初始姿态阵后使用存储数据进行回溯导航解算；回溯过程中将惯导和车辆之间的安装偏差角作为误差状态，利用车辆侧向和垂向速度为零的约束条件进行卡尔曼滤波精对准。本发明方法不需要专门的静止时间，有效提高了行进间对准精度。经测试，方位对准结果达到2.148′。同时，对准过程不丢失位置和速度信息，定位精度优于20m。

主权项

一种车辆运动学约束辅助的回溯式行进间对准方法，其特征在于包括以下步骤：(a)惯导系统获得初始位置并进入对准状态后，启动车辆按既定路线行驶；(b)行驶过程中，采用惯性系二次积分双矢量定姿方法进行实时粗对准；定义解算所需的参考坐标系如下：n―导航坐标系，X、Y、Z轴分别指向载体所在地理位置的东‑北‑天方向；b－IMU坐标系，X、Y、Z轴分别指向惯性测量单元的右‑前‑上方向；m－车体坐标系，X、Y、Z轴分别指向车体的右‑前‑上方向；n₀－初始导航惯性坐标系，在对准起始时刻将导航坐标系n相对惯性空间凝固所得的惯性坐标系；b₀－初始IMU惯性坐标系，在对准起始时刻将IMU坐标系b相对惯性空间凝固所得的惯性坐标系；忽略车辆运动的影响，采用惯性系二次积分TRIAD方法求解初始矩阵$C_{b_0}^{n_0}={\left[\begin{array}{c}{\left(p_{t_1}^{n_0}\right)}^T\\ {\left(p_{t_1}^{n_0}\times p_{t_2}^{n_0}\right)}^T\\ {\left(p_{t_1}^{n_0}\times p_{t_2}^{n_0}\times p_{t_1}^{n_0}\right)}^T\end{array}\right]}^{-1}\left[\begin{array}{c}{\left({\tilde{p}}_{t_1}^{b_0}\right)}^T\\ {\left({\tilde{p}}_{t_1}^{b_0}\times {\tilde{p}}_{t_2}^{b_0}\right)}^T\\ {\left({\tilde{p}}_{t_1}^{b_0}\times {\tilde{p}}_{t_2}^{b_0}\times {\tilde{p}}_{t_1}^{b_0}\right)}^T\end{array}\right]---\left(1\right)$式中下标t₁、t₂表示不同时间点，为对重力加速度信息二次积分获得的n₀系位置增量；为根据陀螺和加速度计数据二次积分计算的b₀系位置增量；(c)整个对准期间，提取陀螺和加速度计的关键信息，每1s存储6个浮点数：姿态四元数和速度增量第k秒的四元数计算$Q_{b_k}^{b_0}=Q_{b_{k-1}}^{b_0}\otimes q\left(\Phi \right)---\left(2\right)$式中下标k‑1和k表示前周期和本周期，递推初值q(Φ)为根据每周期陀螺输出构造的转动四元数；第k秒的速度增量计算$\Delta {\tilde{v}}_k^{b_0}={\int }_{t_{k-1}}^{t_k}{C}_b^{b_0}\left(t\right){\tilde{f}}^b\left(t\right)dt---\left(3\right)$式中通过式(2)解算的转换得到，为加速度计输出的比力信息；(d)粗对准结束后，根据存储信息进行回溯导航解算，包括速度更新、姿态更新和位置更新$v_k^n=v_{k-1}^n+C_{n_{k-1}}^{n_{k-1/2}}{C}_{n_0}^{n_{k-1}}{C}_{b_0}^{n_0}\Delta {\tilde{v}}_k^{b_0}-(2{\omega }_{ie,k-1}^n+{\omega }_{en,k-1}^n)\times v_{k-1}^n{T}_k+g_{k-1}^n{T}_k---\left(4\right)$$Q_{b_k}^{n_k}=Q_{n_0}^{n_k}\otimes Q_{b_0}^{n_0}\otimes Q_{b_k}^{b_0}---\left(5\right)$$\left[\begin{array}{c}{\lambda }_k\\ L_k\\ h_k\end{array}\right]=\left[\begin{array}{c}{\lambda }_{k-1}\\ L_{k-1}\\ h_{k-1}\end{array}\right]+\frac{1}{2}\left[\begin{array}{c}\frac{v_{E,k-1}+v_{E,k}}{{\left(R_N+h\right)}_{k-1}\cos L_{k-1}}\\ \frac{v_{N,k-1}+v_{N,k}}{2{\left(R_M+h\right)}_{k-1}}\\ v_{U,k-1}+v_{U,k}\end{array}\right]T_k---\left(6\right)$式中T_k＝1，为更新周期；vⁿ为导航速度，且vⁿ＝[v_E v_N v_U]^T；[λ,L,h]为位置经度、纬度和高度；为当前姿态四元数；和为读取存储数据；矩阵根据地球自转角速率表面运动角速率当地纬度L和对准时间获得；为粗对准获得的初始姿态阵，为对应的四元数；gⁿ为重力加速度矢量；为矩阵对应的四元数；R_M、R_N为子午圈和卯酉圈半径，根据当前位置计算；(e)回溯导航解算的同时，选择误差状态为：速度误差平台失准角φⁿ，陀螺零偏ε^b，加速度计零位IMU坐标系b与车体坐标系m之间绕X、Z轴的安装偏差角α_x和α_z；建立17维卡尔曼滤波器状态变量$X={\left[\begin{array}{ccccccc}{\left({\phi }^n\right)}^T& {\left({\mathrm{\delta v}}_I^n\right)}^T& {\left(\delta p\right)}^T& {\left({ϵ}^b\right)}^T& {\left({▿}^b\right)}^T& {\alpha }_x& {\alpha }_z\end{array}\right]}^T---\left(7\right)$建立相应的状态方程$\stackrel{·}{X}=\left[\begin{array}{cc}{F}_I& 0_{12\times 5}\\ 0_{5\times 12}& 0_{5\times 5}\end{array}\right]X+\left[\begin{array}{c}-{ϵ}_w^n\\ {▿}_w^n\\ 0_{11\times 1}\end{array}\right]---\left(8\right)$式中F_I为按经典捷联惯导误差方程建立的15×15维状态转移矩阵，为陀螺和加速度计数据对应的系统噪声；根据车辆运动学模型构造非完整性约束，每1s进行量测更新$Z=\left[\begin{array}{c}{v}_x^b\\ v_z^b\end{array}\right]=\left[\begin{array}{ccc}\begin{array}{cc}{M}_1(1,:)& M_2(1,:)\\ M_1(3,:)& M_2(3,:)\end{array}& 0_{2\times 9}& \begin{array}{cc}{M}_3(1,1)& M_3(1,3)\\ M_3(3,1)& M_3(3,3)\end{array}\end{array}\right]X+w_v---\left(9\right)$式中量测值符号M₁(1,:)表示取3×3矩阵M₁的第1行，其余符号按同样规则定义，且w_v为速度量测噪声。