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

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

【技术实现步骤摘要】
一种基于运动模型的预测控制算法


[0001]本专利技术涉及智能汽车控制
，具体涉及一种运动模型的预测控制算法
。

技术介绍

[0002]在
AGV/AMR
和自动驾驶领域，运动控制算法及轨迹跟踪算法都为系统中的核心基础模块
。
车辆该如何行驶，车体底层执行机构应当如何输出对应的控制指令，都为运动控制模块需要处理的功能
。
当全局定位模块及路径规划模块分别为提供车辆全局位姿和全局路径后，路径跟踪模块需要根据当前位姿和路径计算合理的运动控制速度命令
。
当机器人收到运动控制指令后，同时还需要将控制指令根据车辆运动模型分解输出到各个执行模块，实现对规划路径的实时跟踪
。
[0003]目前市面上大部分控制算法仅针对车辆当前时刻的角度进行闭环比例控制，对于选取的路径上控制点的相对位置固定不变，并且无法考虑车辆自身状态，仅根据偏差值大小输出固定的比例放大后的控制指令，无法做到对所有情况如偏差过大
、
车辆直行
、
后退
、
转向的自适应性，鲁棒性较差
。

技术实现思路

[0004]本专利技术的目的在于提供一种运动模型的预测控制算法，该算法解决了车辆路径跟随问题
。
[0005]为实现上述目的，本专利技术采用如下技术方案：
[0006]一种基于运动模型的预测控制算法，包括如下步骤：
[0007]（1）根据建立的运动模型计算当前位姿下跟随路径点所有控制指令；
[0008]（2）根据建立的速度模型模拟出所有控制指令的未来车辆位置；
[0009]（3）通过代价函数计算每个控制指令的代价值，并选取最小代价值的控制指令下发到车辆执行机构；代价函数，式中，为车辆位置偏差代价函数，为车辆前轮偏转角偏差代价函数，
ka
为常数，
m
为预测路径步长，即从车辆当前位置达到实际路径的距离；
(x
m
,y
m
)
为实际路径上的期望到达的坐标；
(x
i
,y
i
)
为车辆预测路径走过的路径；
i
为车辆预测过程中某一时刻的前轮偏转角；为车辆预测过程中车轮偏转角的平均值
。
[0010]进一步地，所述运动模型，
δ
为车辆前轮偏转角，
L
为车辆车身长度，
dy
为车辆后轮与跟随路径点的横向偏差，
ld
为车辆后轮与跟随的路径点的距离
。
[0011]进一步地，给定的任意向前或向后的车辆速度，根据所述运动模型能计算出跟随路径所需的前轮转角
。
[0012]进一步地，所述速度模型，（
X,Y
）为未来车辆未位置，为未来车辆车身与
X
轴夹角，为当前车辆车身与
X
轴夹角，（
X0,Y0）为当前车辆位置，
v
为车辆向前或向后的车辆速度，
δ
为车辆前轮偏转角，为当前车辆位置到未来车辆位置所需时间
。
[0013]本专利技术根据车体结构建立车辆运动学模型与速度模型，通过运动学模型计算当前位姿下跟随路径点所需的控制指令；通过速度模型将计算出来的控制指令进行模拟，推算出使用当前控制指令后未来车辆的位置；再根据车辆控制指令的调整幅度及未来车辆位置与路径的偏差建立代价函数，通过代价函数选定一条控制幅度与路径偏差都尽可能小的控制指令，下发给执行模块实际执行，最终实现对车辆基于运动模型的预测优化控制
。
附图说明
[0014]图1为本专利技术的流程示意图
。
[0015]图2为本专利技术的运动模型建立过程简图
。
[0016]图3为本专利技术的速度模型建立简图
。
[0017]图4为本专利技术的代价函数示意图
。
[0018]图5为本专利技术应用于环形路径的效果图
。
[0019]图6为本专利技术应用于环形路径的偏差计算图
。
[0020]图7为本专利技术应用于
S
形路径的效果图
。
[0021]图8为本专利技术应用于
S
形路径的偏差计算图
。
具体实施方式
[0022]如图1所示，本实施例提供的一种基于运动模型的预测控制算法包括如下步骤：
[0023]（1）根据建立的运动模型计算当前位姿下跟随路径点所有控制指令，
[0024]如图2所示，所述运动模型，
δ
为车辆前轮偏转角，
L
为车辆车身长度，
dy
为车辆后轮与跟随路径点的横向偏差，
ld
为车辆后轮与跟随的路径点的距离；
[0025]给定的任意向前或向后的车辆速度，根据车辆模型能计算出跟随路径所需的前轮转角
。
[0026]具体地，首先，获取车辆的全局位姿和任务路径，然后，计算车辆在任务路径上距离最近的投影点，根据当前车辆速度从投影点开始截取前进一段路径作为根据路径，速度越快截取的路径越长；根据运动模型依次计算达到跟随路径点到跟随路径上每个路径点的控制指令
。
[0027]（2）根据建立的速度模型模拟出所有所述控制指令的未来车辆位置，
[0028]如图3所示，所述速度模型，（
X,Y
）为未来车
辆未位置，为未来车辆车身与
X
轴夹角，为当前车辆车身与
X
轴夹角，（
X0,Y0）为当前车辆位置，
v
为车辆向前或向后的车辆速度，
δ
为车辆前轮偏转角，为当前车辆位置到未来车辆位置所需时间；
[0029]可通过上述运动模型能预测出车辆在跟踪当前路径点的过程中未来行驶过的轨迹所有位姿情况
。
[0030]（3）通过代价函数计算每个控制指令的代价值，并选取最小代价值的控制指令下发到车辆执行机构；
[0031]如图4所示，代价函数，为车辆位置偏差代价函数，为车辆前轮偏转角偏差代价函数，
ka
为常数，
m
为预测路径步长，即从车辆当前位置达到实际路径的距离；
(x
m
,y
m
)
为实际路径上的期望到达的坐标；
(x
i
,y
i
)
为车辆预测路径走过的路径；
δ
i
为车辆预测过程中某一时刻的车轮偏转角；为车辆预测过程中车轮偏转角的平均值
。
[0032]代价函数原理为预测路径所有偏差的均值乘路径偏差影响系数，加上前轮偏转角的方差乘偏转角影响系数，体现车辆方向调整幅度大小和车辆与路径的距离偏差
。
...

【技术保护点】

【技术特征摘要】
1.
一种基于运动模型的预测控制算法，其特征在于，包括如下步骤：（1）根据建立的运动模型计算当前位姿下跟随路径点所有控制指令；（2）根据建立的速度模型模拟出所有控制指令的未来车辆位置；（3）通过代价函数计算每个控制指令的代价值，并选取最小代价值的控制指令下发到车辆执行机构；代价函数，式中，为车辆位置偏差代价函数，为车辆前轮偏转角偏差代价函数，
ka
为常数，
m
为预测路径步长，即从车辆当前位置达到实际路径的距离；
(x
m
,y
m
)
为实际路径上的期望到达的坐标；
(x
i
,y
i
)
为车辆预测路径走过的路径；
i
为车辆预测过程中某一时刻的前轮偏转角；为车辆预测过程中车轮偏转角的平均值
。2.
根据权利要求1所述的一种基于运动模型的预测控制算法...

【专利技术属性】
技术研发人员：周军，夏礼帅，龙羽，徐菱，
申请(专利权)人：成都睿芯行科技有限公司，
类型：发明
国别省市：