基于Dubins曲线的车辆转向路径规划方法

基于dubins曲线的车辆转向路径规划方法
技术领域
1.本发明涉及辅助驾驶技术领域，尤其是一种基于dubins曲线的车辆转向路径规划方法。


背景技术：

2.车辆经济性驾驶技术是道路交通节能减排的重要方向。车辆经济性驾驶技术以驾驶人与车辆、道路、交通流的优化与协调为核心，通过重塑驾驶员习惯、辅助驾驶员操作以及车辆自动化控制等手段，满足出行需求的同时降低行驶过程的油耗。目前，经济性辅助驾驶仍拥有着巨大的发展空间，经济性辅助驾驶通过实时反馈、提示等手段辅助驾驶员全面掌握车辆的运行状态以及正确操作油门、制动和挡位，以降低行车过程油耗。可一定程度上削弱行车油耗与驾驶员个体习惯的关联性，克服驾驶员教育方法的缺陷。
3.车辆转弯作为一种比较复杂的驾驶操作，尤其对于新手而言，在转弯的过程中驾驶员会存在许多不当或者多余的操作从而增加了能耗，降低了能源利用率。现有技术中，车辆转向辅助性驾驶系统或策略，面对复杂工时无法做到兼顾车辆转弯的灵活性与能源利用的经济性。


技术实现要素：

4.针对现有技术的不足，本发明提供基于dubins曲线的车辆转向路径规划方法，目的是兼顾转弯过程的灵活性和经济性。
5.本发明采用的技术方案如下：
6.一种基于dubins曲线的车辆转向路径规划方法，包括：
7.s1、获取车辆的起始点坐标和终止点坐标，得出车辆的转弯半径并计算理论前轮转角和理论方向盘转角；
8.s2、基于dubins曲线规划最短转弯路径，将所述最短转弯路径显示给驾驶员；
9.所述基于dubins曲线规划最短转弯路径，包括：
10.获取起始圆和终止圆的圆心坐标，所述起始圆和终止圆各有两个，其中，两个起始圆以起始点为外切点、以起始时车头方向为切线方向，并以起始时转弯半径为半径，两个终止圆以终止点为外切点、以终止时车头方向为切线方向，并以终止时转弯半径为半径；
11.计算出各起始圆与各终止圆之间的所有切线的切点；
12.求出由起始点至终止点之间的相邻切点之间的圆弧或直线，按照圆弧-直线-圆弧的路线生成轨迹；
13.根据起始和终止时的方向，选择距离最短的轨迹即最短转弯路径；
14.s3、当驾驶员控制车辆按照最短转弯路径转向时，判断实际方向盘转角与所述理论方向盘转角是否存在偏差，如果存在偏差，发出指示提醒驾驶员调整方向，直到实际方向盘转角与理论方向盘转角保持一致。
15.进一步技术方案为：
16.所述转弯半径与理论前轮转角之间的关系通过下式计算：
[0017][0018]
式中，r是车辆转弯半径，l是车辆轴距，δ是车辆前轮转角，k1、k2分别是前后轮的侧偏刚度，a、b分别是车辆质心到前、后轮轴心的距离，m是车辆质量，v是车辆质心行驶速度。
[0019]
计算出各起始圆与各终止圆之间的所有切线的切点，包括：
[0020]
计算起始圆与终止圆的外公切线及内公切线上的切点对，所述切点对由同一条切线上的两个切点构成，切点对的求解由以下参数确定：
[0021]
起始圆与终止圆的圆心坐标、起始圆与终止圆圆心之间的距离、起始圆圆心到终止圆圆心的方位角、终止圆圆心到起始圆圆心的方位角以及两组外公切线夹角的一半或两组内公切线夹角的一半。
[0022]
所述按照圆弧-直线-圆弧的路线生成轨迹，包括：
[0023]
以起始点位置、车头方向、终点位置以及前进后退信息为输入，以轨迹长度、终点车头方向以及离散序列点为输出，利用离散序列点拟合生成轨迹。
[0024]
步骤s3，还包括：
[0025]
如果实际方向盘转角与理论方向盘转角不存在偏差，发出指示提醒驾驶员保持当前方向盘转角，并判断当前车辆前轮转角与所述理论前轮转角是否存在偏差，如果存在偏差，发出指示提醒驾驶员微调方向盘，直到当前车辆前轮转角与理论前轮转角相同。
[0026]
本发明的有益效果如下：
[0027]
本发明兼顾车辆转弯的灵活性与能源利用的经济性，辅助驾驶员控制车辆沿最优路径前进或倒车转弯，确保车辆能以平缓的速度一次性通过，避免了因为驾驶员的判断不够准确而多次制动、加速、倒车来调整车辆姿态，提高了车辆发动机的效率与转弯行驶的安全性。本发明具体具有如下优点：
[0028]
本发明利用车载传感器及gps导航系统选取汽车转向时的起始及终止位置坐标点，通过实时采集路况信息对规划路线进行调整，直接测出可以有效避障的转弯半径，并计算出需要的方向盘转角，可以同时避开静止与运动的障碍，无需评估障碍物的威胁程度，提高了安全性的同时，也提高了算法的简洁性。
[0029]
本发明可以快速地规划出复杂工况下的最短转弯路径，解决了必须提前现场采集车辆轨迹路径信息才能进行路径规划的弊端，极大地提升了驾驶的灵活性，同时加入倒车和选择不同起始转弯半径的dubins路径，使该方法在多种工况下都适用，例如道路口转向、不同角度的转弯、侧方停车及启动、倒车入库等。
[0030]
本发明将仿真路径显示在车载显示器上，驾驶员可按照规划路径控制车辆转弯，同时会有车内提示灯辅助驾驶员将方向盘打到合适的角度。本发明在确保驾驶员操作方向盘角度与理论值一致的基础上，提醒驾驶员前轮角度与理论值保持一致，以消除车辆的转向系统误差，有效减小甚至消除因转向系统误差导致的轨迹偏离，确保车辆在转弯时能一次性平缓地通过，提高了转向辅助驾驶的精度。不同于一般的路径规划辅助驾驶系统，本发明充分考虑了车辆转弯辅助驾驶的经济性，考虑了车辆在转弯行驶过程中能尽量保持最优
车速，避免驾驶员因没有把握好车距或路径而频繁的加速、制动、怠速停车造成能源消耗的增加和发动机效率的下降。
[0031]
本发明的其它特征和优点将在随后的说明书中阐述，并且，部分地从说明书中变得显而易见，或者通过实施本发明而了解。
附图说明
[0032]
图1是本发明实施例的规划方法的流程示意图。
[0033]
图2是本发明实施例的车辆二自由度模型图
[0034]
图3是本发明实施例的车辆行驶路线情况的分类图。
[0035]
图4是本发明实施例的考虑不同曲线轨迹下两圆位置情况的分类图。
[0036]
图5是本发明实施例的两圆的外切线及内切线切点计算的几何解析图。
[0037]
图6是本发明实施例的路径生成算法具体输出结果示意图。
具体实施方式
[0038]
以下结合附图说明本发明的具体实施方式。
[0039]
参见图1，本技术的一种基于dubins曲线的车辆转向路径规划方法，包括：
[0040]
s1、获取车辆即将转向时的起始点坐标以及终止点坐标，得出车辆的转弯半径并计算理论前轮转角和理论方向盘转角；
[0041]
s2、基于dubins曲线规划最短转弯路径，将所述最短转弯路径显示给驾驶员；
[0042]
所述基于dubins曲线规划最短转弯路径，包括：
[0043]
获取起始圆和终止圆的圆心坐标，所述起始圆和终止圆各有两个，其中，两个起始圆以起始点为外切点、以起始时车头方向为切线方向，并以起始时转弯半径为半径，两个终止圆以终止点为外切点、以终止时车头方向为切线方向，并以终止时转弯半径为半径；
[0044]
计算出各起始圆与各终止圆之间的所有切线的切点；
[0045]
求出由起始点至终止点之间的相邻切点之间的圆弧或直线，按照圆弧-直线-圆弧的路线生成轨迹；
[0046]
根据起始和终止时的方向，选择距离最短的轨迹即最短转弯路径；
[0047]
s3、当驾驶员控制车辆按照最短转弯路径转向时，判断实际方向盘转角与所述理论方向盘转角是否存在偏差，如果存在偏差，发出指示提醒驾驶员调整方向，直到实际方向盘转角与理论方向盘转角保持一致。
[0048]
其中，步骤s1，所述转弯半径与理论前轮转角之间的关系通过下式计算：
[0049][0050]
式中，r是车辆转弯半径，l是车辆轴距，δ是车辆前轮转角，k1、k2分别是前后轮的侧偏刚度，a、b分别是车辆质心到前、后轮轴心的距离，m是车辆质量，v是车辆质心行驶速度。
[0051]
利用车载传感器及gps导航系统选取汽车转向时的起始及终止位置坐标点，直接测出可以有效避障的转弯半径并计算出需要的方向盘转角，无需评估障碍物的威胁程度。
[0052]
其中，步骤s2中，计算出各起始圆与各终止圆之间的所有切线的切点，包括：
[0053]
计算起始圆与终止圆的外公切线及内公切线上的切点对信息，所述切点对由同一条切线上的两个切点构成，切点对的求解由以下参数确定：
[0054]
起始圆与终止圆的圆心坐标、起始圆与终止圆圆心之间的距离、起始圆圆心到终止圆圆心的方位角、终止圆圆心到起始圆圆心的方位角以及两组外公切线夹角的一半或两组内公切线夹角的一半。
[0055]
其中，所述按照圆弧-直线-圆弧的路线生成轨迹，包括：
[0056]
以起始点位置、车头方向、终点位置以及前进后退信息为输入，以轨迹长度、终点车头方向以及离散序列点为输出，利用离散序列点拟合生成轨迹。
[0057]
其中，步骤s3，还包括：
[0058]
如果实际方向盘转角与理论方向盘转角不存在偏差，发出指示提醒驾驶员保持当前方向盘转角，并判断当前车辆前轮转角与所述理论前轮转角是否存在偏差，如果存在偏差，发出指示提醒驾驶员微调方向盘，直到当前车辆前轮转角与理论前轮转角相同。
[0059]
本技术的一种基于dubins曲线的车辆转向路径规划方法，可以快速地规划出复杂工况下的最短转弯路径，解决了必须提前现场采集车辆轨迹路径信息才能进行路径规划的弊端，极大地提升了驾驶的灵活性，通过选择不同起始转弯半径的dubins路径，适用于在多种工况，例如道路口转向、不同角度的转弯、侧方停车及启动、倒车入库等。
[0060]
以下以具体实施例进一步说明本技术的技术方案。
[0061]
本实施例的一种基于dubins曲线的车辆转向路径规划方法，包括以下步骤：
[0062]
s1、使用gps导航系统确定车辆转向时的起始点及终止点坐标，利用车载传感器对车辆周围环境进行扫描探测，融合超声波传感器、轮速度传感器、视觉传感器信息来辨识车辆周围的障碍物及道路情况，测量出可行的转弯半径，根据转弯半径计算出理论前轮转角及理论方向盘转角，具体包括：
[0063]
参见图2，为车辆二自由度模型图。图2中，o是坐标原点，a、b分别是前、后轮中心点，o’是车辆转向曲率圆的圆心，u1、u2分别是前、后轮的速度。a、b分别是车辆质心到前、后轮轴心的距离，l是车辆轴距。
[0064]
由图2中的角度关系、利用三角函数的近似关系得到转弯半径表达式如下：
[0065][0066]
式(1)中，r是车辆转弯半径，δ是车辆前轮转角，α1、α2分别是前、后轮侧偏角，(α
1-α2)是向心加速度的函数，即：
[0067]
α
1-α2＝aylk
ꢀꢀꢀ
(2)
[0068]
式(2)中，ay是车辆沿y方向的加速度，l是车辆轴距，k是稳定性因数；
[0069]
在车辆稳态转向时，向心加速度的表达式可化为如下形式：
[0070]ay
＝vωrꢀꢀꢀ
(3)
[0071]
式(3)中，v是车辆质心行驶速度，ωr是车辆横摆角速度。
[0072]
由式(1)-式(3)得转弯半径为：
[0073][0074]
将ωr的表达式代入式(4)，得转弯半径的表达式：
[0075][0076]
式(5)中，其中k(δ)是“动态”稳定性因数，将k和k(δ)代入上式(5)，r可写成：
[0077][0078]
式(6)中，a、b分别是车辆质心到前、后轮轴心的距离，m是车辆质量，v是车辆质心行驶速度。k1、k2分别是前后轮的侧偏刚度。
[0079]
当δ较小时，cosδ≈1，k(δ)＝k，式(6)近似为：
[0080][0081]
进而得理论前轮转角的表达式如下：
[0082][0083]
式(8)中，r和v均可由传感器测得。
[0084]
进一步，可以得到方向盘的转角：
[0085]
φ
方向盘
≈kδ
ꢀꢀꢀ
(9)
[0086]
式(9)中，k为转向系传动比。
[0087]
s2、基于dubins曲线规划最短转弯路径，将所述最短转弯路径显示给驾驶员，具体包括：
[0088]
s21、获取起始圆和终止圆的圆心坐标，起始圆和终止圆各有两个。其中，两个起始圆以起始点a为外切点、以起始时车头方向(a点箭头所示)为切线方向，并以起始时转弯半径为半径，两个终止圆以终止点b为外切点、以终止时车头方向(b点箭头所示)为切线方向，并以终止时转弯半径为半径。
[0089]
图3给出了车辆行驶路线情况的分类图，图示为四种情况下的起始圆和终止圆的位置关系。图中，a1、a2、a3、a4、b1、b2、b3、b4分别为对应切线上的切点。“右边出发”是指从起始点a向右起出发，“左边出发”是指从起始点a向左起出发，“左边达到”是指在终止点b左侧到达，“右边达到”是指在终止点b右侧到达。左右两个起始圆及终止圆包含了所有可能的出发和到达的区域。
[0090]
根据起始点a的坐标参数(xa，ya，αa)及起始时转弯半径r，可以求得a点左右两侧的起始圆的圆心ca(xi，yi)(i＝1，2)，圆心坐标的求解公式：
[0091][0092]
式(10)中，x，y为圆心的横、纵坐标，xa，ya为起点的横纵坐标位置，αa为起始点的车
头方向；“ ”表示圆心位置在a点右侧的求法，
“‑”
表示圆心位置在a点左侧的求法。
[0093]
同样，可求得b点左右两侧的终止圆的圆心cb(xj，yj)(j＝1，2)
[0094]
s22、计算出各起始圆与各终止圆之间的所有切线的切点，具体包括：
[0095]
参见图4，当起始圆和终止圆(半径相同时)的位置存在四种情况：有四组公切线(两圆相离)，三组公切线(两圆外切)，两组公切线(两圆相交)，没有公切线(两圆重合)。图4中，a1、a2、a3、a4、b1、b2、b3、b4分别为对应切线上的切点，ca、cb为起始圆圆心和终止圆圆心。
[0096]
以起始圆和终止圆相离为例，求所有的切点对，切点对由同一切线上的两个切点组成。参见图5，为起始圆和终止圆半径不同的情况下，两圆相离的计算几何解析图。图5中各符号的含义与图4中相同，外切线上的切点对[a1，b1]、[a4，b4]通过以下模型求解：
[0097][0098]
式(11)中，ra、rb分别为起始圆半径和终止圆半径，为两个圆心间的距离，cacb为从点起始圆圆心ca到终止圆圆心cb的方位角，cbca为从cb到ca的方位角，θ为两条外切线夹角的一半，x，y为横纵坐标，下标ca、cb、a1、a2......等代表对应的圆心和切点。
[0099]
计算切点的x，y坐标时，公式中sin，cos括号内的“ ”表示顺时针旋转，
“‑”
表示逆时针旋转。
[0100]
式(11)也适用于求解内切线上的切点对，例如[a2，b2]，只是其中θ表示的是两条内切线的夹角的一半，表达式为
[0101]
式(11)也适用于如图4所示的两圆相外切、相交以及两圆同心情形下的切点对的求解。
[0102]
s23、按照上述步骤求出图3所示的四种情况下所有的路线，每种情况最多16组路径，共计最多有64种结果，将各结果下的由起始点至终止点之间的相邻切点之间的圆弧或直线，按照圆弧-直线-圆弧的路线生成轨迹，三段路径可用相同的算法进行编写。
[0103]
例如，参见图6，左侧为直线路径，右侧为圆弧路径。图6中，a、b分别为起始和终止点，左侧直行路径中，b’代表从a点倒车到该点的情形。圆弧路径的输入参数为起点位置及车头方向a(xa，ya，αa)、终点位置b(xb，yb)、向前或向后行驶，输出为终点车头朝向αb，路径长
度d
ab
，离散路径序列点直线路径类似，输入为起点位置及车头方向a(xa，ya，αa)、终点位置b(xb，yb)、向前或向后行驶，输出为终点车头朝向αb，路径长度d
ab
，离散路径序列点由于路径为离散采样，可以根据不同的需求进行设计，如等长度分割，等数量分割等。
[0104]
s24、根据起始和终止时的方向，在s23中计算出的多种情况下的多种结果中，选择距离最短的轨迹即最短转弯路径。
[0105]
选择符合要求的路径进行分析，一般情况下，更关注正向的没有倒挡情况下的路径，此外在有倒车情形下的最短路径也值得关注，综合对比可以选择出任意复杂工况下的最短转弯路径，同时也适用于倒车的情形。
[0106]
s25、将最短转弯路径参数导入车载系统中，通过仿真将路径结果显示在车载显示屏上，驾驶员可按照显示屏上的仿真路线驾驶车辆转弯。
[0107]
s3、当驾驶员控制车辆按照最短转弯路径转向时，判断实际方向盘转角与理论方向盘转角是否存在偏差，如果存在偏差，发出指示提醒驾驶员调整方向，直到实际方向盘转角与理论方向盘转角保持一致。当实际方向盘转角达到计算出的理论方向盘转角φ
方向盘
时，车内仪表盘上的提示灯开启提醒驾驶员保持当前方向盘转角及车速不变，车辆便可顺利按照算法生成的最短路径转向，避免了制动、换挡和反复踩油门等一系列增大能源消耗的操作，降低发动机因动态工况造成的不必要能源消耗，从而实现了节能的目的。
[0108]
当实际方向盘转角达到理论方向盘转角时，用车载传感器实时采集在转弯过程中车辆的前轮转角，然后与理论前轮转角δ进行比较，如果理论值与实际值不相等，则说明车辆转向系统存在误差，会影响车辆的转弯半径，此时令车内提示灯开始闪烁，提醒驾驶员对方向盘进行微调，直到前轮转角与理论前轮转角相同提示灯由闪烁变为常亮，从而消除转向系统误差，防止车辆偏离规划路径。可以有效减小甚至消除因转向系统误差导致的轨迹偏离，提高了转向辅助驾驶的精度。
[0109]
本技术考虑多种工况下车辆的转向路径，采集车辆周围道路环境测出可行的转弯半径并计算出方向盘转角，利用dubins路径生成算法规划出最短的转弯路径，面对各种复杂工况时可兼顾车辆转弯的灵活性与能源利用的经济性。同时适用于前进和倒车的情形。本技术可用于普通燃油车辆及新能源车辆转向辅助驾驶，具有较广的适用性和较好的鲁棒性。
[0110]
本领域普通技术人员可以理解：以上所述仅为本发明的优选实施例而已，并不用于限制本发明，尽管参照前述实施例对本发明进行了详细的说明，对于本领域的技术人员来说，其依然可以对前述各实施例记载的技术方案进行修改，或者对其中部分技术特征进行等同替换。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。