CN109102525A - 一种基于自适应位姿估计的移动机器人跟随控制方法 - Google Patents

Abstract

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

Description

一种基于自适应位姿估计的移动机器人跟随控制方法

技术领域

本发明涉及基于视觉的移动机器人目标追踪跟随控制系统，尤其涉及的是存在输入限制下自适应位姿估计的移动机器人跟随控制方法。

背景技术

随着科学技术和控制技术的发展，计算机视觉在各个领域已有广泛的运用，视觉数据信息量丰富，处理手段丰富等特点使得基于视觉的移动机器人控制被广泛应用于科研、军事、工业以及物流等领域。机器人的位姿作为机器人运动控制中的基本问题之一，一直备受广泛关注，针对基于视觉的移动机器人目标跟随伺服控制技术的研究，不仅可以丰富移动机器人运动控制的理论成果，还可以满足多领域对运动控制技术越来越高的要求，具有重大的理论和工程意义。此外，通过引入视觉信息，延展了移动机器人的能力范围，可以有效满足人机交互的需求。

然而在实际环境中，特别是在复杂背景下，视觉信息不可避免地存在光线因素以及运动过程中抖动等各种干扰问题，给基于视觉的移动机器人路径跟踪控制带来了新的挑战。

基于自适应位姿估计的移动机器人跟随控制方法是将位姿估计系统和PID参数驱动控制系统组合起来并且设计控制器使得整个系统快速渐近稳定的控制策略。相较于其他控制方法，在线学习目标追踪自适应位姿估计方法使得机器人在复杂背景下移动时也能稳定跟踪特征点，可以处理无法追踪特征点以及特征点缺失等不确定性问题，近年来在移动机器人视觉伺服控制领域受到了普片关注。朱建章等在论文(复杂场景下实时视觉目标跟踪的若干研究)中采用协同训练框架下变权重实时压缩的目标跟踪，倪洪印等在论文(基于视频的人体检测与目标跟踪方法研究)中采用自主选择学习的单目标长期跟踪，王佳丽等在论文(基于改进在线多示例学习算法的机器人目标跟踪)中采用立体视觉在线多示例学习的机器人目标跟踪。然而，这些结果都没有利用单目视觉在线自主学习目标追踪特征点和位姿估计来设计移动机器人的PID伺服跟随控制器。并且，在实际应用中，无论是陀螺仪还是立体视觉相机，对位姿的获取存在一定的实用局限性，因此，针对单目视觉目标跟踪自适应实时位姿估计的研究是很有必要的。

发明内容

为了克服现有技术无法解决移动机器人单目相机位姿估计视觉伺服控制系统的不足，本发明提供一种基于自适应位姿估计的移动机器人跟随控制方法，通过机器人运动学建模以及像素转化计算，并在复杂背景下自主学习追踪目标物提取特征点，进行自适应匹配估计位姿，基于位姿估计结果提供了一种增量式PID移动机器人控制器的设计。

本发明解决其技术问题所采用的技术方案是：

一种基于自适应位姿估计的移动机器人跟随控制方法，所述方法包括以下步骤：

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)中，追踪特征区域的步骤如下：

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。

本发明的技术构思为：首先，建立移动机器人运动学模型以及像素的转化计算。然后，基于模型给出目标追踪自适应位姿估计的移动机器人跟随控制问题。利用追踪的目标自适应分配特征点，并采取solvePNP进行位姿估计。最后，采用增量式PID控制算法，结合位姿反馈信息以及重投影信息设计PID控制器实现实时视觉伺服机器人跟随控制。

本发明的有益效果主要表现在：通过在线学习自主追踪方式追踪目标，复杂背景下易于追踪目标物；目标物追踪不丢失，复杂背景下可以准确获取特征点，有效解决特征点无法追踪或追踪缺失的问题；目标物区域提取分割自适应匹配四个特征点，实时在线进行位姿估计，得到位姿信息，给移动机器人提供有效距离以及角度信息；给出增量式PID控制器的具体参数，有效解决机器人无法快速渐近稳定跟随的问题。

附图说明

图1是移动机器人相机模型坐标系建立示意图。

图2是一种基于自适应位姿估计的移动机器人跟随控制方法的程序框图。

具体实施方式

下面结合附图对本发明作进一步描述。

参照图1和图2，一种基于自适应位姿估计的移动机器人跟随控制方法，包括以下步骤：

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)中，追踪特征区域的步骤如下：

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。

Claims (2)

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所述的一种基于自适应位姿估计的PID移动机器人跟随控制方法，其特征在于：所述步骤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。