[发明专利]一种基于多机器人协作的管道环境SLAM方法

权利要求书

1.一种基于多机器人协作的管道环境SLAM方法，其特征在于：

Step1：在管道环境中，将两个机器人组成前后队列，前面的领航者命名为Leader，后面的跟随者命名为Follower，机器人队列的具体配置如下：

Step1.1：Leader和Follower之间采用广播方式进行通信，实现数据信息的交换；

Step1.2：每个机器人上均搭载高精度的轮式里程计、惯性导航传感器对自身运动进行基本定位；

Step1.3：Leader上搭载超声波传感器对Follower进行观测，获得Leader与Follower之间相对距离的观测值，Leader接收观测值并实时发送给Follower；

Step1.4：在两个机器人上均搭载GPS接收机，利用进出口位置的GPS信号进一步减少累计误差；

Step1.5：Leader上搭载1台16线激光雷达利用自身融合后的位姿进行点云投影，完成管道三维重建；

Step2：机器人队列采用周期蠕动式协作策略沿管道轴向运动，每个蠕动周期分为伸展和收缩两个阶段；

stage1为伸展阶段，此时Leader的角色是活动者，Follower的角色是参照者，Leader向前运动到最大伸展距离D_max后停止，Follower静止不动；

stage2为收缩阶段，此时Leader角色转换为参照者，Follower角色转换为活动者，Leader静止不动，Follower向Leader方向运动至最小收缩距离D_min；

Step3：采用因子图优化对Leader和Follower机器人上多传感器信息进行融合，获得机器人位姿并完成管道三维重构，整体的因子图由两幅因子图ares1和ares2构成，分别代表两台机器人Leader和Follower的因子图，每个蠕动式协作周期对应一个局部因子图，包含了伸展阶段和收缩阶段两部分，在整个蠕动式协作周期中，首先，融合活动者的轮式里程计和IMU数据，获得其估计位姿，然后，计算活动者的估计位姿和参照者位姿之间的相对平移量，并与超声波观测值组合构成超声波range因子，最后，将超声波range因子作为多机器人位姿约束因子，添加到活动者的因子图中进行信息融合，对活动者的估计位姿进行矫正，而参照者静止不动，其位姿无需矫正，机器人Leader搭载的多线激光雷达利用融合后的准确位姿信息进行点云投影，生成管道三维地图。

2.根据权利要求1所述的基于多机器人协作的管道环境SLAM方法，其特征在于：所述Step2中，首先，为Leader的超声波传感器安装云台，然后，通过解算Follower在超声波云台坐标系下的偏转角度对云台进行旋转控制，使超声波传感器探测方向始终指向Follower的中心点，确保超声波观测的可靠性，解算的具体步骤如下：

Step2.1.1：设世界坐标系为{W}-O_WX_WY_W，Leader上超声波云台坐标系为{U}-O_uX_UY_U，{U}相对于{W}的变换矩阵为Follower在{W}下的二维平面位置齐次坐标为^WP_F＝[x_wy_w1]^T；

则Follower在{U}下的位置齐次坐标^UP_F＝[x_Uy_U1]^T可由公式(1)求出；

Step2.1.2：设Follower相对于Leader上{U}坐标系的位置关系为：

Step2.1.3：定义顺时针旋转为正，逆时针为负，则超声波探测方向矫正时，云台需要旋转的角度θ可以由公式(3)确定：