本发明属于机器人控制领域，更具体地涉及一种基于点特征的视觉控制方法。
背景技术
在机器人控制过程中，对目标的精确识别、定位及检测是控制成功的关键。视觉传感器能够感知丰富的环境信息，且安装使用方便，是进行机器人控制的重要传感器。通过对视觉传感器获取的图像信息进行处理，可以得到目标的轮廓、形状、颜色等信息，还可以实现目标的运动检测、相对定位等。
根据视觉误差信号的选择，可将视觉系统分为基于位置的视觉控制、基于图像的视觉控制和混合视觉控制。其中，基于图像的视觉控制方法对摄像机的标定误差和空间模型误差不敏感，控制器设计的灵活性大，计算过程较简单，在视觉控制领域得到了最为广泛的应用。
在基于图像的视觉伺服控制方法中，点特征是机器人视觉跟踪、视觉控制中的常用特征。然而，单目视觉场景中，由于难以获取目标的深度信息，点特征的使用受到制约。基于此，liu等建立深度无关的雅克比矩阵，采用eye-in-hand形式的视觉系统，利用点特征在机器人的关节空间进行视觉伺服控制(liuyh,wangh,chenw,etal.adaptivevisualservoingusingcommonimagefeatureswithunknowngeometricparameters[j],automatica,2013,49(8):2453-2460.)。该方法摆脱了深度信息对视觉控制的约束，但在雅克比矩阵估计过程中，需要将深度信息相关的项单独提取出来作为一个比例项，计算过程繁琐。kawamura等将图像平面的四个点特征构建成一个四边形，通过实时跟踪四个点特征的位置，更新所构建的四边形的虚拟质心，根据虚拟质心当前图像坐标和期望图像坐标之间的偏差，规划机器人的运动路线，实现户外自主移动机器人的运动控制(kawamurah,iwatas,sahashis,hasegawat.visualattitudecontrolusingavirtualvarycenterofaquadranglethatconstructedfromfeaturepointsforoutdoorautonomousmobilerobots[c].internationalsymposimonmicro-nanomechatronicsandhumanscience(mhs).nagoya,japan.november4-7,2012:316-318.)。该方法仅用一个虚拟质心进行机器人多自由度控制，精度难以保证。cai等提出一种虚拟复合视觉系统以及新的立体视觉模型，分别将任务空间中的点特征的位置信息以及机器人位姿信息映射到新的三维视觉笛卡尔空间中，实现机器人的六自由度控制(caicx,somanin,knolla.orthogonalimagefeaturesforvisualservoingofa6-dofmanipulatorwithuncalibratedstereocameras[j],ieeetransactionsonrobotics,2016,32(2):452-461.)。该方法建立了点特征信息和机器人位姿之间的对应关系，但没有考虑平移控制和旋转控制之间的耦合，难以保证目标一直出现在视野当中。
技术实现要素：
基于上述背景，本发明的主要目的在于提供一种基于点特征的机器人六自由度控制方法。
为达到上述目的，本发明利用点特征的雅克比矩阵，在深度估计的基础上，将平移运动和旋转运动进行解耦，用平移运动来补偿旋转运动引起的点特征信息的变化，实现机器人的六自由度控制。该方法包括步骤如下：
步骤s0，使用一个视觉传感器获取目标图像；
步骤s1：从所述视觉传感器获得的目标图像中选择视觉传感器敏感的至少三个点特征，离线得到所述点特征的期望图像坐标，在线提取所述点特征的当前图像坐标，比较当前值和期望值，得到当前点特征和期望点特征之间的图像偏差；
步骤s2：基于小孔模型，利用摄像机一次运动前后引起的点特征坐标变化，对点特征的当前深度值进行估计；
步骤s3：根据s1的点特征信息和s2的深度信息，得到点特征的雅克比矩阵；
步骤s4：基于s3的点特征雅克比矩阵，采用基于图像的视觉伺服控制方法，求取摄像机的平移量和旋转量；
步骤s5：基于s3的点特征雅克比矩阵，求取s4的旋转量产生的点特征坐标变化量；
步骤s6：根据s3的点特征雅克比矩阵，计算补偿s5的点特征坐标变化量所需要的平移量；
步骤s7：根据s4的平移量和s6的平移补偿量，得到摄像机的平移总量；
步骤s8：根据s4的旋转量和s7的平移总量，得到下一步的运动控制量，实现机器人的六自由度控制。
进一步说明，其中步骤s1中所述点特征的当前图像偏差是通过下式来计算的：
其中，△u、△v分别表示点特征在u方向和v方向的图像坐标偏差。(ud,vd)表示点特征的期望图像坐标，(u,v)表示点特征的当前图像坐标。
进一步说明，其中步骤s2中所述的点特征的当前深度值估计过程如下：
对于摄像机坐标系中的点(xc,yc,zc)，焦距归一化成像平面上的成像点坐标为(x1c,y1c,1)。
将式(2)对时间求导数，改写为矩阵形式
式(3)为点特征在笛卡儿空间的平移运动速度与投影到成像平面空间的运动速度之间的关系。由小孔模型可知，
摄像机的运动会导致点特征在摄像机坐标系中的运动。点特征在摄像机坐标系中的运动速度与摄像机在笛卡儿空间运动速度之间的关系为
式中：xc＝[xc,yc,zc]t是点特征的位置向量，vca＝[vcax,vcay,vcaz]t是摄像机的线速度向量，ωca＝[ωcax,ωcay,ωcaz]t是摄像机的角速度向量。
将式(5)代入式(3)，合并同类项，并应用式(2)，有
式(6)中，计算xc1和yc1时，涉及摄像机的内参数。zc的值是该点相对于摄像机坐标系的深度。因此，采用如上形式的任何控制方案必须估计或近似给出zc的值。
由式(6)可得
其中，zcpi是第i个点特征的zc坐标。
对式(7)化简，可得第i个点特征的当前深度值：
在利用式(8)估计点特征的深度时，需要用到摄像机运动前后的特征参数和相机运动量，因此在处理第一帧图像时，只提取特征参数，然后使机器人主动运动，在得到第二帧图像时，根据主动运动前后的特征参数和摄像机主动运动量，进行深度估计。
进一步说明，其中步骤s3中所述的点特征的雅克比矩阵计算如下：
将式(6)重写为
其中，lp就是点特征的雅克比矩阵。
利用摄像机的内参数，可以求出图像点(u,v)在焦距归一化成像平面上的成像点坐标：
因此，根据式(8)估计点特征的当前深度，根据式(11)计算点特征的归一化坐标，将结果带入式(10)即可得到点特征的雅克比矩阵lp。
进一步说明，其中步骤s4中所述的平移量和旋转量计算如下：
对于n个点特征，其特征变化量与摄像机运动量之间满足关系：
其中，lpn是n个点特征的雅克比矩阵。
采用基于图像的控制律：
其中，λp是调整系数，(δx1cn,δy1cn)是第n个点特征在归一化平面坐标的期望值与当前值的差值。
分解式(13)，得到摄像机的平移量vcap和旋转量wcap。
进一步说明，其中步骤s5中所述的点特征坐标变化量计算如下：
由式(6)可知，旋转运动引起的点特征的坐标变化如下：
其中，lwn是点特征与旋转运动的雅克比矩阵，可由点特征的雅克比矩阵得到。
进一步说明，其中步骤s6中所述的平移补偿量如下：
由式(14)得到的旋转运动引起的n个点特征的坐标变化，计算出需要的平移补偿量为：
其中，vcaw是由旋转运动引起的平移补偿量。lvn是点特征与平移运动的雅克比矩阵,可由点特征的雅克比矩阵得到。
进一步说明，其中步骤s7中所述的平移总量表示如下：
vca＝vcap+vcaw(18)
进一步说明，其中步骤s8中所述的机器人运动调整量计算如下：
由式(13)和式(18),得到摄像机运动调整量：
根据式(19)和手眼关系矩阵tm,得到机器人的运动调整量：
δt＝tmδtc(20)
基于上述技术方案可知，本发明具有以下有益效果：单目视觉场景中，难以获取目标的深度信息，点特征的使用受到制约。另外，由于视觉传感器的自身特性，相对于平移运动，点特征对旋转运动更加敏感，不恰当的旋转容易导致点特征在视野中丢失，导致伺服失败。
本发明针对仅使用点特征的单目视觉场景，基于点特征的雅克比矩阵，利用机器人的一次主动运动，对目标的深度信息进行估计，并通过平移运动补偿旋转运动引起的点特征坐标，将机器人的平移和旋转进行解耦，实现机器人的六自由度控制。
由此可见，本发明能够在单目视觉场景下，不需要繁琐的计算，仅利用相机的一次主动运动对目标的深度进行估计，摆脱了深度信息对基于点特征的视觉控制方法的制约，同时将平移运动和旋转运动进行了解耦，保证目标不在视野中丢失。本发明减少了求解深度信息的计算量，提高了视觉控制的成功率，能够进一步推广点特征在单目视觉场景的使用。
附图说明
图1为本发明的基于点特征的视觉控制方法的控制框图。
具体实施方式
下面结合附图对本发明的实施例作详细的说明：本实施例在以本发明技术方案为前提下进行实施，结合详细的实施方式和具体的操作过程，但本发明的保护范围不限于下述实施例。
本发明公开了一种基于点特征的机器人视觉控制方法，该方法在深度估计的基础上，利用点特征的雅克比矩阵，将平移运动和旋转运动进行解耦，用平移运动来补偿旋转运动引起的点特征信息的变化，实现机器人的六自由度控制。
更具体地，作为本发明的一个优选实施例，如图1示出本发明的基于点特征的机器人视觉控制方法的框图。在控制过程中，从视觉图像得到当前点特征和期望点特征的图像偏差，在深度估计的基础上，得到点特征雅克比矩阵，进而得到当前的平移误差和旋转误差，然后用平移运动来补偿旋转运动引起的点特征信息的变化，实现机器人的六自由度控制。该方法包括以下步骤：
第一步：从目标图像中选择视觉传感器敏感的至少三个点特征，离线得到所述点特征的期望图像坐标，在线提取所述点特征的当前图像坐标，比较当前值和期望值，得到当前点特征和期望点特征之间的图像偏差；；
第二步：基于小孔模型，利用摄像机一次运动前后引起的点特征坐标变化，对点特征的当前深度值进行估计；
第三步：根据第一步的点特征信息和第二步的深度信息，得到点特征的雅克比矩阵；
第四步：基于第三步的点特征雅克比矩阵，采用基于图像的视觉伺服控制方法，求取摄像机的平移量和旋转量；
第五步：基于第三步的点特征雅克比矩阵，求取第四步的旋转量产生的点特征坐标变化量；
第六步：根据第三步的点特征雅克比矩阵，计算补偿第五步的点特征坐标变化量所需要的平移量；
第七步：根据第四步的平移量和第六步的平移补偿量，得到摄像机的平移总量；
第八步：根据第四步的旋转量和第七步的平移总量，得到下一步的运动控制量，实现机器人的六自由度控制。
所述第一步，具体如下：
根据视觉传感器获取的目标图像，提取至少三个点特征，得到式(1)所示的当前图像偏差如。
所述第二步，具体如下：
基于小孔模型，利用摄像机一次运动前后引起的点特征坐标变化，得到式(8)所示的点特征当前深度估计值。
所述第三步，具体如下：
根据第一步的点特征信息和第二步的深度信息，经过推导，得到式(10)所示的点特征的雅克比矩阵；
所述第四步，具体如下：
基于第三步的点特征雅克比矩阵，采用基于图像的视觉伺服控制方法，经过推导，求取式(13)所示的摄像机的平移量和旋转量；
所述第五步，具体如下：
基于第三步的点特征雅克比矩阵，经过推导，求取式(14)所示的第四步的旋转量产生的点特征坐标变化量；
所述第六步，具体如下：
根据第三步的点特征雅克比矩阵，经过推导，计算式(16)所示的补偿第五步的点特征坐标变化量所需要的平移量；
所述第七步，具体如下：
根据第四步的平移量和第六步的平移补偿量，经过推导，得到式(18)所示的摄像机的平移总量；
所述第八步，具体如下：
根据第四步的旋转量和第七步的平移总量，经过推导，得到式(20)所示的下一步的运动控制量，实现机器人的六自由度控制。
以上所述的具体实施例，对本发明的目的、技术方案和有益效果进行了进一步详细说明，应理解的是，以上所述仅为本发明的具体实施例而已，并不用于限制本发明，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。