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)放宽这些约束:nhlolo nhlolo
其中,e 为高斯白噪声,协方差矩阵为R ,用以反映相应约束条件的放松程度。
4.根据权利要求1所述的兼顾车辆动力学和非完整约束的融合定位方法,其特征在于,在所述步骤6中,基于车辆动力学的航迹推算模型如下:其中,X、Y表示车辆在当地水平坐标系中的坐标,vx、vy表示其速度在车体坐标系中的表达,ψ表示车辆的横摆角速度。
5.根据权利要求4所述的兼顾车辆动力学和非完整约束的融合定位方法,其特征在于,在所述步骤6中,扩展卡尔曼滤波的状态空间模型的搭建包括以下步骤:步骤6.1、建立系统非线性状态方程如下:
xk=f(xk‑1,uk)+wk,cov(wk)=Qk (7)xk=f(xk‑1,uk)具体为下列方程:
ψk=ψk‑1+Tsrk‑1
其中,vx、vy分别是车速沿车体坐标系x轴和y轴方向的分量,r是车辆的横摆角速度,δ是前轮转角,lf、lr分别是从车辆的质心到前后轴的距离,Cαf、Cαr分别为单个前轮轮胎和单个后轮轮胎的侧偏刚度;
步骤6.2、建立系统观测方程如下:
其中,vrL和vrR分别代表左右后轮中心的速度,dr代表后轮轮距;利用后轮轮速与车辆的纵向速度和横摆角速度之间的关系,将ABS轮速传感器的测量值也用于观测方程的建立。