一种基于点镇定的轮式移动机器人目标跟踪控制方法
【专利摘要】本发明公开了一种基于点镇定的轮式移动机器人目标跟踪控制方法。首先，建立轮式移动机器人运动学模型，通过外部传感器获取目标的相对位置，并建立虚拟跟踪目标。然后，设计线速度及角速度控制器，通过李雅普诺夫稳定性理论及LaSalle不变原理证明本发明所设计的控制器能使得虚拟目标轨迹收敛至实际目标，代表轮式移动机器人跟踪上目标。本发明所涉及的跟踪控制方法能够使得轮式移动机器人系统渐近稳定，机器人有效地跟踪上目标，仿真及实验结果验证了本发明所提出的控制方法的合理性。
【专利说明】
一种基于点镇定的轮式移动机器人目标跟踪控制方法
技术领域
[0001] 本发明涉及轮式移动机器人伺服控制领域，尤其是一种基于点镇定的轮式移动机 器人目标跟踪控制方法。
【背景技术】
[0002] 轮式移动机器人在物料自动搬运、特殊人群服务、抢险救灾，以及危险地域探索等 方面的应用具有不可比拟的优势，已广泛地应用于工业、农业、服务业、国防和宇宙探索等 领域，对人类社会的生产和生活产生了积极而深远的影响。例如，美国国家宇航局研制的 "好奇号"火星探测车成功登陆火星，为人类探测外星生命打下坚实的基础。近年来，针对轮 式移动机器人的控制研究产生诸多分支，其中一个重要的分支就是视觉伺服控制的研究。 随着各类传感器的发展，特别是视觉传感器，为轮式移动机器人视觉伺服控制研究提供了 更为广阔的应用空间。
[0003] 根据摄像机安装位置的不同，机器人视觉伺服系统分为eye-to-hand(固定摄像 机)系统及eye-in-hand (手眼)系统。eye-to-hand系统的摄像机安装在机器人本体之外，通 过摄像机获得目标及机器人的控制位姿来控制机器人跟踪目标，此类系统不易出现目标丢 失的现象，但机器人的运动容易造成目标的遮挡。eye-in-hand系统的摄像机安装于机器人 本体上，由机器人运动带动摄像机运动，此类系统对于防止目标遮挡有较强的优势，但又容 易出现目标丢失的情况。因此，如何更好地解决以上两种问题是轮式移动机器人目标跟踪 控制研究的难点。
[0004] 综上所述，研究如何令轮式移动机器人在已知环境下，不出现目标丢失和目标遮 挡且快速有效地跟踪上目标，对于轮式移动机器人的智能化、自主化，都具有重要的理论价 值和实际意义。
【发明内容】
[0005] 发明目的：针对上述现有技术存在的缺陷，本发明旨在提供一种解决轮式移动机 器人目标跟踪控制问题的基于点镇定的轮式移动机器人目标跟踪控制方法。
[0006] 技术方案:一种基于点镇定的轮式移动机器人目标跟踪控制方法，具体包括如下 步骤：
[0007] (1)对轮式移动机器人进行分析，建立轮式移动机器人非完整运动学模型；
[0008] (2)利用单目摄像头获取目标T对机器人本体的相对位置差p和相对姿态差a;
[0009] (3)建立虚拟跟踪目标I，并根据步骤(2)中所得的P和a，与步骤(1)中的运动学模 型结合，设计机器人的线速度v与角速度w ;
[0010] (4)将步骤(3)中设计的线速度v与角速度co代入李雅普诺夫函数，若李雅普诺夫 函数收敛至零，则验证了所设计的线速度v和角速度《使得系统渐近稳定，且机器人已准确 地跟踪上目标t若李雅普诺夫函数未收敛至零，则返回步骤(3)重新设计机器人的线速度v 与角速度《。
[0011] 进一步的，步骤(1)中所述轮式移动机器人非完整运动学模型具体为： Ix - v cos 9 y - v sin 0 9 ~ a>
[0013]其中（x，y)是机器人在世界坐标系下的坐标，0是机器人在世界坐标系下的航向 角。
[0014] 进一步的，步骤(2)中所述获取目标;T对机器人本体的相对位置差P和相对姿态差 a具体为：
[0016]其中，（xb，yb)为目标T在世界坐标系下的坐标，九)为目标T在机器人本体笛 卡尔坐标系下的坐标且4 > &
[0017]进一步的，步骤(3)中所述虚拟跟踪目标心在世界坐标系下的坐标为(x。，y。），满 足： 剛[y:] = [y]+d=]
[0019]进一步的，步骤⑶中所述机器人的线速度￥与角速度《为：
[0021 ]其中，Vmax、《 max分别为最大线速度和角速度，kv、ku G (〇，1 ]分别为线速度和角速 度控制增益，P^P分别为机器人本体与目标之间的期望距离和实际距离，a为目标在机器 人本体笛卡尔坐标系下的偏向角。
[0022]进一步的，在步骤(3)中，附加如下条件：
[0023] ku ? maxP* 多 kvVmax
[0024]进一步的，步骤(4)中所述李雅普诺夫函数即：
[0025] V = - [(xc - Xf,)^ + (yc - yb)2] -
[0026]有益效果：本发明通过在轮式移动机器人正前方P*处建立虚拟跟踪目标的方式， 将虚拟目标巧?与目标:T:之间的误差作为反馈控制量，控制虚拟目标忑1的轨迹收敛到目标 T，解决了一类轮式移动机器人目标跟踪问题，相比于现有的轮式移动机器人目标跟踪控制 方法，本发明设计基于视觉的运动控制器结构简单，硬件要求低，控制精度高，较好地解决 了目标丢失等问题。
【附图说明】
[0027]图1是本发明中轮式移动机器人运动学模型及其坐标系示意图；
[0028]图2是本发明中轮式移动机器人目标跟踪控制原理图；
[0029]图3是本发明中轮式移动机器人跟踪运动示意图；
[0030]图4是本发明中轮式移动机器人运动控制器的速度控制量；
[0031]图5是本发明中轮式移动机器人运动控制器的角速度控制量；
[0032]图6是本发明中轮式移动机器人的实际距离曲线图；
[0033] 图7是本发明中轮式移动机器人的偏差角曲线图。
【具体实施方式】
[0034] 下面结合附图对本发明作进一步说明：本发明适用于带有单目摄像头等外部传感 器的轮式移动机器人控制系统，其运动学模型及坐标系建立如图1所示。系统由目标测距模 块得到目标偏差，与期望跟踪距离结合设计运动控制器，控制轮式移动机器人持续跟踪目 标。
[0035] 如图2所示，一种基于点镇定的轮式移动机器人目标跟踪控制方法，具体包括如下 步骤：
[0036] (1)对轮式移动机器人进行分析，建立轮式移动机器人非完整运动学模型，具体 为： (X - V cos 0 y = v sin 0 0 - a)
[0038]其中（x，y)是机器人在世界坐标系下的坐标，0是机器人在世界坐标系下的航向 角。
[0039] (2)利用单目摄像头获取目标：T对机器人本体的相对位置差p和相对姿态差a，具体 为：
[0041 ]其中，（xb，yb)为目标T:在世界坐标系下的坐标，(4,丸）为目标T.在机器人本体笛 卡尔坐标系下的坐标且吞> 〇,则：
[0042] ^l = [cos0 Sin0ir^-^1 [yj i~sin9 cosG\bh-y\
[0043] (3)建立虚拟跟踪目标;虚拟跟踪目标在世界坐标系下的坐标为（Xc，y c)，满 足： \X{~\ X~\ 『COS0
[0044] / = cos〈 L^c-J Ly」 Lsm 0-
[0045]根据步骤(2)中所得的P和a，与步骤（1)中的运动学模型结合，设计机器人的线速 度v与角速度《为：
[0047]其中，vmax、《 max分别为最大线速度和角速度，kv、ku G (〇，1 ]分别为线速度和角速 度控制增益，pIPp分别为机器人本体与目标之间的期望距离和实际距离，a为目标在机器 人本体笛卡尔坐标系下的偏向角。
[0048]为了确保机器人能快速有效地跟踪上目标T，还需附加如下条件：
[0049] ku 〇 maxP* 多 kvVmax
[0050] (4)将步骤(3)中设计的线速度v与角速度CO代入李雅普诺夫函数，BP:
[0052] 对其求导得：
[0053] V - (xL - x.b)(v cos 6 - p~a> sin 0) + (yc- - vb)(x? sin 9 + pxco cos 6)
[0054] 将控制器v、《代入f，得： V = -kJCx, - xb) cos 0 + (yc - yh) sin 9]2
[0055] -. /c2 [_(xL. _ h) sin 0 _ (yc - yfc) cos 0]2 + - xh)(p - xh) cos 0 + k^v,. - yB)(p - %)sina
[0056] 其中
均大于零且k2>ki。
[0057] 因_ .元fc) 2 〇.*若（xc-xb)cos9+(yc-yb)sin9<0,则坎 〇。'
[0058] 考虑(1。11))(3〇89+(5^-5^)8；[119>0情形，下面分两种情况讨论：
[0059]情形1:%20
[0060]由兔2 〇，可得p -知=Jib2 + fhZ -宏&幺fh，从而有： V < -/cjcos 6 (xL. - xb) + sin G {yc - yh)]2
[0061 ] - /cjsin 6 (xc - xb) - cos 9 (ye： - yb)]2 + k^x,. - xb)yh cos 9 + h(yc - y-n)% sin 9
[0062] 展开整理得： V < -k1l(cos20 - sin 6 cos 6 + sin20)(xt. - xb)2
[0063] + (cos20 - sin20)(xc - xh)(yc - yb) + (cos20 + sin 0 cos 0 + sin26〇(yt.-凡)2]
[0064] 将不等式右边整理为平方项和形式：
[0066] 当饮=0时，有：
[0067] \}；c }，h ~ Un = 〇
[0068] 所以= 〇j|的最大不变集是{(xb，yb)}，根据LaSalle不变原理，（xc，yc)的 轨迹会收敛到(xb，yb)。
[0069] 情形2:爲 < 0
[0070] 由為 <:〇，可得p: -為=J%2 + fb2 一％ S _&，从而有： V < -A：! [cos 6 {xc - + sin 6 {yc - yb)]2
[0071 ] - ^^[sin 9 (xc - xb) - cos 6 (v,. - y；,)]2 + k1{x(： - cos 6 ~ ^(yc - yb)yk sin 6
[0072] 展开整理得： F < -ifejtCcos2^ + sin 9 cos 0 + sin20)(xc. - xb)2
[0073] 4- (sin20 - cos29)(xc - xb)(yc -- y^) + (cos2& - sin 6 cos 9 -f sin20)(yc- - ^)2]
[0074] 将不等式右边整理为平方项和形式：
[0076] 当玖=〇时，有：
[0077] 产 "二 [xc - Xf, = 0
[0078] 所以= 0}的最大不变集是{(a，yb)}，根据LaSalle不变原理(拉萨尔不 变原理），（x c，yc)的轨迹会收敛到(xb，yb)。
[0079] 因此，（x。，y。)的轨迹最终会收敛到(Xb，yb)，（x，y)的轨迹会收敛到以（ Xb，yb)为中 心、为半径的圆环上。轮式移动机器人最终会跟踪上目标且正对着目标:T，即：
[0080] 在控制器的作用下，李雅普诺夫函数最终收敛至零，证明所设计的线速度v和角速 度《能使得系统渐近稳定，故可知虚拟跟踪目标?;的轨迹最终收敛至目标T，代表轮式移动 机器人最终跟踪上目标:T。仿真实验结果如图3-图7所示。
[0081]综合以上稳定性分析，本发明设计的基于点镇定的轮式移动机器人目标跟踪控制 方法是稳定的，且目标跟踪效果良好。
[0082]以上仅是本发明的优选实施方式，应当指出：对于本技术领域的普通技术人员来 说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为 本发明的保护范围。
【主权项】
1. 一种基于点镇定的轮式移动机器人目标跟踪控制方法，其特征在于，具体包括如下 步骤： (1) 对轮式移动机器人进行分析，建立轮式移动机器人非完整运动学模型； (2) 利用单目摄像头获取目标y对机器人本体的相对位置差P和相对姿态差α; (3) 建立虚拟跟踪目标巧，并根据步骤(2)中所得的Ρ和α，与步骤（1)中的运动学模型结 合，设计机器人的线速度ν与角速度ω ; (4) 将步骤(3)中设计的线速度ν与角速度ω代入李雅普诺夫函数，若李雅普诺夫函数 收敛至零，则验证了所设计的线速度ν和角速度ω使得系统渐近稳定，且机器人已准确地跟 踪上目标:Γ;若李雅普诺夫函数未收敛至零，则返回步骤(3)重新设计机器人的线速度ν与角 速度ω。2. 根据权利要求1所述的一种基于点镇定的轮式移动机器人目标跟踪控制方法，其特 征在于，步骤(1)中所述轮式移动机器人非完整运动学模型具体为：其中(x，y)是机器人在世界坐标系下的坐标，θ是机器人在世界坐标系下的航向角。3. 根据权利要求1所述的一种基于点镇定的轮式移动机器人目标跟踪控制方法，其特 征在于，步骤(2)中所述获取目标:Γ对机器人本体的相对位置差P和相对姿态差α具体为：其中，U，yb)为目标Γ在世界坐标系下的坐标，(?.?)为目标『在机器人本体笛卡尔坐 标系下的坐标且％ 〇。4. 根据权利要求1所述的一种基于点镇定的轮式移动机器人目标跟踪控制方法，其特 征在于，步骤(3)中所述虚拟跟踪目标3；在世界坐标系下的坐标为(X。， y。），满足：5. 根据权利要求1所述的一种基于点镇定的轮式移动机器人目标跟踪控制方法，其特 征在于，步骤(3)中所述机器人的线速度ν与角速度ω为：其中，vmax、〇max分别为最大线速度和角速度，kv、kue (〇，1]分别为线速度和角速度控制 增益，pIPp分别为机器人本体与目标之间的期望距离和实际距离，α为目标在机器人本体 笛卡尔坐标系下的偏向角。6. 根据权利要求1所述的一种基于点镇定的轮式移动机器人目标跟踪控制方法，其特 征在于，在步骤(3)中，附加如下条件： kcj ω maxp 多 kvVmax 〇7. 根据权利要求1所述的一种基于点镇定的轮式移动机器人目标跟踪控制方法，其特 征在于，步骤(4)中所述李雅普诺夫函数即：
【文档编号】G05D1/12GK105929849SQ201610272043
【公开日】2016年9月7日
【申请日】2016年4月28日
【发明人】翟军勇, 肖大伟
【申请人】东南大学