4. 运动学解算：前向运动学（轮速->里程计）、逆向运动学（线速度角速度->轮速舵角）

好，咱们今天聊点硬核的——运动学解算。

说白了，就是搞清楚两件事：

• 轮子转了多少，车子到底走了多远？（前向运动学）
• 想让车子怎么走，轮子该怎么转？（逆向运动学）

这两个问题，是阿克曼底盘控制的核心。我刚开始做机器人那会儿，总觉得这玩意儿不就是个公式嘛，套进去就完事了。结果第一次上路测试，里程计漂得跟喝醉了似的……嗯，后来才明白，细节都在坑里。

4.1 前向运动学：从轮速到里程计

前向运动学，说白了就是“猜位置”。

你给电机发了PWM，轮子转了，但车到底走了多远？方向偏没偏？这得靠轮速传感器和运动学模型算出来。

4.1.1 阿克曼底盘的里程计模型

阿克曼底盘跟差速底盘不一样。差速底盘靠左右轮速差转弯，阿克曼底盘靠前轮转向。所以它的里程计模型，得考虑转向角。

我习惯把模型拆成两步：

1. 先算后轴中心的速度——后轮是驱动轮，速度最直接。
2. 再根据转向角推算车体朝向变化——前轮转向决定了车往哪拐。

公式其实不复杂：

// 假设后轮左右轮速分别为 v_left, v_right
// 轮距为 track_width
// 后轴中心线速度 v = (v_left + v_right) / 2
// 后轴中心角速度 omega = (v_right - v_left) / track_width

// 但注意！这是差速底盘的公式。
// 阿克曼底盘，你得用转向角来算角速度。

这里有个坑——阿克曼底盘的角速度，不能直接用轮速差算。因为左右后轮虽然转速可能不同（差速器允许），但它们的转速差是由地面摩擦和转向几何决定的，不是直接控制量。

正确的做法是：

// 前向运动学（阿克曼）
// 输入：后轮平均速度 v_rear, 前轮转向角 steering_angle
// 输出：车体位姿变化 (dx, dy, dtheta)

// 轴距
double wheelbase = 0.5; // 单位：米

// 转弯半径
double turning_radius = wheelbase / tan(steering_angle);

// 角速度
double omega = v_rear / turning_radius;

// 单位时间 dt 内的位姿变化
double dx = v_rear * cos(theta) * dt;
double dy = v_rear * sin(theta) * dt;
double dtheta = omega * dt;

你看，关键是用转向角算转弯半径，再用线速度除以半径得到角速度。这才是阿克曼底盘的正确打开方式。

4.1.2 里程计积分与误差累积

里程计本质上是积分过程。每过一个dt，就把当前速度积分到位移上。但积分嘛，误差会累积。

我个人的经验是：

• 轮速编码器精度：普通霍尔编码器每圈几十个脉冲，误差在5%左右。想做好里程计，至少用每圈500脉冲以上的编码器。
• 转向角传感器：别用电位器，温漂太严重。我建议用磁性角度传感器，比如AS5600，精度0.1度以内。
• 更新频率：里程计更新频率至少50Hz，最好100Hz。频率太低，积分误差会放大。

4.2 逆向运动学：从线速度角速度到轮速舵角

逆向运动学，就是“怎么走”。

你给机器人一个目标速度（比如：向前0.5m/s，同时左转0.2rad/s），底盘得算出：后轮该转多快？前轮该打多少角度？

4.2.1 阿克曼转向的几何约束

阿克曼转向有个核心约束：所有车轮的轴线必须交于一点（瞬时转动中心）。

这个约束决定了内外前轮的转角不一样。内轮转角大，外轮转角小。如果左右前轮打同样的角度，轮胎会侧滑，磨损严重，里程计也会不准。

公式如下：

// 逆向运动学（阿克曼）
// 输入：目标线速度 v, 目标角速度 omega
// 输出：后轮转速 rpm_rear, 左前轮转角 steer_left, 右前轮转角 steer_right

// 1. 计算转弯半径
double turning_radius = v / omega;

// 2. 计算后轮转速（假设后轮半径 r_wheel）
double rpm_rear = v / (2 * PI * r_wheel) * 60; // 转/分钟

// 3. 计算前轮转角（轴距 wheelbase）
// 注意：这里算的是单车模型转角
double steer_center = atan(wheelbase / turning_radius);

// 4. 阿克曼几何修正
// 轮距 track_width
double steer_left = atan(wheelbase / (turning_radius - track_width / 2));
double steer_right = atan(wheelbase / (turning_radius + track_width / 2));

你看，内外轮的转角是不一样的。实际项目中，我会把这两个角度分别发给左右转向舵机。

4.2.2 速度与转向的协调控制

逆向运动学算出来的轮速和转角，不能直接扔给执行器。为什么？

你想想看，如果车子正在高速行驶，突然给一个很大的转角，会怎样？——翻车！

所以，实际控制中需要做速度-转向协调：

• 转角越大，速度上限越低。我习惯用一条曲线来限制：max_speed = f(steering_angle)，转角30度时，速度不超过0.3m/s。
• 加速度限制：轮速和转角的变化率都要限幅。突然打方向会让底盘抖动，里程计也会跳变。
• 死区处理：小转角（比如小于1度）直接归零，避免舵机频繁微调。

4.3 运动学解算的整体流程

我把整个流程画了张图，方便你理解：

这张图把整个运动学解算的脉络理清了。左边是输入，中间是解算模块，右边是输出。注意看，前向和逆向是两条独立的路径，但通过实际执行反馈形成了闭环。

4.4 实战中的调优建议

最后，分享几个我在项目中积累的调优经验：

1. 标定轮速：编码器每圈的脉冲数，跟实际轮子转一圈走的距离，不一定完全匹配。因为轮胎有弹性，负载不同时滚动半径会变。我建议做一次标定：让车子走10米，对比编码器累计值，算出实际每脉冲对应的距离。
2. 标定转向角：舵机中位不一定正好是0度。用角度传感器实测一下，把偏移量补偿掉。否则直线都走不直。
3. 里程计滤波：卡尔曼滤波太复杂？可以先试试简单的滑动平均。窗口大小取5-10个样本，效果立竿见影。
4. 逆向运动学的平滑：目标速度和角速度的变化率要限幅。我一般设最大加速度0.5m/s²，最大角加速度1.0rad/s²。这样底盘动作会很柔和。

好了，运动学解算就聊到这儿。记住，公式是死的，但车是活的。多调试、多观察，你就能找到最适合你底盘的参数。

---