一种基于运动模型的预测控制算法

技术领域

本发明涉及智能汽车控制技术领域，具体涉及一种运动模型的预测控制算法。

背景技术

在AGV/AMR和自动驾驶领域，运动控制算法及轨迹跟踪算法都为系统中的核心基础模块。车辆该如何行驶，车体底层执行机构应当如何输出对应的控制指令，都为运动控制模块需要处理的功能。当全局定位模块及路径规划模块分别为提供车辆全局位姿和全局路径后，路径跟踪模块需要根据当前位姿和路径计算合理的运动控制速度命令。当机器人收到运动控制指令后，同时还需要将控制指令根据车辆运动模型分解输出到各个执行模块，实现对规划路径的实时跟踪。

目前市面上大部分控制算法仅针对车辆当前时刻的角度进行闭环比例控制，对于选取的路径上控制点的相对位置固定不变，并且无法考虑车辆自身状态，仅根据偏差值大小输出固定的比例放大后的控制指令，无法做到对所有情况如偏差过大、车辆直行、后退、转向的自适应性，鲁棒性较差。

发明内容

本发明的目的在于提供一种运动模型的预测控制算法，该算法解决了车辆路径跟随问题。

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

一种基于运动模型的预测控制算法，包括如下步骤：

（1）根据建立的运动模型计算当前位姿下跟随路径点所有控制指令；

（2）根据建立的速度模型模拟出所有控制指令的未来车辆位置；

（3）通过代价函数计算每个控制指令的代价值，并选取最小代价值的控制指令下发到车辆执行机构；

代价函数

式中，

进一步地，所述运动模型

进一步地，给定的任意向前或向后的车辆速度，根据所述运动模型能计算出跟随路径所需的前轮转角。

进一步地，所述速度模型

本发明根据车体结构建立车辆运动学模型与速度模型，通过运动学模型计算当前位姿下跟随路径点所需的控制指令；通过速度模型将计算出来的控制指令进行模拟，推算出使用当前控制指令后未来车辆的位置；再根据车辆控制指令的调整幅度及未来车辆位置与路径的偏差建立代价函数，通过代价函数选定一条控制幅度与路径偏差都尽可能小的控制指令，下发给执行模块实际执行，最终实现对车辆基于运动模型的预测优化控制。

附图说明

图1为本发明的流程示意图。

图2为本发明的运动模型建立过程简图。

图3为本发明的速度模型建立简图。

图4为本发明的代价函数示意图。

图5为本发明应用于环形路径的效果图。

图6为本发明应用于环形路径的偏差计算图。

图7为本发明应用于S形路径的效果图。

图8为本发明应用于S形路径的偏差计算图。

具体实施方式

如图1所示，本实施例提供的一种基于运动模型的预测控制算法包括如下步骤：

（1）根据建立的运动模型计算当前位姿下跟随路径点所有控制指令，

如图2所示，所述运动模型

给定的任意向前或向后的车辆速度，根据车辆模型能计算出跟随路径所需的前轮转角。

具体地，首先，获取车辆的全局位姿和任务路径，然后，计算车辆在任务路径上距离最近的投影点，根据当前车辆速度从投影点开始截取前进一段路径作为根据路径，速度越快截取的路径越长；根据运动模型依次计算达到跟随路径点到跟随路径上每个路径点的控制指令。

（2）根据建立的速度模型模拟出所有所述控制指令的未来车辆位置，

如图3所示，所述速度模型

可通过上述运动模型能预测出车辆在跟踪当前路径点的过程中未来行驶过的轨迹所有位姿情况。

（3）通过代价函数计算每个控制指令的代价值，并选取最小代价值的控制指令下发到车辆执行机构；

如图4所示，代价函数

代价函数原理为预测路径所有偏差的均值乘路径偏差影响系数，加上前轮偏转角的方差乘偏转角影响系数，体现车辆方向调整幅度大小和车辆与路径的距离偏差。当所有路径模拟完成后，计算代价值最小控制指令的用以控制车辆跟随路径。

本实施例依据车辆实际的物理特性，将车辆抽象为数字化的运动模型，使用车辆运动模型进行对路径点的轨迹跟踪计算。同时根据计算出来的控制指令通过建立的车辆速度模型，进行对车辆未来时刻位姿的提前预测。在得到一定时间刻度内所有预测的车辆位姿后，通过建立的代价函数进行判定，最终选取最优的结果；对于不同情况下应输出的控制值皆为一定时间刻度内的最优结果。

本实施例以环形路径为例，采用电脑软件模拟出设定路径与本实施例跟随路径效果图，如图5所示，实线为设定路径，长虚线为本实施例算法控制车辆走过路径；点虚线表示现有算法控制车辆走过的路径。从图中可知，本实施例算法优化明显优于现有算法。

对本实施例环形路径跟随偏差计算，如图6所示，该曲线表示运行中与车辆与路径偏差，在弯道时偏差最大，最大在0.03米左右，标准差在0.02米左右。

本实施例以S形路径为例，采用电脑软件模拟路径跟随效果，如图7所示，实线为设定路径，长虚线为本实施例算法控制车辆走过路径；点虚线表示现有算法控制车辆走过的路径。从图中可知，本实施例算法优化明显优于现有算法。

对本实施例S形路径跟随偏差计算，如图8所示，该曲线表示运行中与车辆与路径偏差，在起步时偏差最大，最大在0.04米左右，标准差在0.01米左右

以上所述仅是本发明优选的实施方式，但本发明的保护范围并不局限于此，任何基于本发明所提供的技术方案和发明构思进行的改造和替换都应涵盖在本发明的保护范围内。