移动机器人沿给定路径的完备且最短时间轨迹规划方法

摘要

一种移动机器人沿给定路径的完备且最短时间轨迹规划方法，包括：1.将多维轨迹规划问题转换为路径位置和路径速度的两维问题；1.1以路径位置s，路径速度和路径加速度表示机器人系统；1.2机器人系统的运动学和动力学约束转为路径速度和路径加速度约束；1.3计算最大和最小路径加速度以及最大速度限制曲线MVC*(s)；2.以最大路径加速度正向积分加速曲线；3.从加速曲线与MVC*(s)相交点开始，沿MVC*(s)寻找路径加速度切换区域；4.以最小路径加速度反向积分减速曲线；5.当遍历到路径终点时，结束；本发明所提的规划方法能够为给定路径实时输出时间最短轨迹，且该规划方法具有完备性。

说明书

技术领域

本发明属于工业自动化领域，特别是涉及轮式移动机器人沿给定路径的完备且最短时间轨迹生成方法。

背景技术

众所周知，最短时间轨迹规划在工业自动化领域应用广泛，是提高工业机器人生产效率的重要工具。为了快速地生成最短时间轨迹，具有较低时间复杂度的解耦策略获得许多关注[1]。解耦策略共分为两步：第一，路径规划输出满足高层任务要求(避障，曲率连续等)的可行路径；第二，沿给定路径的最短时间轨迹规划输出满足运动学和动力学约束(电机的转速，加速度，力矩等)的最优轨迹。沿给定路径的最短时间轨迹规划方法是本发明的核心内容。

目前，沿给定路径的最短时间轨迹规划方法可以保证规划算法的最优性和实时性，但是，规划算法的完备性依然缺少理论证明和实验分析。规划算法的完备性是指如果规划问题有解，则该规划算法能够在有限时间内给出可行解，否则输出无解[2]。如果规划算法缺少完备性，那么该算法可能无法为有解的规划问题输出有效解，或者无法在有限时间内为无解的规划问题输出无解提示。因此，不完备的规划算法会降低机器人的生产效率，甚至危及到生产安全[3]。

为了使得机器人高效安全地工作，需要设计同时具有完备性、最优性和实时性的沿给定路径的轨迹规划方法。针对该问题，E.Barnett等为绳力驱动的平行机构提出一种满足绳子张力和电机力矩的最短时间轨迹规划方法，但是其计算时间复杂度高。动态规划技术(Dynamic Programming)将给定路径离散化，并迭代寻找每个离散路径点上满足运动学和动力学约束的最大路径速度[4-6]，然而轨迹时间和计算时间由离散化程度决定。凸优化技术(Convex Optimization)在离散的路径和速度空间内沿合适的梯度方向搜索满足运动学和动力学约束的时间最优轨迹[7-10]，因而存在收敛到局部最优解的可能性。D.Constantinescu等人为工业机械臂提出一种满足力矩和力矩一阶导约束的光滑且时间最优轨迹规划方法[11]。S.Macfarlane等以五次多项式拟合沿给定路径的速度曲线，从而快速输出满足加加速度约束的轨迹[12]，但是轨迹时间不是全局最优的。基于庞得里亚金极大值原理，K.Shin等提出沿给定路径计算路径加速度切换点，然后从切换点出发以最大和最小路径加速度进行数值积分，生成满足力矩约束的时间最优速度曲线[13]，但该算法不是完备的且计算时间复杂度高。Q.Pham给出该数值积分方法的C++/Python开源代码并与凸优化方法进行对比，证明其开源代码的实时性[14]。当同时考虑速度和力矩约束时，P.Shen等发现该数值积分方法[13]是不完备的，无法为有解的规划问题输出可行解，并给出具体的条件和证明[15]。文献[16],[17]虽然给出数值积分方法在速度和力矩约束下的改进策略，但是缺少严格的完备性证明。总结文献可知，现有沿给定路径轨迹方法虽然能够保证最优性和实时性，但是缺少规划算法的完备性及其证明。

发明内容

本发明的目的是提供一种移动机器人沿给定路径的完备且最短时间轨迹规划方法，能够同时确保规划算法的完备性和最优性，为轮式移动机器人实时生成时间最优轨迹。

为了实现上述目的，本发明首先将多维轨迹规划问题转换为由路径位置和路径速度组成的两维轨迹规划问题。在此基础上，机器人系统的运动学和动力学约束转换为关于路径速度和路径加速度约束。接着，计算一条满足路径速度和路径加速度约束的最大速度限制曲线。沿着该最大速度限制曲线，搜寻所有的路径加速度切换区域(包括“切换点”和“切换弧”)。这些区域均满足路径速度和加速度约束。从这些切换区域出发，以最小路径加速度积分的减速曲线和以最大路径加速度积分的加速曲线相互交叉，共同连接成一条最短时间轨迹。本发明在前人基础(“切换点”)上补充了一类具有重要作用的路径加速度切换区域，称为“切换弧”。该切换弧的存在不仅使得本发明所提出的轨迹规划方法输出满足运动学和动力学约束的最短时间轨迹，而且保证了该规划方法的完备性。实验结果表明本发明算法同时具有时间最优、计算实时和规划完备的优点。

本发明提供的移动机器人沿给定路径的完备且最短时间轨迹规划方法包括：

第1步，将多维轨迹规划问题转换为路径位置和路径速度的两维轨迹规划问题；

第1.1步，机器人系统路径参数化，用路径位置、路径速度和路径加速度重新表达机器人系统；

以基于主动偏心万向轮的全方位移动机器人为例，机器人系统运动学模型：

其中，ξ＝[x y θ]^T代表机器人位姿，[x>T∈R²是机器人的位置，θ∈R是机器人的方向角,ω,a∈R⁴分别代表机器人主动轮的速度和加速度，矩阵J如下：

J＝[J₁>2>3>4]^T,

其中，η_i,i∈[1,2]是轮子偏转角，r,L,d分别是轮子半径，机器人本体半径以及机器人中心点到轮子转向轴距离。

沿着给定路径，机器人位姿重新表示为ξ(s)，其中s代表路径位置。对ξ(s) 做关于时间t的一阶导可得

ξ_s＝[x_s>s>s]^T,(4)

其中，x_s＝dx/ds,y_s＝dy/ds,θ_s＝dθ/ds。

沿给定路径，重新表达机器人运动学模型：将公式(4)带入(3)，再将公式(3) 带入公式(1)得到公式(5)；将公式(5)带入公式(2)得到公式(6)；

其中，矩阵向量

由于沿给定路径移动，每个主动轮偏转角η₁,η₂关于路径位置s如下：

第1.2步，机器人系统的运动学和动力学约束转换为路径位置，路径速度和路径加速度约束；

主动轮的速度和加速度约束如下：

-ω_max≤ω≤ω_max,>

-a_max≤a≤a_max.>

其中，常向量ω_max∈R⁴和a_max∈R⁴分别是主动轮速度和加速度的上限。

为满足主动轮的加速度约束，将公式(6)带入公式(10)得

其中，

A(s)＝[(Jξ_s)^T>s)^T]^T,

为满足主动轮的速度约束，将公式(5)带入公式(9)得

其中，

第1.3步，计算最小和最大路径加速度以及在路径位置和路径速度组成的二维平面内的最大速度限制曲线；

根据公式(11)，计算最小和最大路径加速度分别如下：

其中，标量A_i(s),B_i(s),C_i(s)分别是向量A(s),B(s),C(s)的元素。

利用公式(13)和(14)，得到满足加速度约束(10)的速度限制曲线

接着，根据公式(12)，得到满足速度约束(9)的速度限制曲线

V(s)＝min{-D_i(s)/A_i(s)|A_i(s)＞0,i∈[1,8]},>

其中，标量D_i(s)是向量D(s)的元素。

综合公式(15)和(16)，得到同时满足约束(9)和(10)的最大速度限制曲线

MVC^*(s)＝min(MVC(s),V(s)).>

第2步，从路径起点或路径加速度切换区域开始，以最大路径加速度正向积分加速曲线；路径加速度切换区域包括两类：切换点和切换弧，其中切换弧是位于最大速度限制曲线上满足路径加速度约束的连续弧线段；即，

该步总共分为三种情况：

①从给定路径起点开始，以最大路径加速度正向积分加速曲线；

②从位于最大速度限制曲线MVC^*(s)的切换点开始，以最大路径加速度正向积分加速曲线；

③从位于最大速度限制曲线V(s)的切换弧右端开始，以最大路径加速度正向积分加速曲线；

对加速曲线正向积分，直到与最大速度限制曲线MVC^*(s)、运动静止界线或路径终点界线s＝s_e相交为止。其中，s_e代表给定路径总长度。如果与最大速度限制曲线MVC^*(s)相交，转步骤3；如果与路径终点界线s＝s_e相交，转步骤4；如果与运动静止界线相交，返回无解提示。

第3步，从加速曲线与最大速度限制曲线MVC^*(s)相交点开始，沿MVC^*(s)>

路径加速度切换区域总共分为两类：切换点和切换弧；

a)切换点包含以下三种类别：

①正切点：从该点开始，正反向积分的加减速曲线与最大速度限制曲线

MVC^*(s)相切；

②断点：在该点上，最大速度限制曲线MVC^*(s)是不连续的；

③奇点：在该点上，关于路径位置，路径速度和路径加速度不等式约束的第一特定向量包含至少一个零元素，即公式(11)的向量A(s)包含至少一个零元素。

b)切换弧定义：位于曲线V(s)上满足路径加速度约束(13)和(14)的连续弧线段；

如果沿MVC^*(s)寻找到第一个路径加速度切换区域，那么转第4步并从路径加速度切换区域开始以最小路径加速度反向积分减速曲线(第4步中的情况②或③)。否则，转第4步并从路径终点开始以最小路径加速度反向积分减速曲线>

第4步，以最小路径加速度反向积分减速曲线

该步总共分为三种情况：

①从给定路径终点开始，以最小路径加速度反向积分减速曲线；

②从切换点开始，以最小路径加速度反向积分减速曲线；

③从切换弧左端开始，以最小路径加速度反向积分减速曲线。

对减速曲线反向积分，直到与运动静止界线或由第2步生成的加速曲线相交。如果与相交，则返回规划问题无解提示。如果与由第2步生成的加速曲线相交，则对于上述的情况②和③，分别转第2步的情况②和③，对于上述的情况①，直接返回最短时间轨迹。

本发明的优点和积极效果：

本发明提供了一种完备且最短时间的沿给定路径轨迹规划方法。在理论上，该方法同时具有最优性，实时性以及完备性。在应用上，该方法适用于多种不同机器人平台，可以为其在线输出时间最短轨迹。实验结果充分证明了本发明算法的有效性。

附图说明

图1是基于主动偏心万向轮的全方位移动机器人图；

图2是三阶贝塞尔给定路径图；

图3是Shin等[13]提出的轨迹规划方法输出的不可行轨迹图；

图4是本发明提出的轨迹规划方法输出的时间最优轨迹图；

图5是跟踪误差图；

图6是主动轮的速度曲线图；

图7是主动轮的加速度曲线图；

图8是本发明提出算法的完整流程图。

具体实施方式

为了使本技术领域的人员更好地理解本发明方案，下面结合附图和实施方式对本发明作进一步的详细说明。

实施例1

第1步，将多维轨迹规划问题转换为路径位置和路径速度的两维轨迹规划问题；

第1.1步，机器人系统路径参数化

以基于主动偏心万向轮的全方位移动机器人为例，其运动学模型(如图1 所示)：

其中，ξ＝[x y θ]^T是机器人位姿，[x>T∈R²是机器人中心O_r在世界坐标系X_wO_wY_w下的位置，θ∈R是机器人的方向角，ω,a∈R⁴分别代表主动轮的速度和加速度，矩阵J如下：

J＝[J₁>2>3>4]^T,

给定路径选择n阶贝塞尔曲线，其数学表达式如下：

其中，在世界坐标系X_wO_wY_w下位置坐标P_i＝[x_i>i]^T,i∈[0,n]是路径控制点。λ∈[0,1]是路径参数，与路径位置s存在非线性映射。由于路径是已知，可以提前建立λ与s的映射表。

沿该给定路径，机器人位姿重新表示为ξ(s)，其中s代表路径位置。对ξ(s) 做关于时间t的一阶导可得

ξ_s＝[x_s>s>s]^T,>

其中，x_s＝dx/ds,y_s＝dy/ds,θ_s＝dθ/ds。

将公式(3)和(4)带入公式(1)和(2)可得

其中，矩阵向量

由于沿给定路径移动，每个主动轮偏转角η₁,η₂关于路径位置s如下：

第1.2步，机器人系统的速度和加速度约束转换为路径速度和路径加速度约束

主动轮的速度和加速度约束如下：

-ω_max≤ω≤ω_max,>

-a_max≤a≤a_max,>

其中，常向量ω_max∈R⁴和a_max∈R⁴分别是主动轮速度和加速度的上限。

为满足主动轮的加速度约束，将公式(6)带入公式(10)得

其中，

A(s)＝[(Jξ_s)^T(-Jξ_s)^T]^T,

为满足主动轮的速度约束，将公式(5)带入公式(9)得

其中，

第1.3步，计算最小和最大路径加速度以及在两维平面内的最大速度限制曲线

根据公式(11)，计算最小和最大路径加速度，分别如下：

其中，标量A_i(s),B_i(s),C_i(s)分别是向量A(s),B(s),C(s)的元素。

利用公式(13)和(14)，得到满足加速度约束(10)的速度限制曲线

接着，根据公式(12)，得到满足速度约束(9)的速度限制曲线

V(s)＝min{-D_i(s)/A_i(s)|A_i(s)＞0,i∈[1,8]}.>

其中，标量D_i(s)是向量D(s)的元素。

综合公式(15)和(16)，得到同时满足约束(9)和(10)的最大速度限制曲线

MVC^*(s)＝min(MVC(s),V(s)).>

第2步，以最大路径加速度正向积分加速曲线

该步总共分为三种情况：

①从给定路径起点开始，以最大路径加速度正向积分加速曲线，如图3和图4中黑色实线β₀；

②从位于速度限制曲线MVC^*(s)的切换点开始，以最大路径加速度正向积分加速曲线；

③从位于速度限制曲线V(s)的切换弧右端开始，以最大路径加速度正向积分加速曲线；如图4所示，从q₂开始，正向积分加速曲线立即击穿速度限制曲线V(s)；

以最大路径加速度正向积分加速曲线的具体步骤如下：

a)将给定路径等分为N份，每份路径长度为ds＝s_e/N，其中s_e是给定路径总长度。

b)从起点开始，利用如下公式

s_i+1＝s_i+ds,

正向数值积分加速曲线，直到与曲线MVC^*(s)，或s＝s_e相交为止。如果与MVC^*(s)相交，转步骤3；如果与s＝s_e相交，转步骤4；如果与相交，返回无解提示。

第3步，从加速曲线与MVC^*(s)相交点开始，沿MVC^*(s)寻找路径加速度切换区域

路径加速度切换区域总共分为两类：切换点和切换弧。

切换点包含以下三种类别：

①正切点：从该点开始，正反积分的加减速曲线与MVC^*(s)相切；

②断点：在该点上，MVC^*(s)是不连续的；

③奇点：在该点上，公式(11)的向量A(s)包含至少一个零元素。

切换弧定义：位于速度限制曲线V(s)上满足约束路径加速度约束(13)和(14) 的连续弧线段，如图4中黑色实线q₁q₂；

如果沿MVC^*(s)寻找到第一个路径加速度切换区域，那么转步骤4的情况②或③。否则，转步骤4的情况①。

第4步，以最小路径加速度反向积分减速曲线

该步总共分为三种情况：

①从给定路径终点开始，以最小路径加速度反向积分减速曲线，如图3和图4中黑色实线α_e；

②从位于速度限制曲线MVC^*(s)的切换点开始，以最小路径加速度反向积分减速曲线；

③从位于速度限制曲线V(s)的切换弧左端开始，以最小路径加速度反向积分减速曲线。如图4所示，从q₁开始，反向积分减速曲线立即击穿速度限制曲线；

以最小路径加速度反向积分减速曲线的具体步骤如下：

a)将给定路径等分为N份，每份路径长度为ds＝s_e/N，其中s_e是给定路径总长度。

b)从起点开始，利用如下公式

s_i-1＝s_i-ds,

反向数值积分减速曲线，直到与或由步骤2生成的加速曲线相交。如果与相交，则返回规划问题无解提示。如果与由步骤2生成的加速曲线相交，则对于情况②和③，分别转步骤2的情况②和③，对于情况①，直接返回最短时间轨迹。

第5步，实验效果描述

为验证上述完备且最短时间沿给定路径轨迹规划方法的有效性，本发明在型号为“NK-OMNI I”的全方位移动机器人上进行了实验验证。给定路径选择三阶贝塞尔曲线(图2)，路径控制点为P₀＝[0.0>T,P₁＝[1.0>T,P₂＝[4.0>T,>3＝[5.0>T，单位m。主动轮的速度约束设定为ω_max＝[6.0>T，单位rad/s。主动轮的加速度约束设定为a_max＝[1.0>T，单位rad/s²。另外，在第2步和第4步中，给定路径的等分数设定为N＝10000。

参考图3，由Shin等人提出的轨迹规划方法[13]无法生成可行轨迹。与之相比，如图4所示，本发明所提轨迹规划方法仅耗费39毫秒输出一条满足主动轮速度和加速度约束的时间最优轨迹。其中，黑色虚线代表满足主动轮加速度约束的限制曲线MVC(s)，黑色点虚线代表满足主动轮速度约束的限制曲线V(s)。黑色实线β₀和α_e分别代表由步骤2和4生成的加速和减速曲线。黑色实线q₁q₂代表路径加速度切换弧。该实验结果验证了本发明所提轨迹规划方法的完备性以及实时性。

实验利用一个简单的PID控制器跟踪图4内的时间最优轨迹。参考图5，跟踪误差收敛于零。参考图6和图7，主动轮的四个电机不仅满足主动轮速度和加速度约束，而且在整个跟踪过程中总会有一个电机的速度或加速度达到饱和。该实验结果表明本发明所提轨迹规划方法的最优性。

参考文献

[1]E.Barnett,C.Gosselin.Time-optimal trajectory planning of cable-driven parallel mechanisms for fully-specified paths withG1discontinuities.Journal of Dynamic Systems Measurement and Control,2013,137(7):603-617.

[2]K.Goldberg.Completeness in robot motion planning.WorkshopAlgorithmic Foundations ofRobotics(WAFR),San Francisco,1994.

[3]M.Dogar,A.Spielberg,S.Baker,D.Rus.Multi-robot grasp planning forsequential assembly operations.Proceedings of 2015IEEE InternationalConference on Robotics andAutomation,2015:193-200.

[4]K.Shin,N.McKay.A dynamic programming approach to trajectoryplanning of robotic manipulators.IEEE Transactions Automatic Control,1986,31(6): 491-500.

[5]K.Shin,N.McKay.Selection of near-minimum time geometric paths forrobotic manipulators.IEEE Transactions on Automatic Control,1986,31(6):501-511.

[6]S.Singh,M.Leu.Optimal trajectory generation for roboticmanipulators using dynamic programming.Journal ofDynamic Systems Measurementand Control, 1987,109(2):88-96.

[7]K.Hauser.Fast interpolation and time-optimization withcontact.International Journal ofRobotics Research,2014,33(9):1231-1250.

[8]D.Verscheure,B.Demeulenaere,J.Swevers,J.Schutter,M.Diehl.Time-optimal path tracking for robots:A convex optimization approach.IEEETransactions on Automatic Control,2009,54(10):2318-2327.

[9]F.Debrouwere,W.Loock,G.Pipeleers,Q.Dinh,M.Diehl,J.Schutter,J.Swevers.Time-optimal path following for robots with convex-concaveconstraints using sequential convex programming.IEEE Transactions onRobotics,2013,29(6):1485-1495.

[10]A.Singh,K.Krishna.A class of non-linear time scaling functionsfor smooth time optimal control along specified paths.Proceedings of2015IEEE/RSJ International Conference on IntelligentRobots andSystems,2015:5809-5816.

[11]D.Constantinescu,E.Croft.Smooth and time-optimal trajectoryplanning for industrial manipulators along specified paths.Journal ofRoboticSystems,2000, 17(5):233-249.

[12]S.Macfarlane,E.Croft.Jerk-bounded manipulator trajectoryplanning:Design for real-time applications.IEEE Transactions on RoboticsandAutomation,2003, 19(1):42-52.

[13]K.Shin,N.Mckay.Minimum-time control of robotic manipulators withgeometric path constraints.IEEE Transactions on Automatic Control,1985,30(6):531-541.

[14]Q.Pham.A general,fast,and robust implementation of the time-optimal path parameterization algorithm.IEEE Transactions on Robotics,2014,30(6): 1533-1540.

[15]P.Shen,X.Zhang,Y.Fang.Essential properties of numericalintegration for time-optimal trajectory planning along a specified path.IEEERobotics and Automation Letters,2017,2(2):888-895.

[16]L.Zlajpah.On time optimal path control of manipulators withbounded joint velocities and torques.Proceedings of IEEE InternationalConference on Robotics andAutomation,1996:1572-1577.

[17]F.Lamiraux,J.Laumond.From paths to trajectories for multibodymobile robots.Proceedings of the 5th International Symposium on ExperimentalRobotics,1998:301-309。