一种基于道路类型和工况识别的快速MPC自动驾驶控制方法与流程

本发明属于自动驾驶，更具体地，涉及一种基于自适应道路类型和工况识别的快速mpc自动驾驶控制方法和系统。

背景技术：

1、mpc（model predictive control，多变量控制策略）由于其对多约束优化问题具有较好的求解能力，目前在车辆自动驾驶技术上的应用受到广泛关注。mpc框架包含模型预测模块，滚动求解最优控制序列的优化模块以及校正反馈模块。其控制原理为基于车辆动力学模型，建立模型预测矩阵，在系统状态量和控制量的约束条件下滚动求解每步预测矩阵的最优控制量，得到最优控制序列，并将第一步求解的最优控制量的值输出给执行器，同时根据车辆当前状态更新未来状态量约束。mpc算法具有优秀的处理多状态量多控制量和多约束的复杂系统下的优化问题的能力，具有较强的鲁棒性。尽管其控制结果并非全局最优解，但能把环境干扰产生的误差实时修正，实现在预测域内的控制效果最优。

2、mpc求解器的控制效果与预测步长和控制步长成正比，控制步长和预测步长越长控制效果越好，但是计算耗时也显著增加，要获取更好的控制效果，对执行器的硬件要求更高。传统mpc在自动驾驶控制中应用时其预测步长和控制步长无法更改，因此为了满足各约束条件且具备较好的优化效果，其预测步长和控制步长设置较大，导致在车辆即使在一些简单的道路工况下行驶时仍需要做大量的矩阵运算，大大增加了mpc计算耗时，增加了其在线应用的难度。

技术实现思路

1、针对现有技术的缺陷，本发明的目的在于提供一种基于自适应道路类型和工况识别的快速mpc自动驾驶控制方法和系统，旨在解决传统mpc求解器无法动态改变预测步长，也不能动态控制步长导致计算效率低且耗时的问题。

2、本发明提供了一种基于自适应道路类型和工况识别的快速mpc自动驾驶控制方法，包括下述步骤：

3、（1）根据当前时刻参考轨迹给出的未来道路曲率识别道路类型；

4、（2）根据当前时刻参考轨迹给出的未来速度和加速度信息识别未来工况类型；

5、（3）根据上述识别结果自适应选择实际的预测步长和控制步长；

6、（4）通过车辆的横纵向误差动力学模型，建立预测和控制模型并获得mpc求解器框架；

7、（5）利用搭建的mpc框架通过设置不同的预测步长和控制步长，根据仿真测试结果选择满足约束条件的预测步长和控制步长作为基准；

8、（6）根据道路识别结果、工况识别结果、基准预测步长和基准控制步长生成实际应用矩阵；

9、（7）根据输入的实际预测和控制矩阵规模更新求解器矩阵计算规模，并输出控制序列。

10、更进一步地，步骤（1）具体为：

11、步骤1.1：获取参考轨迹上的坐标序列，，利用数值求解出坐标的差分序列，；

12、步骤1.2：根据横、纵坐标的差分序列计算参考轨迹点上的航向序列，并对航向序列进行差分解出航向增量序列和轨迹长度增量序列；

13、步骤1.3：根据航向增量序列和轨迹长度增量序列计算参考轨迹的道路曲率序列；

14、步骤1.4：计算道路曲率序列绝对值的平均值，并根据平均值来识别未来参考道路类型。

15、更进一步地，步骤（2）具体为：提取参考轨迹中的速度序列和加速度序列，并计算速度序列与当前参考速度值的差值或判断加速度序列与0的差值；

16、当参考速度序列与当前参考速度的差值始终保持在0附近时，未来参考行驶工况为匀速，否则为变速；

17、当参考加速度序列中的值始终保持在0附近时，未来参考行驶工况为匀速，否则为变速行驶。

18、更进一步地，步骤（3）中选择实际的预测步长和控制步长具体为：

19、设置选取预测步长和控制步长的评价标准；

20、搭建仿真测试场景；所述仿真测试场景包括不同的道路类型和工况类型，且在仿真测试场景中直道匀速、直道变加速、大曲率弯道匀速、大曲率弯道变加速、小曲率弯道匀速和小曲率弯道变加速均有设置；

21、基于仿真测试场景，设置不同的预测步长和控制步长，并根据选取最合适的预测步长和控制步长作为mpc求解基准步长；

22、其中，n表示仿真步长，，，分别表示单步耗时，单步横向误差以及单步纵向误差的权重系数。

23、更进一步地，步骤（4）具体为：

24、基于车辆二自由度模型并考虑轮胎侧偏性能建立横纵向误差动力学模型；

25、所述横纵向误差动力学模型包括6个状态变量和2个控制变量，状态变量包括横向状态变量和纵向状态变量，横向状态变量包括横向误差，横向速度，航向误差和航向误差角速度；纵向状态变量包括位置误差和速度误差；控制变量包括横向控制变量和纵向控制变量，横向控制变量为前轮转角，纵向控制变量为纵向误差加速度补偿，其中

26、所述横纵向误差动力学模型中误差动力学的状态方程为；其中，，，，，，，，，，分别表示整车质量，车辆纵向速度，前轮轮胎刚度，后轮轮胎刚度，绕z轴的转动惯量，道路固有性质；

27、对所述误差动力学的状态方程进行离散化处理，获得

28、；

29、其中，，，表示单步采样时间；

30、通过合并状态量和控制量获得离散增量方程

31、；

32、通过离散增量方程创建预测步长为且控制步长为的预测方程：

33、；

34、设置控制模型代价函数并获得二次规划函数

35、；

36、通过从实际控制量增量的上、下限约束，实际控制量的上、下限约束以及输出状态量的上、下限约束对所述二次规划的控制量进行约束；；根据实际的预测步长和控制步长滚动求解出控制序列。

37、更进一步地，步骤（5）具体为：

38、步骤5.1：获得实际行车道路状况，并当车辆在直道上行驶且当前航向与参考误差在容许误差内时车辆路径跟踪不涉及横向控制，只需考虑纵向mpc；当在小曲率弯道行驶时，曲率越小越接近直道，横向的控制输出变化相对较小，可设置较小的预测步长和控制步长；在大曲率弯道上行驶时，为跟上参考轨迹，横向的控制输出变化会相对较大，需要设置较大的预测步长和控制步长；

39、步骤5.2：获得参考工况，并在匀速行驶工况下保持纵向加速踏板和制动压力，只需要控制横向控制量，并采用较小的控制和预测步长；在变速行驶工况下需要结合实际道路类型来设置合适的预测步长和控制步长；

40、步骤5.3：根据实际行车道路状况、参考工况和仿真实验结果选择在当前时刻下参考轨迹输出的信息选择mpc实际的预测步长和控制步长。

41、更进一步地，步骤（6）具体为：

42、步骤6.1：根据预测步长和控制步长基准和当前选择的实际预测步长和控制步长，设置控制转化矩阵；

43、步骤6.2：将预测转化矩阵和控制转化矩阵带入mpc求解器框架中获得约束表达形式为：；

44、其中，为控制转化矩阵，为预测转化矩阵。

45、进一步优选地，当前选择的实际预测步长小于等于预测步长，当前选择的实际控制步长小于等于控制步长。

46、其中，步骤（7）具体为：根据引入的控制转化矩阵和预测转化矩阵求解实际控制序列，将控制序列第一项结果输出给执行机构。

47、本发明还提供了一种基于自适应道路类型和工况识别的快速mpc自动驾驶控制系统，包括：道路类型识别模块、工况识别模块、步长控制模块、步长基准选择模块、转化矩阵生成模块、mpc模块和mpc更新模块；道路类型识别模块根据当前时刻参考轨迹给出的未来道路曲率识别道路类型；工况识别模块根据当前时刻参考轨迹给出的未来速度和加速度信息识别未来工况类型；步长控制模块根据上述识别结果自适应选择实际的预测步长和控制步长；mpc模块通过车辆的横纵向误差动力学模型，建立预测和控制模型，设计mpc求解器框架；步长基准选择模块是利用搭建的mpc框架通过设置不同的预测步长和控制步长，根据仿真测试结果选择合适的满足约束条件的预测步长和控制步长作为基准；转化矩阵生成模块根据道路识别结果、工况识别结果、基准预测步长和基准控制步长生成实际应用矩阵；mpc更新模块根据输入的实际预测和控制矩阵规模更新求解器矩阵计算规模并输出控制序列。

48、通过本发明所构思的以上技术方案，与现有技术相比，由于在传统mpc求解器框架下引入动态输入变预测步长和控制步长接口，采用本发明所提出的算法能快速根据实时的参考轨迹信息选择适合当前行驶状态的预测步长和控制步长，解决了传统mpc求解器无法动态改变预测步长和控制步长的问题，显著提升计算效率，为mpc算法在线应用提供思路。