CN113885501A - 一种用于无人车位姿调节的轨迹规划方法 - Google Patents

Abstract

本发明公开了一种用于无人车位姿调节的轨迹规划方法，包括步骤：采用由圆弧、直线段和回旋曲线组成的Reeds‑Shepp曲线构造起点和终点之间曲率连续且距离最短的最优路径；给定离散时间间隔，获取离散的路径点；根据Reeds‑Shepp曲线上的起点、终点和路径点组成的点对将最优路径分为前进路径片段和倒车路径片段；分别针对每个路径片段的长度和运动方向生成各阶导数光滑有界的速度规划曲线；将速度规划曲线叠加到相应的路径片段上得到最优运行轨迹。本发明实现在无障碍物环境下任意两个位姿之间生成满足无人车运动约束的可行轨迹，保证生成的轨迹满足车辆的转向和驱动特性，并有助于减少轨迹跟踪误差。

Description

一种用于无人车位姿调节的轨迹规划方法

技术领域

本发明涉及无人车轨迹规划领域，特别涉及一种用于无人车位姿调节的轨迹规划方法。

背景技术

轨迹规划是无人车应用的关键技术，轨迹规划能力也成为无人车智能化程度的重要标志。

对于工业和园区物流等场景，无人车在执行运输作业的过程中经常会遇到站点停靠、自动充电、临时避让等需要改变自身位置或姿态的任务。这要求无人车具有灵活的位姿调节能力，能够根据具体的任务规划一条连接起始位姿和目标位姿的可行轨迹。

对于采用阿克曼转向机构的无人车，由于自身存在运动约束，通常采用Dubins曲线、Reeds-Shepp曲线、多项式曲线或者在离散点以及样条曲线的基础上优化得到可行的路径，然后采用梯形或S形曲线进行速度规划。

Dubins曲线和Reeds-Shepp曲线由于具有距离最短、易于构造的优点经常被使用。但是该类最优曲线具有bang-bang控制的特点，其由若干段最小转向半径的圆弧和直线段拼接组成。圆弧的曲率为非零值，直线段的曲率为零，因此Reeds-Shepp曲线在圆弧与圆弧之间以及圆弧与直线段之间的曲率是突变的。该类曲线只能保证切线连续，曲率并不连续，给无人车实际跟踪造成了困难。传统的速度规划方法需要根据输入和约束条件划分为几种情况，然后分别构造速度函数，难以保证高阶导数光滑而且计算较复杂。

发明内容

本发明的目的在于提供一种用于无人车位姿调节的轨迹规划方法，解决路径曲率不连续、速度不光滑和计算复杂的问题，实现在无障碍物环境下任意两个位姿之间生成满足无人车运动约束的可行轨迹。

实现本发明目的的技术方案为：一种用于无人车位姿调节的轨迹规划方法，包括步骤：

采用由圆弧、直线段和回旋曲线组成的Reeds-Shepp曲线构造起点和终点之间曲率连续且距离最短的最优路径；

给定离散时间间隔，获取离散的路径点；

根据Reeds-Shepp曲线上的起点、终点和路径点组成的点对将最优路径分为前进路径片段和倒车路径片段；

基于指数函数，分别针对每个路径片段的长度和运动方向生成各阶导数光滑有界的速度规划曲线；

将速度规划曲线叠加到相应的路径片段上得到最优运行轨迹。

进一步的，所述圆弧的半径为无人车的最小转向半径，圆弧与圆弧以及圆弧与直线段之间通过回旋曲线过渡连接。

进一步的，通过遍历路径中所有圆弧和直线段的组合方式找到距离最短的最优路径。

进一步的，给定离散时间间隔，获取离散的路径点具体包括：将构造的最优路径用圆弧方程、直线方程和回旋曲线方程表示，将给定的离散时间间隔带入圆弧方程、直线方程和回旋曲线方程得到离散的路径点；每个路径点表示无人车位姿(x,y,θ)的三维坐标，x、y是路径点在全局坐标系中的横、纵坐标，θ是无人车与全局坐标系X轴的夹角。

进一步的，将最优路径分为前进路径片段和倒车路径片段具体包括以下步骤：

步骤5-1，计算无人车的车头指向的方向向量v_e为：

v_e＝(cosθ,sinθ)

步骤5-2，计算路径点方向向量v_i：

v_i＝(x_i+1-x_i,y_i+1-y_i)

其中，x_i和y_i是当前路径点i的横、纵坐标，x_i+1和y_i+1是x_i和y_i对应的路径点的下一个路径点的横、纵坐标；

步骤5-3，确定车头指向的方向向量v_e和路径点方向向量v_i的夹角α：

α＝v_e·v_i

如果夹角α<π/2，此时速度应为正，该路径点归类为前进路径片段；如果两个向量的夹角α≥π/2，此时速度应为负，路径点归类为倒车路径片段；

步骤5-4，重复上述步骤，将最优路径分割成若干条交替进行的前进路径片段和倒车路径片段。

进一步的，分别针对每个路径片段的长度和运动方向生成各阶导数光滑有界的速度曲线，具体包括以下步骤：

构造速度函数；

根据姿态调节时对无人车驱动系统的约束条件确定速度函数中控制参数的取值范围；

基于控制参数的取值范围、路径片段的长度和运动方向，确定速度函数中控制参数值，获取速度曲线。

进一步的，所述速度函数为：

其中，e是自然指数，v_m、k和c均为控制参数，时间t是函数的自变量，v_m是函数的极限值，对应最大速度；k反应函数曲线的倾斜程度，决定最大加速度；c对应速度达到最大加速度所需的时间，对于速度的上升阶段和下降阶段，参数k取值相同，上升阶段v_m＞0，下降阶段v_m＜0。

进一步的，根据姿态调节时对无人车驱动系统的约束条件确定速度函数中控制参数的取值范围，具体包括以下步骤：

步骤8-1，根据开始时刻的速度v(0)＝v_m/(1+e^kc)和无人车能够响应的最小速度v_min，通过v(0)＝v_min确定参数k和c的关系(kc)＝ln(v_m/v_min-1)；

步骤8-2，计算速度函数上的最大加速度a_m＝kv_m/4，根据无人车最大允许加速度a_max，通过关系a_m≤a_max确定参数k的取值范围k≤4a_max/v_m；

步骤8-3，计算速度曲线上的最大加加速度

根据无人车最大允许加加速度j_max，通过关系j_m≤j_max确定参数k的取值范围

步骤8-4，根据步骤8-2和步骤8-3，确定同时满足两个约束时参数k的可行范围：k≤k_m，其中

进一步的，所述确定速度函数中控制参数值具体包括以下步骤：

步骤9-1，使用参数值v_m＝v_d和c＝(kc)/k_m，对速度函数v(t)积分，得到速度上升阶段经历的距离

其中v_d是无人车在执行位姿调节时的期望速度；

步骤9-2，计算对应路径片段的长度s；

步骤9-3，另参数v_m＝v_d、k＝k_m和c＝(kc)/k计算速度上升阶段经历的距离L＝cv_d＝ln(v_m/v_min-1)v_d/k_m；

步骤9-4，比较路径片段的长度s和速度上升阶段经历的距离L，如果s/2≥L，则参数v_m＝v_d、k＝k_m和c＝(kc)/k，得到起始段的速度函数，结束段的速度函数达到最大加速度，此时参数c₂＝2(s/2-L)/v_d+3c；如果s/2＜L，求在约束s/2＝cv_m，kc＝In(v_m/v_min-1)和k≤k_m下最大的v_m和k，根据c＝(kc)/k求出参数c，令结束段的参数c₂＝3c。

进一步的，对于s/2＜L，通过求解以下两组非线性方程得到两组参数(k₁，v_m1)、(k₂，v_m2)，

确定控制参数v_m＝min(v_m1，v_m2)，对应的参数k也确定。

本发明与现有技术相比，其显著效果为：(1)本发明使用改进的Reeds-Shepp曲线由半径为r的圆弧、与圆弧相切的直线段和回旋曲线构成，圆弧与圆弧以及圆弧与直线段之间不是直接连接，而是通过回旋曲线过渡，实现曲率的连续过渡，且曲率连续，通过遍历组合方式找到距离最短的最优路径，实现了最优路径规划；(2)本发明将路径分为前进和倒车片段，在路径片段上采用指数函构造速度轮廓，保证速度及其高阶导数光滑有界，完成速度规划；(3)本发明的路径规划和速度规划方法可在无障碍物环境中任意两个位姿之间生成满足无人车运动约束的可行轨迹，且计算复杂度低；(4)本发明生成的轨迹满足车辆的转向和驱动特性，并有助于减少轨迹跟踪误差。

附图说明

图1是本发明利用改进的Reeds-Shepp曲线在位姿和位姿之间生成的无人车路径图。

图2是本发明无人车路径上的曲率图。

图3是本发明速度规划生成的速度曲线图，图3(a)、图3(b)、图3(c)、(d)分别表示三个连续分段路径和整条路径的速度曲线图。

图4是本发明速度函数中控制参数的可行空间和最优值的示意图。

具体实施方式

下面将结合附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

无人车的后轮提供驱动力，不能转向；前轮用于转向，不提供驱动力。前轮具有最大转角限制，因此无人车存在最小转向半径，记为r。

选择无人车后轮驱动轴中心作为参考点，无人车的状态向量定义为Q＝(x，y，θ，v)。其中，(x，y)是参考点在全局坐标系中的坐标，θ是无人车与全局坐标系x轴的夹角，角度规定逆时针为正方向，v是无人车的驱动速度。

本发明实施例提出了一种无人驾驶车辆的轨迹规划方法，所述的轨迹规划包括路径规划和路径上的分段速度规划。路径规划采用改进的Reeds-Shepp曲线构造曲率连续、有界且距离最优的路径。速度规划阶段将路径分为前进和倒车片段，分别针对每个片段的长度和运动方向生成速度各阶导数光滑有界的速度轮廓。最后，将速度分配到所有路径点上得到参考轨迹。

在路径规划阶段，使用改进的Reeds-Shepp曲线生成无人车的路径。改进的Reeds-Shepp曲线由半径为r的圆弧、与圆弧相切的直线段和回旋曲线构成。圆弧与圆弧以及圆弧与直线段之间不是直接连接，而是通过回旋曲线过渡，实现曲率的连续过渡。回旋曲线的曲率连续变化，导致最终的路径曲率也是连续的。然后通过遍历各种组合方式找到距离最短的最优参考路径。

输入无人车出发点的状态向量Q_S和目标点的状态向量Q_T，改进的Reeds-Shepp曲线输出连接两个状态的最短路径。最短路径以圆弧方程、直线方程和回旋曲线方程的解析形式给出。在给定离散时间间隔dt时，带入路径的解析方程可以得到离散的路径点，每个路径点是表示无人车位姿(x，y，θ)的三维坐标。图1为本发明实施例的路径规划结果。无人车的最小转向半径是r＝3.5m，对应最大的曲率是κ_max＝1/r＝0.286。无人车的出发状态是Q_S＝(2，1，0，0)，目标状态是Q_T＝(8，2，π，0)。完整的路径由三段路径片段组成，路径片段1对应前进状态，路径片段2对应倒车状态，路径片段3对应前进状态。

图2为本发明实施例中生成的路径曲率图。从图中可知曲率连续变化，并且始终位于(-κ_max，κ_max)范围之内。

根据无人车的速度和Reeds-Shepp曲线上的起点、终点和转折点组成的点对将路径点分为前进路径片段和倒车路径片段；在位于两个点对中间的路径点上，如果无人车的期望速度为正，将两个点对之间的路径点归类为前进路径片段；在位于两个点对中间的路径点上，如果无人车的期望速度为负，将两个点对之间的路径点归类为倒车路径片段，具体根据路径上无人车姿态与运动方向对路径进行分割，分割方法如下：

步骤1，计算无人车的车头指向的方向向量v_e，公式如下

v_e＝(cosθ，sinθ) (1)

步骤2，计算路径方向向量v_i，公式如下，其中x_i和y_i是当前处理的路径点平面坐标，x_i+1和y_i+1是离散后的路径点序列中x_i和y_i对应的路径点的下一个点的坐标。

v_i＝(x_i+1-x_i，y_i+1-y_i) (2)

步骤3，求车头指向v_e和路径方向向量v_i两个向量的夹角α，公式如下，其中的符号·表示向量点乘。

α＝v_e·v_i (3)

如果两个向量的夹角α＜π/2，说明无人车的朝向与路径延伸的方向相同，此时无人车的速度应为正，如果两个向量的夹角α≥π/2，说明无人车的朝向与路径延伸的方向相反，此时无人车的速度应为负。将连续且速度相同的路径点标记为同一条路径片段。通过这种方式将整条轨迹分割成若干条路径片段。

速度规划利用指数函数光滑的性质构造速度曲线，采用的指数函数具体形式如下

式(4)表示的速度函数以时间t为自变量，时间t≥0。式中，e是自然指数，常数v_m、k和c为控制参数，通过调节控制参数的值可以改变函数曲线的形状。控制参数的含义如下：v_m是函数的极限值，对应无人车的最大速度；k反应函数曲线的倾斜程度，决定最大加速度；c对应速度达到最大加速度所需的时间。

一段路径片段上，无人车的速度不改变方向，假设开始和结束时刻无人车的速度均为零。此时无人车的速度轮廓包含上升阶段和下降阶段，两个阶段由两个式表示的速度函数组合得到，而且两个函数参数k取值相同，区别在于参数c不同，具体形式如下

优选地，根据姿态调节时对无人车驱动系统的约束条件确定控制参数的可行范围，具体方法如下：

步骤1，根据开始时刻的速度v(0)＝v_m/(1+e^kc)和无人车能够响应的最小速度v_min，通过关系v(0)＝v_min确定参数k和c的关系(kc)＝ln(v_m/v_min-1)；

步骤2，计算式(4)的一阶导数，结果如下

式(6)为加速度，计算式(4)的二阶导数，结果如下

式(7)对应加加速度，令其等于0即可得到一个关于时间t的方程，此方程的解为t＝c，说明在c时刻加速度达到最大值，此时的最大加速度为a(c)＝kv_m/4，在位姿调节时，规定无人车最大允许加速度为a_max，由此可以得到关于参数k的一个约束条件：

步骤3，计算式(4)的三阶导数，结果如下

式(9)对应加加加速度，令n(t)＝0即可得到一个关于时间t的方程，此方程的解如下：

将式(10)带入式能够得到最小和最大加加速度

在位姿调节时，规定无人车最大允许加加速度为j_max，由此可以得到关于参数k的另一个约束条件：

步骤4，根据步骤2和步骤3，确定同时满足两个约束时参数k的可行范围：k≤k_m，其中

根据路径规划生成的路径片段长度和以上得到的约束确定最优的参数值，具体方法如下：

步骤1，使用参数值v_m＝v_d和c＝(kc)/k_m对速度函数v(t)积分，得到经过的速度上升阶段经历的距离如下：

其中v_d是无人车在执行位姿调节时的期望速度；

步骤2，计算路径片段的长度s；

步骤3，选择参数v_m＝v_d、k＝k_m和c＝(kc)/k计算速度上升阶段经历的距离，如下：

步骤4，比较路径片段的长度s和速度上升阶段经历的距离L，如果s/2≥L，选择参数v_m＝v_d、k＝k_m和c₁＝(kc)/k生成上升阶段的速度函数，下降阶段的速度函数达到最大加速度的参数c₂计算方法是c₂＝2(s/2-L)/v_d+3c₁；如果s/2＜L，求在约束s/2＝cv_m，kc＝ln(v_m/v_min-1)和k≤k_m下最大的v_m和k，根据c₁＝(kc)/k求出参数c₁，然后可得下降阶段的参数c₂＝3c₁。

优选地，步骤4中，如果s/2＜L时，可以通过求解以下两组非线性方程得到两组参数(k₁，v_m1)、(k₂，v_m2)，然后选择两组解中v_m较小的一个作为控制参数，即v_m＝min(v_m1，v_m2)；参数k也因此确定：

实施例

本实施例无人车驱动系统的参数如下：期望行驶速度为v_d＝1m/s，最大允许加速度为a_max＝1m/s²，最大允许加加速度为j_max＝4m/s³，最小响应速度为v_min＝0.001m/s。三段路径片段的长度分别为6.386m、6.967m、1.91m。计算得到的路径片段1的速度函数控制参数为v_m＝1m/s，k＝4，c₁＝1.727，c₂＝8.113；路径片段2的速度函数控制参数为v_m＝-1m/s，k＝4，c₁＝1.727，c₂＝8.694；路径片段3的速度函数控制参数为v_m＝0.759m/s，k＝5.269，c₁＝1.258，c₂＝3.775；基于本发明一种用于无人车位姿调节的轨迹规划方法得到速度轮廓如图3所示，图3(a)、图3(b)和图3(c)分别为路径片段1、路径片段2和路径片段3对应的速度轮廓曲线，图3(d)为完整路径对应的速度轮廓曲线，其中图3(c)中路径片段对应的参数可行范围和最优的参数值如图4所示。

本实施例提出的方法适用于类车移动机器人或无人驾驶车辆，可在无障碍物环境中生成任意给定位置和姿态边界条件下的可行轨迹。

Claims (10)

1.一种用于无人车位姿调节的轨迹规划方法，其特征在于，包括步骤：

采用由圆弧、直线段和回旋曲线组成的Reeds-Shepp曲线构造起点和终点之间曲率连续且距离最短的最优路径；

给定离散时间间隔，获取离散的路径点；

根据Reeds-Shepp曲线上的起点、终点和路径点组成的点对将最优路径分为前进路径片段和倒车路径片段；

基于指数函数，分别针对每个路径片段的长度和运动方向生成各阶导数光滑有界的速度规划曲线；

将速度规划曲线叠加到相应的路径片段上得到最优运行轨迹。

2.根据权利要求1所述的轨迹规划方法，其特征在于，所述圆弧的半径为无人车的最小转向半径，圆弧与圆弧以及圆弧与直线段之间通过回旋曲线过渡连接。

3.根据权利要求2所述的轨迹规划方法，其特征在于，通过遍历路径中所有圆弧和直线段的组合方式找到距离最短的最优路径。

4.根据权利要求2所述的轨迹规划方法，其特征在于，给定离散时间间隔，获取离散的路径点具体包括：将构造的最优路径用圆弧方程、直线方程和回旋曲线方程表示，将离散时间间隔带入圆弧方程、直线方程和回旋曲线方程得到离散的路径点；每个路径点表示无人车位姿(x,y,θ)的三维坐标，x、y是路径点在全局坐标系中的横、纵坐标，θ是无人车与全局坐标系X轴的夹角。

5.根据权利要求4所述的轨迹规划方法，其特征在于，将最优路径分为前进路径片段和倒车路径片段具体包括以下步骤：

步骤5-1，计算无人车的车头指向的方向向量v_e为：

v_e＝(cosθ,sinθ)

步骤5-2，计算路径点方向向量v_i：

v_i＝(x_i+1-x_i,y_i+1-y_i)

其中，x_i和y_i是当前路径点i的横、纵坐标，x_i+1和y_i+1是x_i和y_i对应的路径点的下一个路径点的横、纵坐标；

步骤5-3，确定车头指向的方向向量v_e和路径点方向向量v_i的夹角α：

α＝v_e·v_i

其中，·表示向量点积，如果夹角α<π/2，此时速度应为正，该路径点归类为前进路径片段；如果两个向量的夹角α≥π/2，此时速度应为负，路径点归类为倒车路径片段；

步骤5-4，重复上述步骤，将最优路径分割成若干条交替进行的前进路径片段和倒车路径片段。

6.根据权利要求1所述的轨迹规划方法，其特征在于，基于指数函数，分别针对每个路径片段的长度和运动方向生成各阶导数光滑有界的速度规划曲线，具体包括以下步骤：

基于指数函数构造速度函数；

根据姿态调节时对无人车驱动系统的约束条件确定速度函数中控制参数的取值范围；

基于控制参数的取值范围、路径片段的长度和运动方向，确定速度函数中控制参数值，获取速度规划曲线。

7.根据权利要求6所述的轨迹规划方法，其特征在于，所述速度函数为：

其中，e是自然指数，v_m、k和c均为控制参数，时间t是函数的自变量，v_m是函数的极限值，对应最大速度；k反应函数曲线的倾斜程度，决定最大加速度；c对应速度达到最大加速度所需的时间，对于速度的上升阶段和下降阶段，参数k取值相同，上升阶段v_m>0，下降阶段v_m<0。

8.根据权利要求7所述的轨迹规划方法，其特征在于，根据姿态调节时对无人车驱动系统的约束条件确定速度函数中控制参数的取值范围，具体包括以下步骤：

步骤8-1，根据开始时刻的速度v(0)＝v_m/(1+e^kc)和无人车能够响应的最小速度v_min，通过方程v(0)＝v_min确定参数k和c的关系(kc)＝ln(v_m/v_min-1)；

步骤8-2，计算速度函数上的最大加速度a_m＝kv_m/4，根据无人车最大允许加速度a_max，通过关系a_m≤a_max确定参数k的取值范围k≤4a_max/v_m；

步骤8-3，计算速度曲线上的最大加加速度

根据无人车最大允许加加速度j_max，通过关系j_m≤j_max确定参数k的取值范围

步骤8-4，根据步骤8-2和步骤8-3，确定同时满足两个约束时参数k的可行范围：k≤k_m，其中

9.根据权利要求8所述的轨迹规划方法，其特征在于，所述确定速度函数中控制参数值具体包括以下步骤：

步骤9-1，使用参数值v_m＝v_d和c＝(kc)/k_m对速度函数v(t)积分，得到速度上升阶段经历的距离

其中v_d是无人车在执行位姿调节时的期望速度；

步骤9-2，计算对应路径片段的长度s；

步骤9-3，另参数v_m＝v_d、k＝k_m和c＝(kc)/k，计算速度上升阶段经历的距离L＝cv_d＝ln(v_m/v_min-1)v_d/k_m；

步骤9-4，比较路径片段的长度s和速度上升阶段经历的距离L，如果s/2≥L，则参数v_m＝v_d、k＝k_m和c＝(kc)/k，得到起始段的速度函数，结束段的速度函数达到最大加速度，此时参数c₂＝2(s/2-L)/v_d+3c；如果s/2<L，求在约束s/2＝cv_m，kc＝ln(v_m/v_min-1)和k≤k_m下最大的v_m和k，根据c＝(kc)/k求出参数c，令结束段的参数c₂＝3c。

10.根据权利要求9所述的轨迹规划方法，其特征在于，对于s/2<L，通过求解以下两组非线性方程得到两组参数(k₁,v_m1)、(k₂,v_m2)：

确定控制参数v_m＝min(v_m1,v_m2)和对应的参数k。

Images