CN115574818A - 基于改进联邦滤波的结构化道路车辆导航定位方法及系统 - Google Patents

基于改进联邦滤波的结构化道路车辆导航定位方法及系统 Download PDF

Info

Publication number
CN115574818A
CN115574818A CN202211575680.0A CN202211575680A CN115574818A CN 115574818 A CN115574818 A CN 115574818A CN 202211575680 A CN202211575680 A CN 202211575680A CN 115574818 A CN115574818 A CN 115574818A
Authority
CN
China
Prior art keywords
covariance matrix
state
sub
measurement data
updated
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202211575680.0A
Other languages
English (en)
Other versions
CN115574818B (zh
Inventor
张旭东
唐嘉乔
邹渊
范晓临
尹鑫
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beijing Institute of Technology BIT
Original Assignee
Beijing Institute of Technology BIT
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Beijing Institute of Technology BIT filed Critical Beijing Institute of Technology BIT
Priority to CN202211575680.0A priority Critical patent/CN115574818B/zh
Publication of CN115574818A publication Critical patent/CN115574818A/zh
Application granted granted Critical
Publication of CN115574818B publication Critical patent/CN115574818B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Landscapes

  • Navigation (AREA)

Abstract

本发明涉及一种基于改进联邦滤波的结构化道路车辆导航定位方法及系统，属于车辆导航定位技术领域，基于惯性测量单元采集的第一测量数据、激光雷达采集的第二测量数据和全球导航卫星系统采集的第三测量数据对各个子滤波器的状态和协方差矩阵进行更新，并计算各个子滤波器的前向分配系数，以在主滤波器进行融合，得到融合后状态和融合后协方差矩阵，融合后状态即为当前定位时刻车辆的导航定位信息，从而以联邦滤波为基本框架，融合惯性测量单元、激光雷达和全球导航卫星系统，充分发挥各种传感器在不同场景下的优势，实现了在场景多变的结构化道路下车辆的高精度的导航与定位。

Description

基于改进联邦滤波的结构化道路车辆导航定位方法及系统
技术领域
本发明涉及车辆导航定位技术领域，特别是涉及一种基于改进联邦滤波的结构化道路车辆导航定位方法及系统。
背景技术
高精度的车辆导航与定位是实现车辆自动驾驶的先决条件，采样融合多种传感器的方式进行车辆的导航与定位正在逐渐成为主流的车辆导航与定位方式。不同传感器具有不同的适用场景，面对目前复杂多变的结构化道路场景，需要采用融合算法将多种传感器进行融合，以在各种场景下均能保证导航定位的准确度。目前车辆导航与定位常用的传感器为惯性测量单元IMU，激光雷达以及全球导航卫星系统GNSS，其中惯性测量单元IMU可以提供导航定位所需的基础数据，但受其工作原理和精度限制，需要将其与其他传感器进行融合使用，否则将产生累计误差，影响导航定位精度；而激光雷达在周围建筑较多等环境信息较为丰富的条件下可以更好的实现定位功能，但在较为空旷的场景下将会影响其使用效果；与激光雷达相反，在环境信息较为丰富的条件下往往全球导航卫星系统GNSS的精度将受到巨大的干扰，但在空旷场景下可以达到较高的精度。
基于此，亟需一种能够良好融合惯性测量单元，激光雷达和全球导航卫星系统的高精度车辆导航定位技术。
发明内容
本发明的目的是提供一种基于改进联邦滤波的结构化道路车辆导航定位方法及系统，以联邦滤波为基本框架，融合惯性测量单元、激光雷达和全球导航卫星系统，充分发挥各种传感器在不同场景下的优势，实现了在场景多变的结构化道路下车辆的高精度的导航与定位。
为实现上述目的，本发明提供了如下方案：
一种基于改进联邦滤波的结构化道路车辆导航定位方法，包括：
获取从上一定位时刻到当前定位时刻惯性测量单元采集的第一测量数据、激光雷达采集的第二测量数据和全球导航卫星系统采集的第三测量数据；所述第一测量数据包括各个惯导采样时刻采集的车辆加速度、车辆角速度和车辆航向角；所述第二测量数据包括各个雷达采样时刻采集的车辆周围的环境点云；所述第三测量数据包括各个卫星采样时刻采集的车辆经度、车辆纬度、车辆海拔以及车辆经度、车辆纬度和车辆海拔各自对应的均方根值；
以所述第一测量数据和所述第二测量数据作为输入，按照所述第二测量数据的采样频率对第一子滤波器的第一状态和第一协方差矩阵进行更新，得到当前定位时刻的第一更新后状态和第一更新后协方差矩阵；所述状态包括车辆姿态、车辆位置和车辆速度；
以所述第一测量数据和所述第三测量数据作为输入，按照所述第三测量数据的采样频率对第二子滤波器的第二状态和第二协方差矩阵进行更新，得到当前定位时刻的第二更新后状态和第二更新后协方差矩阵；
根据所述第一测量数据和所述第二测量数据进行回环检测，以对第三子滤波器的第三状态和第三协方差矩阵进行更新，得到当前定位时刻的第三更新后状态和第三更新后协方差矩阵；
根据所述第二测量数据和所述第三测量数据计算当前定位时刻的所述第一子滤波器的第一前向分配系数、所述第二子滤波器的第二前向分配系数和所述第三子滤波器的第三前向分配系数；
当达到当前定位时刻时，基于所述第一前向分配系数、所述第二前向分配系数和所述第三前向分配系数对所述第一更新后状态、所述第二更新后状态和所述第三更新后状态进行融合，得到融合后状态；基于所述第一前向分配系数、所述第二前向分配系数和所述第三前向分配系数对所述第一更新后协方差矩阵、所述第二更新后协方差矩阵和所述第三更新后协方差矩阵进行融合，得到融合后协方差矩阵；所述融合后状态即为当前定位时刻车辆的导航定位信息；
根据所述第一更新后协方差矩阵、所述第二更新后协方差矩阵和所述第三更新后协方差矩阵计算所述第一子滤波器的第一信息分配系数、所述第二子滤波器的第二信息分配系数和所述第三子滤波器的第三信息分配系数；基于所述第一信息分配系数、所述第二信息分配系数和所述第三信息分配系数对所述融合后状态和所述融合后协方差矩阵进行分配，得到下一定位时刻第一子滤波器的第一状态和第一协方差矩阵、第二子滤波器的第二状态和第二协方差矩阵以及第三子滤波器的第三状态和第三协方差矩阵，并返回“获取从上一定位时刻到当前定位时刻惯性测量单元采集的第一测量数据、激光雷达采集的第二测量数据和全球导航卫星系统采集的第三测量数据”的步骤，进行下一定位时刻车辆的导航定位。
一种基于改进联邦滤波的结构化道路车辆导航定位系统，包括：
数据获取模块，用于获取从上一定位时刻到当前定位时刻惯性测量单元采集的第一测量数据、激光雷达采集的第二测量数据和全球导航卫星系统采集的第三测量数据；所述第一测量数据包括各个惯导采样时刻采集的车辆加速度、车辆角速度和车辆航向角；所述第二测量数据包括各个雷达采样时刻采集的车辆周围的环境点云；所述第三测量数据包括各个卫星采样时刻采集的车辆经度、车辆纬度、车辆海拔以及车辆经度、车辆纬度和车辆海拔各自对应的均方根值；
更新模块，用于以所述第一测量数据和所述第二测量数据作为输入，按照所述第二测量数据的采样频率对第一子滤波器的第一状态和第一协方差矩阵进行更新，得到当前定位时刻的第一更新后状态和第一更新后协方差矩阵；所述状态包括车辆姿态、车辆位置和车辆速度；以所述第一测量数据和所述第三测量数据作为输入，按照所述第三测量数据的采样频率对第二子滤波器的第二状态和第二协方差矩阵进行更新，得到当前定位时刻的第二更新后状态和第二更新后协方差矩阵；根据所述第一测量数据和所述第二测量数据进行回环检测，以对第三子滤波器的第三状态和第三协方差矩阵进行更新，得到当前定位时刻的第三更新后状态和第三更新后协方差矩阵；
融合模块，用于根据所述第二测量数据和所述第三测量数据计算当前定位时刻的所述第一子滤波器的第一前向分配系数、所述第二子滤波器的第二前向分配系数和所述第三子滤波器的第三前向分配系数；当达到当前定位时刻时，基于所述第一前向分配系数、所述第二前向分配系数和所述第三前向分配系数对所述第一更新后状态、所述第二更新后状态和所述第三更新后状态进行融合，得到融合后状态；基于所述第一前向分配系数、所述第二前向分配系数和所述第三前向分配系数对所述第一更新后协方差矩阵、所述第二更新后协方差矩阵和所述第三更新后协方差矩阵进行融合，得到融合后协方差矩阵；所述融合后状态即为当前定位时刻车辆的导航定位信息；
分配模块，用于根据所述第一更新后协方差矩阵、所述第二更新后协方差矩阵和所述第三更新后协方差矩阵计算所述第一子滤波器的第一信息分配系数、所述第二子滤波器的第二信息分配系数和所述第三子滤波器的第三信息分配系数；基于所述第一信息分配系数、所述第二信息分配系数和所述第三信息分配系数对所述融合后状态和所述融合后协方差矩阵进行分配，得到下一定位时刻第一子滤波器的第一状态和第一协方差矩阵、第二子滤波器的第二状态和第二协方差矩阵以及第三子滤波器的第三状态和第三协方差矩阵，并返回“获取从上一定位时刻到当前定位时刻惯性测量单元采集的第一测量数据、激光雷达采集的第二测量数据和全球导航卫星系统采集的第三测量数据”的步骤，进行下一定位时刻车辆的导航定位。
根据本发明提供的具体实施例，本发明公开了以下技术效果：
本发明用于提供一种基于改进联邦滤波的结构化道路车辆导航定位方法及系统，获取从上一定位时刻到当前定位时刻惯性测量单元采集的第一测量数据、激光雷达采集的第二测量数据和全球导航卫星系统采集的第三测量数据，然后基于第一测量数据、第二测量数据和第三测量数据对各个子滤波器的状态和协方差矩阵进行更新，并计算各个子滤波器的前向分配系数，以在主滤波器进行融合，得到融合后状态和融合后协方差矩阵，融合后状态即为当前定位时刻车辆的导航定位信息，从而以联邦滤波为基本框架，融合惯性测量单元、激光雷达和全球导航卫星系统，充分发挥各种传感器在不同场景下的优势，实现了在场景多变的结构化道路下车辆的高精度的导航与定位。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为本发明实施例1所提供的结构化道路车辆导航定位方法的原理框图；
图2为本发明实施例2所提供的结构化道路车辆导航定位系统的系统框图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
本发明的目的是提供一种基于改进联邦滤波的结构化道路车辆导航定位方法及系统，以联邦滤波为基本框架，融合惯性测量单元、激光雷达和全球导航卫星系统，充分发挥各种传感器在不同场景下的优势，实现了在场景多变的结构化道路下车辆的高精度的导航与定位。
为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。
实施例1：
本实施例用于提供一种基于改进联邦滤波的结构化道路车辆导航定位方法，如图1所示，包括：
步骤1：获取从上一定位时刻到当前定位时刻惯性测量单元采集的第一测量数据、激光雷达采集的第二测量数据和全球导航卫星系统采集的第三测量数据；所述第一测量数据包括各个惯导采样时刻采集的车辆加速度、车辆角速度和车辆航向角；所述第二测量数据包括各个雷达采样时刻采集的车辆周围的环境点云；所述第三测量数据包括各个卫星采样时刻采集的车辆经度、车辆纬度、车辆海拔以及车辆经度、车辆纬度和车辆海拔各自对应的均方根值。
本实施例会同时获取多个传感器所提供的信息，传感器包括惯性测量单元IMU、激光雷达和全球导航卫星系统GNSS。所获取的数据具体包括：惯性测量单元提供的从上一定位时刻到当前定位时刻以内的各个惯导采样时刻的车辆加速度、车辆角速度和车辆航向角，激光雷达提供的从上一定位时刻到当前定位时刻以内的各个雷达采样时刻的车辆周围的环境点云，全球导航卫星系统提供的从上一定位时刻到当前定位时刻以内的各个卫星采样时刻的车辆经度、车辆纬度、车辆海拔以及车辆经度、车辆纬度和车辆海拔各自对应的均方根值RMS，在获取上述数据的同时确定其采样时间。
需要说明的是，惯导采样时刻、雷达采样时刻、卫星采样时刻和定位时刻是相互独立的，其中，惯性测量单元的采样频率高于激光雷达和全球导航卫星系统的采样频率。
本实施例的联邦滤波包括三个子滤波器和一个主滤波器，三个子滤波器分别接收来自主滤波器反馈的结果，主滤波器得到的融合后协方差矩阵
Figure DEST_PATH_IMAGE001
和融合后状态
Figure 702579DEST_PATH_IMAGE002
根据信息分配系数分别反馈给三个子滤波器，其中第一子滤波器F 1接收到的第一状态记为x 1，第一协方差矩阵记为
Figure DEST_PATH_IMAGE003
，第二子滤波器F 2接收到的第二状态记为x 2，第二协方差矩阵记为
Figure 767487DEST_PATH_IMAGE004
，第三子滤波器F 3接收到的第三状态记为x 3，第三协方差矩阵记为
Figure DEST_PATH_IMAGE005
。另外，本实施例的每个子滤波器都有一个初始的协方差矩阵，在收到主滤波器反馈的结果之前都独自进行预测更新，直到收到主滤波器反馈的结果，然后再以主滤波器反馈的结果继续进行更新，直到再次收到主滤波器反馈的结果，初始的协方差矩阵可根据车辆状态模型构建得到。
三个子滤波器和主滤波器的更新在时间上都是相对独立的，第一子滤波器根据第一测量数据和第二测量数据进行更新，其更新频率与激光雷达的采样频率是一致的，每采样得到一个第二测量数据即进行一次更新；第二子滤波器根据第一测量数据和第三测量数据进行更新，其更新频率与全球导航卫星系统的采样频率是一致的，每采样得到一个第三测量数据即进行一次更新；第三子滤波器根据第一测量数据和第二测量数据的回环检测结果进行更新，若检测到回环，则第三子滤波器进行一次更新，其更新频率完全取决于回环检测的结果。当主滤波器到达定位时刻，将接收来自各子滤波器当前的更新后状态和更新后协方差以进行融合更新，并将结果反馈给各子滤波器，子滤波器若收到反馈，则以反馈结果作为新状态继续进行预测更新，否则就以自己得到的结果继续更新。
步骤2：以所述第一测量数据和所述第二测量数据作为输入，按照所述第二测量数据的采样频率对第一子滤波器的第一状态和第一协方差矩阵进行更新，得到当前定位时刻的第一更新后状态和第一更新后协方差矩阵；所述状态包括车辆姿态、车辆位置和车辆速度。
本实施例将惯性测量单元作为公共参考系统，全局坐标系下车辆的初始方向为惯性测量单元提供的初始航向角，利用惯性测量单元采集的加速度和角速度进行车辆的状态估计，该状态估计过程将分别用于不同子滤波器。
状态估计过程中所用的车辆状态模型可以表示为：
Figure 179621DEST_PATH_IMAGE006
；
其中，x为车辆状态；
Figure DEST_PATH_IMAGE007
为根据上一惯导采样时刻的车辆姿态和IMU测量值估计得到的当前惯导采样时刻的车辆姿态；
Figure 327574DEST_PATH_IMAGE008
为根据上一惯导采样时刻的车辆位置和IMU测量值估计得到的当前惯导采样时刻的车辆位置；
Figure DEST_PATH_IMAGE009
为根据上一惯导采样时刻的车辆速度和IMU测量值估计得到的当前惯导采样时刻的车辆速度；b w ，b a 为惯性测量单元的误差；
Figure 398561DEST_PATH_IMAGE010
为重力加速度。车辆状态模型中，IMU测量值是指惯性测量单元采集的车辆加速度和车辆角速度。
本实施例以惯性测量单元的采样周期（也即相邻两个惯导采样时刻之间的时间差）构建离散的运动学模型，利用该离散的运动学模型实现用于不同子滤波器的车辆状态的预测过程，离散的运动学模型为：
Figure DEST_PATH_IMAGE011
；
不同子滤波器对应的观测值具有不同的采样周期，即不同子滤波器相邻两次采样观测值之间的时间差不同，第一子滤波器的采样周期为相邻两次雷达采样时刻的时间差，第二子滤波器的采样周期为相邻两次卫星采样时刻的时间差，第三子滤波器的采样周期不固定，为相邻两次检测到回环的时间差。其中，x i+1为子滤波器一个采样周期内的第i+1个惯导采样时刻的车辆状态；x i 为子滤波器一个采样周期内的第i个惯导采样时刻的车辆状态；
Figure 142526DEST_PATH_IMAGE012
为惯性测量单元的采样周期；f为车辆状态、IMU测量值和IMU噪声之间的函数关系，u i 为第i个惯导采样时刻的IMU测量值，w i 为第i个惯导采样时刻的IMU噪声。
忽略IMU噪声，上述离散的运动学模型可转化为如下的矩阵形式：
Figure DEST_PATH_IMAGE013
；
其中，F i+1为状态转移矩阵，B i+1为控制矩阵，u i+1为第i+1个惯导采样时刻的IMU测量值。
按照上述矩阵形式，以初始车辆状态为输入，依次递推，即可得到该子滤波器对应观测值两次采样之间最后一个惯导采样时刻的车辆状态，其即为当前状态预测值，本实施例即通过运动学模型的前向传播过程来获得当前状态预测值。每次接收到观测值将会进行滤波优化，得到子滤波器的最优状态估计，此时车辆状态将随着子滤波器的最优状态估计进行更新，作为下一定位时刻的初始车辆状态。
第一子滤波器可选用扩展卡尔曼滤波，此时以第一测量数据和第二测量数据作为输入，按照第二测量数据的采样频率对第一子滤波器的第一状态和第一协方差矩阵进行更新，得到当前定位时刻的第一更新后状态和第一更新后协方差矩阵可以包括：
（1）利用第一测量数据对第一子滤波器的第一状态和第一协方差矩阵进行预测，得到第一预测状态和第一预测协方差矩阵。
本实施例忽略噪声w，前向推断第一预测状态和第一预测协方差矩阵：
Figure 910630DEST_PATH_IMAGE014
；
其中，
Figure DEST_PATH_IMAGE015
为第一预测状态；
Figure 140404DEST_PATH_IMAGE016
为状态转移矩阵；
Figure DEST_PATH_IMAGE017
为第一状态；
Figure 462801DEST_PATH_IMAGE018
为控制矩阵；
Figure DEST_PATH_IMAGE019
为第i+1个惯导采样时刻的IMU测量值。
Figure 761190DEST_PATH_IMAGE020
；
其中，
Figure DEST_PATH_IMAGE021
为第一预测协方差矩阵；
Figure 852642DEST_PATH_IMAGE022
为第一协方差矩阵；
Figure DEST_PATH_IMAGE023
为状态的不确定性因素，即高斯噪声。
（2）根据第二测量数据计算第一子滤波器的第一观测值；
第一子滤波器利用第二测量数据计算第一观测值
Figure 296393DEST_PATH_IMAGE024
，具体采用正太分布变换（NDT）算法匹配当前雷达采样时刻的环境点云和上一雷达采样时刻的环境点云，得到两帧环境点云之间的位姿变换矩阵，该位姿变换矩阵即为第一子滤波器对应的第一观测值
Figure 778190DEST_PATH_IMAGE024
。
（3）根据第一观测值对第一预测状态和第一预测协方差矩阵进行更新，得到第一更新后状态和第一更新后协方差矩阵。
计算卡尔曼增益：
Figure DEST_PATH_IMAGE025
；
其中，
Figure 18285DEST_PATH_IMAGE026
为卡尔曼增益；
Figure DEST_PATH_IMAGE027
为测量矩阵，其根据状态量和观测值之间的非线性函数关系，利用泰勒公式构建得到；
Figure 292141DEST_PATH_IMAGE028
为测量噪声矩阵。
根据第一观测值
Figure 765847DEST_PATH_IMAGE024
更新状态：
Figure DEST_PATH_IMAGE029
；
其中，
Figure 564301DEST_PATH_IMAGE030
为第一更新后状态。
更新协方差矩阵：
Figure DEST_PATH_IMAGE031
；
其中，
Figure 657022DEST_PATH_IMAGE032
为第一更新后协方差矩阵；I为行列数与测量矩阵相同的单位矩阵。
（4）判断是否达到当前定位时刻；若是，则以当前的第一更新后状态和第一更新后协方差矩阵作为当前定位时刻的第一更新后状态和第一更新后协方差矩阵。
本实施例按照上述过程，每采样得到一第二测量数据，则对第一子滤波器的状态和协方差矩阵进行一次更新，直至达到当前定位时刻，则将当前的第一更新后状态和第一更新后协方差矩阵作为定位时刻的第一更新后状态和第一更新后协方差矩阵。
步骤3：以所述第一测量数据和所述第三测量数据作为输入，按照所述第三测量数据的采样频率对第二子滤波器的第二状态和第二协方差矩阵进行更新，得到当前定位时刻的第二更新后状态和第二更新后协方差矩阵。
具体的，步骤3可以包括：
（1）利用第一测量数据对第二子滤波器的误差状态和第二协方差矩阵进行预测，得到第二预测状态和第二预测协方差矩阵；
第二子滤波器选用误差状态卡尔曼滤波。第二子滤波器对应的误差状态可设为
Figure DEST_PATH_IMAGE033
，
Figure 113280DEST_PATH_IMAGE034
为当前惯导采样时刻的车辆姿态误差；
Figure DEST_PATH_IMAGE035
为当前惯导采样时刻的车辆位置误差；
Figure 898834DEST_PATH_IMAGE036
为当前惯导采样时刻的车辆速度误差；
Figure DEST_PATH_IMAGE037
为惯性测量单元的误差；
Figure 199232DEST_PATH_IMAGE038
为重力加速度，则此第二子滤波器的最优状态估计为
Figure DEST_PATH_IMAGE039
，误差状态的运动方程为
Figure 610491DEST_PATH_IMAGE040
，其中f 2为误差状态和IMU测量值之间的函数关系，w 2为噪声。
忽略噪声w 2，前向推断第二预测状态和第二预测协方差矩阵：
Figure DEST_PATH_IMAGE041
；
其中，
Figure 344092DEST_PATH_IMAGE042
为第二预测状态；
Figure DEST_PATH_IMAGE043
为状态转移矩阵；
Figure 113596DEST_PATH_IMAGE044
为误差状态。
Figure DEST_PATH_IMAGE045
；
其中，
Figure 198226DEST_PATH_IMAGE046
为第二预测协方差矩阵；
Figure DEST_PATH_IMAGE047
为第二协方差矩阵；
Figure 85280DEST_PATH_IMAGE048
为状态的不确定性因素，即高斯噪声。
（2）根据第三测量数据计算第二子滤波器的第二观测值；
第二子滤波器利用GNSS得到的经纬度及海拔数据获取第二观测值
Figure DEST_PATH_IMAGE049
，具体将GNSS获得的第三测量数据从WGS84坐标系转到ENU坐标系下，得到当前卫星采样时刻的位置坐标，该位置坐标即为第二观测值
Figure 686769DEST_PATH_IMAGE049
。
（3）根据第二子滤波器的第二状态和第二观测值对第二预测状态和第二预测协方差矩阵进行更新，得到第二更新后状态和第二更新后协方差矩阵。
计算卡尔曼增益：
Figure 407600DEST_PATH_IMAGE050
；
其中，
Figure DEST_PATH_IMAGE051
为卡尔曼增益；
Figure 166478DEST_PATH_IMAGE052
为测量矩阵，其为根据状态量和观测值之间的非线性函数关系，利用泰勒公式计算得到的雅可比矩阵；
Figure DEST_PATH_IMAGE053
为测量噪声矩阵。
根据第二观测值
Figure 404692DEST_PATH_IMAGE054
更新误差状态：
Figure DEST_PATH_IMAGE055
；
其中，
Figure 191514DEST_PATH_IMAGE056
为更新后误差状态；
Figure DEST_PATH_IMAGE057
为第二状态。
更新协方差矩阵：
Figure 224192DEST_PATH_IMAGE058
；
其中，
Figure DEST_PATH_IMAGE059
为第二更新后协方差矩阵；I为行列数与测量矩阵相同的单位矩阵。
根据误差状态更新最优状态：
Figure 798261DEST_PATH_IMAGE060
；
其中，
Figure DEST_PATH_IMAGE061
为第二更新后状态。
（4）判断是否达到当前定位时刻；若是，则以当前的第二更新后状态和第二更新后协方差矩阵作为当前定位时刻的第二更新后状态和第二更新后协方差矩阵。
本实施例按照上述过程，每采样得到一第三测量数据，则对第二子滤波器的状态和协方差矩阵进行一次更新，直至达到当前定位时刻，则将当前的第二更新后状态和第二更新后协方差矩阵作为定位时刻的第二更新后状态和第二更新后协方差矩阵。
步骤4：根据所述第一测量数据和所述第二测量数据进行回环检测，以对第三子滤波器的第三状态和第三协方差矩阵进行更新，得到当前定位时刻的第三更新后状态和第三更新后协方差矩阵。
具体的，步骤4可以包括：
（1）利用第一测量数据对第三子滤波器的第三状态和第三协方差矩阵进行预测，得到第三预测状态和第三预测协方差矩阵；
第三子滤波器选用扩展卡尔曼滤波。
忽略噪声w，前向推断第三预测状态和第三预测协方差矩阵，直到下一次主滤波器进行时间更新（即直至达到当前定位时刻）：
Figure 531512DEST_PATH_IMAGE062
；
其中，
Figure DEST_PATH_IMAGE063
为第三预测状态；
Figure 94212DEST_PATH_IMAGE064
为状态转移矩阵；
Figure DEST_PATH_IMAGE065
为第三状态；
Figure 609375DEST_PATH_IMAGE066
为控制矩阵；
Figure DEST_PATH_IMAGE067
为第i+1个惯导采样时刻的IMU测量值。
Figure 578731DEST_PATH_IMAGE068
；
其中，
Figure DEST_PATH_IMAGE069
为第三预测协方差矩阵；
Figure 221065DEST_PATH_IMAGE070
为第三协方差矩阵；
Figure DEST_PATH_IMAGE071
为状态的不确定性因素，即高斯噪声。
（2）根据第二测量数据进行回环检测，若构成回环，则得到第三子滤波器的第三观测值；
具体的，（2）可以包括：判断是否存在潜在回环，并确定潜在回环位姿点。具体的，在距离阈值和时间阈值的约束下，利用k-d tree在由历史位姿点组成的点云数据库中搜索，得到潜在回环位姿点，即采用k-d tree搜索点云数据库，确定满足距离阈值和时间阈值的潜在回环位姿点。若存在潜在回环，则第三子滤波器利用回环检测及回环矫正计算第三观测值
Figure 418697DEST_PATH_IMAGE072
。在得到潜在回环位姿点的条件下，利用最近点迭代算法（ICP算法）对当前环境点云和潜在回环位姿点对应的历史环境点云进行匹配，得到匹配分数。若匹配分数小于分数阈值，则认为构成回环，然后利用ICP算法计算当前环境点云和潜在回环位姿点对应的历史环境点云的位姿变换矩阵，该位姿变换矩阵即为第三子滤波器的第三观测值。若匹配分数大于分数阈值，则认为未构成回环，不再计算位姿变换矩阵，继续寻找潜在回环。
（3）根据第三观测值对第三预测状态和第三预测协方差矩阵进行更新，得到第三更新后状态和第三更新后协方差矩阵。
计算卡尔曼增益：
Figure DEST_PATH_IMAGE073
；
其中，
Figure 58756DEST_PATH_IMAGE074
为卡尔曼增益；
Figure DEST_PATH_IMAGE075
为测量矩阵，其根据状态量和观测值之间的非线性函数关系，利用泰勒公式构建得到；
Figure 512478DEST_PATH_IMAGE076
为测量噪声矩阵。
根据第三观测值
Figure 489661DEST_PATH_IMAGE072
更新状态：
Figure DEST_PATH_IMAGE077
；
其中，
Figure 213904DEST_PATH_IMAGE078
为第三更新后状态。
更新协方差矩阵：
Figure DEST_PATH_IMAGE079
；
其中，
Figure 775597DEST_PATH_IMAGE080
为第三更新后协方差矩阵；I为单位矩阵。
（4）判断是否达到当前定位时刻；若是，则以当前的第三更新后状态和第三更新后协方差矩阵作为当前定位时刻的第三更新后状态和第三更新后协方差矩阵。
本实施例按照上述过程，每构成一次回环，则对第三子滤波器的状态和协方差矩阵进行一次更新，直至达到当前定位时刻，则将当前的第三更新后状态和第三更新后协方差矩阵作为定位时刻的第三更新后状态和第三更新后协方差矩阵。若在达到当前定位时刻之前，始终没有构成回环，则在上一定位时刻和当前定位时刻之间不会对第三状态和第三协方差矩阵进行更新，此时则以当前的第三状态和第三协方差矩阵作为当前定位时刻的第三更新后状态和第三更新后协方差矩阵。
步骤5：根据所述第二测量数据和所述第三测量数据计算当前定位时刻的所述第一子滤波器的第一前向分配系数、所述第二子滤波器的第二前向分配系数和所述第三子滤波器的第三前向分配系数。
本实施例根据环境信息丰富度、GNSS数据质量以及发生回环后ICP算法的匹配分数，确定联邦滤波器的三个子滤波器
Figure DEST_PATH_IMAGE081
各自的前向分配系数
Figure 93446DEST_PATH_IMAGE082
。根据子滤波器的估计结果结合前向分配系数，可得到最终的融合估计结果。
需要说明的是，每得到一环境点云，该环境点云在进入第一子滤波器的同时，也将用于进行环境信息丰富度系数的计算，作为第一子滤波器在该环境点云的前向分配系数。每得到一第三测量数据，该第三测量数据在进入第二子滤波器的同时，也将用于进行GNSS数据质量系数的计算，作为第二子滤波器在该第三测量数据的GNSS数据质量系数。每成功进行一次回环检测，将用于进行回环矫正质量系数的计算，作为第三子滤波器在该回环检测的回环矫正质量系数。持续进行计算，直至达到当前定位时刻，则依据当前的环境信息丰富度系数、GNSS数据质量系数和回环矫正质量系数计算前向分配系数。
具体的，步骤5可以包括：
（1）计算第二测量数据的环境点云中除前N个点和后N个点之外的每一个雷达点的曲率，根据曲率判断雷达点为平面点或角点；根据所有雷达点中平面点的数量和角点的数量计算环境信息丰富度系数；
本实施例利用激光雷达获取的环境点云计算环境信息丰富度系数。N可为5，即本实施例将环境点云中除前5个点和后5个点之外的全部雷达点作为待检测雷达点。前后按照雷达点的采样时刻进行定义。
对于每一雷达点，计算雷达点的曲率的方法为：利用当前雷达点的前5个点和后5个点来计算当前雷达点的曲率。首先根据当前雷达点的坐标值计算其深度，
Figure DEST_PATH_IMAGE083
，range i 为当前雷达点i的深度，x i 、y i 和z i 为当前雷达点i的三维坐标。然后计算当前雷达点i的插值，
Figure 530113DEST_PATH_IMAGE084
，diffrange i 为当前雷达点i的插值，当k=i-5，...，i时，range k 为当前雷达点i的前5个点中的第k个点的深度，当k=i，...，i+5时，range k 为当前雷达点i的后5个点中的第k个点的深度。最后将当前雷达点的插值的平方作为当前雷达点的曲率，
Figure DEST_PATH_IMAGE085
，cloudCurvature i 为当前雷达点i的曲率。
判断雷达点为平面点或角点的方式为：设置曲率阈值为
Figure 187490DEST_PATH_IMAGE086
，若雷达点的曲率小于曲率阈值，即
Figure DEST_PATH_IMAGE087
，则认为该雷达点为平面点，否则该雷达点为角点。
环境信息丰富度系数的计算公式为：
Figure 848979DEST_PATH_IMAGE088
；
其中，
Figure DEST_PATH_IMAGE089
为环境信息丰富度系数；
Figure 841075DEST_PATH_IMAGE090
为所有雷达点中角点的数量；
Figure DEST_PATH_IMAGE091
为所有雷达点中平面点的数量。
环境信息丰富度系数越接近1，代表环境信息越丰富。
（2）将第三测量数据的车辆经度、车辆纬度和车辆海拔各自对应的均方根值分别与预设阈值进行比较，确定GNSS数据是否满足质量要求；若满足质量要求，则根据车辆经度、车辆纬度和车辆海拔各自对应的均方根值计算GNSS数据质量系数；
利用经度的均方根RMS值
Figure 628902DEST_PATH_IMAGE092
、纬度的均方根RMS值
Figure DEST_PATH_IMAGE093
和海拔的均方根RMS值
Figure 891519DEST_PATH_IMAGE094
，可判断GNSS数据质量。具体的，设置预设阈值
Figure DEST_PATH_IMAGE095
，若
Figure 231233DEST_PATH_IMAGE096
<
Figure 54833DEST_PATH_IMAGE095
，
Figure DEST_PATH_IMAGE097
<
Figure 584034DEST_PATH_IMAGE095
，且
Figure 229386DEST_PATH_IMAGE098
<
Figure 287471DEST_PATH_IMAGE095
，则认为该GNSS数据满足质量要求。
GNSS数据质量系数的计算公式为：
Figure DEST_PATH_IMAGE099
Figure 254159DEST_PATH_IMAGE100
其中，E GNSS 为GNSS数据质量系数。
GNSS数据质量系数越接近1，代表GNSS数据质量越高。若GNSS数据不满足质量要求，则断开第二子滤波器向主滤波器传递信息的开关，进行故障隔离。
（3）若回环检测成功，则根据回环检测过程所得到的匹配分数计算回环矫正质量系数；
回环矫正质量系数的计算公式为：
Figure DEST_PATH_IMAGE101
；
其中，E loop 为回环矫正质量系数；ICP score 为匹配分数；ICP threshold 为分数阈值。
回环矫正质量系数越接近1，代表回环矫正的质量越高。
（4）根据环境信息丰富度系数、GNSS数据质量系数和回环矫正质量系数计算第一子滤波器的第一前向分配系数、第二子滤波器的第二前向分配系数和第三子滤波器的第三前向分配系数。
本实施例先对环境信息丰富度系数，GNSS数据质量系数以及回环矫正质量系数进行归一化处理，再根据不同的工况确定三个子滤波器的前向分配系数：
（1）若GNSS数据满足质量要求且发生回环（即回环检测成功），则
Figure 587052DEST_PATH_IMAGE102
；
其中，
Figure DEST_PATH_IMAGE103
为第一子滤波器的第一前向分配系数；
Figure 824260DEST_PATH_IMAGE104
为第二子滤波器的第二前向分配系数；
Figure DEST_PATH_IMAGE105
为第三子滤波器的第三前向分配系数。
（2）若GNSS数据满足质量要求且未发生回环，则
Figure 849985DEST_PATH_IMAGE106
；
（3）若GNSS数据未满足质量要求且发生回环，则
Figure 569548DEST_PATH_IMAGE107
；
（4）若GNSS数据未满足质量要求且未发生回环，则
Figure DEST_PATH_IMAGE108
；
步骤6：当达到当前定位时刻时，基于所述第一前向分配系数、所述第二前向分配系数和所述第三前向分配系数对所述第一更新后状态、所述第二更新后状态和所述第三更新后状态进行融合，得到融合后状态；基于所述第一前向分配系数、所述第二前向分配系数和所述第三前向分配系数对所述第一更新后协方差矩阵、所述第二更新后协方差矩阵和所述第三更新后协方差矩阵进行融合，得到融合后协方差矩阵；所述融合后状态即为当前定位时刻车辆的导航定位信息。
主滤波器接收来自各子滤波器传递的状态和协方差矩阵，第一子滤波器F 1传递至主滤波器的状态为
Figure 237290DEST_PATH_IMAGE109
，协方差矩阵为
Figure DEST_PATH_IMAGE110
，第二子滤波器F 2传递至主滤波器的状态为
Figure 738459DEST_PATH_IMAGE111
，协方差矩阵为
Figure DEST_PATH_IMAGE112
，第三子滤波器F 3传递至主滤波器的状态为
Figure 918774DEST_PATH_IMAGE113
，协方差矩阵为
Figure DEST_PATH_IMAGE114
，主滤波器对其进行最优融合，并确定信息分配系数，将融合后结果根据信息分配系数反馈至各子滤波器，最终的融合后状态将作为车辆的导航与定位信息。
融合后协方差矩阵所用的计算公式为：
Figure 407524DEST_PATH_IMAGE115
；
其中，
Figure DEST_PATH_IMAGE116
为融合后协方差矩阵；
Figure 567372DEST_PATH_IMAGE117
为第k前向分配系数；k=1，2，3；
Figure DEST_PATH_IMAGE118
为第k更新后协方差矩阵。
融合后状态所用的计算公式为：
Figure 949812DEST_PATH_IMAGE119
；
其中，
Figure DEST_PATH_IMAGE120
为融合后状态，包含了车辆的导航定位信息；
Figure 317339DEST_PATH_IMAGE116
为融合后协方差矩阵；
Figure 558965DEST_PATH_IMAGE117
为第k前向分配系数；k=1，2，3；
Figure 253995DEST_PATH_IMAGE118
为第k更新后协方差矩阵；
Figure 100728DEST_PATH_IMAGE121
为第k更新后状态。
步骤7：根据所述第一更新后协方差矩阵、所述第二更新后协方差矩阵和所述第三更新后协方差矩阵计算所述第一子滤波器的第一信息分配系数、所述第二子滤波器的第二信息分配系数和所述第三子滤波器的第三信息分配系数；基于所述第一信息分配系数、所述第二信息分配系数和所述第三信息分配系数对所述融合后状态和所述融合后协方差矩阵进行分配，得到下一定位时刻第一子滤波器的第一状态和第一协方差矩阵、第二子滤波器的第二状态和第二协方差矩阵以及第三子滤波器的第三状态和第三协方差矩阵，并返回“获取从上一定位时刻到当前定位时刻惯性测量单元采集的第一测量数据、激光雷达采集的第二测量数据和全球导航卫星系统采集的第三测量数据”的步骤，进行下一定位时刻车辆的导航定位。
计算三个子滤波器的信息分配系数的步骤可以包括：计算第一更新后协方差矩阵的第一特征值，计算第二更新后协方差矩阵的第二特征值，计算第三更新后协方差矩阵的第三特征值。根据第一特征值、第二特征值和第三特征值计算第一子滤波器的第一信息分配系数、第二子滤波器的第二信息分配系数和第三子滤波器的第三信息分配系数。
第k（k=1，2，3）信息分配系数的计算公式为：
Figure DEST_PATH_IMAGE122
；
其中，
Figure 622845DEST_PATH_IMAGE123
为第k信息分配系数；
Figure DEST_PATH_IMAGE124
为第k子滤波器的更新后协方差矩阵的特征值；
信息分配系数满足信息分配原则，即
Figure 23871DEST_PATH_IMAGE125
，因此反馈到子滤波器状态F 1的状态为
Figure DEST_PATH_IMAGE126
，协方差矩阵为
Figure 587838DEST_PATH_IMAGE127
，反馈到子滤波器状态F 2的状态为
Figure 492341DEST_PATH_IMAGE126
，协方差矩阵为
Figure DEST_PATH_IMAGE128
，反馈到子滤波器状态F 3的状态为
Figure 450938DEST_PATH_IMAGE126
，协方差矩阵为
Figure 73680DEST_PATH_IMAGE129
。
本实施例提出了一种基于改进联邦滤波的结构化道路车辆导航定位方法，用于在园区、城市道路及高速公路等不同结构化道路场景实现车辆的高精度定位与导航。该系统以融合-重置式联邦滤波器为基础，将惯性测量单元作为公共参考系统，给各子滤波器作为公共状态变量。激光雷达得到的点云数据、全球导航卫星系统提供的位置信息和回环矫正后的位置信息作为各子滤波器观测输入。在系统运行过程中，实时将激光雷达点云分割为平面点和角点，并根据角点的占比确定环境信息丰富度；同时获取GNSS定位的均方根值，判断GNSS定位的精度及是否需要进行故障隔离；利用回环矫正时所用ICP算法的匹配分数判断回环矫正的质量；结合环境信息丰富度、GNSS的定位精度以及发生回环后回环矫正的质量，确定子滤波器传递给主滤波器的前向信息分配系数。最后融合各子滤波器的估计值得到全局最优估计，即为最终的车辆导航与定位信息，该最优估计与对应的主滤波器协方差矩阵以一定的信息分配原则反馈给各子滤波器，以重置各子滤波器。通过上述方式实现了可适应结构化道路不同场景下的高精度车辆导航定位。
本实施例实现了在各种场景的结构化道路车辆的高精度导航与定位，原因如下：
（1）融入了回环检测与矫正，将回环矫正结果作为其中一个子滤波器的观测值，可以修正累计误差。
（2）将激光雷达点云划分为角点和平面点，并利用其判断环境信息丰富度；利用GNSS输出结果的均方根值判断GNSS数据的质量；利用回环矫正ICP算法匹配分数判断回环矫正的质量；结合环境信息丰富度、GNSS数据质量及发生回环后回环矫正的质量确定前向信息分配系数，分配各子滤波器传递给主滤波器的状态估计值和协方差。
实施例2：
本实施例用于提供一种基于改进联邦滤波的结构化道路车辆导航定位系统，如图2所示，包括：
数据获取模块M1，用于获取从上一定位时刻到当前定位时刻惯性测量单元采集的第一测量数据、激光雷达采集的第二测量数据和全球导航卫星系统采集的第三测量数据；所述第一测量数据包括各个惯导采样时刻采集的车辆加速度、车辆角速度和车辆航向角；所述第二测量数据包括各个雷达采样时刻采集的车辆周围的环境点云；所述第三测量数据包括各个卫星采样时刻采集的车辆经度、车辆纬度、车辆海拔以及车辆经度、车辆纬度和车辆海拔各自对应的均方根值；
更新模块M2，用于以所述第一测量数据和所述第二测量数据作为输入，按照所述第二测量数据的采样频率对第一子滤波器的第一状态和第一协方差矩阵进行更新，得到当前定位时刻的第一更新后状态和第一更新后协方差矩阵；所述状态包括车辆姿态、车辆位置和车辆速度；以所述第一测量数据和所述第三测量数据作为输入，按照所述第三测量数据的采样频率对第二子滤波器的第二状态和第二协方差矩阵进行更新，得到当前定位时刻的第二更新后状态和第二更新后协方差矩阵；根据所述第一测量数据和所述第二测量数据进行回环检测，以对第三子滤波器的第三状态和第三协方差矩阵进行更新，得到当前定位时刻的第三更新后状态和第三更新后协方差矩阵；
融合模块M3，用于根据所述第二测量数据和所述第三测量数据计算当前定位时刻的所述第一子滤波器的第一前向分配系数、所述第二子滤波器的第二前向分配系数和所述第三子滤波器的第三前向分配系数；当达到当前定位时刻时，基于所述第一前向分配系数、所述第二前向分配系数和所述第三前向分配系数对所述第一更新后状态、所述第二更新后状态和所述第三更新后状态进行融合，得到融合后状态；基于所述第一前向分配系数、所述第二前向分配系数和所述第三前向分配系数对所述第一更新后协方差矩阵、所述第二更新后协方差矩阵和所述第三更新后协方差矩阵进行融合，得到融合后协方差矩阵；所述融合后状态即为当前定位时刻车辆的导航定位信息；
分配模块M4，用于根据所述第一更新后协方差矩阵、所述第二更新后协方差矩阵和所述第三更新后协方差矩阵计算所述第一子滤波器的第一信息分配系数、所述第二子滤波器的第二信息分配系数和所述第三子滤波器的第三信息分配系数；基于所述第一信息分配系数、所述第二信息分配系数和所述第三信息分配系数对所述融合后状态和所述融合后协方差矩阵进行分配，得到下一定位时刻第一子滤波器的第一状态和第一协方差矩阵、第二子滤波器的第二状态和第二协方差矩阵以及第三子滤波器的第三状态和第三协方差矩阵，并返回“获取从上一定位时刻到当前定位时刻惯性测量单元采集的第一测量数据、激光雷达采集的第二测量数据和全球导航卫星系统采集的第三测量数据”的步骤，进行下一定位时刻车辆的导航定位。
本说明书中每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。
本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。

Claims (10)

1.一种基于改进联邦滤波的结构化道路车辆导航定位方法，其特征在于，包括：
获取从上一定位时刻到当前定位时刻惯性测量单元采集的第一测量数据、激光雷达采集的第二测量数据和全球导航卫星系统采集的第三测量数据；所述第一测量数据包括各个惯导采样时刻采集的车辆加速度、车辆角速度和车辆航向角；所述第二测量数据包括各个雷达采样时刻采集的车辆周围的环境点云；所述第三测量数据包括各个卫星采样时刻采集的车辆经度、车辆纬度、车辆海拔以及车辆经度、车辆纬度和车辆海拔各自对应的均方根值；
以所述第一测量数据和所述第二测量数据作为输入，按照所述第二测量数据的采样频率对第一子滤波器的第一状态和第一协方差矩阵进行更新，得到当前定位时刻的第一更新后状态和第一更新后协方差矩阵；所述状态包括车辆姿态、车辆位置和车辆速度；
以所述第一测量数据和所述第三测量数据作为输入，按照所述第三测量数据的采样频率对第二子滤波器的第二状态和第二协方差矩阵进行更新，得到当前定位时刻的第二更新后状态和第二更新后协方差矩阵；
根据所述第一测量数据和所述第二测量数据进行回环检测，以对第三子滤波器的第三状态和第三协方差矩阵进行更新，得到当前定位时刻的第三更新后状态和第三更新后协方差矩阵；
根据所述第二测量数据和所述第三测量数据计算当前定位时刻的所述第一子滤波器的第一前向分配系数、所述第二子滤波器的第二前向分配系数和所述第三子滤波器的第三前向分配系数；
当达到当前定位时刻时，基于所述第一前向分配系数、所述第二前向分配系数和所述第三前向分配系数对所述第一更新后状态、所述第二更新后状态和所述第三更新后状态进行融合，得到融合后状态；基于所述第一前向分配系数、所述第二前向分配系数和所述第三前向分配系数对所述第一更新后协方差矩阵、所述第二更新后协方差矩阵和所述第三更新后协方差矩阵进行融合，得到融合后协方差矩阵；所述融合后状态即为当前定位时刻车辆的导航定位信息；
根据所述第一更新后协方差矩阵、所述第二更新后协方差矩阵和所述第三更新后协方差矩阵计算所述第一子滤波器的第一信息分配系数、所述第二子滤波器的第二信息分配系数和所述第三子滤波器的第三信息分配系数；基于所述第一信息分配系数、所述第二信息分配系数和所述第三信息分配系数对所述融合后状态和所述融合后协方差矩阵进行分配，得到下一定位时刻第一子滤波器的第一状态和第一协方差矩阵、第二子滤波器的第二状态和第二协方差矩阵以及第三子滤波器的第三状态和第三协方差矩阵，并返回“获取从上一定位时刻到当前定位时刻惯性测量单元采集的第一测量数据、激光雷达采集的第二测量数据和全球导航卫星系统采集的第三测量数据”的步骤，进行下一定位时刻车辆的导航定位。
2.根据权利要求1所述的结构化道路车辆导航定位方法，其特征在于，所述以所述第一测量数据和所述第二测量数据作为输入，按照所述第二测量数据的采样频率对第一子滤波器的第一状态和第一协方差矩阵进行更新，得到当前定位时刻的第一更新后状态和第一更新后协方差矩阵具体包括：
利用所述第一测量数据对第一子滤波器的第一状态和第一协方差矩阵进行预测，得到第一预测状态和第一预测协方差矩阵；
根据所述第二测量数据计算所述第一子滤波器的第一观测值；
根据所述第一观测值对所述第一预测状态和所述第一预测协方差矩阵进行更新，得到第一更新后状态和第一更新后协方差矩阵；
判断是否达到当前定位时刻；若是，则以当前的第一更新后状态和第一更新后协方差矩阵作为当前定位时刻的第一更新后状态和第一更新后协方差矩阵。
3.根据权利要求1所述的结构化道路车辆导航定位方法，其特征在于，所述以所述第一测量数据和所述第三测量数据作为输入，按照所述第三测量数据的采样频率对第二子滤波器的第二状态和第二协方差矩阵进行更新，得到当前定位时刻的第二更新后状态和第二更新后协方差矩阵具体包括：
利用所述第一测量数据对第二子滤波器的误差状态和第二协方差矩阵进行预测，得到第二预测状态和第二预测协方差矩阵；
根据所述第三测量数据计算所述第二子滤波器的第二观测值；
根据所述第二子滤波器的第二状态和所述第二观测值对所述第二预测状态和所述第二预测协方差矩阵进行更新，得到第二更新后状态和第二更新后协方差矩阵；
判断是否达到当前定位时刻；若是，则以当前的第二更新后状态和第二更新后协方差矩阵作为当前定位时刻的第二更新后状态和第二更新后协方差矩阵。
4.根据权利要求1所述的结构化道路车辆导航定位方法，其特征在于，所述根据所述第一测量数据和所述第二测量数据进行回环检测，以对第三子滤波器的第三状态和第三协方差矩阵进行更新，得到当前定位时刻的第三更新后状态和第三更新后协方差矩阵具体包括：
利用所述第一测量数据对第三子滤波器的第三状态和第三协方差矩阵进行预测，得到第三预测状态和第三预测协方差矩阵；
根据所述第二测量数据进行回环检测，若构成回环，则得到所述第三子滤波器的第三观测值；
根据所述第三观测值对所述第三预测状态和所述第三预测协方差矩阵进行更新，得到第三更新后状态和第三更新后协方差矩阵；
判断是否达到当前定位时刻；若是，则以当前的第三更新后状态和第三更新后协方差矩阵作为当前定位时刻的第三更新后状态和第三更新后协方差矩阵。
5.根据权利要求4所述的结构化道路车辆导航定位方法，其特征在于，所述根据所述第二测量数据进行回环检测，若构成回环，则得到所述第三子滤波器的第三观测值具体包括：
在距离阈值和时间阈值的约束下，利用k-d tree在由历史位姿点组成的点云数据库中搜索，得到潜在回环位姿点；
利用ICP算法对当前环境点云和所述潜在回环位姿点对应的历史环境点云进行匹配，得到匹配分数；若所述匹配分数小于分数阈值，则构成回环，利用所述ICP算法计算所述当前环境点云和所述历史环境点云的位姿变换矩阵，得到所述第三子滤波器的第三观测值。
6.根据权利要求1所述的结构化道路车辆导航定位方法，其特征在于，所述根据所述第二测量数据和所述第三测量数据计算当前定位时刻的所述第一子滤波器的第一前向分配系数、所述第二子滤波器的第二前向分配系数和所述第三子滤波器的第三前向分配系数具体包括：
计算所述第二测量数据的环境点云中除前N个点和后N个点之外的每一个雷达点的曲率，根据所述曲率判断所述雷达点为平面点或角点；根据所有所述雷达点中所述平面点的数量和所述角点的数量计算环境信息丰富度系数；
将所述第三测量数据的车辆经度、车辆纬度和车辆海拔各自对应的均方根值分别与预设阈值进行比较，确定GNSS数据是否满足质量要求；若满足质量要求，则根据所述车辆经度、车辆纬度和车辆海拔各自对应的均方根值计算GNSS数据质量系数；
若回环检测成功，则根据回环检测过程所得到的匹配分数计算回环矫正质量系数；
根据所述环境信息丰富度系数、所述GNSS数据质量系数和所述回环矫正质量系数计算所述第一子滤波器的第一前向分配系数、所述第二子滤波器的第二前向分配系数和所述第三子滤波器的第三前向分配系数。
7.根据权利要求1所述的结构化道路车辆导航定位方法，其特征在于，所述融合后协方差矩阵所用的计算公式为：
Figure 107418DEST_PATH_IMAGE001
；
其中，
Figure 555717DEST_PATH_IMAGE002
为融合后协方差矩阵；
Figure 741979DEST_PATH_IMAGE003
为第k前向分配系数；k=1，2，3；
Figure 215817DEST_PATH_IMAGE004
为第k更新后协方差矩阵。
8.根据权利要求1所述的结构化道路车辆导航定位方法，其特征在于，所述融合后状态所用的计算公式为：
Figure 702293DEST_PATH_IMAGE005
；
其中，
Figure 55914DEST_PATH_IMAGE006
为融合后状态；
Figure 978739DEST_PATH_IMAGE002
为融合后协方差矩阵；
Figure 708798DEST_PATH_IMAGE003
为第k前向分配系数；k=1，2，3；
Figure 112097DEST_PATH_IMAGE004
为第k更新后协方差矩阵；
Figure 511986DEST_PATH_IMAGE007
为第k更新后状态。
9.根据权利要求1所述的结构化道路车辆导航定位方法，其特征在于，所述根据所述第一更新后协方差矩阵、所述第二更新后协方差矩阵和所述第三更新后协方差矩阵计算所述第一子滤波器的第一信息分配系数、所述第二子滤波器的第二信息分配系数和所述第三子滤波器的第三信息分配系数具体包括：
计算所述第一更新后协方差矩阵的第一特征值；计算所述第二更新后协方差矩阵的第二特征值；计算所述第三更新后协方差矩阵的第三特征值；
根据所述第一特征值、所述第二特征值和所述第三特征值计算所述第一子滤波器的第一信息分配系数、所述第二子滤波器的第二信息分配系数和所述第三子滤波器的第三信息分配系数。
10.一种基于改进联邦滤波的结构化道路车辆导航定位系统，其特征在于，包括：
数据获取模块，用于获取从上一定位时刻到当前定位时刻惯性测量单元采集的第一测量数据、激光雷达采集的第二测量数据和全球导航卫星系统采集的第三测量数据；所述第一测量数据包括各个惯导采样时刻采集的车辆加速度、车辆角速度和车辆航向角；所述第二测量数据包括各个雷达采样时刻采集的车辆周围的环境点云；所述第三测量数据包括各个卫星采样时刻采集的车辆经度、车辆纬度、车辆海拔以及车辆经度、车辆纬度和车辆海拔各自对应的均方根值；
更新模块，用于以所述第一测量数据和所述第二测量数据作为输入，按照所述第二测量数据的采样频率对第一子滤波器的第一状态和第一协方差矩阵进行更新，得到当前定位时刻的第一更新后状态和第一更新后协方差矩阵；所述状态包括车辆姿态、车辆位置和车辆速度；以所述第一测量数据和所述第三测量数据作为输入，按照所述第三测量数据的采样频率对第二子滤波器的第二状态和第二协方差矩阵进行更新，得到当前定位时刻的第二更新后状态和第二更新后协方差矩阵；根据所述第一测量数据和所述第二测量数据进行回环检测，以对第三子滤波器的第三状态和第三协方差矩阵进行更新，得到当前定位时刻的第三更新后状态和第三更新后协方差矩阵；
融合模块，用于根据所述第二测量数据和所述第三测量数据计算当前定位时刻的所述第一子滤波器的第一前向分配系数、所述第二子滤波器的第二前向分配系数和所述第三子滤波器的第三前向分配系数；当达到当前定位时刻时，基于所述第一前向分配系数、所述第二前向分配系数和所述第三前向分配系数对所述第一更新后状态、所述第二更新后状态和所述第三更新后状态进行融合，得到融合后状态；基于所述第一前向分配系数、所述第二前向分配系数和所述第三前向分配系数对所述第一更新后协方差矩阵、所述第二更新后协方差矩阵和所述第三更新后协方差矩阵进行融合，得到融合后协方差矩阵；所述融合后状态即为当前定位时刻车辆的导航定位信息；
分配模块，用于根据所述第一更新后协方差矩阵、所述第二更新后协方差矩阵和所述第三更新后协方差矩阵计算所述第一子滤波器的第一信息分配系数、所述第二子滤波器的第二信息分配系数和所述第三子滤波器的第三信息分配系数；基于所述第一信息分配系数、所述第二信息分配系数和所述第三信息分配系数对所述融合后状态和所述融合后协方差矩阵进行分配，得到下一定位时刻第一子滤波器的第一状态和第一协方差矩阵、第二子滤波器的第二状态和第二协方差矩阵以及第三子滤波器的第三状态和第三协方差矩阵，并返回“获取从上一定位时刻到当前定位时刻惯性测量单元采集的第一测量数据、激光雷达采集的第二测量数据和全球导航卫星系统采集的第三测量数据”的步骤，进行下一定位时刻车辆的导航定位。
CN202211575680.0A 2022-12-09 2022-12-09 基于改进联邦滤波的结构化道路车辆导航定位方法及系统 Active CN115574818B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211575680.0A CN115574818B (zh) 2022-12-09 2022-12-09 基于改进联邦滤波的结构化道路车辆导航定位方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211575680.0A CN115574818B (zh) 2022-12-09 2022-12-09 基于改进联邦滤波的结构化道路车辆导航定位方法及系统

Publications (2)

Publication Number Publication Date
CN115574818A true CN115574818A (zh) 2023-01-06
CN115574818B CN115574818B (zh) 2023-02-28

Family

ID=84590279

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211575680.0A Active CN115574818B (zh) 2022-12-09 2022-12-09 基于改进联邦滤波的结构化道路车辆导航定位方法及系统

Country Status (1)

Country Link
CN (1) CN115574818B (zh)

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108802786A (zh) * 2018-07-20 2018-11-13 北斗星通（重庆）汽车电子有限公司 一种车载定位方法
KR102119254B1 (ko) * 2019-03-14 2020-06-04 국방과학연구소 연합형 필터 기반 관성항법, 위성항법 및 지형대조항법을 정보 융합한 통합 항법 설계 방법 및 이 방법을 저장한 컴퓨터 판독 가능 저장 매체
CN112180361A (zh) * 2020-09-30 2021-01-05 南京航空航天大学 一种基于动态联邦滤波的车载雷达目标跟踪方法
CN112711055A (zh) * 2020-12-08 2021-04-27 重庆邮电大学 一种基于边缘计算的室内外无缝定位系统及方法
CN113029137A (zh) * 2021-04-01 2021-06-25 清华大学 一种多源信息自适应融合定位方法及系统
CN115014347A (zh) * 2022-05-16 2022-09-06 上海交通大学 一种快速可观测度分析及其指导的多传感器信息融合方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108802786A (zh) * 2018-07-20 2018-11-13 北斗星通（重庆）汽车电子有限公司 一种车载定位方法
KR102119254B1 (ko) * 2019-03-14 2020-06-04 국방과학연구소 연합형 필터 기반 관성항법, 위성항법 및 지형대조항법을 정보 융합한 통합 항법 설계 방법 및 이 방법을 저장한 컴퓨터 판독 가능 저장 매체
CN112180361A (zh) * 2020-09-30 2021-01-05 南京航空航天大学 一种基于动态联邦滤波的车载雷达目标跟踪方法
CN112711055A (zh) * 2020-12-08 2021-04-27 重庆邮电大学 一种基于边缘计算的室内外无缝定位系统及方法
CN113029137A (zh) * 2021-04-01 2021-06-25 清华大学 一种多源信息自适应融合定位方法及系统
CN115014347A (zh) * 2022-05-16 2022-09-06 上海交通大学 一种快速可观测度分析及其指导的多传感器信息融合方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
张旭东等: "一种面向未知非结构化环境的无人车辆运动规划算法", 《2021中国汽车工程学会年会论文集》 *

Also Published As

Publication number Publication date
CN115574818B (zh) 2023-02-28

Similar Documents

Publication Publication Date Title
CN108759833B (zh) 一种基于先验地图的智能车辆定位方法
CN110243358B (zh) 多源融合的无人车室内外定位方法及系统
CN108226980B (zh) 基于惯性测量单元的差分gnss与ins自适应紧耦合导航方法
CN112083726B (zh) 一种面向园区自动驾驶的双滤波器融合定位系统
CN110779518B (zh) 一种具有全局收敛性的水下航行器单信标定位方法
JP5162849B2 (ja) 不動点位置記録装置
CN110554396A (zh) 一种室内场景下激光雷达建图方法、装置、设备及介质
JP4984659B2 (ja) 自車両位置推定装置
CN110231636B (zh) Gps与bds双模卫星导航系统的自适应无迹卡尔曼滤波方法
CN112762957B (zh) 一种基于多传感器融合的环境建模及路径规划方法
CN109059911B (zh) 一种gnss、ins和气压计的数据融合方法
CN112577496B (zh) 一种基于自适应选权的多源融合定位方法
CN110572139B (zh) 用于车辆状态估计的融合滤波实现方法以及装置、存储介质、车辆
CN113740871B (zh) 一种在高动态环境下的激光slam方法、系统设备及存储介质
CN111156986B (zh) 一种基于抗差自适应ukf的光谱红移自主组合导航方法
WO2024041156A1 (zh) 车辆定位校准方法、装置、计算机设备及存储介质
CN114323033A (zh) 基于车道线和特征点的定位方法、设备及自动驾驶车辆
CN115127543A (zh) 一种激光建图优化中异常边剔除方法及系统
CN114915913A (zh) 一种基于滑窗因子图的uwb-imu组合室内定位方法
CN114440895A (zh) 基于因子图的气压辅助Wi-Fi/PDR室内定位方法
CN115574818B (zh) 基于改进联邦滤波的结构化道路车辆导航定位方法及系统
Rahman et al. Earth-centered earth-fixed (ecef) vehicle state estimation performance
CN113237482B (zh) 一种基于因子图的城市峡谷环境下车辆鲁棒定位方法
CN113911123B (zh) 一种道路模型的更新方法及装置
CN115639585A (zh) 一种gps/imu和激光雷达的多传感器融合定位方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant