[发明专利]一种基于自适应位姿估计的移动机器人跟随控制方法

摘要

一种基于自适应位姿估计的移动机器人跟随控制方法，包括以下步骤：1)建立机器人运动学模型；2)追踪特征区域；3)自主在线学习进行目标跟踪；4)提取特征区域，膨胀、腐蚀、滤波优化处理特征区域，提取特征点并自适应匹配特征点；5)匹配的特征点进行位姿估计；6)设计PID视觉伺服跟随控制器。本发明提供了一种可以有效解决特征点无法追踪或特征点缺失复杂背景下的自适应位姿估计的PID移动机器人视觉跟随控制方法。

主权项

1.一种基于自适应位姿估计的移动机器人跟随控制方法，其特征在于，所述方法包括以下步骤：1)建立基于视觉的移动机器人模型，定义x和y为归一化后的相机横纵坐标，z^c为相机在z轴的坐标，相机在相机坐标系下的速度矢量为v_c和ω_c分别为移动机器人的在z轴线速度和x‑z平面角速度，机器人在自身坐标系下的速度矢量为v_r和ω_r分别为移动机器人的参考在z轴线速度和x‑z平面角速度，则基于视觉的移动机器人运动学模型为：2)追踪特征区域提取特征点；追踪特征区域，提取特征区域，在HSV颜色空间模型中将蓝色区域标注为255，其他区域标注为0进行二值化，并且利用膨胀、腐蚀、滤波优化二值化图像，获取标注为255的白色连通区域，计算连通区域的四个重心，即四个特征点；定义四个连通区域的重心为连通区域重心计算如下：其中，f(u,v)为像素点值，Ω为连通区域，利用公式(2)得同理计算其他三个重心点像素坐标转换成图像坐标计算如下：其中，dx是一个像素点占x方向的长度单位，dy是一个像素点占y方向的长度单位，u₀，v₀是图像中心像素坐标和图像原点像素坐标之间相差的横向和纵向像素数，利用公式(3)将像素坐标转化为图像坐标系下的坐标同理计算另外三个点在图像坐标系下的坐标图像坐标转换成相机坐标计算如下：其中，f是焦距，利用公式(4)将图像坐标转换成相机坐标系下的坐标同理计算另外三个点在相机坐标系下的坐标3)位姿估计步骤2)得到特征点在相机坐标系下的坐标而世界坐标系建立在物体坐标系上，第一个特征点是物体坐标系的原点，也即世界坐标系的原点；由此，根据实际测量可得目标板上四个特征点的世界坐标相机坐标系与世界坐标系的转换关系为：其中，是旋转矩阵，是平移矩阵，利用公式(5)将相机坐标系的四个点与世界坐标系上的四个点对应求解出R旋转矩阵以及t平移矩阵；利用旋转矩阵求解旋转角的计算如下：其中，θx是相机坐标系Xc轴相对于世界坐标系Xw轴的旋转角，θy是相机坐标系Yc轴相对于世界坐标系Yw轴的旋转角，θz是相机坐标系Zc轴相对于世界坐标系Zw轴的旋转角，即相机的姿态；利用平移矩阵计算相机的世界坐标：其中，是相机的世界坐标位置，为了验证位姿是否正确，将第五个世界坐标系下的点坐标重投影到像素坐标系中验证位姿是否正确，重投影计算方式如下：其中，是第五个特征点的世界坐标，(u₅,v₅)是重投影后的像素坐标，是第五个特征点转化到相机坐标系下的深度值，是相机内参矩阵；4)设计PID控制器角速度PID控制器的输入信号是像素横坐标值320，输出信号是第五个重投影点的横坐标u5，反馈信号亦是第五个重投影点的横坐标u5，角速度增量式PID算法如下：其中，角速度PID控制器参数中Kωp是比例控制系数，Kωi是积分控制系数，Kωd是微分控制系数，epix[k]是k时刻像素误差信号；线速度PID控制器的输入信号是500mm深度信息值，输出信号是相机到目标板的距离反馈信号亦是相机到目标板的距离线速度增量式PID算法如下：Δv[k]＝Kvp{ed[k]‑ed[k‑1]}+Kvied[k]+Kvd{ed[k]‑2ed[k‑1]+ed[k‑2]}   (10)其中，线速度PID控制器参数中Kvp是比例控制系数，Kvi是积分控制系数，Kvd是微分控制系数，ed[k]是k时刻深度距离误差信号。