技术领域
[0001] 本
发明属于
计算机视觉与
移动机器人的技术领域，特别是涉及一种动态场景中轮式移动机器人的视觉镇定控制方法。
背景技术
[0002] 日常生活中，人类针对绝大多数外界信息是通过眼睛接收的，可以看出视觉
传感器的重要性。在移动机器人上安装视觉传感器，能使其变得更加智能并且应用领域更加广泛，诸如智能交通、深海勘探和家居服务。随着控制理论和视觉
感知的进步，吸引着越来越多的人研究
机器人视觉镇定控制。对于单目视觉系统，视觉特征的3D模型缺失是其固有问题，再加上轮式移动机器人非完整性约束给机器人视觉镇定带来了更大的挑战。现有的一些方法是在场景中建立固定特征点来进行视觉镇定，所以当特征点移动时现有的方法难以实现机器人的视觉镇定任务。
[0003] 对于轮式移动机器人，它是一个具有非完整的运动约束欠驱动系统。移动机器人有两个基本任务：第一是轨迹
跟踪控制；第二个是从初始
位姿到目标位姿，即镇定控制。移动机器人配备了视觉传感器，将视觉信息作为反馈量来控制移动机器人实现镇定控制。但图像特征点在整个视觉伺服过程必须保持不变，在实际应用中由于
工作空间因素等诸多原因有时会移动特征点。因此需要设计在特征点移动的状况下机器人仍然能够进行准确的视觉伺服。
发明内容
[0004] 本发明的目的是提供一种方法解决特征点移动的情况，这将会大大提高机器人的适应能
力。
[0005] 本发明提出一种动态场景中轮式移动机器人的视觉镇定系统的设计。本文基于单目的轮式移动机器人提出了一种新颖的方法，可以让机器人在视觉目标移动的情况下进行视觉伺服。对于现有的方法，当特征点固定时，移动机器人可以正确的到达期望位姿处，不过当特征点移动后，移动机器人难以到达期望的位姿处。为了监视特征点的移动，我们在实验场景中增加了一个监视摄像机，当特征点移动之后移动机器人仍然可以到达期望位姿。通过监视摄像机的图像，利用POSIT
算法计算出移动后的特征点与监视摄像机的关系。进而计算出相应的
坐标系和特征点之间的关系。利用坐标系变换规则，我们就可以根据实时的图像反馈信息计算出当前位姿和期望位姿之间的旋转矩阵和平移向量。利用基于极坐标表示法的
控制器驱动移动机器人到达期望位姿处。仿真与实验结果均证明本方法有效可靠。
[0006] 本发明提供的移动机器人的视觉镇定系统方法包括：
[0007] 一种动态场景中轮式移动机器人的视觉镇定系统，其特征包括以下步骤：
[0008] 第1，定义系统坐标系
[0009] 第1.1，系统坐标系的描述
[0010] 基于视觉目标定义了基准坐标系 设置摄像机与移动机器人的坐标系相重合，进而定义在当前位姿处的机器人/摄像机坐标系为 其中 的原点在摄像机的
光心位置处，也即
车轮轴线中心点的正上方。 的zc轴与摄像机光轴重合，同时也和机器人前进方向重合，xc轴和机器人轮轴平行；yc轴垂直于zcxc平面； 是机器人的期望位姿， 为固定摄像机的位置，监视摄像机的作用是监视特征点的移动；Mi定义为特征点移动之前的位置， 为特征点移动之前的目标坐标系；M*定义为特征点移动之后的位置， 为特征点移动之后的目标坐标系。基于视觉目标定义基准坐标系，假设特征点Mi(i＝1，2……i)是非共面的，其3维坐标是已知的。
[0011] 第1.2，控制方案
[0012] 特征点运动
时移动机器人可以有效地完成视觉伺服任务。视觉伺服镇定策略由三个阶段组成：第一阶段，监视摄像机采用POSIT方法计算特征点移动前和移动后的关系；第二阶段，可以获得实时的机器人的当前
姿态和期望姿势旋转矩阵和平移向量；最后在第三阶段，结合李亚普诺夫理论和极坐标，设计运动控制器来驱动机器人到想要的位姿。经过严格的理论可以证明所提出的视觉伺服方案是有效的。
[0013] 第2，系统设计
[0014] 第2.1，POSIT算法
[0015] 设想固定摄像机的焦距和各个内参数已经测出来，固定摄像机的原点为摄像机的焦点用O表示，正前方为Z轴，右方为X轴，下方为Y轴，i，j，k分别为X，Y，Z的单位向量。
[0016] 定义目标参考坐标系为{M1，u，v，w}，目标坐标系以一系列特征点中的第一个点M1为原点，Mi是第i个特征点的空间位置。
[0017] 用固定摄像机对移动前的特征点进行拍摄，然后用POSIT算法计算目标的旋转矩阵R和平移向量T。特征点Mi在参考坐标系下的坐标{ui，vi，wi}是已知的，Mi在图像坐标系下对应的点mi的坐标(xi，yi)也是已知的。特征点在摄像机坐标系下的位姿是未知的，所以Mi在摄像机坐标系下的(Xi，Yi，Zi)是未知的，所以要求出目标的旋转矩阵和平移向量。该方法使用了特征点Mi的尺度
正交投影点Pi。为了构造Pi，通过特征点M1画平行于平面G的平面K，投影中心O到它的距离Z1，将特征点Mi正交投影到平面K与Pi，再将Pi投影到平面G与Pi，将所有特征点都进行类似正交投影。旋转矩阵R的行是摄像机坐标系单位向量i，j，k在目标坐标系{M1，u，v，w}各分量上的投影。
[0018] 第2.2，监视摄像机控制策略
[0019] 第2.2.1，特征点移动之前坐标系与监视摄像机坐标系之间的关系
[0020] 定义目标坐标系为{M1，u，v，w}，目标坐标系以特征点中序号为1的点M1为原点，Mi是第i个特征点的空间位置，所求出的旋转矩阵 和平移向量sTo为：
[0021]
[0022] 特征点移动之前坐标系和监视摄像机坐标系之间的关系为：
[0023]
[0024] 第2.2.2，特征点移动之后坐标系与监视摄像机坐标系之间的关系
[0025] 定义目标坐标系为{M*1，u*，v*，w*}，其中目标坐标系以特征点中序号为1的点M*1为原点，M*i是第i个特征点的空间位置，所求出的旋转矩阵 和平移向量sTa为：
[0026]
[0027] 特征点移动之后坐标系和监视摄像机坐标系之间的关系为：
[0028]
[0029] 由此可知特征点在移动前和移动后相对于摄像机坐标系的空间位置。不过也需要知道特征点移动前与移动后坐标系之间的旋转与平移关系。
[0030] 第2.2.3，特征点移动前与移动后坐标系之间的关系
[0031] 设移动后的特征点到移动前坐标系的旋转矩阵为 平移向量为oTa为：
[0032]
[0033] 根据坐标系变换规则，由监视摄像机坐标系、特征点移动之前坐标系和移动之后坐标系的旋转关系可得：
[0034]
[0035] 由此可得 的值。
[0036] 特征点移动后与移动前坐标系的关系为：
[0037]
[0038] 其中(Ai，Bi，Ci)表示的就是特征点移动后在移动前所在坐标系的坐标值；
[0039]
[0040] 求出oTa，因此可得移动前与移动后两个坐标系之间的关系。
[0041] 第2.3，移动机器人控制策略
[0042] 第2.3.1，特征点移动之前坐标系与机器人期望坐标系的关系
[0043] 机器人坐标系的建立：机器人前进的方向为zc轴，左右方向为xc轴，上下方向为yc轴，g，e，f分别是其单位向量。
[0044] 首先在移动机器人的期望位置进行取像，用POSIT算法可以算出，特征点移动前相对移动机器人期望坐标系的旋转矩阵 和平移向量dTo为：
[0045]
[0046] (Xc1，Yc1，Zc1)是点M1在移动机器人期望坐标系下坐标值，特征点移动之前坐标系与机器人期望坐标系的关系为：
[0047]
[0048] 第2.3.2，特征点移动后的坐标系与移动机器人初始位姿坐标系的关系[0049] 移动机器人放到初始位姿，然后特征点做相应的移动，移动机器人对其进行
图像采集。设现在移动机器人的坐标系：x′c轴、y′c轴、z′c轴，分别对应着期望位姿处的xc轴、yc轴、zc轴，单位向量分别为e′，f′，g′。用POSIT算法可以计算出特征点移动后的坐标系相对于移动机器人初始位姿坐标系的旋转矩阵 和平移向量cTa为：
[0050]
[0051] (X′c1，Y′c1’Z′c1)是点M′1在移动机器人初始位姿坐标系下坐标值，特征点移动后与移动机器人初始位姿的关系如下所示：
[0052]
[0053] 第2.3.3，移动机器人期望位姿与初始位姿之间关系
[0054] 最后要求出的是移动机器人期望位姿与初始位姿之间的旋转矩阵 和平移向量dTc。由上述计算可得：
[0055]
[0056] 由此可以求出旋转矩阵
[0057]
[0058]
[0059]
[0060]
[0061] 由此可得出dTc。
[0063] 本部分介绍了在极坐标下移动机器人的运动。如
附图3所示，移动机器人位姿由极坐标表示。令 为移动机器人方向和距离e之间的
角度，可以得到以下运动学方程为：
[0064]
[0065] 设计线速度控制器为：
[0066] v＝(γcosα)e (19)
[0068]附图说明：
[0070] 图2为视觉伺服策略的坐标系关系
[0071] 图3为移动机器人的极坐标示意图
[0072] 图4为仿真图：机器人运动轨迹
[0073] 图5为仿真图：机器人运行速度和角速度
[0074] 图6为仿真图：摄像机图像点运动轨迹
[0075] 图7为仿真图：机器人的位姿变化
[0076] 图8为实验图：移动机器人的监视摄像机和特征点
[0077] 图9为实验图：特征点移动前后的图像
[0078] 图10为实验图：机器人的运动路径
[0079] 图11为实验图：特征点的运动轨迹
[0080] 图12为实验图：移动机器人的实时位姿的变化
[0081] 图13为实验图：机器人的线速度和角速度具体实施方式：
[0082] 1、一种动态场景中轮式移动机器人的视觉镇定系统，其特征包括以下步骤：
[0083] 第1，定义系统坐标系
[0084] 第1.1，系统坐标系的描述
[0085] 基于视觉目标定义了基准坐标系 设置摄像机与移动机器人的坐标系相重合，定义在当前位姿处的机器人/摄像机坐标系为 其中 的原点在摄像机的光心位置处，即车轮轴线中心点的正上方， 的zc轴与摄像机光轴重合，同时也和机器人前进方向重合，xc轴和机器人轮轴平行。yc轴垂直于zcxc平面， 是机器人的期望位姿， 为固定摄像机的位置，固定摄像机的作用是监视特征点的移动，坐标系相对关系如附图2、3所示。Mi定义为特征点移动之前的位置， 为特征点移动之前的目标坐标系。M*定义为特征点移动之后的位置， 为特征点移动之后的目标坐标系，基于视觉目标定义基准坐标系，假设特征点Mi(i＝1，2……i)是非共面的，其3维坐标是已知的。
[0086] 第1.2，控制方案
[0087] 特征点运动时移动机器人可以有效地完成视觉镇定任务。视觉伺服镇定策略由三个阶段组成：第一阶段，监视摄像机采用POSIT方法计算出特征点移动前和移动后的关系；第二阶段，可以获得实时的机器人的当前姿态和期望姿势旋转矩阵和平移向量；最后在第三阶段，结合李亚普诺夫理论和极坐标，设计运动控制器来驱动机器人到达想要的位姿。附图1给出了所提供方案的框图；经过严格的理论可以证明所提出的视觉伺服方案是有效的。
[0088] 第2，系统设计
[0089] 第2.1，监视摄像机控制策略
[0090] 第2.1.1，特征点移动之前的坐标系与监视摄像机坐标系之间的关系
[0091] 定义目标坐标系为{M1，u，v，w}，目标坐标系以特征点中序号为1的点M1为原点，Mi是第i个特征点的空间位置，经POSIT方法所求出的旋转矩阵 和平移向量sTo为：
[0092]
[0093] 特征点移动之前坐标系和监视摄像机坐标系之间的关系为：
[0094]
[0095] 第2.1.2，特征点移动之后坐标系与监视摄像机坐标系之间的关系
[0096] 定义目标坐标系为{M*1，u*，v*，w*}，其中目标坐标系以特征点中序号为1的点M*1为原点，M*i是第i个特征点的空间位置，所求出的旋转矩阵 和平移向量sTa为：
[0097]
[0098] 特征点移动之后坐标系和监视摄像机坐标系之间的关系为：
[0099]
[0100] 由此可知特征点在移动前和移动后相对于摄像机坐标系的空间位置，不过也需要知道特征点移动前与移动后坐标系之间的旋转与平移关系。
[0101] 第2.1.3，特征点移动前与移动后坐标系之间的关系
[0102] 设移动后的特征点变换到移动前坐标系的旋转矩阵为 平移向量为oTa：
[0103]
[0104] 根据坐标系变换规则，由监视摄像机坐标系、特征点移动之前坐标系和移动之后坐标系的旋转关系可得：
[0105]
[0106] 由此可得 的值。
[0107] 特征点移动后与移动前坐标系的关系为：
[0108]
[0109] 其中(Ai，Bi，Ci)表示的是特征点移动后在移动前所在坐标系的坐标值；
[0110]
[0111] 求出oTa，因此可得移动前与移动后两个坐标系之间的关系。
[0112] 第2.2，移动机器人控制策略
[0113] 第2.2.1，特征点移动之前坐标系与机器人期望坐标系的关系
[0114] 建立机器人坐标系，机器人前进的方向为zc轴，左右方向为xc轴，上下方向为yc轴，g，e，f分别是其单位向量。
[0115] 首先在移动机器人的期望位置进行取像，用POSIT算法可以算出，特征点移动前相对移动机器人期望坐标系的旋转矩阵 和平移向量dTo为：
[0116]
[0117] (Xc1，Yc1，Zc1)是点M1在移动机器人期望坐标系下坐标值，特征点移动之前坐标系与机器人期望坐标系的关系可以表示为：
[0118]
[0119] 第2.2.2，特征点移动后的坐标系与移动机器人初始位姿坐标系的关系[0120] 移动机器人放到初始位姿，然后特征点做相应的移动，移动机器人对其进行图像采集。设现在移动机器人的坐标系：x′c轴、y′c轴、z′c轴，分别对应着期望位姿处的xc轴、yc轴、zc轴，单位向量分别为e′，f′，g′。用POSIT算法可以计算出特征点移动后的坐标系相对c于移动机器人初始位姿坐标系的旋转矩阵 和平移向量Ta为：
[0121]
[0122] (X′c1，Y′c1，Z′c1)是点M′1在移动机器人初始位姿坐标系下坐标值，特征点移动后与移动机器人初始位姿的关系如下所示：
[0123]
[0124] 第2.2.3，移动机器人期望位姿与初始位姿之间关系
[0125] 最后要求出的是移动机器人期望位姿与初始位姿之间的旋转矩阵 和平移向量dTc，由上述计算可得：
[0126]
[0127] 由此可以求出旋转矩阵
[0128]
[0129]
[0130]
[0131]
[0132] 由此可得出dTc。
[0133] 第3，机器人运动学方程的建立
[0134] 本部分介绍了在极坐标下移动机器人的运动。如附图3所示，移动机器人位姿由极坐标表示，令 为移动机器人方向和距离e之间的角度，可以得到以下运动学方程为：
[0135]
[0136] 设计线速度控制器为：
[0137] v＝(γcosα)e (19)
[0138] 角速度控制器为：
[0139]
[0140] 第4，仿真实验和结论
[0141] 第4.1，仿真
[0142] 通过仿真来证明该方法的有效性。利用空间中6个特征点来计算移动机器人的位姿。
[0143] 摄像机内参数为：
[0144]
[0145] 移动机器人的初始位姿为(0.8m，-2.2m，35°)，期望位姿为(0.0m，0.0m，0°)，图像噪声的标准差为σ＝0.1个
像素。选取的控制参数为：γ＝1，k＝1.5，h＝0.9。
[0146] 如附图，图4是移动机器人在3D场景中的移动轨迹。图5是机器人随时间变化的线速度和角速度。图6是六个特征点在摄像机图像上的移动轨迹，其中圆圈表示的是初始位置的像素坐标，五角星表示的是特征点移动之前的像素坐标，方
块表示的是机器人到达期望位姿处的像素坐标。图7是X轴、Z轴和角度的变化，可见三个数据全部趋近于0。
[0147] 第4.2，实验
[0148] 经过实验，进一步验证所提出策略的有效性。如图8，图中配备有固定摄像机的IN-R移动机器人和六个非平面特征点(两个正方形的公共
顶点)。在VC++6.0环境下借助OpenCV库移动机器人实现其功能。监控摄像头(映美精DMK23G618)在Visual Studio 2013工作环境下实现其功能。
[0149] 移动机器人的初始位姿为(-2.3m，0.62m，26°)，期望位姿为(0.0m，0.0m，0°)。选取的控制参数为：γ＝0.09，k＝0.2，h＝0.8。
[0150] 摄像机内参数为：
[0151]
[0152] 如附图，图9是监视摄像机观察到的特征点移动前和移动后图像。在图9中，左边是特征点移动前的图像，右边是特征点移动后的图像。机器人的运动路径如图10所示，星号表示移动前特征点，圆圈表示移动后特征点。图11是特征点的运动轨迹，圆形点表示特征点的初始位置，星形点表示特征点的期望位置，正方形表示当机器人移动到期望位置时特征点的位姿。移动机器人的实时位姿的变化如图12所示，三条曲线都收敛于期望值，表明机器人能够移动到期望位姿。图13表示机器人的线速度和角速度。
[0153] 第4.3，结论
[0154] 本文是基于单目板载摄像机的轮式移动机器人。在试验场景中，当特征点移动时移动机器人仍然能够到达期望位置。本文提出了一个新的解决方案。为了得到特征点移动的信息，我们在实验场景中添加了一个监视摄像机。利用POSIT算法可以计算出特征点移动前后的关系。然后用POSIT算法得到各个坐标系之间的关系。进而得到移动机器人当前位姿和期望位姿的实时旋转矩阵和平移向量。通过李雅普诺夫
稳定性，设计一个线速度和角速度的控制器。仿真和实验结果可以说明此方法的有效性。
