CN102608997A - 一种基于人工场的轨迹跟踪控制方法 - Google Patents

Abstract

本发明涉及一种基于人工场的轨迹跟踪控制方法，包括以下步骤：1)开启运动控制平台，控制计算机与机器人连接，判断是否成功连接，若为是，执行步骤2)；2)机器人对自身的状态进行初始化，并将起始位姿p₀发送给控制计算机；3)控制计算机根据设定的最终期望位姿p_r和起始位姿p₀，计算方向角控制律w＝f(w)和线速度控制律v＝f(v)，并将其发送给机器人；4)机器人根据方向角控制律w＝f(w)和线速度控制律v＝f(v)进行相应动作，动作完后，将最终实际位姿p发送给控制计算机；5)控制计算机比较最终期望位姿p_r与最终实际位姿p，得到位姿误差p_e，判断该位姿误差p_e是否小于设定值，若为是，认为该系统合格。与现有技术相比，本发明具有设计简单、可靠性高、通用性强等优点。

Description

一种基于人工场的轨迹跟踪控制方法

技术领域

本发明涉及一种轨迹跟踪控制方法，尤其是涉及一种基于人工场的轨迹跟踪控制方法。

背景技术

非完整移动机器人之所以成为研究热点，成为大家关注的焦点，主要有三个方面的原因：其一，是社会生产发展、人类进步的需要。移动机器人的应用范围很广，包括国防工业、制造业、轻重工业以及服务业等诸多领域；其二，目前国内外高端技术领域的市场需求也是促使移动机器人发展的客观因素与潜在动力，比如星际探索(火星探索和月球探索)等，海洋开发(资源调查、石油矿藏开采和沉船打捞等)；其三，移动机器人研究的快速发展也是相关技术领域交叉互促的结果。移动机器人的研究涉及图像实时处理、计算机视觉、传感器技术、人工智能、自动控制、计算机并行处理技术、机械学等多学科理论与技术，体现了信息科学和人工智能技术的最新成果。这些学科为移动机器人在未知或动态环境下的实时控制开辟道路。非完整移动机器人技术已经成为众多高新技术的产物，同时也为其它技术的发展提供了广阔的应用场所。

非完整移动机器人一般工作在动态的、未知的复杂环境中，应该具有完全自主性甚至高度智能性，不需要任何人为干预就可以完成各种高级任务比如决策、导航、漫游、地图构建等等。但出于安全的考虑，在必要的情况下，特别是机器人自身的决策规划单元已经无效的情况下，人可以直接对非完整移动机器人进行远程控制，具体控制非完整移动机器人的各种行为。利用非完整移动机器人自主完成各种任务，要解决一系列问题，主要包括体系结构问题、环境建模与信息处理问题、定位问题、路径规划问题、运动控制问题。其中，运动控制问题是非完整移动机器人研究中最基本最重要的问题，也是机器人学中只有依靠控制理论才能予以解决的问题。非完整移动机器人最大的特点之一，就是其特有的空间运动能力，所以不解决运动控制问题，无论采取怎样的体系结构，最终都无法有效实现各种决策意图和规划任务。

从控制理论的角度出发，对于控制系统而言，控制问题的理论研究主要包括两方面内容，一是控制系统分析问题，一是控制系统综合问题，研究的前提是已知控制对象模型的结构和参数。获得控制对象模型的方法有机理建模和统计建模，机理建模是利用已知的物理原理建立对象的数学模型，统计建模是采用参数估计和系统辨识的方法获得对象模型。在分析问题中，根据已知的控制输入作用，来确定控制系统的定性行为(能控性、能观测性、稳定性等)以及定量的变化规律。在综合问题中，恰好与分析问题相反，根据所期望的受控系统运动形式或某些性能指标，来确定需要施加于控制对象的控制输入作用即控制律。

移动机器人的控制问题可以分为三种：对固定点的镇定问题；含有时间参数的轨迹跟踪问题；不含时间参数的几何路径跟踪问题。这三个基本问题本质上属于控制系统综合问题范畴，研究的都是控制器设计问题，即寻求某种控制律，使非完整移动机器人的移动平台能够镇定到某个期望点、跟随到某条期望路径或跟踪到某条期望轨迹。在根据某种控制理论设计出了运动控制律之后，还要给出闭环系统的稳定性证明，这属于控制系统分析问题。

本设计主要研究机器人轨迹跟踪问题。路径跟踪与轨迹跟踪同属于跟踪问题范畴，直观上看，区别仅在于描述路径和轨迹的曲线方程本身是不是时间的函数。从控制器设计的角度看，理论上讲路径跟随问题要比轨迹跟踪问题容易，但由于时间因素的介入，两者不能等同。不过，从现有的各种非完整移动机器人平台实验来看，落实到程序上采用的都是计算机控制，每隔一定时间进行一次采样，所以路径跟踪问题的期望路径方程实质上仍是时间的隐函数而且，从这个意义上看，可以用轨迹跟踪来实现路径跟踪的控制目的，即把期望路径加入时间因素后重新定义成期望轨迹，因此，本设计的研究重点主要是轨迹跟踪。

轨迹跟踪，是指根据某种控制理论，为非完整移动机器人系统设计一个控制输入即控制律，使非完整移动机器人能够到达并最终以给定的速度跟踪运动平面上给定的某条与时间呈函数关系的轨迹。

发明内容

本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种设计简单、可靠性高、通用性强的基于人工场的轨迹跟踪控制方法。

本发明的目的可以通过以下技术方案来实现：

一种基于人工场的轨迹跟踪控制方法，其特征在于，包括以下步骤：

1)开启运动控制平台，控制计算机与机器人连接，判断是否成功连接，若为是，执行步骤2)，若为否，继续执行步骤1)；

2)机器人对自身的状态进行初始化，并将起始位姿p₀发送给控制计算机；

3)控制计算机根据设定的最终期望位姿p_r和起始位姿p₀，计算方向角控制律w＝f(w)和线速度控制律v＝f(v)，并将其发送给机器人；

4)机器人根据方向角控制律w＝f(w)和线速度控制律v＝f(v)进行相应动作，动作完后，将最终实际位姿p发送给控制计算机；

5)控制计算机比较最终期望位姿p_r与最终实际位姿p，得到位姿误差p_e，判断该位姿误差p_e是否小于设定值，若为是，认为该系统合格，若为否，调整方向角控制律w＝f(w)和线速度控制律v＝f(v)的正控制参数后返回步骤4)。

所述的方向角控制律w＝f(w)和线速度控制律v＝f(v)的计算过程如下：

当x_e＝0时，可以得出以下方程组：

$\left\{\begin{array}{c}{\mathrm{\θ}}_e+2a=\mathrm{\β}\\ {\mathrm{\θ}}_a+a=\frac{1}{2}\mathrm{\π}\end{array}\right.---\left(1\right)$

解此方程组，可得：

β＝π-θ_e                    (2)

当x_e≠0时，可以得出以下方程组：

$\left\{\begin{array}{c}{\mathrm{\θ}}_e+2a=\mathrm{\β}\\ {\mathrm{\θ}}_e+a=\arctan \frac{y_e}{x_e}\end{array}\right.---\left(3\right)$

解此方程组可得：

$\mathrm{\β}=2\arctan \frac{y_e}{x_e}-{\mathrm{\θ}}_e---\left(4\right)$

机器人运动学特性微分方程如下：

$\left[\begin{array}{c}\stackrel{\·}{x}\\ \stackrel{\·}{y}\\ \stackrel{\·}{\mathrm{\θ}}\end{array}\right]=\left[\begin{array}{cc}\cos \mathrm{\θ}& 0\\ \sin \mathrm{\θ}& 0\\ 0& 1\end{array}\right]\left[\begin{array}{c}v\\ \mathrm{\ω}\end{array}\right]---\left(5\right)$

根据坐标变换公式，可得到在机器人坐标内描述的位姿误差为：

$P_e=\left[\begin{array}{c}{x}_e\\ y_e\\ {\mathrm{\θ}}_e\end{array}\right]=\left[\begin{array}{ccc}\cos \mathrm{\θ}& \sin \mathrm{\θ}& 0\\ -\sin \mathrm{\θ}& \cos \mathrm{\θ}& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_r-x\\ y_r-y\\ {\mathrm{\θ}}_r-\mathrm{\θ}\end{array}\right]---\left(6\right)$

对误差方程(6)微分，并联立(5)式，可得如下机器人位势误差为：

$\left[\begin{array}{c}{\stackrel{\·}{x}}_e\\ {\stackrel{\·}{y}}_e\\ {\stackrel{\·}{\mathrm{\θ}}}_e\end{array}\right]=\left[\begin{array}{c}{\mathrm{\ωy}}_e-v+v_r\cos \mathrm{\θ}\\ -{\mathrm{\ωx}}_e+v_r\sin {\mathrm{\θ}}_e\\ {\mathrm{\ω}}_r-\mathrm{\ω}\end{array}\right]---\left(7\right)$

针对机器人位姿误差模型，利用人工场控制方法设计控制律：

$\left\{\begin{array}{c}v=v_r\cos {\mathrm{\θ}}_e+k_x{x}_e\\ w={\mathrm{\ω}}_r+v_r{k}_{\mathrm{\θ}}{y}_e+v_r{k}_{\mathrm{\β}}\sin {\mathrm{\θ}}_e{\sin }^2\mathrm{\β}\end{array}\right.---\left(8\right)$

其中k_x，k_θ，k_β均为正控制参数，A为起始位姿，B为最终期望位姿，得出由位置A与位置B之间的期望人工电力线为ASB，其与直线AB之间夹角为弦切角α，β为A点运动方向与电力线夹角；θ_A和θ_B分别为机器人在A点和B点的方向角，x_e，y_e和θ_e为A，B两点的位姿误差。

与现有技术相比，本发明具有设计简单、可靠性高、通用性强、轨迹跟踪误差小。

附图说明

图1为本发明的控制流程图；

图2为本发明的硬件结构示意图；

图3为本发明电藕极子导向静电场示意图；

图4为本发明人工场计算示意图；

图5为本发明直线跟踪控制误差曲线图；

图6为本发明直线跟踪控制输入曲线图；

图7为本发明直线跟踪控制效果图；

图8为本发明圆跟踪控制误差曲线图；

图9为本发明圆跟踪控制输入曲线图；

图10为本发明圆跟踪控制效果图。

具体实施方式

下面结合附图和具体实施例对本发明进行详细说明。

实施例

如图1、图2所示，一种基于人工场的轨迹跟踪控制方法，包括以下步骤：

步骤101、开启运动控制平台，控制计算机1与机器人2连接，判断是否成功连接，若为是，执行步骤102，若为否，继续执行步骤101；

步骤102、机器人2对自身的状态进行初始化，并将起始位姿p₀发送给控制计算机1；

步骤103、控制计算机1根据设定的最终期望位姿p_r和起始位姿p₀，计算方向角控制律w＝f(w)和线速度控制律v＝f(v)，并将其发送给机器人2；

步骤104、机器人2根据方向角控制律w＝f(w)和线速度控制律v＝f(v)进行相应动作，动作完后，将最终实际位姿p发送给控制计算机1；

步骤105、控制计算机1比较最终期望位姿p_r与最终实际位姿p，得到位姿误差p_e，判断该位姿误差p_e是否小于设定值，若为是，认为该系统合格，若为否，调整方向角控制律w＝f(w)和线速度控制律v＝f(v)的正控制参数后返回步骤104。

系统的位姿可表示为：p＝[x，y，θ]^T，其中θ∈[-2π，2π]，并假定最终期望位姿p_r＝[0，0，0]^T。可以设想在期望位姿处沿x轴方向存在一电藕极子(设其正电荷在右侧)，则在坐标平面内可形成一个静电场，见图3，此即所建立的人工导向静电场。在所示坐标平面内，给定任意一点即可据该点处的电场力矢量求出其电力线方向。如果控制系统在y轴左侧沿电力线方向运动而在y轴右侧背电力线反向运动，在角度跟踪误差足够小的情况下系统必将沿着某条电力线运动到期望位姿。为计算方便可将每条电力线设计成一个过原点且与X轴相切的圆，此时电力线方向即圆的切线方向，而该方向即设定为系统当前期望的运动方向θ_r。人工场轮式移动机器人轨迹跟踪问题于是可描述为：给定系统坐标平面内任意起始位姿p₀和最终期望位姿p_r，寻求方向角控制律w＝f(w)和线速度控制律v＝f(v)，使系统在有限时间内沿着一条假想的电力线从p₀收敛到p_r。

本发明运动控制平台提供进行机器人实验的直观接口，实现了机器人与计算机之间较为友好的人机交互功能。运动控制平台建立在目前全世界最成熟的轮式移动机器人研究平台——Pioneer系列机器人之上。整个平台系统包括六个功能模块：用户操作管理模块，机器人通信模块，基本控制模块，运动控制模块，速度和角速度控制模块，显示管理模块。它们之间通过流经其间的数据信号和控制信号联系在一起，构成一个统一的整体。

平台系统选用Windows操作系统作为程序的开发和运行环境。使用C++语言在Visual Sudio 2008 t环境下进行编程，采用开放式源代码技术和面向对象编程技术的高性能接口套件——ARIA(ActivMedia Robotics Interface for Application)类库进行开发。ARIA封装了最底层的细节，包括串口通信、命令和服务器命令包处理、时钟周期、多线程，以及多种附件的控制，如激光测距仪、陀螺仪等等。基于ARIA套件，控制平台开发主实现了与机器人通信，机器人动作代码移植及补充等内容。

人工场的计算如图4所示，A为起始位姿，B为最终期望位姿，得出由位置A与位置B之间的期望人工电力线为ASB，其与直线AB之间夹角为弦切角α，β为A点运动方向与电力线夹角；θ_A和θ_B分别为机器人在A点和B点的方向角，x_e，y_e和θ_e为A，B两点的位姿误差。计算机器人方向角和引导方向之间的夹角β和θ_e之间关系。

当x_e＝0时，可以得出以下方程组：

$\left\{\begin{array}{c}{\mathrm{\θ}}_e+2a=\mathrm{\β}\\ {\mathrm{\θ}}_a+a=\frac{1}{2}\mathrm{\π}\end{array}\right.---\left(1\right)$

解此方程组，可得：

β＝π-θ_e                (2)

当x_e≠0时，可以得出以下方程组：

$\left\{\begin{array}{c}{\mathrm{\θ}}_e+2a=\mathrm{\β}\\ {\mathrm{\θ}}_e+a=\arctan \frac{y_e}{x_e}\end{array}\right.---\left(3\right)$

解此方程组可得：

$\mathrm{\β}=2\arctan \frac{y_e}{x_e}-{\mathrm{\θ}}_e---\left(4\right)$

机器人运动学特性微分方程如下：

$\left[\begin{array}{c}\stackrel{\·}{x}\\ \stackrel{\·}{y}\\ \stackrel{\·}{\mathrm{\θ}}\end{array}\right]=\left[\begin{array}{cc}\cos \mathrm{\θ}& 0\\ \sin \mathrm{\θ}& 0\\ 0& 1\end{array}\right]\left[\begin{array}{c}v\\ \mathrm{\ω}\end{array}\right]---\left(5\right)$

根据坐标变换公式，可得到在机器人坐标内描述的位姿误差为：

$P_e=\left[\begin{array}{c}{x}_e\\ y_e\\ {\mathrm{\θ}}_e\end{array}\right]=\left[\begin{array}{ccc}\cos \mathrm{\θ}& \sin \mathrm{\θ}& 0\\ -\sin \mathrm{\θ}& \cos \mathrm{\θ}& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_r-x\\ y_r-y\\ {\mathrm{\θ}}_r-\mathrm{\θ}\end{array}\right]---\left(6\right)$

对误差方程(6)微分，并联立(5)式，可得如下机器人位势误差为：

$\left[\begin{array}{c}{\stackrel{\·}{x}}_e\\ {\stackrel{\·}{y}}_e\\ {\stackrel{\·}{\mathrm{\θ}}}_e\end{array}\right]=\left[\begin{array}{c}{\mathrm{\ωy}}_e-v+v_r\cos \mathrm{\θ}\\ -{\mathrm{\ωx}}_e+v_r\sin {\mathrm{\θ}}_e\\ {\mathrm{\ω}}_r-\mathrm{\ω}\end{array}\right]---\left(7\right)$

针对机器人位姿误差模型，利用人工场控制方法设计控制律：

$\left\{\begin{array}{c}v=v_r\cos {\mathrm{\θ}}_e+k_x{x}_e\\ w={\mathrm{\ω}}_r+v_r{k}_{\mathrm{\θ}}{y}_e+v_r{k}_{\mathrm{\β}}\sin {\mathrm{\θ}}_e{\sin }^2\mathrm{\β}\end{array}\right.---\left(8\right)$

其中k_x，k_θ，k_β均为正控制参数。

下面对系统的稳定性进行分析。

按照李亚普诺夫存在性理论，对于一个全局一致渐近稳定的系统，存在一个非负的李亚普诺夫函数V(s)，其满足V(s)≥0，

V(s)＝0当且仅当s＝0，并且随着s→0，

为证明控制系统的稳定性，选取正定函数

$v=\frac{x_e^2}{2}+\frac{{y_e}^2}{2}+\frac{1-{\cos \mathrm{\θ}}_e}{k_{\mathrm{\θ}}}---\left(9\right)$

作为系统李亚谱诺夫函数，则其时间导数为

$\stackrel{\·}{V}=x_e{\stackrel{\·}{x}}_e+y_e{\stackrel{\·}{y}}_e+\frac{{\stackrel{\·}{\mathrm{\θ}}}_e\sin {\mathrm{\θ}}_e}{k_{\mathrm{\θ}}}$

结合轮式移动机器人位姿误差模型，得：

$\stackrel{\·}{V}=x_e(y_e\mathrm{\ω}-v+v_r\cos {\mathrm{\θ}}_e)+y_e(-\mathrm{\ω}{x}_e+v_r\sin {\mathrm{\θ}}_e)+\frac{{\sin \mathrm{\θ}}_e}{k_{\mathrm{\θ}}}({\mathrm{\ω}}_r-\mathrm{\ω})$

再由控制律公式，可得：

$\stackrel{\·}{V}=-k_x{x_e}^2-v_r\frac{k_{\mathrm{\β}}}{k_{\mathrm{\θ}}}{\sin }^2{\mathrm{\θ}}_e{\sin }^2\mathrm{\β}\≤0$

所以，闭环系统是稳定的。

使用MATLAB/SIMULINK软件，对本发明设计的轨迹跟踪控制律进行仿真，以验证控制律的有效性。仿真结果都通过仿真图形的方式给出，每种情况下的仿真结果都由三幅子图形组成。

预设初始时刻为零，对每一种期望轨迹情况，需要通过轨迹跟踪控制律仿真来给出的结果应包括下列内容：轨迹跟踪效果图、位姿误差变化曲线、线速度曲线和角速度曲线。其中，轨迹跟踪效果图由两条曲线组成，一条是非完整移动机器人从当前初始位姿跟踪到期望轨迹的几何曲线，由各个时刻代表移动机器人当前位姿的位置点连接而成，一条是期望轨迹曲线。位姿误差变化曲线是指全局坐标系下用直角坐标表示的轨迹跟踪位姿误差p_e对时间的变化关系，因为用p_e表示的位姿误差最直观，共有三条曲线组成，分别表示X方向位置误差变化、Y方向位置误差变化和θ方向角误差变化。线速度变化曲线由两条曲线组成，一是轨迹跟踪控制律中的线速度v_r控制输入对时间t的变化关系，一是参考线速度ω_r控制输入对时间t的变化关系。

首先对直线跟踪进行仿真，跟踪线速度和角速度均为匀速运动的直线轨迹。选v_r＝1.41，ω_r＝0，给定参考移动机器人的运动轨迹p_r＝(x_r，y_r，θ_r)^T为：

$\left\{\begin{array}{c}{x}_r=t\\ y_r=t\\ {\mathrm{\θ}}_r=\mathrm{\π}/4\end{array}\right.$

位姿初始误差为[1，1，0]，机器人初始位姿为[-1，-1，π]，参考轨迹起点为(0，0)，将控制律(8)式中的控制参数取为：k_x＝5，k_θ＝13，k_β＝3.5。采用控制律(8)可得移动机器人对直线轨迹跟踪的各种参数仿真结果如图5、6、7所示。

然后对圆轨迹跟踪进行仿真，跟踪线速度和角速度均为匀速运动的圆轨迹。选v_r＝2，ω_r＝1，给定参考移动机器人的运动轨迹p_r＝(x_r，y_r，θ_r)^T为：

$\left\{\begin{array}{c}{x}_r=r\cos t\\ y_r=r\sin t\\ {\mathrm{\θ}}_r=t\end{array}\right.$

其中r＝2，位姿初始误差为[1，-1，0]，机器人初始位姿为[2，1，pi/2]，将控制律(8)式中的控制参数取为：k_x＝5，k_θ＝13，k_β＝3.5。采用控制律(8)可得移动机器人对圆轨迹跟踪的各种参数仿真结果如图8、9、10所示。

Claims (2)

1.一种基于人工场的轨迹跟踪控制方法，其特征在于，包括以下步骤：

1)开启运动控制平台，控制计算机与机器人连接，判断是否成功连接，若为是，执行步骤2)，若为否，继续执行步骤1)；

2)机器人对自身的状态进行初始化，并将起始位姿p₀发送给控制计算机；

3)控制计算机根据设定的最终期望位姿p_r和起始位姿p₀，计算方向角控制律w＝f(w)和线速度控制律v＝f(v)，并将其发送给机器人；

4)机器人根据方向角控制律w＝f(w)和线速度控制律v＝f(v)进行相应动作，动作完后，将最终实际位姿p发送给控制计算机；

5)控制计算机比较最终期望位姿p_r与最终实际位姿p，得到位姿误差p_e，判断该位姿误差p_e是否小于设定值，若为是，认为该系统合格，若为否，调整方向角控制律w＝f(w)和线速度控制律v＝f(v)的正控制参数后返回步骤4)。

2.根据权利要求1所述的一种基于人工场的轨迹跟踪控制方法，其特征在于，所述的方向角控制律w＝f(w)和线速度控制律v＝f(v)的计算过程如下：

当x_e＝0时，可以得出以下方程组：

$\left\{\begin{array}{c}{\mathrm{\θ}}_e+2a=\mathrm{\β}\\ {\mathrm{\θ}}_a+a=\frac{1}{2}\mathrm{\π}\end{array}\right.---\left(1\right)$

解此方程组，可得：

β＝π-θ_e                                        (2)

当x_e≠0时，可以得出以下方程组：

$\left\{\begin{array}{c}{\mathrm{\θ}}_e+2a=\mathrm{\β}\\ {\mathrm{\θ}}_e+a=\arctan \frac{y_e}{x_e}\end{array}\right.---\left(3\right)$

解此方程组可得：

$\mathrm{\β}=2\arctan \frac{y_e}{x_e}-{\mathrm{\θ}}_e---\left(4\right)$

机器人运动学特性微分方程如下：

$\left[\begin{array}{c}\stackrel{\·}{x}\\ \stackrel{\·}{y}\\ \stackrel{\·}{\mathrm{\θ}}\end{array}\right]=\left[\begin{array}{cc}\cos \mathrm{\θ}& 0\\ \sin \mathrm{\θ}& 0\\ 0& 1\end{array}\right]\left[\begin{array}{c}v\\ \mathrm{\ω}\end{array}\right]---\left(5\right)$

根据坐标变换公式，可得到在机器人坐标内描述的位姿误差为：

$P_e=\left[\begin{array}{c}{x}_e\\ y_e\\ {\mathrm{\θ}}_e\end{array}\right]=\left[\begin{array}{ccc}\cos \mathrm{\θ}& \sin \mathrm{\θ}& 0\\ -\sin \mathrm{\θ}& \cos \mathrm{\θ}& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_r-x\\ y_r-y\\ {\mathrm{\θ}}_r-\mathrm{\θ}\end{array}\right]---\left(6\right)$

对误差方程(6)微分，并联立(5)式，可得如下机器人位势误差为：

$\left[\begin{array}{c}{\stackrel{\·}{x}}_e\\ {\stackrel{\·}{y}}_e\\ {\stackrel{\·}{\mathrm{\θ}}}_e\end{array}\right]=\left[\begin{array}{c}{\mathrm{\ωy}}_e-v+v_r\cos \mathrm{\θ}\\ -{\mathrm{\ωx}}_e+v_r\sin {\mathrm{\θ}}_e\\ {\mathrm{\ω}}_r-\mathrm{\ω}\end{array}\right]---\left(7\right)$

针对机器人位姿误差模型，利用人工场控制方法设计控制律：

$\left\{\begin{array}{c}v=v_r\cos {\mathrm{\θ}}_e+k_x{x}_e\\ w={\mathrm{\ω}}_r+v_r{k}_{\mathrm{\θ}}{y}_e+v_r{k}_{\mathrm{\β}}\sin {\mathrm{\θ}}_e{\sin }^2\mathrm{\β}\end{array}\right.---\left(8\right)$

其中k_x，k_θ，k_β均为正控制参数，A为起始位姿，B为最终期望位姿，得出由位置A与位置B之间的期望人工电力线为ASB，其与直线AB之间夹角为弦切角α，β为A点运动方向与电力线夹角；θ_A和θ_B分别为机器人在A点和B点的方向角，x_e，y_e和θ_e为A，B两点的位姿误差。

Images