阿克曼机器人运动控制与代码实现

阿克曼小车：从转向原理到运动学模型详解

---

一、阿克曼转向几何：为什么汽车转弯时内外轮角度不同？

1.1 核心原理：让车轮轨迹成为同心圆

阿克曼转向几何的本质是确保车辆转弯时，所有车轮的延长线交于同一点（即瞬时转向中心，ICR）。这类似于用圆规画圆时，所有笔尖必须指向圆心。若内外轮转角相同，车轮轨迹会偏离同心圆，导致轮胎侧滑（如图1）。

图1：阿克曼转向避免侧滑的原理

数学关系推导

假设轴距为 L ，轮距为 W ，转弯半径为 R ，内外轮转角分别为 $$\alpha$$ 和 $$\beta$$，则有：
$$\{\begin{array}{l}\tan \alpha =\frac{L}{R-K/2}\\ \tan \beta =\frac{L}{R+K/2}\end{array}$$
由公式可得：外侧轮转角 $$\beta$$ 必须小于内侧轮转角 $$\alpha$$，且两者满足关系：
$$\cot \beta -\cot \alpha =\frac{K}{L}$$
这一公式表明转向机构的核心：通过机械连杆或电子控制，强制内外轮转角遵循此规律。

---

1.2 实例

参数定义

符号	物理意义	典型值（家用轿车）
$$L$$	轴距（前后轮距离）	2.5米
$$W$$	轮距（左右轮距离）	1.6米
$$K$$	转向主销中心距	0.3米

生活中的阿克曼转向

以停车场直角转弯为例：

• 现象：方向盘左打时，左前轮明显比右前轮“转得更急”。
• 原理：若左前轮转角为 $$25^{\circ }$$，右前轮可能仅 $$20^{\circ }$$，确保四轮轨迹交于ICR点，避免轮胎横向摩擦。

---

二、逆运动学模型：如何控制小车转弯？（原理→举例）

2.1 模型原理：从整体运动到轮子动作

输入：车辆速度 $$v$$ 和角速度 $$\omega$$
输出：左右轮速度 $$v_L,v_R$$ 和前轮转角 $$\alpha$$
核心公式：

1. 转弯半径 $$R=\frac{v}{\omega }$$
2. 内侧轮转角 $$\alpha ={\tan }^{-1}\left(\frac{L}{R-K/2}\right)$$
3. 左右轮速度差 $$v_R-v_L=\omega \cdot W$$

---

2.2 举例：低速直角转弯控制

场景：小车以 $$v=0.5\text{m/s}$$、角速度 $$\omega =0.1\text{rad/s}$$ 左转，参数 $$L=2\text{m},W=1.2\text{m},K=0.2\text{m}$$。

分步计算

1. 计算转弯半径：
   $$R=\frac{0.5}{0.1}=5\text{m}$$
2. 计算左前轮转角：
   $$\alpha ={\tan }^{-1}\left(\frac{2}{5-0.1}\right)\approx 21.8^{\circ }$$
3. 计算右前轮转角：
   由 $$\cot \beta -\cot 21.8^{\circ }=\frac{0.2}{2}$$，解得 $$\beta \approx 17.5^{\circ }$$
4. 计算左右轮速度：
   $$\{\begin{array}{l}{v}_L=0.5-0.1\times 0.6=0.44\text{m/s}\\ v_R=0.5+0.1\times 0.6=0.56\text{m/s}\end{array}$$

结果验证：

• 速度差 $$0.56-0.44=0.12\text{m/s}$$，与公式 $$\omega \cdot W=0.1\times 1.2=0.12\text{m/s}$$ 一致。
• 前轮转角差异 $$21.8^{\circ }-17.5^{\circ }=4.3^{\circ }$$，符合阿克曼几何约束。

---

三、正运动学模型：如何通过轮速推算车辆运动？（原理→举例）

3.1 模型原理：从轮子动作到整体运动

输入：左右轮速度 $$v_L,v_R$$ 和前轮转角 $$\alpha$$
输出：车辆速度 $$v$$ 和角速度 $$\omega$$
核心公式：

1. 车辆速度 $$v=\frac{v_L+v_R}{2}$$
2. 角速度 $$\omega =\frac{v_R-v_L}{W}$$ 或 $$\omega =\frac{v}{\frac{L}{\tan \alpha }+K/2}$$

---

3.2 举例：小车的运动状态反推

场景：左轮速度 $$v_L=0.3\text{m/s}$$，右轮速度 $$v_R=0.5\text{m/s}$$，左前轮转角 $$\alpha =15^{\circ }$$，参数 $$L=1.5\text{m},W=1.0\text{m},K=0.1\text{m}$$。

分步计算

1. 计算车辆速度：
   $$v=\frac{0.3+0.5}{2}=0.4\text{m/s}$$
2. 方法一：通过轮速差计算角速度：
   $$\omega =\frac{0.5-0.3}{1.0}=0.2\text{rad/s}$$
3. 方法二：通过转向角计算角速度：
   转弯半径 $$R=\frac{1.5}{\tan 15^{\circ }}+0.05\approx 5.8\text{m}$$
   $$\omega =\frac{0.4}{5.8}\approx 0.069\text{rad/s}$$

---

四、实际应用与拓展思考

4.1 自动驾驶中的路径跟踪

• 控制逻辑：根据目标路径计算所需 $$v$$ 和 $$\omega$$，通过逆运动学模型驱动车轮。
• 难点：动态环境中需实时调整参数，例如紧急避障时瞬间增大 $$\omega$$。

4.2 四轮独立转向

• 原理：取消阿克曼约束，四轮同向偏转，实现横向平移。
• 运动学变化：
  • 前进速度 $$v=\frac{v_L+v_R}{2}$$（与阿克曼相同）
  • 横向速度 $$v_{\text{lat}}=\frac{v_F+v_B}{2}\cdot \sin \theta$$
  • （$$\theta$$ 为转向角）

---

五、代码实现：

 void AckermannInverseKinematics(float linear_velocity, float steering_angle_rad) 
{
    // 核心目的：将线速度与转向角度转换为舵机控制信号和轮速，实现阿克曼转向模型
    // --- 校准阶段：获取舵机中位偏差值，补偿硬件安装误差 ---
    // 通过电位器读取当前舵机中位偏差（例如ADC采样值），并缩放为实际角度偏差
    // 公式：校准值 = -原始电位器读数 / 缩放系数
    Akm_Servo.Bias = -get_DMA_ServoBias() / 5.0f;

    // --- 阿克曼转向参数定义 ---
    float turn_radius = 0;      // 当前转弯半径（单位：米）
    float inner_wheel_angle = 0; // 内侧前轮转向角度（单位：弧度）

    // 将输入参数 steering_angle_rad 解析为内侧前轮转向角度
    // 注：此处代码假设输入 steering_angle_rad 为右前轮角度，但命名为 inner_wheel_angle 以统一模型
    inner_wheel_angle = steering_angle_rad;

    // --- 转向角度限幅：防止机械结构过载 ---
    const float max_steering_angle = 30.0f; // 最大允许转向角度（单位：度）
    
    // 计算左转极限时的最小转弯半径（基于阿克曼几何公式）
    // 公式：最小转弯半径 = 轴距 / tan(最大转向角) + 半轮距
    float min_turn_radius = robot.HardwareParam.AxleSpacing / tan(degrees_to_radians(max_steering_angle)) 
                          + 0.5f * robot.HardwareParam.WheelSpacing;
    
    // 计算右转极限时的最小内侧轮转向角（负号表示右转方向）
    // 公式：最小右转角度 = arctan(轴距 / ( -最小转弯半径 - 半轮距 ))
    float min_right_steering_angle = atan(
        robot.HardwareParam.AxleSpacing / (-min_turn_radius - 0.5f * robot.HardwareParam.WheelSpacing)
    );

    // 对实际转向角度进行限幅，确保在机械允许范围内
    // 函数 target_limit_float 将角度限制在 [min_right_steering_angle, max_steering_angle_rad] 区间
    inner_wheel_angle = clamp_steering_angle(
        inner_wheel_angle, 
        min_right_steering_angle, 
        degrees_to_radians(max_steering_angle)
    );

    // --- 计算转弯半径（基于阿克曼几何） ---
    // 公式：转弯半径 = 轴距 / tan(内侧轮转向角) + 半轮距
    if (inner_wheel_angle != 0) {
        turn_radius = robot.HardwareParam.AxleSpacing / tan(inner_wheel_angle) 
                    + 0.5f * robot.HardwareParam.WheelSpacing;
    }

    //  逆运动学计算：根据转弯半径和线速度求解轮速
    if (inner_wheel_angle != 0) {
        // 内侧轮速度公式：V_inner = Vx * (R - 半轮距) / R
        robot.LEFT_WHEEL.Target = linear_velocity 
                                * (turn_radius - 0.5f * robot.HardwareParam.WheelSpacing) 
                                / turn_radius;
        
        // 外侧轮速度公式：V_outer = Vx * (R + 半轮距) / R
        robot.RIGHT_WHEEL.Target = linear_velocity 
                                 * (turn_radius + 0.5f * robot.HardwareParam.WheelSpacing) 
                                 / turn_radius;
    } else {
        // 直行时左右轮速度相等
        robot.LEFT_WHEEL.Target = linear_velocity;
        robot.RIGHT_WHEEL.Target = linear_velocity;
    }

    // --- 舵机控制信号生成：转向角度 → PWM值（多项式拟合非线性映射） ---
    // 使用三次多项式拟合转向角度与舵机PWM的关系，补偿机械非线性误差
    // 实验标定系数：PWM = a + bθ + cθ² + dθ³
    robot.SERVO.Target = 7.75f 
                       + 850.0f * inner_wheel_angle 
                       - 396.0f * pow(inner_wheel_angle, 2) 
                       - 76.2f * pow(inner_wheel_angle, 3);

    // --- 舵机最终输出：叠加中位值、校准偏差和计算值 ---
    // 公式：最终PWM = 理论中位值 + 校准偏差 + 非线性补偿值
    robot.SERVO.Output = Akm_Servo.Mid    // 舵机理论中位PWM（如1500μs）
                       + Akm_Servo.Bias   // 校准值（补偿安装误差）
                       + robot.SERVO.Target; // 非线性补偿后的目标PWM
}