[发明专利]一种基于ROS机器人的相机与多激光雷达融合方法

权利要求书

1.一种基于ROS机器人的相机与多激光雷达融合方法，用于同时搭载相机和多激光雷达的ROS机器人的图像处理，其特征在于，包括如下步骤：

(1)使用多激光雷达采集标定板的三维点云数据，并拟合出标定板所在的平面；

(2)用人工鱼群算法进行多激光雷达之间的配准；

(3)使用相机采集标定板的二维图像，提取二维图像中标定板的角点；

(4)在三维点云数据中提取标定板的角点；

(5)根据已获得角点的标定板二维和三维图像数据，求解相机坐标系与三维场景结构的世界坐标系之间的位姿关系和二维与三维数据之间变换的最优解，完成数据融合；

所述步骤(2)中，定义人工鱼结构体，每个个体鱼维数为6，其中3维表示旋转，3维表示平移，定义人工鱼群优化器类，并初始化该优化器的参数以及算法在空间中的搜索角度；设置鱼群中个体鱼的个数，对所有的个体鱼进行初始化，随机选取一个个体鱼并认为此个体鱼为全局最优个体；以激光雷达对应面的一幅点云当前所处的状态为人工鱼的状态，对每条鱼i执行人工鱼群的觅食行为、随机行为、聚群行为以及追尾行为；得到每条人工鱼当前所处的状态；记录当前鱼群中的最优个体X_best，以及计算得到的点云平面之间的最小距离值；

判断当前最小距离值是否满足阈值或者是否达到最大迭代次数，如果满足则输出全局最优个体，根据最优个体鱼的6维数据值计算两激光雷达之间的刚性变换，将源点云数据变换到目标点云数据，算法结束；否则对每条鱼i执行人工鱼群的觅食行为、随机行为、聚群行为以及追尾行为得到人工鱼当前所处的状态，直至求解出最优值；

步骤(2)中执行觅食行为对人工鱼状态进行寻优，具体步骤为：如当前状态X₁点云中的所有点到对应的目标点云平面之间的距离D_min(X₁)与状态X₂点云中所有点到对应目标点云平面之间的距离D_min(X₂)满足D_min(X₁)D_min(X₂)，将状态X₁更新为X₂；

当状态X₁和X₂的关系不满足D_min(X₁)＞D_min(X₂)时，则在可移动距离范围内重新随机选择一个状态X₂，继续进行上述比较，如果尝试的次数达到预设值后仍然不满足条件，则随机选择一个状态，从状态X₁到状态X₂更新后所处的状态X_next按照下式计算：

其中step为步长表示下一次可以移动距离的最大值，rand()表示在0和1之间产生的随机数；

所述步骤(2)中，当觅食行为中尝试若干次后仍不能找到更小距离值，则进行随机行为；所述随机行为的下一个状态按下式计算：

X_next＝X₁+visual×rand()；

所述步骤(2)中，执行聚群行为控制人工鱼群的运动方向和密度，对于状态为X₂时的人工鱼，设其最大探知范围内的个体鱼的数目为N_n，这N_n个鱼的中心位置为X_c，计算的值；若且0＜δ＜1，表示在X_c处有比较多的食物，即满足觅食行为中的大小关系，并且个体鱼之间的距离适中，说明此时找到一个状态比当前状态更优，让当前状态更新为这个更优的状态；否则执行觅食行为；

所述步骤(2)中，执行追尾行为寻找最优个体鱼，所述最优个体为满足D_min(X₁)＞D_min(X₂)，并且该个体X_next周围的个体鱼数目N_n满足若且0＜δ＜1，当状态为X₁时，在其感知距离内寻找到的某一个个体为X_next，比较两个状态下源点云中的所有点到目标点云平面的距离，如果满足D_min(X₁)＞D_min(X_next)，并且该个体X_next周围的个体鱼数目N_n满足若且0＜δ＜1；表示该个体X_next为当前发现的最优个体，其他个体鱼应更新自己的状态为最优个体鱼的状态；否则执行觅食行为；

所述步骤(4)包括：使用直通滤波和条件滤波去除标定板上无用的点云数据；对标定板的四条边所在的进行拟合，得到四条边所在直线的空间一般式；求取相邻两条直线在三维空间的交点，求出的交点即为所求的激光雷达点云角点。