admin管理员组文章数量:1597416
- Vehicle Model Predictive Controller
- Model Predictive Control
- Why MPC?
- Structure of a simple case
- Vehicle Motion Control
- Objective/Cost function
- Inequality constraints and slack
- Model
- Vehicle Model
- Code frame
- MPC控制器代码工作流程
- FG_eval 类
- mpc_controller 类
- ROS Node Configuration
代码具体实现参考： link
Vehicle Model Predictive Controller
Model Predictive Control
模型预测控制（Model Predictive Control, MPC）是基于对未来行为预测和当前状态优化的控制策略，具备处理多变量、具有约束的系统的能力，尤其适用于动态复杂和不确定性高的系统。
Why MPC?
Objective:
• Minimize lap time
Constraints:
• Avoid other cars
• Stay on road
• Don’t skid
• Limited acceleration
Problem:
What to do if something unexpected happens?
• We didn’t see a car around the corner!
• Must introduce feedback
Process:
• Obtain series of planned control actions
• Apply first control action
• Repeat the planning procedure
Structure of a simple case
1. Build Model
x
k
+
1
=
f
(
x
k
,
u
k
)
y
k
=
h
(
x
k
,
u
k
)
x_{k+1} = f(x_k, u_k) \\ y_k = h(x_k, u_k)
xk+1=f(xk,uk)yk=h(xk,uk)
2. Predict
The predicted output ( y_p ):
{ y p ∣ k + 1 ∣ k , y p ∣ k + 2 ∣ k , … , y p ∣ k + p ∣ k } \{y_{p|k+1|k}, y_{p|k+2|k}, \dots, y_{p|k+p|k}\} {yp∣k+1∣k,yp∣k+2∣k,…,yp∣k+p∣k}
3. Optimal
The optimal input ( U_k ):
U
k
≜
{
u
k
∣
k
,
u
k
+
1
∣
k
,
…
,
u
k
+
p
−
1
∣
k
}
U_k \triangleq \{u_{k|k}, u_{k+1|k}, \dots, u_{k+p-1|k}\}
Uk≜{uk∣k,uk+1∣k,…,uk+p−1∣k}
The reference ( r_k ):
{
r
k
+
1
,
r
k
+
2
,
…
,
r
k
+
p
}
\{r_{k+1}, r_{k+2}, \dots, r_{k+p}\}
{rk+1,rk+2,…,rk+p}
At time k, we have obtained x_k, y_k, but we need to calculate u_k from these values, so u will be updated at the next time.
The cost function:
J
(
y
k
,
U
k
)
=
∑
i
=
k
+
1
k
+
p
(
r
i
−
y
p
∣
i
∣
k
)
2
J(y_k, U_k) = \sum_{i=k+1}^{k+p} (r_i - y_{p|i|k})^2
J(yk,Uk)=i=k+1∑k+p(ri−yp∣i∣k)2
Optimal problem can be described as
min
U
k
J
(
y
k
,
U
)
\min_{U_k} J(y_k, U)
UkminJ(yk,U)
s.t. x k + 1 = f ( x k , u k ) y k = h ( x k , u k ) \begin{aligned} \text{s.t.} \quad & x_{k+1} = f(x_k, u_k) \\ & y_k = h(x_k, u_k) \end{aligned} s.t.xk+1=f(xk,uk)yk=h(xk,uk)
-
Add Contrains
u min ≤ u k + i ∣ k ≤ u max , i = 0 , 1 , … , p − 1 y min ≤ y k + i ∣ k ≤ y max , i = 0 , 1 , … , p . \begin{aligned} & u_{\text{min}} \leq u_{k+i|k} \leq u_{\text{max}}, i = 0,1, \dots, p - 1 \\ & y_{\text{min}} \leq y_{k+i|k} \leq y_{\text{max}}, i = 0,1, \dots, p. \end{aligned} umin≤uk+i∣k≤umax,i=0,1,…,p−1ymin≤yk+i∣k≤ymax,i=0,1,…,p. -
Feedback and Update
U k ∗ ≜ { u k ∣ k ∗ , u k + 1 ∣ k ∗ , … , u k + p − 1 ∣ k ∗ } U k ∗ = arg min U k J ( y k , U k ) \begin{aligned} & U^*_k \triangleq \{ u^*_{k|k}, u^*_{k+1|k}, \dots, u^*_{k+p-1|k} \} \\ & U^*_k = \text{arg min}_{U_k} J(y_k, U_k) \end{aligned} Uk∗≜{uk∣k∗,uk+1∣k∗,…,uk+p−1∣k∗}Uk∗=arg minUkJ(yk,Uk)
only use the first one
Vehicle Motion Control
Objective/Cost function
The objective function is our main mechanism to shape driving behavior.
The objective function has different types of components:
- Tracking objectives (Reference from planner)
- Comfort objectives (Comfortability of vehicle motion, perceived danger)
- Safety objectives (Constraint violation)
Different weights can be applied to separate cost terms in order to favor a certain trade-off.
J = w t ⋅ J tracking + w c ⋅ J comfort + w s ⋅ J safety J = w_t \cdot J_{\text{tracking}} + w_c \cdot J_{\text{comfort}} + w_s \cdot J_{\text{safety}} J=wt⋅Jtracking+wc⋅Jcomfort+ws⋅Jsafety
where
- ( w t w_t wt ) is the weight for tracking cost,
- ( w c w_c wc ) is the weight for comfort cost,
- ( w s w_s ws ) is the weight for safety cost.
a. Tracking objective function:
- follow the planner path
- follow the “speed profile”
J tracking = w ˙ s ⋅ ( s ˙ − s ˙ ref ) 2 + w n ⋅ n 2 + w μ ⋅ ( μ + β ) 2 J_{\text{tracking}} = \dot{w}_s \cdot (\dot{s} - \dot{s}_{\text{ref}})^2 + w_n \cdot n^2 + w_{\mu} \cdot (\mu + \beta)^2 Jtracking=w˙s⋅(s˙−s˙ref)2+wn⋅n2+wμ⋅(μ+β)2
where
- ( w ˙ s \dot{w}_s w˙s ) is the weight for minimizing the longitudinal speed tracking error,
- ( w n w_n wn ) is the weight for minimizing the lateral error,
- ( w μ w_{\mu} wμ ) is the weight for minimizing the heading error.
- β + μ = δ \beta + \mu = \delta β+μ=δ
b. Comfort Objective/Cost function
The comfort objective function:
J comfort = w j x ⋅ j x 2 + w a x ⋅ a x 2 + w a y ⋅ a y 2 + w δ ˙ ⋅ δ ˙ 2 + w δ ¨ ⋅ δ ¨ 2 J_{\text{comfort}} = w_{jx} \cdot {j}_x^2 + w_{ax} \cdot a_x^2 + w_{ay} \cdot a_y^2 + w_{\dot{\delta}} \cdot \dot{\delta}^2 + w_{\ddot{\delta}} \cdot \ddot{\delta}^2 Jcomfort=wjx⋅jx2+wax⋅ax2+way⋅ay2+wδ˙⋅δ˙2+wδ¨⋅δ¨2
Where (
w
j
x
w_{jx}
wjx ) is the weight for minimizing the longitudinal jerk,
(
w
a
x
w_{ax}
wax ) is the weight for minimizing the longitudinal acceleration,
(
w
a
y
w_{ay}
way) is the weight for minimizing the lateral acceleration,
(
w
δ
˙
w_{\dot{\delta}}
wδ˙ ) is the weight for minimizing the steering rate,
(
w
δ
¨
w_{\ddot{\delta}}
wδ¨ ) is the weight for minimizing the steering acceleration.
c. Safety Objective/Cost function
-
Input constraints are dictated by physical constraints on the actuators and are usually “hard”.
-
State constraints are softened.
-
Hard state/output constraints always lead to complications in the controller implementation,
- Feasible operating regime is constrained even for stable systems
- Controller patches must be implemented to generate reasonable control action when measured/estimated states move outside feasible range because of disturbances or noise.
-
Slack variables ( λ k \lambda_k λk) are, just like states and inputs, free variables the optimizer can alter and soften the constraint.
-
Violating constraints is not cheap and should be penalized accordingly.
“Softened” problem:
min U = { u k ∣ k , u k + 1 ∣ k , . . . } J ( x k , U ) = ∑ k = 0 N − 1 J stage ( x k , u k , λ k ) \begin{aligned} & \min_{U=\{u_{k|k},u_{k+1|k},... \}} J(x_k, U) = \sum_{k=0}^{N-1} J_{\text{stage}}(x_k, u_k, \lambda_k) \end{aligned} U={uk∣k,uk+1∣k,...}minJ(xk,U)=k=0∑N−1Jstage(xk,uk,λk)
s.t. x k + 1 = f ( x k , u k ) y k = h ( x k , u k ) c k ( x k , u k , λ k ) ≤ 0 , x ∈ X , u ∈ U , λ ∈ Λ . \begin{aligned} \text{s.t.} \quad & x_{k+1} = f(x_k, u_k) \\ & y_k = h(x_k, u_k) \\ & c_k(x_k, u_k, \lambda_k) \leq 0, \\ & x \in \mathcal{X}, \\ & u \in \mathcal{U}, \\ & \lambda \in \Lambda. \end{aligned} s.t.xk+1=f(xk,uk)yk=h(xk,uk)ck(xk,uk,λk)≤0,x∈X,u∈U,λ∈Λ.
The safety objective function:
Can use the constraint violation => slack to estimate
J safety = λ soft T E λ soft + H ˉ λ hard J_{\text{safety}} = \lambda_{\text{soft}}^T E \lambda_{\text{soft}} + \bar{H}\lambda_{\text{hard}} Jsafety=λsoftTEλsoft+Hˉλhard
with E = diag ( w n , soft , w v , soft , w a , soft ) E = \text{diag}(w_{n,\text{soft}}, w_{v,\text{soft}}, w_{a,\text{soft}}) E=diag(wn,soft,wv,soft,wa,soft), H ˉ = [ w n , w a , w s ] \bar{H} = [w_{n}, w_{a}, w_{s}] Hˉ=[wn,wa,ws].
λ
soft
=
[
λ
n
,
soft
λ
v
,
soft
λ
a
,
soft
]
Lateral error violation
Speed violation
Acceleration violation
\begin{aligned} \lambda_{\text{soft}} = \begin{bmatrix} \lambda_{n,\text{soft}} \\ \lambda_{v,\text{soft}} \\ \lambda_{a,\text{soft}} \end{bmatrix} & \begin{array}{l} \text{Lateral error violation} \\ \text{Speed violation} \\ \text{Acceleration violation} \end{array} \end{aligned}
λsoft=
which is represents a tolerable range.
λ
hard
=
[
λ
n
λ
a
λ
s
]
it can represent the distance of the front of object.
\begin{aligned} \lambda_{\text{hard}} = \begin{bmatrix} \lambda_n \\ \lambda_a \\ \lambda_s \end{bmatrix} & \begin{array}{l} \\ \\ \text{it can represent the distance of the front of object.} \end{array} \end{aligned}
λhard=
which is represent a unsurpassable range.
Inequality constraints and slack
General nonlinear inequality constraints
c station ( x , λ s ) ≤ 0 Collision constraint derived from hard upper speed bound. c v el ( x , λ v soft ) ≤ 0 Speed constraint derived from soft upper speed bound. c tube,hard ( x , λ n ) ≤ 0 Spatial soft/hard tube constraints imposed on car footprint. c tube,soft ( x , λ n , soft ) ≤ 0 Spatial soft/hard tube constraints imposed on car footprint. c a ,hard ( x , λ a ) ≤ 0 Combined acceleration constraint on hard bounds. c a ,soft ( x , λ a , soft ) ≤ 0 Acceleration constraint on comfort bounds. c δ ˙ ( x ) ≤ 0 Speed dependent steering rate constraint. \begin{array}{ll} c^{\text{station}}(x, \lambda_s) \leq 0 & \text{Collision constraint derived from hard upper speed bound.} \\ c^{v_{\text{el}}}(x, \lambda_{v_{\text{soft}}}) \leq 0 & \text{Speed constraint derived from soft upper speed bound.} \\ c^{\text{tube,hard}}(x, \lambda_{n}) \leq 0 & \text{Spatial soft/hard tube constraints imposed on car footprint.} \\ c^{\text{tube,soft}}(x, \lambda_{n,\text{soft}}) \leq 0 & \text{Spatial soft/hard tube constraints imposed on car footprint.} \\ c^{a_{\text{,hard}}}(x, \lambda_a) \leq 0 & \text{Combined acceleration constraint on hard bounds.} \\ c^{a_{\text{,soft}}}(x, \lambda_{a,\text{soft}}) \leq 0 & \text{Acceleration constraint on comfort bounds.} \\ c^{\dot{\delta}}(x) \leq 0 & \text{Speed dependent steering rate constraint.} \end{array} cstation(x,λs)≤0cvel(x,λvsoft)≤0ctube,hard(x,λn)≤0ctube,soft(x,λn,soft)≤0ca,hard(x,λa)≤0ca,soft(x,λa,soft)≤0cδ˙(x)≤0Collision constraint derived from hard upper speed bound.Speed constraint derived from soft upper speed bound.Spatial soft/hard tube constraints imposed on car footprint.Spatial soft/hard tube constraints imposed on car footprint.Combined acceleration constraint on hard bounds.Acceleration constraint on comfort bounds.Speed dependent steering rate constraint.
-
Collision constraints
Receive real station(position) constraints imposed by the environment directly, rather than relying on indirect methods that convert velocity constraints into station constraints.
c station ( x , λ s ) ≤ 0 Collision constraint derived from hard upper speed bound. c k station ( x k , λ s ) = { s k − s k max − λ s , k ≤ 0 , − s k + s k min − λ s , k ≤ 0 , ∀ k ∈ { 0 , … , N } \begin{array}{ll} c^{\text{station}}(x, \lambda_s) \leq 0 & \text{Collision constraint derived from hard upper speed bound.}\\ \\ c^{\text{station}}_k(x_k, \lambda_s) = \begin{cases} s_k - s^{\text{max}}_k - \lambda_{s,k} \leq 0, \\ -s_k + s^{\text{min}}_k - \lambda_{s,k} \leq 0, \end{cases} \quad \forall k \in \{0, \ldots, N\} \end{array} cstation(x,λs)≤0ckstation(xk,λs)={sk−skmax−λs,k≤0,−sk+skmin−λs,k≤0,∀k∈{0,…,N}Collision constraint derived from hard upper speed bound. -
Speed constraint
c vel ( x , λ v , soft ) ≤ 0 Speed constraint derived from soft upper speed bound. ν − λ ≤ v max \begin{array}{ll} c^{{\text{vel}}}(x, \lambda_{v_{\text{, soft}}}) \leq 0 & \text{Speed constraint derived from soft upper speed bound.}\\ \\ \nu - \lambda \leq v_{\text{max}} \end{array} cvel(x,λv, soft)≤0ν−λ≤vmaxSpeed constraint derived from soft upper speed bound.
The existence of slack variables is motivated by:
- Due to suddenly changing environments/constraints, being outside of the feasible region is inevitable.
- When entering the infeasible region is the only option, we still want MPC to succeed in finding a solution that makes a trade-off in constraint violations.
- Slack helps the solver converge when initialized outside of the feasible region.
- Spatial tube constraints
Lateral position and shift of corners due to rotation of the car wrt the reference path
c
tube,hard
(
x
,
λ
n
)
=
{
n
k
+
d
car,left,front
(
μ
k
)
−
d
road,left,front,k
−
λ
n
,
k
≤
0
,
n
k
+
d
car,left,rear
(
μ
k
)
−
d
road,left,rear,k
−
λ
n
,
k
≤
0
,
−
n
k
+
d
car,right,front
(
μ
k
)
+
d
road,right,front,k
−
λ
n
,
k
≤
0
,
−
n
k
+
d
car,right,rear
(
μ
k
)
+
d
road,right,rear,k
−
λ
n
,
k
≤
0
,
∀
k
∈
{
0
,
…
,
N
}
c^{\text{tube,hard}}(x, \lambda_n) = \begin{cases} n_k + d_{\text{car,left,front}}(\mu_k) - d_{\text{road,left,front,k}} - \lambda_{n,k} \leq 0, \\ n_k + d_{\text{car,left,rear}}(\mu_k) - d_{\text{road,left,rear,k}} - \lambda_{n,k} \leq 0, \\ -n_k + d_{\text{car,right,front}}(\mu_k) + d_{\text{road,right,front,k}} - \lambda_{n,k} \leq 0, \\ -n_k + d_{\text{car,right,rear}}(\mu_k) + d_{\text{road,right,rear,k}} - \lambda_{n,k} \leq 0, \end{cases} \quad \forall k \in \{0, \ldots, N\}
ctube,hard(x,λn)=⎩
- Max Acceleration constraint
• Vehicle dynamics are naturally defined in total accelerations (lateral and longitudinal combined).
• An elliptical constraint is formulated to capture the constraint on total accelerations.
c
k
a
,hard
(
x
k
,
λ
a
)
=
a
y
2
a
y
,
max
,
k
2
+
a
x
2
a
x
,
max
,
k
2
−
1
−
λ
a
≤
0
,
∀
k
∈
{
0
,
…
,
N
}
c^{a_{\text{,hard}}}_k(x_k, \lambda_a) = \sqrt{\frac{a^2_{y}}{a^2_{y,\text{max},k}} + \frac{a^2_{x}}{a^2_{x,\text{max},k}}} - 1 - \lambda_a \leq 0, \quad \forall k \in \{0, \ldots, N\}
cka,hard(xk,λa)=ay,max,k2ay2+ax,max,k2ax2
- Comfort Acceleration constraint
• Accelerations in the driving direction within comfortable bounds should be free to execute.
• Accelerations beyond comfort level are penalized with a soft bound on forward and backward accelerations individually.
c k a ,soft ( x k , λ a , soft ) = { a k − a comfort − λ a , soft ≤ 0 , − a k − decel comfort − λ a , soft ≤ 0 ∀ k ∈ { 0 , … , N } c^{a_{\text{,soft}}}_k(x_k, \lambda_{a,\text{soft}}) = \begin{cases} a_k - a_{\text{comfort}} - \lambda_{a,\text{soft}} \leq 0, \\ -a_k - \text{decel}_{\text{comfort}} - \lambda_{a,\text{soft}} \leq 0 \end{cases} \quad \forall k \in \{0, \ldots, N\} cka,soft(xk,λa,soft)={ak−acomfort−λa,soft≤0,−ak−decelcomfort−λa,soft≤0∀k∈{0,…,N}
Model
Vehicle Model
Dynamic Bicycle Model
x
˙
=
v
x
cos
(
ψ
)
−
v
y
sin
(
ψ
)
y
˙
=
v
x
sin
(
ψ
)
+
v
y
cos
(
ψ
)
ψ
˙
=
r
v
˙
x
=
1
m
(
F
r
,
x
−
F
f
,
y
sin
(
δ
)
+
m
v
y
r
)
v
˙
y
=
1
m
(
F
r
,
y
+
F
r
,
y
cos
(
δ
)
−
m
v
x
r
)
r
˙
=
1
I
z
(
F
f
,
y
l
f
cos
(
δ
)
−
F
r
,
y
l
r
)
\begin{align*} & \dot{x} = v_x \cos(\psi) - v_y \sin(\psi) \\ & \dot{y} = v_x \sin(\psi) + v_y \cos(\psi) \\ & \dot{\psi} = r \\ & \dot{v}_x = \frac{1}{m} (F_{r,x} - F_{f,y} \sin(\delta) + m v_y r) \\ & \dot{v}_y = \frac{1}{m} (F_{r,y} + F_{r,y} \cos(\delta) - m v_x r) \\ & \dot{r} = \frac{1}{I_z} (F_{f,y} l_f \cos(\delta) - F_{r,y} l_r) \end{align*}
x˙=vxcos(ψ)−vysin(ψ)y˙=vxsin(ψ)+vycos(ψ)ψ˙=rv˙x=m1(Fr,x−Ff,ysin(δ)+mvyr)v˙y=m1(Fr,y+Fr,ycos(δ)−mvxr)r˙=Iz1(Ff,ylfcos(δ)−Fr,ylr)
Linearizing tire cornering forces
m
v
˙
=
−
m
u
r
+
2
(
c
f
(
δ
f
−
v
+
a
r
x
˙
)
+
c
r
b
r
−
v
u
)
m
u
˙
=
m
v
r
+
2
(
c
f
s
f
+
c
f
(
δ
−
v
+
a
r
x
˙
)
δ
f
+
c
r
s
r
)
I
r
r
˙
=
2
(
a
c
f
(
δ
f
−
v
+
a
r
x
˙
)
−
b
c
r
b
r
−
v
u
)
Y
˙
=
u
sin
ϕ
+
v
cos
ϕ
X
˙
=
u
cos
ϕ
−
v
sin
ϕ
\begin{align*} m\dot{v} &= -mur + 2 \left( c_{f} \left( \delta_f - \frac{v + a r}{\dot{x}} \right) + c_{r} \frac{br - v}{u} \right) \\ m\dot{u} &= mvr + 2 \left( c_{f} s_f+ c_{f} \left( \delta - \frac{v + ar}{\dot{x}} \right) \delta_f + c_{r} s_r \right) \\ I_r \dot{r} &= 2 \left( a c_{f} \left( \delta_f - \frac{v + ar}{\dot{x}} \right) - b c_{r} \frac{br - v}{u}\right) \\ \dot{Y} &= u \sin \phi + v \cos \phi \\ \dot{X} &= u \cos \phi - v \sin \phi \end{align*}
mv˙mu˙Irr˙Y˙X˙=−mur+2(cf(δf−x˙v+ar)+crubr−v)=mvr+2(cfsf+cf(δ−x˙v+ar)δf+crsr)=2(acf(δf−x˙v+ar)−bcrubr−v)=usinϕ+vcosϕ=ucosϕ−vsinϕ
Code frame
实现一个基于模型预测控制（MPC）的车辆控制系统，主要用于自动驾驶模拟器CARLA中的车辆。MPC是一个用于控制和规划的优化框架，能够考虑车辆的动力学特性和对未来状态的预测。
MPC控制器代码工作流程
- 初始化和配置
预测时域（Np）和控制时域（Nc）：这决定了控制器看多远未来的时间范围以及在这个范围内计划多少个控制动作。
时间步长（dt）：控制和预测的时间间隔，影响到模型预测的精度和计算量。
车辆动力学参数（Lf等）：这些参数用于描述车辆动力学模型，是模型预测的基础。
初始化变量：包括车辆的当前状态（位置、速度、方向等）和目标路径的系数（用于描述期望路径的多项式）。 - 设定优化问题
目标函数：构造一个目标函数来最小化路径偏差（CTE）、航向偏差（Epsi）和速度偏差等，同时也可能考虑减少执行器使用的急变以平滑控制过程。
约束条件：包括车辆动力学模型的约束，确保预测状态遵循物理定律；执行器的限制，如转向角度和加速度的界限；以及确保优化问题的初始状态是当前车辆状态的约束。 - 求解优化问题
求解器选择：选用如IPOPT这样的优化求解器，它能够处理有约束的非线性优化问题。
计算控制输入序列：求解器根据目标函数和约束条件计算出最优的控制输入序列。
选择控制输入：从求解得到的控制输入序列中，取出第一组控制输入（通常包括转向角和加速度）用于立即执行。 - 执行控制并更新状态
执行控制：将求解得到的第一组控制输入应用于车辆，影响车辆的实际状态。
状态更新：根据执行的控制输入和车辆模型，更新车辆的状态。
反馈循环：在每个控制周期，根据新的车辆状态和目标路径重新设定优化问题，然后重复求解优化问题和执行控制的过程，实现闭环控制。
以下是代码的主要组成部分的详细说明：
FG_eval 类
这个类用于定义优化问题，包括成本函数和动力学模型的约束。Ipopt（一个用于大型非线性优化问题的求解器）会调用这个类。其中FG_eval类利用了CppAD库，这是一个用于自动微分的C++库。它可以计算函数导数，使得可以在优化过程中求解函数的梯度和Hessian矩阵，这是大多数非线性优化算法所需要的。
构造函数
FG_eval 的构造函数接收当前车辆状态、多项式路径系数、目标速度、各种权重因子、预测和控制的时域长度、控制步长、车辆参数以及上一个控制动作的状态。它初始化了成本函数权重、控制和预测时域的长度、控制步长等，这些都是MPC调整的关键参数。
车辆状态参数：
x_start 是车辆的全局X坐标。
y_start 是车辆的全局Y坐标。
psi_start 是车辆的航向角。
v_longitudinal_start 是车辆的纵向速度。
v_lateral_start 是车辆的侧向速度。
yaw_rate_start 是车辆的横摆速率。
cte_start 是车辆的横向跟踪误差（CTE）。
epsi_start是车辆的航向误差（Epsi）。
控制参数：
front_wheel_angle
longitudinal_acceleration
operator()
operator() 函数是核心，定义了整个优化问题。此外，控制动作的平滑性也作为成本的一部分来考虑。该函数还包括动力学模型的约束，保证车辆状态的连续性和物理合理性。
1. 成本函数 (fg[0])
fg数组在这个上下文中代表一个包含了成本函数和所有约束的向量。fg[0]是优化问题的目标函数，即要被最小化的部分。它是一个累加的总成本，包括了以下各项：
Tracking objective function
- 路径误差（CTE, cross-track error）的平方乘以一个权重（cte_weight）。
- 方向误差（Epsi, orientation error）的平方乘以一个权重（epsi_weight）。
- 速度误差（与目标速度ref_v之差）的平方乘以一个权重（v_weight）。
上述成本项的目的是让车辆尽可能地跟随参考轨迹，并以期望的速度行驶。
Comfort objective function
- 前轮转角的平方乘以一个权重（steer_actuator_cost_weight_fg），用于惩罚大的转向动作。
- 纵向加速度的平方乘以一个权重（acc_actuator_cost_weight_fg），用于惩罚大的加速或减速动作。
这两项成本的目的是减少执行器（在汽车中通常是转向和油门）的使用，因为过多地使用执行器会导致车辆行为突兀，并增加磨损。
Safety objective function
控制量平滑
- 连续转向动作之间的变化的平方乘以一个权重（change_steer_cost_weight）。
- 连续加速动作之间的变化的平方乘以一个权重（change_accel_cost_weight）。
这两项成本的目的是鼓励动作的平滑性，减少连续控制动作之间的剧烈变化，以实现平稳驾驶。
2. 等式约束条件的初始化(fg[1 + …])，t=0
优化变量vars中的状态变量（比如位置、航向、速度等）与实际车辆状态相匹配。这相当于告诉优化器，我们的起始点是车辆的当前状态，优化应该从这个状态开始计算未来的控制动作。
每个 fg[1 + …] 表示一个特定的约束，在优化问题中索引从1开始，因为索引0用于存储成本函数值。对于每个车辆状态变量：
fg[1 + x_start] 是车辆的全局X坐标的约束。 fg[1 + y_start] 是车辆的全局Y坐标的约束。
fg[1 + psi_start] 是车辆的航向角的约束。
fg[1 + v_longitudinal_start] 是车辆的纵向速度的约束。
fg[1+ v_lateral_start] 是车辆的侧向速度的约束。
fg[1 + yaw_rate_start] 是车辆的横摆速率的约束。
fg[1 + cte_start] 是车辆的横向跟踪误差（CTE）的约束。
fg[1 + epsi_start]是车辆的航向误差（Epsi）的约束。
在这里，通过将vars中的相应状态变量直接赋值给fg中的约束变量，强制这些初始状态约束与车辆当前状态相等。这样做的目的是为了在解决优化问题时保证初始条件是正确的，并且后续的优化步骤（即优化变量中的未来状态和控制动作）都是基于这个实际的起始点计算的。
vars中的这些预测值是通过求解优化问题得到的，其中优化问题旨在最小化一个关于预测轨迹的成本函数，同时满足车辆动力学的约束。
3. Predict model，t(1, Np)
- 进入for循环。
- 计算参考路径和参考航向角（psi_des_0），这在模型预测控制（MPC）中至关重要。MPC旨在使车辆沿着预定的路径行驶，同时尽可能减小航向误差和路径偏差。
- 参考路径 (f_0) f_0 计算的是给定位置 x_0 下，参考路径上的 y 值。这是通过对 x_0 应用一个多项式函数来实现的，该多项式由系数 coeffs 定义。 这个多项式可以是任意阶数，具体由 coeffs
的长度决定。在这个例子中，多项式最高到五次方。 - 参考航向角 (psi_des_0) psi_des_0 计算的是在 x_0 处，参考路径的切线角度，即车辆理论上应该具有的航向角，以保持沿着参考路径行驶。 这个角度是通过计算多项式 f_0 在 x_0处的导数，并取其反正切值 atan 来得到的。这里的导数代表了在 x_0处参考路径的斜率。通过计算导数，我们能得到多项式曲线在特定点的切线斜率，反正切值即为这个切线与水平轴的夹角，即参考航向角。
- 参考路径 (f_0) f_0 计算的是给定位置 x_0 下，参考路径上的 y 值。这是通过对 x_0 应用一个多项式函数来实现的，该多项式由系数 coeffs 定义。 这个多项式可以是任意阶数，具体由 coeffs
- 通过计算时间t 时和时间 t+1 时车辆的状态变量（x_0, y_0 等，x_1, y_1 等）：这些变量代表了时间 t - 1 时车辆的状态，t- 1 时刻的状态用于预测 t 时刻的状态。
- 控制输入变量：front_wheel_angle_0 代表前轮转角，longitudinal_acceleration_0 代表纵向加速度。这些变量是优化器用来调整的控制量，以达到在 t时刻预测的车辆状态。
- 将动力学模型进行前向迭代，通过模型（根据前面计算出的车辆状态变量和控制输入变量）来预测车辆的未来运动。然后满足等式：约束优化过程中计算出的横向位置的预测值必须等于根据车辆动力学模型计算出的预测值。然后通过设置 fg[1 + … + t] 等于这两个预测值的差值。
x_1 代表优化变量中在时间步 t+1 的横向位置 x 的预测值。
x_0 + …后一项是根据车辆动力学模型计算出的在时间步 t+1 的横向位置 x 的预测值。
mpc_controller 类
- 这是MPC控制器的主要类，它封装了求解MPC问题的逻辑。Solve方法接收当前车辆状态、目标路径的系数以及其他MPC参数，然后配置并求解MPC优化问题。
- 关键是调用 CppAD::ipopt::solve函数，它使用IPOPT算法来找到最小化成本函数的控制输入。
- 求解器返回的第一个控制输入（转向角和加速度）将被用于控制车辆。
- 除了最优控制命令之外，它还返回预测的车辆状态，这通常用于调试或在仿真器中可视化MPC的路径。
参数初始化：
- Nc 和 Np 分别代表控制和预测的时间范围（即预测时域和控制时域的长度）。通常，控制时域小于或等于预测时域。
- x_start,y_start, psi_start 等变量定义了优化变量向量中各状态起始位置的索引。
- a_lateral, old_steer_value, old_throttle_value 等变量用于初始化车辆模型的参数或之前的控制状态。
定义优化变量 (vars)：
这一步初始化所有优化变量（包括车辆的状态变量和控制输入变量）为零，并为它们设置合适的上下界。这些变量将在优化过程中被调整，以最小化成本函数。
定义不等式约束
执行器输出的限制，如转向角和加速度的界限。
状态变量的限制，如车辆速度的最大和最小值。
定义初始状态约束 (constraints_lower_bounds 和 constraints_upper_bounds)：
为优化问题设置初始状态约束，确保求解器（优化算法）在寻找最优解时从当前实际车辆状态开始。这是通过对优化变量（vars）中代表车辆初始状态的部分设置相等的上下界来实现的。
构造目标和约束函数 (FG_eval)：
这一部分利用 FG_eval 类，创建一个 FG_eval 对象的实例fg_eval，这个对象是为了定义在模型预测控制（MPC）问题中使用的目标函数和约束条件。fg_eval 计算给定优化变量的总成本，并评估所有约束是否得到满足。
配置并求解优化问题：
配置IPOPT求解器的参数，如打印级别、计算速度优化等。
调用 CppAD::ipopt::solve 函数求解优化问题。求解器根据目标函数和约束函数计算最优的控制输入。
print_level：控制输出信息的详细程度。
Sparse：告诉求解器约束和雅可比矩阵是稀疏的，这可以显著提高求解速度。
max_cpu_time：限制求解器的最大CPU时间。
处理求解结果：
- result向量被用来存储优化的结果，首先提取控制输入值，即转向角 (front_wheel_angle) 和加速度 (longitudinal_acceleration) 的值。这两个控制输入将直接用于车辆的控制系统，指导车辆的即时行驶行为。
- 随后的循环通过solution.x[x_start + i] 和 solution.x[y_start + i] 从优化结果中提取了车辆未来位置的预测值。Np - 1表示我们提取从当前时间步到预测时域结束之前的所有预测位置。这些信息对于可视化MPC预测的轨迹非常有用，可以帮助我们理解控制策略是如何引导车辆逐步靠近目标路径的。
ROS Node Configuration
- 参数配置和环境设置：通过ROS参数服务器获取MPC控制器的配置参数，如车辆的参考速度、控制时域长度、权重等，并根据这些参数初始化MPC控制器。
- 数据订阅和处理：通过订阅车辆的位置、速度和IMU数据，获取车辆的实时状态信息，这些信息被用作MPC算法的输入。
- 控制指令计算：利用mpc_controller.cpp 中定义的MPC算法，根据车辆的实时状态和目标路径，计算出用于路径跟踪的最优控制指令。
- 控制指令发布：将计算得到的控制指令发布给车辆，使其按照预定的路径行驶。
- 路径可视化：为了方便观察和调试，该代码还包括将MPC计算的路径和参考路径通过ROS话题发布出来，以便在RViz等工具中可视化。
本文标签： PredictiveModelVehicleControllercontrol
版权声明：本文标题：Vehicle Model Predictive Controller【Autonomous Vehicle Planning and Control】 内容由热心网友自发贡献，该文观点仅代表作者本人， 转载请联系作者并注明出处：https://www.elefans.com/xitong/1728284192a1152106.html， 本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如发现本站有涉嫌抄袭侵权/违法违规的内容，一经查实，本站将立刻删除。
发表评论