[发明专利]应用全天域中性点辅助定向的车辆自主导航方法

摘要

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

主权项

一种应用全天域中性点辅助定向的车辆自主导航方法，其特征在于：1)建立车载中性点导航定向模型：大气偏振模式具有围绕中性点偏振态表征，以太阳子午线稳定对称分布的特性，建立车辆偏振光导航定向模型，定向原理具体流程为：a)利用大气偏振探测装置，采用宏观性好易测量的Stokes矢量表征成像全天域光的偏振态，实现偏振图像数据的获取、传输和预处理、偏振图像Stokes参数、偏振度和偏振方位角信息的计算；b)结合中性点在偏振图像中同偏振度分布与中性线分布间的关系特征，利用图像处理算法连续对图像中偏振像素单元偏振度为零的点进行跟踪检测和识别，确定载体坐标系下偏振图像中的对称轴即太阳子午线位置，根据偏振模式的分布特性，进一步区分太阳子午线与逆太阳子午线方向，从而获得载体朝向与太阳子午线之间的夹角；c)根据天文历相关理论公式，由载体的经纬度、观测年份、日期及时间来确定当地太阳方位角，结合载体朝向与太阳子午线之间的夹角，得到与地理正北方向夹角即航向信息；2)建立改进的车辆动态数学模型：车辆在道路行驶是一个较为复杂的运动学过程，一般情况下，假设路面是平坦和水平的，车辆转向时简化的运动学模型：车辆任意t时刻的状态以{xt yt zt ψt}来表征，其中，t取非负整数，{xt yt zt}定义为t时刻载体坐标系下的位置值，载体坐标系定义为坐标原点位于载体重心、x轴指向载体横轴方向、y轴指向载体纵轴方向、z轴指向载体竖轴方向，通常用表示为载体坐标系(b)系，ψt表示t时刻载体纵轴方向相对于x轴的方位角，结合车辆四轮转向模型，Δ表示连续时刻后轮轴心行驶的距离，ω表示横摆角速度，ρ表示车体转弯半径，e表示轮距的一半，L表示轴距，θ表示虚前轮的转向角，这个角度可以近似的代替车辆的前轮摆角，并且假设在两个采样点之间车辆没有滑动，e和L保持常值，根据运动学理论，由各变量之间的几何关系可得到以下表征车辆运动学关系的方程：xt+1=xt+((ΔRR+ΔRL)/2)·cos(θt+(ΔRR-ΔRL)/4e)yt+1=yt+((ΔRR+ΔRL)/2)·sin(θt+(ΔRR-ΔRL)/4e)zt+1=ztθt+1=θt+(ΔRR-ΔRL)/2e---(1)]]>式中，cos表示数学中的余弦函数,sin表示数学中的正弦函数，ΔRR表示t至t+1两个连续时刻右后轮行使距离，ΔRL表示t至t+1两个连续时刻左后轮行使距离，且：ΔRR＝ω×(ρ+e)，ΔRL＝ω×(ρ‑e)；但是，由于差动里程表在实际工作过程中存在车轮半径及车体结构尺寸的误差、运动中的滑动及地面的起伏影响，由以上运动学方程得到的航向角精度会越来越差，其误差将交联影响到运动学模型中位置和速度的解算精度，如不加控制，运动学模型的输出将可能成为野值，进而失去对惯导的校正作用；因此，本发明采用基于车载偏振光传感器实时给出的高精度航向信息，和惯导输出一起经过组合系统滤波器修正后的航向角来实时校正车辆运动学模型，改进后的运动学模型如下：xt+1=xt+((ΔRR+ΔRL)/2)·cos(ψ~t+(ΔRR-ΔRL)/4e)yt+1=yt+((ΔRR+ΔRL)/2)·sin(ψ~t+(ΔRR-ΔRL)/4e)zt+1=zt---(2)]]>车辆运动学模型可以提供车辆位置和行驶方向上的速度信息，但是无法提供与车辆行驶方向垂直方向上的速度信息，理想情况下，车辆在地面上运动且不发生侧滑的条件下，载体坐标系(b)中y和z轴方向上速度应为零，实际应用中可用高斯白噪声表示：Vby-vy=0Vbz-vz=0---(3)]]>式中，{Vby Vbz}是车辆在载体坐标系(b)中y和z轴方向上的速度分量，{vy vz}是均值为零，方差分别为的高斯白噪声，结合车辆前行方向速度为Vb，运动学模型辅助惯性导航系统的速度观测量在导航坐标系中可表示为VVDM，导航坐标系定义为坐标原点位于载体重心、x轴指向东、y轴指向北、z轴指向天，即东北天坐标系，通常表示为导航坐标系(n)，下式中的表示载体坐标系(b)至导航坐标系(n)的转换矩阵：VVDM=CbnVbvyvz---(4)]]>3)建立车载偏振光导航/动态数学模型/惯性导航组合导航系统：车载组合导航系统信息源包括惯性导航系统、车载偏振光二维定向传感系统和车辆动态数学模型组成，系统作用机制为：首先，对导航系统进行初始化设置，然后，惯性导航系统直接参与滤波，车载偏振光二维导航定向系统和车辆动态数学模型解算出的导航信息经过惯性导航系统提供的平台转换矩阵转换到导航坐标系中的值，与惯性导航系统的输出一起，再根据多源信息融合技术进行系统最优融合估计，利用滤波值一方面对车辆动态数学模型中的航向角信息进行实时修正，另一方面对惯性导航系统进行闭环反馈校正，最后，输出高精度导航数据；4)建立车载组合导航系统状态方程：组合导航系统数学模型—状态方程如下：X.=FX+GW---(5)]]>式中，X为系统状态矢量：其中，{δL δλ δh}为纬、经、高位置误差，{δVE δVN δVU}为东、北、天速度误差，为东、北、天平台误差角，{ε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=FNFS0FM,G=Cbn03×303×309×309×309×303×3I3×303×303×303×3I3×3---(8)]]>其中，0m×n为m×n维零矩阵：m和n取正整数，Im×n为m×n维单位矩阵：m和n取正整数，FN为位置、速度及平台角度误差参数对应的系统阵，FS和FM分别为：FS=CbnCbn03×303×303×3Cbn03×303×303×3]]>其中，{Tgx Tgy Tgz}与{Tax Tay Taz}为相关时间参数；按以上公式，系统状态方程取线性方程；5)建立车载组合导航系统量测方程：组合导航系统数学模型—量测方程：组合框架中，量测输入值有三组：位置、速度及航向角误差观测量{δP δV δψ}，即由车辆运动学模型得到的位置PVDM和前向速度，约束条件得到的另外两个方向上的速度信息VVDM与惯性导航系统输出的位置PSINS、速度VSINS之差作为位置、速度误差观测量；由车载偏振光测角传感器得到航向角信息ψPL与惯性导航系统输出的航向角ψSINS之差作为航向角误差观测量，构建量测方程如下：Zp,v,ψ=δPδVδψ=PSINS-PVDMVSINS-VVDMψSINS-ψPL=Hp,v,ψX+ηp,v,ψ---(10)]]>式中，Hp,v,ψ为量测转换矩阵；Hp,v,ψ=03×6diagRMRNcosL103×903×3diag11103×12-1cosθ000000sinψsinθcosψθcos-cosθ03×303×12---(11)]]>其中，cos表示数学中的余弦函数,sin表示数学中的正弦函数，0m×n为m×n维零矩阵：m和n取正整数，diag[1×n]为n维对角阵：n取正整数，RM和RN分别为子午圈和卯酉圈曲率半径，L为纬度，ψ为航向角，θ为俯仰角，ηp,v,ψ为量测噪声，其值与器件的仿真精度有关，设置为随机白噪声，测噪声协方差阵为R＝diag[Rp Rv Rψ]，其中，{Rp Rv Rψ}分别是位置、速度及航向观测量的噪声方差阵，由以上两式可知，量测方程为线性方程；故用于组合系统滤波的系统状态方程与量测方程的离散形式为：Xk＝Φk/k‑1Xk‑1+Γk‑1Wk‑1                                        (12)Zk＝HkXk+Vk式中，Φk/k-1=Σn=0∞[FkT]n/n!,Γk-1={Σn=1∞[1n!(FkT)n-1]}GkT,]]>其中，k/(k‑1)代表前一时刻到当前时刻的递推预测，k取非负整数的时间参数，T为迭代周期取非负整数，n为方程离散化程度参数取正整数，Fk和Gk分别为时刻k的系统方程状态系数矩阵和误差系数矩阵，Wk‑1为k‑1时刻的系统白噪声随机误差矢量，Hk为时刻k的系统量测转换矩阵，Vk为k时刻的量测噪声矩阵；最后，对以上系统状态方程与量测方程采用标准线性卡尔曼滤波算法过程，进行系统状态的递推估计。