[发明专利]一种基于车辆里程计辅助的姿态测量方法

权利要求书

1.一种基于车辆里程计辅助的姿态测量系统，其特征在于：包括惯性导航模块，及与惯性导航模块电连接的卡尔曼滤波器，及与卡尔曼滤波器电连接的里程计模块；所述惯性导航模块输出端输出至用户端。

2.根据权利要求1所述的基于车辆里程计辅助的姿态测量系统，其特征在于：所述惯性导航模块由三轴陀螺、三轴加速度计、三轴加速度计信号采集器和计算电路组成；所述三轴陀螺包括机械陀螺、激光陀螺、光纤陀螺及MEMS陀螺；所述三轴加速度计包括摆式加速度计和振梁式加速度计。

3.根据权利要求1所述的基于车辆里程计辅助的姿态测量系统，其特征在于：所述里程计模块包括速度传感器；所述速度传感器安装于车轮处；所述里程计模块通过车辆OBD接口与车辆电连接。

4.一种基于车辆里程计辅助的姿态测量方法，其特征在于：包括以下步骤：

第一步：惯性导航模块将三轴陀螺和三轴加速度计测量信号数字化，设定随动的导航坐标系，车前进方向为导航坐标系x轴，天向为y轴，z向根据右手法则确定，并进行导航计算，计算周期为5ms～30ms，得到姿态角和位置信息后输出至接口；

第二步：若导航时间到修正周期即1s～300s，则执行以下步骤，否则返回第一步；

第三步：卡尔曼滤波器读取里程计信息，根据卡尔曼滤波器计算公式计算观测矩阵，计算公式如下：

X(k+1/k)＝Φ(k)X(k)；

P(k+1/k)＝Φ(k)P(k/k)Φ^T(k)+GQ(k)Gdt；

Γ(k+1)＝[R(k+1)+H(k+1)P(k+1/k)H^T(k+1)]^-1；

K(k+1)＝P(k+1/k)H^T(k+1)Γ(k+1)；

X(k+1)＝X(k+1/k)+K(k+1)(Z(K+1))-H(k+1)X(k+1/k))；

P(k+1/k+1)＝(I-K(k+1)H(k+1))P(k+1/k)；

公式中，

X(k+1/k)为根据k时刻预测k+1时刻的状态向量值；

K(k+1)为增益矩阵；

P(k+1/k)为根据k时刻预测k+1时刻的状态向量协方差矩阵；

X(k+1)为k+1时刻的状态向量最佳估计值；

P(k+1/k+1)为k时刻的状态向量协方差估计矩阵；

Φ(k)为状态转移矩阵；

观测矩阵为：Z=Vy-VlccosΔψVx+VlcsinΔψ;]]>

观测矩阵求解公式中，

V_y车辆前进方向；

V_1C为里程计速度；

Δψ为修正间隔车辆转弯角度，从惯性导航模块得到；

第四步：卡尔曼滤波器计算；

第五步：将卡尔曼滤波器计算得到姿态角误差和速度误差结果反馈至惯性导航模块进行导航结果的修正。

5.根据权利要求4所述的基于车辆里程计辅助的姿态测量方法，其特征在于：所述第一步中的导航计算为四元数计算，所述四元数计算中微分方程计算方法为四阶龙格-库达方法或旋转矢量方法。