基于车辆运动学模型和滑模控制的横向控制算法设计

1. 车辆运动学模型选择

采用以后轴为中心的自行车模型（简化二维平面运动）：

$$\{\begin{array}{l}\dot{x}=v\cdot \cos (\psi )\\ \dot{y}=v\cdot \sin (\psi )\\ \dot{\psi }=\frac{v}{L}\cdot \tan (\delta )\end{array}$$

• (x,y)：车辆后轴中心坐标
• ($\psi$)：航向角
• (v)：纵向速度（假设恒定）
• (L)：轴距
• ($\delta$)：前轮转向角（控制输入）

2. 横向误差定义

• 航向误差：($e_{\psi }$ = ${\psi }_{ref}$ - $\psi$)
• 横向位置误差：($e_y$ = $y_{ref}$ - $y$)
• 误差动态方程：(${\dot{e}}_y$ = $v\cdot \sin (e_{\psi })$)

3. 滑模面设计

选择包含误差和积分项的滑模面：
$$s=e_y+\lambda \int e_{y}dt+k\cdot e_{\psi }$$

• ($\lambda$, $k$)：权重系数，调节动态响应

4. 控制律推导

等效控制：使滑模面导数为零
$$\dot{s}={\dot{e}}_y+\lambda e_y+k{\dot{e}}_{\psi }=0$$
代入动态方程后解得：
$${\delta }_{eq}=\arctan \left(\frac{L}{v}\cdot \left(\frac{\lambda e_y+k\cdot {\dot{\psi }}_{ref}}{v}\right)\right)$$
期望航向角变化率由路径曲率 $\kappa$决定，即${\dot{\psi }}_{ref}=v\kappa$

切换控制：补偿模型不确定性和扰动
$${\delta }_{sw}=-K\cdot \text{sat}(s/\Phi )$$

• ($K$)：切换增益
• ($\Phi$)：边界层厚度
• ($sat$)：饱和函数（减少抖振）

总控制量：
$$\delta ={\delta }_{eq}+{\delta }_{sw}$$

---

C++代码实现框架

#include <cmath>
#include <vector>

// 车辆状态结构体
struct VehicleState {
    double x;     // X坐标
    double y;     // Y坐标
    double psi;   // 航向角
    double v;     // 速度
};

// 参考路径点结构体
struct ReferencePoint {
    double x_ref; // 参考X坐标
    double y_ref; // 参考Y坐标
    double psi_ref; // 参考航向角
    double kappa_ref; // 参考曲率
};

class SlidingModeController {
private:
    double L;          // 轴距
    double lambda;     // 积分项权重
    double k;          // 航向误差权重
    double K;          // 切换增益
    double phi;        // 边界层厚度
    double integral_ey; // 横向误差积分

public:
    SlidingModeController(double wheelbase, double lambda_coeff, 
                          double k_coeff, double K_gain, double boundary) 
        : L(wheelbase), lambda(lambda_coeff), k(k_coeff), 
          K(K_gain), phi(boundary), integral_ey(0.0) {}

    // 计算前轮转向角
    double computeSteeringAngle(const VehicleState& current_state, 
                               const ReferencePoint& ref_point, 
                               double dt) {
        // 计算横向误差和航向误差
        double e_y = ref_point.y_ref - ref_point.y;
        double e_psi = ref_point.psi_ref - current_state.psi;
        
        // 更新误差积分
        integral_ey += e_y * dt;

        // 计算滑模面
        double s = e_y + lambda * integral_ey + k * e_psi;

        // 等效控制项（需根据模型推导具体形式）
        double psi_ref_dot = current_state.v * ref_point.kappa_ref;
        double delta_eq = atan((L / current_state.v) * 
                              (lambda * e_y + k * psi_ref_dot) / current_state.v);

        // 切换控制项
        double delta_sw = -K * saturation(s / phi);

        // 总控制量
        double delta = delta_eq + delta_sw;
        return delta;
    }

private:
    // 饱和函数代替符号函数减少抖振
    double saturation(double value) {
        if (value > 1.0) return 1.0;
        if (value < -1.0) return -1.0;
        return value;
    }
};

// 示例用法
int main() {
    // 初始化控制器参数
    SlidingModeController controller(2.8, 0.5, 0.3, 1.2, 0.1);

    // 当前车辆状态
    VehicleState state{0.0, 0.0, 0.0, 10.0}; // x=0, y=0, psi=0, v=10m/s

    // 参考路径点（假设由规划模块提供）
    ReferencePoint ref{5.0, 1.0, 0.1}; // x_ref=5, y_ref=1, psi_ref=0.1rad

    // 控制周期（例如0.02秒）
    double dt = 0.02;

    // 计算转向角指令
    double delta = controller.computeSteeringAngle(state, ref, dt);
    // 输出delta给执行机构
    return 0;
}

---

关键实现细节说明

模块	实现说明
误差计算	横向误差(e_y)通过当前车辆位置与参考路径差值计算，航向误差(e_{\psi})同理
积分项处理	使用欧拉法进行离散积分，避免累计误差溢出
饱和函数	用tanh或分段线性函数替代符号函数，减少高频抖振
参数调试	通过仿真调整(\lambda, k, K, \Phi)平衡响应速度与稳定性
参考路径接口	实际需实现轨迹插值或预瞄点搜索（如Pure Pursuit逻辑）

---

扩展优化方向

1. 动态调整边界层：根据车速自适应调整(\Phi)，高速时增大减少抖动。
2. 前馈补偿：加入路径曲率前馈项提升跟踪精度。
3. 抗积分饱和：限制积分项幅值，避免长时间误差累积。
4. 联合仿真验证：通过CarSim/Simulink验证算法鲁棒性。