﻿ 基于路径跟踪控制方法的拖挂式机器人系统路径规划算法
 计算机应用   2017, Vol. 37 Issue (4): 1116-1121  DOI: 10.11772/j.issn.1001-9081.2017.04.1116 0

### 引用本文

FANG Xiaobo, QIAN Hong, LIU Zhenming, MENG Dezhuang. Path planning algorithm of tractor-trailer mobile robots system based on path-following control method[J]. Journal of Computer Applications, 2017, 37(4): 1116-1121. DOI: 10.11772/j.issn.1001-9081.2017.04.1116.

### 文章历史

Path planning algorithm of tractor-trailer mobile robots system based on path-following control method
FANG Xiaobo, QIAN Hong, LIU Zhenming, MENG Dezhuang
Marine Design & Research Institute of China, Shanghai 200011, China
Abstract: Concerning the low accuracy, poor stability and security of the path planning algorithm of tractor-trailer mobile robots system, a path planning algorithm based on path-following method was proposed. On the basis of Rapid-exploring Random Tree (RRT) method and the equations of path-following, the path accuracy was improved by automatically fitting spline curve and tracking and generating the path between nodes; an angle constraint condition between systems and node hitting mechanism were added to the algorithm to improve the stability of algorithm and the security of results. In addition, an optimization algorithm based on greedy strategy was added to optimize results. The simulations results indicate that compared with the basic RRT algorithm, the path calculated by the improved algorithm is more close to the actual trajectory, and the success rate and security are better than the original algorithm, which can meet the requirement of quick design and real-time systems.
Key words: trailer system    path-planning    path-following    Rapid-exploring Random Tree (RRT)    robot
0 引言

1 路径跟踪控制方法 1.1 运动学模型

 图 1 拖挂式移动机器人系统示意图 Figure 1 Diagram of the tractor trailer mobile robot system

 $\left\{ \begin{array}{l} {{\dot x}_1} = {u_1}{\rm{cos}}{\theta _1}\\ {{\dot y}_1} = {u_1}{\rm{sin}}{\theta _1}\\ {{\dot q}_1} = {w_1}\\ {{\dot q}_2} = [{u_1}{\rm{sin}}({\theta _1} - {\theta _2}) - {c_1}{w_1}{\rm{cos}}({\theta _1} - {\theta _2})]/{l_2} \end{array} \right.$ (1)

 $\left\{ \begin{array}{l} {x_2} = {x_1} - {c_1}{\rm{cos}}{\theta _1} - {l_2}{\rm{cos}}{\theta _2}\\ {y_2} = {y_1} - {c_1}{\rm{sin}}{\theta _1} - {l_2}{\rm{sin}}{\theta _2} \end{array} \right.$ (2)
1.2 路径跟踪控制方程

 图 2 路径跟踪示意图 Figure 2 Diagram of path-following method
1.2.1 离轴式和连轴式正车路径跟踪

 $\left\{ \begin{array}{l} {u_1} = {u_{1d}}\\ {\omega _1} = {\rm{ }}{\omega _d} + {\rm{ }}\frac{2}{{{k_2}}}[{\rm{ }}2{k_3}{u_{1d}}\cdot{\rm{ }}({\rm{ }}{e_y}{\rm{cos}}\frac{{{e_\theta }}}{2} - {\rm{ }}{e_x}{\rm{sin}}\frac{{{e_\theta }}}{2}){\rm{ }} + \\ \;\;\;\;\;\;\;\;{\rm{sin}}\frac{{{e_\theta }}}{2}] \end{array} \right.$ (3)

 $\left\{ \begin{array}{l} {u_{1d}} = \sqrt {\dot x_d^2 + \dot y_d^2} \\ {\omega _d}\left( t \right) = \frac{{\ddot y\left( t \right){{\dot x}_d}\left( t \right) - {{\ddot x}_d}\left( t \right){{\dot y}_d}\left( t \right)}}{{{{\dot x}^2}_d\left( t \right) + {{\dot y}^2}_d\left( t \right)}}\\ \left\{ \begin{array}{l} {e_x} = ({x_d} - x){\rm{cos}}\theta + ({y_d} - y){\rm{sin}}\theta \\ {e_y} = - ({x_d} - x){\rm{sin}}\theta + ({y_d} - y){\rm{cos}}\theta \\ {e_\theta } = {\theta _d} - \theta \end{array} \right.\\ {\theta _d} = {\rm{arctan}}\left( {\frac{{{{\dot y}_d}}}{{{{\dot x}_d}}}} \right) + k{\rm{\pi }}\\ k = \left\{ \begin{array}{l} 0.5,\;\;\;\;\;\;{{\dot x}_d} = 0,{{\dot y}_d} \ge 0\\ - 0.5,\;\;\;\;{{\dot x}_d} = 0,{{\dot y}_d} < 0\\ 0,\;\;\;\;\;\;\;\;{{\dot x}_d} > 0\\ 1,\;\;\;\;\;\;\;\;\;\dot x{_d} < 0 \end{array} \right. \end{array} \right.$ (4)

1.2.2 离轴式倒车系统路径跟踪控制方程

 $\left\{ \begin{array}{l} {{\bar u}_2} = \bar u{_{2d}} = - \sqrt {\dot x_{2d}^2 + \dot y_{2d}^2} \\ k = \left\{ \begin{array}{l} 0.5,\;\;\;\;\;\;{{\dot x}_{2d}} = 0,{{\dot y}_{2d}} \ge 0\\ - 0.5,\;\;\;\;{{\dot x}_{2d}} = 0,{{\dot y}_{2d}} < 0\\ 0,\;\;\;\;\;\;\;\;{{\dot x}_{2d}} > 0\\ 1,\;\;\;\;\;\;\;\;\;{{\dot x}_{2d}} < 0 \end{array} \right.\\ {{\bar \omega }_2} = {{\bar \omega }_{2d}} + {\rm{ }}\frac{2}{{{k_6}}}[{\rm{ }}2{k_5}{{\bar u}_{2d}}\cdot{\rm{ }}({\rm{ }}{e_{y2}}{\rm{cos}}\frac{{{e_{\theta 2}}}}{2}{\rm{ }} - {\rm{ }}{e_{x2}}{\rm{sin}}\frac{{{e_{\theta 2}}}}{2}{\rm{ }}){\rm{ }} + \\ \;\;\;\;\;\;\;{\rm{sin}}\frac{{{e_{\theta 2}}}}{2}{\rm{ }}] \end{array} \right.$ (5)

 $\left\{ \begin{array}{l} {u_1} = {{\bar u}_2}{\rm{cos}}({\theta _1} - {\theta _2}) + \bar \omega {_2}{l_2}{\rm{sin}}({\theta _1} - {\theta _2})\\ {\omega _1} = {{\bar u}_2}\frac{1}{{{c_1}}}{\rm{sin}}({\theta _1} - {\theta _2}) - {{\bar \omega }_2}\frac{{{l_2}}}{{{c_1}}}{\rm{cos}}({\theta _1} - {\theta _2}) \end{array} \right.$ (6)
1.2.3 连轴式倒车系统路径跟踪控制方程

 $\left\{ \begin{array}{l} {{\dot x}_1} = {u_1}{\rm{cos}}{\theta _1}\\ {{\dot y}_1} = {u_1}{\rm{sin}}{\theta _1}\\ {{\dot \theta }_1} = {\omega _1}\\ {u_2} = {u_1}{\rm{cos}}({\theta _1} - {\theta _2})\\ {{\dot \theta }_2} = {\rm{ }}\frac{{{u_1}}}{{{l_2}}}{\rm{sin}}({\theta _1} - {\theta _2}) \end{array} \right.$ (7)

 ${u_1} = {{\bar u}_{2d}}{\rm{sec}}({\theta _1} - {\theta _2})$ (8)

 ${\omega _2} = {{\dot \theta }_2} = \bar u{_{2d}}{\rm{tan}}({\theta _1} - {\theta _2})/{l_2}$ (9)

 \left\{ \begin{align} & {{u}_{1}}=~{{{\bar{u}}}_{2d}}~\text{sec}~({{\theta }_{1}}-{{\theta }_{2}}) \\ & {{\omega }_{1}}=\frac{{{{\bar{u}}}_{2d}}}{{{l}_{2}}}~\text{tan}~({{\theta }_{1}}-{{\theta }_{2}})+ \\ & \ \ \ \ \ \ \ l\frac{_{2}({{{\dot{\bar{\omega }}}}_{2}}+~{{{\bar{\omega }}}_{2}})-\text{ }({{{\dot{\bar{u}}}}_{2d}}+~{{{\bar{u}}}_{2d}})\cdot ~\text{tan}~({{\theta }_{1}}-{{\theta }_{2}})}{~{{{\bar{u}}}_{2d}}\text{se}{{\text{c}}^{2}}({{\theta }_{1}}-{{\theta }_{2}})} \\ \end{align} \right. (10)
2 基于路径跟踪控制的改进RRT算法 2.1 基本RRT算法

RRT算法[11]是一种随机釆样的典型树结构算法, 采用特定的增量方式进行构造, 其基本思想是由控制理论决定随机树的增长方式, 通过在状态空间随机采样状态点将搜索导向空白区域, 逐步缩短随机状态点与树的期望节点即规划目标点间的距离, 从而找到一条连接起始点与目标点的规划路径。这种规划方式抛弃状态空间对障碍物精确定义的要求, 选用碰撞检测函数 (Extend) 来判定系统每个位形与障碍物的关系。这种方式可以简化对空间的建模, 搜索速度快, 不会出现栅格法、人工势场法等算法中易出现的维数灾难问题。因此用于解决复杂环境下复杂系统的路径规划问题[12-13]

 图 3 基本RRT算法 Figure 3 Basic RRT method
2.2 基于路径跟踪控制方法的改进RRT算法 2.2.1 嵌入路径跟踪控制方法

 图 4 TTMT-RRT节点拓展示意图 Figure 4 Diagram of TTMT-RRT node expansion
 图 5 连轴式倒车系统的路径跟踪结果 Figure 5 Path-following result of the connecting-backing system
2.2.2 节点击中机制

2.2.3 基于贪心思想的结果优化算法

Optimize (T:RRT Tree Points)

1)  Pruning (T);

2)  New TOpt;

4)  for (int i=0;i < T.st-1;i + +)

5)    for (j=T.st-1; j > I)

6)      if (Extend (T, Tj, Ti, env)= =TRUE)

7)        TOpt.add (Tj); i= j; break;

8)      else j--;

9)    end for

10)  end for

 图 6 贪心思想优化算法示意图 Figure 6 Diagram of greedy optimization method

2.2.4 改进算法流程

 图 7 基于路径跟踪的改进RRT算法流程 Figure 7 Improved RRT algorithm flow based on path tracking

TT MR Path Planning (env: environment, T: RRT Tree Points, Si: System state)

1)  Var T0, S0;

3)  TTarget=ChooseTarget ()

4)  Tnear=NearestNeighbour (TTarget);

5)  Tnew=Calulate (TTarget, Tnear, dl);

6)  if (Extend (T, Tnew, Tnear, env)= =TRUE)

8)  if (Tnew= =Tgoal)

9)    Optimize (T) and return;

Extend (T, Tnew, Tnear, env) 代码如下：

1)  if (HitTimes (Tnear) > n)

2)    T.delet (Tnear);

3)    return FALSE;

4)  el se

5)    if (PathTracking (Tnew, Tnear)= =TRUE)

6)      if (CollideTest (env)= =FALSE)

7)       return TRUE;

8)    else return FALSE;

9)  else return FALSE;

3 实验与分析 3.1 仿真初始信息与典型计算结果

 图 8 TTMR-RRT搜索结果 Figure 8 Results of TTMR-RRT method

 图 9 100次仿真计算结果信息 Figure 9 100 simulation results
3.2 仿真结果分析

1) 结果切实性。

2) 结果稳定性和优化能力。

 图 10 拖车双轮中点轨迹线20次计算结果汇总图 Figure 10 The 20 trajectory lines of midpoint of trailer's two-wheels

3) 成功率和安全性。

4) 结果时效性。

 图 11 搜索时间统计结果 Figure 11 Statistical results of searching time
 图 12 路径长度统计结果 Figure 12 Statistical results of path length

5) 搜索快速性。

4 结语

 [1] MYOUNGKUK P, WOOLIN C, MUNSANG K. Control of a mobile robot with passive multiple trailers[C]//Proceedings of the 2004 IEEE International Conference on Robotics and Automation. Piscataway, NJ: IEEE, 2004:4369-4374. [2] 王沛栋. 改进蚁群算法及在路径规划问题的应用研究[D]. 青岛: 中国海洋大学, 2012. ( WANG P D. Research on improved ant colony algorithms and applications for path planning problems[D]. Qingdao: Ocean University of China, 2012. ) [3] 巩敦卫, 曾现峰, 张勇. 基于改进模拟退火算法的机器人全局路径规划[J]. 系统仿真学报, 2013, 25 (3) : 480-483. ( GONG D W, ZENG X F, ZHANG Y. Global path planning method of robot based on modified simulated annealing algorithm[J]. Journal of System Simulation, 2013, 25 (3) : 480-483. ) [4] 刘传领. 基于势场法和遗传算法的机器人路径规划技术研究[D]. 南京: 南京理工大学, 2012. ( LIU C L. Researches on technologies for robot path planning based on artificial potential field and genetic algorithm[D]. Nanjing: Nanjing University of Science & Technology, 2012. ) [5] 杜明博, 梅涛, 陈佳佳, 等. 复杂环境下基于RRT的智能车辆运动规划算法[J]. 机器人, 2015 (4) : 443-450. ( DU M B, MEI D, CHEN J J, et al. RRT-based motion planning algorithm for intelligent vehicle in complex environments[J]. Robot, 2015 (4) : 443-450. ) [6] LAVALLE S M. Rapidly-exploring random trees: a new tool for path planning[EB/OL].[2016-03-01]. http://msl.cs.illinois.edu/~lavalle/papers/Lav98c.pdf. [7] 冯林, 贾菁辉. 基于对比优化的RRT路径规划改进算法[J]. 计算机工程与应用, 2011, 47 (3) : 210-213. ( FENG L, JIA J H. Improved algorithm of RRT path planning based on comparison optimization[J]. Computer Engineering and Applications, 2011, 47 (3) : 210-213. ) [8] 苑晶, 黄亚楼, 孙凤池. 两种连接形式的拖挂式移动机器人路径跟踪控制[J]. 控制理论与应用, 2008, 25 (3) : 398-406. ( YUAN J, HUANG Y L, SUN F C. Path following control for tractor-trailer mobile robots with two kinds of connection forms[J]. Control Theory & Applications, 2008, 25 (3) : 398-406. ) [9] 徐国华. 带拖车的轮式移动机器人系统的研究[D]. 天津: 南开大学, 1998. ( XU G H. Researches on tractor-trailer mobile robots system[D]. Tianjin: Nankai University, 1998. ) [10] 刘作军, 孔鑫, 杨鹏, 等. 带拖车移动机器人的运动学分析与仿真[C]//中国控制与决策学术年会文集. 沈阳: 东北大学, 2007: 1309-1316. ( LIU Z J, KONG X, YANG P, et al. Kinematics analyze and simulation for tractor-trailer mobile robot[C]//Proceedings of the 2006 Chinese Control and Decision Conference. Shengyang: Northeastern University, 2007: 1309-1316. ) [11] LAVALLE S M, KUFFNER J J. Randomized kinodynamic planning[J]. International Journal of Robotics Research, 1999, 20 (5) : 473-479. [12] 徐娜, 陈雄, 孔庆生, 等. 非完整约束下的机器人运动规划算法[J]. 机器人, 2011, 33 (6) : 666-672. ( XU N, CHEN X, KONG Q S, et al. Motion planning for robot with nonholonomic constraints[J]. Robot, 2011, 33 (6) : 666-672. ) [13] CHENG P, LAVALLE S M. Reducing RRT metric sensitivity for motion planning with differential constraints[EB/OL].[2016-03-01].http://msl.cs.uiuc.edu/~pcheng1/papers/thesis.ps.