Coursera self-driving car Part1 Final Project——自动驾驶轨迹跟踪之MPC模型预测控制原理推导及Python实现

无人车轨迹跟踪之MPC模型预测控制原理推导及Python实现

wujiangzhu_xjtu

10624人浏览 · 2021-05-12 22:34:11

wujiangzhu_xjtu · 2021-05-12 22:34:11 发布

Coursera self-driving car Part1 Final Project——自动驾驶轨迹跟踪之MPC模型预测控制原理推导及Python实现

参考1: 车辆模型推导以及MPC控制实现

参考2：Coursera self driving car Part1 Final Project

这是一个Coursera上Self-driving-car的Part 1的Final Project,编写横向控制算法，这里采用MPC(Model-Predict-Control)控制算法。

1.车辆预测模型推导

1.1 车辆运动学模型

1.2 模型线性化

如1.1最后推导的车辆运动学模型(4)，输入为前轮转角 $\delta$ 和车速v，输出为车辆的后轴中心的x,y坐标及车辆航向角 $\varphi$ ，显然是一个非线性模型，要用作预测模型需要进行线性化。

线性化将运动学模型在此刻运动学方程在参考点( $x_{r},y_{r},\varphi _{r}$ )泰勒展开如下：

将A,B代入式(8)就得到当前时刻线性化的车辆运动学模型

1.3离散化

计算机计算时是离散的，将模型进行离散化便于计算。根据式(12)可以预测出车辆后续多个时刻的状态。

1.4预测

这里假设预测当前时刻后的10个时刻, Np=10,写出后续的10个时刻的方程,预测未来状态全部写成只与当前状态和接下来10个时刻的控制量相关的表达式

1.5最优化问题

1.5.1目标函数

确定目标函数，通常控制问题会选择误差平方和最小，以及控制量平方最小为目标。

1.5.2约束条件

1.5.1中约束条件，控制量不可能无限制的大。

由于期望前轮转角为0，尽可能少转向就完成路径跟踪问题

则

假设Vmax,Vmin和max, min，Vr(k)已知。约束条件需要求解Δvmax(k),Δvmin(k)

另一边最小速度的约束同理可求解。约束条件每个时刻都在变的，按上述方法可求出

2.Python实现

2.1Coursera self driving car 课程Part1 Finalproject说明

Cousera链接如下

https://www.coursera.org/learn/intro-self-driving-cars/programming/ac8R5/final-project-self-driving-vehicle-control

在这个项目中，您将编写和实现CARLA模拟器的控制器。您的目标是通过在预设的航路点上导航来控制车辆跟踪赛道。车辆需要以某些所需速度到达这些路点，因此将需要纵向和横向控制。

纵向控制我们这里采用PID控制，本文重点介绍MPC, 不多赘述。

（1）安装CARLA模拟器，按照课程前几课的教程进行下载

（2）下载“ Course1FinalProject.zip”文件，并将其解压缩到“ CarlaSimulator”（根）文件夹内的子文件夹“ PythonClient”中。这将在“ PythonClient”下创建一个子文件夹“ Course1FinalProject”，其中包含打分脚本。

将“ Course1FinalProject.zip”的内容放置在“ PythonClient \ Course1FinalProject”文件夹（对于Windows）或“ PythonClient / Course1FinalProject”（对于Ubuntu）下是非常重要的。将其安装到另一个目录可能会导致运行时问题。

（3）编辑“ controller2d.py”类文件（在“ Course1FinalProject”文件夹中的“ PythonClient”下找到）。这是您的控制器将要实现的地方。

（4）“ controller2d.py”文件包含一个控制器对象。您将在由注释块标记的update_controls方法中实现控制器（查找所有包含“ MODULE 7”的注释块-这些用作定位点，以供您在其下实现代码）。

（5）所有单位均为SI（米，秒，弧度）

（6）waypoints变量是Python的航点列表，用于跟踪每一行，其中每行表示格式[x，y，v]的航点，分别是x和y位置以及该位置的所需速度。访问第三个航路点的y位置的示例是：

waypoints[2][1] # Remember that Python's indexing begins at 0

这里解释下，按照我的理解这个waypoints是程序根据文件夹下racetrack_waypoints参考点文件里每个时刻会读取部分点进行直线插补(插值)，得到的更多参考点。在每个非常小的时间段里的参考点。每个时刻这个waypoints都会更新，只代表全路径非常小的一部分，类似与机床里的精插补。密集的点发给控制器不会引起控制量的剧烈波动。

waypoints将在每个模拟步骤中更新-因此，请不要假定航点变量永远不变。此处，waypoints是整个航点集（来自racetrack_waypoints.txt）的线性插值（用于位置和速度）子集。

(7)期望速度被计算为最接近车辆的航路点的航路点速度。

使用此信息作为控制器，您将输出车辆油门，转向和制动。“ controller2d.py”中有关这些输出的详细信息如下。这里是作为MPC优化的约束条件

(8)您也可以将CARLA的所有测量值都视为相对于车辆的中心位置。如果需要，中心位置到车辆前轴的距离为1.5米。也就是说轴距为3m

(9)在一个终端中，以30hz的固定时间步长启动CARLA模拟器：

这个终端用CMD命令提示符启动，安装了Python 3.6,进入到CarlaUE4.exe的路径下后运行一下代码可启动模拟器

以server模式可以接受外部控制命令，fps 30Hz意味着采样周期Ts=1/30 s

Windows:

CarlaUE4.exe /Game/Maps/RaceTrack -windowed -carla-server -benchmark -fps=30

(10)然后在另一个终端运行

python module_7.py

我是在VSCode中打开，便于调试

（11）如果module_7客户端正确连接到服务器，则模拟器将开始运行。它将打开两个新的反馈窗口（除非禁用live_plotting-有关更多详细信息，请参见下面的options.cfg编辑），其中一个显示轨迹，另一个显示控件反馈

一个窗口绘制小车在全局路径的实时位置，一个窗口实时绘制车子的状态信息，速度，油门等

（12）如果模拟运行缓慢，则可以尝试增加实时绘图仪刷新绘图的时间，或者完全禁用实时绘图。禁用实时绘图不会影响模拟结束时的绘图输出。

为此，请编辑“ Course1FinalProject”文件夹中的options.cfg文件以获取相关参数。下表说明了每个选项：

（13）一旦您到达最终航路点，或在经过约200到250秒的游戏秒后，客户端将关闭。模拟完成后，将保存一个包含控制器生成的轨迹的文本文件。该文件称为“ trajectory.txt”，位于“ Course1FinalProject”文件夹下的“ controller_output”文件夹内。速度，油门，制动，转向和执行的2D轨迹的图也保存在此文件夹中。

评分脚本（下面提供）将您的轨迹与航路点进行比较并对其性能进行评分。将此分级机解压缩到“ Course1FinalProject”文件夹中。

python grade_c1m7.py  racetrack_waypoints.txt controller_output\trajectory.txt

分级脚本会绘制您的轨迹以及每个航路点的速度。距离和速度误差范围也显示在图中。如果轨迹成功到达50％或更多航路点，则该轨迹通过考核。

2.2 关键代码Python实现

环境配置主要就是Numpy, cvxopt，mathplotlib然后提示差什么库下载就好。

注意：cvxopt的安装一定要与Numpy版本匹配，且需要先卸载Numpy安装才不会出问题。

cvxopt库安装参见 python3安装cvxopt正确方法（含高速下载链接）

2.2.1分块矩阵的输入

MPC控制算法涉及到很多比较大的分块矩阵，它的输入在程序相对比较复杂，这方面感觉还是Matlab用起来更顺手。也可能是我是Python刚入门或许有更好的办法，我列出一种比较笨的办法代码作为参考，

以矩阵 $\psi$ 的输入为例，这是一个分块矩阵，是一个30*3的矩阵，Akt本身是一个3*3的矩阵

矩阵的推导见1.3，1.4，参考代码如下：

            Akt=np.mat([[1,0,-vr*math.sin(phir)*Ts],
                        [0,1, vr*math.cos(phir)*Ts],
                        [0,0, 1                   ]])
            Bkt=np.mat([[math.cos(phir)*Ts, 0      ],
                        [math.sin(phir)*Ts, 0      ],
                        [0,                 vr*Ts/L]])
#组成矩阵bigphi，按分块矩阵把Akt当成一个元素，写出每一行，用vstack合并这些分块矩阵，要求列数同
            line1_=Akt
            line2_=np.dot(Akt,line1_)
            line3_=np.dot(Akt,line2_)
            line4_=np.dot(Akt,line3_)
            line5_=np.dot(Akt,line4_)
            line6_=np.dot(Akt,line5_)
            line7_=np.dot(Akt,line6_)
            line8_=np.dot(Akt,line7_)
            line9_=np.dot(Akt,line8_)
            line10_=np.dot(Akt,line9_)
            bigphi=np.vstack((line1_,line2_,line3_,line4_,line5_,line6_,line7_,line8_,line9_,line10_))

2.2.2 python实现cvxopt求解控制量及易错点

参考 python求解（线性和二次）规划问题

目标函数式(15)等效于求解(乘上一个1/2)

按照2.2.1的方法构建各个矩阵，然后采用如下cvxopt python代码求解最优U

求解前需构建标准二次型中的P,q,G,h,A,b

在调用cvxopt求解时，我碰到一个错误卡了很久，前面程序的很多构建P,q,G,h,A,b矩阵时用的是np. mat，结果调用cvxopt一直提示错误，要"用户指定kktsolvers..."什么的，后来发现是P,q,G,h,A,b必须转化为cvxopt的matrix矩阵就成功运行了。

from cvxopt import matrix
import cvxopt.solvers as sol
#将P,q,G,h,A,b转化为cvxopt matrix
P=matrix(P)
q=matrix(q)
G=matrix(G)
h=matrix(h)
A=matrix(A)  #这里不存在等式约束，则A=[], b=[],同样转化为cvxopt matrix
b=matrix(b)
sol=solvers.qp(P,q,G,h) #这里不存在等式约束,不输入A,b。若存在输入sol.qp(P, Q, G, h, A, b)
###求解出的结果是U 20*1矩阵
result=sol['x']
###取第一个预测时刻的控制量里的转向角进行输出，第一个预测时刻的控制量里的速度忽略，纵向采用PID控制
steer_output=result[1,0]    #U中第一个预测时刻的转向角就是result里第二行，第一列的数
self.set_steer(steer_output)