CN105929849A - 一种基于点镇定的轮式移动机器人目标跟踪控制方法 - Google Patents

Abstract

本发明公开了一种基于点镇定的轮式移动机器人目标跟踪控制方法。首先，建立轮式移动机器人运动学模型，通过外部传感器获取目标的相对位置，并建立虚拟跟踪目标。然后，设计线速度及角速度控制器，通过李雅普诺夫稳定性理论及LaSalle不变原理证明本发明所设计的控制器能使得虚拟目标轨迹收敛至实际目标，代表轮式移动机器人跟踪上目标。本发明所涉及的跟踪控制方法能够使得轮式移动机器人***渐近稳定，机器人有效地跟踪上目标，仿真及实验结果验证了本发明所提出的控制方法的合理性。

Description

一种基于点镇定的轮式移动机器人目标跟踪控制方法

技术领域

本发明涉及轮式移动机器人伺服控制领域，尤其是一种基于点镇定的轮式移动机器人目标跟踪控制方法。

背景技术

轮式移动机器人在物料自动搬运、特殊人群服务、抢险救灾，以及危险地域探索等方面的应用具有不可比拟的优势，已广泛地应用于工业、农业、服务业、国防和宇宙探索等领域，对人类社会的生产和生活产生了积极而深远的影响。例如，美国国家宇航局研制的“好奇号”火星探测车成功登陆火星，为人类探测外星生命打下坚实的基础。近年来，针对轮式移动机器人的控制研究产生诸多分支，其中一个重要的分支就是视觉伺服控制的研究。随着各类传感器的发展，特别是视觉传感器，为轮式移动机器人视觉伺服控制研究提供了更为广阔的应用空间。

根据摄像机安装位置的不同，机器人视觉伺服***分为eye-to-hand(固定摄像机)***及eye-in-hand(手眼)***。eye-to-hand***的摄像机安装在机器人本体之外，通过摄像机获得目标及机器人的控制位姿来控制机器人跟踪目标，此类***不易出现目标丢失的现象，但机器人的运动容易造成目标的遮挡。eye-in-hand***的摄像机安装于机器人本体上，由机器人运动带动摄像机运动，此类***对于防止目标遮挡有较强的优势，但又容易出现目标丢失的情况。因此，如何更好地解决以上两种问题是轮式移动机器人目标跟踪控制研究的难点。

综上所述，研究如何令轮式移动机器人在已知环境下，不出现目标丢失和目标遮挡且快速有效地跟踪上目标，对于轮式移动机器人的智能化、自主化，都具有重要的理论价值和实际意义。

发明内容

发明目的：针对上述现有技术存在的缺陷，本发明旨在提供一种解决轮式移动机器人目标跟踪控制问题的基于点镇定的轮式移动机器人目标跟踪控制方法。

技术方案：一种基于点镇定的轮式移动机器人目标跟踪控制方法，具体包括如下步骤：

(1)对轮式移动机器人进行分析，建立轮式移动机器人非完整运动学模型；

(2)利用单目摄像头获取目标对机器人本体的相对位置差ρ和相对姿态差α；

(3)建立虚拟跟踪目标并根据步骤(2)中所得的ρ和α，与步骤(1)中的运动学模型结合，设计机器人的线速度v与角速度ω；

(4)将步骤(3)中设计的线速度v与角速度ω代入李雅普诺夫函数，若李雅普诺夫函数收敛至零，则验证了所设计的线速度v和角速度ω使得***渐近稳定，且机器人已准确地跟踪上目标若李雅普诺夫函数未收敛至零，则返回步骤(3)重新设计机器人的线速度v与角速度ω。

进一步的，步骤(1)中所述轮式移动机器人非完整运动学模型具体为：

$\left\{\begin{array}{c}\stackrel{\·}{x}=vcos\mathrm{\θ}\\ \stackrel{\·}{y}=vsin\mathrm{\θ}\\ \stackrel{\·}{\mathrm{\θ}}=\mathrm{\ω}\end{array}\right.$

其中(x,y)是机器人在世界坐标系下的坐标，θ是机器人在世界坐标系下的航向角。

进一步的，步骤(2)中所述获取目标对机器人本体的相对位置差ρ和相对姿态差α具体为：

$\left\{\begin{array}{c}\mathrm{\ρ}=\sqrt{{(x-x_b)}^2+{(y-y_b)}^2}\\ \mathrm{\α}=\arctan \left(\frac{{\tilde{y}}_b}{{\tilde{x}}_b}\right)\end{array}\right.$

其中，(x_b,y_b)为目标在世界坐标系下的坐标，为目标在机器人本体笛卡尔坐标系下的坐标且

进一步的，步骤(3)中所述虚拟跟踪目标在世界坐标系下的坐标为(x_c,y_c)，满足：

$\left[\begin{array}{c}{x}_c\\ y_c\end{array}\right]=\left[\begin{array}{c}x\\ y\end{array}\right]+{\mathrm{\ρ}}^{*}\left[\begin{array}{c}cos\mathrm{\θ}\\ sin\mathrm{\θ}\end{array}\right]$

进一步的，步骤(3)中所述机器人的线速度v与角速度ω为：

$\left\{\begin{array}{c}v=k_v{v}_{max}(1-\frac{{\mathrm{\ρ}}^{*}}{\mathrm{\ρ}})\\ \mathrm{\ω}=k_{\mathrm{\ω}}{\mathrm{\ω}}_{max}\sin \mathrm{\α}\end{array}\right.$

其中，v_max、ω_max分别为最大线速度和角速度，k_v、k_ω∈(0,1]分别为线速度和角速度控制增益，ρ^*和ρ分别为机器人本体与目标之间的期望距离和实际距离，α为目标在机器人本体笛卡尔坐标系下的偏向角。

进一步的，在步骤(3)中，附加如下条件：

k_ωω_maxρ^*≥k_vv_max

进一步的，步骤(4)中所述李雅普诺夫函数即：

$V=\frac{1}{2}\[{(x_c-x_b)}^2+{(y_c-y_b)}^2\].$

有益效果：本发明通过在轮式移动机器人正前方ρ^*处建立虚拟跟踪目标的方式，将虚拟目标与目标之间的误差作为反馈控制量，控制虚拟目标的轨迹收敛到目标解决了一类轮式移动机器人目标跟踪问题，相比于现有的轮式移动机器人目标跟踪控制方法，本发明设计基于视觉的运动控制器结构简单，硬件要求低，控制精度高，较好地解决了目标丢失等问题。

附图说明

图1是本发明中轮式移动机器人运动学模型及其坐标系示意图；

图2是本发明中轮式移动机器人目标跟踪控制原理图；

图3是本发明中轮式移动机器人跟踪运动示意图；

图4是本发明中轮式移动机器人运动控制器的速度控制量；

图5是本发明中轮式移动机器人运动控制器的角速度控制量；

图6是本发明中轮式移动机器人的实际距离曲线图；

图7是本发明中轮式移动机器人的偏差角曲线图。

具体实施方式

下面结合附图对本发明作进一步说明：本发明适用于带有单目摄像头等外部传感器的轮式移动机器人控制***，其运动学模型及坐标系建立如图1所示。***由目标测距模块得到目标偏差，与期望跟踪距离结合设计运动控制器，控制轮式移动机器人持续跟踪目标。

如图2所示，一种基于点镇定的轮式移动机器人目标跟踪控制方法，具体包括如下步骤：

(1)对轮式移动机器人进行分析，建立轮式移动机器人非完整运动学模型，具体为：

$\left\{\begin{array}{c}\stackrel{\·}{x}=vcos\mathrm{\θ}\\ \stackrel{\·}{y}=vsin\mathrm{\θ}\\ \stackrel{\·}{\mathrm{\θ}}=\mathrm{\ω}\end{array}\right.$

其中(x,y)是机器人在世界坐标系下的坐标，θ是机器人在世界坐标系下的航向角。

(2)利用单目摄像头获取目标对机器人本体的相对位置差ρ和相对姿态差α，具体为：

$\left\{\begin{array}{c}\mathrm{\ρ}=\sqrt{{(x-x_b)}^2+{(y-y_b)}^2}\\ \mathrm{\α}=\arctan \left(\frac{{\tilde{y}}_b}{{\tilde{x}}_b}\right)\end{array}\right.$

其中，(x_b,y_b)为目标在世界坐标系下的坐标，为目标在机器人本体笛卡尔坐标系下的坐标且则：

$\left[\begin{array}{c}{\tilde{x}}_b\\ {\tilde{y}}_b\end{array}\right]=\left[\begin{array}{cc}cos\mathrm{\θ}& sin\mathrm{\θ}\\ -sin\mathrm{\θ}& cos\mathrm{\θ}\end{array}\right]\left[\begin{array}{c}{x}_b-x\\ y_b-y\end{array}\right]$

(3)建立虚拟跟踪目标虚拟跟踪目标在世界坐标系下的坐标为(x_c,y_c)，满足：

$\left[\begin{array}{c}{x}_c\\ y_c\end{array}\right]=\left[\begin{array}{c}x\\ y\end{array}\right]+{\mathrm{\ρ}}^{*}\left[\begin{array}{c}cos\mathrm{\θ}\\ sin\mathrm{\θ}\end{array}\right]$

根据步骤(2)中所得的ρ和α，与步骤(1)中的运动学模型结合，设计机器人的线速度v与角速度ω为：

$\left\{\begin{array}{c}v=k_v{v}_{max}(1-\frac{{\mathrm{\ρ}}^{*}}{\mathrm{\ρ}})\\ \mathrm{\ω}=k_{\mathrm{\ω}}{\mathrm{\ω}}_{max}\sin \mathrm{\α}\end{array}\right.$

其中，v_max、ω_max分别为最大线速度和角速度，k_v、k_ω∈(0,1]分别为线速度和角速度控制增益，ρ^*和ρ分别为机器人本体与目标之间的期望距离和实际距离，α为目标在机器人本体笛卡尔坐标系下的偏向角。

为了确保机器人能快速有效地跟踪上目标还需附加如下条件：

k_ωω_maxρ^*≥k_vv_max

(4)将步骤(3)中设计的线速度v与角速度ω代入李雅普诺夫函数，即：

$V=\frac{1}{2}\[{(x_c-x_b)}^2+{(y_c-y_b)}^2\]$

对其求导得：

$\stackrel{\·}{V}=(x_c-x_b)(vcos\mathrm{\θ}-{\mathrm{\ρ}}^{*}\mathrm{\ω}sin\mathrm{\θ})+(y_c-y_b)(v\sin \mathrm{\θ}+{\mathrm{\ρ}}^{*}\mathrm{\ω}cos\mathrm{\θ})$

将控制器v、ω代入得：

$\begin{array}{c}\stackrel{\·}{V}=-k_1{\[(x_c-x_b)\cos \mathrm{\θ}+(y_c-y_b)\sin \mathrm{\θ}\]}^2\\ -k_2{\[(x_c-x_b)\sin \mathrm{\θ}-(y_c-y_b)\cos \mathrm{\θ}\]}^2\\ +k_1(x_c-x_b)(\mathrm{\ρ}-{\tilde{x}}_b)\cos \mathrm{\θ}+k_1(y_c-y_b)(\mathrm{\ρ}-{\stackrel{\‾}{x}}_b)\sin \mathrm{\θ}\end{array}$

其中均大于零且k₂>k₁。

因若(x_c-x_b)cosθ+(y_c-y_b)sinθ≤0，则

考虑(x_c-x_b)cosθ+(y_c-y_b)sinθ>0情形，下面分两种情况讨论：

情形1：

由可得从而有：

$\begin{array}{c}\stackrel{\·}{V}\≤-k_1{\[\cos \mathrm{\θ}(x_c-x_b)+\sin \mathrm{\θ}(y_c-y_b)\]}^2\\ -k_1{\[\sin \mathrm{\θ}(x_c-x_b)-\cos \mathrm{\θ}(y_c-y_b)\]}^2+k_1(x_c-x_b){\tilde{y}}_b\cos \mathrm{\θ}\\ +k_1(y_c-y_b){\tilde{y}}_b\sin \mathrm{\θ}\end{array}$

展开整理得：

$\begin{array}{c}\stackrel{\·}{V}\≤-k_1\[({\cos }^2\mathrm{\θ}-\sin \mathrm{\θ}\cos \mathrm{\θ}+{\sin }^2\mathrm{\θ}){(x_c-x_b)}^2\\ +({\cos }^2\mathrm{\θ}-{\sin }^2\mathrm{\θ})(x_c-x_b)(y_c-y_b)\\ +({\cos }^2\mathrm{\θ}+\sin \mathrm{\θ}\cos \mathrm{\θ}+{\sin }^2\mathrm{\θ}){(y_c-y_b)}^2\]\end{array}$

将不等式右边整理为平方项和形式：

$\begin{array}{c}\stackrel{\·}{V}\≤-\frac{k_1}{2}\{{\[(x_c-x_b)(cos\mathrm{\θ}-sin\mathrm{\θ})+(y_c-y_b)(cos\mathrm{\θ}+sin\mathrm{\θ})\]}^2+{(x_c-x_b)}^2\\ +{(y_c-y_b)}^2\}\≤0\end{array}$

当时，有：

$\left\{\begin{array}{c}{y}_c-y_b=0\\ x_c-x_b=0\end{array}\right.$

所以的最大不变集是{(x_b,y_b)}，根据LaSalle不变原理，(x_c,y_c)的轨迹会收敛到(x_b,y_b)。

情形2：

由可得从而有：

$\begin{array}{c}\stackrel{\·}{V}\≤-k_1{\[\cos \mathrm{\θ}(x_c-x_b)+\sin \mathrm{\θ}(y_c-y_b)\]}^2\\ -k_1{\[\sin \mathrm{\θ}(x_c-x_b)-\cos \mathrm{\θ}(y_c-y_b)\]}^2+k_1(x_c-x_b){\tilde{y}}_b\cos \mathrm{\θ}\\ +k_1(y_c-y_b){\tilde{y}}_b\sin \mathrm{\θ}\end{array}$

展开整理得：

$\begin{array}{c}\stackrel{\·}{V}\≤-k_1\[({\cos }^2\mathrm{\θ}+\sin \mathrm{\θ}\cos \mathrm{\θ}+{\sin }^2\mathrm{\θ}){(x_c-x_b)}^2\\ +({\cos }^2\mathrm{\θ}-{\sin }^2\mathrm{\θ})(x_c-x_b)(y_c-y_b)\\ +({\cos }^2\mathrm{\θ}-\sin \mathrm{\θ}\cos \mathrm{\θ}+{\sin }^2\mathrm{\θ}){(y_c-y_b)}^2\]\end{array}$

将不等式右边整理为平方项和形式：

$\begin{array}{c}\stackrel{\·}{V}\≤-\frac{k_1}{2}\{{\[(x_c-x_b)(cos\mathrm{\θ}+sin\mathrm{\θ})-(y_c-y_b)(cos\mathrm{\θ}-sin\mathrm{\θ})\]}^2+{(x_c-x_b)}^2\\ +{(y_c-y_b)}^2\}\≤0\end{array}$

当时，有：

$\left\{\begin{array}{c}{y}_c-y_b=0\\ x_c-x_b=0\end{array}\right.$

所以的最大不变集是{(x_b,y_b)}，根据LaSalle不变原理(拉萨尔不变原理)，(x_c,y_c)的轨迹会收敛到(x_b,y_b)。

因此，(x_c,y_c)的轨迹最终会收敛到(x_b,y_b)，(x,y)的轨迹会收敛到以(x_b,y_b)为中心、ρ^*为半径的圆环上。轮式移动机器人最终会跟踪上目标且正对着目标即：

在控制器的作用下，李雅普诺夫函数最终收敛至零，证明所设计的线速度v和角速度ω能使得***渐近稳定，故可知虚拟跟踪目标的轨迹最终收敛至目标代表轮式移动机器人最终跟踪上目标仿真实验结果如图3-图7所示。

综合以上稳定性分析，本发明设计的基于点镇定的轮式移动机器人目标跟踪控制方法是稳定的，且目标跟踪效果良好。

以上仅是本发明的优选实施方式，应当指出：对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (7)

1.一种基于点镇定的轮式移动机器人目标跟踪控制方法，其特征在于，具体包括如下步骤：

(1)对轮式移动机器人进行分析，建立轮式移动机器人非完整运动学模型；

(2)利用单目摄像头获取目标对机器人本体的相对位置差ρ和相对姿态差α；

(3)建立虚拟跟踪目标并根据步骤(2)中所得的ρ和α，与步骤(1)中的运动学模型结合，设计机器人的线速度v与角速度ω；

(4)将步骤(3)中设计的线速度v与角速度ω代入李雅普诺夫函数，若李雅普诺夫函数收敛至零，则验证了所设计的线速度v和角速度ω使得***渐近稳定，且机器人已准确地跟踪上目标若李雅普诺夫函数未收敛至零，则返回步骤(3)重新设计机器人的线速度v与角速度ω。

2.根据权利要求1所述的一种基于点镇定的轮式移动机器人目标跟踪控制方法，其特征在于，步骤(1)中所述轮式移动机器人非完整运动学模型具体为：

$\left\{\begin{array}{c}\stackrel{\·}{x}=vcos\mathrm{\θ}\\ \stackrel{\·}{y}=v\sin \mathrm{\θ}\\ \stackrel{\·}{\mathrm{\θ}}=\mathrm{\ω}\end{array}\right.$

其中(x,y)是机器人在世界坐标系下的坐标，θ是机器人在世界坐标系下的航向角。

3.根据权利要求1所述的一种基于点镇定的轮式移动机器人目标跟踪控制方法，其特征在于，步骤(2)中所述获取目标对机器人本体的相对位置差ρ和相对姿态差α具体为：

$\left\{\begin{array}{c}\mathrm{\ρ}=\sqrt{{(x-x_b)}^2+{(y-y_b)}^2}\\ \mathrm{\α}=\arctan \left(\frac{{\tilde{y}}_b}{{\tilde{x}}_b}\right)\end{array}\right.$

其中，(x_b,y_b)为目标在世界坐标系下的坐标，为目标在机器人本体笛卡尔坐标系下的坐标且

4.根据权利要求1所述的一种基于点镇定的轮式移动机器人目标跟踪控制方法，其特征在于，步骤(3)中所述虚拟跟踪目标在世界坐标系下的坐标为(x_c,y_c)，满足：

$\left[\begin{array}{c}{x}_c\\ y_c\end{array}\right]=\left[\begin{array}{c}x\\ y\end{array}\right]+{\mathrm{\ρ}}^{*}\left[\begin{array}{c}cos\mathrm{\θ}\\ sin\mathrm{\θ}\end{array}\right].$

5.根据权利要求1所述的一种基于点镇定的轮式移动机器人目标跟踪控制方法，其特征在于，步骤(3)中所述机器人的线速度v与角速度ω为：

$\left\{\begin{array}{c}v=k_v{v}_{max}(1-\frac{{\mathrm{\ρ}}^{*}}{\mathrm{\ρ}})\\ \mathrm{\ω}=k_{\mathrm{\ω}}{\mathrm{\ω}}_{max}\sin \mathrm{\α}\end{array}\right.$

其中，v_max、ω_max分别为最大线速度和角速度，k_v、k_ω∈(0,1]分别为线速度和角速度控制增益，ρ^*和ρ分别为机器人本体与目标之间的期望距离和实际距离，α为目标在机器人本体笛卡尔坐标系下的偏向角。

6.根据权利要求1所述的一种基于点镇定的轮式移动机器人目标跟踪控制方法，其特征在于，在步骤(3)中，附加如下条件：

k_ωω_maxρ^*≥k_vv_max。

7.根据权利要求1所述的一种基于点镇定的轮式移动机器人目标跟踪控制方法，其特征在于，步骤(4)中所述李雅普诺夫函数即：

$V=\frac{1}{2}\[{(x_c-x_b)}^2+{(y_c-y_b)}^2\].$