CN113671511A - 一种面向区域场景的激光雷达高精度定位方法 - Google Patents

Abstract

本发明公开了一种面向区域场景的激光雷达高精度定位方法，包括如下步骤：步骤(1)在区域周围放置反光板，利用激光雷达在区域内构建反光板点云地图F_M；步骤(2)利用无人车周期采集第k时刻激光雷达点云数据，对激光雷达点云进行预处理得到预处理后点云集合S(k)；步骤(3)通过点云匹配，解耦优化无人车位姿；步骤(4)重复步骤(2)和步骤(3)，直到无人车导航结束。本发明能够在一定区域内，实现基于三维激光雷达的高精度定位。

Description

一种面向区域场景的激光雷达高精度定位方法

技术领域

本发明属于自主导航与制导领域，涉及一种面向区域场景的激光雷达高精度定位方法。

背景技术

随着智能化程度的提高，无人车的运用越来越广泛，涉及到物流、安防、巡检等各个领域。对自身位姿的精准估计是无人车完成任务的前提，常用的导航方式包括卫星/惯性组合导航、视觉导航、激光雷达导航等。为完成某些特殊任务，无人车被要求在一定区域内对自身位姿拥有高精度的估计，如自主充电。卫星/惯性组合导航常用于室外等开阔场景下，在卫星受遮挡的情况下表现不佳。常用的激光雷达SLAM方法和视觉SLAM方法无法满足其定位需求。

现有的技术中，主流的方法包括磁条导引、视觉二维码定位和激光雷达定位。磁条导引通过磁带引导无人车沿预定轨迹行驶，但其铺设成本高，且轨迹改变难度大，无法灵活地调整路线。基于视觉的方法通过识别二维码获取二维码中的位置信息，从而引导无人车到达指定区域。但是视觉传感器受光照强度的影响较大，在弱光的环境下工作效果不佳。基于激光的定位方法多采用二维激光雷达，利用圆柱型反光标志物进行辅助定位。但二维激光雷达感知范围有限，容易受到遮挡，对环境要求较高。

发明内容

本发明所要解决的技术问题是：提供一种面向区域场景的激光雷达高精度定位方法，以解决无人车在特殊场景下的精准定位问题。

本发明为解决上述技术问题采用以下技术方案：

一种面向区域场景的激光雷达高精度定位方法，包括步骤如下：

步骤(1)：在区域周围放置反光板，利用激光雷达在区域内构建反光板点云地图F_M；

步骤(2)：利用无人车周期采集第k时刻激光雷达点云数据，对激光雷达点云进行预处理得到预处理后点云集合S(k)；

步骤(3)：通过点云匹配，解耦优化无人车位姿；

步骤(4)：重复步骤(2)和步骤(3)，直到无人车导航结束。

进一步的，所述步骤(1)具体为：

步骤11)在区域后方、左方、右方各放置一块反光板，反光板要求为平面，大小至少为1m*1m，反光板中心与无人车上激光雷达保存同一高度；

步骤12)将无人车停放至区域中心，采集激光雷达点云；

步骤13)根据激光雷达点云的强度信息，筛选出强度值高于100的点云集合Q(k)；

步骤14)通过Ransac算法提取出反光板的点云集合{F_M}，并存储其对应平面的法向量。

进一步的，所述步骤14)具体包括如下步骤：

141)根据步骤13)中筛选后的点云集合Q(k)，从该点云数据中选取至少3个点，利用最小二乘法拟合得到平面方程参数，计算Q(k)中所有点云与该平面之间的差值d_i，d_i小于0.05m的点被认定为内点，记录此时的内点数；

142)重复步骤141),记录内点数量最多的平面参数以及对应的内点，该平面参数为最佳模型参数；直到迭代30次时，认为迭代结束，计算出的最佳模型参数即为拟合得到的平面方程参数，对应的内点即为提取的反光板的点云F₁；

143)从点云集合Q(k)从剔除F₁，重复步骤141)、142),得到F₂；

144)从点云集合Q(k)从剔除F₁、F₂，重复步骤141)、142),得到F₃；从而得到反光板点云地图{F_M}＝{F₁,F₂,F₃}。

进一步的，所述步骤(2)中，对激光雷达点云进行预处理，包括如下步骤：

步骤21)根据激光雷达点云的强度信息，筛选出强度值高于100的点云集合I(k)；

步骤22)通过Ransac算法提取出反光板的点云集合{S(k)}，并存储其对应平面的法向量。

进一步的，所述步骤22)具体包括如下步骤：

221)根据步骤21)中筛选后的点云集合I(k)，从该点云数据中选取至少3个点，利用最小二乘法拟合得到平面方程参数，计算I(k)中所有点云与该平面之间的差值d_k，d_k小于0.05m的点被认定为内点，记录此时的内点数；

222)重复步骤221),记录内点数量最多的平面参数以及对应的内点，该平面参数为最佳模型参数；直到迭代30次时，认为迭代结束，计算出的最佳模型参数即为拟合得到的平面方程参数，对应的内点即为提取的反光板的点云S₁；

223)从点云集合I(k)从剔除S₁，重复步骤221)、222),得到S₂；

224)从点云集合I(k)从剔除S₁、S₂，重复步骤(1)、(2),得到S₃；从而得到预处理后点云集合{S(k)}＝{S₁,S₂,S₃}。

进一步的，所述步骤(3)中，解耦优化无人车位姿，包括如下步骤：

步骤31)以k-1时刻无人车位姿T_k-1为初值，对预处理后的点云集合S(k)进行坐标变换得到S′(k)，变换方法如下：

其中s_i表示S(k)中的点，s_i′表示S′(k)中的点，s_i∈S(k)，s′_i∈S′(k)；s_i＝[s_ix,s_iy,s_iz]^T，s′_i＝[s′_ix,s′_iy,s′_iz]^T，s_ix,s_iy,s_iz代表S(k)中点的三维坐标，s′_ix,s′_iy,s′_iz代表S′(k)中点的三维坐标；

步骤32)对于当前时刻预处理后的点云集合S′(k)中的每个点s′_i，在点云地图F_M中寻找最近点m_i作为对应点；

为m_i所属平面的法向量；构建非线性最小二乘问题如下：

式中ΔM_4×4代表变换位姿矩阵，如下所示：

其中，α、β、γ代表无人车的三轴姿态角，t_x、t_y、t_z代表无人车的三轴位置；假设每次优化的姿态角为小量，对ΔM_4×4进行近似处理得到：

不考虑高度的优化，得到新的线性最小二乘问题：

其中，

通过构建正规方程对其进行求解，从而得到新的迭代初值

步骤33)重复步骤31)、步骤32)，直到迭代30次或者

小于阈值

时，认为迭代结束，得到最终的优化结果T′，该结果即为无人车第k时刻解算的位姿T_k。

本方法的有益效果为：与现有技术相比，本发明能够在一定区域内，实现基于三维激光雷达的高精度定位。

附图说明

图1为本发明的流程图。

具体实施方式

为了便于本领域技术人员的理解，下面结合实施例与附图对本发明作进一步的说明，实施方式提及的内容并非对本发明的限定。

本发明实施例提供了一种面向区域场景的激光雷达高精度定位方法，流程图如图1所示，包括步骤如下：

步骤(1)：在区域周围放置反光板，利用激光雷达在区域内构建反光板点云地图F_M；

步骤11)在区域后方、左方、右方各放置一块反光板，反光板要求为平面，大小至少为1m*1m，反光板中心与无人车上激光雷达保存同一高度；

步骤12)将无人车停放至区域中心，采集激光雷达点云；

步骤13)根据激光雷达点云的强度信息，筛选出强度值高于100的点云集合Q(k)；

步骤14)通过Ransac算法提取出反光板的点云集合{F_M}，并存储其对应平面的法向量。

所述步骤14)具体包括如下步骤：

141)根据步骤13)中筛选后的点云集合Q(k)，从该点云数据中选取至少3个点，利用最小二乘法拟合得到平面方程参数，计算Q(k)中所有点云与该平面之间的差值d_i，d_i小于0.05m的点被认定为内点，记录此时的内点数；

142)重复步骤141),记录内点数量最多的平面参数以及对应的内点，该平面参数为最佳模型参数；直到迭代30次时，认为迭代结束，计算出的最佳模型参数即为拟合得到的平面方程参数，对应的内点即为提取的反光板的点云F₁；

143)从点云集合Q(k)从剔除F₁，重复步骤141)、142),得到F₂；

144)从点云集合Q(k)从剔除F₁、F₂，重复步骤141)、142),得到F₃；从而得到反光板点云地图{F_M}＝{F₁,F₂,F₃}。

步骤(2)：利用无人车周期采集第k时刻激光雷达点云数据，对激光雷达点云进行预处理得到预处理后点云集合S(k)；

步骤21)根据激光雷达点云的强度信息，筛选出强度值高于100的点云集合I(k)；

步骤22)通过Ransac算法提取出反光板的点云集合{S(k)}，并存储其对应平面的法向量。

所述步骤22)具体包括如下步骤：

221)根据步骤21)中筛选后的点云集合I(k)，从该点云数据中选取至少3个点，利用最小二乘法拟合得到平面方程参数，计算I(k)中所有点云与该平面之间的差值d_k，d_k小于0.05m的点被认定为内点，记录此时的内点数；

222)重复步骤221),记录内点数量最多的平面参数以及对应的内点，该平面参数为最佳模型参数；直到迭代30次时，认为迭代结束，计算出的最佳模型参数即为拟合得到的平面方程参数，对应的内点即为提取的反光板的点云S₁；

223)从点云集合I(k)从剔除S₁，重复步骤221)、222),得到S₂；

224)从点云集合I(k)从剔除S₁、S₂，重复步骤(1)、(2),得到S₃；从而得到预处理后点云集合{S(k)}＝{S₁,S₂,S₃}。

步骤(3)：通过点云匹配，解耦优化无人车位姿。

步骤31)以k-1时刻无人车位姿T_k-1为初值，对预处理后的点云集合S(k)进行坐标变换得到S′(k)，变换方法如下：

其中s_i表示S(k)中的点，s_i′表示S′(k)中的点，s_i∈S(k)，s′_i∈S′(k)；s_i＝[s_ix,s_iy,s_iz]^T，s′_i＝[s′_ix,s′_iy,s′_iz]^T，s_ix,s_iy,s_iz代表S(k)中点的三维坐标，s′_ix,s′_iy,s′_iz代表S′(k)中点的三维坐标；

步骤32)对于当前时刻预处理后的点云集合S′(k)中的每个点s′_i，在点云地图F_M中寻找最近点m_i作为对应点；

为m_i所属平面的法向量；构建非线性最小二乘问题如下：

式中ΔM_4×4代表变换位姿矩阵，如下所示：

其中，α、β、γ代表无人车的三轴姿态角，t_x、t_y、t_z代表无人车的三轴位置；假设每次优化的姿态角为小量，对ΔM_4×4进行近似处理得到：

不考虑高度的优化，得到新的线性最小二乘问题：

其中，

通过构建正规方程对其进行求解，从而得到新的迭代初值

步骤33)重复步骤31)、步骤32)，直到迭代30次或者

小于阈值

时，认为迭代结束，得到最终的优化结果T′，该结果即为无人车第k时刻解算的位姿T_k。

步骤(4)：重复步骤(2)和步骤(3)，直到无人车导航结束。

Claims (6)

1.一种面向区域场景的激光雷达高精度定位方法，其特征在于，包括步骤如下：

步骤(1)：在区域周围放置反光板，利用激光雷达在区域内构建反光板点云地图F_M；

步骤(2)：利用无人车周期采集第k时刻激光雷达点云数据，对激光雷达点云进行预处理得到预处理后点云集合S(k)；

步骤(3)：通过点云匹配，解耦优化无人车位姿；

步骤(4)：重复步骤(2)和步骤(3)，直到无人车导航结束。

2.根据权利要求1所述一种面向区域场景的激光雷达高精度定位方法，其特征在于，所述步骤(1)具体为：

步骤11)在区域后方、左方、右方各放置一块反光板，反光板要求为平面，大小至少为1m*1m，反光板中心与无人车上激光雷达保存同一高度；

步骤12)将无人车停放至区域中心，采集激光雷达点云；

步骤13)根据激光雷达点云的强度信息，筛选出强度值高于100的点云集合Q(k)；

步骤14)通过Ransac算法提取出反光板的点云集合{F_M}，并存储其对应平面的法向量。

3.根据权利要求2所述一种面向区域场景的激光雷达高精度定位方法，其特征在于，所述步骤14)具体包括如下步骤：

141)根据步骤13)中筛选后的点云集合Q(k)，从该点云数据中选取至少3个点，利用最小二乘法拟合得到平面方程参数，计算Q(k)中所有点云与该平面之间的差值d_i，d_i小于0.05m的点被认定为内点，记录此时的内点数；

142)重复步骤141),记录内点数量最多的平面参数以及对应的内点，该平面参数为最佳模型参数；直到迭代30次时，认为迭代结束，计算出的最佳模型参数即为拟合得到的平面方程参数，对应的内点即为提取的反光板的点云F₁；

143)从点云集合Q(k)从剔除F₁，重复步骤141)、142),得到F₂；

144)从点云集合Q(k)从剔除F₁、F₂，重复步骤141)、142),得到F₃；从而得到反光板点云地图{F_M}＝{F₁,F₂,F₃}。

4.根据权利要求1所述一种面向区域场景的激光雷达高精度定位方法，其特征在于，所述步骤(2)中，对激光雷达点云进行预处理，包括如下步骤：

步骤21)根据激光雷达点云的强度信息，筛选出强度值高于100的点云集合I(k)；

步骤22)通过Ransac算法提取出反光板的点云集合{S(k)}，并存储其对应平面的法向量。

5.根据权利要求4所述一种面向区域场景的激光雷达高精度定位方法，其特征在于，所述步骤22)具体包括如下步骤：

221)根据步骤21)中筛选后的点云集合I(k)，从该点云数据中选取至少3个点，利用最小二乘法拟合得到平面方程参数，计算I(k)中所有点云与该平面之间的差值d_k，d_k小于0.05m的点被认定为内点，记录此时的内点数；

222)重复步骤221),记录内点数量最多的平面参数以及对应的内点，该平面参数为最佳模型参数；直到迭代30次时，认为迭代结束，计算出的最佳模型参数即为拟合得到的平面方程参数，对应的内点即为提取的反光板的点云S₁；

223)从点云集合I(k)从剔除S₁，重复步骤221)、222),得到S₂；

224)从点云集合I(k)从剔除S₁、S₂，重复步骤(1)、(2),得到S₃；从而得到预处理后点云集合{S(k)}＝{S₁,S₂,S₃}。

6.根据权利要求1所述一种面向区域场景的激光雷达高精度定位方法，其特征在于，所述步骤(3)中，解耦优化无人车位姿，包括如下步骤：

步骤31)以k-1时刻无人车位姿T_k-1为初值，对预处理后的点云集合S(k)进行坐标变换得到S′(k)，变换方法如下：

其中s_i表示S(k)中的点，s′_i表示S′(k)中的点，s_i∈S(k)，s′_i∈S′(k)；s_i＝[s_ix,s_iy,s_iz]^T，s′_i＝[s′_ix,s′_iy,s′_iz]^T，s_ix,s_iy,s_iz代表S(k)中点的三维坐标，s′_ix,s′_iy,s′_iz代表S′(k)中点的三维坐标；

步骤32)对于当前时刻预处理后的点云集合S′(k)中的每个点s′_i，在点云地图F_M中寻找最近点m_i作为对应点；

为m_i所属平面的法向量；构建非线性最小二乘问题如下：

式中ΔM_4×4代表变换位姿矩阵，如下所示：

其中，α、β、γ代表无人车的三轴姿态角，t_x、t_y、t_z代表无人车的三轴位置；假设每次优化的姿态角为小量，对ΔM_4×4进行近似处理得到：

不考虑高度的优化，得到新的线性最小二乘问题：

其中，

通过构建正规方程对其进行求解，从而得到新的迭代初值

步骤33)重复步骤31)、步骤32)，直到迭代30次或者

小于阈值

时，认为迭代结束，得到最终的优化结果T′，该结果即为无人车第k时刻解算的位姿T_k。

Images