[发明专利]一种车辆组合定位算法

摘要

本发明公开了一种新型车辆组合定位算法，采用联邦卡尔曼滤波数据融合技术，实现路侧设备定位(RSU)、卫星定位(GNSS)、航位推算(DR)三个定位子系统的信息融合，即采用线性卡尔曼滤波器作为RSU定位子系统的局部滤波器，记为LF1，对应的信息分配系数是β₁；GNSS定位子系统也采用线性卡尔曼滤波器作为局部滤波器，记为LF2，对应的信息分配系数为β₂；DR定位子系统采用扩展卡尔曼滤波器作为局部滤波器，记为LF3，对应的信息分配系数为β₃，主滤波器做信息融合；利用残差χ²检测和冗余硬件检测构建了两级故障检测方法，能同时检测出子系统的硬故障和软故障；同时根据定位子系统的定位精度因子来动态调整组合系统的信息分配系数，使系统能够快速适应环境的变化，提高车辆定位的精度与可靠性。

主权项

1.一种车辆组合定位算法，其特征在于，采用联邦卡尔曼滤波算法，实现RSU定位、GNSS定位、DR定位三个定位子系统的信息融合，即采用线性卡尔曼滤波器作为RSU定位子系统的局部滤波器，记为LF1，对应的信息分配系数是β1；GNSS定位子系统也采用线性卡尔曼滤波器作为局部滤波器，记为LF2，对应的信息分配系数为β2；DR定位系统采用扩展卡尔曼滤波作为局部滤波器，记为LF3，对应的信息分配系数为β3，主滤波器做信息融合，具体包括下述步骤：S1、根据汽车运动学建立系统状态方程与观测方程，所述观测方程包括RSU观测方程、GNSS观测方程以及DR观测方程；S2、根据LF1、LF2和LF3对三个子系统进行时间更新和量测更新；S3、在信息融合过程中进行故障检测，利用两级故障检测技术，检测出故障子系统并实现隔离；S4、信息融合与信息分配，系统在信息分配过程中，根据定位子系统的定位精度因子来动态调整各子系统的信息分配系数，实现故障子系统的隔离与无故障子系统的重构；在步骤S4中，所述两级故障检测的方法：首先各子系统利用残差χ2检测法去检测出现硬故障的子系统，然后利用冗余硬件检测法去检测系统软故障，另外，为了能校验车载定位系统本身的精确度，还提出了基于车辆与路侧设备通信V2R的位置参照检测法，让系统可以及时发现因车载设备自身问题而导致定位不准确的原因。