点云稀疏场景下主动特征辅助的自适应融合定位方法

1.本发明属于车辆导航定位领域，特别涉及一种点云稀疏场景下主动特征辅助的自适应融合定位方法。


背景技术：

2.随着经济社会的快速发展，我国的机动车保有量快速增加，道路交通面临着巨大的挑战，为了解决日益严峻的复杂交通问题，智能交通系统(intelligent transport systems，its)应运而生，并逐渐成为最新的研究发展方向。而智能交通系统的实现，离不开高精度的车辆导航定位技术，只有在实时地获取车辆准确、可靠的位置信息的前提下，全方位实施车车、车路动态实时信息的交互，充分实现人、车、路的有效协同，才能保证车辆的安全行驶，改善交通环境。因此，车辆定位技术是智能交通等研究的基础与核心内容之一。
3.目前，在车辆导航定位领域中应用最为广泛的是全球卫星导航系统(global navigation satellite system，gnss)技术，在无遮挡的环境下，gnss能为车辆实时全天候地提供三维的位置、速度和姿态等信息。但是在隧道、地下停车场等密闭的交通环境中，由于卫星信号的遮挡，gnss会存在时期失效的现象而无法实现准确可靠的连续定位。
4.近年来，同步定位和建图(simultaneous localization and mapping，slam)技术在gnss信号受遮挡的场景下取得了长足的发展。slam的关键技术是构建动载体在未知环境中的位姿与环境地图，根据位姿与地图信息估计自身运动的位置并伴随运动过程构建增量式地图，实现车辆的导航与定位功能。slam技术的实现过程中，常用的传感器之一是激光雷达(lidar)，它具有广阔的视角、低光环境下的鲁棒性以及远距离捕捉环境细节的能力，相较于相机，激光雷达不受光照的影响，白天与黑夜的表现能力俱佳。然而，当车辆行驶至特征点较少的区域时，激光雷达slam会由于稀疏的点云而导致定位精度的降低甚至无法定位。因此，如何在点云稀疏场景下实现基于激光雷达的高精度定位是亟待解决的关键问题。


技术实现要素：

5.本发明的目的是克服现有技术的不足，提出一种基于lidar/uwb融合的车辆定位方法，实现激光slam因点云稀疏无法定位时对车辆位置的准确、可靠估计。本发明通过在激光雷达点云稀疏的路段布设固定位置的特殊形状物体，便于激光雷达检测，该物体所在位置已经事先准确测量，检测到物体的特征点后就可以知道物体的准确位置信息，并可以解算与该物体的相对位置关系，相当于主动地为激光雷达提供特征信息，因此本专利中将该特殊形状的物体称为主动特征，同时主动特征顶部搭载了uwb，可以提供编号信息以区分不同的主动特征，并且增加额外的uwb距离辅助观测信息；最后，根据激光雷达和uwb信号的距离观测值，基于模糊逻辑分类算法对激光雷达和uwb的观测精度分类，进而根据分类结果自适应的调整扩展卡尔曼滤波的观测噪声阵，最终实时地输出车辆融合定位后的精确位置。具体步骤如下：
6.步骤1：路侧主动特征与uwb基站的布设
7.本发明在激光雷达点云稀疏的区域每隔10-20m等间距的布置主动特征及uwb基站。其中主动特征为低成本的白色圆柱体泡沫，将主动特征安装在相应支架上，在每个主动特征的顶部都安装相应的uwb基站。以正东方向为ox轴，正北方向为oy轴，地球表面某一固定点为原点建立坐标系。各个主动特征及uwb基站在该坐标系下的位置可通过提前标定获取。uwb信号接收标签及激光雷达安装在行驶车辆的顶端。
8.步骤2：激光雷达观测量识别
9.根据步骤1中的布局，在同一时刻uwb标签所能接收到的观测信息为r1,r2,
…
,rn，其中rn表示第n个主动特征与车辆之间的距离值；激光雷达检测到主动特征后所能解算的观测信息为d,θ，其中d为路侧某个未知的主动特征与激光雷达之间的距离值，θ为该主动特征与激光雷达之间相对于正北方向的夹角的绝对值。由于激光雷达对主动特征进行检测时无法判断该主动特征的编号，而主动特征顶部的uwb基站恰好弥补了这一缺点。通过将激光雷达解算的距离信息d与uwb标签接收到的距离信息作对比，若|d-ri|《ε，(i＝1,2,
…
,n,ε为事先设定的阈值)，则可判定当前激光雷达检测的距离d与角度θ隶属的主动特征编号i(i＝1,2,
…
,n)。
10.步骤3：基于模糊逻辑算法的uwb与激光雷达观测精度分类
11.对于激光雷达与uwb来说，二者检测物体与测距的精度都会随着传感器与被测物之间距离的增加而降低。基于此特征，本发明采用模糊逻辑算法分别对uwb基站和激光雷达的观测精度进行分类，一共分为a,b,c三个等级，对应uwb与激光雷达观测值的精度由低到高，模糊逻辑分类算法的具体步骤如下：
12.1)确定距离特征的三个隶属度函数，并对距离分类，分别为n，m，r，代表距离的“近”、“中”、“远”。关于距离的三种隶属度函数如下所示(d，单位：米)：
[0013][0014][0015][0016]
2)根据模糊分类规则，确定模糊逻辑分类输出的3个精度等级a，b，c，模糊分类规则如下：
[0017]
1.当距离为n时，精度等级为a；
[0018]
2.当距离为m时，精度等级为b；
[0019]
3.当距离为r时，精度等级为c；
[0020]
步骤4：基于自适应扩展卡尔曼滤波的车辆定位
[0021]
由步骤2可知，在离散时刻k可接收到的观测值包括uwb基站提供的距离信息r1,r2,
…
,rn以及激光雷达扫描到的主动特征的距离和角度信息d和θ，则可通过建立扩展卡尔曼滤波器获取实时的车辆位置。建立离散的状态方程和观测方程为：
[0022][0023]
其中k表示离散的时间，x(k)＝[pe(k)pn(k)]
′
为状态向量，上角标
′
表示对矩阵转置，pe(k)和pn(k)分别表示车辆的东西和北向位置坐标，状态转移向量(k)分别表示车辆的东西和北向位置坐标，状态转移向量(k)分别表示车辆的东西和北向位置坐标，状态转移向量为系统的观测向量，其中为n个uwb基站的距离观测值，和分别为激光雷达检测到的相应主动特征的距离和角度观测值；h是相应的观测函数：
[0024][0025]
(xi,yi)是事先标定好的第i个主动特征的坐标，表示相应的观测噪声向量；w和v分别为系统的状态噪声和观测噪声向量，它们的协方差矩阵分别为q和r，其中r＝diag[r
uwbrlidar
]，r
uwb
与r
lidar
分别为uwb与激光雷达的观测噪声方差阵，根据步骤3中所述的模糊分类算法对uwb基站与激光雷达的观测精度进行分类，进而根据分类结果自适应调节uwb与激光雷达的噪声方差阵r
uwb
、r
lidar
，得到最终的观测噪声方差阵r(k)，具体方法如下：
[0026]
1.计算第i个uwb基站与激光雷达检测的第j个主动特征的观测噪声1.计算第i个uwb基站与激光雷达检测的第j个主动特征的观测噪声其中和分别为uwb距离观测值和激光雷达主动特征观测值的噪声方差基础值；μ和ρ为调节系数，由模糊分类的结果确定，三种不同的精度等级a,b,c对应的调节系数分别为μa,μb,μc和ρa,ρb,ρc。模型调节系数与精度等级如下表所示：
[0027]
表1距离精度等级与模型调节系数
[0028]
距离精度等级模型调节系数a1b2.5c6
[0029]
2.计算uwb与激光雷达的观测噪声方差阵：2.计算uwb与激光雷达的观测噪声方差阵：
[0030]
3.更新噪声方差阵r(k)：r(k)＝diag[r
uwbrlidar
]；
[0031]
根据上述步骤所述的系统状态模型和观测模型以及噪声方差阵的自适应调节后，即可进行扩展卡尔曼滤波的迭代更新，标准扩展卡尔曼滤波过程包括时间更新和测量更新：
[0032]
时间更新：
[0033][0034]
p(k,k-1)＝f(k,k-1)p(k-1)a
′
(k,k-1) q(k-1)
ꢀꢀꢀꢀꢀꢀ
(7)
[0035]
测量更新：
[0036]
k(k)＝p(k,k-1)h
′
(k)[h(k)p(k,k-1)h
′
(k) r(k)]-1
ꢀꢀꢀꢀꢀꢀ
(8)
[0037][0038]
p(k)＝[i-k(k)h(k)]p(k,k-1)
ꢀꢀꢀꢀꢀꢀ
(10)
[0039]
其中i表示单位矩阵，h是h(
·
)对x求导的雅可比矩阵。
[0040]
通过滤波计算，则可计算出每一时刻车辆的位置信息，实现点云稀疏场景下车辆的可靠定位。
[0041]
本发明的显著效果是：
[0042]
(1)本发明提出了一种点云稀疏场景下的车辆可靠定位方法，针对激光slam算法在特征稀疏场景下定位精度下降甚至无法定位的情况，提出了一种基于lidar/uwb的车辆融合定位方法，通过在路侧布设主动特征和uwb基站来增加激光雷达的观测量从而显著提升车辆定位精度；
[0043]
(2)本发明利用模糊逻辑算法对uwb和激光雷达的观测精度进行分类，进而自适应调节uwb与激光雷达的观测噪声方差阵，可有效解决测距精度随着传感器与被测物之间距离的增加而降低的问题。
附图说明：
[0044]
图1是本发明的技术路线图；
[0045]
图2是主动特征与uwb的布局图；
[0046]
图3是模糊逻辑分类算法的隶属度函数图；
[0047]
图4是采用了自适应扩展卡尔曼滤波算法前后的车辆定位轨迹对比图；
[0048]
图5是采用了自适应扩展卡尔曼滤波算法前后的车辆定位误差对比图。
具体实施方式
[0049]
近年来，智能交通系统在全球范围内得到了飞速的发展，它能将新进的传感器技术、数据通讯传输技术、信息技术等各种技术综合运用于交通系统中，从而有效地实现对交通系统的综合管理，最大限度地实现人、车、路的和谐统一。而对于大部分智能交通系统而言，获取准确、可靠的位置信息是发挥其功能的前提。
[0050]
目前，在车辆导航定位领域中应用最为广泛的是全球卫星导航系统(global navigation satellite system，gnss)技术，在无遮挡的环境下，gnss能为车辆实时全天候地提供三维的位置、速度和姿态等信息。但是在隧道、地下停车场等密闭的交通环境中，由于卫星信号的遮挡，gnss会存在时期失效的现象而无法实现准确可靠的连续定位。
[0051]
近年来，同步定位和建图(simultaneous localization and mapping，slam)技术在gnss信号受遮挡的场景下取得了长足的发展。slam的关键技术是构建动载体在未知环境中的位姿与环境地图，根据位姿与地图信息估计自身运动的位置并伴随运动过程构建增量
式地图，实现车辆的导航与定位功能。slam技术的许多实现都利用了激光雷达(lidar)传感器，因为它具有广阔的视角、低光环境下的鲁棒性以及远距离捕捉环境细节的能力，相较于相机，激光雷达不受光照的影响，白天与黑夜的表现能力俱佳。然而，当车辆行驶至特征点较少的区域时，激光雷达slam会由于稀疏的点云而导致定位精度的降低甚至无法定位。因此，在点云稀疏场景下结合道路交通设施，选取合适的方法实现基于激光雷达的高精度定位是亟待解决的关键问题。
[0052]
为弥补激光slam因点云稀疏而导致定位精度的降低甚至无法定位的问题，本发明提出了一种基于lidar/uwb的车辆融合定位方法，通过在激光雷达点云稀疏的路段布设白色圆柱体主动特征以增加激光雷达观测量；同时在主动特征顶部搭载uwb基站来配合激光雷达识别相应主动特征编号，并且增加额外的uwb距离辅助观测信息；最后，根据激光雷达和uwb信号的距离特征，基于模糊逻辑分类算法对激光雷达和uwb的观测精度分类，进而根据分类结果自适应的调整扩展卡尔曼滤波的观测噪声阵，最终实时地输出车辆融合定位后的精确位置。具体步骤如下：
[0053]
步骤1：路侧主动特征与uwb基站的布设
[0054]
本发明在激光雷达点云稀疏的区域每隔10-20m等间距的布置主动特征及uwb基站。其中主动特征为低成本的白色圆柱体泡沫，将主动特征安装在相应支架上，在每个主动特征的顶部都安装相应的uwb基站。以正东方向为ox轴，正北方向为oy轴，地球表面某一固定点为原点建立坐标系。各个主动特征及uwb基站在坐标系下的位置可通过提前标定获取。uwb信号接收标签及激光雷达安装在行驶车辆的顶端。
[0055]
步骤2：激光雷达观测量识别
[0056]
根据步骤1中的布局，在同一时刻uwb标签所能接收到的观测信息为r1,r2,
…
,rn，其中rn表示第n个主动特征与车辆之间的距离值；激光雷达检测到主动特征后所能解算的观测信息为d,θ，其中d为路侧某个未知的主动特征与激光雷达之间的距离值，θ为该主动特征与激光雷达之间相对于正北方向的夹角的绝对值。由于激光雷达对主动特征进行检测时无法判断该主动特征的编号，而主动特征顶部的uwb基站恰好弥补了这一缺点。通过将激光雷达解算的距离信息d与uwb标签接收到的距离信息作对比，若|d-ri|《ε，(i＝1,2,
…
,n,ε为事先设定的阈值)，则可判定当前激光雷达检测的距离d与角度θ隶属的主动特征编号i(i＝1,2,
…
,n)。
[0057]
步骤3：基于模糊逻辑算法的uwb与激光雷达观测精度分类
[0058]
对于激光雷达与uwb来说，二者检测物体与测距的精度都会随着传感器与被测物之间距离的增加而降低。基于此特征，本发明采用模糊逻辑算法分别对uwb基站和激光雷达的观测精度进行分类，一共分为a,b,c三个等级，对应uwb与激光雷达观测值的精度由低到高，模糊逻辑分类算法的具体步骤如下：
[0059]
1)确定距离特征的三个隶属度函数，并对距离分类，分别为n，m，r，代表距离的“近”、“中”、“远”。关于距离的三种隶属度函数如下所示(d，单位：米)：
[0060]
[0061][0062][0063]
2)根据模糊分类规则，确定模糊逻辑分类输出的3个精度等级a，b，c，模糊分类规则如下：
[0064]
1.当距离为n时，精度等级为a；
[0065]
2.当距离为m时，精度等级为b；
[0066]
3.当距离为r时，精度等级为c；
[0067]
步骤4：基于自适应扩展卡尔曼滤波的车辆定位
[0068]
由步骤2可知，在离散时刻k可接收到的观测值包括uwb基站提供的距离信息r1,r2,
…
,rn以及激光雷达扫描到的主动特征的距离和角度信息d和θ，则可通过建立扩展卡尔曼滤波器获取实时的车辆位置。建立离散的状态方程和观测方程为：
[0069][0070]
其中k表示离散的时间，x(k)＝[pe(k)pn(k)]
′
为状态向量，上角标
′
表示对矩阵转置，pe(k)和pn(k)分别表示车辆的东西和北向位置坐标，状态转移向量(k)分别表示车辆的东西和北向位置坐标，状态转移向量(k)分别表示车辆的东西和北向位置坐标，状态转移向量为系统的观测向量，其中为n个uwb基站的距离观测值，和分别为激光雷达检测到的相应主动特征的距离和角度观测值；h是相应的观测函数：
[0071][0072]
(xi,yi)是事先标定好的第i个主动特征的坐标，表示相应的观测噪声向量；w和v分别为系统的状态噪声和观测噪声向量，它们的协方差矩阵分别为q和r，其中r＝diag[r
uwbrlidar
]，r
uwb
与r
lidar
分别为uwb与激光雷达的观测噪声方差阵，根据步骤3中所述的模糊分类算法对uwb基站与激光雷达的观测精度进行分类，进而根据分类结果自适应调节uwb与激光雷达的噪声方差阵r
uwb
、r
lidar
，得到最终的观测噪声方差阵r(k)，具体方法如下：
[0073]
1.计算第i个uwb基站与激光雷达检测的第j个主动特征的观测噪声1.计算第i个uwb基站与激光雷达检测的第j个主动特征的观测噪声其中和分别为uwb距离观测值和激光雷达主动特征观测值的噪声方差基础值；μ和ρ为调节系数，由模糊分类的结果确定，三种不同的精
度等级a,b,c对应的调节系数分别为μa,μb,μc和ρa,ρb,ρc。模型调节系数与精度等级如下表所示：
[0074]
表1距离精度等级与模型调节系数
[0075]
距离精度等级模型调节系数a1b2.5c6
[0076]
2.计算uwb与激光雷达的观测噪声方差阵：2.计算uwb与激光雷达的观测噪声方差阵：
[0077]
3.更新噪声方差阵r(k)：r(k)＝diag[r
uwb r
lidar
]；
[0078]
根据上述步骤所述的系统状态模型和观测模型以及噪声方差阵的自适应调节后，即可进行扩展卡尔曼滤波的迭代更新，标准扩展卡尔曼滤波过程包括时间更新和测量更新：
[0079]
时间更新：
[0080][0081]
p(k,k-1)＝f(k,k-1)p(k-1)f
′
(k,k-1) q(k-1)
ꢀꢀꢀꢀꢀꢀ
(7)
[0082]
测量更新：
[0083]
k(k)＝p(k,k-1)h
′
(k)[h(k)p(k,k-1)h
′
(k) r(k)]-1
ꢀꢀꢀꢀꢀꢀ
(8)
[0084][0085]
p(k)＝[i-k(k)h(k)]p(k,k-1)
ꢀꢀꢀꢀꢀꢀ
(10)
[0086]
其中i表示单位矩阵，h是h(
·
)对x求导的雅可比矩阵。
[0087]
通过滤波计算，则可计算出每一时刻车辆的位置信息，实现点云稀疏场景下车辆的可靠定位。
[0088]
在本实施实例中，为了检验所提出的方法对车辆定位精度提高的有益效果，开展了实车实验，对采用了模糊自适应算法前、后的车辆定位结果进行了对比试验。另外，需指出的是：此处试验中“采用模糊算法处理前”的方法指的是基于传统扩展卡尔曼滤波的lidar/uwb融合方法，“采用模糊算法处理后”的方法指的基于自适应扩展卡尔曼滤波的lidar/uwb融合方法。
[0089]
图4为一组试验结果图，由图中的可以明显看出，采用了自适应扩展卡尔曼滤波算法后，车辆的定位轨迹更接近于真实值。图5为采用自适应扩展卡尔曼滤波算法前后车辆定位的欧式误差对比图，由图中的定位误差可以明显看出，采用了自适应调节的扩展卡尔曼滤波的观测噪声矩阵后，定位误差减小，车辆定位精度显著提高。