2023年12月 — 2024年06月
面向安全的智能小车自适应编队控制系统
面向三辆阿克曼智能小车的轨迹跟踪与纵向安全编队，融合双闭环滑模控制、扰动上界鲁棒补偿、控制障碍函数安全过滤、轨迹索引编队决策、俯视视觉定位与 ROS 实车闭环，实现从数学模型到物理验证的一体化控制系统。
Ackermann ModelSliding Mode ControlCBF–QPLeader–FollowerVisual LocalizationMATLAB / ROS
System Architecture
系统总体架构
点击架构图可放大查看
01 · 阿克曼车辆与双闭环误差
以车辆后轴中心描述平面位姿，外环将世界坐标误差旋转到车体坐标系，生成期望线速度与角速度；内环再根据速度误差计算驱动力矩和转向力矩。内环带宽高于外环，使位置跟踪与动力学响应可以分层设计。
\[
\begin{aligned}
\dot x_i &= v_i\cos\theta_i, & \dot y_i &= v_i\sin\theta_i, & \dot\theta_i &= \omega_i,\\
\dot v_i &= \frac{1}{rm}\tau_{vi}, & \dot\omega_i &= -\frac{l}{rI_0}\tau_{\omega i}.
\end{aligned}
\]
\[
\begin{bmatrix}e_x\\e_y\\e_\theta\end{bmatrix}
=
\begin{bmatrix}\cos\theta&\sin\theta&0\\-\sin\theta&\cos\theta&0\\0&0&1\end{bmatrix}
\begin{bmatrix}x_d-x\\y_d-y\\\theta_d-\theta\end{bmatrix}.
\]
02 · 双闭环滑模与稳定性
外环以纵向误差和“航向—横向耦合误差”构造滑模面，饱和函数削弱切换抖振；内环以线速度、角速度误差构造幂次趋近律，在误差较小时仍保持足够的收敛能力。相应 Lyapunov 导数非正，从而保证两级滑模变量稳定收敛。
\[
s_1=e_x,\qquad s_2=e_\theta+\arctan(v_de_y),\qquad
\dot s_i=-\varepsilon_i\operatorname{sat}(s_i)-k_is_i.
\]
\[
V_1=\tfrac12s_1^2+\tfrac12s_2^2,\qquad
\dot V_1=-k_1s_1^2-\varepsilon_1|s_1|-k_2s_2^2-\varepsilon_2|s_2|\le0.
\]
03 · CBF–QP 安全过滤与编队参考
滑模控制负责跟踪性能，倒数控制障碍函数把动态车头时距转化为不可跨越的安全边界。二次规划仅在名义指令可能破坏安全约束时修正驱动力；与此同时，两辆跟随车分别跟踪全局期望轨迹上滞后固定索引的目标点，避免领航车实际误差逐车向后传播。
\[
h=z-d-T_hv_f,\quad T_h=1.8\,\mathrm{s},\qquad
B=-\log\!\left(\frac{h}{1+h}\right).
\]
\[
p_{F_j}^{d}(t)=p_d[i(t)-jn],\qquad j=1,2.
\]
\[
\begin{aligned}
u^\star&=\underset{u}{\arg\min}\;\tfrac12(u-u_{\mathrm{SMC}})^{\mathrm T}H(u-u_{\mathrm{SMC}}),\\
\text{s.t.}\quad L_fB+L_gBu&\le\frac{\gamma}{B},\qquad u_{\min}\le u\le u_{\max}.
\end{aligned}
\]
Robust Control
外部扰动下的鲁棒滑模控制
地面摩擦、电机误差、载荷变化与测量噪声被等效为驱动和转向力矩扰动。控制器不要求实时估计精确扰动，而是利用已知上界加入与滑模变量方向相反的补偿项，以最坏情况裕度维持速度跟踪。截断高斯随机力矩扰动仿真用于对比标称控制器与抗扰控制器。
\[
\begin{aligned}
\tau_v &= rm\dot v_c+\beta_1r\tau_{vd,\max}\operatorname{sgn}(s_3)+\frac{rm}{c_v}k_3|s_3|^{a_3}\operatorname{sgn}(s_3),\\
\tau_\omega &= -\frac{rI_0}{l}\dot\omega_c-\beta_2r\tau_{\omega d,\max}\operatorname{sgn}(s_4)-\frac{rI_0}{c_\omega l}k_4|s_4|^{a_4}\operatorname{sgn}(s_4).
\end{aligned}
\]
Visual Localization
RGB 投影峰值三车定位算法
固定俯视摄像头同时观察红、绿、蓝三组大小圆标识。通道差分抑制白色反光、阴影和地砖纹理，行列投影峰值确定大小圆圆心，颜色区分车辆，大小圆相对方向给出航向角。相比依赖单车里程计，该方法统一了三辆车的坐标系，并避免累计漂移。
\[
R_L(j)=\sum_iR_{\mathrm{bin}}(i,j),\qquad R_H(i)=\sum_jR_{\mathrm{bin}}(i,j),
\]
\[
\theta=\operatorname{atan2}(y_s-y_b,\;x_s-x_b).
\]
3车辆同步识别
≈ 30 Hz定位更新频率
≈ 1.5 cm实测位置误差
2.6 m顶部相机高度
Physical Experiments
单车跟踪与三车编队实验
图片可放大；按钮打开对应实物视频
单车闭合轨迹跟踪
车辆平稳完成直线与圆弧组成的闭合轨迹，位置、航向和速度误差逐步收敛；实测 y 方向最大偏差约为 4 cm，控制信号在直线—曲线切换过程中保持平稳。
一辆领航者和两辆跟随者在初始调整后进入稳定纵向编队。各车独立回归同一条全局期望轨迹，CBF–QP 在接近安全边界时修正纵向驱动力，实验中未出现追尾或间距失控。