[发明专利]基于联邦滤波的INS/VKM/VDM车载导航系统

摘要

本发明属于车辆导航技术领域，特别涉及一种基于联邦滤波的INS/VKM/VDM车载导航系统，适用于车辆的自主导航。包括下列步骤：（1）建立车辆运动学模型；（2）建立车辆动力学模型；（3）建立车载惯性导航/运动学模型/动力学模型组合导航系统；（4）建立车载组合导航系统状态方程；（5）建立车载组合导航系统量测方程。该车载导航系统能有效抑制纯惯性导航的误差随时间累积问题，提高导航精度和可靠性。

主权项

一种基于联邦滤波的INS/VKM/VDM车载导航系统，其特征在于包括下列步骤：(1)建立车辆运动学模型；(2)建立车辆动力学模型；(3)建立车载惯性导航/运动学模型/动力学模型组合导航系统；(4)建立车载组合导航系统状态方程；(5)建立车载组合导航系统量测方程；步骤(1)所述建立的车辆运动学模型方法如下：车辆运动学模型简化为两自由度的自行车模型，前后轮都用一个单轮替代；G为车辆质心，O为转弯的圆心，r_R,r_G,r_F分别为后轮、质心、前轮的转弯半径，V_R,V_F为后轮和前轮的速度，用一组坐标[X,Y,ψ]表示车辆当前时刻的位置；转向角与偏移角速率直接相关，得到：$\mathrm{\β}={\tan }^{-1}\left(\frac{l_{r}tan\left(\mathrm{\δ}\right)}{l_r+l_f}\right)---\left(1\right)$这里，δ为前轮转向角，l_f,l_r分别为前轮和后轮与质心的距离，β为侧滑角，得到：$\left[\begin{array}{c}\stackrel{\·}{X}\\ \stackrel{\·}{Y}\\ \stackrel{\·}{\mathrm{\ψ}}\end{array}\right]=\left[\begin{array}{c}{V}_{G}cos(\mathrm{\ψ}+\mathrm{\β})\\ V_{G}sin(\mathrm{\ψ}+\mathrm{\β})\\ V_{G}cos\left(\mathrm{\β}\right)tan\left(\mathrm{\δ}\right)/(l_f+l_r)\end{array}\right]---\left(2\right)$这里V_G为质心的速度，ψ为车辆的方向角；步骤(2)所述的建立车辆动力学模型方法如下：将考虑车辆滑动和横向速度考虑进去时，得到相应的车辆动力学模型，定义轮胎滑移角为车轮速度矢量与车轮轴向的夹角，轮胎滑移角正比于作用于轮胎上侧向力；根据质心的受力情况和运动状态可以得到：$\ΣF_y=F_{yf}+F_{yr}={\mathrm{ma}}_y=m({\stackrel{\·}{V}}_y+V_{x}r)---\left(3\right)$$\ΣM_z=l_f-l_r{F}_{yr}=I_z\stackrel{\·\·}{\mathrm{\ψ}}---\left(4\right)$其中，ΣF_y表示总侧向力，F_yf,F_yr分别为前后轮侧向力，a_y表示总侧向加速度，m表示车辆的质量，ΣM_z为总力矩，I_z车辆的偏航惯性矩，V_x,V_y分别表示车辆纵向和横向速度；$F_{yf}=2C_f{\mathrm{\α}}_f\≈2C_f(\mathrm{\β}+\frac{l_f\stackrel{\·}{\mathrm{\ψ}}}{V_x}-\mathrm{\δ})---\left(5\right)$$F_{yr}=2C_r{\mathrm{\α}}_r\≈2C_r(\mathrm{\β}-\frac{l_r\stackrel{\·}{\mathrm{\ψ}}}{V_x})---\left(6\right)$这里，C_f，C_r分别为前后轮的侧偏刚度，α_f，α_r为前后轮的滑移角；联立方程可得：$\left[\begin{array}{c}\stackrel{\·}{X}\\ \stackrel{\·}{Y}\\ \stackrel{\·\·}{\mathrm{\ψ}}\\ {\stackrel{\·}{V}}_y\end{array}\right]=\left[\begin{array}{c}{V}_{x}cos\left(\mathrm{\ψ}\right)-V_{y}sin\left(\mathrm{\ψ}\right)\\ V_{y}cos\left(\mathrm{\ψ}\right)+V_x\sin \left(\mathrm{\ψ}\right)\\ -\left(\frac{2C_f{l}_f^2+2C_r{l}_r^2}{I_z{V}_x}\right)\stackrel{\·}{\mathrm{\ψ}}-\left(\frac{2C_f{l}_f-2C_r{l}_r}{I_z{V}_x}\right)V_y+\frac{2C_f{l}_f}{I_z}\\ -(V_x+\frac{2C_f{l}_f-2C_r{l}_r}{{\mathrm{mV}}_x})\stackrel{\·}{\mathrm{\ψ}}-\left(\frac{2C_f+2C_r}{{\mathrm{mV}}_x}\right)V_y+\frac{2C_f\mathrm{\δ}}{m}\end{array}\right]---\left(7\right)$对于确定的车辆参数m，I_z,l_f,l_r,C_f,C_r都是确定的。