基于联邦滤波的INS/VKM/VDM车载导航系统

1.一种基于联邦滤波的INS/VKM/VDM车载导航系统，其特征在于包括下列步骤：
(1)建立车辆运动学模型；
(2)建立车辆动力学模型；
(3)建立车载惯性导航/运动学模型/动力学模型组合导航系统；
(4)建立车载组合导航系统状态方程；
(5)建立车载组合导航系统量测方程；
步骤(1)所述建立的车辆运动学模型方法如下：
车辆运动学模型简化为两自由度的自行车模型，前后轮都用一个单轮替代；G为车辆质心，O为转弯的圆心，rR,rG,rF分别为后轮、质心、前轮的转弯半径，VR,VF为后轮和前轮的速度，用一组坐标[X,Y,ψ]表示车辆当前时刻的位置；
转向角与偏移角速率直接相关，得到：
这里，δ为前轮转向角，lf,lr分别为前轮和后轮与质心的距离，β为侧滑角，得到：
这里VG为质心的速度，ψ为车辆的方向角；
步骤(2)所述的建立车辆动力学模型方法如下：
将考虑车辆滑动和横向速度考虑进去时，得到相应的车辆动力学模型，定义轮胎滑移角为车轮速度矢量与车轮轴向的夹角，轮胎滑移角正比于作用于轮胎上侧向力；
根据质心的受力情况和运动状态可以得到：
其中，ΣFy表示总侧向力，Fyf,Fyr分别为前后轮侧向力，ay表示总侧向加速度，m表示车辆的质量，ΣMz为总力矩，Iz车辆的偏航惯性矩，Vx,Vy分别表示车辆纵向和横向速度；
这里，Cf，Cr分别为前后轮的侧偏刚度，αf，αr为前后轮的滑移角；
联立方程可得：
对于确定的车辆参数m，Iz,lf,lr,Cf,Cr都是确定的。
2.根据权利要求1所述的基于联邦滤波的INS/VKM/VDM车载导航系统，其特征在于：
步骤(3)所述的建立车载惯性导航/运动学模型/动力学模型组合导航系统方法如下：
将车辆运动学模型和动力学模型同时用于辅助惯导系统采用联邦滤波方法以提升车载导航系统的定位精度与可靠性，
以惯性导航系统作为公共参考系统，以VKM、VDM为子系统构成车载惯导/VKM/VDM组合导航系统，INS与VKM构成的子组合系统经局部滤波器Ⅰ滤波，INS与VDM构成的子组合系统经局部滤波器Ⅱ滤波，并将每一步的滤波输出X1,P1,X2,P2传递给主滤波器；主滤波器利用惯导系统信息进行独立时间，再利用各子滤波器和主滤波器的输出Xm,Pm进行最优融合得到全局最优估计Xg,Pg；按信息分配原则将Xg,Pg分配到主滤波器和各子滤波器中，提高定位精度与可靠性；
即：X1＝X2＝Xm＝Xg， 取分配系数为β1＝β2＝βm＝
1/3；
车辆运动学模型可以提供车辆位置和行驶方向上的速度信息，但是无法提供与车辆行驶方向垂直方向上的速度信息，在二维平面中行驶的车辆受到两个非完整性约束条件的限制：即与前行方向垂直的方向上的速度等于零，实际应用中可用高斯白噪声来模拟满足非完整约束的情况:
式中，Vbx,Vbz是车辆在机体坐标系b中x，z方向上的速度分量，vx,vz是均值为0，方差分别为 的高斯白噪声；
假设车辆前行方向速度为Vb(t)，若转换到导航坐标系n，则运动学模型辅助惯性导航系统的速度观测量可表示为：
这里V1为子滤波器1的速度观测量， 为机体系到导航系的转移矩阵，n为导航坐标系，b为机体坐标系；
对于动力学模型可以提供横向速度Vbx但不能提供车辆的天向速度，V2为子滤波器2的速度观测量，则动力学模型辅助惯性导航系统的速度观测量可表示为：
3.根据权利要求1所述的基于联邦滤波的INS/VKM/VDM车载导航系统，其特征在于：
步骤(4)所述的建立车载组合导航系统状态方程方法如下：
该方案以INS为公共参考系统，则全局状态矢量为：
式中， 为东北天三个方向平台角误差，δVE,δVN,δVU为东北天三方向上的速度误差，δL,δλ,δh为位置误差；
参考系统状态方程为：
式中，F(t)为相对于九个基本导航误差量的系数矩阵；G(t)为误差系数矩阵，W(t)为系统动态噪声阵；子系统的状态矢量及状态方程与全局状态矢量，状态方程相同；即Xi＝Xg(i＝1,2)；
步骤(5)所述的建立车载组合导航系统量测方程方法如下：
VKM/INS子系统和VDM/INS子系统和非完整性约束条件辅助惯性导航系统，量测方程可一致表述为：
Z(t)＝H(t)X(t)+V(t)   (13)
惯性导航系统输出的位置和速度信息与模型输出的位置、速度观测量的差值作为观测量；取子系统量测周期为0.5s，定义观测量如下式：
式中，PN,VN为惯导输出的位置、速度，Pi,Vi为模型输出的位置、速度，其中i＝1,2；
式中，RM,RN分别为子午圈和卯酉圈曲率半径，L为当地纬度，观测噪声V(t)为随机白噪声，其值与里程计仿真精度有关；量测噪声协方差阵R(t)＝diag(Rp(t),Rv(t))，其中，Rp和Rv是位置和速度观测量的噪声方差阵，其值的大小由模型误差、控制不确定性、差动里程计的量测噪声和车辆不满足非完整性约束条件的程度确定。

基于联邦滤波的INS/VKM/VDM车载导航系统
技术领域
[0001] 本发明属于车辆导航技术领域，特别涉及一种基于联邦滤波的INS/VKM/VDM车载导航系统，适用于车辆的自主导航。
背景技术
[0002] 智能交通系统(Intelligent Transportation System,ITS)的发展依赖于导航系统的发展，这些系统需要提供车辆准确的位置信息。低成本的基于微机电系统(Micro Electro Mechanical Systems,MEMS)的惯性测量单元(Inertial Measurement Unit,IMU)能提供三维姿态，位置和速度信息在车载导航系统中已有较多应用。但导航误差随时间累积，不能长时间单独使用，一般与地图匹配(Map Match,MM)、全球定位系统(Global Positioning System,GPS)等导航系统组合采用信息融合技术以抑制导航误差提高精度与可靠性。
[0003] 在城市环境中由于GPS信号被遮挡、多路径效应、人为干扰等原因使得GPS信号不可靠，复杂的城市交通系统也使得MM的匹配精度下降，导致车载GPS/INS、MM/INS组合导航系统仅运行在纯惯性导航系统模式下而不能提供车辆的准确的位置信息，需要另外的辅助手段来抑制导航误差。
发明内容
[0004] 本发明为了克服上述缺陷，目的在于提供一种针对车载导航系统运行在纯惯性导航系统模式下，导航误差迅速发散的问题，提出了一种利用车辆运动学模型和动力学模型辅助惯性导航系统的基于联邦滤波的INS/VKM/VDM车载导航系统。该车载导航系统能有效抑制纯惯性导航的误差随时间累积问题，提高导航精度和可靠性。
[0005] 为了达到上述目的，本发明采用如下技术方案：
[0006] 一种基于联邦滤波的INS/VKM/VDM车载组合导航系统，包括下列步骤：
[0007] (1)建立车辆运动学模型；
[0008] (2)建立车辆动力学模型；
[0009] (3)建立车载惯性导航/运动学模型/动力学模型组合导航系统；(4)建立车载组合导航系统状态方程；
[0010] (5)建立车载组合导航系统量测方程。
[0011] 步骤(1)所述建立的车辆运动学模型方法如下：
[0012] 车辆运动学模型简化为两自由度的自行车模型，前后轮都用一个单轮替代；G为车辆质心，O为转弯的圆心，rR,rG,rF分别为后轮、质心、前轮的转弯半径，VR,VF为后轮和前轮的速度，用一组坐标[X,Y,ψ]表示车辆当前时刻的位置；
[0013] 转向角与偏移角速率直接相关，得到：
[0014]
[0015] 这里，δ为前轮转向角，lf,lr分别为前轮和后轮与质心的距离，β为侧滑角，得到：
[0016]
[0017] 这里VG为质心的速度，ψ为车辆的方向角；
[0018] 步骤(2)所述的建立车辆动力学模型方法如下：
[0019] 将考虑车辆滑动和横向速度考虑进去时，得到相应的车辆动力学模型，定义轮胎滑移角为车轮速度矢量与车轮轴向的夹角，轮胎滑移角正比于作用于轮胎上侧向力；
[0020] 根据质心的受力情况和运动状态可以得到：
[0021]
[0022]
[0023] 其中，ΣFy表示总侧向力，Fyf,Fyr分别为前后轮侧向力，ay表示总侧向加速度，m表示车辆的质量，ΣMz为总力矩，Iz车辆的偏航惯性矩，Vx,Vy分别表示车辆纵向和横向速度；
[0024]
[0025]
[0026] 这里，Cf，Cr分别为前后轮的侧偏刚度，αfαr为前后轮的滑移角；
[0027] 联立方程可得：
[0028]
[0029] 对于确定的车辆参数m，Iz,lf,lr,Cf,Cr都是确定的；
[0030] 步骤(3)所述的建立车载惯性导航/运动学模型/动力学模型组合导航系统方法如下：
[0031] 将车辆运动学模型和动力学模型同时用于辅助惯导系统采用联邦滤波方法以提升车载导航系统的定位精度与可靠性，
[0032] 以惯性导航系统作为公共参考系统，以VKM、VDM为子系统构成车载惯导/VKM/VDM组合导航系统，INS与VKM构成的子组合系统经局部滤波器Ⅰ滤波，INS与VDM构成的子组合系统经局部滤波器Ⅱ滤波，并将每一步的滤波输出X1,P1,X2,P2传递给主滤波器；主滤波器利用惯导系统信息进行独立时间，再利用各子滤波器和主滤波器的输出Xm,Pm进行最优融合得到全局最优估计Xg,Pg；按信息分配原则将Xg,Pg分配到主滤波器和各子滤波器中，提高定位精度与可靠性；
[0033] 即：X1＝X2＝Xm＝Xg， 取分配系数为β1＝β2
＝βm＝1/3；
[0034] 车辆运动学模型可以提供车辆位置和行驶方向上的速度信息，但是无法提供与车辆行驶方向垂直方向上的速度信息，在二维平面中行驶的车辆受到两个非完整性约束条件的限制：即与前行方向垂直的方向上的速度等于零，实际应用中可用高斯白噪声来模拟满足非完整约束的情况:
[0035]
[0036] 式中，Vbx,Vbz是车辆在机体坐标系b中x，z方向上的速度分量，vx,vz是均值为0，方差分别为 的高斯白噪声；
[0037] 假设车辆前行方向速度为Vb(t)，若转换到导航坐标系n，则运动学模型辅助惯性导航系统的速度观测量可表示为：
[0038]
[0039] 这里V1为子滤波器1的速度观测量， 为机体系到导航系的转移矩阵，n为导航坐标系，b为机体坐标系；
[0040] 对于动力学模型可以提供横向速度Vbx但不能提供车辆的天向速度，V2为子滤波器
2的速度观测量，则动力学模型辅助惯性导航系统的速度观测量可表示为：
[0041]
[0042] 步骤(4)所述的建立车载组合导航系统状态方程方法如下：
[0043] 该方案以INS为公共参考系统，则全局状态矢量为：
[0044]
[0045] 式中， 为东北天三个方向平台角误差，δVE,δVN,δVU为东北天三方向上的速度误差，δL,δλ,δh为位置误差；
[0046] 参考系统状态方程为：
[0047]
[0048] 式中，F(t)为相对于九个基本导航误差量的系数矩阵；G(t)为误差系数矩阵，W(t)为系统动态噪声阵；子系统的状态矢量及状态方程与全局状态矢量，状态方程相同；即Xi＝Xg(i＝1,2)；
[0049] 步骤(5)所述的建立车载组合导航系统量测方程方法如下：
[0050] VKM/INS子系统和VDM/INS子系统和非完整性约束条件辅助惯性导航系统，量测方程可一致表述为：
[0051] Z(t)＝H(t)X(t)+V(t)  (13)
[0052] 惯性导航系统输出的位置和速度信息与模型输出的位置、速度观测量的差值作为观测量；取子系统量测周期为0.5s，定义观测量如下式：
[0053]
[0054] 式中，PN,VN为惯导输出的位置、速度，Pi,Vi为模型输出的位置、速度，其中i＝1,2。
[0055]
[0056] 式中，RM,RN分别为子午圈和卯酉圈曲率半径，L为当地纬度，观测噪声V(t)为随机白噪声，其值与里程计仿真精度有关；量测噪声协方差阵R(t)＝diag(Rp(t),Rv(t))，其中，Rp和Rv是位置和速度观测量的噪声方差阵，其值的大小由模型误差、控制不确定性、差动里程计的量测噪声和车辆不满足非完整性约束条件的程度确定。
[0057] 本发明的方法具有如下优点：不需要引入其它辅助设备，只综合考虑车辆本身的运动状态，提高了车载导航的定位精度与可靠性。算法简单易行，基本不增加系统算法的复杂性和计算量。对以上发明的有益效果说明如下：
[0058] 设计车辆以30m/s作匀速转弯运动，轨迹为圆。假设加速度计量测噪声为随机白噪声，标准差为1×10-3g。陀螺仪的量测噪声为随机常值漂移加随机白噪声，标准差均为10(°)/h。差动里程计输出的车辆速度输出白噪声标准差为0.5m/s，常值误差为0.1m/s，横摆角为白噪声，标准差为0.05rad/s。仿真步长为0.02s，仿真500s
[0059] 本发明给出了综合车辆运动学模型和动力学辅助方法与纯惯性导航水平位置对比曲线和车辆东向、北向速度误差协方差对比曲线，如从图4-图6所示。运动学辅助方法精度明显优于纯惯性导航，能够有效地抑制惯性导航误差累积。
附图说明
[0060] 图1车辆运动学模型示意图。
[0061] 图2车辆运动学模型示意图。
[0062] 图3基于联邦滤波的辅助导航方法框图。
[0063] 图4综合车辆运动学模型和动力学辅助方法与纯惯性导航水平位置对比图。
[0064] 图5两种方法车辆东西速度误差协方差对比图。
[0065] 图6两种方法车辆东西速度误差协方差对比图。
具体实施方式
[0066] 下面结合附图1～6对本发明进行详细说明：
[0067] 一种基于联邦滤波的INS/VKM/VDM车载组合导航系统，其特征在于包括下列步骤：
[0068] (1)建立车辆运动学模型；
[0069] (2)建立车辆动力学模型；
[0070] (3)建立车载惯性导航/运动学模型/动力学模型组合导航系统；(4)建立车载组合导航系统状态方程；
[0071] (5)建立车载组合导航系统量测方程。
[0072] 步骤(1)所述建立的车辆运动学模型方法如下：
[0073] 车辆运动学模型简化为两自由度的自行车模型，如图1所示前后轮都用一个单轮替代；G为车辆质心，O为转弯的圆心，rR,rG,rF分别为后轮、质心、前轮的转弯半径，VR,VF为后轮和前轮的速度，用一组坐标[X,Y,ψ]表示车辆当前时刻的位置；
[0074] 转向角与偏移角速率直接相关，得到：
[0075]
[0076] 这里，δ为前轮转向角，lf,lr分别为前轮和后轮与质心的距离，β为侧滑角，得到：
[0077]
[0078] 这里VG为质心的速度，ψ为车辆的方向角。
[0079] 步骤(2)所述的建立车辆动力学模型方法如下：
[0080] 将考虑车辆滑动和横向速度考虑进去时，得到相应的车辆动力学模型，如图2所示：定义轮胎滑移角为车轮速度矢量与车轮轴向的夹角，轮胎滑移角正比于作用于轮胎上侧向力；
[0081] 根据质心的受力情况和运动状态可以得到：
[0082]
[0083]
[0084] 其中，ΣFy表示总侧向力，Fyf,Fyr分别为前后轮侧向力，ay表示总侧向加速度，m表示车辆的质量，ΣMz为总力矩，Iz车辆的偏航惯性矩，Vx,Vy分别表示车辆纵向和横向速度；
[0085]
[0086]
[0087] 这里，Cf，Cr分别为前后轮的侧偏刚度，αfαr为前后轮的滑移角；
[0088] 联立方程可得：
[0089]
[0090] 对于确定的车辆参数m，Iz,lf,lr,Cf,Cr都是确定的。
[0091] 步骤(3)所述的建立车载惯性导航/运动学模型/动力学模型组合导航系统方法如下：
[0092] 将车辆运动学模型和动力学模型同时用于辅助惯导系统采用联邦滤波方法以提升车载导航系统的定位精度与可靠性，其具体思路如图3所示。
[0093] 以惯性导航系统作为公共参考系统，以VKM、VDM为子系统构成车载惯导/VKM/VDM组合导航系统，INS与VKM构成的子组合系统经局部滤波器Ⅰ滤波，INS与VDM构成的子组合系统经局部滤波器Ⅱ滤波，并将每一步的滤波输出X1,P1,X2,P2传递给主滤波器；主滤波器利用惯导系统信息进行独立时间，再利用各子滤波器和主滤波器的输出Xm,Pm进行最优融合得到全局最优估计Xg,Pg；按信息分配原则将Xg,Pg分配到主滤波器和各子滤波器中，提高定位精度与可靠性；
[0094] 即：X1＝X2＝Xm＝Xg， 取分配系数为β1＝β2
＝βm＝1/3；
[0095] 车辆运动学模型可以提供车辆位置和行驶方向上的速度信息，但是无法提供与车辆行驶方向垂直方向上的速度信息，在二维平面中行驶的车辆受到两个非完整性约束条件的限制：即与前行方向垂直的方向上的速度等于零，实际应用中可用高斯白噪声来模拟满足非完整约束的情况:
[0096]
[0097] 式中，Vbx,Vbz是车辆在机体坐标系b中x，z方向上的速度分量，vx,vz是均值为0，方差分别为 的高斯白噪声；
[0098] 假设车辆前行方向速度为Vb(t)，若转换到导航坐标系n，则运动学模型辅助惯性导航系统的速度观测量可表示为：
[0099]
[0100] 这里V1为子滤波器1的速度观测量， 为机体系到导航系的转移矩阵，n为导航坐标系，b为机体坐标系；
[0101] 对于动力学模型可以提供横向速度Vbx但不能提供车辆的天向速度，V2为子滤波器
2的速度观测量，则动力学模型辅助惯性导航系统的速度观测量可表示为：
[0102]
[0103] 步骤(4)所述的建立车载组合导航系统状态方程方法如下：
[0104] 该方案以INS为公共参考系统，则全局状态矢量为：
[0105]
[0106] 式中， 为东北天三个方向平台角误差，δVE,δVN,δVU为东北天三方向上的速度误差，δL,δλ,δh为位置误差；
[0107] 参考系统状态方程为：
[0108]
[0109] 式中，F(t)为相对于九个基本导航误差量的系数矩阵；G(t)为误差系数矩阵，W(t)为系统动态噪声阵；子系统的状态矢量及状态方程与全局状态矢量，状态方程相同；即Xi＝Xg(i＝1,2)。
[0110] 步骤(5)所述的建立车载组合导航系统量测方程方法如下：
[0111] VKM/INS子系统和VDM/INS子系统和非完整性约束条件辅助惯性导航系统，量测方程可一致表述为：
[0112] Z(t)＝H(t)X(t)+V(t)  (13)
[0113] 惯性导航系统输出的位置和速度信息与模型输出的位置、速度观测量的差值作为观测量；取子系统量测周期为0.5s，定义观测量如下式：
[0114]
[0115] 式中，PN,VN为惯导输出的位置、速度，Pi,Vi为模型输出的位置、速度，其中i＝1,2。
[0116]
[0117] 式中，RM,RN分别为子午圈和卯酉圈曲率半径，L为当地纬度，观测噪声V(t)为随机白噪声，其值与里程计仿真精度有关；量测噪声协方差阵R(t)＝diag(Rp(t),Rv(t))，其中，Rp和Rv是位置和速度观测量的噪声方差阵，其值的大小由模型误差、控制不确定性、差动里程计的量测噪声和车辆不满足非完整性约束条件的程度确定。
[0118] 综上所述，对该方法进行效果分析。如图4所示。图中实线所示为理想轨迹，点划线为纯惯性导航轨迹，虚线为运动学辅助方法。可见运动学辅助方法与理想轨迹较为吻合、误差较小，误差发散较为缓慢，轨迹为闭合的圆。而纯惯性导航轨迹在较短时间内与理想轨迹较为吻合，但后期误差累积较为明显，并呈现迅速发散的特点，轨迹不是闭合的圆。由此可以看出运动学辅助方法精度明显优于纯惯性导航，能够有效地抑制惯导误差累积。
[0119] 图5-6给出了运动学模型辅助方法与综合考虑车辆动力学模型和运动学模型辅助方法的东向和北向速度误差协方差曲线。由于里程计的前向速度误差是有界的，并且在引入了非完整约束条件后使得车辆东向、北向误差均有界，误差并不发散，，仅用运动学模型辅助的稳态值在0.07m/s左右，而在综合考虑运动学模型和动力学模型的辅助方法东、北向速度误差稳定在0.04m/s左右。由图中可以看出综合模型收敛速度快于动力学模型，而且收敛误差小于仅用动力学模型辅助。可以看出综合模型比单纯的动力学模型具有更高的精度。
[0120] 本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。