写在前面：
🌟 欢迎光临 清流君 的博客小天地，这里是我分享技术与心得的温馨角落。📝
个人主页：清流君_CSDN博客，期待与您一同探索 移动机器人 领域的无限可能。
🔍 本文系 清流君 原创之作，荣幸在CSDN首发🐒
若您觉得内容有价值，还请评论告知一声，以便更多人受益。
转载请注明出处，尊重原创，从我做起。
👍 点赞、评论、收藏，三连走一波，让我们一起养成好习惯😜
在这里，您将收获的不只是技术干货，还有思维的火花！
📚 系列专栏：【文献精读】系列，带您深入浅出，领略控制之美。🖊
愿我的分享能为您带来启迪，如有不足，敬请指正，让我们共同学习，交流进步！
🎭 人生如戏，我们并非能选择舞台和剧本，但我们可以选择如何演绎 🌟
感谢您的支持与关注，让我们一起在知识的海洋中砥砺前行~~~
文章目录
摘要
-
设计了一种具有自适应APF的MPC策略，针对平滑无碰撞换道路径构造了自适应APF函数，实现了自动驾驶车辆的路径跟踪和车道变换，以实现自动驾驶车辆的避碰。
-
与恒定纵向速度不同，可变纵向速度被构造为区间矩阵以显示路径跟踪系统的稳定性。
-
与圆形碰撞避免约束相比，开发了三角形碰撞避免约束以减少计算负担并解决约束冲突。
一、引言
1、主要贡献
- 通过MPC策略的代价函数中的自适应APF函数，确保具有多个约束的自主车辆的平滑无碰撞路径。
- 通过基于自主车辆和障碍物尺寸的三角形避碰约束解决了约束冲突和计算负担。
- 通过构造一个纵向速度边界的区间矩阵，证明了在纵向速度变化时路径跟踪系统的稳定性。
二、问题描述和准备工作
1、问题描述
自动驾驶车辆的基本方式是当以低速遇到前方障碍物车辆时改变车道进行超车。注意，当障碍车辆来自相反方向时，超车问题变为相遇问题。
一般来说，超车和会车是自主车辆运动控制中最常见的避碰问题。
2、路径跟踪模型
为了示出道路坐标系中的参考路径与自主车辆之间的关系，下图表示了基于自主车辆的运动学模型的路径跟踪模型：
{
e
˙
y
(
t
)
=
v
(
t
)
sin
(
e
φ
(
t
)
)
e
˙
φ
(
t
)
=
ω
(
t
)
−
v
(
t
)
c
R
(
t
)
\left\{\begin{matrix}\dot{e}_y(t)=v(t)\sin(e_\varphi(t))\\\dot{e}_\varphi(t)=\omega(t)-v(t)c_R(t)\end{matrix}\right.
{e˙y(t)=v(t)sin(eφ(t))e˙φ(t)=ω(t)−v(t)cR(t) 近似化：
{
e
˙
y
(
t
)
=
v
(
t
)
e
φ
(
t
)
,
e
˙
φ
(
t
)
=
ω
(
t
)
.
\left\{\begin{matrix}\dot e_y(t)=v(t)e_\varphi(t),\\\dot e_\varphi(t)=\omega(t).\end{matrix}\right.
{e˙y(t)=v(t)eφ(t),e˙φ(t)=ω(t). 从连续时间路径跟踪系统转换的离散时间路径跟踪系统：
e
(
k
+
1
)
=
A
(
k
)
e
(
k
)
+
B
u
(
k
)
e(k+1)=A(k)e(k)+Bu(k)
e(k+1)=A(k)e(k)+Bu(k)
3、障碍物运动模型
{ d o ( k + 1 ) = d o ( k ) + T v o ( k ) φ o ( k ) , φ o ( k + 1 ) = φ o ( k ) + T ω o ( k ) , s o ( k + 1 ) = s o ( k ) + T v o ( k ) , \begin{cases}d_o(k+1)=d_o(k)+Tv_o(k)\varphi_o(k),\\\varphi_o(k+1)=\varphi_o(k)+T\omega_o(k),\\s_o(k+1)=s_o(k)+Tv_o(k),\end{cases} ⎩ ⎨ ⎧do(k+1)=do(k)+Tvo(k)φo(k),φo(k+1)=φo(k)+Tωo(k),so(k+1)=so(k)+Tvo(k),
三、具有自适应APF策略的MPC
1、控制器结构
μ j ∈ R 2 \mu_j\in\mathbb{R}^2 μj∈R2
表示的是二维实数空间，也可以被理解为实数集的二维版本。
设计了带有自适应APF模块的MPC作为上层控制器，实现路径跟踪和避碰。采用自适应APF策略，将参考纵向速度用于MPC。
2、三角形避碰约束
障碍物车辆保持恒定速度 v o v_o vo，并以右侧车道为中心。障碍车的横向位置为 d o = 0 d_o = 0 do=0，分别以自主车和障碍车的长度 L L L 和宽度 W W W 为基础设计两个矩形区域， W r W_r Wr 为单车道的宽度， ρ s = 0.5 W 2 + L 2 \rho_s=0.5\sqrt{W^2+L^2} ρs=0.5W2+L2为恰好包围自主车的圆的安全半径，不安全区域和安全区域分别由黄色区域和灰色道路示出。
黄色三角形的边界与障碍车辆矩形的顶点相交，黄色三角形的边界表示为：
d
(
k
)
−
d
o
l
a
+
∣
s
(
k
)
−
s
o
(
k
)
∣
ρ
p
=
1
\frac{d(k)-d_{ol}}{a}+\frac{|s(k)-s_{o}(k)|}{\rho_{p}}=1
ad(k)−dol+ρp∣s(k)−so(k)∣=1
如图所示，当自动驾驶车辆矩形的角接触不安全区域边界时，不安全三角形区域的扩展系数λ最高。利用三角形的相似关系计算最大展开系数 λ m a x λ_{max} λmax
在研究自动驾驶安全的许多工作中，自动驾驶车辆和障碍车辆通常被视为粒子来构建圆形约束。虽然圆形约束保证了自动驾驶车辆的安全性，但是当障碍物是如下图所示的车辆时：
在狭窄道路上存在与道路约束的冲突。一般车辆的长度和宽度通常分别为 L = 4.8 m L =4.8 m L=4.8m和 W = 1.8 m W =1.8 m W=1.8m 左右。一条国家标准道路的宽度通常约为 W r = 3.75 m W_r =3.75 m Wr=3.75m。为克服圆形约束与道路约束之间的冲突，设计了三角形避碰约束，提高了安全性
3、自适应人工势场函数(Adaptive APF Function)
利用障碍物和道路结构的势函数，由有源滤波器产生势场。自动驾驶车辆被目标车道吸引，并且在车道变换中被障碍物车辆排斥以避免碰撞。障碍车辆对自主车辆的排斥力随着两车纵向距离的减小而增大，以确保平稳的变道路径;自主车辆对目标车道的吸引力随着纵向距离的减小而增大。自适应 APF 函数系数设计为
H
(
k
)
=
{
H
max
1
+
e
(
s
(
k
)
−
s
o
(
k
)
)
2
,
ρ
(
k
)
≤
ρ
p
0
,
ρ
(
k
)
>
ρ
p
H(k)=\begin{cases}\frac{H_{\max}}{1+e^{(s(k)-s_o(k))^2}},& \rho(k)\leq\rho_p\\0,& \rho(k)>\rho_p\end{cases}
H(k)={1+e(s(k)−so(k))2Hmax,0,ρ(k)≤ρpρ(k)>ρp 人工势场系数由自主车辆和障碍物车辆之间的纵向距离确定。因此，具有来自障碍物车辆的排斥场和来自目标车道的重力场的自适应 APF 函数被设计为
O
(
e
(
k
)
)
=
∣
∣
d
r
−
e
y
(
k
)
∣
∣
H
(
k
)
2
O\left( e\left( k \right) \right) =||d_r-e_y\left( k \right) ||_{H\left( k \right)}^{2}
O(e(k))=∣∣dr−ey(k)∣∣H(k)2
如图所示，APF 函数随着颜色的亮度而增加。通过引入势场，APF 方法使用引力和排斥力来描述目标路径和障碍车辆。虽然通过 APF 方法可以生成无障碍路径，但是难以保证无障碍路径的可行性。
4、优化问题
设设为
Ω
f
Ω_f
Ωf。设输入约束集为
U
=
{
u
(
k
)
∣
∣
u
(
k
)
∣
≤
u
m
a
x
}
U=\left\{ u(k)|\ |u(k)|≤u_{max} \right\}
U={u(k)∣ ∣u(k)∣≤umax}，其中
u
m
a
x
u_max
umax表示最大控制输入。设状态约束集为
Ω
=
{
e
(
k
)
∣
∣
e
y
(
k
)
∣
≤
E
y
,
∣
e
(
k
)
∣
≤
E
}
\varOmega =\left\{ e\left( k \right) |\ |e_y\left( k \right) |≤E_y,|e\left( k \right) |≤E \right\}
Ω={e(k)∣ ∣ey(k)∣≤Ey,∣e(k)∣≤E}，其中
E
y
E_y
Ey和
E
E
E 分别是最大横向和偏航角误差。对于每个时刻
k
k
k，优化问题被公式化为
min
U
(
k
)
J
(
e
(
k
)
,
u
(
k
)
)
s
.
t
.
e
(
k
∣
k
)
=
e
(
k
)
e
(
k
+
i
+
1
∣
k
)
=
A
(
k
+
i
)
e
(
k
+
i
∣
k
)
+
B
u
(
k
+
i
∣
k
)
e
(
k
+
i
∣
k
)
∈
Ω
u
(
k
+
i
∣
k
)
∈
U
,
e
(
k
+
N
p
∣
k
)
∈
Ω
f
s
o
(
k
+
i
+
1
∣
k
)
=
s
o
(
k
+
i
∣
k
)
+
v
o
(
k
+
i
)
T
d
o
(
k
+
i
+
1
∣
k
)
=
d
o
(
k
+
i
∣
k
)
e
y
(
k
+
i
∣
k
)
≥
c
i
(
V
r
(
k
)
,
s
(
k
∣
k
)
,
s
o
(
k
)
,
v
o
)
\min_{U(k)} J(e(k),u(k))\\s.t. \ \ \ \ \ \begin{aligned} \\ &e(k|k)=e(k)\\ &e(k+i+1|k)=A(k+i)e(k+i|k)+Bu(k+i|k) \\ &e(k+i|k)\in\Omega \\ &u(k+i|k)\in\mathbb{U},\\ &e(k+N_{p}|k)\in\Omega_{f} \\ &s_{o}(k+i+1|k)=s_{o}(k+i|k)+v_{o}(k+i)T \\ &d_{o}(k+i+1|k)=d_{o}(k+i|k) \\ &e_{y}(k+i|k)\geq c_{i}(V_{r}(k),s(k|k),s_{o}(k),v_{o}) \end{aligned}
U(k)minJ(e(k),u(k))s.t. e(k∣k)=e(k)e(k+i+1∣k)=A(k+i)e(k+i∣k)+Bu(k+i∣k)e(k+i∣k)∈Ωu(k+i∣k)∈U,e(k+Np∣k)∈Ωfso(k+i+1∣k)=so(k+i∣k)+vo(k+i)Tdo(k+i+1∣k)=do(k+i∣k)ey(k+i∣k)≥ci(Vr(k),s(k∣k),so(k),vo) MPC 代价函数为：
J
(
e
∗
(
k
)
,
u
∗
(
k
)
)
=
∑
i
=
1
N
p
−
1
(
N
(
e
∗
(
k
+
i
∣
k
)
,
u
∗
(
k
+
i
∣
k
)
)
)
+
∑
i
=
1
N
p
−
1
O
(
e
∗
(
k
+
i
∣
k
)
)
+
g
(
e
∗
(
k
+
N
p
∣
k
)
)
\begin{aligned}J(e^{*}(k),u^{*}(k))&=\sum_{i=1}^{Np-1}(N(e^{*}(k+i|k),u^{*}(k+i|k)))\\&+\sum_{i=1}^{Np-1}O(e^{*}(k+i|k))+g(e^{*}(k+N_{p}|k))\end{aligned}
J(e∗(k),u∗(k))=i=1∑Np−1(N(e∗(k+i∣k),u∗(k+i∣k)))+i=1∑Np−1O(e∗(k+i∣k))+g(e∗(k+Np∣k))
N
(
e
(
k
+
i
∣
k
)
,
u
(
k
+
i
∣
k
)
)
=
∣
∣
e
(
k
+
i
∣
k
)
∣
∣
Q
2
+
∣
∣
u
(
k
+
i
∣
k
)
∣
∣
R
2
O
(
e
(
k
+
i
∣
k
)
)
=
∣
∣
d
r
−
e
y
(
k
+
i
∣
k
)
∣
∣
H
(
k
+
i
∣
k
)
2
g
(
e
(
k
+
N
p
∣
k
)
)
=
∣
∣
e
(
k
+
N
p
∣
k
)
∣
∣
P
2
\begin{aligned}N(e(k+i|k),u(k+i|k))&=||e(k+i|k)||_{Q}^{2}+||u(k+i|k)||_{R}^{2}\\O(e(k+i|k))&=||d_r-e_y(k+i|k)||_{H(k+i|k)}^2\\g(e(k+N_p|k))&=||e(k+N_p|k)||_P^2\end{aligned}
N(e(k+i∣k),u(k+i∣k))O(e(k+i∣k))g(e(k+Np∣k))=∣∣e(k+i∣k)∣∣Q2+∣∣u(k+i∣k)∣∣R2=∣∣dr−ey(k+i∣k)∣∣H(k+i∣k)2=∣∣e(k+Np∣k)∣∣P2
其中， N ( e ( k + i ∣ k ) , u ( k + i ∣ k ) ) N(e(k+i|k),u(k+i|k)) N(e(k+i∣k),u(k+i∣k)) 为标称代函数， O ( e ( k + i ∣ k ) ) O(e(k+i|k)) O(e(k+i∣k)) 为自适应 APF 代价函数， g ( e ( k + N p ∣ k ) ) g(e(k+N_p|k)) g(e(k+Np∣k)) 为终端代价函数。
5、终端集合
四、可行性和稳定性结果
略
五、实验结果
1、实验平台
由于类车移动的机器人和自主车辆具有相同的运动学模型，类车移动的机器人被用来验证所提出的MPC与自适应APF策略。
图示为汽车式移动的机器人的实验平台。该实验平台主要包括两个阿克曼转向和两个动力轮的机电框架、一个超宽带定位系统、两个惯性测量单元、两个微控制器芯片、两个微型计算机和一个 PC 计算单元。自主车辆和障碍车辆的位置通过UWB 定位系统获得。UWB 定位系统由四个基站和两个标签组成。通过无线载波通信技术获得标签与四个基站之间的相对位置。自主车和障碍车的姿态由地磁传感器模块（JY 901）获取。自主车通过基于 TCP/IP 的主题通信机制获取障碍车的姿态和位置。由于微型计算机的计算能力有限，建议 MPC 与自适应 APF 部署在PC计算单元与 Intel i5 CPU 上。
在固定采样时间 T = 0.05 s T =0.05 s T=0.05s 时，利用内点优化算法（IPOPT）得到了精确的结果。位置、姿态信息和最优控制信号通过无线WiFi网络在自主车辆和PC计算单元之间传输。所提出的用于路径跟踪和防撞系统的具有自适应APF策略的MPC的参数选择在表中。为了实现路径跟踪和防撞，在超车和会车条件下， d r d_r dr 分别设置为 0.60 m 0.60 m 0.60m 和 − 0.60 m -0.60 m −0.60m。预测范围 N p N_p Np 为 30 30 30。根据（4）和（5）， a a a 和最大膨胀系数 λ m a x λ_{max} λmax计算为 0.355 0.355 0.355 和 1.854 1.854 1.854。膨胀系数 λ λ λ 设定为 1.5 1.5 1.5，
2、超车
验证所提出的 MPC 和自适应 APF 策略在路径跟踪和避碰方面的有效性。图中提供了超车实验场地。
在超车场景中，自动驾驶车辆以
0.2
m
/
s
0.2 m/s
0.2m/s 的速度沿着当前直线车道的中心线行驶，而障碍车辆被设置为以
0.1
m
/
s
0.1 m/s
0.1m/s 的速度在同一方向上在同一车道上行驶。自主车辆通过向左成功地改变车道来避开障碍车辆
展示了自动驾驶车辆的具有碰撞避免结果的路径跟踪。
自主车辆的参考路径和实际路径分别由红色实线和蓝色实线表示。三个蓝色空心圆圈和三个绿色空心圆圈代表自主车辆
3、会车
会车实验现场如图所示：
路径跟踪和碰撞避免结果：
本文提出的 MPC 与自适应 APF 和三角形约束，实现了平滑的车道变换和碰撞避免。三角形约束和圆形约束如图所示。由于圆形约束相对保守，自动驾驶车辆在超车过程中通常会违反圆形约束或道路约束。
4、对比实验
六、结论
- 本文提出了一种具有自适应APF策略的MPC，实现了自主车辆在多约束条件下的路径跟踪和避碰。
- MPC方法采用自适应的APF代价函数保证变道路径的平滑性，并根据自主车辆和障碍车辆的大小设计了三角形避碰约束。
- 对于变纵向速度闭环系统，证明了MPC与自适应APF的递推可行性和实际稳定性。
实验结果表明，基于自适应APF的MPC在自主车辆路径跟踪和避碰方面的有效性和优越性。
参考文献
后记：
🌟 感谢您耐心阅读这篇关于 基于MPC和自适应APF的自主车辆避碰 的技术博客。 📚
🎯 如果您觉得这篇博客对您有所帮助，请不要吝啬您的点赞和评论 📢
🌟您的支持是我继续创作的动力。同时，别忘了收藏本篇博客，以便日后随时查阅。🚀
🚗 让我们一起期待更多的技术分享，共同探索移动机器人的无限可能！💡
🎭感谢您的支持与关注，让我们一起在知识的海洋中砥砺前行 🚀
2734
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
