广告
结构化道路智能车辆的时域可预测轨迹规划和分层运动控制
2019-09-18 06:40:45· 来源：同济智能汽车研究所
编者按：基于增量采样的快速扩展随机树（RRT），能很好处理考虑车辆kinodynamics约束的规划问题。然而基础RRT算法存在采样效率低、生成轨迹抖动等等问题。运动控
编者按：基于增量采样的快速扩展随机树（RRT），能很好处理考虑车辆kinodynamics约束的规划问题。然而基础RRT算法存在采样效率低、生成轨迹抖动等等问题。运动控制是自动驾驶车辆另外一个重要技术，被广泛应用的以PID 控制为基础的算法认为车辆相对于轨迹的运动学关系假设为线性的，在动力学上，缺少对车辆横摆角速度上限和对底层执行器饱和约束的考虑。基于上述存在的问题，本文基于优化RRT的路径规划方法和解耦的速度规划方法，结合闭环仿真的预测，将路径-速度解耦的轨迹实现时间-空间的配准，准确预估实际跟踪轨迹。
本文译自：
IEEE IV 2019
原文标题：
" Predictable Trajectory Planner in Time-domain and Hierarchical Motion Controller for Intelligent Vehicles in Structured Road "
原作者为来自同济智能汽车研究所的熊璐教授、李志强硕士等。
摘要：路径规划及其跟踪作为智能车辆的基本模块，近来得以迅速发展。但路径-速度解耦的规划方法生成的路径在时域内并不可行。在本文中，提出了一种基于改进RRT的路径规划方法，实现高效规划与路径平滑。文章采用闭环预测跟踪路径的方法，实现轨迹点在空间与时间上的匹配，更准确地得知控制系统跟踪路径。为适应车辆横/纵向动力学的非线性约束和执行器饱和，文章设计了统一的条件积分控制律，该方法可以保证跟踪误差全局渐近稳定，同时避免因为控制量约束使积分运算发散所造成的控制器性能退化。经仿真和试验证明，本文提出的规划方法更为高效，控制算法能够有效跟踪规划轨迹，预测方法能准确预测实际轨迹，这对碰撞检测非常重要。
1、前言
运动规划模块是自动驾驶车辆的基本技术之一，同样受到重视而发展迅速。运动规划问题最早在移动机器人领域提出和研究，其中一些著名的规划方法包括Dijkstra、A*和它们的变形（D*，混合A*等）[1-4]，基于网格化的规划环境搜索路径，不能容易地考虑车辆复杂的动力学和微分约束。
相比之下，基于增量采样的快速扩展随机树[5]（RRT），则更能应对这种考虑车辆动态约束的规划问题。基础RRT算法同样存在采样效率低、生成轨迹抖动等不足。为提高规划效率，双向RRT[6]和RRT-connect[7]相继被提出。目标偏向等加速手段同样被应用。为适应特殊场景，基于障碍物边界的RRT算法[8]和关注狭窄通道的算法[9]相继提出。考虑车辆的非完整约束，文献[10]在构造树的过程中考虑扩展的相邻边夹角符合车辆约束。为减少RRT轨迹的抖动，文献[9]设计了后处理方法，对成功子树进行修剪。获得成功子树后，采用各类曲线平滑，如Dubins路径、回旋曲线和贝赛尔曲线等[11]。
通过这种方式，速度规划器实现路径-速度解耦[12-13]，也就是说——在生成轨迹之后，在每个轨迹点上规划速度。这种方式先在空间域内得到符合曲率约束的轨迹形状，并满足如横向加速度等动态约束。然而，它的主要缺点在于，由于速度的变化，轨迹点间时间差不固定。而实际上，车辆的控制器和执行器有固定的计算频率，控制动作间时间差固定。显然地，上述规划方法获得的轨迹在现实中是不可行的。
在轨迹预测方面，MIT团队[14-15]在参加DARPA比赛时应用闭环轨迹预测的方法平滑路径形状，但是其速度规划策略不适用于对起/终点速度有约束的局部路径规划场景。后续的工作[16-17]多用此方法消除定位导致的位置或航向偏差，而忽略了对轨迹跟踪预测的作用。
运动控制是自动驾驶车辆另外一个重要技术。基于车辆动力学模型的闭环反馈控制算法,根据车辆相对路径的侧向位移误差,对其进行闭环反馈控制，其中PID [18]控制被应用得较为广泛。Chaib S [19]通过PID和H∞的控制方法相结合对车辆侧向运动进行控制，有较好的鲁棒性。在一般PID控制基础上，Marino R [20]设计了嵌套PID控制，对侧向位移误差和横摆角速度分层进行反馈控制，并对系统的稳定性进行了分析。总的来说,这些控制算法认为车辆相对于轨迹的运动学关系假设为线性的，在动力学上，缺少对车辆横摆角速度上限和对底层执行器饱和约束的考虑，会导致车辆失稳[21]。
基于上述问题，提出了一种基于优化RRT的路径规划方法和解耦的速度规划方法来进行轨迹平滑。在获得轨迹之后，结合闭环仿真的预测，实现时间-空间的配准，准确预估实际跟踪轨迹。基于纵侧解耦的运动控制框架，分别设计了侧向运动的转向控制律和纵向运动的驱/制动扭矩/压力控制律，并统一使用条件积分控制律。本文剩下部分组织如下：第二节主要介绍基于优化RRT的轨迹规划和解耦的速度规划，并利用闭环预测方法获取可行的路径。第三节介绍了用于规划路径的跟踪的统一控制算法。第四节分析仿真和试验结果，第五节是结论。
2、轨迹规划与预测
由于基础的RRT轨迹计算方法存在前述的问题，我们依据结构化道路的特点优化基础的RRT算法（称为PB-RRT，prunedbias-sampled RRT）。选择带有偏向性的采样方法来加速扩展和适应动态需求，例如适用于车辆的最临近点判断策略和后处理方法。然后可以规划每一个轨迹点满足车辆动力学约束的速度。由于位置和速度是已知的，因此可以预测智能车辆如何跟踪规划的轨迹。总的规划和预测过程如下所示：
A. 采样方法
在结构化道路上，在整个空间内完全随机采样会浪费大量时间。选择偏向目标的高斯采样以提高效率。每个采样点生成为
其中(x0,y0)表示采样参考点的坐标，(sx,sy)表示采样点坐标，(r0,θ0)是相对于(x0,y0)的偏置参数，即高斯分布的均值，(σr,σθ)是径向和圆周方向的高斯分布标准差，(rrand,θrand)是符合标准高斯分布的随机变量。在这种情况下，采样点将在环形或扇形区域中随机生成，如图1所示。
图1 高斯采样区域
图2 结构化道路上不同场景的采样区域
根据驾驶模式和目标点位置，可以设置合适的(r0, θ0)和(σr, σθ)，引导搜索向着目标点的方向进行。换道工况下，将采样区域固定在一个扇形区域内，如图2所示，该区域的中心线是以起点和终点为端点的线段。相应的，在转向工况，根据目标点位置和车道中心线分别构造两个偏向采样区域。兼顾采样效率和概率完备性，在整个空间内完全随机采样并以较小频率选择目标点。
B. 最临近点搜索策略
如何选择最邻近点的方式对随机树的形状和生长方向很重要，这决定了路径的长度和抖动程度。传统的RRT方法直接计算节点之间的欧几里德距离。在针对车辆进行规划时，应考虑节点间的夹角关系，使得轨迹趋于平滑。由于距离和角度的量纲不同，使用简单的线性归一化方法对两个变量进行处理。
其中，N1(•) and N2(•)分别是距离和角度的归一化方法。
C. 考虑最大曲率约束的剪枝方法
RRT采样过程具有随机性，直接利用采样点获得的路径进行平滑会导致生成的轨迹非常曲折。因此有必要对有效路径子树基于最大曲率约束进行处理，删除不必要的节点、插入必要的节点。
如图3所示，从起始节点开始依次连接后续的路径节点，如果连线与空间内障碍物无交集，那么它们之间的路径点删去，将两节点直接相连，依次类推；否则，将保留初始节点和最后一个节点的父节点之间的线路，并将最后一个节点设置为下一个起始节点。继续执行上述过程，直到达到目标节点。在修剪后获得新的子树时，为了满足曲率最大约束，可能需要将另一个无碰撞节点插入当前子树以确保相邻边缘之间的每个角度不小于αmin。
图3 成功子树的Prunimg算法
在获得修剪后的子树后，所有节点及其中点都用作控制点，用于构建B样条曲线以平滑路径。本文选择了B样条，因为它的最大路径曲率很容易根据约束进行限制。相邻顶点的长度L和它们之间的角度α需要满足如下条件[22]：
假设两条边的长度不相等，则较小的长度将被视为L。
D. 碰撞检测
根据环境地图和车辆结构的碰撞检测方法被广泛使用。如图4所示，车辆结构被构造成几个圆圈以检测在网格图中是否将发生碰撞。尺寸参数设计为
每个圆圈占用的每个网格都需要进行安全性判断。只要有一个不安全的圆圈，这个采样点就不安全。
E. 速度规划
根据解耦轨迹-速度规划思想，在生成与静态障碍物无碰撞的平滑路径后，可以根据动态障碍物的动态约束和避障要求来规划速度[12-13]。由于已经获得路径，因此可以根据它们的位置计算从起点开始的每个路径点的长度s。另外，还可以计算航向角和曲率κ，并用s表示为自变量。类似地，其他参数也可以设计为s作为自变量，例如速度v，加速度a和横摆率ω。
速度规划需要考虑边界条件和动态约束。决策模块给出的边界条件是初始位置的速度vi，初始位置的加速度ai和目标位置的速度vg。速度限制的动态约束主要包括：
对于路径上的每个点，满足上述约束的允许最大速度可以计算为：
其中，Δv为安全阈值。此外加速度也应限制在[amin，amax]内。因此，我们的方法是基于具有最大曲率（绝对值）和速度边界的点处的允许速度来设计加速度曲线。然后，每个点的速度可以计算为。在本文中，首先确定曲率峰值处的最大速度（绝对值）。如果起始位置和目标位置的速度较小，则速度不会改变。否则，规划减速或加速线性变化的加速度（以s作为自变量）。例如，我们在26m的纵向距离内构建了车道变换的情景，其中横向距离为3.5m，vi=40km / h，vg=36km / h，ai =0。规划的轨迹和曲率、速度曲线如图5所示。
图5 基于路径-速度解耦方法的速度规划实例
由于已经确定了路径长度和速度，因此还可以计算路径点之间的时间间隔。这种解耦的规划方法清晰且易于计算。但是，也存在明显的缺点。在改变速度的情况下，路径点之间的时间间隔是不确定的和不稳定的，这与实际情况不一致。实际上，当智能车辆跟踪规划路径时，控制模块的执行器具有固定的计算频率（通常为10Hz-50Hz）。这会导致基于空间约束的轨迹与存在时域约束的控制跟踪的失配。因此，存储在路径点中的大量信息（航向，曲率，速度等）将被忽略，因为在跟踪规划轨迹时控制器会跳过大量的点。实际行驶的轨迹与规划的理想轨迹将有较大偏差。
F. 闭环预测轨迹
针对上述问题，本文采用闭环预测的方法解决这种失配。闭环预测的子系统由车辆模型和底层控制器组成，如图6所示。获取运动规划的路径之后，选择某一路径点作为控制器输入，控制器结合车辆当前状态计算控制输入，车辆模型用控制输入估计下一时刻车辆状态。出于计算简便性速度的需求，本文选择车辆运动学模型、Stanley转向控制器和PI速度控制器，这些在闭环预测相关工作中广泛应用[23-24]。
车辆运动学自行车模型被描述为
其中，x和y指后轴位置，是车辆航向角，v是车辆的前行速度，δ是前轮转向角，L是车辆轴距。Stanley方法选取距离前轴中心最近点跟踪，转向和速度控制为
其中θe是当前车辆航向和跟踪点航向间的误差，vcur和vcmd分别指车辆当前实际速度和期望速度。ks，kp和ki是增益参数。
图6 闭环预测流程图
图7 鲁棒分层运动控制器
从起点开始，不断向前模拟更新车辆状态，直至到达路径终点，生成可跟踪的路径和控制序列。由于所用的控制器执行频率为50Hz，设置向前仿真的时间间隔为0.02s。
3、分层运动控制器
为了实现智能车辆的规划轨迹的执行，我们设计了一个鲁棒的分层运动控制器，负责产生期望的驱动扭矩/制动压力和转向角。如图7所示，我们的分层运动控制器由运动控制器（顶层）和动态控制器（底层）组成。由于在跟踪智能车辆的规划轨迹时路径跟踪比速度控制更重要，因此选择纵向和横向解耦车辆模型来减弱纵向运动的影响。由于顶层采用解耦横向和纵向运动控制算法，底层由纵向动力学控制器和横向动力学控制器组成，其中前者产生驱动电机系统和电液制动系统的指令，后者产生电动转向系统的指令。所有控制器都采用统一的条件积分算法，其稳定性可以通过Lyapunov稳定性理论验证[25]。
A. 运动车辆解耦模型
A. 运动车辆解耦模型
如图8所示，X-E0-Y是大地坐标系，而x-Eb-y是车辆坐标系。当跟踪参考轨迹时，选择某个点作为跟踪点（P0），并且Pk是轨迹上的跟踪点。跟踪点的速度（vd）方向与参考轨迹相切。车辆坐标系中的跟踪点的坐标是（ls，lb）。根据车辆的运动学关系，可以将车辆运动学的状态方程描述为
其中，β是车辆侧滑角，φ是履带点的航向角，vx和vy是纵向和横向速度。
图8 轨迹跟踪模型原理图
图9 解耦运动学模型原理图
如图9所示，假设车辆的初始纵向位移误差为零，只要P轴在x轴方向上的速度总是等于vkx，则x轴的速度为P0，其中可以确保纵向位移误差xL_steady在任何纵向速度和P0的要求速度下都可以为零。
以这种方式，实现了在任何期望速度下的车辆轨迹跟踪，并且可以解耦纵向和横向运动控制。解耦的横向运动学表达式可以描述为
B.车辆横向动力学模型
如在智能车辆控制领域中普遍使用的那样，动态单车车辆模型包括力平衡方程和力矩平衡方程
其中，kf和kr是单个前轮或后轮的刚度，lf和lr分别是从车辆质心到前轮轴和后轮轴的距离。
图10 横向动态自行车车辆模型
这两个方程可以写成状态空间方程
C.纵向动力学模型
根据图11的动力学模型图，可以得到驱动轴的纵向动力学方程
其中，JRi是转动惯量，wRi是车轮转速，TRi是驱动转矩，Fx是纵向摩擦阻力，R是车轮半径。系统的输入是由执行器（驱动电机或电液制动系统）提供的正向或反向扭矩。单个轮胎的纵向力可以通过基于magic公式的轮胎纵向力的公式表示：
在行驶或制动期间，车轮的滑移率λ可以计算为
D.统一条件集成控制器
总的来说，上述横向和纵向系统可表示为
图11 纵向二自由度单轮模型
其中，x是系统状态，u是系统输入命令，y是系统输出，b是系统参数，t0是初始时间，x0是状态变量的初始值，f（t，x）是非线性连续系统函数。由于可以以相同的形式描述不同的系统，因此设计了统一的控制算法以应用于每个系统，其也可以满足致动器约束。
在（t0，x0）的某个邻域中，对于所有（t，x），（t，y）和f（t，x），可以满足不等式，如下所示：
其中，Lf是Lipschitz常数[26]。
状态的跟踪误差可以定义为：
误差控制系统是
输入u的控制规则设计如下：
其中，sat（•）是饱和函数，sgn（•）是符号函数：
4、仿真和实验结果
为了验证上述规划和控制算法的有效性，分别设计了仿真和实验测试。在仿真中，将PB-RRT算法与基本RRT算法进行了比较，解释了第二节中各步骤的优化过程，验证了算法的优越性。然后，我们在交叉和车道变换等城市场景下，基于改进的电动智能车辆平台进行了多次实验，验证了算法的实用性。
A.仿真结果
仿真在连续的车道变换场景中进行，以避免静态障碍。两个车道的宽度为7米，该区域的长度为50米，如图12所示。
红色矩形表示车辆的初始位置和最后位置，绿色点是RRT延伸过程的起点和目标点。红色折线是搜索的成功子树，蓝色折线是修剪的子树，绿色曲线是具有B样条曲线的平滑路径。
比较图12（a）和（b），可以看出偏向性采样可以加速扩展，考虑角度的距离测量方法可以成功减少子树的抖动。图12（c）展示了基于PB-RRT修剪原始子树的过程。图12（d）和（e）显示了基于B样条及其曲率曲线的平滑路径，从中可以证明基于B样条构造的曲线具有连续曲率和有限曲率最大值（绝对值）。
图12 路径规划器仿真结果
图13 改装电动智能测试车
B.实验结果
我们的实验测试是在改装的电动智能汽车上进行的，该汽车配备了集中驱动电机，转向电机和电控液压制动系统。通过激光雷达和单目相机获得环境信息，用于决策和轨迹规划。通过RTK和IMU获得车辆的确切位置。实验车允许的最大曲率为0.2m-1。如图14和图15所示，在交叉路口进行左右转弯的实验。黑线表示车道标记。道路宽度为3.5米。由于在转弯过程中曲率较大，因此选择了相对较低的均匀速度（10km/ h）。由于前瞻距离的存在，由车辆实际跟踪的轨迹和由闭环模拟预测的轨迹沿采样轨迹向内偏移。如图14（a）和图15（a）所示，我们的控制器可以实现规划路径的稳定跟随。如图14（b）和图15（b）所示，在转弯过程中，规划轨迹与跟踪轨迹之间的横向误差达到60cm，而预测轨迹与实际轨迹之间的横向误差只有10厘米，这对于碰撞检测非常重要。
图14 十字路口左转的轨迹
图15 十字路口右转的轨迹
图16 变道轨迹和速度跟踪
如图16（a）所示，跟踪轨迹与规划轨迹或预测轨迹之间的横向误差均在20cm以内，这也证明了我们的控制准则在不同情景下的实用性。在这种情况下，当这两个障碍物限制车道变换的纵向距离时，规划轨迹的曲率最大值相对较大。并且初始速度很高（25 km / h），超过了根据动力学约束允许的最大速度。此时，规划模块选择减速。如图16（b）所示，跟踪点的选择不连续，这意味着跳过路径上的某些点。证明了第二节的分析，这也可以解释闭环预测方法的效果。
5、实验
本文的主要贡献是提出一种改进的RRT轨迹规划器，称为PB-RRT，它具有更高的效率，可以为智能车辆获得平滑合适的轨迹。由于通过路径速度解耦方法获得的轨迹实际上是不可行的，因此采用闭环预测来估计车辆将如何跟踪规划路径。通过实验验证预测的准确性，这在检测碰撞时是有用的。为了适应车辆的非线性动力学约束和执行器的饱和，设计了纵向和横向控制的统一条件积分控制方法，可以保证跟踪误差的全局渐近稳定性。通过仿真和实验测试验证了上述所有规划方法，轨迹预测方法和控制方法的有效性。 联系人：李老师
电话：021-69589225
邮箱：11666104@tongji.edu.cn
广告 
广告
编辑推荐
最新资讯
-
大众汽车南京工厂或面临出售
2024-12-19 13:29
-
端到端专题：DDPG 基础算法与方法论介绍
2024-12-19 11:42
-
多模态大模型最新论文介绍
2024-12-19 11:42
-
建科股份收购苏州赛宝
2024-12-19 09:57
-
端云协同创变，中科创达与火山引擎共塑AI智
2024-12-19 09:56
广告
广告
