CN107092266B - 一种移动车轨迹跟踪控制方法 - Google Patents

Abstract

本发明提供的一种移动车轨迹跟踪控制方法，包括如下步骤：S1、获取移动车的实际位置、姿态信息；S2、预先定义的期望轨迹中的所有位置点坐标，确定期望轨迹中距离移动车的实际位置最近的位置点；S3、根据最近的位置点坐标，以及预设区间范围ε，以及移动车的实际位置信息，确定移动车的虚拟牵引点；S4、确定移动车到达虚拟牵引点的期望姿态信息；S5、在期望轨迹中设定期望动点；S6、根据期望动点、虚拟牵引点的位置信息，获取移动车的期望角速度；S7、将期望姿态信息和期望角速度发送移动车，以使移动车根据期望姿态信息和期望角速度运动。本发明方法运算效率更高，运算速度更快；本发明设计的方法能够实时保证移动车跟踪虚拟牵引点，具有较好的控制准确性与控制精度。

Description

一种移动车轨迹跟踪控制方法

技术领域

本发明涉及移动车的控制技术领域，具体涉及一种移动车轨迹跟踪控制方法。

背景技术

近几年，移动车机器人发展迅速，由于其能够在复杂的环境中自主导航，已经在军用和民用上得到广泛应用。其能够在动态的环境中携带物品代替人类完成复杂的任务，如送快递、送货、自动驾驶等，因此具有广泛的应用前景。

典型的移动车具有四个轮子，呈平行两排结构，通过一个电机和一个舵机分别控制移动车的速度和方向，电机通过差速器来控制两个轮子，从而实现转速的控制。移动车通过控制电机和舵机来实现转弯，直线运动的效果。

目前，移动车的控制大都与路径规划相关，可是现有的路径规划算法都存在比较复杂，造成运算量大，效率低的技术缺陷。而在实际中，移动车运动是一个实时运动，四周的环境每时每刻都在发生变化，这对移动车轨迹跟踪算法的实时性要求很高。如果移动车轨迹跟踪算法的实时性不好，很容易由于算法处理信息不及时造成不必要的事故。

发明内容

(一)要解决的技术问题

为了解决现有技术的上述问题，本发明提供一种移动车轨迹跟踪控制方法，该方法设计简单，运算速度快，控制精度高，能有效提高移动车轨迹跟踪控制的实时性与准确性。

(二)技术方案

为了达到上述目的，本发明采用的主要技术方案包括：

一种移动车轨迹跟踪控制方法，其包括如下步骤：

S1、根据定位系统中移动车的信息，获取移动车的实际位置信息和实际姿态信息；

S2、根据预先定义的期望轨迹中的所有位置点坐标，以及所述移动车的实际位置信息，确定所述期望轨迹中距离所述移动车的实际位置最近的位置点；

S3、根据步骤S2中查找的最近的位置点坐标，以及预设区间范围ε，以及所述移动车的实际位置信息，确定移动车的虚拟牵引点，该虚拟牵引点为期望所述移动车相对于当前位置到达的第一个位置信息；

S4、根据所述移动车的实际姿态信息和虚拟牵引点，确定所述移动车到达所述虚拟牵引点的期望姿态信息；

S5、在期望轨迹中将步骤S2中最近的位置点相邻的，且与移动车运动方向对应的下一个位置点作为期望动点；

S6、根据期望动点、虚拟牵引点的位置信息，获取移动车的期望角速度；

S7、将所述期望姿态信息和期望角速度发送所述移动车，以使所述移动车根据所述期望姿态信息和期望角速度运动。

进一步，所述步骤S1中的位置信息包括：后轮车轴中点的坐标为(x,y)，所述步骤S1中的姿态信息包括：移动车方向角为θ，所述步骤S4中的期望姿态信息包括：期望转向角

如上所述的方法，优选地，所述步骤S2中的期望轨迹包括n个位置点的位置点序列S，每个位置点表示为a_i，a_i的位置为(x_i,y_i)，所述S表示为S＝{a₁,a₂,a₃,...,a_n}，其中N⁺为正整数。

如上所述的方法，优选地，步骤S2包括：

获取位置点序列S中任一点a_i与当前移动车中后轮车轴中点(x,y)的距离d；

确定距离d最近的属于位置点序列S中的位置点a_c(x_c,y_c)；

其中，

如上所述的方法，优选地，步骤S3包括：

在位置点a_c点附近设置ε区间范围，c2为所述S中离移动车后轮车轴中点(x,y)的距离小于等于d+ε的一个边界点，c1为S中离移动车后轮车轴中点(x,y)的距离小于等于d+ε的另一个边界点，将S中在点a_c1与点a_c2之间的所有点记为虚拟牵引点集C，根据公式||(x,y)-(x_c2,y_c2)||＜＝d+ε＜＝||(x,y)-(x_c2+1,y_c2+1)||，c2∈n，c2+1∈n和||(x,y)-(x_c1,y_c1)||＜＝d+ε＜＝||(x,y)-(x_c1-1,y_c1-1)||，c1∈n,c1-1∈n，确定虚拟牵引点a_r(x_r，y_r)，其中所述ε的取值范围为0～10。

如上所述的方法，优选地，所述步骤S4包括：

根据虚拟牵引点a_r(x_r，y_r)、移动车后轮车轴中点的坐标(x,y)，通过获取移动车当前位置的期望方向角θ_r；

根据移动车当前的方向角θ、期望方向角θ_r，通过获取移动车当前位置的前轮期望转向角该期望转向角也就是步骤S7中所述的期望姿态信息。

如上所述的方法，优选地，在所述步骤S6中，按如下步骤进行：

S601、在虚拟牵引点集C中查找与期望动点a_m最近的牵引点a_t作为速度虚拟牵引点a_t；

所述期望动点a_m(x_m,y_m)为位置点序列S中的位置点；

S602、根据所述期望动点a_m求导获得期望动点的速度v_m，即所述v_m的单位向量为则

S603、所述速度虚拟牵引点a_t与所述期望动点a_m之间的距离为L，通过公式L＝||(x_t,y_t)-(x_m,y_m)||获得，并根据公式获得所述移动车当前位置的期望角速度ω，其中r为所述移动车后车轮半径。

如上所述的方法，优选地，所述k的取值为v_m与L的比值的倍数的0到2倍。

如上所述的方法，优选地，在所述步骤S7中，根据获得的ω控制所述移动车x方向的期望速度为y方向的期望速度为车头方向的期望角速度为

如上所述的方法，优选地，所述方法还包括：

重复上述步骤S1至步骤S7，以使所述移动车完成预设期望轨迹的运动；

在重复上述步骤S5的过程中，采用所述期望动点的下标每隔设定的时间自动加1，使期望动点a_m按位置点序列S中的点序列移动替换所述步骤S5。

(三)有益效果

本发明的有益效果是：

本发明提供的移动车轨迹跟踪算法，将连续路径简化为一组位置点-序列，简化了期望轨迹的描述方法，在该种描述期望轨迹的新型方法上，提出了新的路径跟踪算法与速度跟踪算法。与现有的轨迹跟踪方法相比，在实时性方面，由于本发明所述算法将路径离散化，简化了路径描述，所以本发明提供的移动车轨迹跟踪控制方法未用到过多复杂的数学运算，因此本发明算法运算效率更高，运算速度更快。

在控制准确性与控制精度方面，由于本发明的路径跟踪和速度跟踪均使用了虚拟牵引点这一新的理念，和生活中的挂车一样，虚拟牵引点相当于挂车的车头，移动车相当于挂车的车尾，本发明设计的控制方法能够实时保证移动车跟踪虚拟牵引点，因此，本发明的方法可以保证较好的控制准确性与控制精度。

附图说明

图1为本发明实施例提供的方法步骤示意图；

图2为本发明实施例提供的移动车建模示意图；

图3为本发明实施例提供的期望轨迹生成示意图；

图4为本发明实施例提供的路径跟踪算法与速度跟踪算法示意图；

图5为本发明实施例提供的物理实验期望轨迹与实际轨迹图；

图6为本发明实施例提供的物理实验期望轨迹与实际轨迹的位置差图；

图7为本发明实施例提供的物理实验期望角度与实际角度图；

图8为本发明实施例提供的物理实验期望角度与实际角度的角度差图；

图9为本发明实施例提供的物理实验期望速度与实际速度图；

图10为本发明实施例提供的物理实验期望速度与实际速度的速度差图；

图11为本发明实施例提供的物理实验期望点位置与车实际位置的位置差图。

具体实施方式

为了更好的解释本发明，以便于理解，下面结合附图，通过具体实施方式，对本发明作详细描述。

实施例1

一种移动车轨迹跟踪控制方法，如图1所示，其包括下述步骤，

S1、根据移动车上定位系统的信息，获取移动车的实际位置信息和实际姿态信息；

举例来说，本实施例中的实际位置信息可为后轮车轴中点的坐标为(x,y)、实际姿态信息可为移动车方向角为θ。

S2、根据预先定义的期望轨迹中的所有位置点坐标，以及所述移动车的实际位置信息，确定所述期望轨迹中距离所述移动车的实际位置最近的位置点；

举例来说，期望轨迹可表示为n个位置点的位置点序列S，位置点坐标表示为a_i(x_i,y_i)，

实际位置最近的位置点可表示为：S表示为S＝{a₁,a₂,a₃,...,a_n}，n∈N⁺，其中N⁺为正整数；

获取位置点序列S中任一点a_i与当前移动车中后轮车轴中点(x,y)的距离d；

确定距离d最近的属于位置点序列S中的位置点a_c(x_c,y_c)；

其中，

S3、根据步骤S2中查找的最近的位置点坐标，以及预设区间范围ε，以及所述移动车的实际位置信息，确定移动车的虚拟牵引点，该虚拟牵引点为期望所述移动车相对于当前位置到达的第一个位置信息；

举例来说，虚拟牵引点通过如下步骤确定：在位置点a_c点附近设置ε区间范围，c2为所述S中离移动车后轮车轴中点(x,y)的距离小于等于d+ε的一个边界点，c1为S中离移动车后轮车轴中点(x,y)的距离小于等于d+ε的另一个边界点，将S中在点a_c1与点a_c2之间的所有点记为虚拟牵引点集C，根据公式||(x,y)-(x_c2,y_c2)||＜＝d+ε＜＝||(x,y)-(x_c2+1,y_c2+1)||，c2∈n，c2+1∈n和||(x,y)-(x_c1,y_c1)||＜＝d+ε＜＝||(x,y)-(x_c1-1,y_c1-1)||，c1∈n,c1-1∈n，确定虚拟牵引点a_r(x_r，y_r)，其中所述ε的取值范围为0～10。

S4、根据所述移动车的实际姿态信息和虚拟牵引点，确定所述移动车到达所述虚拟牵引点的期望姿态信息；

举例来说，期望姿态信息包括：期望方向角θ_r和期望转向角

根据移动车的实际姿态信息即后轮车轴中点的坐标(x,y)和虚拟牵引点a_r(x_r，y_r)，通过获取移动车当前位置的期望方向角θ_r；根据移动车当前的方向角θ、期望方向角θ_r，通过获取移动车当前位置的前轮期望转向角

S5、在期望轨迹中将步骤S3中最近的位置点相邻的，且与移动车运动方向对应的下一个位置点作为期望动点；期望动点记为a_m。

S6、根据期望动点、虚拟牵引点的位置信息，获取移动车的期望角速度；

举例来说，该步骤S6具体包括图中未示出的子步骤：

S601、在虚拟牵引点集C中查找与期望动点a_m最近的牵引点a_t作为速度虚拟牵引点a_t；

所述期望动点a_m(x_m,y_m)为位置点序列S中的位置点；

S602、根据所述期望动点a_m求导获得期望动点的速度v_m，即所述v_m的单位向量为则

S603、所述速度虚拟牵引点a_t与所述期望动点a_m之间的距离为L，通过公式L＝||(x_t,y_t)-(x_m,y_m)||获得，并根据公式获得所述移动车当前位置的期望角速度ω，k的取值为v_m与L的比值的倍数的0到2倍。

S7、将所述期望姿态信息和期望角速度发送所述移动车，以使所述移动车根据所述期望姿态信息和期望角速度运动。

举例来说，期望姿态信息包括x方向的期望速度和y方向的期望速度及期望转向角根据获得的ω控制所述移动车x方向的期望速度为y方向的期望速度为车头方向的期望角速度为

在实际应用中，可重复上述的步骤S1至步骤S7等，由此可实现以使所述移动车完成预设期望轨迹的运动；

其中，在重复上述步骤S5的过程中，采用所述期望动点的下标每隔设定的时间自动加1，使期望动点a_m按位置点序列S中的点序列移动替换所述步骤S5。

实施例2

本实施例是对实施例1中一种移动车轨迹跟踪控制方法，更进一步的描述，具体包括以下步骤：

步骤1、首先对移动车获取实际位置信息和实际姿态信息：

为了分析方便，定义l为移动车前后轴之间的距离，(x,y)为移动车后轮车轴中点的坐标，为x的导数，该导数为x方向的速度，为y的导数，该导数为y方向的速度，r为后轮的半径，ω为后轮的角速度，θ为移动车方向角，该方向角是车头与大地坐标系下的x轴正方向的夹角，为θ的导数，该导数为θ的角速度，为前轮转向角(即上述实施例1中的期望转向角)，该转向角是前轮朝向与移动车方向的夹角；则有公式：

其中，公式(1)用于描述后轮角速度ω与x方向的速度之间的关系，公式(2)用于描述后轮角速度ω与y方向的速度之间的关系，公式(3)用于描述θ的角速度与后轮角速度ω和前轮转向角之间的关系，公式(1)(2)(3)表明，已知期望的即可求出ω和下面分别给出了求解和ω的方法，求出和ω后即可控制移动车按期望的轨迹运动。

步骤2、根据预先定义期望轨迹，确定该期望轨迹中的所有位置点坐标，即对移动车的运动路径进行描述：本实施例使用一组位置点-序列来描述移动车的期望运动路径：将移动车的期望运动路径中的每个点用a_i来表示，其位置为(x_i,y_i)，每个点组成的序列即为期望运动路径S序列，即

S＝{a₁,a₂,a₃,...,a_n}，n∈N⁺ (4)

a_i＝(x_i,y_i)，i∈n (5)

具体地，S＝{a₁,a₂,a₃,...,a_n}，n∈N⁺为正整数，S中的每个点a_i，i∈n，a_i的位置为(x_i,y_i)，公式(4)表示将原有连续路径用n个序列点代替，公式(5)表示每个序列点都有对应的坐标，a_i的下标i,i∈n，表示a_i可以代表S中的任一点，a_i的坐标为(x_i,y_i)表示a_i的横坐标为x_i，a_i的纵坐标为y_i，x_i表示该点在大地坐标系下的x轴方向的坐标，y_i表示该点在大地坐标系下的y方向的坐标；

步骤3、路径跟踪：

应说明的是，本实施例中的路径跟踪考虑了路径的跟踪问题，不考虑速度的跟踪问题，首先，速度，也就是上述所说的是给定的，也就是ω不是根据公式计算的，是根据使用者期望的运动情况人为给定的，这个给定可以提前给定好，也可以在移动车实际运行时根据情况在线实时给定，路径跟踪算法考虑的是路径跟踪问题，也就是当移动车按使用者给定的速度运动时，移动车能够沿着期望的路径行走，具体包括如下步骤：

步骤3.1、寻找位置最近点，采用d表示S中距离当前位置的移动车后轮车轴中点的最近距离，如用公式(6)表示，最近的点用a_c表示，则

d＝||(x,y)-(x_c,y_c)|| (7)

公式(6)x,y分别为当前位置的移动车后轮车轴中点的横坐标和纵坐标，x_i,y_i分别为a_i的横坐标和纵坐标，min表示取最小，由于a_i可以代表S中的任一点，公式(7)中，|| ||表示取范数，(x_c,y_c)表示点a_c的坐标，点a_c是S中的一个点，公式(7)表示，点a_c距离移动车后轮车轴中点的距离为d，因此点a_c就是S中距离移动车后轮车轴中点最近点；

步骤3.2、寻找虚拟牵引点集合，在a_c点附近设置ε区间范围，c2为S中距离移动车后轮车轴中点(x,y)的距离小于等于d+ε的一个边界点，如公式(8)表示；c1为S中距离移动车后轮车轴中点(x,y)的距离小于等于d+ε的另一个边界点；如公式(9)表示；原公式(6)即可表示为公式(10)所示，公式(7)即可表示如公式(11)所示，虚拟牵引点集C表示一个集合，该集合为S中在点a_c1与点a_c2之间的所有点，用如下公式(12)表示，

||(x,y)-(x_c2,y_c2)||＜＝d+ε＜＝||(x,y)-(x_c2+1,y_c2+1)||，c2∈n，c2+1∈n (10)

||(x,y)-(x_c1,y_c1)||＜＝d+ε＜＝||(x,y)-(x_c1-1,y_c1-1)||，c1∈n,c1-1∈n (11)

C＝{a_c1,...,a_c2} (12)

其中，公式(8)(9)中，c,c1,c2,k1,k2均为正整数，c,c1,c2均可表示S中a的下标，n为S中的最大下标，在公式(8)中，若c+k1小于等于n，则c2等于c+k1，若c+k1大于n，则c2等于c+k1-n，在公式(9)中，若c-k2大于等于S中的最小下标1，则c1等于c-k2，若c-k2小于1，则c1等于c-k2+n，公式(8)用于表示c1为从c开始往上增加的整数，且c1在S的下标内，公式(9)用于表示c2为从c开始往下减小的整数，且c2在S的下标内；

公式(10)(11)中，d与公式(6)中的d等效，ε的取值范围为0～10，c1,c2,c1-1,c2+1均可表示S中a的下标，公式(10)表示公式(12)中，C表示一个集合，该集合为S中在点a_c1与点a_c2之间的所有点，该集合就是要寻找的虚拟牵引点集合。

步骤3.3、确定路径虚拟牵引点，为便于下面步骤的说明，用a_r代替a_c2表示要确定的路径虚拟牵引点，r∈n；如公式(13)所示，

a_r＝a_c2 (13)

公式(13)中，c2与公式(12)中的c2等效，其中r与c2相等，所以a_r与a_c2表示同一个点；

步骤3.4、前轮转向角计算，θ_r为移动车在当前位置的期望方向角，如公式(14)所示，

其中，公式(14)中，x,y与公式(6)中的x,y等效，x,y分别为移动车后轮车轴中点的横坐标和纵坐标，x_r,y_r为公式(13)中a_r点的横坐标和纵坐标，公式(14)用于求移动车在当前位置的期望方向角；

公式(15)中，θ与公式(1)中的θ等效，为移动车方向角，与公式(3)中的等效，为移动车前轮转向角，该转向角就是要求的前轮转向角；重复该部分即可获得移动车路径的跟踪。

步骤4、速度跟踪：

速度跟踪用来使移动车在期望的轨迹上按期望的速度运动，为使移动车在期望轨迹上运动的计算按路径跟踪算法所述执行，速度跟踪算法主要关注移动车能够跟上期望的速度，也就是第1部分中的ω，ω是按速度跟踪算法的公式计算来的，从公式(1)，(2)可知，获得期望的ω后就可以控制移动车按期望的速度运动。具体步骤如下：

步骤4.1、设置期望动点，将该期望动点作为所述移动车下一时刻的目标位置，令m为期望动点的下标，则m可表示为如下几种情况：

m＝m1 (16)

m＝m+1 (17)

公式(16)中，m1为S中任一点的下标，即m1是整数且m1大于等于1小于等于n，公式(17)用于给期望动点的下标赋初值；

公式(17)中，m为期望动点的下标，公式(17)用于表示期望动点的下标每隔设定的时间(Δt)自动加1，通过这种方法可使动点a_m按S中的点序列移动，通过调节(Δt)的大小可以实时调节动点a_m的速度；

公式(18)中，m为期望动点的下标，n为S中点的最大下标，公式(18)用于表示m一直在S中点的下标范围内，即m为正整数且m大于等于1小于等于n，经过公式(18)求得的m即为期望的动点下标，即a_m为要设置的期望动点；

步骤4.2、寻找速度虚拟牵引点，令J为公式(12)中C集合中点下标的集合，用公式(19)表示；j为集合J中的任一点，num表示集合J中的点距离m的最小数，用公式(20)表示，t即为集合J中距离m最近的点，用公式(21)表示，即t为要求的速度虚拟牵引点的下标，a_t则为要求的速度跟踪算法中的速度虚拟牵引点，

J＝{c1,…,c2} (19)

num＝min(|m-j|)，j∈J (20)

num＝|m-t|,t∈J (21)

其中，在公式(19)中，J中的每一个点都和C中每个点的下标对应相等；

公式(20)中，m为公式(18)中求出的期望动点的下标，j为集合J中的任一点，num表示集合J中的点距离m的最小数；

公式(21)中，t为集合J中的任一点，t为要求的速度虚拟牵引点的下标，a_t(x_t,y_t)与公式(13)中的a_r意义一样，只是公式(13)中的a_r是路径跟踪算法中的虚拟牵引点，公式(21)中的t是速度跟踪算法中的虚拟牵引点的下标；

步骤4.3、计算期望动点速度，a_m是设置的目标位置的期望动点，该动点在平面上对应的坐标为(x_m,y_m)，对a_m求导就是对(x_m,y_m)求导，可得对应的是(v_xm,v_ym)，也就是v_m，则v_m表示期望动点的期望速度，用公式(22)表示，

公式(22)中，表示点a_m求导，即为点a_m的速度，a_m的下标m与公式(18)中的m相等，公式(22)用于求期望动点a_m的期望速度；

步骤4.4计算期望角速度：L表示为速度虚拟牵引点与期望动点位置之间的距离，用公式(23)表示，也可以写为所述v_m的单位向量为如公式(24)所示，用于计算所述移动车当前位置的期望角速度如公式(25)所示；

L＝||(x_t,y_t)-(x_m,y_m)|| (23)

其中，公式(23)中，(x_t,y_t)为速度虚拟牵引点的横坐标和纵坐标，(x_m,y_m)为期望动点a_m的横坐标和纵坐标，|| ||表示取范数，即L为点(x_t,y_t)与点(x_m,y_m)的距离；

公式(24)中，|| ||表示取范数，v_m与公式(15)中的v_m相等，为v_m的单位向量，公式(24)用于求v_m的单位向量；

公式(25)中，k的含义是一个比例系数，用于调节收敛速度的，k越大则L对ω所起的影响越大，移动车实际位置越快趋近期望动点位置，反之越慢，但是并不是k越大越好，要根据实际车自身的性能进行调试，k的取值为v_m与L的比值的倍数，k取该倍数的0到2倍，r与公式(1)中r一样，表示移动车的后轮半径，ω与公式(1)中的ω一样，表示移动车后轮的角速度，公式(25)用于计算移动车在当前位置的期望角速度；得到移动车的期望控制量输入，即前轮的期望转角和后轮的期望角速度ω，从第1部分中的式(1)、(2)、(3)可知，期望的和ω可控制移动车按期望的轨迹和期望的速度运动。

本发明提供的一种新颖的移动车轨迹跟踪方法，该方法设计简单，运算速度快，控制精度高，能有效提高移动车轨迹跟踪控制的实时性与准确性，解决了现有路径规划算法，造成路径规划运算量大、效率低的技术缺陷问题。

实施例3

为了验证本发明实施例1所述移动车轨迹跟踪算法的有效性，针对本发明所述算法，进行了物理实验，实验步骤为：

A)使用optitrack室内光学定位系统获取移动车的位置和姿态信息，移动车的位置对应公式(6)中的x,y，移动车的姿态也就是移动车的方向角对应公式(1)中的θ；

B)将x,y,θ传给matlab，并在matlab上编写本法明所述的轨迹跟踪算法，经matlab计算后得到期望的前轮的转向角度和期望后轮角速度，其中期望的前轮的转向角度对应公式(15)中的期望后轮角速度对应公式(25)中的ω；

C)将ω通过无线传输发送给移动车，进而控制移动车按期望的轨迹运动，本实验所用的无线传输为xbee，所用的移动车为美国freescale公司生产的飞思卡尔竞赛专用B车模。

下面结合附图，对本发明实施例1中所述方法作进一步详细描述。

具体地，如图2所示，为本实施例的移动车建模示意图，l为移动车前后轴之间的距离为0.2m，(x,y)为后轮车轴中点的坐标，从图4可知(x,y)为(0.7，1.1)，r为后轮的半径为0.028m，ω为移动车后轮的角速度，图2中为了不影响其他变量的表示没有画出ω，本次试验ω等于25弧度每秒，θ为移动车方向角，该方向角是车头与大地坐标系下的x轴正方向的夹角；

本次试验移动车按8字形轨迹运动，如图3所示，因此θ在正负π之间来回变化，为前轮转向角，该转向角是前轮朝向与θ的夹角，试验中未记录随时间的变化，因为主要考虑的是期望转角是否计算正确，从公式(15)可知，期望转角的计算情况可以通过记录θ角的跟踪情况来间接观测，图2中各变量之间的关系与实施例2中公式(1)(2)(3)中描述的关系一致。

本发明的移动车轨迹跟踪算法可以跟踪平面内任意轨迹，这里为了描述方便，将移动车跟踪的期望轨迹设定为两个相切的圆心分别在(-1，0)与(1，0)的半径为1m的圆，这里简称为8字形轨迹。然后从左往右，从上往下将该期望轨迹用一组位置点-序列代替，所使用的单位为m。

如图3所示，为本发明的期望轨迹生成示意图，将期望轨迹用一组位置点-序列代替，其中起点从(1)开始，按图中箭头指示的方向依次经过第二象限(Ⅱ)，第四象限(Ⅳ)，第一象限(Ⅰ)，第三象限(Ⅲ)，最终回到终点(2)，该序列点为一组封闭的位置点-序列，该种新型的描述期望轨迹的方法将连续轨迹离散化，简化了控制的复杂性，本次实验用320个点来描述该8字形轨迹，也就是公式(4)中S的最大下标n等于320，这320个点如图3所示均匀分布在8字形轨迹上，对应公式(4)(5)；

然后设计路径跟踪算法：

如图4所示，为本发明的路径跟踪算法示意图，B代表移动车的实际位置也就是公式(6)中的(x,y)，其中，图4中B的位置为(0.7，1.1)，E代表位置点-序列S中距离B最近的点也就是公式(7)中的(x_c,y_c)，图4中E点在S中对应的点a_c的下标c等于205，d为B与E之间的距离，图中d等于0.2米，d+ε对应公式(10)(11)中的d+ε，C点对应公式(12)中的a_c2，D点对应公式(12)中的a_c1，图中d+ε等于0.24米，图4中C点对应的a_c2的下标c2等于209，D点对应的a_c1的下标c1等于201。

结合图4，路径跟踪算法的具体实施方式为：

具体地，寻找位置最近点，图4中，E点代表位置点-序列S中距离B最近的点，为在S中寻找到E点，可首先通过optitrack室内光学定位系统确定B点，图4中B的位置为(0.7，1.1)，然后求S中的所有点与B的距离，比较这些距离的大小，找到这些距离中的最小距离对应的点，该点就是要求的位置最近点E，对应公式(6)(7)；

首先，寻找路径虚拟牵引点集合，图4中，寻找路径虚拟牵引点集合就是寻找集合的边界点C和D，寻找集合的边界点时，首先在S中找到所有与B的距离小于等于d+ε的点，按点的下标给这些点排序，找到最大下标对应的点和最小下标对应的点，然后判断最大下标对应的点和最小下标对应的点哪个是B前进方向的点，如果最大下标对应的点是B前进方向的点，则最大下标对应的点是C，反之最小下标对应的点是C，确定完C后，另一个点就是D，图4中最大下标对应的点是C，且该点的下标c2等于209，最小下标对应的点是D，且该点的下标c1等于201，当S的点数固定时，d+ε决定着虚拟牵引点集合的大小，减小d+ε时，路径虚拟牵引点集合减小，反之增大。图4中d等于0.2米，d+ε等于0.24米，c2和c1的下标正是基于d+ε等于0.24米算出的，对应公式(8)(9)(10)(11)(12)；

其次，确定路径虚拟牵引点，图4中，通过判断C点和D点哪个是前进方向的点，对应公式(13)来确定路径虚拟牵引点，图4中C点为前进方向的点，所以路径虚拟牵引点是C；

最后，根据移动车实际朝向角θ和期望的朝向角计算获得期望转角即期望姿态信息；

具体地，图4中，通过计算B点与原点(0,0)的连线与x轴正方向的角度可得移动车的实际角度θ，对应公式(14)通过计算B点与C点的连线与x轴正方向的角度来确定期望角度θ_r，对应公式(15)θ_r减去θ得到期望的前轮转向角

接下来，首先设置期望的动点，相当于现实生活中的挂车车头，动点的作用在于为移动车提供期望的运动速度和期望的跟踪方向，然后根据确定的速度虚拟牵引点，然后计算动点的速度，最后计算期望的角速度；

如图4所示，A代表期望的动点也就是公式(18)中的m作为下标时a_m点，图4中m为227，L为A点和C点之间的距离，对应公式(23)中的L，图4中L为0.6922米，B、C、D、E、d与路径跟踪算法中的B、C、D、E、d一致；

结合图4，速度跟踪算法的具体实施为：

1)设置期望动点，图4中，A点为设置的期望动点，A的设置方式是，首先在S序列点中按需求找一个初始点，对应公式(16)中该初始点下标为m1，然后按S中序列点的下标顺序周期性的将A点代表的序列点下标加1，即对应公式(17)；如果A点下标大于S序列点的最大下标n，对应公式(18)A点的下标重新赋值为当前下标减去n之后的值；

2)确定速度虚拟牵引点，图4中，C点为速度虚拟牵引点，寻找C点，首先按路径跟踪算法中的方法找到C点和D点，对应公式(19)找到C点对应的a_c2的下标c2与D点对应的a_c1的下标c1，然后找到A点对应的a_m的下标m，对应公式(20)最后判断c2与m的差值的绝对值是否比c1与m的差值的绝对值大，对应公式(21)如果大于，则速度虚拟牵引点为D点，如果小于，则速度虚拟牵引点为C点，图4中，是小于的情况，所以C点为速度虚拟牵引点；

3)计算期望动点速度，图4中，A点是期望的动点，计算期望动点的速度就是计算A点的速度，本发明所述计算A点的速度就是对A求导，对应公式(22)得到A的速度v_m，本次试验v_m为0.7米/秒；

4)计算期望角速度，图4中，期望角速度ω是移动车后轮的角速度，期望角速度与移动车后轮半径r的乘积就是移动车的速度，也就是图4中B点的速度，为求ω，首先计算A点的速度v_m，本次实验v_m为0.7米/秒，然后对应公式(24)计算v_m的方向向量最后确定比例系数k，本次实验k为0.6，对应公式(23)，然后计算A点与C点之间的距离L，图4中L为0.6922米，最后对应公式(25)计算期望的角速度ω，图4中ω为39.8328弧度/秒。

为进一步说明本发明方法的有效性，下面结合实验数据图做具体分析：

如图5所示，为本发明实施例提供的物理实验期望轨迹与实际轨迹图，图中移动车的实际轨迹用点表示，车的期望轨迹用线表示，图5中移动车起点并不在期望轨迹上，但是很快，移动车的实际轨迹就收敛在期望轨迹附近，本发明所述算法也可以很好的辨识，并让移动车沿期望轨迹运动。

如图6所示，为本实施例提供的物理实验期望轨迹与实际轨迹的位置差图，从图中可见本发明的位置误差在-0.01m到0.02m之间，说明本发明有较高的控制精度。

如图7所示，为本实施例提供的物理实验期望角度与实际角度图，图7中，本发明所述方法能够使实际角度实时跟踪期望角度，两者之间的静态误差较小。

如图8所示，为本实施例提供的物理实验期望角度与实际角度的角度差图，如图8所示，期望角度与实际角度在稳态时，误差维持在-0.62到0.62弧度之间，在过渡阶段，本发明所述方法能够保证快速跟踪，因此本发明所述方法具有较好的角度跟踪性能。

如图9所示，为本实施例提供的物理实验期望速度与实际速度图，从图中可以看出实际速度在期望速度附近波动，因此本发明所述方法具有较好的速度跟踪性能。

如图10所示，为本实施例提供的物理实验期望速度与实际速度的速度差图，从图中可见，速度跟踪的误差在-0.1到0.1m/s之间，说明本发明所述方法具有较好的速度跟踪性能。

如图11所示，为本实施例提供的物理实验期望点位置与移动车实际位置的位置差图，从图中可见，稳态时期望点位置与移动车实际位置的位置差在-0.08到0m之间，说明本发明所述方法能够较好跟踪期望点位置。

本发明提供一种移动车轨迹跟踪控制方法，首先使用定位系统，根据移动车的实际位置，获取移动车的位置信息和姿态信息，然后设计了新型的路径描述即设计期望轨迹，最终针对该种描述路径的方法，设计了路径跟踪算法和速度跟踪算法，通过实验验证，达到较好跟踪期望轨迹的目的。因此，本实施例的控制方法具有一定的实际应用价值，可操作性强，容易实现，控制效果佳。

以上所述，仅是本发明的较佳实施例而已，并非是对本发明做其它形式的限制，任何熟悉本专业的技术人员可能利用上述揭示的技术内容加以变更或改型为等同变化的等效实施例。但是凡是未脱离本发明技术方案内容，依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化与改型，仍属于本发明技术方案的保护范围。

Claims (10)

1.一种移动车轨迹跟踪控制方法，其特征在于，其包括如下步骤：

S1、根据定位系统中移动车的信息，获取移动车的实际位置信息和实际姿态信息；

S2、根据预先定义的期望轨迹中的所有位置点坐标，以及所述移动车的实际位置信息，确定所述期望轨迹中距离所述移动车的实际位置最近的位置点；

S3、根据步骤S2中查找的最近的位置点坐标，以及预设区间范围ε，以及所述移动车的实际位置信息，确定移动车的虚拟牵引点，该虚拟牵引点为期望所述移动车相对于当前位置到达的第一个位置信息；

S4、根据所述移动车的实际姿态信息和虚拟牵引点，确定所述移动车到达所述虚拟牵引点的期望姿态信息；

S5、在期望轨迹中将步骤S2中最近的位置点相邻的，且与移动车运动方向对应的下一个位置点作为期望动点；

S6、根据期望动点、虚拟牵引点的位置信息，获取移动车的期望角速度；

S7、将所述期望姿态信息和期望角速度发送所述移动车，以使所述移动车根据所述期望姿态信息和期望角速度运动。

2.根据权利要求1所述的方法，其特征在于，所述步骤S1中的位置信息包括：后轮车轴中点的坐标为(x,y)，所述步骤S1中的姿态信息包括：移动车方向角为θ，所述步骤S4中的期望姿态信息包括：期望转向角

3.如权利要求2所述的方法，其特征在于，所述步骤S2中的期望轨迹包括n个位置点的位置点序列S，每个位置点表示为a_i，a_i的位置为(x_i,y_i)，所述S表示为S＝{a₁,a₂,a₃,...,a_n}，n∈N⁺，其中N⁺为正整数。

4.根据权利要求3所述的方法，其特征在于，步骤S2包括：

获取位置点序列S中任一点a_i与当前移动车中后轮车轴中点(x,y)的距离d；

确定距离d最近的属于位置点序列S中的位置点a_c(x_c,y_c)；

其中，

5.根据权利要求4所述的方法，其特征在于，步骤S3包括：

在位置点a_c点附近设置ε区间范围，c2为所述S中离移动车后轮车轴中点(x,y)的距离小于等于d+ε的一个边界点，c1为S中离移动车后轮车轴中点(x,y)的距离小于等于d+ε的另一个边界点，将S中在点a_c1与点a_c2之间的所有点记为虚拟牵引点集C，根据公式||(x,y)-(x_c2,y_c2)||＜＝d+ε＜＝||(x,y)-(x_c2+1,y_c2+1)||，c2∈n，c2+1∈n

和||(x,y)-(x_c1,y_c1)||＜＝d+ε＜＝||(x,y)-(x_c1-1,y_c1-1)||，c1∈n,c1-1∈n，确定虚拟牵引点a_r(x_r，y_r)。

6.根据权利要求5所述的方法，其特征在于，所述步骤S4包括：

根据虚拟牵引点a_r(x_r，y_r)、移动车后轮车轴中点的坐标(x,y)，通过获取移动车当前位置的期望方向角θ_r；

根据移动车当前的方向角θ、期望方向角θ_r，通过获取移动车当前位置的前轮期望转向角

7.根据权利要求6所述的方法，其特征在于，在所述步骤S6中，按如下步骤进行：

S601、在虚拟牵引点集C中查找与期望动点a_m最近的牵引点a_t作为速度虚拟牵引点a_t；

所述期望动点a_m(x_m,y_m)为位置点序列S中的位置点；

S602、根据所述期望动点a_m求导获得期望动点的速度v_m，即所述v_m的单位向量为则

S603、所述速度虚拟牵引点a_t与所述期望动点a_m之间的距离为L，通过公式L＝||(x_t,y_t)-(x_m,y_m)||获得，并根据公式获得所述移动车当前位置的期望角速度ω，其中r为所述移动车后车轮半径。

8.如权利要求7所述的移动车轨迹跟踪控制方法，其特征在于，所述k的取值为v_m与L的比值的倍数的0到2倍。

9.根据权利要求8所述的方法，其特征在于，在所述步骤S7中，根据获得的ω控制所述移动车x方向的期望速度为y方向的期望速度为车头方向的期望角速度为

10.根据权利要求9所述的方法，其特征在于，所述方法还包括：

重复上述步骤S1至步骤S7，以使所述移动车完成预设期望轨迹的运动；

在重复上述步骤S5的过程中，采用所述期望动点的下标每隔设定的时间自动加1，使期望动点a_m按位置点序列S中的点序列移动替换所述步骤S5。