CN110361002A - 适用于路基车辆的简化惯导***姿态测量方法 - Google Patents

Abstract

本发明属于惯性导航技术领域，具体涉及的是一种新的适用于路基车辆的简化惯导***姿态测量方法，目的在于提供一种适用于路基车辆的简化惯导***姿态测量方法。采用的技术方案是：首先，给定初始导航参数；然后，递推测量k时刻方位角中间值一和中间值二，计算得到k时刻总的方位角ψ_k；再对横向和前向加速度计进行内杆臂效应补偿，计算得到k时刻的纵摇姿态角θ_k和横摇姿态角γ_k。本发明可以从加速度计计算纵摇姿态角和横摇姿态角，消除了第一次积分引起的误差累积；同时从里程计导出的速度，有更好的性能，避免了任何可能的未补偿加速度计偏差而引起的误差累积，且在位置计算中减少一次积分，有效降低了误差累积。

Description

适用于路基车辆的简化惯导***姿态测量方法

技术领域

本发明属于惯性导航技术领域，具体涉及的是一种新的适用于路基车辆的简化惯导***姿态测量方法。

背景技术

相对于航空、航海惯性导航***，成本对于车辆惯性导航***显得更为重要。在一个常规的中等精度惯性导航***中，惯性测量组件占据了惯性导航***的主要成本，而陀螺占据了惯性测量组件的主要成本，因此研究一种在保证算法有效更新的的情况下减少惯性测量组件的低成本导航***是必要的。

但是在已发表的文章中，如陈长风、时伟、吴美平、曹聚亮、安臣在《计算机测量与控制》上发表的《基于MEMS加速度计的无陀螺车辆导航技术》文章中设计了一种基于低成本MEMS加速度计的无陀螺车载捷联惯性***，将九个高精度的MEMS加速度计安放在载体非质心处，代替陀螺来测量载体角运动信息。从仿真结果来看，对于300s时间内导航的载体，无陀螺车载捷联惯导***的位置误差达到0.0002°，满足导航的精度要求，但是误差随着时间发散明显，无法达到300s以上的长时间导航需求。

还有李安、覃方君、许江宁在《中国惯性技术学报》上发表的《单陀螺多加速度计捷联惯性导航解算方法》文章中采用的是一种单陀螺仪多加速度计(五个加速度计)的捷联惯性导航解算方法，该导航解算方法通过合理配置5个加速度计和1个陀螺仪，可不经过积分而直接解算角速度，消除了加速度计输出方程中角加速度项的影响，能使在姿态和位置解算时分别减少一次积分，从而有效抑制误差随时间发散。从仿真结果来看，该解算方法的确可一定程度抑制***误差发散，但在实际应用中，还需误差补偿算法或和其他导航***组合使用，比较繁琐，成本也更高。

以上已发表的文章都对低成本的简化惯性测量组件导航***进行了叙述和探究，但是没有同时达到长时间导航和实际应用中独立自主导航的要求，因此研究一种在满足精度要求的同时能完全自主、不依赖任何外界信息且长时间工作的低成本导航方法具有创新性和实际工程价值。

发明内容

本发明的目的在于提供一种在保证算法有效更新的的情况下减少惯性测量组件的适用于路基车辆的简化惯导***姿态测量方法。

为了实现本发明的目的，采用的技术方案是：

适用于路基车辆的简化惯导***姿态测量方法，包括以下步骤：

步骤一：给定初始导航参数(k＝0时刻)：地球赤道半径地球自转角速度Ω＝7.27220417×10^-5rad/s、重力加速度g＝9.78049m/s²；外界输入采样周期Δt、当地经度当地纬度λ₀、纵摇姿态角θ₀、横摇姿态角γ₀、方位角ψ₀、高度h₀、里程计速度输出

步骤二：递推测量k时刻方位角中间值一；

步骤三：递推测量k时刻方位角中间值二；

步骤四：由步骤二和步骤三可计算得到k时刻总的方位角ψ_k；

步骤五：对横向和前向加速度计进行内杆臂效应补偿。

步骤六：计算得到k时刻的纵摇姿态角θ_k；

步骤七：计算得到k时刻的横摇姿态角γ_k。

所述方位角中间值一是利用k时刻天向陀螺仪输出角速度计算得到Δt周期内车辆自身旋转角引起的k时刻方位角ψ_k′，具体公式为：

其中：

X^E＝cosθ_k-1sinψ_k-1cosα_k-(cosγ_k-1cosψ_k-1+sinθ_k-1sinγ_k-1sinψ_k-1)sinα_k

X^N＝cosθ_k-1cosψ_k-1cosα_k-(-cosγ_k-1sinψ_k-1+sinθ_k-1sinγ_k-1cosψ_k-1)sinα_k

这里，θ_k-1是k-1时刻纵摇姿态角、γ_k-1是k-1时刻横摇姿态角、ψ_k-1是k-1时刻方位角。

所述方位角中间值二是由于地球自转和当地水平坐标系的变化引起的方位角ψ_k″，具体公式为：

其中，

是k-1时刻当地纬度，h_k-1是k-1时刻高度，R_N是卯酉圈曲率半径。

所述k时刻总的方位角ψ_k为：

ψ_k＝ψ_k′+ψ_k″

所述对横向加速度计进行内杆臂效应补偿是对k时刻横向加速度计输出进行补偿后的加速度值具体公式为：

所述对前向加速度计进行内杆臂效应补偿是对k时刻前向加速度计输出进行补偿后的加速度值具体公式为：

其中，三轴惯性测量组件的角运动矢量为ξ＝[ξ_x ξ_y ξ_z]^T，横向和前向加速度计杆臂长度矢量为r^b＝[r_x r_y]^T。

所述计算得到的k时刻的纵摇姿态角θ_k为：

其中，为前向补偿加速度，为由里程计测得的速度推导出来的加速度，g为重力加速度。

所述计算得到k时刻的横摇姿态角γ_k为：

其中，为横向补偿加速度，为里程计的速度输出，为陀螺的角速度输出，g为重力加速度。

与现有技术相比，本发明的有益效果是：

针对降低车辆惯性导航***成本的问题，本发明提供了一种在保证算法有效更新的情况下减少惯性测量组件的低成本的适用于路基车辆的简化惯导***姿态测量方法，可以从加速度计而不是陀螺仪计算纵摇姿态角和横摇姿态角，消除了第一次积分引起的误差累积；同时从里程计导出的速度而不是由加速度计计算车辆速度有更好的性能，避免了任何可能的未补偿加速度计偏差而引起的误差累积，且在位置计算中减少一次积分，有效降低了误差累积。

附图说明

图1为本发明的流程图。

图2为本发明的原理图。

具体实施方式

下面结合附图对本发明做进一步详细描述：

本发明属于惯性导航技术领域，具体涉及的是一种新的适用于路基车辆的简化惯导***姿态测量方法，目的在于提供一种在保证算法有效更新的的情况下减少惯性测量组件的适用于路基车辆的简化惯导***姿态测量方法。

为了实现本发明的目的，采用的技术方案是：

适用于路基车辆的简化惯导***姿态测量方法，包括以下步骤：

步骤一：给定初始导航参数(k＝0时刻)：地球赤道半径地球自转角速度Ω＝7.27220417×10^-5rad/s、重力加速度g＝9.78049m/s²；外界输入采样周期Δt、当地经度当地纬度λ₀、纵摇姿态角θ₀、横摇姿态角γ₀、方位角ψ₀、高度h₀、里程计速度输出

步骤二：递推测量k时刻方位角中间值一；

步骤三：递推测量k时刻方位角中间值二；

步骤四：由步骤二和步骤三可计算得到k时刻总的方位角ψ_k；

步骤五：对横向和前向加速度计进行内杆臂效应补偿。

步骤六：计算得到k时刻的纵摇姿态角θ_k；

步骤七：计算得到k时刻的横摇姿态角γ_k。

所述方位角中间值一是利用k时刻天向陀螺仪输出角速度计算得到Δt周期内车辆自身旋转角引起的k时刻方位角ψ_k′，具体公式为：

其中：

X^E＝cosθ_k-1sinψ_k-1cosα_k-(cosγ_k-1cosψ_k-1+sinθ_k-1sinγ_k-1sinψ_k-1)sinα_k

X^N＝cosθ_k-1cosψ_k-1cosα_k-(-cosγ_k-1sinψ_k-1+sinθ_k-1sinγ_k-1cosψ_k-1)sinα_k

这里，θ_k-1是k-1时刻纵摇姿态角、γ_k-1是k-1时刻横摇姿态角、ψ_k-1是k-1时刻方位角。

所述方位角中间值二是由于地球自转和当地水平坐标系的变化引起的方位角ψ_k″，具体公式为：

其中，

是k-1时刻当地纬度，h_k-1是k-1时刻高度，R_N是卯酉圈曲率半径。

所述k时刻总的方位角ψ_k为：

ψ_k＝ψ_k′+ψ_k″

所述对横向加速度计进行内杆臂效应补偿是对k时刻横向加速度计输出进行补偿后的加速度值具体公式为：

所述对前向加速度计进行内杆臂效应补偿是对k时刻前向加速度计输出进行补偿后的加速度值具体公式为：

其中，三轴惯性测量组件的角运动矢量为ξ＝[ξ_x ξ_y ξ_z]^T，横向和前向加速度计杆臂长度矢量为r^b＝[r_x r_y]^T。

所述计算得到的k时刻的纵摇姿态角θ_k为：

其中，为步骤五中的补偿加速度，为由里程计测得的速度推导出来的加速度，g为重力加速度。

所述计算得到k时刻的横摇姿态角γ_k为：

其中，为步骤五中的补偿加速度，为里程计的速度输出，为陀螺的角速度输出，g为重力加速度。

与现有技术相比，本发明的有益效果是：

针对降低车辆惯性导航***成本的问题，本发明提供了一种在保证算法有效更新的情况下减少惯性测量组件的低成本的适用于路基车辆的简化惯导***姿态测量方法，可以从加速度计而不是陀螺仪计算纵摇姿态角和横摇姿态角，消除了第一次积分引起的误差累积；同时从里程计导出的速度而不是由加速度计计算车辆速度有更好的性能，避免了任何可能的未补偿加速度计偏差而引起的误差累积，且在位置计算中减少一次积分，有效降低了误差累积。

进一步详细描述如下：

适用于路基车辆的简化惯导***姿态测量方法，包括如下步骤：

步骤一：给定初始导航参数(k＝0时刻)：地球赤道半径地球自转角速度Ω＝7.27220417×10^-5rad/s、重力加速度g＝9.78049m/s²；外界输入采样周期Δt、当地经度当地纬度λ₀、纵摇姿态角θ₀、横摇姿态角γ₀、方位角ψ₀、高度h₀、里程计速度输出

步骤二：递推测量k时刻方位角中间值一，即利用k时刻天向陀螺仪输出角速度计算得到Δt周期内车辆自身旋转角引起的k时刻方位角ψ_k′为：

其中：

X^E＝cosθ_k-1sinψ_k-1cosα_k-(cosγ_k-1cosψ_k-1+sinθ_k-1sinγ_k-1sinψ_k-1)sinα_k

X^N＝cosθ_k-1cosψ_k-1cosα_k-(-cosγ_k-1sinψ_k-1+sinθ_k-1sinγ_k-1cosψ_k-1)sinα_k

这里，θ_k-1是k-1时刻纵摇姿态角、γ_k-1是k-1时刻横摇姿态角、ψ_k-1是k-1时刻方位角。

步骤三：递推测量k时刻方位角中间值二，除了车辆自身旋转外，由于地球自转和当地水平坐标系的变化引起的方位角ψ_k″为：

其中，是k-1时刻当地纬度，h_k-1是k-1时刻高度，R_N是卯酉圈曲率半径：

步骤四：由步骤二和步骤三可计算得到k时刻总的方位角ψ_k为：

ψ_k＝ψ_k′+ψ_k″

步骤五：对横向和前向加速度计进行内杆臂效应补偿。三轴惯性测量组件的角运动矢量为ξ＝[ξ_x ξ_y ξ_z]^T，横向和前向加速度计杆臂长度矢量为r^b＝[r_x r_y]^T，则对k时刻横向加速度计输出进行补偿后的加速度值为：

对k时刻前向加速度计输出进行补偿后的加速度值为：

步骤六：利用步骤五中的补偿加速度和由里程计测得的速度推导出来的加速度以及重力加速度g计算得到k时刻的纵摇姿态角θ_k，即：

步骤七：利用步骤五中的补偿加速度和里程计的速度输出以及陀螺的角速度输出以及重力加速度g计算得到k时刻的横摇姿态角γ_k，即：

至此就得到了简化惯性测量组件导航***的测量三位置姿态角的方法。

综上所述，本发明属于惯性导航技术领域，具体涉及的是一种新的适用于路基车辆的简化惯导***姿态测量方法，目的在于提供一种适用于路基车辆的简化惯导***姿态测量方法。采用的技术方案是：首先，给定初始导航参数；然后，递推测量k时刻方位角中间值一和中间值二，计算得到k时刻总的方位角ψ_k；再对横向和前向加速度计进行内杆臂效应补偿，计算得到k时刻的纵摇姿态角θ_k和横摇姿态角γ_k。本发明可以从加速度计计算纵摇姿态角和横摇姿态角，消除了第一次积分引起的误差累积；同时从里程计导出的速度，有更好的性能，避免了任何可能的未补偿加速度计偏差而引起的误差累积，且在位置计算中减少一次积分，有效降低了误差累积。

Claims (7)

1.适用于路基车辆的简化惯导***姿态测量方法，其特征在于，包括以下步骤：

(1)给定初始导航参数(k＝0时刻)：地球赤道半径地球自转角速度Ω＝7.27220417×10^-5rad/s、重力加速度g＝9.78049m/s²；外界输入采样周期Δt、当地经度当地纬度λ₀、纵摇姿态角θ₀、横摇姿态角γ₀、方位角ψ₀、高度h₀、里程计速度输出

(2)递推测量k时刻方位角中间值一；

(3)递推测量k时刻方位角中间值二；

(4)由步骤二和步骤三可计算得到k时刻总的方位角ψ_k；

(5)对横向和前向加速度计进行内杆臂效应补偿。

(6)计算得到k时刻的纵摇姿态角θ_k；

(7)计算得到k时刻的横摇姿态角γ_k。

2.根据权利要求1所述的适用于路基车辆的简化惯导***姿态测量方法，其特征在于：所述方位角中间值一是利用k时刻天向陀螺仪输出角速度计算得到Δt周期内车辆自身旋转角引起的k时刻方位角ψ_k′，具体公式为：

其中：

X^E＝cosθ_k-1sinψ_k-1cosα_k-(cosγ_k-1cosψ_k-1+sinθ_k-1sinγ_k-1sinψ_k-1)sinα_k

X^N＝cosθ_k-1cosψ_k-1cosα_k-(-cosγ_k-1sinψ_k-1+sinθ_k-1sinγ_k-1cosψ_k-1)sinα_k

这里，θ_k-1是k-1时刻纵摇姿态角、γ_k-1是k-1时刻横摇姿态角、ψ_k-1是k-1时刻方位角。

3.根据权利要求1所述的适用于路基车辆的简化惯导***姿态测量方法，其特征在于：所述方位角中间值二是由于地球自转和当地水平坐标系的变化引起的方位角ψ_k″，具体公式为：

其中，

是k-1时刻当地纬度，h_k-1是k-1时刻高度，R_N是卯酉圈曲率半径。

4.根据权利要求1所述的适用于路基车辆的简化惯导***姿态测量方法，其特征在于：所述k时刻总的方位角ψ_k为：

ψ_k＝ψ_k′+ψ_k″

5.根据权利要求1所述的适用于路基车辆的简化惯导***姿态测量方法，其特征在于：所述对横向加速度计进行内杆臂效应补偿是对k时刻横向加速度计输出进行补偿后的加速度值具体公式为：

所述对前向加速度计进行内杆臂效应补偿是对k时刻前向加速度计输出进行补偿后的加速度值具体公式为：

其中，三轴惯性测量组件的角运动矢量为ξ＝[ξ_x ξ_y ξ_z]^T，横向和前向加速度计杆臂长度矢量为r^b＝[r_x r_y]^T。

6.根据权利要求1所述的适用于路基车辆的简化惯导***姿态测量方法，其特征在于：所述计算得到的k时刻的纵摇姿态角θ_k为：

其中，为前向补偿加速度，为由里程计测得的速度推导出来的加速度，g为重力加速度。

7.根据权利要求1所述的适用于路基车辆的简化惯导***姿态测量方法，其特征在于：所述计算得到k时刻的横摇姿态角γ_k为：

其中，为横向补偿加速度，为里程计的速度输出，为陀螺的角速度输出，g为重力加速度。