一种兼顾车辆动力学和非完整约束的融合定位方法技术

本发明专利技术适用于车辆定位导航技术领域，提供了一种兼顾车辆动力学和非完整约束的融合定位方法，包括以下步骤：读取GPS接收机、IMU传感器、车辆CAN等提供的信息；根据读取的信息建立坐标系；根据IMU传感器与GPS接收机（或UWB）提供的信息，通过INS运动学模型解算出位置、速度和姿态信息；建立状态方程和观测方程，利用误差状态卡尔曼滤波估计导航误差值和IMU传感器偏差值，结果用来对INS运动学模型进行修正；基于车辆动力学建立航迹推算模型，建立状态方程和观测方程，利用扩展卡尔曼滤波估计位置和航向角信息。该方法可提高GNSS信号受干扰或中断时导航定位的精度，有效提高定位精度并延长定位时间。位时间。位时间。

【技术实现步骤摘要】
一种兼顾车辆动力学和非完整约束的融合定位方法


[0001]本专利技术属于车辆定位导航
，尤其涉及一种兼顾车辆动力学和非完整约束的融合定位方法。

技术介绍

[0002]智能车辆的决策、轨迹规划和运动控制的实现，高度依赖于精确和连续的位置信息，GNSS是一种公认的获取车辆位置的方法，可以提供与实时运动学(RTK)相结合的亚分米定位精度。然而，由于复杂的城市环境，其高精度定位可能会因信号中断而退化。INS可以在短时间内确定位置、速度和姿态信息，但加速度计和陀螺仪的偏差和噪声会导致误差随时间累积。通过集成GNSS和INS，可以实现更准确和健壮的导航解决方案。
[0003]然而，因为在GNSS中断期间只有INS运行，所以GNSS/INS导航系统不能在GNSS易受干扰的环境中提供准确的定位信息。航迹推算系统可以根据传感器信息独立推算位置而不依赖GNSS信息，可以在GNSS信号短时间断开的情况下提供定位信息，但是单独的DR算法不能满足智能车辆对定位精度的需求，这是由于实际行驶过程中，路面颠簸导致的二维平面假设不成立会引起较大的误差。UWB可以提供高精度的定位信息，但UWB设备需要预先安装，难以实现室外道路全覆盖，一般用于室内定位。

技术实现思路

[0004]本专利技术实施例的目的在于提供一种兼顾车辆动力学和非完整约束的融合定位方法，旨在解决上述
技术介绍
中提出的问题。
[0005]本专利技术实施例是这样实现的，一种兼顾车辆动力学和非完整约束的融合定位方法，包括以下步骤：
[0006]步骤1、读取GPS接收机提供的位置、速度和姿态信息，IMU传感器提供的加速度和角速度信息车辆CAN总线提供的车速、轮速和前轮转角信息，以及在室内预先安装的UWB设备提供的位置信息；
[0007]步骤2、对读取的信息进行预处理，建立相应的坐标系；
[0008]步骤3、根据水平几何精度因子，即HDOP值判断GNSS信号是否可用，若不可用，则读取UWB提供的位置信息；
[0009]步骤4、根据IMU传感器与GPS接收机，或IMU传感器与UWB提供的信息，通过INS运动学模型解算出位置、速度和姿态信息；
[0010]步骤5、建立状态方程和观测方程，利用误差状态卡尔曼滤波(ESKF)估计导航误差值和IMU传感器偏差值，所得的导航误差估计值和IMU传感器偏差估计值用来对INS运动学模型进行修正；
[0011]步骤6、基于车辆动力学建立航迹推算模型(DR)，建立状态方程和观测方程，利用扩展卡尔曼滤波(EKF)估计位置和航向角信息；
[0012]步骤S7，输出最终估计的位置信息和航向角信息。
[0013]进一步的技术方案，在所述步骤4中，INS运动学模型如下：
[0014][0015]其中，p
k
、v
k
、ε
k
表示GPS接收机提供的(或UWB提供的)位置、速度和姿态(用单位四元数表示)，s
k
、ω
k
表示惯性测量单元(IMU)提供的三维比例和角速度信息，是本体坐标系到导航平台坐标系的旋转矩阵。
[0016]进一步的技术方案，在所述步骤3中，当HDOP小于阈值，则依赖GPS接收机信号进行位置估计；当HDOP大于阈值，则依赖UWB信号进行位置估计。
[0017]进一步的技术方案，在所述步骤5中，误差状态卡尔曼滤波系统的状态空间模型的搭建包括以下步骤：
[0018]步骤5.1、建立系统状态方程如下：
[0019][0020]其中，δp
k
、δv
k
、ε
k
分别表示导航平台位置、速度、姿态的实际值与名义值之间的误差；δs
k
、δω
k
分别表示缓慢变化的IMU测得的比例和角速度偏差；表示测量噪声，设为高斯白噪声，协方差矩阵为Q
m
；表示随机游走的过程噪声，设为高斯白噪声，协方差矩阵为Q
b
，假设随机游走过程噪声与测量噪声不相关；ε
k
为一个欧拉角向量，可根据式(3)求得代表导航平台姿态实际值的单位四元数q：
[0021][0022]步骤5.2、建立系统观测方程如下：
[0023][0024]其中，是x，y方向上速度的虚拟测量值，根据非完整约束确定，和分别表示GPS接收机给出的位置和速度的测量噪声，假设为不相关的高斯白噪声，协方差矩阵为和和为CAN总线上车速的测量噪声，设其为高斯白噪声，用协方差矩阵为为CAN总线上车速的测量噪声，设其为高斯白噪声，用协方差矩阵为是从导航平台坐标系到车体坐标系的旋转矩阵，可通过标定获得，是当地水平
坐标系到导航平台坐标系的旋转矩阵，表示将当地水平坐标系下的导航平台速度转换为车体坐标系下的车辆质心速度。
[0025]进一步的技术方案，在所述步骤5.2中，非完整约束假设车辆在平面运动过程中不会侧滑并且在与路面垂直的方向上没有运动，即车辆质心处沿车体坐标系下的y轴和z轴速度为零，但实际情况由于转向时存在侧滑、道路颠簸等原因使得这两个方向的速度不完全为零，因此根据式(5)放宽这些约束：
[0026][0027]其中，e
nhlolo
为高斯白噪声，协方差矩阵为R
nhlolo
，用以反映相应约束条件的放松程度。
[0028]进一步的技术方案，在所述步骤6中，基于车辆动力学的航迹推算模型如下：
[0029][0030]其中，X、Y表示车辆在当地水平坐标系中的坐标，v
x
、v
y
表示其速度在车体坐标系中的表达，ψ表示车辆的横摆角速度。
[0031]进一步的技术方案，在所述步骤6中，扩展卡尔曼滤波的状态空间模型的搭建包括以下步骤：
[0032]步骤6.1、建立系统非线性状态方程如下：
[0033]x
k
＝f(x
k
‑1,u
k
)+w
k
,cov(w
k
)＝Q
k
ꢀꢀ
(7)
[0034]x
k
＝f(x
k
‑1,u
k
)具体为下列方程：
[0035][0036][0037][0038][0039][0040]ψ
k
＝ψ
k
‑1+T
s
r
k
‑1[0041]其中，v
x
、v
y
分别是车速沿车体坐标系x轴和y轴方向的分量，r是车辆的横摆角速度，δ是前轮转角，l
f
、l
r
分别是从车辆的质心到前后轴的距离，C
αf
、C
αr
分别为单个前轮轮胎和单个后轮轮胎的侧偏刚度；
[0042]步骤6.2、建立系统观测方程如下：
[004本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种兼顾车辆动力学和非完整约束的融合定位方法，其特征在于，包括以下步骤：步骤1、读取GPS接收机提供的位置、速度和姿态信息，IMU传感器提供的加速度和角速度信息车辆CAN总线提供的车速、轮速和前轮转角信息，以及在室内预先安装的UWB设备提供的位置信息；步骤2、对读取的信息进行预处理，建立相应的坐标系；步骤3、根据水平几何精度因子，即HDOP值判断GNSS信号是否可用，若不可用，则读取UWB提供的位置信息；步骤4、根据IMU传感器与GPS接收机，或IMU传感器与UWB提供的信息，通过INS运动学模型解算出位置、速度和姿态信息；步骤5、建立状态方程和观测方程，利用误差状态卡尔曼滤波估计导航误差值和IMU传感器偏差值，所得的导航误差估计值和IMU传感器偏差估计值用来对INS运动学模型进行修正；步骤6、基于车辆动力学建立航迹推算模型，建立状态方程和观测方程，利用扩展卡尔曼滤波估计位置和航向角信息；步骤S7，输出最终估计的位置信息和航向角信息。2.根据权利要求1所述的兼顾车辆动力学和非完整约束的融合定位方法，其特征在于，在所述步骤3中，当HDOP小于阈值，依赖GPS接收机信号进行位置估计；当HDOP大于阈值，则依赖UWB信号进行位置估计。3.根据权利要求1所述的兼顾车辆动力学和非完整约束的融合定位方法，其特征在于，非完整约束假设车辆在平面运动过程中不会侧滑并且在与路面垂直的方向上没有运动，即车辆质心处沿车体坐标系下的y轴和z轴速度为零，但实际情况由于转向时存在侧滑、道路颠簸等原因使得这两个方向的速度不完全为零，因此根据式(5)放宽这些约束：其中，e
nhlolo
为高斯白噪声，协方差矩阵为R
nhlolo
，用以反映相应约束条件的放松程度。4.根据权利要求1所述的兼顾车辆动力学和非完整约束的融合定位方法，其特征在于，在所...

【专利技术属性】
技术研发人员：陈国迎，王佳琦，
申请(专利权)人：吉林大学，
类型：发明
国别省市：