1.一种基于ROS机器人的相机与多激光雷达融合方法,用于同时搭载相机和多激光雷达的ROS机器人的图像处理,其特征在于,包括如下步骤:(1)使用多激光雷达采集标定板的三维点云数据,并拟合出标定板所在的平面;
(2)用人工鱼群算法进行多激光雷达之间的配准;
(3)使用相机采集标定板的二维图像,提取二维图像中标定板的角点;
(4)在三维点云数据中提取标定板的角点;
(5)根据已获得角点的标定板二维和三维图像数据,求解相机坐标系与三维场景结构的世界坐标系之间的位姿关系和二维与三维数据之间变换的最优解,完成数据融合;
所述步骤(2)中,定义人工鱼结构体,每个个体鱼维数为6,其中3维表示旋转,3维表示平移,定义人工鱼群优化器类,并初始化该优化器的参数以及算法在空间中的搜索角度;设置鱼群中个体鱼的个数,对所有的个体鱼进行初始化,随机选取一个个体鱼并认为此个体鱼为全局最优个体;以激光雷达对应面的一幅点云当前所处的状态为人工鱼的状态,对每条鱼i执行人工鱼群的觅食行为、随机行为、聚群行为以及追尾行为;得到每条人工鱼当前所处的状态;记录当前鱼群中的最优个体X_best,以及计算得到的点云平面之间的最小距离值;
判断当前最小距离值是否满足阈值或者是否达到最大迭代次数,如果满足则输出全局最优个体,根据最优个体鱼的6维数据值计算两激光雷达之间的刚性变换,将源点云数据变换到目标点云数据,算法结束;否则对每条鱼i执行人工鱼群的觅食行为、随机行为、聚群行为以及追尾行为得到人工鱼当前所处的状态,直至求解出最优值;
步骤(2)中执行觅食行为对人工鱼状态进行寻优,具体步骤为:如当前状态X1点云中的所有点到对应的目标点云平面之间的距离Dmin(X1)与状态X2点云中所有点到对应目标点云平面之间的距离Dmin(X2)满足Dmin(X1)>Dmin(X2),将状态X1更新为X2;
当状态X1和X2的关系不满足Dmin(X1)>Dmin(X2)时,则在可移动距离范围内重新随机选择一个状态X2,继续进行上述比较,如果尝试的次数达到预设值后仍然不满足条件,则随机选择一个状态,从状态X1到状态X2更新后所处的状态Xnext按照下式计算:其中step为步长表示下一次可以移动距离的最大值,rand()表示在0和1之间产生的随机数;
所述步骤(2)中,当觅食行为中尝试若干次后仍不能找到更小距离值,则进行随机行为;所述随机行为的下一个状态按下式计算:Xnext=X1+visual×rand();
所述步骤(2)中,执行聚群行为控制人工鱼群的运动方向和密度,对于状态为X2时的人工鱼,设其最大探知范围内的个体鱼的数目为Nn,这Nn个鱼的中心位置为Xc,计算 的值;若且0<δ<1,表示在Xc处有比较多的食物,即满足觅食行为中的大小关系,并且个体鱼之间的距离适中,说明此时找到一个状态比当前状态更优,让当前状态更新为这个更优的状态;否则执行觅食行为;
所述步骤(2)中,执行追尾行为寻找最优个体鱼,所述最优个体为满足Dmin(X1)>Dmin(X2),并且该个体Xnext周围的个体鱼数目Nn满足若 且0<δ<1,当状态为X1时,在其感知距离内寻找到的某一个个体为Xnext,比较两个状态下源点云中的所有点到目标点云平面的距离,如果满足Dmin(X1)>Dmin(Xnext),并且该个体Xnext周围的个体鱼数目Nn满足若且0<δ<1;表示该个体Xnext为当前发现的最优个体,其他个体鱼应更新自己的状态为最优个体鱼的状态;否则执行觅食行为;
所述步骤(4)包括:使用直通滤波和条件滤波去除标定板上无用的点云数据;对标定板的四条边所在的进行拟合,得到四条边所在直线的空间一般式;求取相邻两条直线在三维空间的交点,求出的交点即为所求的激光雷达点云角点。
2.根据权利要求1所述的基于ROS机器人的相机与多激光雷达融合方法,其特征在于,所述步骤(1)中拟合标定板所在平面的步骤为:从激光雷达所采集到的点云中,随机选取3个不共线的点,由此3个点构造三维平面的一般性方程,并初始化为此幅点云对应的平面;
计算点云中其余所有点到此平面的距离;统计距离值小于所给阈值的点的个数,并计算满足此阈值的点的个数占所有点的比例;如果计算的占比满足一定的阈值,则认为当前所得的平面参数为此幅点云拟合得出的平面,结束迭代;否则从头开始继续拟合平面,直至达到最大迭代次数为止。
3.根据权利要求1所述的基于ROS机器人的相机与多激光雷达融合方法,其特征在于,所述步骤(3)中,对标定板的四个角点分别贴上四种不同颜色的贴纸,使用相机采集图像,对图像中的像素点进行遍历,寻找具有四种颜色的区域,对每个区域的像素点位置取最边缘值,将边缘值作为角点在相机采集的图像中的像素坐标。
4.根据权利要求1所述的基于ROS机器人的相机与多激光雷达融合方法,其特征在于,所述步骤(5)包括:根据已经获得角点坐标的二维图像和三维点云,求解出相机坐标系与三维世界坐标系之间的绝对旋转矩阵R和平移向量T;根据求得的R和T获取相机坐标系与三维世界坐标系之间的位姿关系;利用求解PNP问题的方法求出三维与二维数据之间变换的最优解,完成数据融合。