一种自动驾驶控制方法与流程

本发明涉及一种自动驾驶控制方法，属于自动驾驶技术领域。

背景技术：

汽车自动驾驶技术包括视频摄像头、雷达传感器以及激光测距器来了解周围的交通状况，并通过数据中心来实现，自动驾驶汽车相当于数据中心的遥控汽车或者智能汽车。智能驾驶技术在智能交通领域方面是极为重要的组成部分，随着计算能力的迅速提升以及智能交通自动化需求的日益提升，越来越多的国家研究机构与公司企业更多地关注智能驾驶这一领域。

目前公开的相关文献、报告主要是使用多个模式的判定以及匹配进行车辆的纵向控制，实现较为简单，容易在多个模式之间频繁切换，传统的车辆纵向控制采用定速巡航cc、自适应巡航acc和自动紧急制动aeb功能的融合算法，这种算法对于简单路况可行，但对于稍复杂的交通环境来说，难以采用统一程序框架进行描述，传统的车辆因状态及目标的频繁切换会造成车辆速度跳变问题，舒适性较差。

技术实现要素：

针对现有技术的缺陷，本发明的目的在于提供一种能够应对复杂交通环境，对行驶路线进行统一描述的，减少无人驾驶汽车速度跳变的舒适性好的自动驾驶控制方法。

为实现上述目的，本发明的技术方案为：

一种自动驾驶控制方法，包括以下步骤：

第一步，通过有人驾驶汽车采集路径地图；

第二步，通过摄像头获取无人驾驶汽车周边图像，并与路径地图的对应位置对比判断出是否存在障碍物，所述障碍物包括车或人或马路桩；

第三步，通过激光雷达测出无人驾驶汽车与障碍物之间的距离

第四步，根据路径地图以及障碍物距离信息，生成行驶路线；

第五步，根据样条插补法对行驶路线进行路线规划；

第六步，采用s型加减速方法对无人驾驶汽车进行速度控制；

第七步，无人驾驶汽车根据规划的路线并按照预定速度进行行驶。

本发明采用样条插补算法，能够对复杂的路线进行准确描述，通过获得路线上离散的点比如拐弯坐标以及障碍物坐标，直接计算出若干个中间点的坐标值，进而根据插补出的坐标值，控制无人驾驶汽车的运动。并采用s型加减速方法控制无人驾驶汽车的行驶速度，s型加减速控制，有速度曲线光滑、均匀、运动平稳、无跳变等优势，舒适好。s型加减速控制能较好的实现车辆的平滑启停和速度切换，可做到无加速的突变，能够较好的减少车辆的运动冲击和震荡，从而有效的提高自动驾驶车辆的舒适性。

现有的样条插补以及s型加减速控制主用用于数控机床加工方面，能够有效提高数控加工的加工精度以及加工效率，本发明打破现有技术偏见，把样条插补以及s型加减速控制应用到自动驾驶控制技术上，构思巧妙，方案详尽，切实可行，能够极大提高自动驾驶车辆的适用性以及舒适性。

作为优选技术措施，

对于复杂的路线进行行驶，首先需要对路线进行准确描述，比较容易获得路线上离散的点，如果根据离散点直接利用常规的参数曲线进行拟合，参数曲线的次数会比较高，也很难准确描述行驶路线；因此利用样条曲线对行驶路线进行描述；

所述行驶路线根据障碍物的移动随时进行调整，根据障碍物的位置以及路径特点并加上始末位置点构成样条插补的控制点；根据控制点以及必要的安全距离生成通过点；根据障碍物的移动速度、路径特点以及法定限速生成速度边界控制曲线；

样条插补法中的nurbs曲线由控制点、节点向量、权重因子、基函数四个因素构成；根据控制点能够通过相关公式得到节点向量，进而由节点向量能够得到基函数，权重因子一般给定或根据偏移量计算得到；根据已知的控制点在哈德利一贾德方法基础上进行变形计算出合适的节点向量；再根据需要的偏移量计算权重因子，得到权重因子和节点向量就能够得到基函数，进而利用控制点、基函数和权重因子构造nurbs曲线。

作为优选技术措施，

计算节点向量：

已知m+p+1个控制点p为所构造的基函数次数；根据控制点之间的距离远近，计算出合适的节点向量值u；

先计算控制点之间距离dj，公式为：

其中xj表示控制点pj的横轴坐标，yj表示控制点pj的纵轴坐标；

哈德利-贾德方法递推公式进行变形，变形后公式为：

其中ui表示第i个节点，进而可得节点向量u递推公式为：

求出的节点向量形式为：u＝[0，0，0，up，up+1，...um+p1，m+1，m+1，m+1]；

nurbs表达式：

利用节点向量求得基函数，nurbs基函数有很多种构造形式，一般常用的构造形式是由如下递推公式给出的，用下列递推方式确定的基函数ni，p(u)称为相应于节点向量u的p次nurbs基函数：

p≥2

其中i是基函数的序列号，给定节点向量u，根据上面的递推公式就能够推导出所需要的基函数；进而可得第i段nurbs曲线pi(u)，表达式为：

u∈[ui+p，ui+p+1]，i＝1，2，3...m+1。

nurbs曲线是一种特殊的参数曲线，它能对复杂曲线简洁、高效表述。nurbs曲线即非均匀有理b样条，它通过引入权重因子以及分母，能够精确的表示二次曲线，即能够准确描述各种复杂的路径。

作为优选技术措施，

权重因子对nurbs曲线的影响：

权重因子能够调节nurbs曲线与控制点的距离，权重因子取值越大，nurbs曲线越靠近相应的控制点，根据权重因子与偏移量的关系式，能够利用偏移量计算对应的权重因子，代入nurbs曲线表达式，进而实现对nurbs曲线的调整，权重因子根据经验取值或根据车身长度以及宽度确定，权重因子初始值都取1；

分别计算当u＝uk，wk＝1时，插补点p1，当u＝uk，wk＝0时，插补点p0；

进而可得距离量sk，kk，计算公式如下：

其中pk是第k个控制点，k＝0，1，2，...，m+p

可求得权重因子wk与偏移量dk之间的关系式，计算公式为：

作为优选技术措施，

无人驾驶汽车从起始位置运动到目标位置，涉及到车身位姿变化矢量的描述，把车身轴简化为一个直杆，描述一个直杆在空间的变化姿态；

给定无人驾驶汽车需要通过的离散点，以及车身轴相应变化角度，来描述它从起始点到目标点之间的位姿变化；根据无人驾驶汽车需要通过的离散点以及车身轴空间变化角度，车身轴的长度一般是固定不变；根据有效长度和对应的空间变化角度，利用车身通过点，能够求得车身轴的通过点，进而通过nurbs理论反算出相应二组控制点，根据控制点利用nurbs理论进行插补计算，生成一系列点，代入样条插补公式中进行反解计算，得到前后端处的位置信息；

给定无人驾驶汽车需要通过的离散点为ti＝(xti，yti，zti)，以及车身轴的一组变化角度(αi，βi)，车身的长度固定，设定为l，那对应的车身轴矢量的离散通过点坐标hi＝(xhi，yhi，zhi)的计算公式为：

xhi＝xti+lcosαicosβi

yhi＝yti+lcosαisinβi

zhi＝zti+lsinαi

通过上述步骤就能够求出车身通过点以及车身轴矢量另一组通过点，这样车身轴的变化就能清楚的描述出来，知道通过点根据nursb曲线生成理论就能够得到对应的控制点；

为了准确描述无人驾驶汽车的变化，还需要知道无人驾驶汽车的轨迹变化的切向量，由于nurbs曲线能够看作是关于u的参数方程，能够通过参数方程的求导理论计算得到车身插补轨迹的切向量，即基函数对u进行求导，然后再代入相关nurbs公式；

计算公式如下：

本发明根据无人驾驶汽车位姿的控制特点，提出通过二组相关的nurbs曲线进行描述位姿的变化，并且给出了曲线偏移量与权重因子的变化关系，以便能灵活的改变nurbs曲线，同时求出样条曲线的切向量便于反解计算，通过权重因子控制车辆与障碍物之间的距离，最大程度保证车辆行驶时安全以及完美避障。

作为优选技术措施，

对于无人驾驶汽车的初始位置和终点位置的速度均为零的情形；s型加减速方法为非对称型s型加减速控制方法：

常规的点到点轨迹规划，采用对称的s型加减速控制方法，运行过程被分为七段，分别是加加速度段、匀加速度段、减加速度段、匀速度段、加减速度段、匀减速度段、减减速度段；其中加加速度段、减加速段、加减速段与减减速段这四个时间段时间值相同，匀加速度段和匀减速度段时间值相同；考虑加加速度对机构运动的影响，本发明采用非对称的s型加减速控制方法，开始阶段即加速度段采用较大加加速度，结束阶段即减速度段采用较小的加加速度，消除因为加加速度过大导致车身停止时出现的较大振动，无法兼顾精度与效率的问题；

对于一段完整对称型s型加减速控制方法，根据时间最优算法，对它的轨迹规划实际上就是求解三个时间量：加加速度时间tj，匀加速度时间ta，匀速度时间tv，采用非对称型s型加减速控制方法，轨迹规划会比对称型s型加减速控制方法复杂的多，加加速度段与减加速段的时间相同，加减速段与减减速段的时间相同，但是二个变加速度时间段和二个变减速度段时间不再相同，同时匀加速度段和匀减速度段时间也不同，时间未知量变为5个；

加加速度变化率与各阶段时间的变化关系

虽然加加速度段和减减速度段时间值不相等，但根据后面的公式推导能够得知，二个阶段的时间值变化与加加速度前后变化比率相关；

不考虑到轨迹规划的约束条件，始末速度为零，加速度对时间积分得到速度值；

采用工程上常用的面积割补法，进行分析计算；

能够解得tj1、tj2的关系，

k为加加速度变化率，即加加速度阶段加加速度值与减减速度阶段加加速度值相比；

当tv1＝0时，据始末速度约束条件即始末速度为零，已知tj1、tj2的变化关系，利用面积割补法能够得到ta1、ta2变化关系；计算公式如下：

这样五个时间未知量又转为求三个时间量；

非对称s型加减速控制各阶段时间求解算法

根据时间最优原则，先不考虑限制条件，轨迹规划时只有加加速度段、减加速度段、加减速度段和减减速度段，不存在其他速度变化段，加速度变化；因此先令ta1＝0，ta2＝0，tv1＝0，求tj1，tj2值；根据非对称s型加减速控制方法可得到关于速度和位移的二元三次方程组，进而可得tj1，计算公式如下：

考虑速度，加速度限制要求由于最大速度出现减加速度段结束时，加速度最大值出现在加加速度段结束时，还必须对tj1进行修正计算公式如下：

经过上面对时间点的修正求出的tj1、tj2能满足速度和加速度的限制要求，但一般无法满足行驶位移的要求，一般会存在匀加速段、匀减速段和匀速段，根据时间最优原则令tv1＝0，类似tj1、tj2求解过程，可求得ta1、ta2值，进而求出tv1值；求出非对称s型加减速控制涉及的五个未知时间量，能够得到整个运动轨迹的速度曲线，进而利用泰勒展开式构造速度与参数u值的关系式，根据变化的u值以及上述关于双nurbs曲线理论完成插补运算，实现对无人驾驶汽车位姿的准确描述以及速度控制，进而实现样条插补公式高效、精确的运作。

本发明对于行驶速度的控制给出一种简洁、易理解的非对称的s型加减速控制方法，加加速度的绝对值前后不同，开始加速阶段的加加速度值较大，停止减速阶段的加加速度值较小，能有效避免车辆因为停止时振动过大，而影响行驶安全、舒适的问题，兼顾速度和安全、舒适问题。

作为优选技术措施，

对于无人驾驶汽车的初始位置和终点位置的速度不为零的情形；s型加减速方法为始末速度不为零的s型加减速控制方法：

始末速度不为零的s型加减速控制方法分为三种变化阶段七个时间段，三变化阶段为匀加加速度阶段、匀加速度阶段、匀速度阶段，匀加加速度阶段包括四个时间段：加加速度段、减加速度段、加减速度段、减减速度段；匀加速度阶段包括两个时间段：加速度段、减速度段；匀速度阶段只包括一个时间段：匀速度段；其中加加速度段与减加速段的时间相同，加减速段与减减速段的时间相同，可得始末速度与加减速时间的关系和位移与加减速时间的关系如下，其中vs表示始速度、ve表示末速度；

对始末速度不为零的轨迹进行规划，与始末速度为零的轨迹规划处理思路类似，需要求各阶段时间值，但是始末速度不为零的三阶轨迹规划涉及到五个时间量，方程组却是只有二个，因此求解难度比始末速度为零的轨迹规划大；始末速度不为零的s型加减速控制方法涉及的时间规划模式有十几种，如果按照它的分类模式进行考虑，就特别繁琐；因此需要采取一种更加合理、更加简便的求解方法；

根据行驶情形考虑，希望以最快的行驶效率完成行驶任务，同时需要满足汽车的精度要求，由于始末速度不为零的s型加减速控制方法涉及方程为五元三次方程组，并且只有二个方程，因此无法按照常规的方程组直接求解，因此能够先考虑行驶效率问题，再考虑行驶安全问题，逐步得到满足要求的时间规划方案即时间最优的算法；

基于牛顿迭代法的匀加加速度段时间求解

先考虑行驶效率问题，整个行驶阶段只存在匀加加速度阶段，匀加速度阶段和匀速度阶段都不存在，这种行驶方式，行驶效率最高；因此先令ta1＝0，ta2＝0，tv1＝0，求ti1，tj2值；

代入上式中可得到关于速度和位移的二元三次方程组：

求解二元三次方程组可采用牛顿迭代求出它的收敛值，首先把方程组构造成二个函数式f(tj1，tj2)、g(tj1，tj2)，：

求上述函数式组3-(39)的一阶偏导数并构成矩阵a：

式中：

f(tj1，tj2)关于tj1的一阶偏导数；f(tj1，tj2)关于tj2的一阶偏导数；

g(tj1，tj2)关于tj1的一阶偏导数；g(tj1，tj2)关于tj2的一阶偏导数；

求得偏导数矩阵a的逆矩阵a_inv，牛顿迭代需要选择合理的迭代初值，大多数情况下进行轨迹规划时，初始速度vs与终止速度ve都位于速度曲线端点，为了避免误差超限，一般该点的允许速度较小，并且速度相差不大，因此，可知tj1和tj2的最终值一般相差不大；在选取迭代初值时，令tj1＝tj2＝tj，可得tj计算公式：

将tj作为tj1的迭代初值，设置迭代精度，把tj赋值给tj10，tj20，把tj10、tj20分别代入上述函数式以及矩阵a，进而计算出一次迭代值tj11，tj21，计算公式如下所示：

利用计算出的迭代值计算迭代精度esp：

根据精度要求，代入迭代计算公式以及迭代精度计算公式进行迭代计算，求出满足要求的收敛值，代入后续的计算；

但是一般情况下，这种行驶方式无法满足行驶安全要求，可能还会超出车辆所能提供的加速能力；采用迭代法求出的收敛值只是近似最优解，并且考虑到速度和位移的限制，求出的收敛值可能与实际值存在较大误差，如果最终收敛值是一组负数解，根据实际情况，这时我们需要的就不是最终收敛值，而是最接近收敛解的一组正数解作为最终计算结果；进而得到符合要求的tj1、tj2值；

考虑速度、加速度限制，由于最大速度出现减加速度段结束时，与最大限制速度的所允许的加速时间进行对比，取其中的较小值，计算公式如下：

再考虑加速度限制要求，加速度最大值出现在加加速度段结束时，与最大限制加速度的所允许的加速时间进行对比，取其中的较小值，计算公式如下：

此时tj2与tj1相关，tj1改变则相应的tj2也需要重新计算，计算公式如下所示：

判断加减速阶段达到的加速度最大值也需要和最大限制加速度所允许的变化时间，时间值只能取较小值，计算公式如下：

这样得到符合最大限制速度和最大限制加速度要求的tj1、tj2的值；

通过牛顿迭代法以及迭代修正的求出的值，在一些情形下，可能存在着误差，需要进行再次修正，利用速始末速度和位移限制要求进行第二次修正，求出符合要求的tj1、tj2值；为了同时兼顾第一次迭代修正结果，tj1、tj2只能减少不能增加；速度误差v_esp计算如下公式所示：

设定某一精度值esp，如果v_esp＞esp，则tj1应减少一个采样周期ts，然后代入速度误差公式进行误差判断，如果还不满足要求，则继续减少，一直减少到满足要求为止，如果tj1减到零时也无法满足要求，则退出循环；如果v_esp＜-esp，则表明tj2应减少一个采样周期ts，然后代入速度误差公式进行误差判断，反复进行此过程，直到满足要求；

进而通过位移限制条件对求出的tj1，tj2，进行迭代修正计算，公式如下所示：

其中位移误差公式s_esp是实际位移与规划位移值之差；如果s_esp＞0，则说明按照计算出的时间进行行驶，实际位移值会小于所希望的位移值，这与实际要求不符，因此应当再次减少tj1的值，由于涉及的是位移的变化，它与tj1、tj2都相关，所以相应的tj2也要改变，可得tj2的计算公式如下：

然后代入位移误差公式进行计算误差值进行对比，反复此过程，当得到满足要求的值停止此过程；当s_esp＜0说明规划出的位移量小于所希望的位移值，这和实际情况相符，说明还有其他的速度变化段，结束迭代修正过程；

综上过程就能求出满足匀加加速度阶段时间tj1、tj2的值；

基于牛顿迭代匀加速时间段时间规划求解

经过上面对时间点的修正求出的tj1、tj2能满足速度和加速度的限制要求，但一般无法满足行驶位移的要求，可能还会有匀加速段、匀减速段和匀速段，考虑时间最优原则令tv1＝0，可得到关于始末速度和位移限制方程组：

根据上述理论，利用牛顿迭代法求出ta1、ta2的初值，然后利用迭代修正理论进行修正，最终得到满足要求的匀加速度阶段时间ta1、ta2的值；

匀速度阶段时间值

对于s型加减速轨迹规划算法涉及到三个阶段七个速度变化段，有五个时间未知量，根据效率最高原则，利用上述算法，能够求出tj1、tj2、ta1、ta2的值，不存在的阶段时间参数值等于0，现在只剩下一个未知量tv1，把tj1、tj2、ta1、ta2代入方程，方程变为关于tv1的一元一次方程，直接能够解出tv1值；可得计算公式如下：

通过上述公式能够计算得到tv1；

根据上述算法涉及的五个时间未知量都已求出，根据规划的时间量，即可规划出速度变化曲线，进而进行相应的插补计算。

由于s型加减速控制涉及求解多元高次方程或者不等式，并且要求求解出来的最终解是非负数，传统的算法是考虑各种加减速模式，逐步判断得到时间点，因此导致了计算过程繁琐，求解用时长。为此提出了根据时间最优原则和速度、加速度的限制条件把s型加减速控制中所涉及的五元非线性方程组分解为二个二元非线性方程组和一个一元线性方程，采用牛顿迭代法和迭代修正的方式逐步求解出符合要求的加减速时间，进而求出速度曲线，对车辆进行准确控制。该本发明能有效避免现有s型加减速控制的复杂、繁琐运算、简化了加减速模式分类，提供了一种精度高、易于理解的加减速控制方法。

作为优选技术措施，

样条插补算法即是确定每一采样周期无人驾驶汽车的坐标，进行曲线的密化生成，这个过程涉及到参数计算问题，需要考虑行驶安全、行驶效率问题以及车辆的性能问题，所以对样条参数有着诸多要求；根据规划出的每一采样周期内的速度值，利用b样条理论知识求出对应的下一插补点参数u值，计算公式如下所示；

ts是采样周期，v(t)规划求出的速度，ui为上一插补点的参数值，x′(u)为样条曲线横坐标的一阶导数，y′(u)为样条曲线纵坐标的一阶导数，x"(u)为样条曲线横坐标的二阶导数，y"(u)为样条曲线纵坐标的二阶导数。

作为优选技术措施，

对规划速度的曲线可能会出现速度突变的情形，这时需要对规划速度曲线进行光滑处理；为保证速度的光滑，必须要考虑加减速能力，根据速度突变程度，并考虑加速度变化情形，进行光滑处理；

采用三阶s型加减速控制时，加加速度是最小控制单元，加速度连续变化，所以光滑处理必须考虑同一周期内加速度大小；由于某一采样周期内速度突变需要考虑前后周期内的加减速能力，同时由于加速度不是固定不变，需要逐个周期判断，得出需要提前几个周期进行减速；

首先要从突变速度点，向前考虑，计算需要几个周期的降低速度，才能满足弓高误差限制要求；可采用循环累积的方式，判断出需要几个周期才能满足限制要求；计算公式如下所示；

acc＝ai-1+ai-2+...ai-n

其中：n为需要提前n个周期进行减速；ai为第i个采样周期的加速度值；acc为n个采样周期的加速度值累加值；vi为第i个采样周期的速度值；j为加加速度的最大值。

作为优选技术措施，

利用样条曲线对行驶路线进行描述；考虑计算精度以及计算量，nurbs曲线权重因子取1，取三次b样条，采用固定节点向量的方式，求出基函数，节点向量为：

u＝{-3，-2，-1，0，1，2，3}

截取u＝[0，1]有效的计算公式可得均匀三次b样条四个基函数：

样条曲线随着参数u的变化关系式为：

其中：p1(u)表示第l段样条曲线，p1+i表示第1段曲线第i个控制点，xl(u)表示控制点横坐标组成的向量，，y1(u)表示控制点纵坐标组成的向量，xl+i表示第1段第i个控制点的横坐标，.y1+i表示第1段第i个控制点的纵坐标；

由于需要行驶的轨迹是不规则的路径，无人驾驶汽车的速度以及方向变化不可能是任意的；在一个采样周期内，无人驾驶汽车的速度大小和方向一般固定，如果轨迹有突变，无人驾驶汽车的速度过大的情况下，就无法行驶出预期的路径；这就需要在一些位置，如位置拐点处，限制无人驾驶汽车的速度，保证行驶安全；根据行驶路径与无人驾驶汽车实际的运动轨迹关系，引入弓高误差的概念，进行分析二者的精度误差；在坐标拐点处对应的速度较小，如果要保证行驶安全，在这些点的进给速度一定不能大于这些给定的速度限制，但如果整个路径速度都按照这些速度极小值运行，就会影响行驶效率；因此综合考虑效率和精度问题，能够根据这些速度极小值所对应位置拐点对整个路径进行分段处理；对每段利用s型路径规划理论进行分析计算然后再运用三次样条理论进行插补规划：

弧形路线的曲率公式如下所示：

曲率半径计算公式如下所示：

已知控制点、采样周期，根据弓高误差要求，利用均匀样条理论生成控制点之间的位置坐标，求出曲率半径进而求出速度限制曲线；可得弓高误差限制下的速度计算公式：

其中：e为弓高误差，r为样条曲线半径，ts为采样周期。

由于非均匀有理b样条计算量大，计算繁琐，在一些样条插补算法中，为了减少计算，采用一种节点向量不变的均匀三次b样条算法。它能大大减少计算量，并且对样条的精度影响很小，因而广泛应用于样条插补计算中。

与现有技术相比，本发明具有以下有益效果：

本发明采用样条插补算法，能够对复杂的路线进行准确描述，通过获得路线上离散的点比如拐弯坐标以及障碍物坐标，直接计算出若干个中间点的坐标值，进而根据插补出的坐标值，控制无人驾驶汽车的运动。并采用s型加减速方法控制无人驾驶汽车的行驶速度，s型加减速控制，有速度曲线光滑、均匀、运动平稳、无跳变等优势，舒适好。s型加减速控制能较好的实现车辆的平滑启停和速度切换，可做到无加速的突变，能够较好的减少车辆的运动冲击和震荡，从而有效的提高自动驾驶车辆的舒适性。

现有的样条插补以及s型加减速控制主用用于数控机床加工方面，能够有效提高数控加工的加工精度以及加工效率，本发明打破现有技术偏见，把样条插补以及s型加减速控制应用到自动驾驶控制技术上，构思巧妙，方案详尽，切实可行，能够极大提高自动驾驶车辆的适用性以及舒适性。

附图说明

图1车身位姿变化；

图2车身切向量变化。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

相反，本发明涵盖任何由权利要求定义的在本发明的精髓和范围上做的替代、修改、等效方法以及方案。进一步，为了使公众对本发明有更好的了解，在下文对本发明的细节描述中，详尽描述了一些特定的细节部分。对本领域技术人员来说没有这些细节部分的描述也能够完全理解本发明。

一种自动驾驶控制方法，包括以下步骤：

第一步，通过有人驾驶汽车采集路径地图；

第二步，通过摄像头获取无人驾驶汽车周边图像，并与路径地图的对应位置对比判断出是否存在障碍物，所述障碍物包括车或人或马路桩；

第三步，通过激光雷达测出无人驾驶汽车与障碍物之间的距离

第四步，根据路径地图以及障碍物距离信息，生成行驶路线；

第五步，根据样条插补法对行驶路线进行路线规划；

第六步，采用s型加减速方法对无人驾驶汽车进行速度控制；

第七步，无人驾驶汽车根据规划的路线并按照预定速度进行行驶。

本发明采用样条插补算法，能够对复杂的路线进行准确描述，通过获得路线上离散的点比如拐弯坐标以及障碍物坐标，直接计算出若干个中间点的坐标值，进而根据插补出的坐标值，控制无人驾驶汽车的运动。并采用s型加减速方法控制无人驾驶汽车的行驶速度，s型加减速控制，有速度曲线光滑、均匀、运动平稳、无跳变等优势，舒适好。s型加减速控制能较好的实现车辆的平滑启停和速度切换，可做到无加速的突变，能够较好的减少车辆的运动冲击和震荡，从而有效的提高自动驾驶车辆的舒适性。

现有的样条插补以及s型加减速控制主用用于数控机床加工方面，能够有效提高数控加工的加工精度以及加工效率，本发明打破现有技术偏见，把样条插补以及s型加减速控制应用到自动驾驶控制技术上，构思巧妙，方案详尽，切实可行，能够极大提高自动驾驶车辆的适用性以及舒适性。

对于复杂的路线进行行驶，首先需要对路线进行准确描述，比较容易获得路线上离散的点，如果根据离散点直接利用常规的参数曲线进行拟合，参数曲线的次数会比较高，也很难准确描述行驶路线；因此利用样条曲线对行驶路线进行描述；

所述行驶路线根据障碍物的移动随时进行调整，根据障碍物的位置以及路径特点并加上始末位置点构成样条插补的控制点；根据控制点以及必要的安全距离生成通过点；根据障碍物的移动速度、路径特点以及法定限速生成速度边界控制曲线；

样条插补法中的nurbs曲线由控制点、节点向量、权重因子、基函数四个因素构成；根据控制点能够通过相关公式得到节点向量，进而由节点向量能够得到基函数，权重因子一般给定或根据偏移量计算得到；根据已知的控制点在哈德利-贾德方法基础上进行变形计算出合适的节点向量；再根据需要的偏移量计算权重因子，得到权重因子和节点向量就能够得到基函数，进而利用控制点、基函数和权重因子构造nurbs曲线。

计算节点向量：

已知m+p+1个控制点p为所构造的基函数次数；根据控制点之间的距离远近，计算出合适的节点向量值u；

先计算控制点之间距离dj，公式为：

其中xj表示控制点pj的横轴坐标，yj表示控制点pj的纵轴坐标；

哈德利-贾德方法递推公式进行变形，变形后公式为：

其中ui表示第i个节点，进而可得节点向量u递推公式为：

求出的节点向量形式为：u＝[0，0，0，up，up+1，...um+p1，m+1，m+1，m+1]；

nurbs表达式：

利用节点向量求得基函数，nurbs基函数有很多种构造形式，一般常用的构造形式是由如下递推公式给出的，用下列递推方式确定的基函数ni，p(u)称为相应于节点向量u的p次nurbs基函数：

p≥2

其中i是基函数的序列号，给定节点向量u，根据上面的递推公式就能够推导出所需要的基函数；进而可得第i段nurbs曲线pi(u)，表达式为：

u∈[ui+p，ui+p+1]，i＝1，2，3...m+1

nurbs曲线是一种特殊的参数曲线，它能对复杂曲线简洁、高效表述。nurbs曲线即非均匀有理b样条，它通过引入权重因子以及分母，能够精确的表示二次曲线，即能够准确描述各种复杂的路径。

作为优选技术措施，

权重因子对nurbs曲线的影响：

权重因子能够调节nurbs曲线与控制点的距离，权重因子取值越大，nurbs曲线越靠近相应的控制点，根据权重因子与偏移量的关系式，能够利用偏移量计算对应的权重因子，代入nurbs曲线表达式，进而实现对nurbs曲线的调整，权重因子根据经验取值或根据车身长度以及宽度确定，权重因子初始值都取1；

分别计算当u＝uk，wk＝1时，插补点p1，当u＝uk，wk＝0时，插补点p0；

进而可得距离量sk，kk，计算公式如下：

其中pk是第k个控制点，k＝0，1，2，...，m+p

可求得权重因子wk与偏移量dk之间的关系式，计算公式为：

作为优选技术措施，

无人驾驶汽车从起始位置运动到目标位置，涉及到车身位姿变化矢量的描述，把车身轴简化为一个直杆，描述一个直杆在空间的变化姿态；

给定无人驾驶汽车需要通过的离散点，以及车身轴相应变化角度，来描述它从起始点到目标点之间的位姿变化；根据无人驾驶汽车需要通过的离散点以及车身轴空间变化角度，车身轴的长度一般是固定不变；根据有效长度和对应的空间变化角度，利用车身通过点，能够求得车身轴的通过点，进而通过nurbs理论反算出相应二组控制点，根据控制点利用nurbs理论进行插补计算，生成一系列点，代入样条插补公式中进行反解计算，得到前后端处的位置信息；

给定无人驾驶汽车需要通过的离散点为ti＝(xti，yti，zti)，以及车身轴的一组变化角度(αi，βi)，车身的长度固定，设定为l，那对应的车身轴矢量的离散通过点坐标hi＝(xhi，yhi，zhi)的计算公式为：

xhi＝xti+lcosαicosβi

yhi＝yti+lcosαisinβi

zhi＝zti+lsinαi

通过上述步骤就能够求出车身通过点以及车身轴矢量另一组通过点，这样车身轴的变化就能清楚的描述出来，知道通过点根据nursb曲线生成理论就能够得到对应的控制点；

为了准确描述无人驾驶汽车的变化，还需要知道无人驾驶汽车的轨迹变化的切向量，由于nurbs曲线能够看作是关于u的参数方程，能够通过参数方程的求导理论计算得到车身插补轨迹的切向量，即基函数对u进行求导，然后再代入相关nurbs公式；

计算公式如下：

本发明根据无人驾驶汽车位姿的控制特点，提出通过二组相关的nurbs曲线进行描述位姿的变化，并且给出了曲线偏移量与权重因子的变化关系，以便能灵活的改变nurbs曲线，同时求出样条曲线的切向量便于反解计算，通过权重因子控制车辆与障碍物之间的距离，最大程度保证车辆行驶时安全以及完美避障。

本发明一种速度控制实施例：对于无人驾驶汽车的初始位置和终点位置的速度均为零的情形；s型加减速方法为非对称型s型加减速控制方法：

常规的点到点轨迹规划，采用对称的s型加减速控制方法，运行过程被分为七段，分别是加加速度段、匀加速度段、减加速度段、匀速度段、加减速度段、匀减速度段、减减速度段；其中加加速度段、减加速段、加减速段与减减速段这四个时间段时间值相同，匀加速度段和匀减速度段时间值相同；考虑加加速度对机构运动的影响，本发明采用非对称的s型加减速控制方法，开始阶段即加速度段采用较大加加速度，结束阶段即减速度段采用较小的加加速度，消除因为加加速度过大导致车身停止时出现的较大振动，无法兼顾精度与效率的问题；

对于一段完整对称型s型加减速控制方法，根据时间最优算法，对它的轨迹规划实际上就是求解三个时间量：加加速度时间tj，匀加速度时间ta，匀速度时间tv，采用非对称型s型加减速控制方法，轨迹规划会比对称型s型加减速控制方法复杂的多，加加速度段与减加速段的时间相同，加减速段与减减速段的时间相同，但是二个变加速度时间段和二个变减速度段时间不再相同，同时匀加速度段和匀减速度段时间也不同，时间未知量变为5个；

加加速度变化率与各阶段时间的变化关系

虽然加加速度段和减减速度段时间值不相等，但根据后面的公式推导能够得知，二个阶段的时间值变化与加加速度前后变化比率相关；

不考虑到轨迹规划的约束条件，始末速度为零，加速度对时间积分得到速度值；

采用工程上常用的面积割补法，进行分析计算；

能够解得tj1、tj2的关系，

k为加加速度变化率，即加加速度阶段加加速度值与减减速度阶段加加速度值相比；

当tv1＝0时，据始末速度约束条件即始末速度为零，已知tj1、tj2的变化关系，利用面积割补法能够得到ta1、ta2变化关系；计算公式如下：

这样五个时间未知量又转为求三个时间量；

非对称s型加减速控制各阶段时间求解算法

根据时间最优原则，先不考虑限制条件，轨迹规划时只有加加速度段、减加速度段、加减速度段和减减速度段，不存在其他速度变化段，加速度变化；因此先令ta1＝0，ta2＝0，tv1＝0，求tj1，tj2值；根据非对称s型加减速控制方法可得到关于速度和位移的二元三次方程组，进而可得tj1，计算公式如下：

考虑速度，加速度限制要求由于最大速度出现减加速度段结束时，加速度最大值出现在加加速度段结束时，还必须对tj1进行修正计算公式如下：

经过上面对时间点的修正求出的tj1、tj2能满足速度和加速度的限制要求，但一般无法满足行驶位移的要求，一般会存在匀加速段、匀减速段和匀速段，根据时间最优原则令tv1＝0，类似tj1、tj2求解过程，可求得ta1、ta2值，进而求出lv1值；求出非对称s型加减速控制涉及的五个未知时间量，能够得到整个运动轨迹的速度曲线，进而利用泰勒展开式构造速度与参数u值的关系式，根据变化的u值以及上述关于双nurbs曲线理论完成插补运算，实现对无人驾驶汽车位姿的准确描述以及速度控制，进而实现样条插补公式高效、精确的运作。

本发明对于行驶速度的控制给出一种简洁、易理解的非对称的s型加减速控制方法，加加速度的绝对值前后不同，开始加速阶段的加加速度值较大，停止减速阶段的加加速度值较小，能有效避免车辆因为停止时振动过大，而影响行驶安全、舒适的问题，兼顾速度和安全、舒适问题。

本发明另一种速度控制实施例：对于无人驾驶汽车的初始位置和终点位置的速度不为零的情形；s型加减速方法为始末速度不为零的s型加减速控制方法：

始末速度不为零的s型加减速控制方法分为三种变化阶段七个时间段，三变化阶段为匀加加速度阶段、匀加速度阶段、匀速度阶段，匀加加速度阶段包括四个时间段：加加速度段、减加速度段、加减速度段、减减速度段；匀加速度阶段包括两个时间段：加速度段、减速度段；匀速度阶段只包括一个时间段：匀速度段；其中加加速度段与减加速段的时间相同，加减速段与减减速段的时间相同，可得始末速度与加减速时间的关系和位移与加减速时间的关系如下，其中vs表示始速度、ve表示末速度；

对始末速度不为零的轨迹进行规划，与始末速度为零的轨迹规划处理思路类似，需要求各阶段时间值，但是始末速度不为零的三阶轨迹规划涉及到五个时间量，方程组却是只有二个，因此求解难度比始末速度为零的轨迹规划大；始末速度不为零的s型加减速控制方法涉及的时间规划模式有十几种，如果按照它的分类模式进行考虑，就特别繁琐；因此需要采取一种更加合理、更加简便的求解方法；

根据行驶情形考虑，希望以最快的行驶效率完成行驶任务，同时需要满足汽车的精度要求，由于始末速度不为零的s型加减速控制方法涉及方程为五元三次方程组，并且只有二个方程，因此无法按照常规的方程组直接求解，因此能够先考虑行驶效率问题，再考虑行驶安全问题，逐步得到满足要求的时间规划方案即时间最优的算法；

基于牛顿迭代法的匀加加速度段时间求解

先考虑行驶效率问题，整个行驶阶段只存在匀加加速度阶段，匀加速度阶段和匀速度阶段都不存在，这种行驶方式，行驶效率最高；因此先令ta1＝0，ta2＝0，tv1＝0，求tj1，tj2值；

代入上式中可得到关于速度和位移的二元三次方程组：

求解二元三次方程组可采用牛顿迭代求出它的收敛值，首先把方程组构造成二个函数式f(tj1，tj2)、g(tj1，tj2)，：

求上述函数式组3-(39)的一阶偏导数并构成矩阵a：

式中：

f(tj1，tj2)关于tj1的一阶偏导数；f(tj1，tj2)关于tj2的一阶偏导数；

g(tj1，tj2)关于tj1的一阶偏导数；g(tj1，tj2)关于tj2的一阶偏导数；

求得偏导数矩阵a的逆矩阵a_inv，牛顿迭代需要选择合理的迭代初值，大多数情况下进行轨迹规划时，初始速度vs与终止速度ve都位于速度曲线端点，为了避免误差超限，一般该点的允许速度较小，并且速度相差不大，因此，可知tj1和tj2的最终值一般相差不大；在选取迭代初值时，令tj1＝tj2＝tj，可得tj计算公式：

将tj作为tj1的迭代初值，设置迭代精度，把tj赋值给tj10，tj20，把tj10、tj20分别代入上述函数式以及矩阵a，进而计算出一次迭代值tj11，tj21，计算公式如下所示：

利用计算出的迭代值计算迭代精度esp：

根据精度要求，代入迭代计算公式以及迭代精度计算公式进行迭代计算，求出满足要求的收敛值，代入后续的计算；

但是一般情况下，这种行驶方式无法满足行驶安全要求，可能还会超出车辆所能提供的加速能力；采用迭代法求出的收敛值只是近似最优解，并且考虑到速度和位移的限制，求出的收敛值可能与实际值存在较大误差，如果最终收敛值是一组负数解，根据实际情况，这时我们需要的就不是最终收敛值，而是最接近收敛解的一组正数解作为最终计算结果；进而得到符合要求的tj1、tj2值；

考虑速度、加速度限制，由于最大速度出现减加速度段结束时，与最大限制速度的所允许的加速时间进行对比，取其中的较小值，计算公式如下：

再考虑加速度限制要求，加速度最大值出现在加加速度段结束时，与最大限制加速度的所允许的加速时间进行对比，取其中的较小值，计算公式如下：

此时tj2与tj1相关，tj1改变则相应的tj2也需要重新计算，计算公式如下所示：

判断加减速阶段达到的加速度最大值也需要和最大限制加速度所允许的变化时间，时间值只能取较小值，计算公式如下：

这样得到符合最大限制速度和最大限制加速度要求的tj1、tj2的值；

通过牛顿迭代法以及迭代修正的求出的值，在一些情形下，可能存在着误差，需要进行再次修正，利用速始末速度和位移限制要求进行第二次修正，求出符合要求的tj1、tj2值；为了同时兼顾第一次迭代修正结果，tj1、tj2只能减少不能增加；速度误差v_esp计算如下公式所示：

设定某一精度值esp，如果v_esp＞esp，则tj1应减少一个采样周期ts，然后代入速度误差公式进行误差判断，如果还不满足要求，则继续减少，一直减少到满足要求为止，如果tj1减到零时也无法满足要求，则退出循环；如果v_esp＜-esp，则表明tj2应减少一个采样周期ts，然后代入速度误差公式进行误差判断，反复进行此过程，直到满足要求；

进而通过位移限制条件对求出的tj1，tj2，进行迭代修正计算，公式如下所示：

其中位移误差公式s_esp是实际位移与规划位移值之差；如果s_esp＞0，则说明按照计算出的时间进行行驶，实际位移值会小于所希望的位移值，这与实际要求不符，因此应当再次减少tj1的值，由于涉及的是位移的变化，它与tj1、tj2都相关，所以相应的tj2也要改变，可得tj2的计算公式如下：

然后代入位移误差公式进行计算误差值进行对比，反复此过程，当得到满足要求的值停止此过程；当s_esp＜0说明规划出的位移量小于所希望的位移值，这和实际情况相符，说明还有其他的速度变化段，结束迭代修正过程；

综上过程就能求出满足匀加加速度阶段时间tj1、tj2的值；

基于牛顿迭代匀加速时间段时间规划求解

经过上面对时间点的修正求出的tj1、tj2能满足速度和加速度的限制要求，但一般无法满足行驶位移的要求，可能还会有匀加速段、匀减速段和匀速段，考虑时间最优原则令tv1＝0，可得到关于始末速度和位移限制方程组：

根据上述理论，利用牛顿迭代法求出ta1、ta2的初值，然后利用迭代修正理论进行修正，最终得到满足要求的匀加速度阶段时间ta1、ta2的值；

匀速度阶段时间值

对于s型加减速轨迹规划算法涉及到三个阶段七个速度变化段，有五个时间未知量，根据效率最高原则，利用上述算法，能够求出tj1、tj2、ta1、ta2的值，不存在的阶段时间参数值等于0，现在只剩下一个未知量tv1，把tj1、tj2、ta1、ta2代入方程，方程变为关于tv1的一元一次方程，直接能够解出tv1值；可得计算公式如下：

通过上述公式能够计算得到tv1；

根据上述算法涉及的五个时间未知量都已求出，根据规划的时间量，即可规划出速度变化曲线，进而进行相应的插补计算。

由于s型加减速控制涉及求解多元高次方程或者不等式，并且要求求解出来的最终解是非负数，传统的算法是考虑各种加减速模式，逐步判断得到时间点，因此导致了计算过程繁琐，求解用时长。为此提出了根据时间最优原则和速度、加速度的限制条件把s型加减速控制中所涉及的五元非线性方程组分解为二个二元非线性方程组和一个一元线性方程，采用牛顿迭代法和迭代修正的方式逐步求解出符合要求的加减速时间，进而求出速度曲线，对车辆进行准确控制。该本发明能有效避免现有s型加减速控制的复杂、繁琐运算、简化了加减速模式分类，提供了一种精度高、易于理解的加减速控制方法。

样条插补算法即是确定每一采样周期无人驾驶汽车的坐标，进行曲线的密化生成，这个过程涉及到参数计算问题，需要考虑行驶安全、行驶效率问题以及车辆的性能问题，所以对样条参数有着诸多要求；根据规划出的每一采样周期内的速度值，利用b样条理论知识求出对应的下一插补点参数u值，计算公式如下所示；

ts是采样周期，v(t)规划求出的速度，ui为上一插补点的参数值，x′(u)为样条曲线横坐标的一阶导数，y′(u)为样条曲线纵坐标的一阶导数，x"(u)为样条曲线横坐标的二阶导数，y"(u)为样条曲线纵坐标的二阶导数。

对规划速度的曲线可能会出现速度突变的情形，这时需要对规划速度曲线进行光滑处理；为保证速度的光滑，必须要考虑加减速能力，根据速度突变程度，并考虑加速度变化情形，进行光滑处理；

采用三阶s型加减速控制时，加加速度是最小控制单元，加速度连续变化，所以光滑处理必须考虑同一周期内加速度大小；由于某一采样周期内速度突变需要考虑前后周期内的加减速能力，同时由于加速度不是固定不变，需要逐个周期判断，得出需要提前几个周期进行减速；

首先要从突变速度点，向前考虑，计算需要几个周期的降低速度，才能满足弓高误差限制要求；可采用循环累积的方式，判断出需要几个周期才能满足限制要求；计算公式如下所示；

acc＝ai-1+ai-2+...ai-n

其中：n为需要提前n个周期进行减速；ai为第i个采样周期的加速度值；acc为n个采样周期的加速度值累加值；vi为第i个采样周期的速度值；j为加加速度的最大值。

作为优选技术措施，

利用样条曲线对行驶路线进行描述；考虑计算精度以及计算量，nurbs曲线权重因子取1，取三次b样条，采用固定节点向量的方式，求出基函数，节点向量为：

u＝{-3，-2，-1，0，1，2，3}

截取u＝[0，1]有效的计算公式可得均匀三次b样条四个基函数：

样条曲线随着参数u的变化关系式为：

其中：p1(u)表示第1段样条曲线，p1+i表示第1段曲线第i个控制点，x1(u)表示控制点横坐标组成的向量，，y1(u)表示控制点纵坐标组成的向量，xl+i表示第1段第i个控制点的横坐标，y1+i表示第1段第i个控制点的纵坐标；

由于需要行驶的轨迹是不规则的路径，无人驾驶汽车的速度以及方向变化不可能是任意的；在一个采样周期内，无人驾驶汽车的速度大小和方向一般固定，如果轨迹有突变，无人驾驶汽车的速度过大的情况下，就无法行驶出预期的路径；这就需要在一些位置，如位置拐点处，限制无人驾驶汽车的速度，保证行驶安全；根据行驶路径与无人驾驶汽车实际的运动轨迹关系，引入弓高误差的概念，进行分析二者的精度误差；在坐标拐点处对应的速度较小，如果要保证行驶安全，在这些点的进给速度一定不能大于这些给定的速度限制，但如果整个路径速度都按照这些速度极小值运行，就会影响行驶效率；因此综合考虑效率和精度问题，能够根据这些速度极小值所对应位置拐点对整个路径进行分段处理；对每段利用s型路径规划理论进行分析计算然后再运用三次样条理论进行插补规划：

弧形路线的曲率公式如下所示：

曲率半径计算公式如下所示：

已知控制点、采样周期，根据弓高误差要求，利用均匀样条理论生成控制点之间的位置坐标，求出曲率半径进而求出速度限制曲线；可得弓高误差限制下的速度计算公式：

其中：e为弓高误差，r为样条曲线半径，ts为采样周期。

由于非均匀有理b样条计算量大，计算繁琐，在一些样条插补算法中，为了减少计算，采用一种节点向量不变的均匀三次b样条算法。它能大大减少计算量，并且对样条的精度影响很小，因而广泛应用于样条插补计算中。

本发明基于非对称s型加减速控制的双nurbs曲线无人驾驶汽车位姿轨迹控制实施例：

利用matlabgui进行仿真验算本发明的可行性以及对车辆运行速度的影响。

为了进一步为验证本发明的可行性，在solidworks对车辆三维建模，进而把模型导入matlabsimulink模块进行仿真计算。给定无人驾驶汽车一组离散通过点，以及车身轴空间角度变化值，值如表1所示：

表1车身位姿变化离散数据

位移的单位为千米，角度的单位为度*100。

根据本发明控制模型在matlabsimulink模块进行编程仿真，把表1数据输入运动控制程序中，同时给定限制条件：最大限制加加速度j＝10000m/s³，最大限制加速度am＝1000m/s²，最大限制速度vm＝800m/s，采用周期ts＝1ms，加加速度变化率k＝4。经过仿真实验验证，本发明能够控制无人驾驶汽车高效、准确的运动。无人驾驶汽车运动轨迹如图1所示，部分切向量如图2所示。

本发明提出了一种基于非对称s型加减速控制的双nurbs曲线无人驾驶汽车位姿轨迹控制研究，利用双nurbs曲线描述车身轴矢量变化，并计算出无人驾驶汽车轨迹变化的切向量，实现对车身轴变化位姿的准确控制，根据偏移量计算相应的权重因子能实现对车身轴矢量的准确调整。采用非对称s型加减速控制方法能有效提高行驶速度，减少因加加速度过大导致的振动问题，经过仿真实验验证本发明切实可行。