自动驾驶.motion 圆周运动仿真

## 0\. 实验信息

• 来源：《自动驾驶与机器人中的 SLAM 技术：从理论到实践》第二章 motion.cpp

• 实验目的：

  1. 理解车体坐标系 / 世界坐标系速度、角速度坐标变换

  2. 理解 SO3 李群指数映射（解析精确解）

  3. 理解四元数一阶近似更新的原理与固有截断误差

  4. 理解圆周运动 $r=\frac{v}{\omega }$ 的物理关系

  5. 区分平移积分与姿态更新两套独立运算

• 运行环境：slam_in_autonomous_driving 工程，Pangolin 可视化、glog/gflags、Eigen

• 基础公式回顾

  1. 速度坐标变换：${\mathbf{v}}_w=R_{wb}{\mathbf{v}}_b$

  2. 平移欧拉积分：${\mathbf{p}}_{k+1}=p_k+v_w\cdot \Delta t$

  3. SO3 姿态更新：$R_{k+1}=R_k\cdot Exp(\mathbf{\omega }\Delta t)$，解析解（罗德里格斯）

  4. 四元数一阶近似：$q_{k+1}\approx q_k\otimes \left[1,\frac{1}{2}\mathbf{\omega }\Delta t\right]$，泰勒一阶截断，仅小角度准确

  5. 圆周运动半径：$r=\frac{v}{\omega }$

• 基础启动命令

# 默认基准：10deg/s角速度，5m/s线速度，SO3指数映射
./bin/motion

完整代码（详细注释）

#include <gflags/gflags.h>
#include <glog/logging.h>

#include "common/eigen_types.h"
#include "common/math_utils.h"
#include "tools/ui/pangolin_window.h"

/// 本节程序演示一个正在作圆周运动的车辆
/// 车辆的角速度与线速度可以在flags中设置

// ==================== 命令行参数定义 ====================
// 可在运行时通过 --angular_velocity=20 来修改
DEFINE_double(angular_velocity, 10.0, "角速度（角度）制");           // 默认 10 度/秒
DEFINE_double(linear_velocity, 5.0, "车辆前进线速度 m/s");            // 默认 5 米/秒
DEFINE_bool(use_quaternion, false, "是否使用四元数计算");              // 切换旋转更新方式

int main(int argc, char** argv) {
    // ==================== 初始化 ====================
    google::InitGoogleLogging(argv[0]);          // 初始化 glog 日志库
    FLAGS_stderrthreshold = google::INFO;        // 设置日志输出级别（INFO 及以上输出到终端）
    FLAGS_colorlogtostderr = true;               // 终端日志带颜色
    google::ParseCommandLineFlags(&argc, &argv, true);  // 解析命令行参数

    /// 可视化
    sad::ui::PangolinWindow ui;                  // 创建 Pangolin 可视化窗口对象
    if (ui.Init() == false) {                    // 初始化窗口（后台启动渲染线程）
        return -1;
    }

    // ==================== 状态初始化 ====================
    double angular_velocity_rad = FLAGS_angular_velocity * sad::math::kDEG2RAD;  // 角速度：度→弧度
    SE3 pose;                                                                    // TWB 表示的位姿（初始为单位阵）
    Vec3d omega(0, 0, angular_velocity_rad);     // 角速度矢量 [ωx, ωy, ωz] = [0, 0, 10°/s]，绕 Z 轴旋转
    Vec3d v_body(FLAGS_linear_velocity, 0, 0);   // 车辆本体系（body）下的速度：[前进, 侧向, 垂向] = [5, 0, 0]
    const double dt = 0.05;                      // 每次更新步长 0.05 秒（20 Hz）

    // ==================== 主循环 ====================
    while (ui.ShouldQuit() == false) {           // 循环直到用户关闭窗口

        // ---------- 1. 更新位置 ----------
        Vec3d v_world = pose.so3() * v_body;     // 将 body 系速度变换到世界（world）系：v_w = R_wb * v_b
        pose.translation() += v_world * dt;       // 位置积分：t = t + v_w * Δt

        // ---------- 2. 更新旋转（两种方式）----------
        if (FLAGS_use_quaternion) {
            // 方式一：四元数更新（一阶近似）
            // 四元数微分方程：q(t+Δt) ≈ q(t) ⊗ [1, 0.5ωΔt]
            // 其中 ω = [ωx, ωy, ωz] 是角速度矢量
            Quatd q = pose.unit_quaternion() * Quatd(1, 0.5 * omega[0] * dt, 0.5 * omega[1] * dt, 0.5 * omega[2] * dt);
            q.normalize();                        // 四元数需要归一化保持单位长度
            pose.so3() = SO3(q);                  // 更新旋转部分
        } else {
            // 方式二：SO3 指数映射（李代数 → 李群）
            // 旋转矩阵的指数更新：R(t+Δt) = R(t) * exp(ωΔt)^
            // 其中 exp(ωΔt)^ 是李代数 so3 到李群 SO3 的指数映射
            pose.so3() = pose.so3() * SO3::exp(omega * dt);
        }

        // 将当前位姿打印到终端（便于调试观察）
        LOG(INFO) << "pose: " << pose.translation().transpose();

        // 将导航状态（时间戳、位姿、速度）发送给 UI 线程显示
        ui.UpdateNavState(sad::NavStated(0, pose, v_world));

        // 睡眠 0.05 秒，控制循环频率为 20 Hz
        usleep(dt * 1e6);
    }

    // ==================== 退出清理 ====================
    ui.Quit();    // 等待渲染线程退出，释放资源
    return 0;
}

1. 核心代码解析

// 状态初始化
SE3 pose;                          // T_wb 世界到车体位姿，初始单位阵
Vec3d omega(0, 0, angular_velocity_rad); // 车体Z轴角速度
Vec3d v_body(FLAGS_linear_velocity, 0, 0); // 车体X向前速度
const double dt = 0.05;            // 仿真步长20Hz

while (!ui.ShouldQuit()) {
    // ----------平移更新----------
    Vec3d v_world = pose.so3() * v_body;     // v_w = R_wb * v_b 坐标变换
    pose.translation() += v_world * dt;      // 欧拉积分更新位置

    // ----------姿态更新两种方式----------
    if (FLAGS_use_quaternion) {
        // 四元数一阶近似更新
        Quatd q = pose.unit_quaternion() * Quatd(1,
                0.5*omega[0]*dt,0.5*omega[1]*dt,0.5*omega[2]*dt);
        q.normalize();      // 仅修复模长，不能修复角度截断误差
        pose.so3() = SO3(q);
    } else {
        // SO3指数映射，罗德里格斯解析解，无截断误差
        pose.so3() = pose.so3() * SO3::exp(omega * dt);
    }
    ui.UpdateNavState(sad::NavStated(0, pose, v_world));
    usleep(dt * 1e6);
}

关键代码要点

1. v_world = pose.so3() * v_body：必须把车体速度旋转到世界坐标系再积分，不能直接累加v_body。

2. SO3::exp：李代数→李群，任意旋转增量都是精确解析解。

3. 四元数0.5系数不可丢；normalize()只保证四元数模 = 1，不能消除泰勒一阶截断带来的角度误差。

4. 姿态更新、位置更新是两套相互独立的计算。

2. PangolinWindow 可视化界面说明

窗口分为三部分：左侧控制面板、中间 3D 视图、右侧时序曲线图。

2.1 左侧控制面板

选项	功能说明
Follow 勾选框	勾选：镜头跟随小车；取消：固定视角，观察完整轨迹
Reset 3D View	重置 3D 相机视角，场景丢失时复位
Set to front View	切换到小车车头第一人称视角

**鼠标操作：左键拖拽旋转视角；滚轮缩放；右键平移场景。

2.2 中间 3D 视图

红色线条：小车运动轨迹；小车模型为载体。

2.3 右侧 4 条时序曲线（颜色：红‑X，绿‑Y，紫‑Z）

曲线标签	物理含义
ba_x / ba_y / ba_z	**车体坐标系角速度 **${\omega }_b$
bg_x / bg_y / bg_z	**世界坐标系角速度 **${\omega }_w$
vel_x / vel_y / vel_z	**世界坐标系速度 **$v_w$
baselink_vel_x/y/z	**车体本体系速度 **$v_b$

核心现象记忆：

• 本体系速度、本体系角速度，参数给定后恒定不变，对应曲线是水平直线；

• 世界坐标系下速度因姿态旋转，呈现正弦 / 余弦周期振荡。

如何理解上述结论呢？
(1)、先区分两个坐标系

• 车体坐标系（body 本体系）：绑定在小车上，跟着车一起转。原点在小车中心，X 永远指向小车车头前方。

• 世界坐标系（world 全局）：固定不动，相当于地面，坐标轴永远不变。

baselink_vel、ba：车体坐标系下的物理量
vel、bg：世界坐标系下的物理量
(2)、本体系速度 baselink_vel（曲线是水平直线）

代码：

Vec3d v_body(FLAGS_linear_velocity, 0, 0);

• v_body = [5, 0, 0]，含义：站在小车自己的视角看，小车永远往自己车头 X 方向跑，侧向、上下没有速度。

• 不管小车怎么转圈、车身朝向怎么改变，小车看自己永远向前跑。

• 所以不管仿真跑多久，baselink_vel_x始终等于 5，y、z 始终等于 0 → 在曲线图上就是一条水平直线，不会波动。

角速度ba（车体角速度）同理：
omega(0,0,ω_z)，代表站在车上看，车一直绕自己 Z 轴以恒定速度转。车身朝向怎么变，车体角速度始终不变，所以ba_z也是一条水平线。

(3)、世界坐标系速度 vel（呈现正弦、余弦振荡曲线）

公式：
${\mathbf{v}}_w=R_{wb}\cdot {\mathbf{v}}_b$
$R_{wb}$：把车体向量变换到世界坐标系的旋转矩阵，随着小车转圈，$R_{wb}$一直在随时间变化。

举一个简单数值例子：

• 小车本体系速度 $v_b=[5,0,0]$

• t 时刻小车绕 Z 转过角度 $\theta (t)$

✅ 所以：

• $v_{wx}=5\cos \theta (t)$

• $v_{wy}=5\sin \theta (t)$

$\theta (t)$随时间持续增大，cos、sin 就是周期振荡函数。
因此曲线图上，vel_x、vel_y就是一条余弦、一条正弦，相位差 90°，上下周期性波动。

物理人话翻译

小车车头一会指向世界 X 方向，一会指向 Y 方向，一会指向‑X 方向。
同样是 “向前 5m/s”，车头朝向世界哪个方向，世界系的速度就往哪个方向。
车头不停转圈，世界下的速度方向就跟着不停转圈 → 表现为 X、Y 分量周期性变大变小，就是正弦余弦波形。

(4)、核心对比总结表格

物理量	本体系 (body)	世界坐标系 (world)
速度来源	小车自身的运动设定	经过旋转矩阵变换之后得到
数值变化	恒定不变，水平直线	随姿态旋转，正弦 / 余弦周期性振荡
直观理解	站在车上看自己，一直往前跑	站在地面看车，速度方向随车头转向不停变化

(5)、关键易错点

1. ❌误区：“小车速度大小在变化”

✔真相：速度的模长大小不变，只有方向在变。 $||v_w||=||v_b||=5m/s$，只是分解到世界 X、Y 坐标轴上的分量在忽大忽小。
圆周运动就是典型：速率不变，速度方向时刻改变。

2. ❌误区：车体速度会随着车身旋转自动改变

✔真相：本体系速度是相对车身，车身怎么转，它相对于车身始终向前，数值不会自己变；必须乘旋转矩阵才会体现出世界下的方向变化。

(6)、联系实验现象记忆

• 实验 3：angular_velocity=0，小车不旋转，$R_{wb}$恒等于单位矩阵。
  此时 $v_w=v_b$，世界速度曲线也变成水平线，不再出现正弦振荡。

只有姿态持续旋转的时候，才会产生振荡曲线。

---

3. 对照实验记录

每组记录：实验目的、运行命令、参数修改、物理含义、现象、验证结论；

实验 1：增大角速度，减小转弯半径

实验目的：验证圆周运动公式 $r=\frac{v}{\omega }$；理解车体坐标系角速度特性。

终端指令：

./bin/motion --angular_velocity=20 --linear_velocity=5

• 修改：angular_velocity：10 →20 °/s；线速度保持 5m/s。

• 物理含义：车辆前进速度不变，绕车体 Z 轴旋转变快，等效方向盘打得更死。

• 界面现象
  

  1. 3D 视图：红色轨迹圆环半径显著变小，小车转圈频率变高。

  2. 曲线图：ba_z水平线数值翻倍；baselink_vel_x保持 5 不变；vel_x/vel_y正弦曲线周期变短，振幅不变。

• ✅验证结论

  1. 线速度固定，角速度越大，圆周运动半径越小。

  2. 车体坐标系角速度由参数给定，不受车辆姿态变化影响。

  3. 世界坐标系速度大小等于本体速度大小，方向随姿态周期性变化。

实验 2：增大线速度，增大转弯半径

实验目的：验证圆周运动公式 $r=\frac{v}{\omega }$；验证坐标变换 $v_w=R_{wb}{v}_b$。
终端指令：

./bin/motion --angular_velocity=10 --linear_velocity=10

• 修改：linear_velocity：5→10 m/s；角速度保持 10°/s。

• 物理含义：方向盘转角不变，车辆行驶速度加快。

• 界面现象
  

  1. 3D 视图：轨迹圆环半径变大。

  2. 曲线图：ba_z高度不变；baselink_vel_x抬高到 10；vel_x/vel_y正弦曲线振幅增大，振荡周期不变。

• ✅验证结论

  1. 角速度固定，线速度越大转弯半径越大。

  2. 本体速度放大，投影到世界坐标系的速度幅值同步放大。

实验 3：角速度置 0，车辆直线运动

实验目的：理解坐标系变换；当$R_{wb}=I$，$v_w=v_b$。
终端指令：

./bin/motion --angular_velocity=0 --linear_velocity=5

• 修改：角速度设置 0 度每秒。

• 物理含义：方向盘回正，车体不旋转。

• 界面现象
  

  1. 3D 视图：红色轨迹是一条直线，不再形成圆环。

  2. 曲线图：ba_z=0；baselink_vel_x=5；vel_x=5恒定，vel_y≈0，不再出现正弦振荡。

• ✅验证结论

  1. $\omega =0$，姿态矩阵$R_{wb}$恒等于单位矩阵。

  2. 车体坐标系与世界坐标系对齐，车体速度直接等于世界坐标系速度。

  3. 只有姿态持续旋转，才会将恒定本体速度投影为世界下的周期变化速度。

实验 4：高角速度，SO3 解析解 vs 四元数一阶近似（核心对比实验）

实验目的：体会四元数一阶近似的截断误差；理解 SO3 指数映射是解析解。

4‑A SO3 基准（无近似误差）

./bin/motion --angular_velocity=60 --linear_velocity=5 --use_quaternion=false

• 现象：轨迹完美闭合正圆，长时间运行无漂移。

4‑B 开启四元数一阶近似

./bin/motion --angular_velocity=60 --linear_velocity=5 --use_quaternion=true

• 修改：开启四元数更新，大角速度 60°/s。

• 物理含义：每一步增量旋转角度大，放大一阶泰勒截断误差。

• 界面现象：3D 轨迹无法闭合，跑一段时间变成螺旋漂移；即使代码执行q.normalize()，漂移依旧存在。
  

• ✅验证结论

1. SO3::exp()罗德里格斯公式是解析解，任意旋转增量精确，无截断误差。
2. $q\approx q\otimes [1,0.5\omega \Delta t]$是一阶泰勒近似，仅适合单步小角度旋转。
3. normalize()只能保证四元数模长为 1，不能消除角度本身的截断误差。

补充对照：角速度调小--angular_velocity=5 --use_quaternion=true，单步角度很小，漂移几乎观察不到。

实验 5：线速度为 0，原地自转

实验目的：证明旋转、平移计算互相独立。

./bin/motion --angular_velocity=30 --linear_velocity=0

• 修改：线速度置 0，角速度 30°/s。

• 物理含义：车辆原地只自转，不发生位移。

• 界面现象
  

  1. 3D 视图：小车在原点原地旋转，轨迹只有一个点。

  2. 曲线图：baselink_vel_x=0；vel_x,vel_y全部为 0；ba_z保持恒定水平线。

• ✅验证结论

  1. 姿态更新、位置积分是两套独立运算，可以只旋转不产生位移。

  2. $v_b=0$，则世界速度$v_w$恒等于 0，位置不会发生变化。

实验 6：修改代码仿真步长 dt（需要修改源码重新编译）

实验目的：观察步长对近似误差的影响。

//原 const double dt = 0.05;
const double dt = 0.2; //大时间步长5Hz

分别测试use_quaternion=true与false。

• 现象：
  

  • SO3 模式：无论 dt 多大，轨迹都是完美圆形；

  • 四元数模式：dt 越大，单步旋转角度越大，漂移螺旋现象越严重。

• ✅验证结论

四元数一阶近似误差来源于单步旋转增量$\omega \Delta t$的大小；减小 dt 可以抑制误差，但不能完全消除；SO3 指数映射不受步长影响。

4. 整体总结与易错点汇总

4.1 核心结论

1. 圆周运动半径满足 $r=\frac{v}{\omega }$；线速度、角速度分别定义在车体坐标系。

2. 坐标变换：车体速度必须左乘旋转矩阵转换到世界坐标系，再做位置积分。

3. SO3::exp 指数映射是解析解，任意旋转增量都精确。

4. 四元数 [1,0.5ωΔt]一阶近似只适合小增量；归一化≠消除角度截断误差。

5. 姿态更新和平移积分相互独立，可以单独旋转、单独平移。

4.2 高频踩坑清单

1. ❌直接拿车体速度v_body去做世界位置积分，忘记乘旋转矩阵。

为什么：必须把车体速度转到世界系再积分，不能直接累加 v_body？

1. 先分清两个速度的本质

• v_body（车体坐标系速度） 永远是 固定 (5, 0, 0) 含义：车自己看自己，永远向前直走，不管车身转成什么角度。
• v_world（世界坐标系速度） 是 随车身角度实时变化 的速度 是地面、全局视角下的真实运动方向。

2. 积分的本质：只能在同一个坐标系累加位置

位置 pose.translation() 是 世界坐标系下的坐标。

数学规则：

世界坐标的更新，只能用世界坐标系的速度积分

公式：

(P_{world} += V_{world} \cdot dt)

绝对不能：

(P_{world} += V_{body} \cdot dt \quad \text{（完全错误）})

3. 为什么直接加 v_body 会彻底错？

举个直观例子：

1. 车子右转 90 度
2. 此时车身朝前是 世界 Y 轴方向
3. 但 v_body 仍然是 (5,0,0)（车体 X）

如果你直接累加 v_body：

• 程序会让车继续往世界 X 方向跑
• 但现实车已经朝向 Y 方向，应该往 Y 跑

结果：轨迹完全错乱，不会出现圆，完全不符合物理运动。

4. 为什么乘旋转矩阵 R_wb 就对了？

v_world = R_wb * v_body;

作用：
把 “车身前方” 这个方向，映射到当前世界坐标系的真实方向

车身每转一点，R_wb 就变一点 → v_world 方向自动跟着偏转 → 积分出来就是完美圆周运动

5. 一句话终极总结

• v_body 是相对车身的，会随车身转动而变方向。
• 位置是世界坐标，绝对不能用相对速度累加。
• 必须通过旋转矩阵，把车体速度矫正到世界方向，才能正确积分。

3. ❌四元数更新丢掉 0.5 系数，角速度效果翻倍。

4. ❌误以为 q.normalize () 可以修复角度近似误差；它只修复模长。

5. ❌混淆车体坐标系速度和世界坐标系速度。

6. ❌gflags 输入角度制，忘记转为弧度送入数学运算。

5. 课后思考题

1. 四元数代码已经调用q.normalize()，为什么大角速度依然会轨迹漂移？

答：normalize 只保证四元数模等于 1，解决数值漂移带来模长偏离 1；不能补偿泰勒一阶截断带来的角度本身误差。

2. 如果 IMU 采样频率很低，dt 很大，还能不能直接用 $q\leftarrow q\otimes [1,0.5\omega dt]$？

答：不适合；应当使用完整的四元数指数映射，而不是一阶近似。

---

结语

关于本次的学习分享就到此结束了，小小扫地僧觉得这次的笔记非常重要，希望有心的读者一定要反复学习。

如果需要相关学习资料的小伙伴可以私信我，我会将相关的书籍和详细代码工程分享给你。