基于分块高斯消元的运动规划方法、系统、设备及介质与流程

本发明属于智能驾驶，涉及到轨迹规划，具体涉及到一种基于分块高斯消元的运动规划方法、系统、设备及介质。

背景技术：

1、自动驾驶技术中的运动规划是确保车辆在复杂交通环境中安全、高效移动的关键组成部分。目前，主流的自动驾驶运动规划方法主要集中在基于离散空间搜索，基于样条取消的规划和基于最优控制问题的方法上。

2、最优控制问题（ocp）是基于对车辆动力学和环境的建模，通过预测未来一系列可能的状态和轨迹，从中选择最优解来制定控制策略。此种方法的优势在于能够考虑到车辆的动力学模型以及整个连续空间的可行驶空间，使得车辆能够更加灵活地应对各种复杂交通情况。然而，现有的最优控制问题应用于量产级别的自动驾驶仍然面临挑战。其中，最主要的就是模型预测控制方法通常需要高计算复杂度，因为需要在实时性要求下进行多次迭代和优化，这可能限制了在嵌入式系统上的实际应用，因为实时性对于自动驾驶系统至关重要。

技术实现思路

1、针对上述问题，本发明的主要目的在于设计一种基于分块高斯消元的运动规划方法、系统、设备及介质，解决最优控制问题面临量产级别的自动驾驶，计算复杂度高及实时性需要多次迭代和优化的技术问题。

2、为了实现上述目的本发明采用如下技术方案：

3、一种基于分块高斯消元的最优运动规划方法，包括如下步骤：

4、步骤1：获取车辆当前时刻的状态；

5、步骤2：获取车辆行驶下的障碍物信息以及规划定位，构建当前时刻的最优控制问题；

6、步骤3：将最优控制问题输入到求解器，在求解器的每次牛顿迭代步中，根据块高斯消元算法，计算牛顿步长，并更新最优控制问题的待优化变量，直至迭代收敛；

7、步骤4：将优化得到的第一个控制点的油门以及方向盘转角输入到车辆系统中，并演进到车辆下一个时刻，持续更新和优化控制输入，实现车辆的运动规划。

8、作为本发明进一步的描述，车辆的运动规划是一条具体的行驶轨迹，则定义数组，找到最优的轨迹，需满足：

9、，

10、，

11、，

12、，

13、，

14、其中，表示车辆初始位姿，表示车辆参考轨迹，表示空间中的可通行空间，由静态和动态的障碍物以及车道信息决定，表示代价函数，表示预测时域，表示车辆动力学以及运动学方程。

15、作为本发明进一步的描述，所述步骤2中，有限时域的最优控制问题，表达式为：

16、，

17、，

18、，

19、，

20、，

21、其中，下标表示第个控制节点，表示预测步长，表示当前的系统初始状态信息，表示代价函数，表示系统状态变量，表示系统控制输入变量，表示系统动力学方程，表示不等式约束方程，表示系统终端不等式约束方程；

22、定义拉格朗日函数，将上述最优控制问题转化为无约束问题，表达式为：

23、，

24、其中，，分别为动力学系统和不等式约束的拉格朗日乘子。

25、作为本发明进一步的描述，通过拉格朗日函数对待优化变量的梯度以及不等式约束条件，推导最优控制问题每个控制点对应的最优必要性条件，表达式为：

26、，

27、，

28、，

29、，

30、，

31、其中，和分别表示代价函数对和的导数，和分别表示系统动力学方程对和的导数，和分别表示不等式约束方程对和的导数，表示不等式松弛因子，分别表示拉格朗日函数对的导数；

32、每个控制点对应的最优控制条件，从联立，得到整个最优控制问题的最优性必要条件，记为。

33、作为本发明进一步的描述，所述步骤3中，通过牛顿下降法求解时，每个控制节点的kkt矩阵，表达式为：

34、，

35、其中，分别表示代价函数对的二阶导数，和分别表示将松弛变量和拉格朗日乘子向量对角化后的矩阵，代表变量的变化量；和表示对与之间的递推关系的假设形式的系数矩阵，即。

36、作为本发明进一步的描述，所述块高斯消元算法对上述kkt矩阵进行消元变换，并通过数学归纳法原理证明得到递推关系的假设形式的正确性，包括如下步骤：

37、s31：将上述kkt矩阵的倒数第三行乘以并加到倒数第四行，消去，表达式为：

38、，

39、其中，表示将拉格朗日乘子向量对角化后的矩阵；

40、s32：将s31得到的矩阵倒数第三行乘以并加到第三行，消去，表达式为：

41、，

42、其中，定义，，；

43、s33：将s32得到的矩阵进一步消去，，得到：

44、，

45、其中：

46、，

47、，

48、，

49、，

50、；

51、s34：将s33得到的矩阵下三角化，得到：

52、；

53、令：，则得到和之间的递推关系，即：

54、；

55、根据数学归纳法，上述的假设形式成立。

56、作为本发明进一步的描述，每次牛顿迭代步对kkt方程组求解的计算分为反向递归、前向递归；

57、其中，反向递归：从最后一个控制节点开始到；

58、当时，令，；

59、当时，利用s34的公式计算矩阵，，，并储存；

60、前向递归：从开始到最后一个控制节点；

61、当时，令；

62、当时，计算优化变量的牛顿下降方向，表达式：

63、，

64、，

65、，

66、，

67、。

68、一种基于分块高斯消元的最优运动规划系统，包括最优控制模型构建模块、求解模块、系统状态更新模块和块高斯消元模块；

69、其中，通过自车传感器以及上游信息获取车辆当前时刻的状态；

70、结合车辆当前行驶下的障碍物信息以及规划定位，通过所述最优控制模型构建模块，构建当前时刻的最优控制问题；

71、将构建的最优控制问题，输入所述求解模块，配合块高斯消元模块进行更新优化变量，直至迭代收敛；

72、将优化后得到的控制输入作为所述系统状态更新模块的输入，持续更新和优化控制输入，进行车辆的运动规划。

73、一种电子设备，包括处理器、通信接口、存储器和通信总线，其中，所述处理器、所述通信接口和所述存储器通过所述通信总线完成相互间的通信，所述存储器，用于存储计算机程序；

74、所述处理器，用于通过运行所述存储器上所存储的所述计算机程序来执行上述的方法。

75、一种计算机可读的存储介质，所述存储介质中存储有计算机程序，其中，所述计算机程序被处理器执行时实现上述的方法。

76、相对于现有技术，本发明的技术效果为：

77、本发明提供了一种基于分块高斯消元的运动规划方法、系统、设备及介质，采用最优控制模型构建模块、求解模块、系统状态更新模块和块高斯消元模块相结合，通过构建最优控制问题，并根据块高斯消元算法针对最优控制问题求解，利用前向递归与反向递归两个循环计算牛顿下降方向，将大规模线性方程组的求解复杂度从关于预测步长的三次阶降低为关于预测步长的一次阶，减少了实时计算复杂度，缩短了实时计算所需要的时间，极大地优化了在线求解的性能；将大规模不等式约束问题的复杂度从关于状态变量和控制变量总个数的三次阶变为仅关于控制变量总个数的三次阶，进一步提升实时计算性能。