[发明专利]一种基于载体运动条件约束和单轴旋转调制的车载自主导航方法

权利要求书

1.一种基于载体运动条件约束和单轴旋转调制的车载自主导航方法，其特征在于，包括以下步骤：

步骤1.基于旋转IMU的虚拟里程计计算载体前行速度和车轮旋转角度；

将IMU安装在车轮中心，使IMU随着载体运动而旋转；o-x_sy_sz_s代表传感器坐标系，o-x_by_bz_b代表载体坐标系；当载体运动时，o-x_sy_sz_s绕x_s轴旋转；在传感器坐标系中，IMU实际输出的比力和角速率/分别由式(1)和式(2)表示：

其中，为载体坐标系至传感器坐标系的转换矩阵，下标b表示载体坐标系，上标s表示传感器坐标系，φ＝∫ωdt为车轮旋转角度，ω为车轮旋转角速度，t为时间；f^b为载体坐标系下的比力，a^b和g^b分别为载体坐标系下载体加速度矢量和当地重力加速度矢量，γ^s为加速度计误差；/为在载体坐标系下载体坐标系相对于惯性坐标系的旋转角速度，/为在传感器坐标系下载体坐标系相对于传感器坐标系的旋转角速度，d^s表示陀螺仪误差；

y_s轴和z_s轴加速度计的实际输出和/表示为：

其中，g为当地重力加速度，为载体坐标系中y_b轴的加速度，/和/分别表示y_s轴和z_s轴的加速度计输出误差；

传感器坐标系中x_s轴陀螺仪实际输出为：

其中，为x_s轴陀螺仪输出误差；/为载体坐标系中x_b轴陀螺仪输出；

y_s轴的加速度计实际输出z_s轴的加速度计实际输出/和x_s轴陀螺仪实际输出/为：

其中，表示车轮旋转角度φ的一阶时间导数；

基于扩展型卡尔曼滤波，通过IMU输出来计算载体沿y_b轴速度和车轮旋转角度φ；

系统状态量x_o和系统方程分别由式(9)和(10)表示：

其中，r_w为车轮半径，w_o为系统噪声，δφ为车轮旋转角度φ的误差，/为载体沿y_b轴速度/的误差，上标T表示转置，/表示系统状态量x_o的一阶时间导数；

经过线性化之后的系统观测模型由式(11)表示：

z_o＝H_ox_o+v (11)

其中，为线性化之后的系统观测量，δf_y为y_s轴的加速度计输出/的闭合差、δf_z为z_s轴的加速度计输出/的闭合差，/为x_s轴陀螺仪输出/的闭合差，v为观测量噪声；

步骤2.建立载体自主导航误差方程

将比力f^s和旋转角速度转换至载体坐标系，如式(12)和(13)所示：

其中，为传感器坐标系至载体坐标系的转换矩阵；

基于转换至载体坐标系的比力和角速率，利用传统捷联导航解算方程解算出载体位置rⁿ、速度vⁿ与姿态信息

(1)姿态误差方程

捷联惯性导航中姿态更新方程为：

其中，为/的一阶时间导数，/为载体坐标系至导航坐标系的转换矩阵，/是/的斜对称矩阵，/为载体坐标系下载体坐标系相对于导航坐标系的旋转角速度；

根据姿态误差定义其中/为含有姿态误差的转换矩阵，I为单位矩阵，Eⁿ是姿态误差εⁿ的斜对称矩阵，εⁿ＝[ε_E ε_N ε_U]^T，ε_E、ε_N、ε_U分别代表姿态角误差在东向、北向和天向的分量；

在小失准角的情况下，通过对式(14)进行扰动分析推导得出姿态误差方程：

其中，δrⁿ为位置误差矢量，δλ和δh分别表示载体纬度误差、经度误差和高度误差；δvⁿ为速度误差矢量，δvⁿ＝[δv_E δv_N δv_U]^T，δv_E、δv_N和δv_U分别代表了东向、北向和天向的速度误差；d^b为载体坐标系下的陀螺仪误差矢量，d^b＝[d_x d_y d_z]^T，d_x、d_y和d_z分别表示x_b轴、y_b轴和z_b轴的陀螺仪误差；F_εr，F_εv和F_εε分别代表姿态误差变化率与位置误差、速度误差和姿态误差的关系矩阵，可以表示为：

/

其中，M为载体所在位置子午圈的曲率半径，N为载体所在位置卯酉圈的曲率半径，ω_ie为地球自转角速度值，h为载体高度，表示经度λ的一阶时间导数；

(2)速度误差方程

导航坐标系下速度更新方程表示为：

其中，为vⁿ的一阶时间导数，vⁿ为载体相对于导航坐标系的速度，/为地球自转角速率/的斜对称矩阵；/为旋转角速度/的斜对称矩阵，/为导航坐标系下导航坐标系相对于地球坐标系的旋转角速度；gⁿ为导航坐标系下的当地重力加速度矢量；

根据速度误差定义δvⁿ为速度误差，/为含有误差的速度，对式(16)进行扰动分析推导出速度误差方程：

其中，为/的一阶时间导数，γ^b为载体坐标系下的加速度计误差矢量，γ^b＝[γ_x γ_yγ_z]^T，γ_x、γ_y和γ_z表示x_b轴、y_b轴和z_b轴的加速度计误差；F_vr、F_vv和F_vε分别代表速度误差变化率与位置误差、速度误差和姿态误差的关系矩阵，表示为：

其中，ν_E、v_N和v_U分别表示东向、北向和天向的载体的速度值，f_E、f_N和f_U分别表示东向、北向和天向的载体的比力值，γ表示随载体维度和高度变化的当地重力加速度；

(3)位置误差方程

捷联惯性导航中位置更新方程：

其中，为载体纬度；

通过对式(18)进行扰动分析得出位置误差方程：

其中，为/的误差，F_rr为位置误差变化率和速度误差的关系矩阵，F_rv为位置误差和速度误差的关系矩阵，表示为：

(4)MEMSIMU传感器误差方程

MEMS加速度计和陀螺仪误差建模为一阶高斯马可夫随机过程，如式(20)所示：

其中，为η的一阶时间导数，η为加速度计或陀螺仪误差，α为一阶高斯马尔科夫随机过程中的相关系数，w为驱动白噪声；

加速度计和陀螺仪误差方程：

其中，为γ^b的一阶时间导数，/为d^b的一阶时间导数，/为/的一阶时间导数，/为γ^s的一阶时间导数，γ^s为加速度计误差，/是/的斜对称矩阵，/为载体坐标系下传感器坐标系相对于载体坐标系的旋转角速度，α_f为加速度计一阶高斯马尔可夫随机模型的相关系数，/α_fx、α_fy、α_fz分别为α_f在x_s轴、y_s轴、z_s轴方向的分量，w_f为加速度计的高斯马尔可夫模型的驱动白噪声，w_f＝[w_fx y_fy w_fz]^T，w_fx、w_fy、w_fz分别为w_f在x_s轴、y_s轴、z_s轴方向的分量；/为d^s的一阶时间导数，d^s为陀螺仪误差，α_ω为陀螺仪一阶高斯马尔可夫随机模型的相关系数，/α_ωx、α_ωy、α_ωz分别为α_ω在x_s轴、y_s轴、z_s轴方向的分量，w_ω为陀螺仪的高斯马尔可夫模型的驱动白噪声，w_ω＝[w_ωx w_ωy w_ωz]^T，w_ωx、w_ωy、w_ωz分别为w_ω在x_s轴、y_s轴、z_s轴方向的分量；

步骤3.载体运动约束条件下的自主导航模型建立

利用扩展型卡尔曼滤波计算出惯性系统的位置误差、速度误差、姿态误差以及加速度计和陀螺仪误差；

首先建立系统状态方程；根据车载惯性导航系统特性，选取位置误差、速度误差、姿态误差、加速度计和陀螺仪误差组成15维状态量，如式(23)所示：

x＝[δrⁿ δvⁿ εⁿ γ^b d^b]^T (23)

根据式(15)、(17)、(19)、(21)和(22)，系统状态方程由式(24)表示：

其中，表示x的一阶时间导数，/w为系统驱动白噪声，w_r、w_v和w_ε分别代表状态方程中，对应位置误差、速度误差和姿态误差的驱动白噪声；0_3×3为3×3的零矩阵；

其次建立系统观测方程；载体坐标系o-x_by_bz_b下的速度矢量表示为：基于扰动分析得出载体坐标系下的速度误差δv^b：

其中，v^b是vⁿ的斜对称矩阵，表示载体坐标系下的速度；为导航坐标系至载体坐标系的转换矩阵；

系统观测量方程为：

z＝Hx+v (26)

其中，z＝δv^b，v为观测量噪声；

基于系统状态方程和系统观测量方程，利用扩展型卡尔曼滤波计算得到位置误差、速度误差和姿态误差，来修正惯性导航系统中的位置、速度和姿态状态量。