[发明专利]用于大尺度多站点场景的移动机器人位姿测量方法及系统

权利要求书

1.一种用于大尺度多站点场景的移动机器人位姿测量方法，其特征在于，包括如下步骤：

S1以各站点处本身的特征物体作为站点标志；将两个激光雷达分别安装在移动机器人车体对角方向，且均位于移动机器人车体高度的中间位置；将四个CCD相机分别安装在移动机器人车体侧面前、后、左、右四个方向，且安装高度与站点标志高度相同；

移动机器人在站点示教，通过激光雷达获取移动机器人在该站点示教位置的绝对位姿，即目标位姿；包括如下步骤：

S11获取地图点云作为站点处的源点集Q；通过移动机器人上的激光雷达采集周围环境的点云数据，然后将其与移动机器人的初始位姿P₀结合，得到移动机器人在站点处的目标点集P_n，n＝1；

S12计算源点集Q到目标点集P_n之间，使误差函数达到最小值的变换矩阵T_n；

S13通过变换矩阵T_n校正当前的目标点集P_n，得到新的目标点集P_n+1与当前变换矩阵T_n，并执行P_n＝P_n+1，n＝n+1；

S14判断是否满足收敛条件，否则返回步骤S12，是则计算完成当前迭代的总变换矩阵T_t＝T₁·T₂·T₃…T_n；

S15重复S11～S14若干次，得到多个总变换矩阵，将其分别与初始位姿P₀结合，得到多个绝对位姿，对该多个绝对位姿求平均值得到移动机器人在站点示教位置的目标位姿；

S2利用视觉测量方法获得移动机器人在该站点目标位姿时与该站点标志的相对位姿；包括如下步骤：

S21移动机器人在站点示教位置时，通过移动机器人上的CCD相机拍摄站点标志并提取站点标志上的特征点，且特征点数不小于4个；

S22根据张正友标定法建立特征点在相机坐标系的坐标位置与其像素位置的线性关系，从而获得CCD相机与站点标志特征点的相对位置；

S23根据CCD相机与站点标志特征点的相对位置，以及移动机器人坐标系与相机坐标系的对应关系，得到移动机器人与站点标志特征点的相对位置，即移动机器人与站点标志的相对位姿；

S3重复步骤S1、S2，直至获得移动机器人在所有站点的目标位姿，以及在目标位姿与相应站点标志的相对位姿；

S4移动机器人在多站点场景中移动，当移动机器人停靠某一站点或在站点附近，且移动机器人上的CCD相机能够清晰观测到该站点的站点标志时，利用视觉测量方法获得移动机器人在当前绝对位姿与观测站点标志的相对位姿；

S5根据移动机器人在该观测站点目标位姿与站点标志的相对位姿，以及移动机器人在当前绝对位姿与观测站点标志的相对位姿，计算出移动机器人当前绝对位姿与该观测站点目标位姿的相对位姿，进而结合该观测站点的目标位姿得到移动机器人当前绝对位姿，完成移动机器人位姿测量。

2.如权利要求1所述的用于大尺度多站点场景的移动机器人位姿测量方法，其特征在于，所述步骤S11中，将由移动机器人建图功能得到的地图进行栅格化处理得到栅格化地图，将该栅格化地图转化为地图点云，并对该地图点云进行去噪，以去噪后的地图点云作为站点处的源点集Q。

3.如权利要求2所述的用于大尺度多站点场景的移动机器人位姿测量方法，其特征在于，对栅格化地图中存在障碍物的部分进行中心点选取，从而将栅格化地图转化为地图点云。

4.如权利要求2所述的用于大尺度多站点场景的移动机器人位姿测量方法，其特征在于，采用具有噪声的基于密度的聚类方法对地图点云进行去噪。

5.如权利要求1所述的用于大尺度多站点场景的移动机器人位姿测量方法，其特征在于，通过蒙特卡洛算法估算出移动机器人的初始绝对位姿，以该初始绝对位姿作为移动机器人的初始位姿P₀。