写在前面：
🌟 欢迎光临 清流君 的博客小天地，这里是我分享技术与心得的温馨角落。📝
个人主页：清流君_CSDN博客，期待与您一同探索 移动机器人 领域的无限可能。
🔍 本文系 清流君 原创之作，荣幸在CSDN首发🐒
若您觉得内容有价值，还请评论告知一声，以便更多人受益。
转载请注明出处，尊重原创，从我做起。
👍 点赞、评论、收藏，三连走一波，让我们一起养成好习惯😜
在这里，您将收获的不只是技术干货，还有思维的火花！
📚 系列专栏：【运动控制】系列，带您深入浅出，领略控制之美。🖊
愿我的分享能为您带来启迪，如有不足，敬请指正，让我们共同学习，交流进步！
🎭 人生如戏，我们并非能选择舞台和剧本，但我们可以选择如何演绎 🌟
感谢您的支持与关注，让我们一起在知识的海洋中砥砺前行~~~
文章目录
引言
本篇博客将介绍自动驾驶车辆控制中MPC约束的构建和松弛因子的考量。
一、不等式约束的构建
在MPC中，除了要满足输入输出和系统状态的约束:
x
min
≤
x
≤
x
max
u
min
≤
u
≤
u
max
x_{\min}\leq x\leq x_{\max}\\u_{\min}\leq u\leq u_{\max}
xmin≤x≤xmaxumin≤u≤umax
还要考虑以下约束：
1、安全距离约束
c station ( x , λ s ) ≤ 0 c^\text{station}{(x,\lambda_s)}\leq0 cstation(x,λs)≤0这个安全距离约束用于确保车辆与其他车辆保持足够的安全距离，以防止碰撞。
2、速度约束
车辆速度的软约束，用于限制车辆的速度在一定范围内，以保证行驶的安全性和舒适性：
c
v
e
l
(
x
,
λ
v
,
s
o
f
t
)
≤
0
c^{\mathrm{vel}}(\boldsymbol{x},\lambda_{v,soft})\leq0
cvel(x,λv,soft)≤0用来保证车辆速度在安全范围之内。
3、横向约束
在横向上有一个被称为 管道 (tube) 的约束，用于限制车辆在横向方向上的移动，以保持车辆在车道内的稳定行驶：
c
t
u
b
e
,
h
a
r
d
(
x
,
λ
n
)
≤
0
c
t
u
b
e
,
s
o
f
t
(
x
,
λ
n
,
s
o
f
t
)
≤
0
c^{\mathrm{tube,hard}}(\boldsymbol{x},\lambda_n)\leq0\\c^{\mathrm{tube,soft}}(\boldsymbol{x},\lambda_{n,soft})\leq0
ctube,hard(x,λn)≤0ctube,soft(x,λn,soft)≤0这个Tube约束包括软约束和硬约束两部分，确保车辆的轨迹保持在一定的空间范围内。
4、加速度约束
加速度约束则同时考虑了安全性能和舒适性能，确保车辆的加速行为既安全又舒适：
c
a
,
h
a
r
d
(
x
,
λ
a
)
≤
0
c
a
,
s
o
f
t
(
x
,
λ
a
,
s
o
f
t
)
≤
0
\begin{aligned}c^{\mathrm{a,hard}}(\boldsymbol{x},\lambda_{a})&\leq0\\c^{\mathrm{a,soft}}(x,\lambda_{a,soft})&\leq0\end{aligned}
ca,hard(x,λa)ca,soft(x,λa,soft)≤0≤0
5、转向角速度约束
当然还有转向角约束，以保证车辆在转弯时不会超过安全范围，从而确保驾驶的安全性：
c
δ
˙
(
x
)
≤
0
c^{\dot{\delta}}(x)\leq0
cδ˙(x)≤0这里的转向角变化量的约束通常与行驶速度有关，以确保在不同的行驶速度下，车辆的转向行为都是安全且可控的。
二、不等式约束详细解析
接下来将针对每个约束进行详细的讲解：
1、纵向空间约束
首先来看纵向空间约束表达式：
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
}
c_k^{\text{station}}(x_k,\lambda_s)=\begin{cases}s_k-s_k^{\text{max}}-\lambda_{s,k}&\le0\\-s_k+s_k^{\text{min}}-\lambda_{s,k}&\le0\end{cases},\quad\forall k\in\{0,\dots,N\}
ckstation(xk,λs)={sk−skmax−λs,k−sk+skmin−λs,k≤0≤0,∀k∈{0,…,N}在纵向方向上，构建硬约束通常有两种方法：
a）直接距离法
这种方法是通过直接测量与前车的距离来构建硬约束。
例如，如果车辆检测到前方有车辆切入其前进道路，它可以设置一个与前车保持的相对安全距离，并根据这个距离来构建硬约束。
b）速度反向计算法
这种方法是基于相对距离，反向计算出一个合理的速度值作为硬约束。即使用速度作为硬约束，通过积分来表示碰撞约束。这种方法可能会引入由积分引起的误差，尤其是在速度约束推导出行驶距离量时。
由于直接使用速度反向计算法构建的碰撞约束可能不够准确，为了更精确地约束车辆的纵向行进距离，我们更倾向于使用真实环境中的距离约束，即第一种方法。这种方法可以直接基于实际距离进行硬约束的构建，从而避免在优化过程中初始条件不在约束范围内的问题，提高了约束的准确性和优化效率。
2、速度约束
下面来看速度约束，一般把速度约束写成以下形式：
v
−
λ
≤
v
m
a
x
v-\lambda\leq v_{max}
v−λ≤vmax也就是
v
≤
v
max
+
λ
v\le v_{\max}+\lambda
v≤vmax+λ，其中
v
v
v 是车辆的速度，
v
m
a
x
v_{max}
vmax 是最大允许速度，
λ
\lambda
λ 是松弛因子。引入松弛因子主要是为了应对环境或限制的突然变化，例如在行驶过程中突然进入下坡路段。这种环境突变可能导致车辆的速度超过预设的最大速度。
引入松弛因子 λ \lambda λ 可以帮助MPC在遇到这样的情况时，即使速度超过可行区域范围，也能成功地找到一个解。这意味着在找到解和违反约束之间进行权衡。同时，松弛因子还可以帮助MPC在初始条件不在可行域范围内时收敛，从而提高了算法的鲁棒性和适应性。
3、横向空间约束
a）软硬约束的设计
在横向空间约束中，我们通常采用两种类型的约束：硬约束和软约束。
-
硬约束：不可违反的边界条件，用于限制车辆行驶的物理边界，如道路边缘的障碍物（如路牙子）或其他不可穿越的障碍。这些约束确保车辆不会超出预定的安全边界，防止车辆偏离车道或与其他物体发生碰撞。
-
软约束：允许一定程度的违反，用于限制车辆行驶的虚拟边界，如车辆的轮胎轨迹。这些约束的目的是在车辆接近车道边缘时提供预警，而不是强制车辆保持在车道中心。
无论是硬约束还是软约束，它们都是基于车辆的“Footprint”与“Bound”之间的距离来描述的：
- Footprint 是指车辆在横向行驶时轮胎接触地面的区域
- Bound 是指车辆行驶时必须遵守的物理或虚拟边界
硬约束确保车辆不会触碰到这些边界，而软约束则允许车辆在一定范围内接近这些边界，但不会实际触碰到它们，如下图所示：
软约束通常用来描述一个更加舒适或适合人类驾驶的行驶范围，它与硬约束之间通常保持一定的距离。这种设计考虑了驾驶员的驾驶习惯和车辆行驶的舒适性，允许车辆在接近硬约束之前做出一定的调整，从而避免因过于严格的硬约束导致的频繁调整和驾驶不适。
软约束的设置为驾驶员提供了一定的操作空间，使他们能够在遵守硬约束的同时，更加自然和流畅地驾驶。这种设计有助于提高驾驶体验，减少因硬约束过于严格而导致的驾驶员疲劳和驾驶压力。
b）横向空间约束的计算
那么怎样来计算横向空间的约束呢？
c t u b e , h a r d ( x , λ n ) = { n k + d c a r , l e f t , f r o n t ( μ k ) − d r o a d , l e f t , f r o n t , k − λ n , k ≤ 0 n k + d c a r , l e f t , r e a r ( μ k ) − d r o a d , l e f t , r e a r , k − λ n , k ≤ 0 − n k + d c a r , r i g h t , f r o n t ( μ k ) + d r o a d , r i g h t , f r o n t , k − λ n , k ≤ 0 − n k + d c a r , r i g h t , r e a r ( μ k ) + d r o a d , r i g h t , r e a r , k − λ n , k ≤ 0 \left.c^{\mathrm{tube,hard}}(x,\lambda_{n})=\left\{\begin{array}{ll}n_{k}+d_{\mathrm{car,left,front}}(\mu_{k})-d_{\mathrm{road,left,front,k}}-\lambda_{n,k}&\leq0\\n_{k}+d_{\mathrm{car,left,rear}}(\mu_{k})-d_{\mathrm{road,left,rear,k}}-\lambda_{n,k}&\leq0\\-n_{k}+d_{\mathrm{car,right,front}}(\mu_{k})+d_{\mathrm{road,right,front,k}}-\lambda_{n,k}&\leq0\\-n_{k}+d_{\mathrm{car,right,rear}}(\mu_{k})+d_{\mathrm{road,right,rear,k}}-\lambda_{n,k}&\leq0\end{array}\right.\right. ctube,hard(x,λn)=⎩ ⎨ ⎧nk+dcar,left,front(μk)−droad,left,front,k−λn,knk+dcar,left,rear(μk)−droad,left,rear,k−λn,k−nk+dcar,right,front(μk)+droad,right,front,k−λn,k−nk+dcar,right,rear(μk)+droad,right,rear,k−λn,k≤0≤0≤0≤0其中， ∀ k ∈ { 0 , … , N } \forall k\in\{0,\ldots,N\} ∀k∈{0,…,N}
首先，我们根据横向位移
n
n
n，计算车辆四个角的位移
d
c
a
r
d_{car}
dcar ，包括由于转弯而未引入的横向偏差。然后，将这四个角的位移加上横向偏差
μ
k
\mu_k
μk，确保这一项的值小于等于在这一点上的边界
d
r
o
a
d
d_{road}
droad。这样做的目的是确保车辆在横向行驶时不会超出预定的边界：
对于软边界来说，车辆的四个角的位置不能超过预定的边界。为了处理这种情况，引入松弛因子(Slack)。通过这种方式，确保车辆的横向位置运动受到有效约束，同时考虑到实际情况中可能出现的微小偏差。
4、加速度约束
a）硬约束
下面来看施加在加速度上的硬约束，在车辆行驶过程中，纵向加速度较高时，横向加速度通常会相应减少。这是因为高速行驶时，车辆需要保持稳定，因此驾驶员不会对方向盘进行剧烈操作。这种情况下，硬约束有助于确保车辆在加速过程中保持横向稳定性，避免因横向加速度过大而导致的失控风险。横纵向加速度的关系如下如所示：
当车辆进行大角度的横向加速运动，例如在急转弯时，我们也会对纵向加速度施加限制。这是因为急转弯时，车辆的速度会相应下降，以避免发生漂移并进入非线性控制区域。
在这样的情况下，对横向和纵向加速度的约束有助于保持车辆的稳定性，确保在复杂路况下的安全行驶。通过这种方式，我们可以确保车辆在各种驾驶条件下都能保持安全，避免因加速度过大而导致的失控风险。
因此，加速度的硬约束定义为如下形式：
c
k
a
,
h
a
r
d
(
x
k
,
λ
a
)
=
a
y
2
a
y
,
m
a
x
,
k
2
+
a
x
2
a
x
,
m
a
x
,
k
2
−
1
−
λ
a
≤
0
c_{k}^{a,\mathrm{hard}}(x_{k},\lambda_{a})=\sqrt{\frac{a_{y}^{2}}{a_{y,\mathrm{max},k}^{2}}+\frac{a_{x}^{2}}{a_{x,\mathrm{max},k}^{2}}}-1-\lambda_{a}\leq0
cka,hard(xk,λa)=ay,max,k2ay2+ax,max,k2ax2−1−λa≤0其中，
∀
k
∈
{
0
,
…
,
N
}
\forall k\in\{0,\ldots,N\}
∀k∈{0,…,N}
那这样做有什么好处呢？
通过施加横向和纵向加速度的约束，我们实际上对车辆在 x x x 和 y y y 方向上的最大值百分比进行了限制。这意味着车辆在行驶过程中，其横向和纵向加速度的增加是有限度的，一旦达到预设的百分比，车辆将无法继续加速。这种限制有助于防止车辆在紧急情况下失控，从而提高安全性。
相当于以下形式：
a
x
≤
a
x
,
max
a
y
≤
a
y
,
max
a_x\le a_{x,\max}\ \ \ \ \ a_y\le a_{y,\max}
ax≤ax,max ay≤ay,max同时，我们还对横向和纵向加速度的合成加速度进行了有效的约束，以保证车辆的安全性和舒适性。
b）软约束
这种约束是针对舒适度的加速度考量，通常定义为软约束。在这种情况下，我们设定一个加速度的阈值，当车辆的加速度超过这个值时，认为是不舒适的。即：
a
k
≤
a
c
o
m
f
o
r
t
+
λ
a
,
s
o
f
t
a_k\leq a_{comfort}+\lambda _{a,soft}
ak≤acomfort+λa,soft这种软约束的目的是在确保车辆安全的前提下，尽量减少乘客的不适感。
对于减速度也同理：
a
k
≥
−
d
e
c
e
l
c
o
m
f
o
r
t
−
λ
a
,
s
o
f
t
a_k\geq -decel_{comfort}-\lambda _{a,soft}
ak≥−decelcomfort−λa,soft来保证对加速度舒适度的约束，将上面两个式子整合成如下形式：
c
k
a
,
s
o
f
t
(
x
k
,
λ
a
,
s
o
f
t
)
=
{
a
k
−
a
c
o
m
f
o
r
t
−
λ
a
,
s
o
f
t
≤
0
−
a
k
−
d
e
c
e
l
c
o
m
f
o
r
t
−
λ
a
,
s
o
f
t
≤
0
c_k^{a,\mathrm{soft}}(\boldsymbol{x}_k,\lambda_{a,\mathrm{soft}})=\begin{cases}a_k-a_{\mathrm{comfort}}-\lambda_{a,\mathrm{soft}}&\leq0\\-a_k-\mathrm{decel_{comfort}}-\lambda_{a,\mathrm{soft}}&\leq0\end{cases}
cka,soft(xk,λa,soft)={ak−acomfort−λa,soft−ak−decelcomfort−λa,soft≤0≤0其中，
∀
k
∈
{
0
,
…
,
N
}
\forall k\in\{0,\ldots,N\}
∀k∈{0,…,N}
三、MPC车辆控制约束完整形式
这样就完整的构建出运用 MPC 的车辆控制约束：
min
U
≜
{
u
k
∣
k
,
u
k
+
1
∣
k
,
.
.
.
}
J
(
x
k
,
U
)
=
∑
k
=
0
N
−
1
J
s
t
a
g
e
(
x
k
,
u
k
,
λ
k
)
+
J
t
e
r
m
i
n
a
l
(
x
N
)
s.t.
x
k
+
1
=
f
(
x
k
,
u
k
)
,
c
k
(
x
k
,
u
k
,
λ
k
)
≤
0
,
x
∈
X
,
u
∈
U
,
λ
∈
Λ
.
\begin{aligned} \min_{U\triangleq\{u_{k|k},u_{k+1|k},...\}}J(x_{k},U)& =\sum_{k=0}^{N-1}J_{stage}(x_{k},u_{k},\lambda_{k})+J_{terminal}(x_{N}) \\ \text{s.t.}\ \ \ \ & x_{k+1}=f(x_{k},u_{k}), \\ &c_k(x_k,u_k,\lambda_k)\leq0, \\ &x\in\mathcal{X}, \\ &u\in\mathcal{U}, \\ &\lambda\in\Lambda. \end{aligned}
U≜{uk∣k,uk+1∣k,...}minJ(xk,U)s.t. =k=0∑N−1Jstage(xk,uk,λk)+Jterminal(xN)xk+1=f(xk,uk),ck(xk,uk,λk)≤0,x∈X,u∈U,λ∈Λ.
四、总结
可以看一下，首先有了这样的模型: x k + 1 = f ( x k , u k ) x_{k+1}=f(x_{k},u_{k}) xk+1=f(xk,uk)
例如，在之前博客中介绍的曲线坐标车辆模型，就是一个能够有效的把纵向和横向的运动耦合起来，并且很好地描述车辆运动与相对参考轨迹之间的关系。
接着定义了目标函数：
∑
k
=
0
N
−
1
J
s
t
a
g
e
(
x
k
,
u
k
,
λ
k
)
+
J
t
e
r
m
i
n
a
l
(
x
N
)
\sum_{k=0}^{N-1}J_{stage}(x_{k},u_{k},\lambda_{k})+J_{terminal}(x_{N})
k=0∑N−1Jstage(xk,uk,λk)+Jterminal(xN)
通过之前的讲解，我们将标准的MPC目标函数扩展到了包含松弛因子的软约束，并引入了终端目标函数来保证系统的稳定性和可行性。这种扩展的目标函数允许在预测时域的最后一个阶段施加额外的成本，以确保系统状态在预测结束时达到预定的目标状态。
- 引入松弛因子：可以在硬约束无法完全满足的情况下，通过软约束提供一定的灵活性，从而找到一个可行的解。
- 终端目标函数：确保系统在预测时域结束时能够稳定地达到预期的状态，即使初始条件不在可行域内。
最后，我们讲解了如何构建约束条件 c k ( x k , u k , λ k ) ≤ 0 c_k(x_k,u_k,\lambda_k)\leq0 ck(xk,uk,λk)≤0，以及状态约束 x ∈ X x\in\mathcal{X} x∈X 和输入约束 u ∈ U u\in\mathcal{U} u∈U，这些约束条件确保了系统在预测时域内和预测时域外的稳定性和可行性。
通过这些约束条件，我们可以找到一个优化的控制序列，即在每一个采样时刻 k k k，选择最优的控制值 u k u_k uk ，并将其作用于系统，以达到优化控制的目的。
这种方法使得MPC能够在有限预测时域内实现对系统状态的优化控制，同时确保系统在预测时域结束时能够稳定地达到预期的状态，从而提高整个控制过程的稳定性和效率。
参考资料
后记：
🌟 感谢您耐心阅读这篇关于 不等式约束和松弛因子 的技术博客。 📚
🎯 如果您觉得这篇博客对您有所帮助，请不要吝啬您的点赞和评论 📢
🌟您的支持是我继续创作的动力。同时，别忘了收藏本篇博客，以便日后随时查阅。🚀
🚗 让我们一起期待更多的技术分享，共同探索移动机器人的无限可能！💡
🎭感谢您的支持与关注，让我们一起在知识的海洋中砥砺前行 🚀
3124
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
