一种面向区域场景的激光雷达高精度定位方法
阅读说明：本技术 一种面向区域场景的激光雷达高精度定位方法 (Laser radar high-precision positioning method for regional scene ) 是由 季博文 吕品 赖际舟 李志敏 袁诚 方玮 郑国庆 温烨贝 于 2021-07-30 设计创作，主要内容包括：本发明公开了一种面向区域场景的激光雷达高精度定位方法,包括如下步骤：步骤(1)在区域周围放置反光板,利用激光雷达在区域内构建反光板点云地图F-(M)；步骤(2)利用无人车周期采集第k时刻激光雷达点云数据,对激光雷达点云进行预处理得到预处理后点云集合S(k)；步骤(3)通过点云匹配,解耦优化无人车位姿；步骤(4)重复步骤(2)和步骤(3),直到无人车导航结束。本发明能够在一定区域内,实现基于三维激光雷达的高精度定位。(The invention discloses a high-precision positioning method of a laser radar facing to an area scene, which comprises the following steps: step (1) placing reflectors around an area, and constructing a reflector point cloud map F in the area by using a laser radar M (ii) a Step (2) periodically collecting laser radar point cloud data at the kth moment by using an unmanned vehicle, and preprocessing the laser radar point cloud to obtain a preprocessed point cloud set S (k); decoupling and optimizing the pose of the unmanned vehicle through point cloud matching; and (4) repeating the step (2) and the step (3) until the navigation of the unmanned vehicle is finished. The invention can realize high-precision positioning based on the three-dimensional laser radar in a certain area.)
技术领域
本发明属于自主导航与制导领域，涉及一种面向区域场景的激光雷达高精度定位方法。
背景技术
随着智能化程度的提高，无人车的运用越来越广泛，涉及到物流、安防、巡检等各个领域。对自身位姿的精准估计是无人车完成任务的前提，常用的导航方式包括卫星/惯性组合导航、视觉导航、激光雷达导航等。为完成某些特殊任务，无人车被要求在一定区域内对自身位姿拥有高精度的估计，如自主充电。卫星/惯性组合导航常用于室外等开阔场景下，在卫星受遮挡的情况下表现不佳。常用的激光雷达SLAM方法和视觉SLAM方法无法满足其定位需求。
现有的技术中，主流的方法包括磁条导引、视觉二维码定位和激光雷达定位。磁条导引通过磁带引导无人车沿预定轨迹行驶，但其铺设成本高，且轨迹改变难度大，无法灵活地调整路线。基于视觉的方法通过识别二维码获取二维码中的位置信息，从而引导无人车到达指定区域。但是视觉传感器受光照强度的影响较大，在弱光的环境下工作效果不佳。基于激光的定位方法多采用二维激光雷达，利用圆柱型反光标志物进行辅助定位。但二维激光雷达感知范围有限，容易受到遮挡，对环境要求较高。
发明内容
本发明所要解决的技术问题是：提供一种面向区域场景的激光雷达高精度定位方法，以解决无人车在特殊场景下的精准定位问题。
本发明为解决上述技术问题采用以下技术方案：
一种面向区域场景的激光雷达高精度定位方法，包括步骤如下：
步骤(1)：在区域周围放置反光板，利用激光雷达在区域内构建反光板点云地图FM；
步骤(2)：利用无人车周期采集第k时刻激光雷达点云数据，对激光雷达点云进行预处理得到预处理后点云集合S(k)；
步骤(3)：通过点云匹配，解耦优化无人车位姿；
步骤(4)：重复步骤(2)和步骤(3)，直到无人车导航结束。
进一步的，所述步骤(1)具体为：
步骤11)在区域后方、左方、右方各放置一块反光板，反光板要求为平面，大小至少为1m*1m，反光板中心与无人车上激光雷达保存同一高度；
步骤12)将无人车停放至区域中心，采集激光雷达点云；
步骤13)根据激光雷达点云的强度信息，筛选出强度值高于100的点云集合Q(k)；
步骤14)通过Ransac算法提取出反光板的点云集合{FM}，并存储其对应平面的法向量。
进一步的，所述步骤14)具体包括如下步骤：
141)根据步骤13)中筛选后的点云集合Q(k)，从该点云数据中选取至少3个点，利用最小二乘法拟合得到平面方程参数，计算Q(k)中所有点云与该平面之间的差值di，di小于0.05m的点被认定为内点，记录此时的内点数；
142)重复步骤141),记录内点数量最多的平面参数以及对应的内点，该平面参数为最佳模型参数；直到迭代30次时，认为迭代结束，计算出的最佳模型参数即为拟合得到的平面方程参数，对应的内点即为提取的反光板的点云F1；
143)从点云集合Q(k)从剔除F1，重复步骤141)、142),得到F2；
144)从点云集合Q(k)从剔除F1、F2，重复步骤141)、142),得到F3；从而得到反光板点云地图{FM}＝{F1,F2,F3}。
进一步的，所述步骤(2)中，对激光雷达点云进行预处理，包括如下步骤：
步骤21)根据激光雷达点云的强度信息，筛选出强度值高于100的点云集合I(k)；
步骤22)通过Ransac算法提取出反光板的点云集合{S(k)}，并存储其对应平面的法向量。
进一步的，所述步骤22)具体包括如下步骤：
221)根据步骤21)中筛选后的点云集合I(k)，从该点云数据中选取至少3个点，利用最小二乘法拟合得到平面方程参数，计算I(k)中所有点云与该平面之间的差值dk，dk小于0.05m的点被认定为内点，记录此时的内点数；
222)重复步骤221),记录内点数量最多的平面参数以及对应的内点，该平面参数为最佳模型参数；直到迭代30次时，认为迭代结束，计算出的最佳模型参数即为拟合得到的平面方程参数，对应的内点即为提取的反光板的点云S1；
223)从点云集合I(k)从剔除S1，重复步骤221)、222),得到S2；
224)从点云集合I(k)从剔除S1、S2，重复步骤(1)、(2),得到S3；从而得到预处理后点云集合{S(k)}＝{S1,S2,S3}。
进一步的，所述步骤(3)中，解耦优化无人车位姿，包括如下步骤：
步骤31)以k-1时刻无人车位姿Tk-1为初值，对预处理后的点云集合S(k)进行坐标变换得到S′(k)，变换方法如下：
其中si表示S(k)中的点，si′表示S′(k)中的点，si∈S(k)，s′i∈S′(k)；si＝[six,siy,siz]T，s′i＝[s′ix,s′iy,s′iz]T，six,siy,siz代表S(k)中点的三维坐标，s′ix,s′iy,s′iz代表S′(k)中点的三维坐标；
步骤32)对于当前时刻预处理后的点云集合S′(k)中的每个点s′i，在点云地图FM中寻找最近点mi作为对应点；为mi所属平面的法向量；构建非线性最小二乘问题如下：
式中ΔM4×4代表变换位姿矩阵，如下所示：
其中，α、β、γ代表无人车的三轴姿态角，tx、ty、tz代表无人车的三轴位置；假设每次优化的姿态角为小量，对ΔM4×4进行近似处理得到：
不考虑高度的优化，得到新的线性最小二乘问题：
其中，通过构建正规方程对其进行求解，从而得到新的迭代初值
步骤33)重复步骤31)、步骤32)，直到迭代30次或者小于阈值时，认为迭代结束，得到最终的优化结果T′，该结果即为无人车第k时刻解算的位姿Tk。
本方法的有益效果为：与现有技术相比，本发明能够在一定区域内，实现基于三维激光雷达的高精度定位。
附图说明
图1为本发明的流程图。
具体实施方式
为了便于本领域技术人员的理解，下面结合实施例与附图对本发明作进一步的说明，实施方式提及的内容并非对本发明的限定。
本发明实施例提供了一种面向区域场景的激光雷达高精度定位方法，流程图如图1所示，包括步骤如下：
步骤(1)：在区域周围放置反光板，利用激光雷达在区域内构建反光板点云地图FM；
步骤11)在区域后方、左方、右方各放置一块反光板，反光板要求为平面，大小至少为1m*1m，反光板中心与无人车上激光雷达保存同一高度；
步骤12)将无人车停放至区域中心，采集激光雷达点云；
步骤13)根据激光雷达点云的强度信息，筛选出强度值高于100的点云集合Q(k)；
步骤14)通过Ransac算法提取出反光板的点云集合{FM}，并存储其对应平面的法向量。
所述步骤14)具体包括如下步骤：
141)根据步骤13)中筛选后的点云集合Q(k)，从该点云数据中选取至少3个点，利用最小二乘法拟合得到平面方程参数，计算Q(k)中所有点云与该平面之间的差值di，di小于0.05m的点被认定为内点，记录此时的内点数；
142)重复步骤141),记录内点数量最多的平面参数以及对应的内点，该平面参数为最佳模型参数；直到迭代30次时，认为迭代结束，计算出的最佳模型参数即为拟合得到的平面方程参数，对应的内点即为提取的反光板的点云F1；
143)从点云集合Q(k)从剔除F1，重复步骤141)、142),得到F2；
144)从点云集合Q(k)从剔除F1、F2，重复步骤141)、142),得到F3；从而得到反光板点云地图{FM}＝{F1,F2,F3}。
步骤(2)：利用无人车周期采集第k时刻激光雷达点云数据，对激光雷达点云进行预处理得到预处理后点云集合S(k)；
步骤21)根据激光雷达点云的强度信息，筛选出强度值高于100的点云集合I(k)；
步骤22)通过Ransac算法提取出反光板的点云集合{S(k)}，并存储其对应平面的法向量。
所述步骤22)具体包括如下步骤：
221)根据步骤21)中筛选后的点云集合I(k)，从该点云数据中选取至少3个点，利用最小二乘法拟合得到平面方程参数，计算I(k)中所有点云与该平面之间的差值dk，dk小于0.05m的点被认定为内点，记录此时的内点数；
222)重复步骤221),记录内点数量最多的平面参数以及对应的内点，该平面参数为最佳模型参数；直到迭代30次时，认为迭代结束，计算出的最佳模型参数即为拟合得到的平面方程参数，对应的内点即为提取的反光板的点云S1；
223)从点云集合I(k)从剔除S1，重复步骤221)、222),得到S2；
224)从点云集合I(k)从剔除S1、S2，重复步骤(1)、(2),得到S3；从而得到预处理后点云集合{S(k)}＝{S1,S2,S3}。
步骤(3)：通过点云匹配，解耦优化无人车位姿。
步骤31)以k-1时刻无人车位姿Tk-1为初值，对预处理后的点云集合S(k)进行坐标变换得到S′(k)，变换方法如下：
其中si表示S(k)中的点，si′表示S′(k)中的点，si∈S(k)，s′i∈S′(k)；si＝[six,siy,siz]T，s′i＝[s′ix,s′iy,s′iz]T，six,siy,siz代表S(k)中点的三维坐标，s′ix,s′iy,s′iz代表S′(k)中点的三维坐标；
步骤32)对于当前时刻预处理后的点云集合S′(k)中的每个点s′i，在点云地图FM中寻找最近点mi作为对应点；为mi所属平面的法向量；构建非线性最小二乘问题如下：
式中ΔM4×4代表变换位姿矩阵，如下所示：
其中，α、β、γ代表无人车的三轴姿态角，tx、ty、tz代表无人车的三轴位置；假设每次优化的姿态角为小量，对ΔM4×4进行近似处理得到：
不考虑高度的优化，得到新的线性最小二乘问题：
其中，通过构建正规方程对其进行求解，从而得到新的迭代初值
步骤33)重复步骤31)、步骤32)，直到迭代30次或者小于阈值时，认为迭代结束，得到最终的优化结果T′，该结果即为无人车第k时刻解算的位姿Tk。
步骤(4)：重复步骤(2)和步骤(3)，直到无人车导航结束。
- 上一篇：一种医用注射器针头装配设备
- 下一篇：接近传感器角度调整方法、装置及存储介质