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

权利要求书

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是相机坐标系X^c轴相对于世界坐标系X^w轴的旋转角，θ_y是相机坐标系Y^c轴相对于世界坐标系Y^w轴的旋转角，θ_z是相机坐标系Z^c轴相对于世界坐标系Z^w轴的旋转角，即相机的姿态；

利用平移矩阵计算相机的世界坐标：

其中，是相机的世界坐标位置，为了验证位姿是否正确，将第五个世界坐标系下的点坐标重投影到像素坐标系中验证位姿是否正确，重投影计算方式如下：

其中，是第五个特征点的世界坐标，(u₅,v₅)是重投影后的像素坐标，是第五个特征点转化到相机坐标系下的深度值，是相机内参矩阵；

4)设计PID控制器

角速度PID控制器的输入信号是像素横坐标值320，输出信号是第五个重投影点的横坐标u₅，反馈信号亦是第五个重投影点的横坐标u₅，角速度增量式PID算法如下：

其中，角速度PID控制器参数中K_ωp是比例控制系数，K_ωi是积分控制系数，K_ωd是微分控制系数，e_pix[k]是k时刻像素误差信号；

线速度PID控制器的输入信号是500mm深度信息值，输出信号是相机到目标板的距离反馈信号亦是相机到目标板的距离线速度增量式PID算法如下：

Δv[k]＝K_vp{e_d[k]-e_d[k-1]}+K_vie_d[k]+K_vd{e_d[k]-2e_d[k-1]+e_d[k-2]} (10)

其中，线速度PID控制器参数中K_vp是比例控制系数，K_vi是积分控制系数，K_vd是微分控制系数，e_d[k]是k时刻深度距离误差信号。

2.如权利要求1所述的一种基于自适应位姿估计的移动机器人跟随控制方法，其特征在于：所述步骤2)中，追踪特征区域的步骤如下：

2.1：初始化：初始化相机并启动，手动或自动选定像素点数大于10的追踪区域，设定追踪算法的基本参数；

2.2：迭代开始：复杂背景下取第h帧时的目标区域，并均匀产生一些点，采用Lucas-Kanade追踪器追踪这些点到h+1帧，并反追踪回去得到第h帧这些点的预测位置，则偏差公式计算如下：

其中，ΔX_h为欧氏距离，X_h为初试位置，为反追踪预测位置，ΔX_h作为筛选追踪点的条件之一，ΔX_h＜10留下，否则删除；

2.3：归一化交叉相关：结合归一化交叉相关法描述两个目标的相关程度并删选相关程度不高的点，算法如下：

其中，f(u,v)为像素值，为像素均值，g(x,y)为模板像素值，为模板像素均值，n为追踪点数，NCC为相关性，NCC越大相关程度越高，NCC＞0留下点，否则删除点，删选后剩下的追踪点求平移尺度中值以及缩放尺度中值以获取新的特征区域；

2.4：产生正负样本：为了提高识别精度，在线学习，使用最近邻分类器产生正负样本：

正最近邻相似度：

负最近邻相似度：

相对相似度：

其中，S(p_i,p_j)为(p_i,p_j)图像元的相似度，N是规范化的相关系数，M是目标区域，相对相似度S^r越大，则表示相似度越高，设定相对相似度大于0.2为正样本，小于0.2为负样本；

2.5：迭代更新：令h＝h+1，跳转至2.2。