一种带多激光雷达的滚动机器人位姿计算方法与流程

1.本发明属于机器人技术环境感知领域，具体涉及一种带多激光雷达的滚动机器人位姿计算方法。


背景技术：

2.机器人是一种通过手动或者自动控制，模拟人类完成各种指令的一种智能化装置。机器人可以代替人体进行各种复杂精细的操作，也可以代替人类进入复杂、危险的环境进行探索作业，保证人员安全。现有的机器人有固定设置、在一定区域范围内进行操作的固定式机器人，也有可移动的，通过机械腿、履带、滚轮等结构实现机器人的行走移动。移动式机器人可以代替人类进入一些复杂、危险的场景，如有毒害气体的空间、火场等，采集信号，指导救援。
3.滚动机器人是一种依靠转动体进行转动运动的移动式机器人，用来进行滚转运动的主转动体可以是任何适合滚动的形状，比如球体、椭球体、圆环型、圆柱型、车轮型、鼓型、其它类似形状或它们的组合。
4.上述滚动机器人由于特殊的形态特征，无法像传统机器人一样将激光雷达安装在顶部位置，感知机器人周围360
°
的环境，而只能通过固连或运动平台安装在球侧，带来了雷达视野的遮挡；且上述滚动机器人在运动过程中不稳定，容易在运动过程中发生滑动、侧滑、晃动或类似情况，位姿变换剧烈，导致单个雷达容易丢失用于感知外部环境的有效点云，无法满足持续稳定感知外部环境的要求。
5.移动机器人的位姿计算是移动机器人能够实现自主导航、规划的基础功能，对于移动机器人平台有重要意义。由于其感知距离远、测距准确、受光线变化影响小等优势，多线机械式激光雷达在移动机器人位姿计算领域中非常常用。但是由于激光雷达传感器大量的数据及本身特性，存在计算开销大导致延迟高等问题，且动态物体在位姿计算中一直是一个棘手的问题，影响位姿计算的精度和稳定性。


技术实现要素：

6.本发明提供一种带多激光雷达的滚动机器人位姿计算方法，用以解决滚动机器人结构和运动特性造成激光雷达视野缺失、点云质量差的缺陷，实现环境感知的持续稳定；解决激光雷达数据量、本身特性带来的开销大、延迟高，和动态物体影响位姿计算效果的缺陷；降低点云处理的计算开销，提高位姿计算的精度和稳定性。
7.本发明提供一种带多激光雷达的滚动机器人位姿计算方法，包括以下步骤：
8.步骤1：完成雷达与雷达间、雷达与主机间及雷达与imu间的时钟同步；
9.步骤2：根据各台雷达所获得的点云数据的时间戳信息和imu数据的时间戳信息，建立点云数据和imu数据的对应关系；
10.步骤3：对获取到的各雷达点云进行投影，即：将三维点云投影到固定尺寸的二维平面上，并分别构建图像序列；即每个雷达获得的数据建立一个图像序列；
11.步骤4：将步骤3所述各图像序列中的每张图像平均分成k个区域，每个区域随机取1个像素点作为聚类起点，采用搜索算法进行簇类生长，并使用哈希表构建簇类与像素映射关系；
12.步骤5：进行离群簇类过滤，更新簇类映射哈希表；
13.步骤6：对步骤3所述各序列中最新相邻两帧图像，按照逐像素方式逐通道进行差分，得到差分图像，并添加到相应的差分图像序列中；
14.步骤7：根据imu数据，求解差分图像所对应两帧间位姿变换；
15.步骤8：将各差分图像和对应的位姿变换分别输入到神经网络中进行推理，网络输出为softmax层，根据网络输出获取相应像素点的类别，像素点的类别包括地面点、动态点和可用静态点，删除所有簇类中的所有地面点；
16.步骤9：根据每个簇类中像素的类别，通过判别准则判断簇类的类别，若簇类为动态点簇或模糊点簇则删除该簇类；
17.步骤10：将删除动态簇类后的各簇类进行合并，统计每个雷达最终所有簇类可用像素点总数量，根据可用像素点总数量计算各激光雷达权重；即按照每个雷达可用的点数，占所有雷达、所有可用点数的比例计算权重；
18.步骤11：使用各激光雷达数据与对应imu数据，在簇类约束下进行位姿计算，根据上述权重进行融合，得到最终位姿。
19.作为优选，步骤2包含以下步骤：
20.步骤2.1：完成同步后，将后续获取到的imu信息加入到imu数据序列中， imu数据序列包括时间戳；
21.步骤2.2：依次判断雷达获得的点云数据的时间戳是否大于imu最新数据的时间戳或小于imu初始数据的时间戳，若是，则该点云数据为无法匹配数据，进行滤除；
22.步骤2.3：对过滤后的点云数据，使用线性插值或者球面插值方法，找到对应的imu数据。
23.作为优选，步骤3所述投影，方法是前视图投影，每个像素点中含有三个通道，分别是反射强度、深度和高度信息。
24.作为优选，步骤4包含以下步骤：
25.步骤4.1：使用所述投影图像初始化未聚类像素集合，初始化空的簇类映射哈希表；对所述投影图像，平均分成k个区域，每个区域随机取1个像素点作为聚类起点；
26.步骤4.2：采用深度通道，对聚类起点并行进行欧式聚类，获取同簇类像素点，将其添加到相应簇类映射哈希表，并在未聚类像素集合中将其删除；
27.步骤4.3：将步骤4.2所得各簇类像素点，分别取距离聚类起点上、下、左和右四个方向最远的像素点作为新的聚类起点，在未聚类像素集合中执行步骤 4.2，直到未聚类像素集合稳定，其中所有像素点归为离群簇类。
28.作为优选，步骤5中的过滤方式为：将包含像素点数少于点数阈值的簇类作为离群簇类删除。点数阈值为预设参数。
29.作为优选，步骤8中的神经网络需先进行训练，再进行推理，训练过程如下：
30.步骤8.1：采集多个不同场景下的滚动机器人运动时激光雷达点云和对应位姿变换数据集合，并对点云中各像素点的类别进行标注，形成训练、测试和验证数据集；此处的
各像素点也是经过与步骤3和步骤6相同的投影、差分处理得到；
31.步骤8.2：相邻两帧的训练数据中的差分图像增加有关imu位姿变换信息的 6个通道，6个通道分别为yaw、pitch、roll、x、y和z，yaw为偏航角， pitch为俯仰角，roll为翻滚角，x、y、z为三维坐标；增加后的差分图像分批次经过若干个降采样单元，每个降采样单元包括依次连接的卷积层、bn层、relu 激活函数和池化层，不同降采样单元中的卷积层参数不同；降采样单元一般为 4-9个；降采样单元中，卷积层输出的数据先经过bn层使数据分布归一化；bn 层输出的数据经过relu激活函数后进入池化层以剔除较差的特征，然后进入下一个降采样单元；
32.步骤8.3：经过多次降采样的特征首先进行升采样后再次按照步骤8.2处理，重复多次直到宽度和高度恢复至输入尺寸；升采样具体为把特征入后续串联的、输出数据维度递增的网络层实现；
33.步骤8.4：将图像特征输入到softmax层，输出逐像素分割结果；
34.步骤8.5：计算损失函数，通过反向传播更新网络参数；损失函数值连续若干轮没有降低的时候结束训练；
35.推理及测试时，相邻两帧的数据直接输入训练后的神经网络，按照步骤 8.2-8.4处理，最后即可得到逐像素分割结果。
36.作为优选，步骤9所述判别准则为：统计各簇类中动态点的数量n1及可用静态点的数量n2，计算相对比例p＝n1/n2，若p大于2，则判断该簇类为动态点簇；若p小于0.5，则判断该簇类为可用静态点簇；若p在0.5～2之间，则判断该簇类为模糊点簇；删除所有动态点簇和模糊点簇。
37.作为优选，步骤10所述的将删除动态簇类后的各簇类进行合并，包含以下步骤：
38.步骤10.1.1：基于步骤9所得可用点簇，及步骤6所得簇类映射哈希表，求取各簇类重心坐标；
39.步骤10.1.2：计算各簇类间交并比；
40.步骤10.1.3：若簇类间交并比大于交并比阈值，或重心间距离小于重心间距离阈值，则看作同一簇类合并；
41.步骤10.1.4：将步骤10.1.3所得合并后簇类，重新进行步骤10.1.1到步骤10.1.3，直到无新的簇类合并产生，完成簇类合并。
42.作为优选，步骤11所述的位姿计算，是指使用点云配准算法(例如icp算法)，对相邻两帧点云数据进行特征点匹配，并得到位姿矩阵；所述簇类约束，是指相邻两帧点云数据的特征点匹配仅在步骤10所得同一簇类中进行；所述雷达融合算法为按照步骤10所得各权重进行加权平均。
43.作为优选，所述的带多激光雷达的滚动机器人壳体表面设有若干组激光雷达，每个激光雷达组包括两个激光雷达，同摄像头组的两个激光雷达存在共视区域，机器人上搭载至少一个获取角度姿势信息的惯性测量装置；所述激光雷达在壳体外设置，激光雷达的垂直视野必须满足：机器人在不同姿态时，地面和正前方点云至少分别出现在一个雷达的视野中。
44.本方案带来的有益效果是：避免了滚动机器人结构和运动特性造成激光雷达视野缺失、点云质量差的缺陷，实现环境感知的持续稳定；解决激光雷达数据量、本身特性带来
的开销大、延迟高，和动态物体影响位姿计算效果的缺陷；降低点云处理的计算开销，提高位姿计算的精度和稳定性。
附图说明
45.图1是本发明的一种流程示意图；
46.图2是本发明的滚动机器人平台整体结构示意图；
47.图中：1、激光雷达；2、可滚动的球形壳体；3、摆动块。
具体实施方式
48.为使本发明的目的、技术方案和优点更加清楚，下面将结合本发明中的附图，对本发明中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
49.以下结合具体实施例对本发明的具体实现进行详细描述。
50.本发明提供一种带多激光雷达的滚动机器人位姿计算方法。图1是本发明的一种带多激光雷达的滚动机器人位姿计算方法的流程示意图，如图1所示，该方法包括：
51.(1)硬件同步及数据预处理，将点云数据投影为二维图像。
52.(1.1)完成所述雷达间、雷达与主机及imu间的时钟同步，并且根据各台激光雷达时间戳信息和imu数据时间戳信息，建立点云数据和imu数据的对应关系；
53.此处通过点云数据时间戳和imu数据时间戳信息建立对应关系，可以采用线性插值的方法。首先判断该帧点云数据的时间戳是否大于imu最新数据的时间戳，或小于imu最初数据的时间戳，若是则该帧点云数据无法匹配imu数据；若不是，则使用距离该帧点云数据时间最近的两帧imu数据(时间戳为t1、t2)，插值计算对应imu数据的姿态四元数得到，其中q1、q2分别为前后两帧imu数据姿态部分的四元数；计算对应imu数据的位移对应imu数据的位移其中x1、v1、a1分别为第一帧imu数据的位移值、速度值和去掉bias和重力加速度影响后的加速度值，假定两帧间机器人以匀加速形式运动。本发明实施例对建立点云数据和imu数据对应关系的方法不作具体限定，具体可根据实际情况设定。
54.(1.2)分别对同步后得到的各雷达点云进行投影，具体来说，给定一个所述雷达的点云ci，对于其中每一个点p
ij
，根据公式
[0055][0056]
分别得到p
ij
在三维空间中的水平方位角和俯仰方位角。本发明实施例对图像二维平面上的坐标范围不作具体限定，具体可根据雷达型号及系统性能设定。考虑图像二维平面上的x坐标范围为[0,a
x
],y坐标范围为[0,ay],a
x
、ay为正整数。按照公式：p
ij
在图像上的的离散化坐标为
[0057][0058]
p
ij
所在像素的三个通道分别为p
ij
到原点的距离
[0059][0060]
p
ij
的反射强度信息i
ij
，p
ij
的高度z
ij
,按照[0,255]的范围对上述三个通道进行离散化，其中，距离超过d
max
时，r
ij
通道设为255，高度超过z
max
时，z
ij
通道设为255，其余数据均分布在[0,254]。本发明实施例对d
max
和z
max
的取值不作具体限定，具体可根据雷达型号及实际应用场景设定。将所得rgb图像添加到相应的图像序列中。
[0061]
(2)构建图像簇类，获取差分图像。
[0062]
(2.1)使用所述投影图像初始化未聚类像素集合，初始化空的簇类映射哈希表，哈希表的key值为簇类id，value为图像中的像素点，通过簇类的id可以查到该簇类包含的所有像素点。对所述投影图像，平均分成k个区域，每个区域随机取1个像素点作为聚类起点。本发明实施例对k的取值不作具体限定，具体可根据实际情况设定；
[0063]
(2.2)采用步骤(1.2)所述三通道，对步骤(2.1)所述聚类起点并行进行聚类，聚类条件为：计算步骤(2.1)所述同区域像素点p
ij
到聚类起点 pn(n＝1,...,k)的距离及p
ij
到聚类起点pn反射强度之差δi
ij
＝abs(i
ij-in)，当同时满足d
ij
《d0,αi
ij
《i0时，判断为同一簇类。本发明实施例对d0和i0的取值不作具体限定，具体可根据实际情况设定。获取同簇类像素点，将其添加到相应簇类映射哈希表，并在未聚类像素集合中将其删除；
[0064]
(2.3)将步骤(2.2)所得各簇类像素点，分别取距离聚类起点上、下、左、右四个方向最远的像素点作为新的聚类起点，在未聚类像素集合中执行步骤 (2.2)，直到未聚类像素集合稳定，其中所有像素点归为离群簇类。
[0065]
(2.4)对步骤(1.2)所述各序列中最新相邻两帧图像，按照逐像素方式逐通道进行差分，得到差分图像，并添加到相应的差分图像序列中；
[0066]
(3)根据imu数据获取两帧间位姿变换，前后两帧相对初始时刻的导航坐标系的位姿变换分别以变换矩阵的形式表示为t1和t2,
[0067][0068][0069]
其中r1、r2分别为前后两帧相对初始时刻的导航坐标系的旋转矩阵；t1、t2分别为前后两帧相对初始时刻的导航坐标系的平移变换；δ、β、γ分别为前后两帧相对初始时刻
的欧拉角(yaw,pitch,roll)，由imu数据中包含的四元数转换得到，具体可用eigen库函数，也可采用其他转换方法。两帧间的位姿变换
[0070]
值得注意的是，在变换矩阵计算的过程中，旋转矩阵r分别由三个方向上的旋转依次相乘得到：r＝rz(α)ry(β)r
x
(γ)，其中rz(α)、ry(β)、rz(γ)分别为绕z轴、y轴、x轴的旋转矩阵：
[0071][0072][0073][0074]
三个矩阵的相乘顺序不定，可根据自己规定的坐标轴方向确定。本方法实施例取xyz的顺序，但对该相乘顺序的规定不作具体限定。
[0075]
(4)将各差分图像和对应的位姿变换输入到神经网络中进行推理，获取像素点的类别。
[0076]
(5)判断各簇类的类别，进行簇类过滤和簇类合并，并统计雷达权重。
[0077]
(5.1)判断各簇类类别，过滤簇类。根据步骤(4)中所得各像素点的类别统计各簇类中动态点的数量n1及可用静态点的数量n2，计算相对比例设置参数a,b，若p》a，则判断该簇类为动态点簇；若p《b，则判断该簇类为可用静态点簇；若b《p《a，则判断该簇类为模糊点簇，即无法准确判断该簇类是否为位姿计算可用静态点簇，为保证后续位姿计算的精确度，将动态点簇和模糊点簇均划为需要过滤的簇类。a和b的取值优选为0.5和2，具体可根据实际情况设定。
[0078]
(5.2)簇类合并。包括以下步骤：
[0079]
(5.2.1)基于(5.1)过滤所得可用点簇，及步骤(2)所得簇类映射哈希表，求取各簇类重心坐标
[0080]
(m＝1,
…
,k；nm为第m个簇类所含像素点的总数)
[0081]
(5.2.2)计算各簇类间交并比
[0082]
(5.2.3)设定参数iou0和d0，若簇类间交并比iou
ab
》iou0，或重心间距离则看作同一簇类合并。本发明实施例对iou0和d0的取值不作具体限定，具体可根据实际情况设定。；
[0083]
(5.2.4)将步骤(5.2.3)所得合并后簇类，重新进行步骤(5.2.1)到步骤(5.2.3)，直到无新的簇类合并产生，完成簇类合并。
[0084]
(5.2.5)对步骤(5.2.4)所得合并后各簇类，计算权重(5.2.5)对步骤(5.2.4)所得合并后各簇类，计算权重ei为第i个簇类所含可用静态像素点个数)。
[0085]
(6)在簇类约束下进行位姿计算，根据权重进行融合，得到最终位姿。
[0086]
此处所述簇类约束，是指在位姿计算步骤中的相邻两帧特征点匹配，仅在步骤(5)所得同一簇类中进行，即单个像素点仅与和它在相同簇类下的其它像素点进行特征匹配，不和非同一簇类下的其余像素点做特征匹配，能大幅降低误匹配的概率，提高精确度，并大幅降低所需计算量，提高程序运行效率。各雷达计算出的位姿矩阵为h_i，根据步骤(5)计算出的各雷达权重，融合各雷达计算结果，权重融合公式：
[0087]
(hi为各雷达计算出的位姿矩阵，ci为各雷达权重)。该方法可以将所有雷达经过滤后得到的可用静态点都纳入计算，保证了位姿计算的精确度，且由于多个雷达共同参与计算，能够在滚动机器人运动过程中不断在各个方向晃动时，保证位姿计算的鲁棒性，具有较强的适应能力。
[0088]
本发明的带多激光雷达的滚动机器人如图2所示，该平台特征在于：(1) 壳体表面设有若干组激光雷达，每个激光雷达组包括两个激光雷达，同摄像头组的两个激光雷达存在共视区域，机器人上搭载至少一个获取角度姿势信息的惯性测量装置。(2)所述激光雷达在壳体外设置，激光雷达的垂直视野必须满足：机器人在不同姿态时，地面和正前方点云至少分别出现在一个雷达的视野中。
[0089]
最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。