This article is close to 25,000 words

From theory to practice, start with zero foundation, and complete the basics of autonomous vehicle control step by step

Thanks: loyal and honest Pharaoh

The following is his homepage: Personal Space of the Honest and Honest Pharaoh_哔哩哔哩_bilibili

Table of contents

Lecture 1 Introduction

Lecture 2 Three Coordinate Systems and Kinematic Equations

1. Control principle

Two, three coordinate systems

Three, left-handed and right-handed

4. The Kinematic Equations of the Bicycle Model

Lecture 3 Tire cornering and vehicle dynamics equations

1. Decoupling of the dynamic equation and the Frenet coordinate system

2. The cornering characteristics of the tire

3. Bicycle model considering tire cornering characteristics

Lecture 4 Coordinate Transformation and Lateral Error Differential Equation

Lecture 5 Discretization of continuous equations and principle of discrete LQR

1. Discretization model

Second, solve lqr

3. Riccati Equation

Four. Summary

Lecture 6 Feedforward Control and Heading Error

1. Feedforward control

Lecture 7 Error Calculation of Discrete Programming Trajectory

1. Error calculation of discrete trajectory points

eighth lecture

1. Horizontal control algorithm and flow chart

(1) Algorithm input and output process

(2) Flowchart

(3) Specific algorithm

(4) Prediction module

2. CarSim and Matlab co-simulation basis

3. Code and model

Lecture 9: Vertical Control Beginning

1. Vertical control logic

2. Engine and transmission

3. Basic settings of CarSim

Lecture 10 Making of Accelerator and Brake Calibration Table

1. Theoretical part

2. Production of throttle calibration table

3. Production of brake calibration table

Lecture 11 Vertical Dual PID Control

1. Preparation

2. Preparation of accelerator and brake calibration table

3. Longitudinal speed PID control simulation

Lecture 12 Completion of Horizontal and Vertical Integrated Control

1. Planning interface

2. Code practice part

3. Understeer

Lecture 1 Introduction

Please use a cautious, critical, and skeptical attitude to study the relevant literature on autonomous driving

New technologies are often ambiguous and controversial. Autonomous driving is a new technology and cannot be avoided

Tools: google, Github

Software: matlab (2018a), Carsim 2016, ROS, Linux

Book: "Model Predictive Control of Unmanned Vehicles" Gong Jianwei (Second Edition) (Many errors in the first edition)

"Vehicle Dynamics and Control" => "Vehicle Dynamics and Control" RR (Baidu Apollo)

Autonomous driving technology: Perception->Decision->Path Planning->Control

Perception and decision-making are difficult, and control is difficult in tuning parameters. The main algorithms are PID, LQR, and MPC

芯片计算速度（台式机性能的芯片）车载芯片工作环境极为恶劣

第二讲三个坐标系与运动学方程

控制的前提是路径规划，默认已经有路径规划的储备知识

一、控制原理

油门/刹车 -> 力 -> 加速度 -> 速度 -> 位置（纵向控制）

方向盘 -> 前轮转角 -> 横向位移/航向角（横向控制）

二、三个坐标系

Frenet坐标系优点是：可以把纵向控制和横向控制解耦，减少计算量

车身坐标系与自然坐标系转换会难一些

三、左手系与右手系

一般数学，物理学是右手系。计算机图形，视觉为左手系

四、自行车模型的运动学方程

自行车模型将车简化为前后两个轮子和一个轴，假设车轮轮距为0，

求质心速度：瞬心法

假设前轮速度v'，后轮速度为v，利用瞬心法求出质心速度vc

横摆角与航向角

注意这里的前轮转角和后轮转角会由于轮胎的刚度而并不与方向盘转角一一对应，这里假设低速不考虑轮胎变形的自行车模型，所以前轮转角可以控制航向角

纵向车速与横向车速

下面建立微分方程：即前轮转角如何影响到侧向位移与航向角

①根据几何关系建立运动学模型；②根据牛顿力学建立动力学模型

动力学模型更加复杂，考虑轮胎变形，参数很难准确。

运动学模型简单，所以只适用于低速转弯半径大的情况。

运动学模型

首先角度推导

利用速度投影得到几何关系：

此时并未体现前轮转角 $\delta _{f}$ 对于 $X, Y, \varphi$ 的影响，要进行进一步推导

设前轮到质心距离a，后轮到质心距离b，根据正弦定理

展开后得到

由于质心位置会变，所以a和b是变量，需要用一些不变的量来表示

使用（a+b）/R来表示，因为a+b=L为轴距不会变

这里由于质心侧偏角≈0，所以cosβ≈1

带入上面第三个式子，由此得到运动学方程：

在低速条件下，认为车不会发生侧向滑动（漂移），vy≈0，这个时候质心侧偏角 $\beta$ 为0

一般后轮不转向，在低速条件下，认为 $\delta_{r}$ =0，所以有下面这个运动学方程

Since beta=0, the yaw angle is approximately equal to the heading angle

where $\varphi$ is the yaw angle and v is the velocity of the center of mass

Lecture 3 Tire cornering and vehicle dynamics equations

Kinematic equations: too much simplification, not applicable at high speed

Kinetic Equations: Considering Tire Properties

1. Decoupling of the dynamic equation and the Frenet coordinate system

When the Frenet coordinate system is selected, the vertical control can be decoupled from the horizontal control

Longitudinal control only needs to control the accelerator and brake, and does not need to control the steering wheel, that is, it is decoupled from the lateral control

When longitudinally stable, lateral control is only related to steering wheel angle

Whereas the kinematic equations cannot be decoupled, which is why the kinetic equations are used

Dynamics + Frenet coordinate system can be decoupled

The longitudinal displacement s, the second derivative of s is the longitudinal acceleration determined by the accelerator and brake

The relationship between the lateral displacement d and the steering wheel angle $d = f(v,\delta)$ is given in the car theory, but it is under the body coordinate system, so how to convert this formula to the Frenet coordinate system is the key

The following lateral displacements are expressed in y

2. The cornering characteristics of the tire

After the vehicle tire is stressed, it will become the shape of a circular platform, and it can only walk in a circular arc.

Why is the deformation of the tire a trapezoid, because the trapezoid rotates 180 around the tire axis and still looks the same, but the parallelogram is not

The side slip will cause the actual speed of the tire to have an angle with the expected speed direction $\alpha$ , which is called the side slip angle

The slip angle is directly proportional to the lateral force

Definition of slip angle: $F = C \alpha$ , where C is cornering stiffness, cornering stiffness * side slip angle = lateral force

Note: The cornering stiffness must be a negative number, negative cornering force => positive cornering angle

In addition, the lateral force is generally expressed by Fy

3. Bicycle model considering tire cornering characteristics

The two-degree-of-freedom model can be expressed more accurately under the premise of two degrees of freedom.

Note that $\alpha_{f}, \alpha_{r}$ they are all negative, according to the force analysis: (note that the first formula is $m_{is}$ may, not ay)

Assuming that $\delta$ it is relatively small, there is $cos \delta \approx 1$ , and the lateral deviation formula is brought into

$cos \delta \approx 1$ Verification: L is the wheelbase, approximately equal to 3m; R is the turning radius, generally 8-12m;

$tan \delta = 3/8$ Right now $cos ? = 0.93$

The $Oh}$ relationship with y and $\alpha_{f}, \alpha_{r}$ the specific expression of

$Oh}$ The relationship to y is directly given by:

注意这里 $Oh}$ 后面还多了一项，因为这个公式是在车身坐标系下，要带上惯性力

加速度是个矢量，等于xy两个方向的速度乘单位矢量和，

在直角坐标系下，单位矢量ex和ey是常矢量

在车身坐标系下，ex和ey不是常矢量，所以其导数不等于0，因此多出来了一项

下面推导 $\alpha_{f}, \alpha_{r}$ ，

根据刚体的速度合成与分解，把vr移动到质心处，连接vr与v的线一定与车的轴线垂直，并且大小等于 $\dot{\varphi} b$

下面放大看

蓝色线的长度 $= v cos \beta = v_x$ ，竖着的蓝线为，进而推出 $\alpha_{r}$

为了保证 $\alpha_{r}$ 是负数，一半phib比较大，vy比较小，所以可以交换一下位置，改变正负号

下面画出 $\theta$ 和 $\alpha_{f}$ ，图中绿色角是 $\theta$ ，上面的角是 $\alpha_{f}$

注意这里 $\theta$ 是前轮速度与车身x轴的夹角， $\delta$ 是前轮转角。 $\theta$ 可以通过公式推出，这样就得到 $\alpha_{f}$ 表达式。

我们把局部图放大来看，蓝色的角为上面绿色的角 $\theta$

对 $\theta$ 取tan得到，根据几何关系进而得到： $\theta + \alpha_{f} = \delta$ ，即 $\alpha_{f} = \delta - \theta$ ，然后跟上面一样需要加一个负号保证\alpha_{f}为负数，就得到了下式

得到 $\alpha_{f}, \alpha_{r}$ 带入上面公式得到（这里 $a_{y}=\dot{v_{y}}$ ，下面公式写错）

整理得到

即可得到状态空间方程

第四讲坐标变换与横向误差微分方程

要控制的目标就是：让汽车当前状态与规划的状态接近

规划给出：车位置（ $x_{r}, y_{r}$ ），速度 $v_{r}$ 、航向角 $\theta_{r}$ 、加速度 $a_{r}$ ，则可以计算出五个误差

定义：真实的-规划的 = 误差，得到误差的表达式

控制目标是

Define a cost function J=a*error square+b*u square, which can be rewritten into vector form

Define 5 errors according to the projection method, project the velocity v in the natural coordinate system to the tangential and normal directions of the planned position to obtain the sum $\dot{s}$ d, the projected $\ can$ velocity on it is $\dot{s}$ , and the distance from the car to the projection is set to

The error is defined as follows: the lateral error is naturally d, the heading error is $\theta - \theta_{r}$ , the projected speed is $\dot{s}$ , the speed error is $v - \dot{s}$ (here is the size, not including the direction, because the heading angle controls the direction), and the speed error belongs to longitudinal control, so let’s ignore it temporarily

Note $\ can$ : \tau and n here and x and y of the body coordinate system are not a coordinate system, because v is the center of mass velocity, the angle between the center of mass velocity and the x-axis of the absolute coordinate system is the heading angle \theta, and the body coordinate system is based on The yaw angle is the direction, the x-axis is the direction along the yaw angle, and the y-axis is perpendicular to the yaw angle direction, so $\ can$ and n are not the x and y of the body coordinate system. (Personal understanding, the difference between the two is a center of mass side slip angle $\beta$ )

d is the lateral error, and the planned position vector xr and the real position vector x form a triangle,

Write the expression according to the vector relationship, (here d is a number, not a vector, so it needs to be multiplied by the direction vector nr)

It is not enough to calculate the expression of d, because our state space equation $(\dot{y},\dot{\varphi })$ is considered to be an unknown quantity, but d has no connection $v_y$ with and $\dot{\varphi}$ . In order to connect, we can derive d from

Note here that nr is not a fixed constant vector, and its direction will change with the planned position, which is related to the curvature

Derivation, assuming that the vector r reaches r+dr after dt time, the blue one is the difference vector dr, and the red arc is ds

Substitute into the derivative of d to get (note that in the front brackets here, $\tau_{r}$ and are $n_{r}$ vertical, so dot multiplication = 0 can be eliminated)

The last item , $ds/dt = \dot{s}$ , this is nothing to say, but $dn_{r}/ds$ what is it?

Here we need to use the Frenet formula to find $dn_{r}/ds$

brought into $dn_{r}/ds$

Bring it into the above formula to get, ( $\tau_{r}$ and $n_{r}$ vertical, so dot multiplication = 0 can eliminate the latter item)

In order to $\tau_{r}$ sum $n_{r}$ the relationship between the angles, draw a graph, then the angle between $\tau_{r}$ and can be obtained through the geometric relationship $n_{r}$

brought in to get

Next calculate $\dot{s}$

Derivation to get

Then bring the results of the above derivation and the Frenet formula into

Both sides get:

$\ can$ $\tau_r$ The angle between and is actually $\theta - \theta_{r}$

Derived here, we get two formulas:

These two formulas are very important and are the starting point of all work

It has to $v_{y}, \varphi$ be linked with and brought into the heading angle $\theta = \varphi + \beta$ to get:

Note that in the above formula, both vy and vx in the bicycle model can be represented by v and the side slip angle of the center of mass

d is the longitudinal error, let $e_{d} = d, e_{\varphi} = \varphi - \theta_{r}$ , ( note : it $e_{\varphi }$ is not the heading error, it should be $\theta - \theta_{r}$ the side slip angle of the center of mass $\beta$ , but it is generally $\beta$ relatively small and can be ignored, but this is more important and cannot be ignored later)

Find the first and second derivatives of vy and ephi

Generally the road is flat, so ignore $\theta_{r}$ the second derivative of

Bring in the two-degree-of-freedom dynamic equation , and then simplify to get two points of ed (here because vx and vy need to be eliminated)

In the same way, get \e_{\varphi} two points

These two equations are the differential equations of the error, which are simplified below

Compose the following governing equation , and thus get the error linear differential equation

Three Remaining Questions

(1) Heading error problem: missing side slip angle of center of mass

(2) How to calculate the last term in the equation: $\dot{\theta}_{r}$

(3) You can use LQR to calculate the small tail after removing the last item, how to control it after adding it

Lecture 5 Discretization of continuous equations and principle of discrete LQR

Ignoring this item for now $C\dot{\theta}_{r}$ , the formula is converted to $\dot{e}_{rr} = A e_{rr} + Bu$

Matlab has ready-made packages: lqr(A,b,Q,R) and dlqr(A,B,Q,R) (discretized LQR) can calculate u

The main application is dlqr, continuous lqr is difficult

The dlqr principle only involves the Lagrangian multiplier method

Lagrange multiplier method: Find the extreme value of f(x,y) under the constraint g(x,y)=0:

$L(x,y) = f(x,y) + \lambda g(x,y)$

dlqr question:

beg

This minimum must be the minimum

So decompose the dlqr problem into two problems

One is how to discretize, and the other is how to solve the optimization problem

1. Discretization model

Discretization: You can use forward Euler method or midpoint Euler method, apollo is a mixture of the two

Integrate to eliminate x first, then use the mean value theorem to approximate

How forward Euler's law, backward and midpoint Euler's law are replaced by $x(\xi)$

x(t+dt) can be represented by x(t)

For our problem, we can first integrate and then use the median value theorem

Then $x(\xi)$ replace the sum $and(\xi)$ with $and(\xi)$ forward Euler, because u(t+dt) does not know

Simplify to get

dt is the sampling period to complete discretization

Second, solve lqr

The following deduces the solution of ldqr

Let J calculate from 0 to n first, and then let n tend to infinity. Here it is split into 0 to n-1 and the nth item. (My understanding of this step is that because the recursive relationship between the nth and n-1 steps needs to be deduced, it should be decomposed in this way)

write constraints

The constraint just completely covers the independent variable. If the nth constraint is added, an additional item x_{n+1} will be added

If n tends to infinity, it doesn't matter whether there is n+1 or not

So we can use the Lagrange multiplier method and write J as

Simplify it below

Next, we need to derive J, so first give the definition of vector derivative:

here

Differentiate the independent variables to get

Then put the simplified partial derivative of Hk into the above formula

Get the following formula (note the typo here: here ③ formula k starts from 0), a total of 3n-2 formulas

Based on the above formula, the following recursive formula is obtained

Compare ⑧ with ④

In order to find the relationship $\lambda_{k}$ with $x_{k}$

Get the expressions of Pn=Q and Pk-1:

Based on these two formulas, Pn=Q can be used as the initial value, and it can be derived from k=n to k=0

也就是说我们知道了 $\lambda_{k}$ 与 $x_{k}$ 的关系，下面再关注②式，可以将 $u_{k}$ 由 $x_{k}$ 表示出来

式子中的R，B，A均为已知量，P是黎卡缇方程算出来的

注意这里的xk，一般可以用误差来表示，根据上面对状态空间方程的定义有xk=err

err可以通过定位和规划得到

所以uk可以由已知量算出

此外注意，这里的输入uk是车的前轮转角，只有一个数，因此B是一个4*1的向量

三、黎卡缇方程

注意：对于黎卡缇方程，如果n区域无穷大，那么黎卡缇方程要迭代无数次，但是实际情况是Pk迭代几十次后就会收敛，即从Pk一直到P0均相等，这个性质非常好，可以帮助我们处理无穷时间

所以

解黎卡缇方程的步骤为先给P取初值，然后迭代求出P，然后再根据P求出u

课本上面的黎卡缇方程为

和我们这里其实一样，只是方程形式写的不一样，推导一下

我们上面推导的黎卡缇方程为4*4矩阵求逆，课本给出的这个是1*1求逆，即求倒数。

矩阵求逆是基于数值的方法，所以1*1求逆精度会更高，所以推荐使用下面这种。

四、总结

第六讲前馈控制与航向误差

$\dot{x} = Ax + Bu$ 的框图表示

如果u = -kx，即相当于加上一个反馈，如图所示

容易出现代数环问题，输入影响输出，输出影响输入，这里先不管，后面可以用matlab加延时的方法解决

一、前馈控制

在上面的框图内再加一个输入量 $\delta_{f}$ ，变成前馈控制

因为我们要解决第四讲的遗留问题，即如何处理这个小尾巴

如果只用LQR，

无论k取何值， $\dot{e_{rr}}$ 和 $e_{rr}$ 不可能同时为0，即 $e_{rr}$ 不可能永远为0，因为 $\dot{e_{rr}} = 0$ ， $e_{rr}=0$ 不是该微分方程的解

所以要引入前馈控制： $u= -kx + \delta_{f}$ ，其中-kx是由LQR计算出的反馈控制， $\delta_{f}$ 为前馈控制

前馈控制的引入是为了消除稳态误差

LQR最终会导致 $\dot{e_{rr}} = 0$ ， $e_{rr} \neq 0$ ，此时的稳态误差可以计算得到：

引入前馈控制后，有

稳定后的

目的：选取合适的 $\delta_{f}$ ，使得尽可能为零

上面已经推导出了A，B，C，注意这里的 $K = [k_1,k_2,k_3,k_4]^{T}$

把ABC带入，用软件Mathmatica计算得到err的表达式

可以看出每项都有 $\dot{\theta}_r$ 这一项，此外注意 $e_{rr} = [e_{d},e_{\varphi},\dot{e_{d},\dot{e_{\varphi}]$

$\dot{\theta}_r$ 对 $e_{rr}$ 的影响

先算反馈K，再算前馈

无论前馈反馈取何值， $e_{\varphi } \neq 0$

下面对 $e_{\varphi }$ 进行化简，首先利用几何关系求出 $\dot{\theta}_r$

求曲率

利用曲率k求出 $\dot{\theta}_r$ 与s的关系，这边做了很多假设

带入后得到 $e_{\varphi }$ 与vx的关系

然后求出ay和vx的关系，

再带入上式把vx2消掉，得到

其中，may为总侧向力，cr为后轮侧偏刚度

将车辆等效成前后两部分，分别计算出前后质量

根据后轮侧向力与ay的关系，以及等效关系，将其进一步化简

根据几何关系

得到的 $-\beta$ 刚好等于 $e_{\varphi }$ ，

虽然 $e_{\varphi }$ 不是航向误差，但是其稳态误差为 $-\beta$

虽然 $e_{\varphi }$ 不可能通过 $\delta_{f}$ ，K去调节，但是我们不用理会。

因为最终目的是 $\theta-\theta_{r} = 0$ ，即 $e_{\varphi } = -\beta$ ，而 $e_{\varphi }$ 的稳态误差恰好为 $-\beta$ ，所以不用管。

至此，得到u的表达式

其中K是通过lqr计算得到， $\delta_{f}$ 通过前馈控制得到

第七讲离散规划轨迹的误差计算

基于前六讲已经算出u的表达式

误差的计算在第四讲

其中：

只要知道

就可以把误差计算出来

若规划的曲线是连续的，可能会导致投影不唯一

若A与A'的连线与A'的切线垂直，则A'为A的投影

若曲线是连续的，不仅仅是投影麻烦，而且要处理多值问题，因此要离散

一、离散的轨迹点的误差计算

将轨迹离散成一些点，每个点都有四个信息

如何计算基于离散的误差呢？

方法如下：

①找到离散轨迹规划点中与真实位置（x,y）最近的点

这个点在Apollo里面称之为match-point（匹配点）

②匹配点代替投影点的前提：规划的点很密

现实情况是规划不能很密，否则计算量过大，所以需要通过匹配点近似算出投影点

假设：从匹配到投影点的曲率k不变。即认为匹配点和投影点的轨迹近似用圆弧代替

匹配点的切向向量和法向向量

匹配点与车实际点的向量：

③求出ed（有正负：左为正，右为负）

④求出es：匹配点与投影点的弧长（es有正负）

正代表投影在匹配点前面，负代表在匹配点后面

⑤ $\theta_{r} = \theta_{m}$ （apollo）

$\theta_{r} = \theta_{m} + k_{m}e_{s}$

⑥算出误差

这样就可以算出离散的err，再使用离散LQR计算出K，进而计算出 $u = -Ke_{rr} + \delta_{f}$

第八讲

一、横向控制算法与流程图

（1）算法输入输出过程

首先要搞明白算法，才能写代码

注意这里第四步为： $u = -K e_{rr} + \delta_{f}$ ，不是x

（2）流程图

（3）具体算法

①A，B计算模块

up勘误:这里矩阵第四行第二列第三列写错了,第五讲开头那个矩阵才是对的

注意要考虑当 $v_{x}$ = 0时的奇异性

②LQR模块

因为A，B只与整车参数和 $v_{x}$ 有关，整车参数近似认为不变

情况1：如果加速度非常大，惯性力就比较大，这个时候前轮垂向力减少很多，后轮加大，侧偏刚度改变。此时仍然可以使用自行车模型，只是侧偏刚度改变。

情况2：急速过弯时，左右轮不对称，导致自行车模型不适用，所以在路径规划的时候要极力避免急速过弯的情况

A，B只与整车参数和 $v_{x}$ 有关， $K = lqr(A,B,Q,R)$ 或者 $dlqr(\bar{A},\bar{B},Q,R)$

每个 $v_{x}$ 都有唯一的一个K与之对应

所以可以离线算出 $v_{x}$ 与K的对应表，实际应用中不需求解Riccati方程，直接查表

优点：速度大大加快

缺点：耗费储存空间（空间换时间）

个人理解：针对每个vx，可以求出对应的AB，对于每个AB都可以迭代算出P，然后算出对应的K

MATLAB自带的模块，只需要输入A，B，Q，R即可得到K

③ $e_{rr}, k$ 计算模块

首先要有离散的规划点

遍历找到距离当前车的位置最近的规划点，该点的序列（index）记为dmin

写出匹配点的切向向量与法向向量

写出误差距离向量(个人理解：这里的x_dmin就是上面的xm，即投影点的坐标，d_err为匹配点与车实际位置之间的位矢)

算出误差ed和es（个人理解：ed是投影点和实际位置之间的位矢，es是投影点与匹配点之间的位矢）

算出 $\theta_{r}$ （个人理解：这里的 $k_{d_{min}}$ 是匹配点的曲率）

算出其他误差和变量

输出

④前馈控制计算模块

⑤最终控制计算模块

（4）预测模块

车有惯性，所以要提前控制，要增加一个预测模块

举例：如下图，假设当前位置不在路径上面，但是速度朝向路径，如果是人在开车，他不会打方向盘改变方向，但是算法为了减小误差，它会打方向盘为了减少误差

这个例子也是一样，如果当前点在路径上，算法不会打方向盘，但是人知道要转弯

算法控制具有滞后性，所以要加上预测模块，让算法有预见性

解决办法：使用预测点 $x_{pre}$ 代替真实点去计算误差

预测算法写法：

首先有个预测时间，记为ts，可以使用速度×时间得到位置

根据运动计算出预测点的全部信息

至此介绍完毕算法流程图，下面搭建仿真平台

二、CarSim与Matlab联合仿真基础

CarSim这里使用2019版本，具体下载和安装可参考下面这个视频

最新！Carsim2019详细安装教程【附安装包】_哔哩哔哩_bilibili

（1）CarSim于Simulink基础设置

首先要知道两个重要路径

CarSim默认数据库路径：C:\Users\Public\Documents\CarSim2019.0_Data

CarSim安装路径：E:\CarSim2019.0_Prog

打开CarSim软件，需要选择数据库路径

点击continue进入

注意这个时候是灰色的不可选的状态，点击Lock就可以解锁，进行选择

点击这里选择车辆，ABCDE级车，每个车的惯量，尺寸等参数都不一样

整车尺寸参数

进入轮胎界面，可以看到Fy：侧偏曲线：侧向力与侧偏角的关系曲线，

由于做控制，不允许大侧偏角，容易失稳，只考虑小侧偏角下的曲线，此时斜率为侧偏刚度

model选择Simulink

点击这个地方，新建一个数据库

新建数据库，起名叫a

然后需要填入simulink模型，这里没有，所以新建一个，打开Matlab

打开MATLAB，选择Simulink

创建好空模型，然后直接Save as，保存到数据库路径，设置文件名为a，这里可以和CarSim的不一样

将刚才两个重要路径都添加进MATLAB路径

（2）输入输出设置

设置输入

新建数据库并起名字ain‘

同样把输出aout也新建好

点击ain，进入设置，选择Baseline

选择Select by units

将油门，前轮后轮转角都加入，完成设置

注意：L1是左前，L2是左后，R1是右前，R2是右后

同理设置输出，为如下几个变量

接下来回到主界面，然后选择SHow，Adcanced，再加上这句话： opt_steer_ext(1) 4

因为目前只能通过方向盘转角来控制，而我们是通过前轮转角控制，加上之后就是前轮转角控制

Procedure里面可以设置仿真时长和距离

这里设置为10s和21000m

一切准备就绪，点击Send to simulink

打开Simulink后，有两种方法导入SF函数，方法1是和老王一样，这种在2016可以用，点击选择文件，进入安装路径，选择Solver_SF.mdl打开

方法2是CarSim2019可以直接在libray拉进来

不管是那种方法，都需要把SF函数这个里面填入名字和数据库路径下的sim文件名字相同

然后加一个5输入mux，，分别是输入油门设这位0.5，前后轮转角设置为0。后面再加一个demux，前两个接给示波器看下位置变化

点击仿真Run

再点击示波器

回到CarSim，点击Video就可以看到仿真视频

其结果应该是走一条直线（因为并未给前后轮转角），共花费10s

Simulink将前轮转角改成30，看仿真结果

结果为原地画圆

三、代码与模型

代码下载地址：老王github上VincentWong3/automated-driving-control (github.com)

嫌麻烦的话可以在这里下载

automated-driving-control-main.zip - 蓝奏云

在写代码之前，先要明确算法流程

我们需要的是整车参数，车辆状态以及规划

整车参数可以在CarSim里面

注意这里的质量1270是簧上质量，还需要加上悬架质量71，总质量是1412

此外需要注意的是侧偏刚度的单位，以及加上负号，由于是自行车模型还要乘以2

把空气动力学关掉，暂时忽略风阻的影响

由于需要用到车辆状态，所以需要在CarSim中输出状态

（1）路径规划信息生成

由于需要规划的路径，所以这里需要一个规划函数给出信息，这里对应的是routing_planning.m文件，里面有两个函数，一个是直线，一个是圆弧

straight函数：输入起点和终点坐标以及初始角和离散点数量，生成这样一系列点

function[xr,yr,thetar,kr]=straight(init_coord,end_coord,init_angle,count)
delta_x=(end_coord(1)-init_coord(1))/(count-1);
delta_y=(end_coord(2)-init_coord(2))/(count-1);
for i=1:count
    xr(i)=init_coord(1)+delta_x*i;
    yr(i)=init_coord(2)+delta_y*i;
    thetar(i)=init_angle;
    kr(i)=0;
end      
end

用法举例：

[xr,yr,~,kr]=straight([0,0],[50,50],pi/4,10);
scatter(xr,yr)

arc函数：输入起点终点坐标以及起点终点角度以及点的数量

function[xr,yr,thetar,kr]=arc(init_coord,end_coord,init_angle,end_angle,count)
    L=sqrt((init_coord(1)-end_coord(1))^2+(init_coord(2)-end_coord(2))^2);
    R=L/sqrt(2*(1-cos(end_angle-init_angle)));
    delta_angle=(end_angle-init_angle)/(count-1) ;
  
       for i=1:count
           if delta_angle>0
               xr(i)=init_coord(1)-R*sin(init_angle)+R*sin(init_angle+delta_angle*(i-1));
               yr(i)=init_coord(2)+R*cos(init_angle)-R*cos(init_angle+delta_angle*(i-1));
               thetar(i)=init_angle+delta_angle*i;
               kr(i)=1/R;
           else
               xr(i)=init_coord(1)+R*sin(init_angle)-R*sin(init_angle+delta_angle*(i-1));

               yr(i)=init_coord(2)-R*cos(init_angle)+R*cos(init_angle+delta_angle*(i-1));
               thetar(i)=init_angle+delta_angle*i;
               kr(i)=-1/R;
           end               
       end  
end

用法举例：

[xr,yr,~,kr]=arc([0,0],[50,50],0,pi/2,100);
scatter(xr,yr)

根据这两个函数，可以设计出一条路径出来，并包含每个离散点的xr,yr,thetar以及kr

count=50;
[x1,y1,theta1,kr1]=straight([0,0],[20,0],0,count);
[x2,y2,theta2,kr2]=arc([20,0],[30,10],0,pi/2,count);
[x3,y3,theta3,kr3]=arc([30,10],[40,20],pi/2,0,count);
[x4,y4,theta4,kr4]=arc([40,20],[40,40],0,pi,count);
[x5,y5,theta5,kr5]=arc([40,40],[35,35],pi,3*pi/2,count);
[x6,y6,theta6,kr6]=arc([35,35],[25,35],3*pi/2,pi/2,count);
[x7,y7,theta7,kr7]=arc([25,35],[15,35],pi/2,3*pi/2,count);
[x8,y8,theta8,kr8]=arc([15,35],[5,35],3*pi/2,pi/2,count);
[x9,y9,theta9,kr9]=arc([5,35],[-15,35],pi/2,3*pi/2,count);
[x10,y10,theta10,kr10]=straight([-15,35],[-15,15],3*pi/2,count);
[x11,y11,theta11,kr11]=arc([-15,15],[0,0],3*pi/2,2*pi,count);
xr=[x1,x2,x3,x4,x5,x6,x7,x8,x9,x10,x11];
yr=[y1,y2,y3,y4,y5,y6,y7,y8,y9,y10,y11];
thetar=[theta1,theta2,theta3,theta4,theta5,theta6,theta7,theta8,theta9,theta10,theta11];
kappar=[kr1,kr2,kr3,kr4,kr5,kr6,kr7,kr8,kr9,kr10,kr11];

scatter(xr,yr)

这里使用的路径并不是好路径，会发生曲率突变，只是用作演示用直线和圆弧简单拼接

回到CarSim设置路面

进入Procedure

选择Two Lanes

进入道路设置

设置和刚才规划一样的道路，先解锁，然后Rows改成11，然后照这样填表，

此外把Treat as loop也点上

接下来点击Video查看建造的道路结果，注意一定要改成Flat

这个道路转弯半径非常小，最小只有5m，路径非常严苛，可以查看我们的控制效果

接下来回到MATLAB（注意这里还是跟只前一样，最好send to simulink），先运行一下，让变量在工作区内

接下来打开simulink，pid_lqr_demo里面已经写好了，这里重写一遍

（2）预测模块代码

首先5输入6输出，然后单位换算，

新建matlab 函数，预测模块比较好写，先写预测模块，把每个输出标出来

预测模块的算法

输入为当前状态 $x, y, \varphi$ 和速度v，预测时间ts

输出为预测的状态，算法为

代码如下：

function [pre_x,pre_y,pre_phi,pre_vx,pre_vy,pre_phi_dot] = fcn(x,y,phi,vx,vy,phi_dot,ts)
pre_x = x + vx*ts*cos(phi) - vy*ts*sin(phi);
pre_y = y + vy*ts*cos(phi) - vx*ts*sin(phi);
pre_phi = phi+phi_dot*ts;
pre_vx = vx;
pre_vy = vy;
pre_phi_dot = phi_dot;
end

simulink里面还要输入单位换算，ts设置为0.1

（3）误差计算模块代码

根据流程图，下一步是误差计算模块

输入的为预测点信息，规划的信息。输出为投影点的曲率和误差。

算法流程如下：

代码

function [kr,err] = fcn(x,y,phi,vx,vy,phi_dot,xr,yr,thetar,kappar)
%找到规划轨迹的长度，即多少个规划点，最短距离d_min，序号记为min
n = length(xr);
d_min = (x - xr(1))^2 + (y - yr(1))^2;
min = 1;
for i = 1:n
    d = (x- xr(i))^2 + (y - yr(i))^2;
    if d < d_min
        d_min = d;
        min = i;
    end
end
%找到最短点序号，按照算法一步一步来
dmin = min;
tor = [cos(thetar(dmin)); sin(thetar(dmin))];
nor = [-sin(thetar(dmin)); cos(thetar(dmin))];
d_err = [x - xr(dmin); y - yr(dmin)];
ed = nor' * d_err;
es = tor' * d_err;
% 算thetar两种方法
projection_point_thetar = thetar(dmin); %apollo
% projection_point_thetar = thetar(dmin) + kappar(dmin) *es;
ed_dot = vy * cos(phi - projection_point_thetar) + vx * sin(phi - projection_point_thetar);
%%%%%%
%第九步这里加个sin，假设较小用正弦代替，消除±2pi的印象
ephi = sin(phi - projection_point_thetar) ;
s_dot = vx*cos(phi - projection_point_thetar) - vy * sin(phi - projection_point_thetar);
s_dot = s_dot/(1 - kappar(dmin) * ed);
ephi_dot = phi_dot - kappar(dmin) * s_dot;
kr = kappar(dmin);
err = [ed;ed_dot;ephi;ephi_dot];
end

其中xr，yr，thetar，kappar用from workspace模块导入（运行simulink的时候必须提前工作空间内有，如果避免麻烦都弄成行向量）

（4）LQR模块代码

下面写LQR模块，matlab有自带的lqr包，可以直接使用

首先定义好参数，然后使用matlab自带的lqr包算出来K，然后拆成k1,k2,k3,k4

cf  = -110000;
cr = cf;         %差别不大，近似认为是一样的
m = 1412;   %质量，包含簧上质量和悬架质量
Iz = 1536.7;  
a = 1.015;
b = 2.910 - 1.015;
k = zeros(5000,4); %LQR 的K矩阵
for i = 1:5000
      vx = 0.01*i; %每隔0.01秒刷新一下K，0-50m/s
      A = [0,1,0,0;
          0,(cf+cr)/(m*vx),-(cf+cr)/m,(a*cf-b*cr)/(m*vx);
          0,0,0,1;
          0,(a*cf-b*cr)/(Iz*vx),-(a*cf-b*cr)/Iz,(a*a*cf+b*b*cr)/(Iz*vx)];
      B = [0;
          -cf/m;
          0;
          -a*cf/Iz];
      Q= eye(4);
      R = 100;
      k(i,:) = lqr(A,B,Q,R); %k就是离线lqr的表
end
%由于Simulink没法传矩阵，只能传行向量  
k1 = k(:,1)';
k2 = k(:,2)';
k3 = k(:,3)';
k4 = k(:,4)';

simulink新建一个lqr函数，根据vx来选择对应的k

function k  = fcn(k1,k2,k3,k4,vx)
if abs(vx) < 0.01
    k = [0,0,0,0];
else
    index = round(vx/0.01);
    k = [k1(index),k2(index),k3(index),k4(index)];
end

end

运行一下获取四个k，然后输入进lqr

（5）前馈控制模块代码

接下来是算前馈的模块，需要整车参数，vx，kr（投影点的曲率），直接带公式

代码

function forward_angle = fcn(a,b,m,vx,cf,k,kr)
forward_angle = kr*(a+b-b*k(3)-(m*vx*vx/(a+b))*(b/cf)+(a/cr)*k(3)-(a/cr));
end

（6）整合模块代码

最后一个整合模块

function angle = fcn(k,err,forward_angle)
angle = -k * err + forward_angle;
end

（7）仿真运行

此时所有代码写完了，打包封装一下，选中SF输出的模块 Crtl+G打包

将输出的angle反馈给CarSim，注意弧度转换为角度，CarSim是角度

再加一个延迟模块，作用是：如果输入是这一时刻的值，输出是上一时刻的值。等于延迟一个单元，为了避免代数环。

节气门开度给个0.15，后轮转角给个0

此外给angle加一个限制防止角度太大超过物理限制，再加一个示波器查看angle变化

此外，将初速度调成不设置初速度（默认120），因为速度过大，道路严苛，很难控制容易报错

点击run，使用XY-graph查看

X范围有点小，调大

这个时候点击run

仿真结果如图所示（注意如果时长短是因为上面Procedure里面改成10s仿真了，只需要改成50S然后再Send to Simulink即可）

回到CarSim点击Video查看实际运行视频

前轮转角不停抖来都去

改进方法

计算thetar的时候，加上es，这样就可以防止大抖动，减少抖动

但是这种方法没法解决突变问题，因为规划的曲率突变，控制很难解决，得在规划的时候进行插值让曲率连续，要不然就把ＬＱＲ的Ｒ调大一点

另外，不能让规划的点过少，这样会让车围绕一个点不停打转，因为匹配点不会改变

第九讲纵向控制开篇

纵向控制比较简单，不需要太多的数学知识

一、纵向控制逻辑

首先要明白：当你踩下油门/刹车时，你到底在控制什么？

横向控制逻辑：方向盘-->前轮转角-->车的航向角

纵向控制：油门/刹车--> ？--> 车加速/减速

这里的？到底是什么，到底什么被改变了，导致车速改变

直观上想到发动机转速或者发动机扭矩

正确答案：油门-->发动机功率被改变-->?-->车加速

刹车-->制动压力增大-->制动盘摩擦增大-->阻力增大-->车减速

功率与车加速不直观

什么与车加速直接相关？车速加大，只需要加速度大于0即可，车速与发动机转速相关，而加速度与发动机扭矩相关

纵向控制逻辑：油门 --> 功率 --> 转速 / 扭矩 --> 车速 / 车加速度 --> 车加速

当踩油门时，到底在控制什么？

P与M，ω之间的关系： $P=Fv = M\omega$

真实的发动机扭矩转速功率之间的关系需要用实验曲线来获得

二、发动机与变速器

发动机扭矩-转速曲线

并不呈反比例曲线，因为P一部分用于发热，低速情况热占主导，高速情况做功占主导。

所以发动机最好只在高效/高扭矩区运行，发挥其最大优势，但是其高效区很短

变速器应运而生，把发动机转速降低

变速器通过齿轮切换从而将发动机的高效转速得以拓宽

齿轮切换术语叫换挡：一档二档三档四档五档

一档最慢，扭矩最大，用于启动爬坡

五档最快，扭矩最小，用于高速

发动机与电机的转速扭矩图

电机在前面就有很大扭矩，最高转速16000，高效区间长，效率最高90%

发动机在前面运行，最高转速6500，高效区间很短，效率最高40%

因此发动机一定需要变速器，电机不一定需要变速器

发动机低速差，高速较强。电机在低速加速极强，高速差。

我们的控制基于电动汽车，电动车控制简单且为大势所趋，不需要考虑换挡策略

电机工作区间分为恒扭矩区和恒功率区，在到达最大转速前扭矩不变，到达之后功率不变

三、CarSim基础设置

由于CarSim没有电机，所以需要造个电机，下面为假设

电机参数：最大扭矩380Nm，最大功率180kW

估算出其最大转速

M与ω之间的关系为

虽然是假设的电机，但是方法是通用的，不管电机参数如何，都通用。

我们的方法是通过实验标定来的，哪怕曲线乱七八糟的也有效果

下面进入CarSim设置电机

先新建一个模型

设置输入为发动机扭矩

输出为Vx，Ax以及曲轴的转速

接下来send to simulink

设置电机模型

function torque = fcn(power,rpm)
Tmax = 380*power;
if(rpm <= 4523)
    torque = Tmax;
else
    torque = Tmax * 4523/rpm;
end
end

接下来进入CarSim的C-class

进入变速器设置

新建一个变速器模型

将传动比，惯量和效率都改成如下，和电机匹配

进入发动机配置，可以通过更改发动机曲线来模拟电机，也可以用Simulink

设置好后，一定要Send to simulink，要不然都白设置了

设置油门给1，然后速度和加速度都转换好

得到速度和加速度曲线，

最大速度超过45m/s，符合家用车最高车速

油门调成0.1就不会那么快了

油门调成0，仍然有速度，因为发动机有怠速

怠速设置在发动机曲线内，750rpm

以前的电动机无怠速，现在的电动机都给个怠速，因为如果没有怠速，电机加速度过大，启动速度过大会不平顺。我们这里改成0改成750都行，选择不改。

下面是路面修改，不需要之前的弯道，只需要一个2000m的道路即可（注意这里把loop取消勾，然后把前面path终点也改成2000，否则后面simulink报错）

最后再把Simulink的文件名改一下

OK，至此准备工作完毕，具体的仿真后续第十讲会做

第十讲油门刹车标定表的制作

一、理论部分

找到油门和v，a对应的关系

做实验，踩不同的油门，得到不同的v，a曲线

对于一个thr，不同的时间t得到一系列va点，v，a可以合并，得到v，a曲线

不同的thr会得到不同的va曲线，

使用不同的thr做实验，可以得到一个三维曲面

通过做实验，得到大量的（v，a，thr）的三维点，从而拟合出thr = f(v,a)

f(v,a)叫做模型（深度学习术语）

标定表的制作

二、油门标定表制作

来到CarSim把刹车制动压力加上去

来到Simulink，首先给mux加一个输入（刹车，这里设为0），然后函数输入的油门用一个变量来表示，使用to workspace创建两个变量vx和ax

准备就绪后，就开始写标定算法，回到matlab创建一个文件

这个时候vx和ax就传进matlab里面了，simulink里面也能看到

这时候发现采样太密了，改成0.1秒采样，注意这里要点apply，否则matlab报错

接下来开始画表

在matlab中使用以下代码，


thr = 0;  %初始化油门
for i = 1:11
    sim('calibration');
    v_temp(:,i) = vx.data;
    a_temp(:,i) = ax.data;
    thr_temp(:,i) = ones(length(vx.data),1)*thr;
    thr = thr + 0.1;
end

注意，这代码运行时间比较长，而且如果设置的时间过长的话，车速很快时，会率先到达2000m停下来，此时矩阵维度不一致会报错，所以将仿真时间截至调到30s以内

跑完之后合并起来

% 合并，一定要转成行向量再合并，否则会导致合并失败
v = v_temp(:,1)';
a = a_temp(:,1)';
tr = thr_temp(:,1)';
for i = 2:11
    v = [v, v_temp(:,i)'];
    a = [a, a_temp(:,i)'];
    tr = [tr, thr_temp(:,i)'];
end

然后再拟合

%拟合
F = scatteredInterpolant(v',a',tr');% 转成列向量
vu = 0:0.1:50;
au = 0:0.1:5;
table = zeros(length(vu),length(au));
for i = 1:length(vu)
    for j = 1:length(au)
        table(i,j) = F(vu(i),au(j));
    end
end

运行之后得到table

接下来回到Simulink，使用2D lookup table拟合

然后改成au，vu和table，

接下来点击Edit table and breakpoints查看拟合图像

在Simulink内连接好速度和加速度，给1的加速度，点击run，得到其实际加速度也差不多为1

改成3后，发现刚开始还可以正常跑，后面就不对了，因为油门超过1了，实际上油门最大时1

这个table是用不超过1的油门来拟合的，所以需要加限制，在加一个延迟模块防止代数环

下面是速度控制，控制在10，注意这里加速度输入为车速与控制速度相减

如果直接用车的加速度输入的话，结果不准确。

原因，速度与加速度不匹配，要让加速度慢慢降低至0。上面使用目标速度与实际速度相减的信号作为输入，这样会让加速度慢慢降到0，匹配上了，控制效果就会好很多。

无论是速度控制还是加速度控制，都要匹配，不能乱写。

三、刹车标定表制作

刹车标定和油门差不多

首先进入Simulink，油门设置为0，刹车设置一个变量

初速度设置为最快的速度，这里设置为180

send to simulink，开始写代码（油门和刹车的差不多），这里设置为0-8s的数据

brake = 0;  %初始化油门
for i = 1:80
    % 该程序非常耗时，如果需要更多更密集的数据，请先测试
    sim('calibration');
    v_temp(:,i) = vx.data;
    a_temp(:,i) = ax.data;
    brake_temp(:,i) = ones(length(vx.data),1)*brake;
    brake = brake - 0.1;
end

合并，80个数据进行合并

% 合并，一定要转成行向量再合并，否则会导致合并失败
vbr = v_temp(:,1)';
abr = a_temp(:,1)';
br = brake_temp(:,1)';
for i = 2:80
    vbr = [vbr, v_temp(:,i)'];
    abr = [abr, a_temp(:,i)'];
    br = [br, brake_temp(:,i)'];
end

拟合，这里注意加速度为负的（刹车），v和a都为单调递增的，否则不识别

%拟合
F = scatteredInterpolant(vbr',abr',br');% 转成列向量
vubr = 0:0.05:50;
aubr = -8:0.05:0;  %注意这里加速度和之前不一样，为负的
tablebr = zeros(length(vubr),length(aubr));
for i = 1:length(vubr)
    for j = 1:length(aubr)
        tablebr(i,j) = F(vubr(i),aubr(j));
    end
end

点击运行，程序很耗时，跑完得到tablebr ，竖着为速度0-50，横着为加速度-8到0

最后一列的数值突然变得很大，对应的是v和a=0的情况，停车状态，这个时候任意制动都会导致a=0或者v=0，这个点是所有曲线的交点，有奇异性，matlab取了个平均值。这里改一下，改成0.3，假设仅有手刹。（注：这里图有点问题，因为代码写的有点错误，实际161列第二行开始应该为0.3左右，把第一行也改成0.3即可）

再设置一个lookup2D，把我们的数据输入进去

设置刹车加速度为-3，看看效果

效果不错，以-3加速度制动，然后停车后加速度为0。

以目标速度为0，然后当前速度与目标速度相减输入给加速度，这样也可以

注意加一个制动压力限制，最大9最小0MPa

最后速度与加速度都收敛至0

可以返回CarSim查看车辆运行状态

第十一讲纵向双PID控制

一、前置准备

首先，这里开始使用CarSim 2019版本，纵向控制没法用2016版本。

变速器调整：换成一个挡位，然后换挡时间调短

初速度为0，仿真时间调长一点，这里设置为40S

这里不要忘记吧刹车和转向都关掉，这些都由我们自己在simulink自己设置，

然后把道路改成Double Lane，随便设置一个长度，这里设置成5000m

接下来Send to Simulink

首先要做的是把油门和刹车做到一张表上，会自动插值，这样会更加平顺

设置一个变量x为油门刹车，x大于0为油门，小于0为刹车，使用一个函数用来判断

function [thr,brake] = fcn(x)
% 正数代表油门，负数代表刹车
%不允许同时踩油门和刹车
if x  >= 0
    thr =x;
    brake = 0;
else
    thr = 0;
    brake = -x;
end
end

接下来是标定代码（油门和刹车分开写，因为初速度不一样）

thr_calibration.m 油门采集数据代码

x = 0;  %初始化油门
for i = 1:21
    sim('calibration');
    v_temp(:,i) = vx.data;
    a_temp(:,i) = ax.data;
    thr_temp(:,i) = ones(length(vx.data),1)*x;
    x = x + 0.1;
end

% 合并，一定要转成行向量再合并，否则会导致合并失败
v = v_temp(:,1)';
a = a_temp(:,1)';
tr = thr_temp(:,1)';
for i = 2:length(v_temp(1,:))
    v = [v, v_temp(:,i)'];
    a = [a, a_temp(:,i)'];
    tr = [tr, thr_temp(:,i)'];
end

brake_calibration.m 刹车采集数据代码

% 启动前检查CarSim的初速度是否为180
x = 0;  %初始化刹车
%%% 刹车的初速度一定要比较高，180km/h，144km/h
for i = 1:81
    % 该程序非常耗时，如果需要更多更密集的数据，请先测试
    sim('calibration');
    v_temp1(:,i) = vx.data;
    a_temp1(:,i) = ax.data;
    brake_temp1(:,i) = ones(length(vx.data),1)*x;
    %%% 这里是为了消除奇异性，因为无论brake=1还是2，最后都会导致车的v=0，a=0，这将导致多值性
    for j = 1:length(v_temp1(:,i))
        if v_temp1(j,i)<0.01
            brake_temp1(j,i) = 0;
        end
    end
    x = x - 0.1;
end

% 合并，一定要转成行向量再合并，否则会导致合并失败
vbr = v_temp1(:,1)';
abr = a_temp1(:,1)';
br = brake_temp1(:,1)';
for i = 2:80
    vbr = [vbr, v_temp1(:,i)'];
    abr = [abr, a_temp1(:,i)'];
    br = [br, brake_temp1(:,i)'];
end

二、油门和刹车标定表制作

首先回到CarSim把初速度改成0，然后运行油门代码获得油门标定表

然后再回到CarSim把初速度改成180，然后运行刹车代码，获得刹车标定表

刹车标定表第一行全部都是-7.032，由于初始化问题，我们把第二行数据赋值给第一行

加上代码

a_temp1(1,:) = a_temp1(2,:);

把上面Sim部分注释掉，然后重跑一下

代码generate_callibration.m（把油门和刹车都合并起来）

v2 = [v,vbr];
a2 = [a,abr];
br2 = [tr,br];

%拟合
F = scatteredInterpolant(v2',a2',br2');% 转成列向量
vubr = 0:0.05:50;
aubr = -8:0.05:5;  %注意这里加速度为-8到正5
tablebr = zeros(length(vubr),length(aubr));
for i = 1:length(vubr)
    for j = 1:length(aubr)
        tablebr(i,j) = F(vubr(i),aubr(j));
    end
end

在Simulink把这个表使用lookup2D设置好

把表，转换模块，电机模型整合一下。输入输出变量名设置好

把CarSim初速度改成0，然后Send一下

三、纵向速度PID控制仿真

假设我们要控制加速度为10，使用简单的pid控制：期望速度-当前速度作为加速度进行输入

仿真查看v运行结果，发现没反应，因为我们加速度没有给限制，表格内没有10这么大的加速度

加一个限制

这个时候在运行，查看v确实有这个控制效果

仔细看发现并没有到10，因为接近10的时候加速度平缓导致不加速了

改进办法：加一个比例项，PID的P就是比例，I是积分，D是微分

可以看到，收敛速度更快，结果也好一些。P可以加快收敛

比例项也不是越大越好，这里调到10，发现出现超调。

把比例改成3，然后看下180减速到10会怎么样，可以看到发生震荡

改成pid，这里给0.1积分，发现会出现超调，但是最后还是会收敛到10

I主要目的：消除稳态误差，但是可能会引起超调

I改成1后，发现超调严重，但是仍然能回到10，因为I存在就会对误差消除

现在吧积分去掉，然后加一点微分D，微分的作用是抑制超调

这里放大并与上面不加微分的对比，发现震荡和超调被抑制

所以如果误差不是特别大的情况，一般用PD即可，因为I会引起超调。

下面设计一段真实的轨迹，在Simulink中新建一个函数

function y = fcn(t)
if t < 10
    s = 0.1* t^3/3;
    v = 0.1*t^2;
    a = 0.2*t;
else
    a = 2 - 0.1*(t-10);
    v = 2*t - 0.05*(t-10)^2-10;
    s = t^2 - 0.05*(t-10)^3/3-10*t+100/3;
end
end

检测一下是否连续

速度一直在增加，所以仿真时间要加长，这里设置到60

此外要把车的坐标输出出来

接下来回到Simulink，速度和位置都需要满足约束，所以把速度和位置的误差都输入进去，位置PID限制±10，此外加速度信号也连上，这样双PID的纵向控制就搭建完了。

注意这里的PID参数为比例6，积分0，微分0.1

查看结果，位置还可以

速度和加速度就不太行

可能是初速度的缘故，现在把初速度调成0，看下结果

可以看到位置还可以

加速度不咋样

速度有点“画龙”，一会儿大一会儿小

下面把速度PID的比例项调小点，调成2

速度“画龙”的现象得到改善

加速度也比较好，后面的波动为停车后的

但是加速度前面还不太好，需要调参，直接给出参数

速度PID这样调

位置PID这样调

速度和加速度结果如图

最后打包一下

第十二讲横纵向综合控制完结

1-2 开篇，运动学方程

3-8 横向控制 LQR

9-11 纵向控制双PID

12 横纵向控制，规划接口

运动学方程： $tan\delta = L/R$ ，适用于低速情况，转角大小均可

LQR适用于小转角，低速高速均可以

所以一般在自动泊车等低速大转角的场景用运动学方程控制

一、规划接口

控制模块功能：接受一条规划的轨迹，让车按照规划的轨迹运动。首先得有规划，然后在进行横纵向控制。

注意这里是轨迹规划，不是路径规划，在自动驾驶里面，路径只会告诉该怎么走，与时间无关，而轨迹规划包含时间，速度，加速度，曲率等信息。

轨迹规划：x(t)，y(t)。（先做直角坐标系下的，由简单开始）

假设有下面的坐标系，起点在原点，终点在（100，10）处

那么有停车场景

或者驶入场景（终点有速度20）

或者超车场景，起点速度为10，终点速度为20

今天就研究给定起点终点速度，加速度信息的规划。（最简单，最常用，覆盖大部分直线场景，但是不能做转弯和掉头，转弯掉头必须在自然坐标系下）。

这个地方的规划主要是为了控制服务。

规划任务可以描述为一个数学问题：设计一条合适的x(t），y(t)，满足始末的边界条件：

其中，T为耗费的时间

机器人领域有这种算法，但是无人驾驶不能直接照搬，因为车不能单独做横向运动，横向运动通常由纵向运动诱发

规划的轨迹有切线，曲率，加速度，速度的限制

如果在起点建立一个直角坐标系，那么对 $\frac{dy}{dx}$ 也有要求

汽车规划的边界条件为

机器人的x与y都与时间相关，而无人车不能做纵向运动，所以y和x有关

仅仅用y'做规划还不够，y与x相关，要把它弄成与时间相关

所以还需要y(t)与y(x)的转化

求y'和y''的时候使用复合求导

边界条件为

先算出y(x)再转化为y(t)

对于x(t)使用五次多项式

五个系数正好对应5个边界条件

对于y(t)也使用五次多项式

因为y也有五个边界条件

通过边界条件求出多项式系数a0-5，b0-5

通过解出y(t)，y'，y''

横向控制与规划的接口

第八讲横向控制中有 $x_r$ ， $y_r$ ， $\theta_r$ ， $k_r$

其中xr,yr代表匹配点， $\theta_r$ 代表轨迹切线与x轴的夹角， $k_r$ 代表轨迹曲率

xr,yr就是规划器中的x(t)，y(t)。

$\theta_r$ ， $k_r$ 可用下式算出

纵向控制与规划的接口

$e_s$ In the horizontal control, es is the projection of the red vector on the blue vector in the figure below

Vertical error: the error between the target point and the current point. In this figure, es should be positive as an error, so a negative sign should be added to the es input of the horizontal control to the vertical

Speed error: $v_p - \dot{s}$ , $\dot{s}$ in lateral control with

desired acceleration

Logical combing

First obtain x(t), y(t) according to the plan

According to x(t), y(t) we can get

According to these we can get

The first two of the vertical control are obtained from the horizontal control, and the actual input is only the last three

2. Code practice part

The first is the horizontal control and vertical control code and model files

Copy the code and model of ch8 and ch11 to the CarSim directory

Then open Matlab

Note that the cf and cr in lqr need to be changed, because a more accurate model is needed, and they cannot be mixed like in the eighth lecture.

It is necessary to calculate the precise vertical force of each wheel, and then roughly estimate the cornering stiffness through the vertical force look-up table

Don't forget to add this sentence

Reset a new model planning_control

INPUT setting

Note that it must be adjusted to replace

OUTPUT setting

Create a new Simulink model and then bind this model to Carsim

The simulation time is changed to 40S, the initial speed is 0, and the brake and steering are not enabled.

Copy the vertical control model into

Calibrate below, here change the input and output to the input and output of the previous lecture,

Uncomment, and then repeat the content of the previous lecture to make a table

If you are too lazy to do it again, here is the direct link

Accelerator and Brake Calibration Table.zip - Lanzuoyun

Load the mat file in matlab

Next, build the vertical double PID, then change the output to 7, delete the calibrated things, and then mark the actual output

Next, change the planning module in the lower left corner and change the output

Next, change the input of the position PID to es, and then add the output to the unit conversion ( note that yaw is pi/180, and the error was discovered later ), and some adjustments are made

Next, merge all the modules as shown in the figure.

The speed should be regarded as the motor input, so write a speed function below

function v = fcn(vx,vy)
    v = sqrt(vx^2 + vy^2);
end

Then we rewrite the plan

function [vp,ap,xr,yr,thetar,kr] = fcn(t)
% 在20s，向前移动100米，向左移动10m
dx = 100;
dy = 10;
T = 20;
%起点终点速度和加速度
xstart = [0,0,0];
xend = [dx,0,0];
ystart = [0,0,0];
yend = [dy,0,0];
% 设置多项式系数向量
a = zeros(1,6);
b = zeros(1,6);
%解x的系数
a(1) = xstart(1);
a(2) = xstart(2);
a(3) = xstart(3)/2;
A1 = [T^3,       T^4,      T^5;
          3*T^2,   4*T^3,   5*T^4;
          6*T,       12*T^2,  20*T^3];
B1 = [xend(1)-a(1)-a(2)*T-a(3)*T^2;
          xend(2)-a(2)-2*a(3)*T;
          xend(3)-2*a(3)];
xs = inv(A1)*B1;
a(4) = xs(1);
a(5) = xs(2);
a(6) = xs(3);
%解y的系数
b(1) = ystart(1);
b(2) = ystart(2);
b(3) = ystart(3)/2;
A2 = [dx^3,       dx^4,      dx^5;
          3*dx^2,   4*dx^3,   5*dx^4;
          6*dx,       12*dx^2,  20*dx^3];
B2 = [yend(1)-b(1)-b(2)*dx-b(3)*dx^2;
          yend(2)-b(2)-2*b(3)*dx;
          yend(3)-2*b(2)];
ys = inv(A2)*B2;
b(4) = ys(1);
b(5) = ys(2);
b(6) = ys(3);
% 求x点，x两点，y‘，y’‘以及曲率，vp，ap
xr = a(1) + a(2)*t + a(3)*t^2 + a(4)*t^3 + a(5) *t^4+a(6)*t^5;
yr = b(1) + b(2)*xr + b(3)*xr^2 + b(4)*xr^3 + ab(5) *xr^4+b(6)*xr^5;
xr_dot = a(2) + 2*a(3)*t + 3*a(4)*t^2 + 4*a(5) *t^3+5*a(6)*t^4;
yr_dx =  b(2) + 2*b(3)*xr + 3*b(4)*xr^2 + 4*ab(5) *xr^3+5*b(6)*xr^4;
yr_dot = yr_dx *xr_dot;
thetar = atan(yr_dx);
xr_dot2 = 2*a(3)+6*a(4)+ 12*a(5) *t^2+20*a(6)*t^3;
yr_dx2 = 2*b(3)+6*b(4)+ 12*b(5) *xr^2+20*b(6)*xr^3;
yr_dot2 = yr_dx2*xr_dot^2 + yr_dx*xr_dot2;
kr = yr_dx2/((1 + yr_dx^2)^1.5);
vp = sqrt(xr_dot^2 + yr_dot^2);
ap = sqrt(xr_dot2^2 + yr_dot2^2);
end

Reconnect the planned lines

Next add the horizontal controls

Model with open lateral controls

Modify the main module as follows

去掉单位换算，然后把xr，yr，thetar，kappar都设置为外部输入

接下来更改误差计算模块，增加输出es和s_dot，然后去掉寻找dmin的过程，因为我们这里的规划不是之前那样给好轨迹

function [kr,err,es,s_dot] = fcn(x,y,phi,vx,vy,phi_dot,xr,yr,thetar,kappar)
    tor=[cos(thetar);sin(thetar)];
    nor=[-sin(thetar);cos(thetar)];
    d_err=[x-xr;y-yr];
    ed=nor'*d_err;
    es=tor'*d_err;
    %projection_point_thetar=thetar(dmin);%apollo
    projection_point_thetar=thetar+kappar*es;
    ed_dot=vy*cos(phi-projection_point_thetar)+vx*sin(phi-projection_point_thetar);
    %%%%%%%%%
    ephi=sin(phi-projection_point_thetar);
    %%%%%%%%%
    ss_dot=vx*cos(phi-projection_point_thetar)-vy*sin(phi-projection_point_thetar);
    s_dot=ss_dot/(1-kappar*ed);
    ephi_dot=phi_dot-kappar*s_dot;
    kr=kappar;
    err=[ed;ed_dot;ephi;ephi_dot];
end

接下来退出来，先把该连的连起来，然后把输入改成6个，增加前后轮转角，后轮转角设置为0，然后给angle加上单位换算和±1的限制，给es乘以-1输入到纵向控制

接下来到纵向控制内，给es增加限制±10，然后PID参数改成如下

接下来增加2个示波器，比较一下实际x,y和规划的xr,yr有多大误差

进入matlab运行lqr_offline获得k

接着运行Simulink

报错，原因：仿真时间过长

Index exceeds array dimensions. Index value 5000 exceeds valid range [1-4999] of array k1. Error in 'planning_control/Subsystem1/lqr_offline' (line 6) k=[k1(index),k2(index),k3(index),k4(index)];

调短一点再run，然后查看scope，发现x，y基本上重合

放大看还是有0.几的误差，仿真中对误差的要求是0.0几m以内才能保证实车上面有误差和噪声的条件下精确

原因分析：在planning模块中，ap可正可负

所以将其中的

ap=sqrt(xr_dot2^2+yr_dot2^2);

改为ap根据xr_dot2的正负改变

if xr_dot2>=0
    ap=sqrt(xr_dot2^2+yr_dot2^2);
else
    ap=-sqrt(xr_dot2^2+yr_dot2^2);
end

调好后再run一下发现横向误差还是大，纵向误差可以接受

发现是angle这里应该先限制再单位转换

为了能直观看到横向误差，我们在LQR模块把ed输出出来

差不多ed是0.04左右

去规划中把速度调快，10s内完成

发现高速下性能不怎么样

原因：加速度ap超了

下面试一下低速大转角

回到CarSim把仿真时间改到80

再回到simulink加一个scope查看速度

表现得很好

横向纵向误差均在0.05以内

lqr需要在小转角，加速度不能超了，一般在2-3以内

一般横向误差控制的不好，原因有三

一是侧偏刚度自己估的，不太准，前轮后轮垂向力不一样，因为加速度导致轴荷转移，垂向力不一样导致侧偏刚度变化

二是因为LQR基于二自由度模型，自行车模型，本来就有简化性，本来就有误差

三是汽车固有特性：转向不足

一般来说仿真达到厘米级可以接受，实车需要达到0.1m可以接受，因为实车有误差和噪声

如果横向误差过大，需要给Q更大的惩罚值，即给ed更大的惩罚值，尽可能让ed收敛到0，而e_phi不可能收敛到0，因为e_phi的稳态误差就是beta

一般来说改三个，一个是转向不足，二一个是LQR的侧偏刚度，三一个是调LQR的Q

三、转向不足

最后讲一下转向不足导致的横向误差过大问题

转向不足和过度转向都是实车会发生的情况，即实际转角小于或者大于理论转角

为什么会发生转向不足\过度呢

如果前后轮侧向力不匹配，质心处会存在力矩

如果 $F_1\cdot a = F_2\cdot b$ ，质心无力矩，导致中性转向

如果 $F_1\cdot a > F_2\cdot b$ ，质心有正力矩，导致过度转向

如果 $F_1\cdot a < F_2\cdot b$ ，质心有负力矩，导致不足转向

一般市面上买到的车都是转向不足，为了安全考虑，一般赛车调校成中性转向，因为赛车天生高速下有过度转向的趋势

那么如何处理呢？

方法：使用PID，给误差做个积分，再补偿到前轮转角上去

这里PID给I设置为3，其他均0。此外把单位转换放到后面去，先做减法再换算。

因为ed为正说明方向盘打多了，就给它减掉，

下面把规划出的横向位置调的严苛一点

然后仿真30秒查看结果

效果更好一点

误差更小一些，横向误差控制在了0.02以内

查看ed ，ed已经在0.01以内了

这里因为模型本来比较准，所以提升不大，但是对于实车而言提升比较大

到这里本课程基本结束

结论：matlab速度太慢了，而且这里只有纯控制无规划，加上规划，决策等模块更慢，所以实车还是需要C++和linux，因为速度会更快

[Automatic driving] [Zero foundation] Basic automatic driving control algorithm notes

Lecture 1 Introduction

第二讲 三个坐标系与运动学方程

一、控制原理

二、三个坐标系

三、左手系与右手系

四、自行车模型的运动学方程

Lecture 3 Tire cornering and vehicle dynamics equations

1. Decoupling of the dynamic equation and the Frenet coordinate system

2. The cornering characteristics of the tire

3. Bicycle model considering tire cornering characteristics

第四讲 坐标变换与横向误差微分方程

Lecture 5 Discretization of continuous equations and principle of discrete LQR

1. Discretization model

Second, solve lqr

三、黎卡缇方程

四、总结

第六讲 前馈控制与航向误差

一、前馈控制

第七讲 离散规划轨迹的误差计算

一、离散的轨迹点的误差计算

第八讲

一、横向控制算法与流程图

（1）算法输入输出过程

（2）流程图

（3）具体算法

（4）预测模块

二、CarSim与Matlab联合仿真基础

（1）CarSim于Simulink基础设置

（2）输入输出设置

三、代码与模型

（1）路径规划信息生成

（2）预测模块代码

（3）误差计算模块代码

（4）LQR模块代码

（5）前馈控制模块代码

（6）整合模块代码

（7）仿真运行

第九讲 纵向控制开篇

一、纵向控制逻辑

二、发动机与变速器

三、CarSim基础设置

第十讲 油门刹车标定表的制作

一、理论部分

二、油门标定表制作

三、刹车标定表制作

第十一讲 纵向双PID控制

一、前置准备

二、油门和刹车标定表制作

三、纵向速度PID控制仿真

第十二讲 横纵向综合控制完结

一、规划接口

2. Code practice part

三、转向不足

Guess you like

第二讲三个坐标系与运动学方程

第四讲坐标变换与横向误差微分方程

第六讲前馈控制与航向误差

第七讲离散规划轨迹的误差计算

第九讲纵向控制开篇

第十讲油门刹车标定表的制作

第十一讲纵向双PID控制

第十二讲横纵向综合控制完结