双2D激光雷达相对位姿标定方法及系统

双2d激光雷达相对位姿标定方法及系统
技术领域
1.本发明属于雷达位姿标定技术领域，更具体地，本发明涉及一种双2d激光雷达相对位姿标定方法及系统。


背景技术：

2.随着激光机器人在现代物流仓储领域中的应用越来越广泛，各个应用场景对于激光机器人的定位导航精度要求也越发重要。激光雷达定位导航精度不仅依赖于算法精度，而且依赖于雷达的位姿标定精度。单激光雷达的机器人视野并不全面，因此很多机器人使用双激光雷达进行定位导航，所以位姿标定问题也越发重要。
3.申请公布号cn112305521b，申请公布日2021.02.02，专利名称：双激光雷达相对位置标定方法、装置、设备和存储介质，其是先获取第一激光雷达对预设场景的第一扫描点集合和第二激光雷达对预设场景的第二扫描点集合；其次，根据第二激光雷达对第一激光雷达的预设相对位置确定位置转换矩阵；最后，根据位置转换矩阵、第一扫描点集合和第二扫描点集合确定第二激光雷达对第一激光雷达的实际相对位置。该技术方案存在的技术问题具体如下：
4.使用scan_to_scan匹配，导致算法非常依赖初值，所以其方案使用了预设相对位姿确定位置转换矩阵，该预设位姿如果和真实值相差一定距离，在迭代优化时容易陷入局部极值，使得优化结果不是最优位姿，标定出现较大误差。


技术实现要素：

5.本发明提供一种双2d激光雷达相对位姿标定方法，旨在改善上述问题。
6.本发明是这样实现的，一种双2d激光雷达相对位姿标定方法，所述方法包括如下步骤：
7.s1、同时获取两个2d激光雷达的激光帧；
8.s2、基于位置步长和角度步长在位姿窗口范围内形成候选位姿pose_m，确定两个激光帧与占据栅格地图m匹配得分最高的候选位姿，即为局部最优位姿；
9.s3、以当前局部最优位姿为中心形成范围较小的位姿窗口，减小位置步长和角度步，执行步骤s2，直至当前最优位姿的得分不大于上次最优位姿的得分，则输出全局最优位姿；
10.s4、基于两个2d激光雷达在地图坐标系下的全局最优位姿计算两个2d激光雷达间的相对位姿。
11.进一步的，在步骤s4之后还包括：
12.s5、在最大位姿窗口范围内改变机器人当前位姿，执行步骤(1)到(4)，将计算得到相对位姿pose_relative依次放入初始相对位姿集合pose_relative_raw中；
13.s6、计算初始相对位姿集合中相对位姿的均值和协方差矩阵；
14.s7、计算初始相对位姿集合中相对位姿的标准差，剔除偏差大的相对位姿，计算两
个2d激光雷达的准确相对位姿。
15.进一步的，当当前相对位姿的偏差pose_err超过标准std_err的时，从初始相对位姿集合中删除相应的相对位姿，得到新相对位姿集合，新相对位姿集合的位姿均值即为2d激光雷达的准确相对位姿。
16.进一步的，在步骤s1之前还包括：
17.s0、通过单2d激光雷达建立高分辨率的占据栅格地图m。
18.进一步的，最大位置步进值step_lin＝3res，其中，res为占据栅格地图m的分辨率；
19.最大角度步进值为最远测距点旋转3个栅格时的角度step_ang，其计算公式具体如下：
20.step_ang＝arc(1-3*res/(2*range_max2))；
21.其中，range_max为2d激光雷达的最大测距值。
22.进一步的，最大姿态窗口范围表示如下：
23.定义最大姿态窗口范围为：两倍机器人尺寸及360度的姿态，表示具体如下：
24.x∈[-dis_max,+dis_max]；
[0025]
y∈[-dis_max,+dis_max]；
[0026]
theta∈[-π,+π]；
[0027]
dis_max为最大对角距离，length、width分别表示机器人的长度、宽度。
[0028]
进一步的，2d激光雷达a的局部最优位姿获取方法具体如下；
[0029]
s21、以位置步进值和角度步进值在位姿窗口内进行候选位姿的搜索；
[0030]
s22、基于候选位姿将激光帧scan_a的点云投影到占据栅格地图m中进行打分，返回步骤s21，直至遍历位姿窗口内的所有候选位姿，得分最高的位姿即为2d激光雷达a的局部最优位姿。
[0031]
进一步的，打分计算公式具体如下：
[0032]
scorei＝∑(m(si(pose_m)))/n
[0033]
其中，si(pose_m)表示通过候选位姿pose_m将激光点云中的第i个激光点投影到栅格地图后获取的二维栅格坐标(r,c)，m(r,c)表示二维栅格坐标(r,c)在栅格地图中的占据概率。
[0034]
本发明是这样实现的，一种2d激光雷达相对位姿标定系统，所述系统包括：
[0035]
设于机器人上的两个2d激光雷达，两个2d激光雷达与处理器通讯连接；处理器基于上述双2d激光雷达相对位姿标定方法标定两个2d激光雷达间的相对位姿。
[0036]
本发明解决了双2d激光雷达需要有重叠区域、特殊墙面或标定初值的标定问题，降低了对环境和雷达安装的依赖，避免陷入局部极值，提高了标定的精度、稳定性和效率。
附图说明
[0037]
图1为本发明实施例提供的双2d激光雷达相对位姿标定系统的结构示意图；
[0038]
图2为本发明实施例提供的双2d激光雷达相对位姿标定方法流程图；
[0039]
图3为本发明实施例提供的两2d激光雷达相对位姿标定示意图。
具体实施方式
[0040]
下面对照附图，通过对实施例的描述，对本发明的具体实施方式作进一步详细的说明，以帮助本领域的技术人员对本发明的发明构思、技术方案有更完整、准确和深入的理解。
[0041]
图1为本发明实施例提供的双2d激光雷达相对位姿标定系统的结构示意图，为了便于说明，仅示出与本发明实施例相关的部分。该系统包括：
[0042]
设于机器人上的2d激光雷达a、2d激光雷达b，2d激光雷达a、2d激光雷达b与处理器通讯连接，处理器基于如下双2d激光雷达相对位姿标定方法确定2d激光雷达b在2d激光雷达a下的相对位姿，如图3所示。
[0043]
图2为本发明实施例提供的双2d激光雷达相对位姿标定方法流程图，该方法包括如下步骤：
[0044]
(1)通过单2d激光雷达建立高分辨率的占据栅格地图m；
[0045]
首先通过机器人上的2d激光雷达a或2d激光雷达b建立一个高分辨率的占据栅格地图m，占据栅格地图采用一般的slam方法进行构建。
[0046]
(2)通过机器人尺寸确定最大位姿窗口范围；
[0047]
已知机器人尺寸为(length,width)，length、width分别表示机器人的长度、宽度，计算最大对角距离
[0048]
定义最大姿态窗口范围为：两倍机器人尺寸及360度的姿态，表示具体如下：
[0049]
x∈[-dis_max,+dis_max]；
[0050]
y∈[-dis_max,+dis_max]；
[0051]
theta∈[-π,+π]；
[0052]
(3)计算最大位置步进值和最大角度步进值；
[0053]
2d激光雷达的最大测距值为range_max；通过双三次插值可知最大位置步进值step_lin＝3res，其中，res为占据栅格地图m的分辨率，最大角度步进值为最远测距点旋转3个栅格时的角度step_ang，其计算公式具体如下：
[0054]
step_ang＝arc(1-3*res/(2*range_max2))
[0055]
(4)获取最大位置步进值和最大角度步进值下的候选位姿pose_m；
[0056]
通过最大位置步进值3res和最大角度步进值step_ang形成一系列的候选位姿pose_m；
[0057]
pose_m.x∈[-dis_max,-dis_max+setp_lin,-dis_max+2step_lin,....,dis_max]；
[0058]
pose_m.y∈[-dis_max,-dis_max+setp_lin,-dis_max+2step_lin,....,dis_max]；
[0059]
pose_m.theta∈[-π,-π+step_ang,-dis_max+2step_ang,....,π]；
[0060]
(5)同时获取2d激光雷达a的激光帧scan_a和2d激光雷达b的激光帧scan_b，确定激光帧scan_a、scan_b与占据栅格地图m匹配得分最高的候选位姿，即为局部最优位姿；
[0061]
针对激光帧scan_a及激光帧scan_b做相同的处理，本发明以激光帧scan_a为例进行详细的说明，说明具体如下：
[0062]
通过候选位姿pose_m将激光帧scan_a中的点云投影至占据栅格地图m，计算激光
帧scan_a和占据栅格地图m匹配得分，将最高得分对应为候选位姿作为最大步进下2d激光雷达a的局部最优位姿；
[0063]
将候选位姿以最大位置步进值和最大角度步进值搜索，基于候选位姿将点云投影到占据栅格地图m(也成概率地图)中进行打分，其计算公式具体如下：
[0064]
scorei＝∑(m(si(pose_m)))/n
[0065]
其中，si(pose_m)表示通过候选位姿pose_m将激光点云中的第i个激光点投影到栅格地图后获取的二维栅格坐标(r,c)，m(r,c)表示二维栅格坐标(r,c)在栅格地图中的占据概率。通过上述方法也可以获取最大步进值下的2d激光雷达b的局部最优位姿pose
bt1
。
[0066]
(6)以局部最优位姿为中心形成位姿窗口，其范围值为当前范围值的一半，位置步进值和角度步进值减半，执行步骤(4)和步骤(5)，得到2d激光雷达a、2d激光雷达b在当前步进值下的局部最优位姿。
[0067]
(7)循环执行步骤(6)，直到当前最大得分不大于上次最大得分，此时获取2d激光雷达a在全局最优位姿posea和2d激光雷达b在全局地图坐标系中的最优位姿poseb。
[0068]
(8)计算2d激光雷达b在2d激光雷达a下的相对位姿pose_relative，或者是2d激光雷达a在2d激光雷达b下的相对位姿pose_relative，放入初始相对位姿集合pose_relative_raw中；
[0069]
2d激光2d激光雷达a下雷达b在2d激光雷达a下的相对位姿pose_relative即2d激光雷达b坐标系下位姿，
[0070]
假设posea＝(px,py,ptheta)，将pose_a转换为位姿矩阵t_pa：
[0071][0072]
同理，将pose_b转换为位姿矩阵t_pb，那么可以得到a雷达坐标系下b的位姿矩阵t ba：
[0073]
t_ba＝t_pa-1
*t_pb；
[0074]
再将相对位姿矩阵转为相对位姿向量：
[0075]
pose_relative＝t2v(t_ba)；
[0076]
其中t2v为3维变换矩阵转3维向量的基本公式。
[0077]
在窗口位姿范围内不断移动机器人，即改变机器人当前位姿，执行步骤(5)到(8)，将计算得到相对位姿pose_relative依次放入初始相对位姿集合pose_relative_raw中；
[0078]
(9)计算初始相对位姿的均值和协方差矩阵；
[0079]
可以计算得到初始相对位姿集合pose_relative_raw中初始相对位姿的均值e_raw和协方差矩阵cov，均值e_raw和协方差矩阵cov的计算公式具体如下：
[0080][0081][0082]
其中，pose_relative_rawi表示初始相对位姿集合中第i个相对位姿pose_relative；
[0083]
(10)计算标准差，剔除偏差大的位姿，计算双雷达的准确相对位姿；
[0084]
初始相对位姿集合pose_relative_raw的标准差std_err和偏差pose_err，其计算公式具体如下：
[0085]
std_err＝sqrt(cov
[0,0]
,cov
[1,1]
,cov
[2,2]
)；
[0086]
pose_err＝pose_relative_raw
i-e_raw。
[0087]
cov
[0,0]
、cov
[1,1]
、cov
[2,2]
分别表示协方差矩阵cov对角线上的三个元素，即第一行第一列cov
[0,0]
，第二行第二列cov
[1,1]
及第三行第三类cov
[2,2]
上的元素值。
[0088]
当pose_err有元素超过std_err的对应元素，删除初始集合中的相应的位姿对，得到新相对位姿集合，其相对位姿数目为k，计算位姿均值e，即为最终标定的双激光雷达相对位姿。
[0089]
e＝∑pose_relative/k。
[0090]
本发明解决了双2d激光雷达需要有重叠区域、特殊墙面或标定初值的标定问题，降低了对环境和雷达安装的依赖，避免陷入局部极值，提高了标定的精度、稳定性和效率。
[0091]
本发明进行了示例性描述，显然本发明具体实现并不受上述方式的限制，只要采用了本发明的方法构思和技术方案进行的各种非实质性的改进，或未经改进将本发明的构思和技术方案直接应用于其它场合的，均在本发明的保护范围之内。