CN104713555A - 应用全天域中性点辅助定向的车辆自主导航方法 - Google Patents

Abstract

应用全天域中性点辅助定向的车辆自主导航方法，包括建立车载中性点导航定向模型，建立改进的车辆动态数学模型，建立车载偏振光导航/动态数学模型/惯性导航组合导航系统，建立车载组合导航系统状态方程，建立车载组合导航系统量测方程。本发明将大气偏振特性、惯性导航、车辆动力学/运动学模型有效结合，通过对信息融合方案方法的研究，提出了合理的组合方案，解决大气偏振导航在车载应用中面临的问题，构建了一种具有强自主性、隐蔽性好、抗干扰的无源导航系统，总体可实现陆地作战车辆精确可靠的自主导航定位。

Description

应用全天域中性点辅助定向的车辆自主导航方法

技术领域

本发明属于车辆导航技术领域，涉及应用全天域大气偏振模式下的中性点来进行测角定向从而改进运动学模型辅助惯性导航系统性能，构建了车载偏振光导航/运动学/惯性组合导航系统，适用于车辆的自主导航。

背景技术

针对特殊战争环境下陆地作战车辆有源导航电波易受干扰失效的问题，自主无源导航定位技术是保证载体在该背景下能够进行可靠的高精度导航定位的关键。近年来，国内外研究者针对单一自主无源导航定位方法存在的性能局限，常采用以捷联惯导作为基本导航传感器，辅以其他外部修正技术，利用多源信息融合技术来提高改善系统的自主导航定位能力。其中，不增加其它外部绝对传感器，仅利用车辆的动态数学模型辅助惯性导航系统已成为一种成熟有效的方案。

然而，通常情况下运动学模型得到的二维航向角信息的精度比车载捷联惯导系统还差，即使利用系统校正后的方位信息反馈修正车辆运动学模型，长时间下去，系统整体航向角误差还是得不到有效抑制，随之，定位精度就会逐渐下降。在此种对二维航向测量和控制至关重要的车辆自主无源导航应用背景下，一种基于自然特性，抗干扰能力强并且定向误差不随时间累积的天空偏振光导航方法逐渐得到发展，其中，大气偏振中性点的分布已证明具有明显的方位特性，可作为偏振光导航的航标提供准确的方向信息。

发明内容

本发明为了提高惯导系统运动学辅助算法的导航定位精度，并保证系统的无源性和抗干扰自主导航的能力，根据天空光偏振模式蕴藏的导航特性以及偏振光检测原理，采用一种基于大气偏振模式的全局特性检测技术实现对中性点的跟踪检测与识别，进行测角定向的方法；利用其得到的高精度航向信息，结合车辆自身的运动学模型及约束条件提供的虚拟位置与速度观测量，与车载惯性导航系统所获得的位置、速度、航向信息一起，取长补短，通过设计组合导航系统信息融合算法，对其进行有效合理的配置，构建了一种具有强自主性、隐蔽性好、抗干扰的无源导航系统，总体可实现陆地作战车辆精确可靠的自主导航定位；

本发明为实现上述目的，采用如下技术方案：

所述应用全天域中性点辅助定向的车辆自主导航方法包括下列步骤：

1)建立车载中性点导航定向模型：

大气偏振模式具有围绕中性点偏振态表征，以太阳子午线稳定对称分布的特性，建立车辆偏振光导航定向模型，定向原理具体流程为：

a)利用大气偏振探测装置，采用宏观性好易测量的Stokes矢量表征成像全天域光的偏振态，实现偏振图像数据的获取、传输和预处理、偏振图像Stokes参数、偏振度和偏振方位角信息的计算；

b)结合中性点在偏振图像中同偏振度分布与中性线分布间的关系特征，利用图像处理算法连续对图像中偏振像素单元偏振度为零的点进行跟踪检测和识别，确定载体坐标系下偏振图像中的对称轴即太阳子午线位置，根据偏振模式的分布特性，进一步区分太阳子午线与逆太阳子午线方向，从而获得载体朝向与太阳子午线之间的夹角；

c)根据天文历相关理论公式，由载体的经纬度、观测年份、日期及时间来确定当地太阳方位角，结合载体朝向与太阳子午线之间的夹角，得到与地理正北方向夹角即航向信息；

2)建立改进的车辆动态数学模型：

车辆在道路行驶是一个较为复杂的运动学过程，一般情况下，假设路面是平坦和水平的，车辆转向时简化的运动学模型：车辆任意t时刻的状态以{x_t y_t z_t ψ_t}来表征，其中，t取非负整数，{x_t y_t z_t}定义为t时刻载体坐标系下的位置值，载体坐标系定义为坐标原点位于载体重心、x轴指向载体横轴方向、y轴指向载体纵轴方向、z轴指向载体竖轴方向，通常用表示为载体坐标系(b)系，ψ_t表示t时刻载体纵轴方向相对于x轴的方位角，结合车辆四轮转向模型，Δ表示连续时刻后轮轴心行驶的距离，ω表示横摆角速度，ρ表示车体转弯半径，e表示轮距的一半，L表示轴距，θ表示虚前轮的转向角，这个角度可以近似的代替车辆的前轮摆角，并且假设在两个采样点之间车辆没有滑动，e和L保持常值，根据运动学理论，由各变量之间的几何关系可得到以下表征车辆运动学关系的方程：

$\left\{\begin{array}{c}{x}_{t+1}=x_t+(({\mathrm{\Δ}}_{\mathrm{RR}}+{\mathrm{\Δ}}_{\mathrm{RL}})/2)\·\cos ({\mathrm{\θ}}_t+({\mathrm{\Δ}}_{\mathrm{RR}}-{\mathrm{\Δ}}_{\mathrm{RL}})/4e)\\ y_{t+1}=y_t+(({\mathrm{\Δ}}_{\mathrm{RR}}+{\mathrm{\Δ}}_{\mathrm{RL}})/2)\·\sin ({\mathrm{\θ}}_t+({\mathrm{\Δ}}_{\mathrm{RR}}-{\mathrm{\Δ}}_{\mathrm{RL}})/4e)\\ z_{t+1}=z_t\\ {\mathrm{\θ}}_{t+1}={\mathrm{\θ}}_t+({\mathrm{\Δ}}_{\mathrm{RR}}-{\mathrm{\Δ}}_{\mathrm{RL}})/2e\end{array}\right.---\left(1\right)$

式中，cos表示数学中的余弦函数,sin表示数学中的正弦函数，Δ_RR表示t至t+1两个连续时刻右后轮行使距离，Δ_RL表示t至t+1两个连续时刻左后轮行使距离，且：Δ_RR＝ω×(ρ+e)，Δ_RL＝ω×(ρ-e)；

但是，由于差动里程表在实际工作过程中存在车轮半径及车体结构尺寸的误差、运动中的滑动及地面的起伏等影响，由以上运动学方程得到的航向角精度会越来越差，其误差将交联影响到运动学模型中位置和速度的解算精度，如不加控制，运动学模型的输出将可能成为野值，进而失去对惯导的校正作用；

因此，本发明采用基于车载偏振光传感器实时给出的高精度航向信息，和惯导输出一起经过组合系统滤波器修正后的航向角来实时校正车辆运动学模型，改进后的运动学模型如下：

$\left\{\begin{array}{c}{x}_{t+1}=x_t+(({\mathrm{\Δ}}_{\mathrm{RR}}+{\mathrm{\Δ}}_{\mathrm{RL}})/2)\·\cos ({\widetilde{\mathrm{\ψ}}}_t+({\mathrm{\Δ}}_{\mathrm{RR}}-{\mathrm{\Δ}}_{\mathrm{RL}})/4e)\\ y_{t+1}=y_t+(({\mathrm{\Δ}}_{\mathrm{RR}}+{\mathrm{\Δ}}_{\mathrm{RL}})/2)\·\sin ({\widetilde{\mathrm{\ψ}}}_t+({\mathrm{\Δ}}_{\mathrm{RR}}-{\mathrm{\Δ}}_{\mathrm{RL}})/4e)\\ z_{t+1}=z_t\end{array}\right.---\left(2\right)$

车辆运动学模型可以提供车辆位置和行驶方向上的速度信息，但是无法提供与车辆行驶方向垂直方向上的速度信息，理想情况下，车辆在地面上运动且不发生侧滑的条件下，载体坐标系(b)中y和z轴方向上速度应为零，实际应用中可用高斯白噪声表示：

$\left\{\begin{array}{c}{V}_{\mathrm{by}}-v_y=0\\ V_{\mathrm{bz}}-v_z=0\end{array}\right.---\left(3\right)$

式中，{V_by V_bz}是车辆在载体坐标系(b)中y和z轴方向上的速度分量，{v_y v_z}是均值为零，方差分别为 $\left\{\begin{array}{cc}{\mathrm{\σ}}_y^2& {\mathrm{\σ}}_z^2\end{array}\right\}$ 的高斯白噪声，结合车辆前行方向速度为V_b，运动学模型辅助惯性导航系统的速度观测量在导航坐标系中可表示为V_VDM，导航坐标系定义为坐标原点位于载体重心、x轴指向东、y轴指向北、z轴指向天，即东北天坐标系，通常表示为导航坐标系(n)，下式中的表示载体坐标系(b)至导航坐标系(n)的转换矩阵：

$V_{\mathrm{VDM}}=C_b^n\left[\begin{array}{c}{V}_b\\ v_y\\ v_z\end{array}\right]---\left(4\right)$

3)建立车载偏振光导航/动态数学模型/惯性导航组合导航系统：

车载组合导航系统信息源包括惯性导航系统、车载偏振光二维定向传感系统和车辆动态数学模型组成，系统主要作用机制为：首先，对导航系统进行初始化设置，然后，惯性导航系统直接参与滤波，车载偏振光二维导航定向系统和车辆动态数学模型解算出的导航信息经过惯性导航系统提供的平台转换矩阵转换到导航坐标系中的值，与惯性导航系统的输出一起，再根据多源信息融合技术进行系统最优融合估计，利用滤波值一方面对车辆动态数学模型中的航向角信息进行实时修正，另一方面对惯性导航系统进行闭环反馈校正，最后，输出高精度导航数据；

4)建立车载组合导航系统状态方程：

组合导航系统数学模型—状态方程如下：

$\stackrel{\·}{X}=\mathrm{FX}+\mathrm{GW}---\left(5\right)$

式中，X为系统状态矢量：

其中，{δL δλ δh}为纬、经、高位置误差，{δV_E δV_N δV_U}为东、北、天速度误差，为东、北、天平台误差角，{ε_bx ε_by ε_bz ε_rx ε_ry ε_rz}为陀螺仪漂移沿载体坐标系分量， $\left\{\begin{array}{ccc}{\▿}_x& {\▿}_y& {\▿}_z\end{array}\right\}$ 为加速度计偏置沿载体坐标系分量，因为陆地车辆导航时姿态误差角为小角，可以不考虑高度通道；

W为系统白噪声随机误差矢量：

W＝[ω_gx ω_gy ω_gz ω_rx ω_ry ω_rz ω_ax ω_ay ω_az]^T   (7)

其中，{ω_gx ω_gy ω_gz}为陀螺仪白噪声，{ω_rx ω_ry ω_rz}为陀螺仪一阶马尔可夫过程随机噪声，{ω_ax ω_ay ω_az}为加速度计一阶马尔可夫过程；

F和G分别为系统方程状态系数矩阵和误差系数矩阵：

$\begin{array}{cc}F=\left[\begin{array}{cc}{F}_N& F_S\\ 0& F_M\end{array}\right]& G=\left[\begin{array}{ccc}{C}_b^n& 0_{3\×3}& 0_{3\×3}\\ 0_{9\×3}& 0_{9\×3}& 0_{9\×3}\\ 0_{3\×3}& I_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& I_{3\×3}\end{array}\right]\end{array}---\left(8\right)$

其中，0_m×n为m×n维零矩阵：m和n取正整数，I_m×n为m×n维单位矩阵：m和n取正整数，F_N为位置、速度及平台角度误差参数对应的系统阵，F_S和F_M分别为：

$F_S=\left[\begin{array}{ccc}{C}_b^n& C_b^n& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& C_b^n\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\end{array}\right]$

F_M＝diag[0 0 0 -1/T_gx -1/T_gy -1/T_gz -1/T_ax -1/T_ay -1/T_az]  (9)

其中，{T_gx T_gy T_gz}与{T_ax T_ay T_az}为相关时间参数；

按以上公式，系统状态方程取线性方程；

5)建立车载组合导航系统量测方程：

组合导航系统数学模型—量测方程：

组合框架中，量测输入值有三组：位置、速度及航向角误差观测量{δP δV δψ}，即由车辆运动学模型得到的位置P_VDM和前向速度，约束条件得到的另外两个方向上的速度信息V_VDM与惯性导航系统输出的位置P_SINS、速度V_SINS之差作为位置、速度误差观测量；由车载偏振光测角传感器得到航向角信息ψ_PL与惯性导航系统输出的航向角ψ_SINS之差作为航向角误差观测量，构建量测方程如下：

$Z_{p,v,\mathrm{\ψ}}=\left[\begin{array}{c}\mathrm{\δP}\\ \mathrm{\δV}\\ \mathrm{\δ\ψ}\end{array}\right]=\left[\begin{array}{c}{P}_{\mathrm{SINS}}-P_{\mathrm{VDM}}\\ V_{\mathrm{SINS}}-V_{\mathrm{VDM}}\\ {\mathrm{\ψ}}_{\mathrm{SINS}}-{\mathrm{\ψ}}_{\mathrm{PL}}\end{array}\right]=H_{p,v,\mathrm{\ψ}}X+{\mathrm{\η}}_{p,v,\mathrm{\ψ}}---\left(10\right)$

式中，H_p,v,ψ为量测转换矩阵；

$H_{p,v,\mathrm{\ψ}}=\left[\begin{array}{ccc}{0}_{3\×6}& \mathrm{diag}\left[\begin{array}{ccc}{R}_M& R_N\cos L& 1\end{array}\right]& 0_{3\×9}\\ 0_{3\×3}& \mathrm{diag}\left[\begin{array}{ccc}1& 1& 1\end{array}\right]& 0_{3\×12}\\ \frac{-1}{\cos \mathrm{\θ}}\left[\begin{array}{ccc}0& 0& 0\\ 0& 0& 0\\ \sin \mathrm{\ψ}\sin \mathrm{\θ}& \cos \mathrm{\ψ}\cos \mathrm{\θ}& -\cos \mathrm{\θ}\end{array}\right]& 0_{3\×3}& 0_{3\×12}\end{array}\right]---\left(11\right)$

其中，cos表示数学中的余弦函数,sin表示数学中的正弦函数，0_m×n为m×n维零矩阵：m和n取正整数，diag[1×n]为n维对角阵：n取正整数，R_M和R_N分别为子午圈和卯酉圈曲率半径，L为纬度，ψ为航向角，θ为俯仰角，η_p,v,ψ为量测噪声，其值与器件的仿真精度有关，设置为随机白噪声，测噪声协方差阵为R＝diag[R_p R_v R_ψ]，其中，{R_p R_v R_ψ}分别是位置、速度及航向观测量的噪声方差阵，由以上两式可知，量测方程为线性方程；

故用于组合系统滤波的系统状态方程与量测方程的离散形式为：

X_k＝Φ_k/k-1X_k-1+Γ_k-1W_k-1     (12)

Z_k＝H_kX_k+V_k

式中， $\begin{array}{cc}{\mathrm{\Φ}}_{k/k-1}=\underset{n=0}{\overset{\∞}{\mathrm{\Σ}}}{\left[F_{k}T\right]}^n/n!& {\mathrm{\Γ}}_{k-1}=\left\{\underset{n=1}{\overset{\∞}{\mathrm{\Σ}}}\right[\frac{1}{n!}{\left(F_{k}T\right)}^{n-1}\left]\right\}{G}_{k}T\end{array},$ 其中，k/(k-1)代表前一时刻到当前时刻的递推预测，k取非负整数的时间参数，T为迭代周期取非负整数，n为方程离散化程度参数取正整数，F_k和G_k分别为时刻k的系统方程状态系数矩阵和误差系数矩阵，W_k-1为k-1时刻的系统白噪声随机误差矢量，H_k为时刻k的系统量测转换矩阵，V_k为k时刻的量测噪声矩阵；

最后，对以上系统状态方程与量测方程采用标准线性卡尔曼滤波算法过程，进行系统状态的递推估计。

与已有技术相比，本发明的优点体现在：

本发明将大气偏振特性、惯性导航、车辆动力学/运动学模型有效结合，通过对信息融合方案方法的研究，提出了合理的组合方案，解决大气偏振导航在车载应用中面临的问题，构建了一种具有强自主性、隐蔽性好、抗干扰的无源导航系统，总体可实现陆地作战车辆精确可靠的自主导航定位。

附图说明

图1为全天空偏振模式图；

图2为车载偏振光导航定向模型；

图3为车辆运动学模型；

图4为系统组合模式图。

具体实施方式

下面结合附图1～4对本发明进行详细说明：

大气偏振模式是太阳光入射到大气层并经过大气层中空气分子和气溶胶等粒子进行传输时，与物质相互作用发生散射现象，散射出的光线以偏振度和偏振化方向来描述，建立空间三维坐标系，某天的某一时刻某一位置，太阳位置已知，天空中具有的相对稳定规律的偏振态分布模式，理想天气条件下，采用宏观性好易测量的Stokes矢量表征成像全天域图像的偏振态，如图1所示，O为载体观测点，其所对应的天顶点为Z，TT′为太阳子午线在坐标系中的投影，S为太阳在天球面的投影点，θ_S为太阳高度角，为太阳方位角，Arago和Babinat为两个大气偏振中性点，二者投影点分别为A和B点，且在TT′线上；

如图1，中性点是大气偏振模式的重要分布特征之一，是由于太阳光受到气溶胶的退偏作用而形成的天空偏振度为零的点或区域，一般处于太阳与天顶所在的主平面上即中性点连线与太阳子午线共线，位置变化与太阳高度息息相关，且全天域偏振度分布以各中性点为中心近似为圆或椭圆向外逐渐增大，其等偏振度线以中性点为中心呈环形分布；

一种应用全天域大气偏振模式下的中性点进行测角定向的改进运动学模型辅助惯性导航精度提高算法，其特征在于下列5个步骤：

1)建立车载中性点导航定向模型：

大气偏振模式具有围绕中性点呈环形分布且稳定对称于太阳子午线的特性，建立车辆偏振光导航定向模型，如图2，建立二维导航坐标系，其中N和E分别指向地理北向和东向；二维载体坐标系，其中x和y分别指向载体横向和纵向，TT′为太阳子午线二维投影、ψ为航向角、为太阳方位角、β代表载体朝向与太阳子午线之间的夹角，利用大气偏振探测装置实现偏振图像数据的获取、传输、偏振图像Stokes参数、偏振度和偏振方位角信息的计算，将所得全局偏振度数值再经阈值去噪和平滑处理，为后续航向角解算提供准确数据；

航向角解算流程：

a)构建等偏振度线特征方程Z：

Z(x,y)＝K·F＝ax²+bxy+cy²+dy+ey+f＝0   (13)

式中，F＝[x² xy y² x y 1]^T，x和y为像素点坐标值，取非负整数；

K＝[a b c d e f]^T，K为满足以下最小二乘椭圆拟合准则的参数值a～f构成的

常值阵，且‖K‖＝1；

$\hat{K}=\arg \underset{k}{\min }\left\{\underset{i=1}{\overset{n}{\mathrm{\Σ}}}E{(K,F_i)}^2\right\}=\arg \underset{k}{\min }{\left|\right|F_k\left|\right|}^2---\left(14\right)$

b)求解体轴与太阳子午线夹角β：

对式(13)进行广义特征值求解，获取载体坐标系下中性点坐标值C_n(x_cn,y_cn)＝((be-2cd)/(4ac-b²),(bd-2ca)/(4ac-b²))，其中n取值A、B，分别代表Arago、Babinet两个中性点，则β角可按下式计算得到；

β＝arctan((x_cA-x_cB)/(y_cA-y_cB))    (15)

上式计算所得角度存在180度的模糊性，需结合图像中偏振态的分布特性，进一步区

分太阳子午线与逆太阳子午线方向，从而获得体轴正向与太阳子午线之间的夹角β；

c)获取航向角ψ：

根据天文历相关理论公式，由载体的经纬度、观测年份、日期及时间来确定当地太阳方位角，式中，cos表示数学中的余弦函数,sin表示数学中的正弦函数，δ为太阳赤纬，L为载体的纬度，t为太阳时角；

再结合计算所得的β角，综合得到导航坐标系下载体朝向与地理正北方向的夹角即

航向信息ψ；

2)建立改进的车辆动态数学模型：

车辆在道路行驶是一个较为复杂的运动学过程，一般情况下，假设路面是平坦和水平的，车辆转向时简化的运动学模型，如图3所示：车辆任意t时刻的状态以{x_t y_t z_t ψ_t}来表征，其中，t取非负整数，{x_t y_t z_t}定义为t时刻载体坐标系下的位置值，载体坐标系定义为坐标原点位于载体重心、x轴指向载体横轴方向、y轴指向载体纵轴方向、z轴指向载体竖轴方向，通常用表示为载体坐标系(b)系，ψ_t表示t时刻载体纵轴方向相对于x轴的方位角，M_t和M_t+1分别表示车辆后轮轴心在行驶中的两个连续时刻位置值，O表示车辆圆弧轨迹所对应的圆心，Δ表示连续时刻后轮轴心行驶的距离，ω表示横摆角速度，结合车辆四轮转向模型，ρ表示车体转弯半径，e表示轮距的一半，L表示轴距，θ表示虚前轮的转向角，这个角度可以近似的代替车辆的前轮摆角，并且假设在两个采样点之间车辆没有滑动，e和L保持常值；

结合运动学理论，由图中各变量之间的几何关系可得到以下表征车辆运动学关系的方程：

$\left\{\begin{array}{c}{x}_{t+1}=x_t+(({\mathrm{\Δ}}_{\mathrm{RR}}+{\mathrm{\Δ}}_{\mathrm{RL}})/2)\·\cos ({\mathrm{\θ}}_t+({\mathrm{\Δ}}_{\mathrm{RR}}-{\mathrm{\Δ}}_{\mathrm{RL}})/4e)\\ y_{t+1}=y_t+(({\mathrm{\Δ}}_{\mathrm{RR}}+{\mathrm{\Δ}}_{\mathrm{RL}})/2)\·\sin ({\mathrm{\θ}}_t+({\mathrm{\Δ}}_{\mathrm{RR}}-{\mathrm{\Δ}}_{\mathrm{RL}})/4e)\\ z_{t+1}=z_t\\ {\mathrm{\θ}}_{t+1}={\mathrm{\θ}}_t+({\mathrm{\Δ}}_{\mathrm{RR}}-{\mathrm{\Δ}}_{\mathrm{RL}})/2e\end{array}\right.---\left(18\right)$

式中，cos表示数学中的余弦函数,sin表示数学中的正弦函数，Δ_RR表示t至t+1两个连续时刻右后轮行使距离，Δ_RL表示t至t+1两个连续时刻左后轮行使距离，且：Δ_RR＝ω×(ρ+e)，Δ_RL＝ω×(ρ-e)；

但是，由于差动里程表在实际工作过程中存在车轮半径及车体结构尺寸的误差、运动中的滑动及地面的起伏等影响，由以上运动学方程得到的航向角精度会越来越差，其误差将交联影响到运动学模型中的位置和速度的解算精度，如不加控制，运动学模型的输出将可能成为野值，进而失去对惯导的校正作用；

因此，本发明采用基于车载偏振光传感器实时给出的高精度航向信息，和惯导输出一起经过组合系统滤波器修正后的航向角来实时校正车辆运动学模型，改进后的运动学模型如下：

$\left\{\begin{array}{c}{x}_{t+1}=x_t+(({\mathrm{\Δ}}_{\mathrm{RR}}+{\mathrm{\Δ}}_{\mathrm{RL}})/2)\·\cos ({\widetilde{\mathrm{\ψ}}}_t+({\mathrm{\Δ}}_{\mathrm{RR}}-{\mathrm{\Δ}}_{\mathrm{RL}})/4e)\\ y_{t+1}=y_t+(({\mathrm{\Δ}}_{\mathrm{RR}}+{\mathrm{\Δ}}_{\mathrm{RL}})/2)\·\sin ({\widetilde{\mathrm{\ψ}}}_t+({\mathrm{\Δ}}_{\mathrm{RR}}-{\mathrm{\Δ}}_{\mathrm{RL}})/4e)\\ z_{t+1}=z_t\end{array}\right.---\left(19\right)$

车辆运动学模型可以提供车辆位置和行驶方向上的速度信息，但是无法提供与车辆行驶方向垂直方向上的速度信息，理想情况下，车辆在地面上运动且不发生侧滑的条件下，载体坐标系(b)中y和z轴方向上速度应为零，实际应用中可用高斯白噪声表示：

$\left\{\begin{array}{c}{V}_{\mathrm{by}}-v_y=0\\ V_{\mathrm{bz}}-v_z=0\end{array}\right.---\left(20\right)$

式中，{V_by V_bz}是车辆在载体坐标系(b)中y和z轴方向上的速度分量，{v_y v_z}是均值为零，方差分别为 $\left\{\begin{array}{cc}{\mathrm{\σ}}_y^2& {\mathrm{\σ}}_z^2\end{array}\right\}$ 的高斯白噪声，结合车辆前行方向速度为V_b，运动学模型辅助惯性导航系统的速度观测量在导航坐标系中可表示为V_VDM，导航坐标系定义为坐标原点位于载体重心、x轴指向东、y轴指向北、z轴指向天，即东北天坐标系，通常表示为导航坐标系(n)，下式中的表示载体坐标系(b)至导航坐标系(n)的转换矩阵：

$V_{\mathrm{VDM}}=C_b^n\left[\begin{array}{c}{V}_b\\ v_y\\ v_z\end{array}\right]---\left(21\right)$

3)建立车载偏振光导航/动态数学模型/惯性导航组合导航系统：

建立车载组合导航系统总体结构框图，如图4所示，其中，信息源包括惯性导航导航系统、车载偏振光二维定向传感系统和车辆动态数学模型组成；

系统主要作用机制为：首先，对导航系统进行初始化设置，然后，惯性导航系统直接参与滤波，车载偏振光二维导航定向系统和车辆动态数学模型解算出的导航信息经过惯性导航系统提供的平台转换矩阵转换到导航坐标系中的值，与惯性导航系统的输出一起，再根据多源信息融合技术进行系统最优融合估计，利用滤波值一方面对车辆动态数学模型中的航向角信息进行实时修正，另一方面对惯性导航系统进行闭环反馈校正，最后，以图形化界面输出高精度导航数据；

4)建立车载组合导航系统状态方程：

组合导航系统数学模型—状态方程如下：

$\stackrel{\·}{X}=\mathrm{FX}+\mathrm{GW}---\left(22\right)$

式中，X为系统状态矢量：

其中，{δL δλ δh}为纬、经、高位置误差，{δV_E δV_N δV_U}为东、北、天速度误差，为东、北、天平台误差角，{ε_bx ε_by ε_bz ε_rx ε_ry ε_rz}为陀螺仪漂移沿载体坐标系分量， $\left\{\begin{array}{ccc}{\▿}_x& {\▿}_y& {\▿}_z\end{array}\right\}$ 为加速度计偏置沿载体坐标系分量，因为陆地车辆导航时姿态误差角为小角，可以不考虑高度通道；

W为系统白噪声随机误差矢量：

W＝[ω_gx ω_gy ω_gz ω_rx ω_ry ω_rz ω_ax ω_ay ω_az]^T  (24)

其中，{ω_gx ω_gy ω_gz}为陀螺仪白噪声，{ω_rx ω_ry ω_rz}为陀螺仪一阶马尔可夫过程随机噪声，{ω_ax ω_ay ω_az}为加速度计一阶马尔可夫过程；

F和G分别为系统方程状态系数矩阵和误差系数矩阵：

$\begin{array}{cc}F=\left[\begin{array}{cc}{F}_N& F_S\\ 0& F_M\end{array}\right]& G=\left[\begin{array}{ccc}{C}_b^n& 0_{3\×3}& 0_{3\×3}\\ 0_{9\×3}& 0_{9\×3}& 0_{9\×3}\\ 0_{3\×3}& I_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& I_{3\×3}\end{array}\right]\end{array}---\left(25\right)$

其中，0_m×n为m×n维零矩阵：m和n取正整数，I_m×n为m×n维单位矩阵：m和n取正整数，F_N为位置、速度及平台角度误差参数对应的系统阵，F_S和F_M分别为：

$F_S=\left[\begin{array}{ccc}{C}_b^n& C_b^n& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& C_b^n\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\end{array}\right]$

F_M＝diag[0 0 0 -1/T_gx -1/T_gy -1/T_gz -1/T_ax -1/T_ay -1/T_az]  (26)

其中，[T_gx T_gy T_gz]与[T_ax T_ay T_az]为相关时间参数；

按以上公式，系统状态方程取线性方程；

5)建立车载组合导航系统量测方程：

组合导航系统数学模型—量测方程：

组合框架中，量测输入值有三组：位置、速度及航向角误差观测量{δP δV δψ}，即由车辆运动学模型得到的位置P_VDM和前向速度，约束条件得到的另外两个方向上的速度信息V_VDM与惯性导航系统输出的位置P_SINS、速度V_SINS之差作为位置、速度误差观测量；由车载偏振光测角传感器得到航向角信息ψ_PL与惯性导航系统输出的航向角ψ_SINS之差作为航向角误差观测量，构建量测方程如下：

$Z_{p,v,\mathrm{\ψ}}=\left[\begin{array}{c}\mathrm{\δP}\\ \mathrm{\δV}\\ \mathrm{\δ\ψ}\end{array}\right]=\left[\begin{array}{c}{P}_{\mathrm{SINS}}-P_{\mathrm{VDM}}\\ V_{\mathrm{SINS}}-V_{\mathrm{VDM}}\\ {\mathrm{\ψ}}_{\mathrm{SINS}}-{\mathrm{\ψ}}_{\mathrm{PL}}\end{array}\right]=H_{p,v,\mathrm{\ψ}}X+{\mathrm{\η}}_{p,v,\mathrm{\ψ}}---\left(27\right)$

式中，H_p,v,ψ为量测转换矩阵；

$H_{p,v,\mathrm{\ψ}}=\left[\begin{array}{ccc}{0}_{3\×6}& \mathrm{diag}\left[\begin{array}{ccc}{R}_M& R_N\cos L& 1\end{array}\right]& 0_{3\×9}\\ 0_{3\×3}& \mathrm{diag}\left[\begin{array}{ccc}1& 1& 1\end{array}\right]& 0_{3\×12}\\ \frac{-1}{\cos \mathrm{\θ}}\left[\begin{array}{ccc}0& 0& 0\\ 0& 0& 0\\ \sin \mathrm{\ψ}\sin \mathrm{\θ}& \cos \mathrm{\ψ}\cos \mathrm{\θ}& -\cos \mathrm{\θ}\end{array}\right]& 0_{3\×3}& 0_{3\×12}\end{array}\right]---\left(28\right)$

其中，cos表示数学中的余弦函数,sin表示数学中的正弦函数，0_m×n为m×n维零矩阵：m和n取正整数，diag[1×n]为n维对角阵：n取正整数，R_M和R_N分别为子午圈和卯酉圈曲率半径，L为纬度，ψ为航向角，θ为俯仰角，η_p,v,ψ为量测噪声，其值与器件的仿真精度有关，设置为随机白噪声，量测噪声协方差阵为R＝diag[R_p R_v R_ψ]，其中，{R_p R_v R_ψ}分别是位置、速度及航向观测量的噪声方差阵，由以上两式可知，量测方程为线性方程；

故用于组合系统滤波的系统状态方程与量测方程的离散形式为：

X_k＝Φ_k/k-1X_k-1+Γ_k-1W_k-1        (29)

Z_k＝H_kX_k+V_k

式中， $\begin{array}{cc}{\mathrm{\Φ}}_{k/k-1}=\underset{n=0}{\overset{\∞}{\mathrm{\Σ}}}{\left[F_{k}T\right]}^n/n!& {\mathrm{\Γ}}_{k-1}=\left\{\underset{n=1}{\overset{\∞}{\mathrm{\Σ}}}\right[\frac{1}{n!}{\left(F_{k}T\right)}^{n-1}\left]\right\}{G}_{k}T\end{array},$ 其中，k/(k-1)代表前一时刻到当前时刻的递推预测，k取非负整数的时间参数，T为迭代周期取非负整数，n为方程离散化程度参数取正整数，F_k和G_k分别为时刻k的系统方程状态系数矩阵和误差系数矩阵，W_k-1为k-1时刻的系统白噪声随机误差矢量，H_k为时刻k的系统量测转换矩阵，V_k为k时刻的量测噪声矩阵；

最后，对以上系统状态方程与量测方程采用标准线性卡尔曼滤波算法过程，进行系统状态的递推估计。

本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。

Claims (1)

1.一种应用全天域中性点辅助定向的车辆自主导航方法，其特征在于：

1)建立车载中性点导航定向模型：

大气偏振模式具有围绕中性点偏振态表征，以太阳子午线稳定对称分布的特性，建立车辆偏振光导航定向模型，定向原理具体流程为：

a)利用大气偏振探测装置，采用宏观性好易测量的Stokes矢量表征成像全天域光的偏振态，实现偏振图像数据的获取、传输和预处理、偏振图像Stokes参数、偏振度和偏振方位角信息的计算；

b)结合中性点在偏振图像中同偏振度分布与中性线分布间的关系特征，利用图像处理算法连续对图像中偏振像素单元偏振度为零的点进行跟踪检测和识别，确定载体坐标系下偏振图像中的对称轴即太阳子午线位置，根据偏振模式的分布特性，进一步区分太阳子午线与逆太阳子午线方向，从而获得载体朝向与太阳子午线之间的夹角；

c)根据天文历相关理论公式，由载体的经纬度、观测年份、日期及时间来确定当地太阳方位角，结合载体朝向与太阳子午线之间的夹角，得到与地理正北方向夹角即航向信息；

2)建立改进的车辆动态数学模型：

车辆在道路行驶是一个较为复杂的运动学过程，一般情况下，假设路面是平坦和水平的，车辆转向时简化的运动学模型：车辆任意t时刻的状态以{x_t y_t z_t ψ_t}来表征，其中，t取非负整数，{x_t y_t z_t}定义为t时刻载体坐标系下的位置值，载体坐标系定义为坐标原点位于载体重心、x轴指向载体横轴方向、y轴指向载体纵轴方向、z轴指向载体竖轴方向，通常用表示为载体坐标系(b)系，ψ_t表示t时刻载体纵轴方向相对于x轴的方位角，结合车辆四轮转向模型，Δ表示连续时刻后轮轴心行驶的距离，ω表示横摆角速度，ρ表示车体转弯半径，e表示轮距的一半，L表示轴距，θ表示虚前轮的转向角，这个角度可以近似的代替车辆的前轮摆角，并且假设在两个采样点之间车辆没有滑动，e和L保持常值，根据运动学理论，由各变量之间的几何关系可得到以下表征车辆运动学关系的方程：

$\left\{\begin{array}{c}{x}_{t+1}=x_t+(({\mathrm{\Δ}}_{\mathrm{RR}}+{\mathrm{\Δ}}_{\mathrm{RL}})/2)\·\cos ({\mathrm{\θ}}_t+({\mathrm{\Δ}}_{\mathrm{RR}}-{\mathrm{\Δ}}_{\mathrm{RL}})/4e)\\ y_{t+1}=y_t+(({\mathrm{\Δ}}_{\mathrm{RR}}+{\mathrm{\Δ}}_{\mathrm{RL}})/2)\·\sin ({\mathrm{\θ}}_t+({\mathrm{\Δ}}_{\mathrm{RR}}-{\mathrm{\Δ}}_{\mathrm{RL}})/4e)\\ z_{t+1}=z_t\\ {\mathrm{\θ}}_{t+1}={\mathrm{\θ}}_t+({\mathrm{\Δ}}_{\mathrm{RR}}-{\mathrm{\Δ}}_{\mathrm{RL}})/2e\end{array}\right.---\left(1\right)$

式中，cos表示数学中的余弦函数,sin表示数学中的正弦函数，Δ_RR表示t至t+1两个连续时刻右后轮行使距离，Δ_RL表示t至t+1两个连续时刻左后轮行使距离，且：Δ_RR＝ω×(ρ+e)，Δ_RL＝ω×(ρ-e)；

但是，由于差动里程表在实际工作过程中存在车轮半径及车体结构尺寸的误差、运动中的滑动及地面的起伏影响，由以上运动学方程得到的航向角精度会越来越差，其误差将交联影响到运动学模型中位置和速度的解算精度，如不加控制，运动学模型的输出将可能成为野值，进而失去对惯导的校正作用；

因此，本发明采用基于车载偏振光传感器实时给出的高精度航向信息，和惯导输出一起经过组合系统滤波器修正后的航向角来实时校正车辆运动学模型，改进后的运动学模型如下：

$\left\{\begin{array}{c}{x}_{t+1}=x_t+(({\mathrm{\Δ}}_{\mathrm{RR}}+{\mathrm{\Δ}}_{\mathrm{RL}})/2)\·\cos ({\widetilde{\mathrm{\ψ}}}_t+({\mathrm{\Δ}}_{\mathrm{RR}}-{\mathrm{\Δ}}_{\mathrm{RL}})/4e)\\ y_{t+1}=y_t+(({\mathrm{\Δ}}_{\mathrm{RR}}+{\mathrm{\Δ}}_{\mathrm{RL}})/2)\·\sin ({\widetilde{\mathrm{\ψ}}}_t+({\mathrm{\Δ}}_{\mathrm{RR}}-{\mathrm{\Δ}}_{\mathrm{RL}})/4e)\\ z_{t+1}=z_t\end{array}\right.---\left(2\right)$

车辆运动学模型可以提供车辆位置和行驶方向上的速度信息，但是无法提供与车辆行驶方向垂直方向上的速度信息，理想情况下，车辆在地面上运动且不发生侧滑的条件下，载体坐标系(b)中y和z轴方向上速度应为零，实际应用中可用高斯白噪声表示：

$\left\{\begin{array}{c}{V}_{\mathrm{by}}-v_y=0\\ V_{\mathrm{bz}}-v_z=0\end{array}\right.---\left(3\right)$

式中，{V_by V_bz}是车辆在载体坐标系(b)中y和z轴方向上的速度分量，{v_y v_z}是均值为零，方差分别为的高斯白噪声，结合车辆前行方向速度为V_b，运动学模型辅助惯性导航系统的速度观测量在导航坐标系中可表示为V_VDM，导航坐标系定义为坐标原点位于载体重心、x轴指向东、y轴指向北、z轴指向天，即东北天坐标系，通常表示为导航坐标系(n)，下式中的表示载体坐标系(b)至导航坐标系(n)的转换矩阵：

$V_{\mathrm{VDM}}=C_b^n\left[\begin{array}{c}{V}_b\\ v_y\\ v_z\end{array}\right]---\left(4\right)$

3)建立车载偏振光导航/动态数学模型/惯性导航组合导航系统：

车载组合导航系统信息源包括惯性导航系统、车载偏振光二维定向传感系统和车辆动态数学模型组成，系统作用机制为：首先，对导航系统进行初始化设置，然后，惯性导航系统直接参与滤波，车载偏振光二维导航定向系统和车辆动态数学模型解算出的导航信息经过惯性导航系统提供的平台转换矩阵转换到导航坐标系中的值，与惯性导航系统的输出一起，再根据多源信息融合技术进行系统最优融合估计，利用滤波值一方面对车辆动态数学模型中的航向角信息进行实时修正，另一方面对惯性导航系统进行闭环反馈校正，最后，输出高精度导航数据；

4)建立车载组合导航系统状态方程：

组合导航系统数学模型—状态方程如下：

$\stackrel{.}{X}=\mathrm{FX}+\mathrm{GW}---\left(5\right)$

式中，X为系统状态矢量：

其中，{δL δλ δh}为纬、经、高位置误差，{δV_E δV_N δV_U}为东、北、天速度误差，为东、北、天平台误差角，{ε_bx ε_by ε_bz ε_rx ε_ry ε_rz}为陀螺仪漂移沿载体坐标系分量，为加速度计偏置沿载体坐标系分量，因为陆地车辆导航时姿态误差角为小角，可以不考虑高度通道；

W为系统白噪声随机误差矢量：

W＝[ω_gx ω_gy ω_gz ω_rx ω_ry ω_rz ω_ax ω_ay ω_az]^T   (7)

其中，{ω_gx ω_gy ω_gz}为陀螺仪白噪声，{ω_rx ω_ry ω_rz}为陀螺仪一阶马尔可夫过程随机噪声，{ω_ax ω_ay ω_az}为加速度计一阶马尔可夫过程；

F和G分别为系统方程状态系数矩阵和误差系数矩阵：

$F=\left[\begin{array}{cc}{F}_N& F_S\\ 0& F_M\end{array}\right],G=\left[\begin{array}{ccc}{C}_b^n& 0_{3\×3}& 0_{3\×3}\\ 0_{9\×3}& 0_{9\×3}& 0_{9\×3}\\ 0_{3\×3}& I_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& I_{3\×3}\end{array}\right]---\left(8\right)$

其中，0_m×n为m×n维零矩阵：m和n取正整数，I_m×n为m×n维单位矩阵：m和n取正整数，F_N为位置、速度及平台角度误差参数对应的系统阵，F_S和F_M分别为：

$F_S=\left[\begin{array}{ccc}{C}_b^n& C_b^n& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& C_b^n\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\end{array}\right]$

其中，{T_gx T_gy T_gz}与{T_ax T_ay T_az}为相关时间参数；

按以上公式，系统状态方程取线性方程；

5)建立车载组合导航系统量测方程：

组合导航系统数学模型—量测方程：

组合框架中，量测输入值有三组：位置、速度及航向角误差观测量{δP δV δψ}，即由车辆运动学模型得到的位置P_VDM和前向速度，约束条件得到的另外两个方向上的速度信息V_VDM与惯性导航系统输出的位置P_SINS、速度V_SINS之差作为位置、速度误差观测量；由车载偏振光测角传感器得到航向角信息ψ_PL与惯性导航系统输出的航向角ψ_SINS之差作为航向角误差观测量，构建量测方程如下：

$Z_{p,v,\mathrm{\ψ}}=\left[\begin{array}{c}\mathrm{\δP}\\ \mathrm{\δV}\\ \mathrm{\δ\ψ}\end{array}\right]=\left[\begin{array}{c}{P}_{\mathrm{SINS}}-P_{\mathrm{VDM}}\\ V_{\mathrm{SINS}}-V_{\mathrm{VDM}}\\ {\mathrm{\ψ}}_{\mathrm{SINS}}-{\mathrm{\ψ}}_{\mathrm{PL}}\end{array}\right]=H_{p,v,\mathrm{\ψ}}X+{\mathrm{\η}}_{p,v,\mathrm{\ψ}}---\left(10\right)$

式中，H_p,v,ψ为量测转换矩阵；

$H_{p,v,\mathrm{\ψ}}=\left[\begin{array}{ccc}{0}_{3\×6}& \mathrm{diag}\left[\begin{array}{ccc}{R}_M& R_N\cos L& 1\end{array}\right]& 0_{3\×9}\\ 0_{3\×3}& \mathrm{diag}\left[\begin{array}{ccc}1& 1& 1\end{array}\right]& 0_{3\×12}\\ \frac{-1}{\cos \mathrm{\θ}}\left[\begin{array}{ccc}0& 0& 0\\ 0& 0& 0\\ \sin \mathrm{\ψ}\sin \mathrm{\θ}& \cos \mathrm{\ψ\θ}\cos & -\cos \mathrm{\θ}\end{array}\right]& 0_{3\×3}& 0_{3\×12}\end{array}\right]---\left(11\right)$

其中，cos表示数学中的余弦函数,sin表示数学中的正弦函数，0_m×n为m×n维零矩阵：m和n取正整数，diag[1×n]为n维对角阵：n取正整数，R_M和R_N分别为子午圈和卯酉圈曲率半径，L为纬度，ψ为航向角，θ为俯仰角，η_p,v,ψ为量测噪声，其值与器件的仿真精度有关，设置为随机白噪声，测噪声协方差阵为R＝diag[R_p R_v R_ψ]，其中，{R_p R_v R_ψ}分别是位置、速度及航向观测量的噪声方差阵，由以上两式可知，量测方程为线性方程；

故用于组合系统滤波的系统状态方程与量测方程的离散形式为：

X_k＝Φ_k/k-1X_k-1+Γ_k-1W_k-1

                                        (12)

Z_k＝H_kX_k+V_k

式中， ${\mathrm{\Φ}}_{k/k-1}=\underset{n=0}{\overset{\∞}{\mathrm{\Σ}}}{\left[F_{k}T\right]}^n/n!,{\mathrm{\Γ}}_{k-1}=\left\{\underset{n=1}{\overset{\∞}{\mathrm{\Σ}}}\right[\frac{1}{n!}{\left(F_{k}T\right)}^{n-1}\left]\right\}{G}_{k}T,$ 其中，k/(k-1)代表前一时刻到当前时刻的递推预测，k取非负整数的时间参数，T为迭代周期取非负整数，n为方程离散化程度参数取正整数，F_k和G_k分别为时刻k的系统方程状态系数矩阵和误差系数矩阵，W_k-1为k-1时刻的系统白噪声随机误差矢量，H_k为时刻k的系统量测转换矩阵，V_k为k时刻的量测噪声矩阵；

最后，对以上系统状态方程与量测方程采用标准线性卡尔曼滤波算法过程，进行系统状态的递推估计。