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

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

背景技术：

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

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

技术实现思路

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

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

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

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

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

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

7、代价函数，

8、式中，为车辆位置偏差代价函数，为车辆前轮偏转角偏差代价函数，ka为常数，m为预测路径步长，即从车辆当前位置达到实际路径的距离；(xm,ym)为实际路径上的期望到达的坐标；(xi,yi)为车辆预测路径走过的路径；i为车辆预测过程中某一时刻的前轮偏转角；为车辆预测过程中车轮偏转角的平均值。

9、进一步地，所述运动模型，δ为车辆前轮偏转角，l为车辆车身长度，dy为车辆后轮与跟随路径点的横向偏差，ld为车辆后轮与跟随的路径点的距离。

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

11、进一步地，所述速度模型，（x,y）为未来车辆未位置，为未来车辆车身与x轴夹角，为当前车辆车身与x轴夹角，（x0,y0）为当前车辆位置，v为车辆向前或向后的车辆速度，δ为车辆前轮偏转角，为当前车辆位置到未来车辆位置所需时间。

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

技术特征：

1.一种基于运动模型的预测控制算法，其特征在于，包括如下步骤：

2.根据权利要求1所述的一种基于运动模型的预测控制算法，其特征在于，所述运动模型，δ为车辆前轮偏转角，l为车辆车身长度，dy为车辆后轮与跟随路径点的横向偏差，ld为车辆后轮与跟随的路径点的距离。

3.根据权利要求2所述的一种基于运动模型的预测控制算法，其特征在于，给定的任意向前或向后的车辆速度，根据所述运动模型能计算出跟随路径所需的前轮转角。

4.根据权利要求1所述的一种基于运动模型的预测控制算法，其特征在于，所述速度模型，（x,y）为未来车辆未位置，为未来车辆车身与x轴夹角，为当前车辆车身与x轴夹角，（x0,y0）为当前车辆位置，v为车辆向前或向后的车辆速度，δ为车辆前轮偏转角，为当前车辆位置到未来车辆位置所需时间。

技术总结
本发明公开了一种基于运动模型的预测控制算法，包括如下步骤：（1）根据建立的运动模型计算当前位姿下跟随路径点所有控制指令；（2）根据建立的速度模型模拟出所有控制指令的未来车辆位置；（3）通过代价函数计算每个控制指令的代价值，并选取最小代价值的控制指令下发到车辆执行机构；本发明将车辆抽象为数字化的运动模型，使用车辆运动模型进行对路径点的轨迹跟踪计算；同时根据计算出来的控制指令通过建立的车辆速度模型，进行对车辆未来时刻位姿的提前预测；在得到一定时间刻度内所有预测的车辆位姿后，通过建立的代价函数进行判定，最终选取最优的结果；对于不同情况下应输出的控制值皆为一定时间刻度内的最优结果。

技术研发人员：周军,夏礼帅,龙羽,徐菱
受保护的技术使用者：成都睿芯行科技有限公司
技术研发日：
技术公布日：2024/1/15