专利名称：基于视觉的移动小车位姿镇定控制方法
技术领域：
本发明涉及智能机器人技术领域，具体地说是指一种基于视觉的移动小车位姿镇定控制方法。
背景技术：
近年来移动小车在工业、民用、科学探索等多种行业和领域得到广泛应用。随着计算机技术、微电子技术及人工智能等技术的发展，移动小车在未知或时变环境下自主工作的可靠性、安全性和智能化水平不断提高。视觉传感器由于具有成本低、信息丰富、可靠性高等特点被广泛应用于移动小车控制系统，而位姿镇定是移动小车运动控制最基本问题，研究移动小车视觉伺服镇定问题具有重要理论与应用价值。移动小车是一种受非完整约束限制的系统，任何连续时不变的状态反馈控制方法都很难有效地解决其镇定问题；另外视觉信息中包含大量的数据，要从中获得有用的信息，需要复杂的算法、耗费大量的运算时间。因此，基于视觉的移动小车位姿镇定是一个十分具有挑战性的课题，吸引众多学者的研 究兴趣。Lopez-Nicolas 等人在 2OlO 年 IEEE Transactions on Systems, Man, andCybernetics, Part B:Cybernetics, 40(4)期刊上利用当前图像和目标图像间的单应性关系设计一种切换控制律，使小车沿着一些最佳的路线到达期望的位姿。Becerra等人在2011 年 IEEEInternational Conference on Robotics and Automation 会议上利用极线几何对移动小车的位姿进行动态估计，获得其位置和方向。虽然这些方法能够有效地解决基于视觉的位姿镇定问题，但是还存在以下主要不足(I)设计视觉控制器时都只考虑小车的运动学模型，把小车当作一个理想的点来看待，未考虑最能反映小车本质特性的动力学模型；(2)伺服速度慢，视觉控制算法在设计时没有和控制理论相结合，在实际应用中很难获得满意的性能。
发明内容
本发明的目的在于提供一种基于视觉的移动小车位姿镇定控制方法，以解决上述方法在基于视觉的位姿镇定控制时未考虑小车动力学模型和伺服速度慢问题。本发明方法利用Epipolar geometry, ID trifocal tensor及反演方法设计一种动态切换控制律,使小车沿着最短路径快速镇定到期望位姿。一种基于视觉的移动小车位姿镇定控制方法，包含以下步骤I. I.构建基于视觉的移动小车运动学模型和动力学模型，摄像机模型；I. 2.根据步骤I. I中的摄像机模型，建立摄像机在起始位姿、运动过程中所处的位姿和期望位姿处拍摄到的图像间的几何约束关系；图像间的几何约束关系包括两幅图像间对极几何关系和二幅图像间二线性约束关系，分别用Epipolar geometry模型和ID trifocal tensor模型表示，其中C1= (x1； Z1, 0 j)、C2=(x2, z2, 0 2)和Cd= (0，0, 0)分别为摄像机的初始位姿、当前位姿和期望位姿，eld, e2d，edl和e21为相应图像上的极点在图像平面x轴方向上的坐标，T1和T2为I阶张量;I. 3.根据步骤I. I中的移动小车运动学模型和步骤I. 2中的图像间的几何约束关系，采用切换控制策略设计数个独立有序的运动学控制器；I. 4.根据步骤I. I中的移动小车动力学模型和步骤I. 3中设计的运动学控制器，利用反演方法设计三个独立有序的动力学控制器，完成整个视觉控制器的设计。
步骤I. 3中根据步骤I. 2中图像间的几何约束关系采用三步控制策略设计切换的运动学控制器，为动力学控制器提供期望速度小车在原地旋转,期望线速度u dl为零，利用Epipolar geometry设计角速度控制器，其输出作为动力学控制器的输入，为控制小车旋转直至摄像机光轴指向期望位置提供所需的期望角速度《dl;当摄像机光轴指向期望位置时，利用ID trifocal tensor和Epipolar geometry分别设计线速度控制器和角速度控制器，其输出作为动力学控制器的输入，为控制小车做直线运动到达期望位置提供所需的期望线速度Ud2和期望角速度COd2;当小车到达期望位置时,期望线速度u d3为零，利用Epipolar geometry设计角速度控制器，其输出作为动力学控制器的输入，为控制小车旋转到期望方向提供所需的期望角速度《d3。步骤I. 4中利用反演方法设计的动力学控制器将步骤I. 3中设计的运动学控制器所提供的期望速度作为输入，用其输出力矩来控制小车的速度，使其趋向于期望速度，从而控制小车沿着最短路径镇定到期望位姿首先，驱动力矩为零，设计转动力矩控制器控制小车的角速度，使其趋向于期望的角速度《dl，从而控制小车旋转使摄像机光轴方向与基线C1Cd重合；其次，当摄像机光轴方向与基线C1Cd重合时，设计驱动力矩控制器和转动力矩控制器分别控制小车的线速度和角速度，使其趋向于期望的线速度Ud2和角速度COd2，从而控制小车沿着基线C1Cd到达期望位置；最后，当小车到达期望位置时，驱动力矩为零，设计转动力矩控制器控制小车的角速度，使其趋向于期望的角速度《 d3，从而控制小车旋转使其方向与期望的方向一致。本发明一种基于视觉的移动小车位姿镇定控制方法，与现有技术相比，具有以下明显的优势和有益效果本发明设计过程明确简单，鲁棒性好，伺服速度快。本发明解决了现有视觉伺服方法在位姿镇定时没有考虑机器人本质特性以及镇定过程时间长的问题，大大提高视觉位姿镇定的速度和精度。
图I是本发明中的一个带有单目摄像机的四轮差动驱动移动小车模型示意图；图2是本发明中的三视角间的对极几何关系示意图；图3是本发明中的摄像机相对位置关系示意图；图4是本发明中的移动小车二步运动不意图；图5是本发明中的基于视觉的移动小车位姿镇定控制方法流程图6 13是本发明中的基于视觉的移动小车位姿镇定仿真结果图。
具体实施例方式为了更好地理解本发明的技术方案，以一个带有单目摄像机的四轮差动驱动移动小车为研究对象对本发明的实施方式做进一步的介绍。I. I.构建基于视觉的移动小车运动学模型和动力学模型，摄像机模型如图I所示为四轮移动小车模型，前两轮从动，后两轮差动驱动，单目摄像机固定安装在小车中心处且其光轴指向小车运动方向。O-XYZ为世界坐标系，C-Xcjc^为摄像机坐标系，原点C与小车质心O重合。该小车的位姿由向量q=[x, z, 0]T表示，(x，z)为小车质心O的坐标，0为车身方向与Z轴夹角，L为后两轮中心的距离为轮子半径，u和o分别为小车的线速度和角速度。根据图I所定义的坐标系，在不考虑轮子侧滑等因素时，移动小车的运动学模型可表示为X = -U sin 6^ Z -V cos 0(I)
9 — co移动小车非完整约束条件为i cos 0 + z sin (9 = 0(2)米用拉格朗日建模方法，移动小车的动力学模型可表不为M{q)q + Vm (q,i[)q + C,Uj) = Nunr + AT (q)A(3)其中M是正定惯性矩阵,1(94)是向心力和哥式力矩阵，G(q)是重力向量矩阵，N(q)是输入转换矩阵，T是小车转矩，\是Lagrange乘子。在不考虑阻力矩时，式(3)中的变量定义如下
~m 0 Ol「0 0 0]
, x =[ x 1； t2]t
r
L -L其中m为小车质量，I为小车相对于质心O的转动惯量，T^t2分别为后两轮左右轮的输出力矩。式⑶变为M{q)q = N{q)r + ,4,' (q)A(4)对式(I)进行微分后代入式(4)，并考虑式(2)，则有
m Oir{>1 I Tl I ![r, I…=-(5)
0I o) r L -L tr _定义Td =-(1, +T2),Tt =-(h -A)分别为小车后两轮的驱动力矩和转动力矩，式
rr
(5)变为
权利要求
1. 一种基于视觉的移动小车位姿镇定控制方法，其特征在于包含以下步骤 1.1.构建基于视觉的移动小车运动学模型和动力学模型，摄像机模型； I. 2.根据步骤I. I中的摄像机模型，建立摄像机在起始位姿、运动过程中所处的位姿和期望位姿处拍摄到的图像间的几何约束关系； 所述的图像间的几何约束关系包括两幅图像间对极几何关系和三幅图像间三线性约束关系，分别用Epipolar geometry模型和ID trifocal tensor模型表示，其中C1= (x1； Z1, Θ j)、C2=(x2, ζ2, Θ 2)和Cd= (O, O, O)分别为摄像机的初始位姿、当前位姿和期望位姿，eld, e2d，edl和e21为相应图像上的极点在图像平面x轴方向上的坐标，T1和T2为I阶张量; I. 3.根据步骤I. I中的移动小车运动学模型和步骤I. 2中的图像间的几何约束关系，采用切换控制策略设计数个独立有序的运动学控制器； 1.4.根据步骤I. I中的移动小车动力学模型和步骤I. 3中设计的运动学控制器，利用反演方法设计三个独立有序的动力学控制器，完成整个视觉控制器的设计。
2.根据权利要求I所述的基于视觉的移动小车位姿镇定控制方法，其特征在于步骤I. 3中根据步骤I. 2中图像间的几何约束关系采用三步控制策略设计切换的运动学控制器，为动力学控制器提供期望速度 小车在原地旋转，期望线速度U dl为零，利用Epipolar geometry设计角速度控制器，其输出作为动力学控制器的输入，为控制小车旋转直至摄像机光轴指向期望位置提供所需的期望角速度ω 1 当摄像机光轴指向期望位置时，利用ID trifocal tensor和Epipolar geometry分别设计线速度控制器和角速度控制器，其输出作为动力学控制器的输入，为控制小车做直线运动到达期望位置提供所需的期望线速度ud2和期望角速度cod2; 当小车到达期望位置时，期望线速度U d3为零，利用Epipolar geometry设计角速度控制器，其输出作为动力学控制器的输入，为控制小车旋转到期望方向提供所需的期望角速度 ω<13。
3.根据权利要求I所述的基于视觉的移动小车位姿镇定控制方法，其特征在于步骤I.4中利用反演方法设计的动力学控制器将步骤I. 3中设计的运动学控制器所提供的期望速度作为输入，用其输出力矩来控制小车的速度，使其趋向于期望速度，从而控制小车沿着最短路径镇定到期望位姿 首先，驱动力矩为零，设计转动力矩控制器控制小车的角速度，使其趋向于期望的角速度《dl，从而控制小车旋转使摄像机光轴方向与基线C1Cd重合； 其次，当摄像机光轴方向与基线C1Cd重合时，设计驱动力矩控制器和转动力矩控制器分别控制小车的线速度和角速度，使其趋向于期望的线速度Ud2和角速度COd2，从而控制小车沿着基线C1Cd到达期望位置； 最后，当小车到达期望位置时，驱动力矩为零，设计转动力矩控制器控制小车的角速度，使其趋向于期望的角速度《d3，从而控制小车旋转使其方向与期望的方向一致。
全文摘要
本发明公开了一种基于视觉的移动小车位姿镇定控制方法，充分考虑小车的运动学模型和动力学模型以及摄像机模型，在起始位姿和期望位姿处通过摄像机分别获取初始图像和期望图像，并在运动过程中实时获取当前图像；然后利用拍摄到的图像间的对极几何关系和三线性约束关系，基于Epipolar geometry和1D trifocal tensor采用三步切换控制策略设计三个独立有序的运动学控制器；最后将运动学控制器的输出作为动力学控制器的输入利用反演方法设计一种动态切换控制律，使小车沿着最短路径快速镇定到期望位姿。本发明解决传统视觉伺服方法在位姿镇定控制时未考虑小车动力学特性以及伺服速度慢问题，更切合实际，能够使小车更快地镇定到期望位姿。
文档编号G05D1/02GK102736626SQ20121014696
公开日2012年10月17日 申请日期2012年5月11日 优先权日2012年5月11日
发明者付宜利, 曹政才, 殷龙杰, 靳保 申请人:北京化工大学