一种基于优化数据集驱动的避障轨迹模糊规划系统及方法

本发明属于汽车智能驾驶领域，涉及一种基于优化数据集驱动的避障轨迹模糊规划系统及方法。

背景技术：

1、随着智慧城市与智慧交通的建设，城市道路交通安全与快捷出行成为人们越来越重视的话题之一。针对汽车路径规划及避障的各个方面的研究正在如火如荼的进行。一种能够完成在线计算避障轨迹的智能规划系统及方法对于智慧交通的建设以及无人驾驶车辆的安全性和舒适性具有重要意义。

2、因此，如何正确计算无人驾驶车辆的最优行驶轨迹来完成驾驶任务是当务之急，研究高效的规划算法来尽早规划汽车最优避障轨迹的运行方案，对于智慧交通的建设和无人驾驶技术的发展具有重大的理论价值和有效的应用价值。

3、cn114194215a，一种智能车辆避障换道轨迹规划方法及系统，根据道路环境、障碍物信息以及当前车速，基于操稳性极限起始回正点以及多项式曲线生成可行轨迹簇，对可行轨迹簇中的换道轨迹进行预碰撞检测，筛选并剔除出不满足预碰撞检测条件的轨迹得到无碰撞轨迹簇；在无碰撞轨迹簇的基础上建立关于舒适性、避障效率、侧滑这三个评价指标的代价函数；并采用模糊推理来确定三个评价指标的权重，通过求解多目标优化问题，搜索出最优的避障换道回正点，即规划出最终的避障换道轨迹。同时本技术还提出了一种智能车辆避障换道轨迹规划系统，可以在兼顾安全性、实时性以及曲率连续的同时，使得轨迹的性能指标如舒适性、避障效率等最大化，有效改善乘员的乘坐体验。

4、该专利是基于操稳性极限起始回正点以及多项式曲线生成可行轨迹簇，再进行后验，剔除不满足条件的轨迹，再结合舒适性、避障效率、侧滑求解多目标优化问题，最终规划出避障换道轨迹。但是该专利未考虑障碍车辆是否距离本车有横向位移，当出现横向位移时，是否可行且并未交代该方案的实时性。而本发明在利用gauss伪谱法在多种约束条件下求解得到的最优的无碰撞轨迹，在障碍物横向位移时进行数据集构建，再利用模糊理论完成了在线避障轨迹生成，并大大提升了求解时间，本发明解决了避障轨迹实时性的需求。

技术实现思路

1、本发明旨在解决以上现有技术的问题。提出了一种基于优化数据集驱动的避障轨迹模糊规划系统及方法。本发明的技术方案如下：

2、一种基于优化数据集驱动的避障轨迹模糊规划系统，其包括：汽车运动学模块与障碍物模型模块、实时环境测量模块、优化数据驱动模块、模糊轨迹规划模块、实时车辆参数检测模块、汽车防撞验证模块、汽车参数设定模块、mcu模块、档位切换模块、牵引制动模块和方向盘控制模块；其中，

3、模糊轨迹规划模块分别与实时环境测量模块、实时车辆参数检测模块、优化数据驱动模块、汽车防撞验证模块相连接，汽车防撞验证模块分别与汽车运动学模块与障碍物模型模块、汽车参数设定模块、mcu模块相连接，实时车辆参数检测模块还分别与汽车参数设定模块、汽车防撞验证模块相连接，所述mcu模块还分别与档位切换模块、牵引制动模块和方向盘控制模块相连接；其中

4、实时环境测量模块，用于获取汽车驾驶过程中实时环境情况；

5、实时车辆参数检测模块，用于获取驾驶过程中的车辆参数变化；

6、优化数据驱动模块，用于采用gauss伪谱法规划模块离线获取数据再进行优化数据处理为模糊规划模块构建局部避障轨迹库；

7、汽车运动学模块与障碍物模块，用于设定汽车的初始参数与障碍物初始参数为防撞验证提供模型支持同时完成碰撞公式验证；

8、模糊轨迹规划模块，用于采用模糊理论方法求解模糊理论轨迹，再输出模糊轨迹进行汽车防撞验证，在确保无碰撞情况下再进行下一步输出；

9、实时车辆参数检测模块和汽车参数设定模块用于对汽车行驶过程的防护和监督；

10、所述汽车防撞验证模块用于将通过汽车防撞验证模块的模糊轨迹输出给mcu模块，并根据获得的数据分配给档位切换模块、牵引制动控制模块和方向盘控制模块。

11、进一步的，所述优化数据驱动模块包括信息收集模块、初始化模块、gauss伪谱法规划模块、优化数据处理模块和构建局部避障轨迹库模块；

12、所述信号采集模块用于采集汽车行驶过程中的实时环境情况、汽车实时速度及速度变化、汽车避障参考位置起点和终点信息，并将采集到的信息输入到初始化模块中；

13、所述初始化模块用于设置汽车不同行驶路况下的高斯配点数n并设定优化精度tol，将迭代次数d置零；

14、所述gauss伪谱法规划模块包括时间变换、gauss配点和变量离散化三部分，用于初步完成对局部避障轨迹的规划；时间变换是指引入新的时间变量进行时间尺度变换，将原问题转化为适合于伪谱法的标准bolza问题；gauss配点是指gauss伪谱法通过求取legendre多项式的零点来得到[-1,1]区间的时间配点；变量离散化gauss伪谱法中，未知的变量包括状态变量、控制变量和未知参数，其中未知参数属于静态变量，对于状态变量和控制变量需要进行离散化处理；

15、所述优化数据处理模块用于对伪谱法计算的避障轨迹进行处理，选取合理且最优的轨迹；

16、所述构建局部避障轨迹库模块用于完成对伪谱法的避障轨迹的汇总整理并生成轨迹库。

17、进一步的，所述模糊轨迹规划模块包括模糊化模块、模糊推理模块和解模糊模块三部分，其中，

18、所述模糊化模块，基于高斯伪谱法的避障轨迹集构建隶属度函数，采用常用的三角隶属函数来描述模糊子集的隶属度；

19、所述模糊推理模块是基于避障轨迹集设计出相应的模糊规则并保证模糊规则符合该避障情况的所有要求；

20、所述解模糊模块采用加权重心法作为去模糊化，以获得模糊规划避障轨迹。

21、进一步的，所述汽车防撞验证模块包括碰撞公式验证模块和性能数据匹配验证模块和模块；其中，碰撞公式验证模块，用于输入模糊规划轨迹，采用p-norm(p准则)来验证模糊规划轨迹是否会与避障路线中是否会与障碍物发生碰撞；p-norm很容易地构造具有矩形和圆形障碍物的几何模型，它可以使障碍物避障约束表达简洁统一，增强算法的通用性，本工作采用了p-norm来刻画障碍物、道路模型。

22、性能数据匹配验证模块，基于实时车辆参数检测模块和汽车参数设定模块对于不同避障场景中规划的避障轨迹是否符合实际车辆的规范；输出合理验证轨迹。

23、进一步的，所述gauss伪谱法规划模块包括以下步骤：

24、步骤b1：通过时间变换引入新的时间变量τ进行时间尺度变换，将时间区间从[t0,tf]转化为[-1,1]，时间变换公式为

25、

26、其中，t表示时间，tf表示汽车的终端避障时刻，t0表示汽车的初始避障时刻；

27、步骤b2：通过运用legendre多项式的零点来得到[-1,1]区间的时间配点，其中，n次legendre多项式的表达式为

28、

29、p0(x)＝1,p-1(x)＝0

30、其中，pn+1(z)表示n次legendre多项式，pn(z)表示n-1次legendre多项式，pn-1(z)表示n-2次legendre多项式，p0(z)表示-1次legendre多项式，p-1(z)表示-2次legendre多项式，z表示多项式参数变量，αn表示n-1次legendre多项式pn(z)的特征参数，βn表示n-2次legendre多项式pn-1(z)的特征参数，n表示配点个数参数；n为时间段[-1,1]的离散时间子区间数；

31、步骤b3：提供设置参数αn和βn的取值，分别为αn＝0，

32、步骤b4：提供运用高斯时间配点将时间区间划分为n段，其中，第m段子区间为[tm-1,tm],m＝0,…,n；tm-1表示第m段子区间的初始时间点；tm表示第m段子区间的终值时间点；

33、步骤b5：将状态变量在前n+1个lg节点上进行lagrange插值近似，得到如下表达式：

34、

35、

36、其中x(τ)为状态向量，x(τ)为lagrange插值多项式，x(τi)＝xi为状态向量在lg配置点τi处的参数，是待优化的参数，li(τ)为lagrange插值多项式的基函数，i和j表示第j段的第i个lg配置点，b(τ)是关于τ的k+1阶多项式。

37、步骤b6：将控制变量在区间(-1,1)中的lg配点上进行离散化，得到如下表达式：

38、

39、其中，

40、

41、其中μ(τ)为控制变量，u(τ)为lagrange插值多项式，ui＝u(τi)为控制向量在lg配置点τi处的参数，也是待优化的参数，与li(τ)类似。

42、步骤b7：把状态方程中的状态变量的导数进行离散化，把原问题转化为非线性规划问题。

43、一种基于任一项所述系统的规划方法，其包括以下步骤：

44、步骤s1：通过信号采集模块采集汽车行驶过程中的实时线路情况、汽车实时速度及速度变化、汽车自身运动学模型与行驶过程中的障碍物模型的信息，并将采集到的信息输入到初始化模块中；

45、步骤s2：gauss伪谱法规划模块根据不同的避障场景设置汽车行驶过程中的伪谱法配点数n，进行时间变换和变量离散化并设定优化精度tol，将迭代次数d置零；

46、步骤s3：通过对伪谱法计算的避障轨迹进行优化数据处理并构建局部避障轨迹库；

47、步骤s4：通过汽车防撞验证模块验证完成碰撞公式验证和性能数据匹配验证，验证得到满足要求的验证轨迹输出到mcu模块；

48、步骤s5：将档位切换、油门、刹车和方向盘控制量利用mcu模块传输到档位切换模块、牵引制动控制模块和方向盘控制模块中。

49、进一步的，所述步骤s3：通过对伪谱法计算的避障轨迹进行优化数据处理并构建局部避障轨迹库，包括以下步骤：

50、步骤a1：通过信息采集模块将实时环境、实时速度进行测量与采集，并设定好局部避障的参考起点与终点；

51、步骤a2：通过初始化模块定义完成gauss配点数n并设定优化精度tol，将迭代次数d置零；

52、步骤a3：通过gauss伪谱法规划模块完成时间尺度变换、gauss配点和控制变量和状态变量离散化，初步完成对局部避障轨迹的规划；

53、步骤a4：通过优化数据处理模块对伪谱法计算的避障轨迹进行处理，来选取采用合理且最优的轨迹；

54、步骤a5：通过构建局部避障轨迹库模块完成对伪谱法的避障轨迹的汇总整理并生成局部避障轨迹集。

55、进一步的，所述模糊轨迹规划模块包括以下步骤：

56、步骤c1：模糊化是基于高斯伪谱法的避障轨迹集构建隶属度函数，采用常用的三角隶属函数来描述模糊子集的隶属度；

57、为了模糊化，定义权重参数为ki,(i＝1,2,...,z)，z表示隶属函数的数量，使用以下公式计算：

58、ki＝trimf(e,[q1 q2 q3...qi]),i＝1,2,...,z

59、其中e是输入值，障碍物中心与水平线之间的垂直距离；trimf(·)是三角形隶属函数；q是trimf(·)的系数；

60、步骤c2：模糊推理是基于避障轨迹集设计出相应的模糊规则并保证模糊规则符合该避障情况的所有要求，并构建模糊规则表：

61、步骤c3：解模糊是采用加权重心法即普通加权平均法作为去模糊化方法完成解模糊，以获得模糊规划避障轨迹；解模糊方程如下：

62、

63、其中μ(ki)是避障优化数据集的拟合轨迹点；u0是获得的在线避障轨迹向量，其中z表示隶属函数的数量。

64、进一步的，所述汽车防撞验证模块包括以下步骤：

65、步骤d1：通过使用p-norm准则进行碰撞公式验证，来验证模糊规划轨迹是否会与避障路线中障碍物发生碰撞，采用p-norm来刻画障碍物模型，其表达式如下：

66、

67、式中，(x,y)为车辆的边界点；rx和ry表示车辆几何结构长度和宽度的一半；x0和y0表示后轴轴心的坐标；不同的p值表示图形的不同形状，从而设置出不同的障碍物模型，使o(x,y)＞0的点，都在图形边界的外面，表示避障路径未与障碍物发生碰撞；相应的，使o(x,y)＜0都表示在图形的里面，表示避障路径会与障碍物发生碰撞；

68、步骤d2：通过性能数据匹配验证是基于实时车辆参数检测模块和汽车参数设定模块对于不同避障场景中规划的避障轨迹是否符合实际车辆的规范。

69、本发明的优点及有益效果如下：

70、本发明提供的基于优化数据集驱动的避障轨迹模糊规划系统及方法，包括汽车运动学模块与障碍物模型模块、实时环境测量模块、优化数据驱动模块、模糊轨迹规划模块、实时车辆参数检测模块、汽车防撞验证模块、汽车参数设定模块、mcu模块、档位切换模块、牵引制动模块和方向盘控制模块。其中实时环境测量模块与实时车辆参数检测模块两者均与模糊轨迹规划模块连接，用于获取汽车驾驶过程中实时环境情况及驾驶过程中的车辆参数变化；其中优化数据驱动模块与模糊轨迹规划模块连接，由于为模糊规划规划模块构建局部避障轨迹库；其中汽车运动学模块与障碍物模块与汽车防撞验证模块连接，用于设定汽车的初始参数与障碍物初始参数为防撞验证提供模型支持同时完成碰撞公式验证；其中模糊轨迹规划模块与汽车防撞验证模块连接，用于对输出模糊轨迹进行汽车防撞验证在确保无碰撞情况下再进行下一步输出；其中实时车辆参数检测模块和汽车参数设定模块均与汽车防撞验证模块连接，用于对汽车行驶过程的防护和监督；其中汽车防撞验证模块与mcu模块连接，用于将通过汽车防撞验证模块的模糊轨迹输出给mcu模块，并根据获得的数据分配给档位切换模块、牵引制动控制模块和方向盘控制模块

71、通过采集汽车行驶过程中的实时线路环境情况、汽车实时速度及速度变化、汽车自身运动学模型与行驶过程中的障碍物模型的信息，在汽车防撞验证模块保护下，使得汽车在行驶中更加安全可靠。

72、本发明的创新步骤主要是：

73、模糊轨迹规划模块步骤：

74、步骤1：使用v2x、毫米波雷达、gps等在实时环境测量模块检测车辆和环境信息，并将信息发送到步骤2。

75、步骤2：使用步骤1中检测到的信息计算距离值e。

76、步骤3：使用模糊策略计算权重参数ki,(i＝1,2,...,z)，然后转到步骤4。

77、步骤4：检查避障条件。如果不需要改变轨迹以避开障碍物，则保持原始轨迹并进入步骤8；否则，应避开障碍物并进入步骤5以规划在线避障轨迹。

78、步骤5：采用模糊推理策略处理距离值e和权重参数。

79、步骤6：通过应用gauss伪谱方法离线求解典型情况tops建立数据库，使用避障优化数据库计算拟合轨迹点μ(ki)。

80、步骤7：使用去模糊化方程计算避障规划轨迹。

81、步骤8：将确定的避障轨迹发送给跟踪控制器，并使用纯跟踪算法来跟踪和控制决策轨迹。

82、模糊轨迹规划模块步骤优点：基于数据驱动的方法可以在原有的最优轨迹集上快速准确的获得一条满足随机设置的横向距离的避障轨迹，速度快，避障轨迹满足条件。