CN111896004A - 一种狭窄通道车辆轨迹规划方法及系统 - Google Patents

Abstract

本发明公开了一种狭窄通道自动驾驶车辆轨迹规划方法，包括以下步骤：步骤S1，获取车辆及作业环境的相关信息；步骤S2，基于所述自车及周边环境的相关信息，构建狭窄环境下车辆的行驶通道；步骤S3，基于构建的行驶通道，对该通道按其直行与拐弯特性进行区域分割；步骤S4，确定狭窄通道的通道中心线，离散通道中心线；步骤S5，将轨迹规划拆分为路径规划与速度规划；步骤S6，确定以高效性为目标的代价函数。本发明的狭窄通道自动驾驶车辆轨迹规划方法，通过步骤S1至步骤S6的设置，便可有效的实现基于车辆及作业环境的相关信息基础上，规划出优化轨迹了。

Description

一种狭窄通道车辆轨迹规划方法及系统

技术领域

本发明涉及自动驾驶领域，特别是关于一种狭窄通道车辆轨迹规划方法及系统。

背景技术

自动驾驶车辆是指在传统车辆上安装自动驾驶系统从而能够自动感知环境信息、自动作出运动决策并自动控制运动决策实施的车辆。自动驾驶因其在改善驾驶安全性、乘坐舒适性、燃油经济性以及道路流通率等方面的光明前景而受到学术界与工业界的广泛关注，成为未来车辆发展的一个重要方向。

轨迹规划是自动驾驶决策控制部分的核心技术之一。车辆的轨迹规划是指根据车辆的初始状态，目标状态以及车辆周围的环境信息规划出一条无碰撞且满足车辆非完整约束和动力学约束的可行路径和速度。目前轨迹规划方法主要包括：(1)基于图搜索的方法，如专利文件CN106643719A，通过分割车辆作业区域为若干多边形，采用直线段连接多边形各顶点，再搜索无碰撞连通直线段为智能割草车规划路径。图搜索方法对模型精度要求不高，生成的路径较为粗糙，需要经过后续精细化处理才能用于控制执行(2)基于随机采样的方法，如专利文件CN110531770A，通过一种由避障模块、分段拟合模块与信息提取模块共同组成的RRT算法生成一条完整路径。使用随机采样生成的轨迹多次处理同一问题的结果不相同，求解质量不稳定，无法得到满足约束条件的精确解。(3)基于曲线插值的方法，如专利文件CN110949374A通过目标位置信息构建轨迹分段判断模型确定轨迹分段位置，采用两段二阶贝塞尔曲线确定泊车轨迹。基于曲线插值的方法通过预设航路点得到连续平滑且可跟踪的路径，但其生成的轨迹因预设航路点的不同而质量不同。(4)基于最优控制的方法，如专利文件CN110647142A,考虑停车场、停车位、车辆运动学约束，计算多个潜在停车路径，再用二次优化计算停车路径成本，为自动驾驶车辆生成优化停车轨迹。基于最优控制的方法通过建立运动系统方程，确定系统约束条件与目标函数求解优化轨迹，生成的轨迹直观准确且具有良好的可跟踪性能。但是，受目前计算机计算能力的限制，基于最优控制的轨迹规划方法计算用时长，难以应用于自动驾驶车辆的实时轨迹规划求解。狭窄通道是自动驾驶车辆日常运行不可避免的场景，特别是对于一些特殊作业的车辆，其作业环境相对封闭，运行通道狭窄而行驶路线固定，因此，这些特殊作业的车辆对轨迹生成的实时性要求不高，理想生成的参考轨迹即可保证其正常作业运行。狭窄通道场景对轨迹的基本要求为安全可跟踪，在通道狭窄的背景下，基于图搜索方法生成的路径不能在保证路径可跟踪性能的同时保证不与通道边界发生碰撞，基于随机采样的方法也不能同时保证路径不与通道边界发生碰撞且具有可跟踪性。基于曲线插值的方法可保证路径的可跟踪性，但是对于通道狭窄的情况其生成的轨迹容易与通道边界发生碰撞。基于最优控制的方法生成的轨迹能够在保证不与边界发生碰撞的前提下生成具有良好可跟踪性能的轨迹。其计算用时长，难以应用于实时在线求解的缺陷并不影响其为特殊作业车辆的狭窄通道运行离线生成参考优化轨迹。

发明内容

针对现有技术存在的不足，本发明的目的在于提供一种狭窄通道车辆轨迹规划方法及系统。其在安全性前提下以高效性为目标，为一些通行道路狭窄，行驶路线固定的特殊环境作业车辆，如地下铲运机，离线生成参考轨迹。

为实现上述目的，本发明提供了如下技术方案：一种狭窄通道自动驾驶车辆轨迹规划方法，包括以下步骤：

步骤S1，获取车辆及作业环境的相关信息；

步骤S2，基于所述自车及周边环境的相关信息，构建狭窄环境下车辆的行驶通道；

步骤S3，基于构建的行驶通道，对该通道按其直行与拐弯特性进行区域分割；

步骤S4，确定狭窄通道的通道中心线，离散通道中心线；

步骤S5，将轨迹规划拆分为路径规划与速度规划，以通道中心线的离散点为参考，确定车辆的运动学方程及车辆通过该狭窄通道应满足的约束关系；

步骤S6，确定以高效性为目标的代价函数，给定一组合适的初始值，迭代求解最小化代价函数所得最优控制序列，最优控制序列向前积分得优化轨迹。

作为本发明的进一步改进，S2中所述的行驶通道构建是指将原通道向内拟合为若干段直行通道，记录直行通道左右边界的位置信息，其中第i段左边界与第i段右边界位置信息分别用下述两式表示：

A_l(i)x+B_l(i)y+C_l(i)＝0；

A_r(i)x+B_r(i)y+C_r(i)＝0。

作为本发明的进一步改进，所述步骤S3中所述区域分割将构建的行驶通道按直行通道段与直行通道段间过渡段两种类型进行分割，其中，过渡段区域由前一通道段的尾端与后一通道段的前端组成，所述通道段的尾端与前端所占据长度约为一个车身车长。

作为本发明的进一步改进，所述步骤S4中所述通道中心线是指在新构建的行驶通道基础上所确定的中心线，对该中心线进行离散的步骤具体为：

步骤S4-1，沿通道中心线依次取特定间隔距离的点为离散点，其中过渡段区域的间隔距离取值须比直行段区域的间隔距离取值小；

步骤S4-2将通道中心线离散为N个点，记录每个离散点的位置信息并匹配其对应的间隔距离取值，如第k个离散点位置为(x_co(k),y_co(k))，其对应相匹配的离散间隔指标为Δs(k)。

作为本发明的进一步改进，所述步骤S5确定约束关系的具体步骤包括如下：

步骤S5-1，对车辆外轮廓近似化处理，确定车辆位置信息；

步骤S5-2，以通道中线心离散点为参考，确定车辆在空间域内的运动学约束；

步骤S5-3，以通道中线心离散点为参考，确定车辆在空间域内不与通道边界发生碰撞的约束条件；

步骤S5-4，以通道中线心离散点为参考，确定车辆执行机构运动限制在空间域内的约束；

步骤S5-5，以通道中线心离散点为参考，确定车辆运动量大小在空间域内的约束；

步骤S5-6，以通道中线心离散点为参考，分析狭窄通道车辆通行代价函数，求解空间域内，最小化代价函数所得的控制量序列及其向前积分所得的轨迹。

作为本发明的进一步改进，所述步骤S5-1中，将车辆外轮廓近似化为一个矩形，以通道中心线离散点为参考点，则取第k个参考点(x_co(k),y_co(k))对应的车辆后轴中心坐标为(x(k),y(k))，车辆横摆角为

根据第k个参考点所对应的车辆后轴坐标、车辆的横摆角以及车辆的车长、车宽、轴距、前悬距、后悬距，可计算出第j个参考点所对应的车辆外轮廓近似矩形各顶点坐标，如下式：

作为本发明的进一步改进，所述步骤S5-2中，采用车辆单自由度前轮转向模型，以通道中心离散点为参考点，若第k个参考点(x_co(k),y_co(k))对应的车辆后轴坐标中心点为(x(k),y(k))，车辆横摆角为

车辆转向角为α(k)，则对于第k+1个参考点而言，车辆的后轴坐标与横摆角应满足下式的关系：

作为本发明的进一步改进，所述步骤S5-3中，车辆与构建的行驶通道不发生碰撞的约束条件为车辆近似拟合矩形左边界与行驶通道左边界无交点且车辆近似拟合矩形右边界与行驶通道右边界无交点，条件式如下：

作为本发明的进一步改进，所述步骤S5-4中，假定车辆最大转向角速度为ω_u，则对于车辆在第k个参考点与第k+1个参考点间，转向角α(k)与α(k+1)应满足的关系如下式；

所述步骤S5-5中，假定车辆最小加速度为a_min，最大加速度为a_max，则对于车辆在第k个参考点与第k+1个参考点间，转向角v(k)与v(k+1)应满足的关系如下式；

所述步骤S5-6中，代价函数可取车辆通过形式通道所需时间，如下式所示：

其中，最小化代价函数，可计算一组转向角α与速度控制序列v，该控制序列积分所得的轨迹即为目标轨迹

本发明另一方面提供了一种系统，包括环境感知模块、通道处理模块和轨迹规划模块；

其中，所述环境感知模块，用于获取车辆运行通道的原始道路信息，所述通道处理模块用于根据环境感知模块所获取的原始道路信息构建新的行驶通道并对新通道中心线作离散处理，所述轨迹规划模块用于根据通道处理模块得到的新通道信息及自身的参数信息生成车辆参考优化轨迹。

本发明的有益效果，采用最优控制方法，直接计算出达到目标轨迹所需的控制量。直接计算出的目标函数控制量准确、直观且便于下层控制器跟踪其轨迹。本方法能为作业环境固定的特殊环境作业车辆提供一条参考运动轨迹，轨迹生成的目标函数为车辆通过狭窄通道所需的时间，最小化代价函数可有效提升车辆的作业效率。

附图说明

图1是本发明实施例提供的狭窄通道离线轨迹规划流程示意图；

图2是本发明实施例提供的运动规划层示意图；

图3是本发明实施例提供的行驶通道构建示意图；

图4是本发明实施例提供的行驶通道区域分割示意图；

图5是本发明实施例提供的中心通道线离散示意图

图6是本发明实施例提供的车辆外轮廓近似矩形拟合示意图；

图7是本发明实施例提供的车辆前轮转向模型；

图8是本发明实施例提供法的边界线碰撞检测示意图；

图9是本发明实施例提供的参考路径生成示意图；

图10是本发明实施例提供的参考速度曲线示意图；

图11是本发明实施例提供的狭窄通道车辆轨迹规划系统。

具体实施方式

下面将结合附图所给出的实施例对本发明做进一步的详述。

如图1至11所示，本实施例所提供的的狭窄通道车辆离线轨迹规划方法包括以下步骤：

S1，获取车辆及作业环境的相关信息。车辆及作业环境信息包括自车的长l，宽w，轴距L，前悬距lf，后悬距lr，以及车辆原始作业通道左右边界的位置信息，这些信息由事先试验测试所得。

S2，基于所述自车及周边环境的相关信息，构建狭窄环境下车辆的行驶通道。如图3所示，所构建的行驶通道是通过将原通道向内拟合为若干直行通道段而获得的，因此构建的行驶通道由若干直行通道段衔接而成。假设行驶通道由n段直行通道组合而成，记录每条直行通道段左右边界的位置信息，其中位置信息由直线的一般表达式表示，如(1)，(2)式，故只需记录左边界系数集{A_l,B_l,C_l},与右边界系数集{Ar,Br,Cr}即可。

A_l(i)x+B_l(i)y+C_l(i)＝0；i＝1,2,…,n (1)

A_r(i)x+B_r(i)y+C_r(i)＝0；i＝1,2,…,n (2)

S3，基于构建的行驶通道，对该通道按其直行与拐弯特性进行区域分割。如图4所示，区域分割将构建的行驶通道分为直行通道区域与直行通道段间过渡区域两种类型，若所构建行驶通道由n段直行通道段构成，则直行通道区域的数目为n，过渡段区域数目为n-1,其中过渡段区域占据空间约为沿通道方向前一通道段尾端一个车身长l的距离与后一通道段前端一个车身长l的距离。

S4，确定狭窄通道的通道中心线，离散通道中心线。如图5所示，取所构建行驶通道的中心线为通道中心线，沿通道中心线方向按离散间隔序列Δs依次取点离散，离散间隔序列Δs遵循的准则为直行段区域与过渡段区域间隔距离分别均匀离散且过渡段区域间隔距离取值需较直行段区域间隔距离取值小，由此适应过渡段区域较直行段区域车辆运动更为复杂的特性。设定直线段区域离散间隔为s1,过渡段区域离散间隔为s2，s₂＜s₁，已知行驶通道由n组直行通道段构成，则Δs由n-1组s1与n组s2构成。通道中心线离散后得N个离散点，记录中心通道线离散点位置信息，由(3)式表示。

S5，将轨迹规划拆分为路径规划与速度规划，以通道中心线的离散点为参考，确定车辆的运动学方程及车辆通过该狭窄通道应满足的约束关系。确定以高效性为目标的代价函数，给定一组合适的初始值，迭代求解最小化代价函数所得最优控制序列，最优控制序列向前积分所得为目标轨迹。由图2，S5可分为如下步骤：

S5-1，对车辆外轮廓近似化处理，确定车辆位置信息。如图6，将车辆外轮廓近似化为一个矩形，以通道中心线离散点(x_co(k),y_co(k))为参考点，则取其对应的车辆后轴中心坐标为(x(k),y(k))，车辆横摆角为

根据离散参考点序列所对应的车辆后轴中心坐标序列、车辆的横摆角以及车辆的车长、车宽、轴距、前悬距、后悬距，可计算出中心参考离散点序列所对应的车辆外轮廓近似矩形各顶点A,B,C,D的坐标序列，如式(4)，(5)，(6)，(7)。

S5-2，以通道中线心离散点为参考，确定车辆在空间域内的运动学约束。如图7采用车辆单自由度前轮转向模型，以通道中心离散点为参考，已知车辆初始位置后轴中心坐标(x₀,y₀)、初始位置横摆角

目标点位置后轴中心坐标(x_e,y_e)、目标点位置车辆横摆角

在控制序列α的作用下，车辆轨形是确定的，且车辆的位姿受(8)，(9)，(10)式约束。

S5-3，以通道中线心离散点为参考，确定车辆在空间域内不与通道边界发生碰撞的约束条件。如图8所示，显然车辆与构建的行驶通道不发生碰撞只需满足车辆近似拟合矩形左边界与行驶通道左边界无交点且车辆近似拟合矩形右边界与行驶通道右边界无交点。对某一直行通道段，若通道左边界与近似拟合矩形左边界无交点则需满足近似矩形左边界上的点指向通道左边界上点构成的向量

在通道左边界指向通道内部的法向量

上的投影为负，同理，若通道右边界与近似拟合矩形右边界无交点则需满足近似矩形右边界上的点指向通道右边界上点构成的向量

在通道右边界指向通道内部的法向量

上的投影为负。因矩形两边线段是凸集，证明某矩形边界与其对应通道边界无交点，仅需分别证明矩形左右边界两端点指向其对应通道边界构成的向量在对应的通道边界指向通道内部的法向量上的投影为负即可，则对图8所示的位置关系，车辆与边界不发生碰撞需满足式(11)，(12)，(13)，(14)的约束。

特别地，设置过渡段区域为衔接处向两直线通道段分别延伸一个车长距离，从车辆开始进入过渡区域后半段到完全进入后半段，车辆近似拟合矩形前段与后端顶点判定边界碰撞对应的行驶通道不同。近似拟合矩形前端对应的判定行驶通道为后段直行通道，近似拟合矩形后端对应的判定行驶通道为前段直行通道。即若此时车辆由第i段直行通道进入第i+1段直行通道时，需满足(15)，(16)，(17)，(18)式关系。

S5-4，以通道中线心离散点为参考，确定车辆执行机构运动限制在空间域内的约束。如图7车辆单自由度前轮转向模型可得车辆转向角α与转向角速度ω关系如(19)式。故若车辆最大转向角速度为ω_u，则车辆转向角与速度应满足的关系(20)式约束条件。

S5-5，以通道中线心离散点为参考，确定车辆运动量大小在空间域内的约束。车辆运动过程中，允许最小加速度为a_min，允许最大加速度为a_max，则对于车辆在通道行驶的过程中，速度应满足的关系如式。

S5-6，以通道中线心离散点为参考，分析狭窄通道车辆通行代价函数，求解空间域内，最小化代价函数所得的控制量序列及其向前积分所得的轨迹。因本发明针对对象为狭窄通道特殊作业车辆，故考虑高效经济效益，则希望车辆尽可能开得通过行驶通道，代价函数如(22)式。综合前述约束条件，最小化目标函数可得一条目标轨迹。

本发明实施例的狭窄通道路径生成如图9所示，速度曲线如图10所示。在狭窄通道场景下，车辆获取通道信息，构建由直行通道段衔接而成的新的行驶通道。在新的行驶通道中基于通道中心线离散点构建最优控制模型，计算由转向角序列与速度序列构成的最优控制序列。计算所得最优控制序列积分得优化轨迹。

相对应的，本发明实施例提供的狭窄通道车辆轨迹规划系统，上述方法执行于该系统中，图11为本发明实施例提供的狭窄通道车辆轨迹规划系统。如图11所示，所述系统包括：环境感知模块、通道处理模块、轨迹规划模块。其中，所述环境感知模块用于获取车辆运行通道的原始道路信息，具体包括通道起始点与终点以及通道左右边界位置信息。所述通道处理模块用于根据环境感知模块所获原始道路信息构建新的行驶通道并对新通道中心线作离散处理，新的行驶通道由若干直行通道段衔接而成。所述轨迹规划模块用于根据通道处理模块得到的新通道信息及自身的参数信息采用最优控制方法，以高效性为目标，建立约束不等式，生成车辆参考优化轨迹。

以上所述仅是本发明的优选实施方式，本发明的保护范围并不仅局限于上述实施例，凡属于本发明思路下的技术方案均属于本发明的保护范围。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理前提下的若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (10)

1.一种狭窄通道自动驾驶车辆轨迹规划方法，其特征在于：包括以下步骤：

步骤S1，获取车辆及作业环境的相关信息；

步骤S2，基于所述自车及周边环境的相关信息，构建狭窄环境下车辆的行驶通道；

步骤S3，基于构建的行驶通道，对该通道按其直行与拐弯特性进行区域分割；

步骤S4，确定狭窄通道的通道中心线，离散通道中心线；

步骤S5，将轨迹规划拆分为路径规划与速度规划，以通道中心线的离散点为参考，确定车辆的运动学方程及车辆通过该狭窄通道应满足的约束关系；

步骤S6，确定以高效性为目标的代价函数，给定一组合适的初始值，迭代求解最小化代价函数所得最优控制序列，最优控制序列向前积分得优化轨迹。

2.根据权利要求1所述的狭窄通道自动驾驶车辆轨迹规划方法，其特征在于：S2中所述的行驶通道构建是指将原通道向内拟合为若干段直行通道，记录直行通道左右边界的位置信息，其中第i段左边界与第i段右边界位置信息分别用下述两式表示：

A_l(i)x+B_l(i)y+C_l(i)＝0；

A_r(i)x+B_r(i)y+C_r(i)＝0。

3.根据权利要求2所述的狭窄通道自动驾驶车辆轨迹规划方法，其特征在于：所述步骤S3中所述区域分割将构建的行驶通道按直行通道段与直行通道段间过渡段两种类型进行分割，其中，过渡段区域由前一通道段的尾端与后一通道段的前端组成，所述通道段的尾端与前端所占据长度约为一个车身车长。

4.根据权利要求3所述的狭窄通道自动驾驶车辆轨迹规划方法，其特征在于：所述步骤S4中所述通道中心线是指在新构建的行驶通道基础上所确定的中心线，对该中心线进行离散的步骤具体为：

步骤S4-1，沿通道中心线依次取特定间隔距离的点为离散点，其中过渡段区域的间隔距离取值须比直行段区域的间隔距离取值小；

步骤S4-2将通道中心线离散为N个点，记录每个离散点的位置信息并匹配其对应的间隔距离取值，如第k个离散点位置为(x_co(k),y_co(k))，其对应相匹配的离散间隔指标为Δs(k)。

5.根据权利要求4所述的狭窄通道自动驾驶车辆轨迹规划方法，其特征在于：所述步骤S5确定约束关系的具体步骤包括如下：

步骤S5-1，对车辆外轮廓近似化处理，确定车辆位置信息；

步骤S5-2，以通道中线心离散点为参考，确定车辆在空间域内的运动学约束；

步骤S5-3，以通道中线心离散点为参考，确定车辆在空间域内不与通道边界发生碰撞的约束条件；

步骤S5-4，以通道中线心离散点为参考，确定车辆执行机构运动限制在空间域内的约束；

步骤S5-5，以通道中线心离散点为参考，确定车辆运动量大小在空间域内的约束；

步骤S5-6，以通道中线心离散点为参考，分析狭窄通道车辆通行代价函数，求解空间域内，最小化代价函数所得的控制量序列及其向前积分所得的轨迹。

6.根据权利要求5所述的狭窄通道自动驾驶车辆轨迹规划方法，其特征在于：所述步骤S5-1中，将车辆外轮廓近似化为一个矩形，以通道中心线离散点为参考点，则取第k个参考点(x_co(k),y_co(k))对应的车辆后轴中心坐标为(x(k),y(k))，车辆横摆角为

根据第k个参考点所对应的车辆后轴坐标、车辆的横摆角以及车辆的车长、车宽、轴距、前悬距、后悬距，可计算出第j个参考点所对应的车辆外轮廓近似矩形各顶点坐标，如下式：

7.根据权利要求5或6所述的狭窄通道自动驾驶车辆轨迹规划方法，其特征在于：所述步骤S5-2中，采用车辆单自由度前轮转向模型，以通道中心离散点为参考点，若第k个参考点(x_co(k),y_co(k))对应的车辆后轴坐标中心点为(x(k),y(k))，车辆横摆角为

车辆转向角为α(k)，则对于第k+1个参考点而言，车辆的后轴坐标与横摆角应满足下式的关系：

8.根据权利要求7所述的狭窄通道自动驾驶车辆轨迹规划方法，其特征在于：所述步骤S5-3中，车辆与构建的行驶通道不发生碰撞的约束条件为车辆近似拟合矩形左边界与行驶通道左边界无交点且车辆近似拟合矩形右边界与行驶通道右边界无交点，条件式如下：

9.根据权利要求8所述的狭窄通道自动驾驶车辆轨迹规划方法，其特征在于：所述步骤S5-4中，假定车辆最大转向角速度为ω_u，则对于车辆在第k个参考点与第k+1个参考点间，转向角α(k)与α(k+1)应满足的关系如下式；

所述步骤S5-5中，假定车辆最小加速度为a_min，最大加速度为a_max，则对于车辆在第k个参考点与第k+1个参考点间，转向角v(k)与v(k+1)应满足的关系如下式；

所述步骤S5-6中，代价函数可取车辆通过形式通道所需时间，如下式所示：

其中，最小化代价函数，可计算一组转向角α与速度控制序列v，该控制序列积分所得的轨迹即为目标轨迹

10.一种应用权利要求1至9任意一项所述方法的系统，其特征在于：包括环境感知模块、通道处理模块和轨迹规划模块；

其中，所述环境感知模块，用于获取车辆运行通道的原始道路信息，所述通道处理模块用于根据环境感知模块所获取的原始道路信息构建新的行驶通道并对新通道中心线作离散处理，所述轨迹规划模块用于根据通道处理模块得到的新通道信息及自身的参数信息生成车辆参考优化轨迹。

Images