一种基于多机器人协作的管道环境SLAM方法

一种基于多机器人协作的管道环境slam方法
技术领域
1.本发明涉及一种基于多机器人协作的管道环境slam方法，属于机器人同时定位与构图(slam)技术领域。


背景技术：

2.随着slam技术的快速发展，目前主流的slam技术多采用以激光和视觉为主的融合策略，但在地下管道、隧道这类特殊场景中，存在几何结构匮乏、周围环境无明显特征点、激光雷达退化、纵向失去约束且管道内部缺失gps信号等问题，导致激光或视觉slam技术难以应用。例如机器人在较长的直线管道中导航时，由于管道内部结构相似度很高，激光雷达测量的结果在管道各处都一致且在管道方向没有约束，导致机器人无法定位自身位置。同样，在这种高相似度的场景内，视觉传感器也很难获取到有用的特征点或参考点，因此依赖于视觉传感器也会导致定位失败的情况发生。对于这类由于环境高相似度引起的退化问题，现有研究提出了两种解决策略。一种是通过判断激光退化的方向和程度，在融合的过程中对退化信息进行过滤或补偿，从而缓解该因素带来的定位误差问题。由于真实环境下，难以对退化信息进行精准的过滤或补偿，这些方法的应用效果难以满足实际需求。另一种则是以高精度的差速里程计和惯性导航传感器imu融合为主，搭配其他传感器辅助来解决问题。为保证定位结果摆脱对周围环境的敏感性，通常使用光电式转速传感器里程轮或惯性测量单元(imu)航位推算进行定位。但在管道环境中，受输送介质及管道壁影响，容易发生轮胎打滑的问题，造成系统误差剧增。而且，对于单机器人来说，由于缺乏参照，随着系统运行时间的加长，累计误差会逐渐增大，使机器人的估计位姿超出管道约束空间，整体的鲁棒性和可靠性较低。可见，针对管道环境下的slam问题，目前还缺乏非常有效的解决方案。


技术实现要素：

3.本发明要解决的技术问题是提供一种基于多机器人协作的管道环境slam方法，能在管道、隧道等具有环境高相似度特征的空间中有效地完成slam任务，克服了由于环境高相似度导致现有slam方法在管道环境中应用效果差的缺点。
4.本发明的技术方案是：一种基于多机器人协作的管道环境slam方法，具体步骤为：
5.step1：在管道环境中，将两个机器人组成前后队列，前面的领航者命名为leader，后面的跟随者命名为follower，机器人队列的具体配置如下：
6.step1.1：leader和follower之间采用广播方式进行通信，实现数据信息的交换；
7.step1.2：每个机器人上均搭载高精度的轮式里程计、惯性导航传感器对自身运动进行基本定位；
8.step1.3：leader上搭载超声波传感器对follower进行观测，获得leader与follower之间相对距离的观测值，leader接收观测值并实时发送给follower；
9.step1.4：考虑到管道的进出口位置可能接收到gps信号，在两个机器人上均搭载gps接收机，利用进出口位置的gps信号进一步减少累计误差；
10.step1.5：为了实现对管道环境的三维重建，leader上搭载1台16线激光雷达利用自身融合后的位姿进行点云投影，完成管道三维重建；
11.step2：机器人队列采用周期蠕动式协作策略沿管道轴向运动，每个蠕动周期分为伸展和收缩两个阶段；
12.stage1为伸展阶段，此时leader的角色是活动者，follower的角色是参照者，leader向前运动到最大伸展距离d
max
后停止，follower静止不动；
13.stage2为收缩阶段，此时leader角色转换为参照者，follower角色转换为活动者，leader静止不动，follower向leader方向运动至最小收缩距离d
min
；
14.为了确保运动过程中两个机器人之间通讯的可靠性，减少意外干扰对队形的影响，需要根据实际硬件参数对最大伸展距离d
max
进行调整和限制，特别是当处于管道的转弯处，d
max
需适当减少。
15.step3：采用因子图优化对leader和follower机器人上多传感器信息进行融合，获得机器人位姿并完成管道三维重构，整体的因子图由两幅因子图ares1和ares2构成，分别代表两台机器人leader(蓝色)和follower(绿色)的因子图，每个蠕动式协作周期对应一个局部因子图，包含了伸展阶段和收缩阶段两部分，在整个蠕动式协作周期中，首先，融合活动者的轮式里程计和imu数据，获得其估计位姿，然后，计算活动者的估计位姿和参照者位姿之间的相对平移量，并与超声波观测值组合构成超声波range因子，最后，将超声波range因子作为多机器人位姿约束因子，添加到活动者的因子图中进行信息融合，对活动者的估计位姿进行矫正，而参照者静止不动，其位姿无需矫正，这样，在一个协作周期中，由于leader和follower交替成为活动者和参照者，leader和follower都有机会对自身的位姿进行矫正，获得较高的定位精度，机器人leader搭载的多线激光雷达利用融合后的准确位姿信息进行点云投影，生成管道三维地图。
16.所述step2中，超声波传感器部署在leader后端指向follower，用于对follower进行观测，以获得leader与follower之间的相对距离。但是，在周期蠕动式协作策略实施过程中，机器人队列并非一直沿直线方向行走。当leader改变运动方向时，follower可能超出其测量范围内，导致超声波无测量信号或测量到其他物体，造成超声波观测不可靠，从而增加系统误差，产生定位错误。为了解决这一问题，首先，为leader的超声波传感器安装云台，然后，通过解算follower在超声波云台坐标系下的偏转角度对云台进行旋转控制，使超声波传感器探测方向始终指向follower的中心点，确保超声波观测的可靠性，解算的具体步骤如下：
17.step2.1.1：设世界坐标系为{w}-owx
wyw
，leader上超声波云台坐标系为{u}-oux
uyu
，{u}相对于{w}的变换矩阵为follower在{w}下的二维平面位置齐次坐标为wpf＝[x
wyw 1]
t
；
[0018]
则follower在{u}下的位置齐次坐标upf＝[x
uyu 1]r可由公式(1)求出；
[0019][0020]
step2.1.2：设follower相对于leader上{u}坐标系的位置关系为：
[0021][0022]
step2.1.3：定义顺时针旋转为正，逆时针为负，则超声波探测方向矫正时，云台需
要旋转的角度θ可以由公式(3)确定：
[0023][0024]
所述step2中，在复杂环境下，当leader作为领航者改变方向时，为了避免follower跟丢造成机器人队列破坏，需要解决follower的跟随问题，具体步骤为：
[0025]
step2.2.1：定义以leader中心的圆周区域为leader的跟随域，由前面蠕动式协作策略可知leader的跟随域半径为d
min
，follower的跟随问题本质上就是确定各时刻下follower的运动速度和方向，使得follower快速到达leader的跟随域边缘处。设t时刻，leader和follower在{w}下的二维平面位置坐标分别为和则t时刻follower中心处的运动速度由公式(4)计算得到：
[0026][0027]
式中，k
p
是正的比例系数，表示与leader和follower之间的距离成比例，可根据机器人运动参数通过实验选取，当leader和follower距离较远时，取值较大，随着leader和follower距离减小，取值逐渐变小，follower到达leader的跟随域边缘时，取值为0；能避免机器人之间发生碰撞。
[0028]
step2.2.2：follower朝着leader前进的过程中需不断的调整角速度的大小和方向，以矫正运动方向，使得follower可以不断靠近leader，γ表示follower与leader之间连线与世界坐标系{w}水平坐标的夹角，β表示follower正前方向与世界坐标系{w}水平坐标的夹角，则在t时刻，follower的角速度可以通过公式(5)计算得到：
[0029][0030]
式中，kω是正的比例系数，表示与follower指向leader的偏差角(γ-β)成比例，可根据机器人运动参数通过实验选取。当follower指向leader的偏差角较大时，取值较大，随着follower指向leader的偏差角减小，取值逐渐变小，follower指向leader时，偏差角为0，取值为0；
[0031]
step2.2.3：将和送入轮式机器人运动方程，即可得到各时刻下follower机器人左右轮电机的运动控制参数，控制follower按照预期速度和方向到达目的位置。
[0032]
所述step3中，涉及到的因子主要有轮式里程计因子、imu预积分因子、gps因子、多机器人位姿约束因子(超声波range因子)等4种因子。其中，多机器人位姿约束因子用于约束活动者和参照者之间的位姿，是本发明实现高质量的机器人位姿矫正的一个关键因子。主要思想是通过解算活动者的估计位姿和参照者位姿之间的相对平移量，并与它们之间相对距离观测值组合构成多机器人位姿约束因子。考虑到机器人协作时，由于通讯等因素影响，机器人之间的相对距离受到d
max
限制。对于这种短距离相对位置测量，超声波传感器拥
有体积小、造价低的优势，在满足测量精确度的同时，还具有对环境光线和电磁场不敏感的特点。因此，使用搭载在leader上的超声波传感器来获取活动者和参照者之间的相对距离观测值，构成多机器人位姿约束因子，称为超声波range因子，其方法如下：
[0033]
假定某一时刻t，通过解算得到活动者与参照者之间的相对平移量为d
t
，超声波观测值为r
t
，则t时刻超声波range因子的优化目标函数如公式(6)所示：
[0034]frange
＝argmin|d
t-r
t
|
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(6)
[0035]
在关键帧处将该超声波range因子插入活动者因子图中，通过因子图优化不断以参照者为参考点矫正活动者位姿。
[0036]
所述step3中，因子图优化求解的具体步骤为(以leader为例，follower同理)：
[0037]
step3.1.1：在因子图求解中，对所有因子乘积最大化来求得概率最大的状态，如公式(7)、(8)所示：
[0038][0039][0040]
step3.1.2：通常情况下，传感器的噪声符合高斯分布，因此将所有的因子都表示为指数模型：
[0041][0042]
step3.1.3：假设各类传感器的观测值为z，则各传感器的误差函数可以抽象表示为：
[0043][0044]
step3.1.4：因子图的求解便可以转为非线性最小二乘问题进行求解：
[0045][0046]
所述step3中，因子图工作时，使用odom频率作为工作主频，对每个关键帧进行处理，具体步骤如下：
[0047]
step3.2.1：在伸展阶段，此时leader的角色是活动者，follower的角色是参照者，活动者leader向前运动，位姿改变；参照者follower静止不动，位姿不变；
[0048]
此过程中，follower和leader获取各自的odom当前帧位姿、当前帧时间戳和上一帧时间戳，位姿信息作为先验因子用于构建各自因子图，当前帧时间戳和上一帧时间戳用来对各传感器数据进行时间对齐处理，参照者follower处于静止状态，将其缓冲的自身位姿信息发送给活动者leader，活动者(leader)将当前帧时间戳和上一帧时间戳之间的imu数据预积分作为约束生成imu因子插入因子图，同时以参照者follower发布的位姿信息为参考点解算自身odom位姿和参照者follower位姿之间的相对位移，利用超声波测量值和相对平移量共同构成超声波range因子插入自身因子图中进行优化求解，输出最终位姿信息；
[0049]
step3.2.2：同理，在收缩阶段，此时follower的角色是活动者，leader的角色是参照者，活动者，follower向前运动，位姿改变；参照者leader静止不动，位姿不变；
[0050]
此过程中，follower和leader获取各自的odom当前帧位姿、当前帧时间戳和上一帧时间戳，位姿信息作为先验因子用于构建各自因子图，当前帧时间戳和上一帧时间戳用来对各传感器数据进行时间对齐处理，参照者leader处于静止状态，将其缓冲的自身位姿信息以及超声波测量值发送给活动者follower。活动者follower将当前帧时间戳和上一帧时间戳之间的imu数据预积分作为约束生成imu因子插入因子图，同时以参照者leader发布的位姿信息为参考点，解算自身odom位姿与参照者leader位姿之间的相对位移，利用超声波测量值和相对平移量共同构成超声波range因子插入自身因子图中进行优化求解，输出最终位姿信息；
[0051]
step3.2.3：当机器人在管道的进出口位置接收到gps信号时，将gps信号作为特殊先验因子加入到因子图中进行融合，从而最大程度减少累计误差。
[0052]
本发明的有益效果是：
[0053]
1、本发明将两个机器人在管道环境中组成前后队列，设计一种独特的周期蠕动方式作为两个机器人之间的协作策略，并构建多机器人位姿约束因子(超声波range因子)对机器人相对位姿进行约束，使机器人队列在管道中行进时能相互提供参照，并实现高质量的自身位姿矫正，从而获得较高的定位精度，克服了由于环境高相似度导致现有slam方法在管道环境中应用效果差的缺点。
[0054]
2、本发明适用于轮式机器人，能在管道、隧道等具有环境高相似度特征的空间中有效地完成slam任务。
[0055]
3、由于多机器人协作的优势，本发明不易受环境影响。除了对房间等普通环境具有适用性，在各种具有高相似度特征的管道环境下，本发明均能取得更好的性能和效果。且随着管道规模的上升以及运行时间的增加，本发明仍然能保持较好的slam效果，具有累计误差可控、可靠性高、鲁棒性强等优点。
附图说明
[0056]
图1是本发明的多机器人队列示意图；
[0057]
图2是本发明的机器人队列周期蠕动式协作策略示意图；
[0058]
图3是本发明的整体因子结构示意图；
[0059]
图4是本发明的局部因子图；
[0060]
图5是本发明的超声波云台坐标系{u}相对于机器人follower的位置关系图；
[0061]
图6是本发明的机器人跟随模型示意图；
[0062]
图7是本发明的机器人跟随控制算法模型图；
[0063]
图8是本发明的多传感信息融合算法流程图；
[0064]
图9是本发明的房间模型示意图；
[0065]
图10是本发明的普通环境下本方法slam测试结果；
[0066]
图11是本发明的简易管道模型图；
[0067]
图12是本发明的lio-sam测试结果；
[0068]
图13是本发明的lio-sam报错详情；
[0069]
图14是本发明的fast-lio2.0测试结果；
[0070]
图15是本发明的简易管道下imu融合odom策略slam测试结果；
[0071]
图16是本发明的简易管道下本方法slam测试结果；
[0072]
图17是本发明的复杂城市管道环境下本方法与imu融合odom策略slam测试结果图；
[0073]
图18是本发明的evo评测结果；
[0074]
图19是本发明的slam结果与imu融合odom策略误差分析对比图。
具体实施方式
[0075]
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述。显然，所描述的实施例仅仅是本发明的一部分实施例，而不是全部的实施例。在不冲突的情况下，本技术中的实施例及实施例中的特征可以相互组合。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0076]
需要说明，若本发明实施例中有涉及方向性指示(诸如上、下、左、右、前、后
……
)，则该方向性指示仅用于解释在某一特定姿态(如附图所示)下各部件之间的相对位置关系、运动情况等，如果该特定姿态发生改变时，则该方向性指示也相应地随之改变。
[0077]
另外，若本发明实施例中有涉及“第一”、“第二”等的描述，则该“第一”、“第二”等的描述仅用于描述目的，而不能理解为指示或暗示其相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个该特征。另外，全文中出现的“和/或”的含义，包括三个并列的方案，以“a和/或b”为例，包括a方案、或b方案、或a和b同时满足的方案。另外，“多个”指两个以上。另外，各个实施例之间的技术方案可以相互结合，但是必须是以本领域普通技术人员能够实现为基础，当技术方案的结合出现相互矛盾或无法实现时应当认为这种技术方案的结合不存在。
[0078]
实施例1：一种基于多机器人协作的管道环境slam方法，具体步骤如下：
[0079]
step1：在管道环境中，将两个机器人组成前后队列，前面的领航者命名为leader，后面的跟随者命名为follower，如附图1所示，机器人队列的具体配置如下：
[0080]
step1.1：leader和follower之间采用广播方式进行通信，实现数据信息的交换。
[0081]
step1.2：每个机器人上均搭载高精度的轮式里程计、惯性导航传感器对自身运动进行基本定位。
[0082]
step1.3：leader上搭载超声波传感器对follower进行观测，获得leader与follower之间相对距离的观测值，leader接收观测值并实时发送给follower。
[0083]
step1.4：考虑到管道的进出口位置可能接收到gps信号，在两个机器人上均搭载gps接收机，可以利用进出口位置的gps信号进一步减少累计误差。
[0084]
step1.5：为了实现对管道环境的三维重建，leader上搭载1台16线激光雷达利用自身融合后的位姿进行点云投影，完成管道三维重建。
[0085]
step2：机器人队列采用周期蠕动式协作策略沿管道轴向运动。每个蠕动周期分为伸展和收缩两个阶段。为了便于描述，以前行为例进行说明，如附图2所示。stage1为伸展阶段，此时leader的角色是活动者，follower的角色是参照者，leader向前运动到最大伸展距离d
max
后停止，follower静止不动。stage2为收缩阶段，此时leader角色转换为参照者，follower角色转换为活动者，leader静止不动，follower向leader方向运动至最小收缩距
离d
min
。为了确保运动过程中两个机器人之间通讯的可靠性，减少意外干扰对队形的影响，需要根据实际硬件参数对最大伸展距离d
max
进行调整和限制，特别是当处于管道的转弯处，d
max
需适当减少。
[0086]
step3：采用因子图优化对leader和follower机器人上多传感器信息进行融合，获得机器人位姿并完成管道三维重构。整体的因子图由两幅因子图ares1和ares2构成，分别代表两台机器人leader(蓝色)和follower(绿色)的因子图，如附图3所示。每个蠕动式协作周期对应一个局部因子图，包含了伸展阶段和收缩阶段两部分，如附图4所示。在整个蠕动式协作周期中，首先，融合活动者的轮式里程计和imu数据，获得其估计位姿。然后，计算活动者的估计位姿和参照者位姿之间的相对平移量，并与超声波观测值组合构成超声波range因子。最后，将超声波range因子作为多机器人位姿约束因子，添加到活动者的因子图中进行信息融合，对活动者的估计位姿进行矫正，而参照者静止不动，其位姿无需矫正。这样，在一个协作周期中，由于leader和follower交替成为活动者和参照者，leader和follower都有机会对自身的位姿进行矫正，获得较高的定位精度。机器人leader搭载的多线激光雷达利用融合后的准确位姿信息进行点云投影，生成管道三维地图。
[0087]
所述step2中，超声波传感器部署在leader后端指向follower，用于对follower进行观测，以获得leader与follower之间的相对距离。但是，在周期蠕动式协作策略实施过程中，机器人队列并非一直沿直线方向行走。当leader改变运动方向时，follower可能超出其测量范围内，导致超声波无测量信号或测量到其他物体，造成超声波观测不可靠，从而增加系统误差，产生定位错误。为了解决这一问题，首先，为leader的超声波传感器安装云台。然后，通过解算follower在超声波云台坐标系下的偏转角度对云台进行旋转控制，使超声波传感器探测方向始终指向follower的中心点，确保超声波观测的可靠性。解算的具体步骤如下：
[0088]
step2.1.1：设世界坐标系为{w}-owx
wyw
，leader上超声波云台坐标系为{u}-oux
uyu
，{u}相对于{w}的变换矩阵为follower在{w}下的二维平面位置齐次坐标为wpf＝[x
wyw 1]
t
。
[0089]
则follower在{u}下的位置齐次坐标upf＝[x
uyu 1]
t
可由公式(1)求出。
[0090][0091]
step2.1.2：设follower相对于leader上{u}坐标系的位置关系如附图5所示，图中α可以由公式(2)求出。
[0092][0093]
step2.1.3：定义顺时针旋转为正，逆时针为负。则超声波探测方向矫正时，云台需要旋转的角度θ可以由公式(3)确定。
[0094][0095]
所述step2中，在复杂环境下，当leader作为领航者改变方向时，为了避免follower跟丢造成机器人队列破坏，需要解决follower的跟随问题。具体步骤为：
[0096]
step2.2.1：定义以leader中心的圆周区域为leader的跟随域，由前面蠕动式协作策略可知leader的跟随域半径为d
min
。follower的跟随问题本质上就是确定各时刻下
follower的运动速度和方向，使得follower快速到达leader的跟随域边缘处。设t时刻，leader和follower在{w}下的二维平面位置坐标分别为和则t时刻follower中心处的运动速度由公式(4)计算得到。
[0097][0098]
式中，k
p
是正的比例系数，表示与leader和follower之间的距离成比例，可根据机器人运动参数通过实验选取。当leader和follower距离较远时，取值较大；随着leader和follower距离减小，取值逐渐变小；follower到达leader的跟随域边缘时，取值为0，能避免机器人之间发生碰撞。
[0099]
step2.2.2：follower朝着leader前进的过程中需不断的调整角速度的大小和方向，以矫正运动方向，使得follower可以不断靠近leader，如附图6所示。图中，γ表示follower与leader之间连线与世界坐标系{w}水平坐标的夹角，β表示follower正前方向与世界坐标系{w}水平坐标的夹角。则在t时刻，follower的角速度可以通过公式(5)计算得到。
[0100][0101]
式中，kω是正的比例系数，表示与follower指向leader的偏差角(γ-β)成比例，可根据机器人运动参数通过实验选取。当follower指向leader的偏差角较大时，取值较大；随着follower指向leader的偏差角减小，取值逐渐变小；follower指向leader时，偏差角为0，取值为0。
[0102]
step2.2.3：将和送入轮式机器人运动方程，即可得到各时刻下follower机器人左右轮电机的运动控制参数，控制follower按照预期速度和方向到达目的位置。控制算法模型如附图7所示。
[0103]
所述step3中，涉及到的因子主要有轮式里程计因子、imu预积分因子、gps因子、多机器人位姿约束因子(超声波range因子)等4种因子。具体如下：
[0104]
(1)轮式里程计因子作为两个机器人的odom先验因子进行因子图优化。轮式里程计包含了位姿(位置和姿态)和速度(前进速度和角速度)两方面的信息，由高精度光电编码器获取。
[0105]
(2)引入imu预积分可以避免重复积分问题的发生。imu预积分因子由相邻关键帧之间的imu测量积分得到，作为两个关键帧之间的约束被插入因子图中，与odom先验因子一起进行优化。
[0106]
(3)gps因子在管道的进出口位置经gps接收机获取，作为一种特殊的先验因子被插入在管道进出口位置处的关键帧当中与其他因子进行联合优化。
[0107]
(4)多机器人位姿约束因子用于约束活动者和参照者之间的位姿，是本发明实现
高质量的机器人位姿矫正的一个关键因子。主要思想是通过解算活动者的估计位姿和参照者位姿之间的相对平移量，并与它们之间相对距离观测值组合构成多机器人位姿约束因子。考虑到机器人协作时，由于通讯等因素影响，机器人之间的相对距离受到d
max
限制。对于这种短距离相对位置测量，超声波传感器拥有体积小、造价低的优势，在满足测量精确度的同时，还具有对环境光线和电磁场不敏感的特点。因此，使用搭载在leader上的超声波传感器来获取活动者和参照者之间的相对距离观测值，构成多机器人位姿约束因子，称为超声波range因子。其计算方法如下：
[0108]
假定某一时刻t，通过解算得到活动者与参照者之间的相对平移量为d
t
，超声波观测值为r
t
，则t时刻超声波range因子的优化目标函数如公式(6)所示：
[0109]frange
＝argmin|d
t-r
t
|
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(6)
[0110]
在关键帧处将该超声波range因子插入活动者因子图中，通过因子图优化不断以参照者为参考点矫正活动者位姿。
[0111]
所述step3中，因子图优化求解(以leader为例，follower同理)的具体步骤如下：
[0112]
step3.1.1：在因子图求解中，对所有因子乘积最大化来求得概率最大的状态，如公式(7)(8)：
[0113][0114][0115]
step3.1.2：通常情况下，传感器的噪声符合高斯分布，因此将所有的因子都表示为指数模型：
[0116][0117]
step3.1.3：假设各类传感器的观测值为z，则各传感器的误差函数可以抽象表示为：
[0118][0119]
step3.1.4：因子图的求解便可以转为非线性最小二乘问题进行求解：
[0120][0121]
所述step3中，因子图工作时，使用odom频率作为工作主频。对每个关键帧进行处理的算法流程如附图8所示，具体步骤如下：
[0122]
step3.2.1：在伸展阶段，此时leader的角色是活动者，follower的角色是参照者。活动者(leader)向前运动，位姿改变；参照者(follower)静止不动，位姿不变。此过程中，follower和leader获取各自的odom当前帧位姿、当前帧时间戳和上一帧时间戳。位姿信息作为先验因子用于构建各自因子图，当前帧时间戳和上一帧时间戳用来对各传感器数据进行时间对齐处理。参照者(follower)处于静止状态，将其缓冲的自身位姿信息发送给活动者(leader)。活动者(leader)将当前帧时间戳和上一帧时间戳之间的imu数据预积分作为约束生成imu因子插入因子图，同时以参照者(follower)发布的位姿信息为参考点解算自
身odom位姿和参照者(follower)位姿之间的相对位移，利用超声波测量值和相对平移量共同构成超声波range因子插入自身因子图中进行优化求解，输出最终位姿信息。
[0123]
step3.2.2：同理，在收缩阶段，此时follower的角色是活动者，leader的角色是参照者。活动者(follower)向前运动，位姿改变；参照者(leader)静止不动，位姿不变。此过程中，follower和leader获取各自的odom当前帧位姿、当前帧时间戳和上一帧时间戳。位姿信息作为先验因子用于构建各自因子图，当前帧时间戳和上一帧时间戳用来对各传感器数据进行时间对齐处理。参照者(leader)处于静止状态，将其缓冲的自身位姿信息以及超声波测量值发送给活动者(follower)。活动者(follower)将当前帧时间戳和上一帧时间戳之间的imu数据预积分作为约束生成imu因子插入因子图，同时以参照者(leader)发布的位姿信息为参考点，解算自身odom位姿与参照者(leader)位姿之间的相对位移，利用超声波测量值和相对平移量共同构成超声波range因子插入自身因子图中进行优化求解，输出最终位姿信息。
[0124]
step3.2.3：当机器人在管道的进出口位置接收到gps信号时，将gps信号作为特殊先验因子加入到因子图中进行融合，从而最大程度减少累计误差。
[0125]
为了进一步说明本发明方法的有益效果，通过计算机仿真，对本实施例进行实验分析。实验环境ubuntu20.04和ros(noetic)系统。机器人外形为圆柱体机体，其直径为150mm、高度70mm，机器人轮胎直径为70mm、宽度30mm。leader相较于follower额外发布超声波话题和激光雷达话题用于融合和建图，传感器的数据录制在gazebo仿真条件下进行。实验包括四个部分。第一部分为普通环境下的slam测试，验证本方法对普通环境的适用性。第二部分为普通管道环境下的slam测试对比。第三部分为复杂城市管道环境下的slam测试。验证本方法对规模更大、结构更为复杂的城市管道环境的适用性。第四部分为evo评测分析。利用slam算法评测工具evo，对本方法在复杂城市管道环境下的slam结果进行定量的评测分析。具体如下：
[0126]
(1)普通环境下的slam测试
[0127]
普通环境为一间房间，摆放了多个具有明显几何特征的物品，房间模型如附图9所示。
[0128]
将两个机器人按照前文所述方式组成队列，并按照周期蠕动式协作策略对房间内环境数据进行采集。为了模拟真实情况下的融合效果，分别给leader和follower的传感器添加一定的高斯噪声，并在角落和墙壁处模拟机器人轮子打滑的情况。本方法在普通环境下的多机器人协作slam效果如附图10所示。从附图10中可以看出，本方法可以在普通环境下顺利完成房间内自身的定位及房间环境的三维重建，对普通环境的还原度很高，体现出了房间的几何特征和深度信息。这一结果，验证了本方法可以在普通环境下完成slam任务，具有对普通环境的适用性。
[0129]
(2)普通管道环境下的slam测试对比
[0130]
为了具有代表性，普通管道环境为一根同时具备长直、四通和90度弯头的管道，包含了多种典型的管道结构。其模型由soildworks绘制后将其转为urdf模型后导入到gazebo仿真环境中进行测试，管道模型如附图11所示。
[0131]
首先，对目前主流的lio-sam方法在简易管道内中进行了可行性测试，经更改配置文件以及进行相关标定后，测试结果如附图12所示。在进入管道后lio-sam系统发出警告，
内容为没有足够的特征点，且在运行一段时间后发生系统错误，其具体内容如附图13所示。根据报错信息可以看出，原因为变量约束不足而发生错误，从数学上讲，该系统处于欠定状态。由附图12、附图13可以得出以下结论：lio-sam中的激光匹配对于长走廊或管道这样的激光退化场景不能适用，且由于管道内没有明显几何特征，激光雷达扫描的结果到处一致，导致机器人无法确定自身处于什么位置从而造成slam任务失败。
[0132]
然后，对fast-lio2.0进行简易管道下的可行性测试。fast-lio2.0相对于fast-lio来说更加完整，在其基础之上提出增量式的kd-tree(ikd-tree)，使得kd-tree能够增量式的删除或添加点云。得益于ikd-tree，fast-lio2.0不再是类似于loam般的提取edge特征与plane特征，而是直接将每个三维点与地图配准，因此能够较稳定地运行在一些较难提取手工特征的场景中。其具体测试结果如附图14所示。在fast-lio2.0运行过程中，由附图14(a)中可以看出，相对于lio-sam来讲系统能重构出一部分地图但还原度不高，效果极差，且随着运行时间的增加点云地图开始发生畸变。在运行期间系统全程伴随着附图14(b)所示的无有效点的告警直至系统停止运行。虽然fast-lio2.0可以稳定运行在一些较难提取手工特征的环境中，但对于管道这类高相似度且几乎无几何特征的环境，尤其是在长直管道内，fast-lio2.0一样无法很好的适用，发生因特征点不足的问题而导致slam任务失败。
[0133]
对于单机器人slam，控制其搭载imu、里程计进入管道内部进行环境信息提取，仿真方式同普通环境下一致，添加了高斯噪声且在弯道处模拟车轮打滑的情况，利用传统方法，融合odom和imu策略经因子图优化之后点云投影如附图15所示。
[0134]
本发明仿真策略同普通环境下一致，给传感器添加高斯噪声后，在转弯处模拟轮胎打滑的情况进行仿真。不同于单机器人的是leader和follower在一个协作周期中不断通过对方的位姿来矫正自身的位姿，最终融合后的效果图如附图16所示。得益于两个机器人在协作周期中不断通过对方来矫正自身位姿的优势，本方法在普通管道环境下能稳定运行，slam任务的完成效果好。特别是在弯道等模拟机器人轮子打滑的位置，能对单个机器人轮式里程计打滑产生的误差进行有效的矫正，没有出现地图重影、噪声点多等问题。管道模型三维重建后，管道结构还原度高，管道的方向信息及深度信息清晰可见。测试结果表明：本方法受益于多机器人协作的优势，两个机器人在蠕动式协作周期中能交替地以对方为参照，对自身位姿进行高质量的矫正，明显降低机器人轮式里程计打滑时的误差影响，使得激光雷达可以在正确的位姿下进行点云投影，结果中没有出现地图重影、噪声点多等问题，机器人位姿更为准确。
[0135]
真实的城市模拟管道环境下slam测试
[0136]
实验中所用管道模型来自于某城市燃气管道分布图，通过soildworks对其中的局部区域建模后转为urdf模型导入到gazebo仿真环境中，管道模型如附图17中(a)所示。管道模型参数如下：
[0137]
管道类型：长直管道(2260m)、三通(1个)、四通(2个)、多角度弯头(9个)
[0138]
管道总长：约4000m
[0139]
管道内径：1016mm
[0140]
slam测试结果中，单机器人采用imu融合odom策略的效果如附图17(b)所示，本方法融合后的效果如附图17(c)所示。从附图17(b)的结果可以看出，在单机器人采用imu融合odom策略对真实城市模拟管道的slam结果中，三维地图存在着大量的噪点以及三处地图重
影，导致局部结果严重变形。而附图17(c)中，本方法的slam结果无明显噪点以及地图重影，整体结果优于单机器人slam结果，在鲁棒性和可靠性上均高于imu融合odom策略。测试结果表明：随着管道规模的上升以及运行时间的增加，本方法仍然能保持较好的slam效果，具有累计误差可控、置信度高、鲁棒性强等优点。
[0141]
evo评测分析
[0142]
利用slam评测工具evo对本方法在复杂城市管道环境下slam测试过程中保存的tum格式轨迹数据进行误差分析。对比的轨迹为最终leader的融合输出结果，数据均为tum格式，利用slam评测工具evo分析结果，轨迹直观图和误差信息直观图如附图18所示。
[0143]
经evo评测后可以看出，附图18(a)为轨迹直观对比，真值和融合后的效果基本吻合，并且从(b)中的轨迹误差直观信息和(d)绝对轨迹误差直方图可以看到，本方法在管道内slam的最大误差为0.713m，平均误差为0.047m。经分析，最大误差发生在较大弧度的转角处，车轮可能发生了打滑的情况，但对比图18中的管道三维地图来看，在弯道处并未发生地图畸变或重影。相比管道内径1016mm以及管道总长4000m来说，平均误差处于合理范围之内。结合附图17(c)的管道还原度可以看出，本方法可以出色的完成真实的城市模拟管道内的slam任务，管道还原度较高且误差较小，并能克服转弯处轮胎打滑造成的地图重影问题。
[0144]
对比imu融合odom策略，经evo评测工具统一对比后结果如附图19所示。结果显示，采用imu融合odom策略的最大误差约为1.54m，对比附图17(b)中的slam结果图来看，在弯道处发生了地图重影问题且具有大量噪点，原因是由于在弯道处可能发生了轮胎打滑从而导致这种情况的发生，造成绝对误差最大值是本方法的2.2倍以上，平均值相对本方法为2.5倍左右。
[0145]
以上结合附图对本发明的具体实施方式作了详细说明，但是本发明并不限于上述实施方式，在本领域普通技术人员所具备的知识范围内，还可以在不脱离本发明宗旨的前提下作出各种变化。