一种面向遮挡测评环境的多传感高精度融合测量方法

1.本发明属于车辆导航定位领域，具体涉及一种面向遮挡测评环境的多传感高精度融合测量方法。


背景技术：

2.近年来，智能交通系统在全球范围内得到了飞速的发展，它能将新进的传感器技术、数据通讯传输技术、信息技术等各种技术综合运用于交通系统中，从而有效地实现对交通系统的综合管理，最大限度地实现人、车、路的和谐统一。而对于大部分智能交通系统而言，获取准确、可靠的位置信息是发挥其功能的前提。
3.目前，在车辆导航定位领域中应用最为广泛的是全球导航卫星系统(gnss)技术。在无遮挡的环境下，gnss能全天候地为车辆提供三维的位置、速度、姿态等信息，但是在密闭的交通环境中，由于卫星信号受到遮挡，gnss会存在长期失效的现象从而无法实现连续可靠的定位。
4.惯性测量单元(imu)是一种不依赖于外部信息也不向外发射能量的自主式导航系统，它通过测量载体的加速度和角速度信息并进行积分运算后就能得到载体的位置、速度和姿态信息。由于与gnss的互补性，通常将它们与gnss集成在一起来提高导航的精度。但由于惯性传感器的特性，imu在长期独立运行后会积累巨大的误差，这些误差可能会导致定位性能的急剧下降。
5.近些年来，同步定位和建图(slam)技术在gnss信号受遮挡的场景下取得了长足的发展。slam技术中常用的传感器之一是激光雷达(lidar)，它具有广阔的视角、低光环境下的鲁棒性以及远距离捕捉环境细节的能力。然而，激光雷达slam只估计局部帧中的位姿，没有全局帧，这意味着位姿估计是局部准确的，但随着时间的推移容易累积误差。此外，当车辆行驶至特征点较少的区域时，激光雷达slam也会由于稀疏的点云而导致定位精度的降低。


技术实现要素：

6.为弥补卫星受遮挡情况下gnss无法提供车辆可靠位置信息的问题，本发明提出了一种基于lidar/uwb/imu的车辆融合定位方法。本方法通过在激光雷达点云稀疏的路段布设固定位置的特殊形状物体，便于激光雷达检测，该物体所在位置已经事先准确测量，检测到物体的特征点后就可以知道物体的准确位置信息，并可以解算与该物体的相对位置关系，相当于主动地为激光雷达提供特征信息，因此本专利中将该特殊形状的物体称为主动特征。同时主动特征顶部搭载了uwb，可以提供编号信息以区分不同的主动特征，并且增加额外的uwb距离观测信息。为了进一步提升定位输出精度和频率，本发明还使用了低成本的imu用以提供车辆的运动信息。本发明的融合算法采用因子图算法，它能充分利用lidar、uwb和imu的历史观测信息，并能实现车辆在实际行驶中各传感器的“即插即用”，具有输出频率高、精度好的显著优点。具体步骤如下：
7.步骤1：布置路侧主动特征与uwb基站
8.本发明在激光雷达点云稀疏的区域每隔10-20m等间距的布置主动特征及uwb基站。其中主动特征为低成本的白色圆柱体泡沫，将主动特征安装在相应支架上，在每个主动特征的顶部都安装相应的uwb基站。以正东方向为ox，正北方向为oy，地球表面一个固定点为原点建立坐标系。各个主动特征及uwb基站在该坐标系下的位置可通过提前标定获取。将uwb标签和激光雷达安装在行驶车辆的顶端。
9.步骤2：激光雷达观测量识别
10.根据步骤1中的布局，在同一时刻uwb标签所能接收到的观测信息为r1，r2，...，rn，其中rn表示第n个主动特征与车辆之间的距离值；激光雷达检测到主动特征后所能解算的观测信息为d、θ，其中d为路侧某个未知的主动特征与激光雷达之间的距离值，θ为该主动特征与激光雷达之间相对于正北方向的夹角的绝对值。由于激光雷达对主动特征进行检测时无法判断该主动特征的编号，而主动特征项部的uwb恰好弥补了这一缺点。通过将激光雷达解算的距离信息d与uwb标签接收到的距离信息作对比，若|d-ri|＜ε，(i＝1，2，...，n，ε为事先设定的阈值)，则可判定当前激光雷达检测的距离d与角度θ隶属的主动特征编号i(i＝1，2，...，n)。
11.步骤3：构建多传感器组合导航的因子图模型
12.假设λ表示车辆的导航状态，包括三维位置x、三维速度v等信息。定义车辆的启动时刻为初始时刻，则初始时刻到当前时刻tk的导航状态可表示为：
[0013][0014]
相应的联合概率分布函数可表示为：
[0015]
p(λk|zk)
ꢀꢀꢀꢀ
(2)
[0016]
其中zk为初始时刻到当前时刻tk接收到的所有观测值。
[0017]
则待估计的导航状态的最大后验估计可表示为：
[0018][0019]
联合概率分布式(2)可以因式分解为一个先验信息和独立过程及测量模型：
[0020][0021]
上式中p(λ0)表示所有可用的先验信息；表示λi中与测量模型相关的变量；表示imu的测量值，包含比力和角速率；表示imu的后验概率密度；p(vi|v
i-1
)表示速度变量的后验概率密度；表示除imu外所有传感器测量值的后验概率密度的乘积。
[0022]
上述的因式分解可以表示成因子图模型，作为一种二分图模型，因子图包含了因子节点和状态节点两类。因子图中的每个因子节点都可以表示为式(4)中的一个独立项，并且每个因子fi(
·
)表示为一个应当最小化的误差函数，假设这个误差函数为则
因子fi(
·
)可定义为：
[0023][0024]
其中d(
·
)表示代价函数。假设各个传感器的误差均服从高斯分布，则因子fi(
·
)满足如下形式：
[0025][0026]
上式中，表示马氏距离，∑i为传感器的误差协方差矩阵。则传感器的测量模型因子可定义为：
[0027][0028]
式中，h(
·
)为非线性测量函数，zi为传感器实际的测量值，因此式(4)的最大后验估计问题就变成了如下的非线性最小二乘问题：
[0029][0030]
本发明构建的因子图模型中，xk和vk分别代表k时刻车辆的位置和速度。其中xk＝[xek，xnk，xuk]
t
分别代表k时刻车辆的东向位置、北向位置和天向位置；vk＝[vek，vnk，vuk]
t
分别代表k时刻车辆的东向速度、北向速度和天向速度。f
prior
代表先验因子，f
mm
代表运动模型因子，f
uwb
代表uwb因子，fv代表速度因子，f
slam
代表激光雷达slam因子，f
af
代表激光雷达检测到的主动特征因子。通过建立各因子的误差函数，就能求得各个时刻车辆位置的最大后验估计：
[0031]
子步骤1：imu等效因子建立
[0032]
本发明中的imu因子包含了运动模型因子f
mm
和速度因子fv，通过接收来自imu的测量值来定义这两种因子。其中fb与ωb分别代表机体坐标系下载体的比力和角速度。因此，车辆的运动模型可表示为：
[0033][0034]
车辆的速度模型可表示为：
[0035][0036]
其中，ae、an、au分别为捷联惯导算法转换的载体在东北天坐标系下的东向、北向和天向加速度；和分别为车辆运动模型和速度模型的误差协方差矩阵。则imu等效因子的误差函数可表示为：
[0037]
[0038]
子步骤2：uwb因子的建立
[0039]
在每个采样时间k，车载uwb标签接收到的距离测量值可表示为rk＝{r
k，1
，r
k，2
，...，r
k，j
}，其中r
k，j
表示k时刻标签接收到的路侧第j个基站的距离值。因此，第k个基站的观测方程可表示为：
[0040][0041]
其中，xk＝[xek，xnk，xuk]
t
表示k时刻车辆的三维位置；表示事先标定好的路侧第j个基站的三维坐标；||
·
||表示车辆位置与基站位置之间的欧氏距离。则uwb距离的观测方程可表示为：
[0042][0043]
其中为uwb因子的误差协方差矩阵。则uwb因子的误差函数为：
[0044][0045]
子步骤3：主动特征因子的建立
[0046]
当激光雷达扫描到路侧主动特征后，将产生d和θ两个观测量，其中d和θ分别为路侧某个未知的主动特征与激光雷达之间的距离值以及该主动特征与激光雷达之间相对于正北方向的夹角的绝对值。通过步骤2可确定该主动特征的编号。因此，主动特征观测方程可表示为：
[0047][0048]
由于uwb基站安装在主动特征顶部，因此上式中的也代表路侧第j个主动特征的三维坐标。则主动特征的测量模型可表示为：
[0049][0050]
其中a
k，j
＝[d
k，j
，θ
k，j
]
t
，为主动特征因子的误差协方差矩阵。则主动特征因子的误差函数为：
[0051][0052]
子步骤4：激光雷达slam因子的建立
[0053]
激光雷达slam技术通过点云匹配算法在提前构建好的地图中进行车辆定位，它能实时地输出车辆的位置信息。在k时刻激光雷达slam输出的观测量可表示为其中分别代表slam输出的车辆的东向位置、北向位置和天向位置信息。则slam的测量模型可表示为：
[0054][0055]
则slam因子的误差函数为：
[0056][0057]
通过计算imu等效因子、uwb因子、主动特征因子和激光雷达slam因子的误差函数，就能得到k时刻系统状态的总体误差函数
[0058][0059]
通过最小化各等效因子的误差函数，即可求得每一时刻车辆的最优位置估计，实现gnss失效情况下车辆的实时定位。
[0060]
本发明的显著效果是：
[0061]
(1)本发明提出了一种gnss失效场景下的车辆可靠定位方法，针对单一激光slam算法输出频率较低且在点云稀疏场景下定位精度下降的情况，提出了一种基于lidar/uwb/imu的车辆融合定位方法，通过在路侧布设主动特征和uwb基站来增加激光雷达的观测量从而显著提升车辆定位精度；
[0062]
(2)本发明的融合方法采用因子图算法，相对于传统的滤波算法，该算法能够充分利用历史时刻的观测信息，提升定位精度，并实现主动特征与uwb等多传感信息的“即插即用”。
附图说明：
[0063]
图1是本发明的技术路线图；
[0064]
图2是主动特征与uwb的布局；
[0065]
图3是lidar/uwb/imu融合定位的因子图模型；
[0066]
图4是引入主动特征和uwb基站前、后的车辆定位轨迹对比图；
[0067]
图5是引入主动特征和uwb基站前、后融合定位误差图。
具体实施方式
[0068]
随着经济社会的发展与进步，我国的机动车保有量快速增加，道路交通面临着巨大的挑战，为了解决日益严峻的城市交通问题，智能交通系统应运而生。它能将新进的传感器技术、数据通讯传输技术、信息技术等各种技术综合运用于交通系统中，从而有效地实现对交通系统的综合管理，最大限度地实现人、车、路的和谐统一。而对于大部分智能交通系统而言，获取准确、可靠的位置信息是发挥其功能的前提。
[0069]
目前，在车辆导航定位领域中应用最为广泛的是gnss技术。在无遮挡的环境下，gnss能全天候地为车辆提供三维的位置、速度、姿态等信息，但是在密闭的交通环境中，由于卫星信号受到遮挡，gnss会存在长期失效的现象从而无法实现连续可靠的定位。
[0070]
惯性测量单元(imu)是一种不依赖于外部信息也不向外发射能量的自主式导航系统，它通过测量载体的加速度和角速度信息并进行积分运算后就能得到载体的位置、速度和姿态信息。由于与gnss的互补性，通常将它们与gnss集成在一起来提高导航的精度。但由于惯性传感器的特性，imu在长期独立运行后会积累巨大的误差，这些误差可能会导致定位性能的急剧下降。
[0071]
近些年来，同步定位和建图(slam)技术在gnss信号受遮挡的场景下取得了长足的
发展。slam技术的许多实现都利用了激光雷达(lidar)传感器，因为它具有广阔的视角、低光环境下的鲁棒性以及远距离捕捉环境细节的能力。然而，激光雷达slam只估计局部帧中的位姿，没有全局帧，这意味着位姿估计是局部准确的，但随着时间的推移容易累积误差。此外，当车辆行驶至特征点较少的区域时，激光雷达slam也会由于稀疏的点云而导致定位精度的降低。
[0072]
为弥补卫星受遮挡情况下gnss无法提供车辆可靠位置信息的问题，本发明提出了一种基于lidar/uwb/imu的车辆融合定位方法。本方法通过在激光雷达点云稀疏的路段布设主动特征以增加激光雷达观测量；同时通过在主动特征顶部搭载uwb基站来识别相应主动特征的编号，并且增加额外的uwb距离观测信息；为了进一步提升定位输出精度和频率，本发明还使用了低成本的imu用以提供车辆的运动信息。本发明的融合算法采用因子图算法，它能充分利用lidar、uwb和imu的历史观测信息，并能实现车辆在实际行驶中各传感器的“即插即用”，具有输出频率高、精度好的显著优点。具体步骤如下：
[0073]
步骤1：布置路侧主动特征与uwb基站
[0074]
本发明在激光雷达点云稀疏的区域每隔10-20m等间距的布置主动特征及uwb基站。其中主动特征为低成本的白色圆柱体泡沫，将主动特征安装在相应支架上，在每个主动特征的顶部都安装相应的uwb基站。以正东方向为ox，正北方向为oy，地球表面一个固定点为原点建立坐标系。各个主动特征及uwb基站在坐标系下的位置可通过提前标定获取。将uwb标签和激光雷达安装在行驶车辆的顶端。
[0075]
步骤2：激光雷达观测量识别
[0076]
根据步骤1中的布局，在同一时刻uwb标签所能接收到的观测信息为r1，r2，...，rn，其中rn表示第n个主动特征与车辆之间的距离值；激光雷达检测到主动特征后所解算的观测信息为d，θ，其中d为路侧某个未知的主动特征与激光雷达之间的距离值，θ为该主动特征与激光雷达之间相对于正北方向的夹角的绝对值。由于激光雷达对主动特征进行检测时无法判断该主动特征的编号，而主动特征顶部的uwb恰好弥补了这一缺点。通过将激光雷达解算的距离信息d与uwb标签接收到的距离信息作对比，若|d-ri|＜ε，(i＝1，2，...，n，ε为事先设定的阈值)，则可判定当前激光雷达检测的距离d与角度θ隶属的主动特征编号i(i＝1，2，...，n)。
[0077]
步骤3：构建多传感器组合导航的因子图模型
[0078]
假设λ表示车辆的导航状态，包括三维位置x、三维速度v等信息。定义车辆的启动时刻为初始时刻，则初始时刻到当前时刻tk的导航状态可表示为：
[0079][0080]
相应的联合概率分布函数可表示为：
[0081]
p(λk|zk)
ꢀꢀꢀꢀ
(2)
[0082]
其中zk为初始时刻到当前时刻tk接收到的所有观测值。
[0083]
则待估计的导航状态的最大后验估计可表示为：
[0084][0085]
联合概率分布式(2)可以因式分解为一个先验信息和独立过程及测量模型：
[0086][0087]
上式中p(λ0)表示所有可用的先验信息；表示λi中与测量模型相关的变量；表示imu的测量值，包含比力和角速率；表示imu的后验概率密度；p(vi|v
i-1
)表示速度变量的后验概率密度；表示除imu外所有传感器测量值的后验概率密度的乘积。
[0088]
上述的因式分解可以表示成因子图模型，作为一种二分图模型，因子图包含了因子节点和状态节点两类。因子图中的每个因子节点都可以表示为式(4)中的一个独立项，并且每个因子fi(
·
)表示为一个应当最小化的误差函数，假设这个误差函数为则因子fi(
·
)可定义为：
[0089][0090]
其中d(
·
)表示代价函数。假设各个传感器的误差均服从高斯分布，则因子fi(
·
)满足如下形式：
[0091][0092]
上式中，表示马氏距离，∑i为传感器的误差协方差矩阵。则传感器的测量模型因子可定义为：
[0093][0094]
式中，h(
·
)为非线性测量函数，zi为传感器实际的测量值，因此式(4)的最大后验估计问题就变成了如下的非线性最小二乘问题：
[0095][0096]
本发明构建的因子图模型如图3所示，在图3中，xk和vk分别代表k时刻车辆的位置和速度。其中xk＝[xek，xnk，xuk]
t
分别代表k时刻车辆的东向位置、北向位置和天向位置；vk＝[vek，vnk，vuk]
t
分别代表k时刻车辆的东向速度、北向速度和天向速度。f
prior
代表先验因子，f
mm
代表运动模型因子，f
uwb
代表uwb因子，fv代表速度因子，f
slam
代表激光雷达slam因子，f
af
代表激光雷达检测到的主动特征因子。
[0097]
子步骤1：imu等效因子建立
[0098]
本发明中的imu因子包含了运动模型因子f
mm
和速度因子fv，通过接收来自imu的测量值来定义这两种因子。其中fb与ωb分别代表机体坐标系下载体的比力和角速度。因此，车辆的运动模型可表示为：
[0099][0100]
车辆的速度模型可表示为：
[0101][0102]
其中，ae、an、au分别为捷联惯导算法转换的载体在东北天坐标系下的东向、北向和天向加速度；和分别为车辆运动模型和速度模型的误差协方差矩阵。则imu等效因子的误差函数可表示为：
[0103][0104]
子步骤2：uwb因子的建立
[0105]
在每个采样时间k，车载uwb标签接收到的距离测量值可表示为rk＝{r
k，1
，r
k，2
，...，r
k，j
}，其中r
k，j
表示k时刻标签接收到的路侧第j个基站的距离值。因此，第k个基站的观测方程可表示为：
[0106][0107]
其中，xk＝[xek，xnk，xuk]
t
表示k时刻车辆的三维位置；表示事先标定好的路侧第j个基站的三维坐标；||
·
||表示车辆位置与基站位置之间的欧氏距离。则uwb距离的观测方程可表示为：
[0108][0109]
其中为uwb因子的误差协方差矩阵。则uwb因子的误差函数为：
[0110][0111]
子步骤3：主动特征因子的建立
[0112]
当激光雷达扫描到路侧主动特征后，将产生d和θ两个观测量，其中d和θ分别为路侧某个未知的主动特征与激光雷达之间的距离值以及该主动特征与激光雷达之间相对于正北方向的夹角的绝对值。通过步骤2可确定该主动特征的编号。因此，主动特征观测方程可表示为：
[0113][0114]
由于uwb基站安装在主动特征项部，因此上式中的也代表路侧第j个主动特征的三维坐标。则主动特征的测量模型可表示为：
[0115][0116]
其中a
k，j
＝[d
k，j
，θ
k，j
]
t
，为主动特征因子的误差协方差矩阵。则主动特征因子的误差函数为：
[0117][0118]
子步骤4：激光雷达slam因子的建立
[0119]
激光雷达slam技术通过点云匹配算法在提前构建好的地图中进行车辆定位，它能实时地输出车辆的位置信息。在k时刻激光雷达slam输出的观测量可表示为其中分别代表slam输出的车辆的东向位置、北向位置和天向位置信息。则slam的测量模型可表示为：
[0120][0121]
则slam因子的误差函数为：
[0122][0123]
通过计算imu等效因子、uwb因子、主动特征因子和激光雷达slam因子的误差函数，就能得到k时刻系统状态的总体误差函数
[0124][0125]
通过最小化各等效因子的误差函数，即可求得每一时刻车辆的最优位置估计，实现gnss失效情况下车辆的实时定位。
[0126]
在本实施实例中，为了检验所提出的方法对车辆定位精度提高的有益效果，开展了实车试验：对本发明提出的lidar/uwb/imu融合定位方法与传统的lidar/imu融合定位方法进行了对比，图4为一组实验对比结果图，图5为两种方法的欧式距离误差图，表1列出了两种方法的定位误差统计。
[0127]
表1两种方法的欧式距离定位误差对比表(单位：厘米)
[0128][0129]
由图4、图5及表1的结果可以明显看出，加了路侧主动特征和uwb基站后的车辆定位轨迹更加接近于真实值，误差更小，相对于传统的基于激光雷达slam和imu融合的定位方法，本实施例中提出的lidar/uwb/imu的融合定位精度更高。