004 坐标系与刚体运动基础
从一次电机堵转说起
去年调试一台四轮差速底盘，电机编码器读数突然跳变，上位机显示机器人原地转圈，实际却纹丝不动。排查三天，最后发现是IMU坐标系定义和电机编码器坐标系差了90度——我定义X轴朝前，电机驱动库默认X轴朝右。这种“坐标系打架”的问题，在运动控制里比算法本身更致命。
刚体运动的基础，说白了就是三件事：你在哪、你朝哪、你怎么动。坐标系是描述这些问题的语言，语言不通，算法再漂亮也是废纸。
坐标系：别让“右手定则”坑了你
嵌入式里最常见的坐标系就三种：世界坐标系（惯性系）、机体坐标系、传感器坐标系。世界坐标系固定在大地上，通常用W表示，Z轴朝天。机体坐标系跟着机器人跑，用B表示，原点在质心或几何中心。
这里有个血泪教训：ROS里规定前为X、左为Y、上为Z（右手系），但很多电机驱动库默认前为Y。如果你混用，姿态解算出来的角度会像喝醉了一样乱跳。我的习惯是：所有传感器数据进MCU之前，先统一到机体坐标系，用矩阵乘法硬转，别指望后续算法去兼容。
右手定则很简单：拇指X，食指Y，中指Z。但注意——右手系和左手系不能混用。有些IMU芯片（比如老款MPU6050某些固件版本）默认是左手系，你按右手系算欧拉角，结果就是万向锁提前出现。怎么查？看数据手册里的坐标轴定义图，别信网上现成的库。
旋转矩阵：数学很美，嵌入式很痛
刚体旋转用旋转矩阵R表示，3x3矩阵，正交且行列式为1。从B系到W系的旋转矩阵记作R_WB，含义是“把B系下的向量转到W系”。
实际写代码时，千万别直接存9个浮点数。STM32的FPU算3x3矩阵乘法一次要几十个周期，如果控制频率1kHz，光旋转矩阵运算就能吃掉20%的CPU。更优的做法是用四元数，4个浮点数，没有万向锁，插值还平滑。
但四元数也有坑：归一化。每次更新完四元数，必须做归一化，否则数值误差积累会让旋转矩阵不再正交，姿态会漂。我见过有人用互补滤波更新四元数后忘了归一化，无人机飞了30秒就开始侧翻。代码里加一行：
// 归一化四元数，不归一化会飘到姥姥家floatnorm=sqrt(q0*q0+q1*q1+q2*q2+q3*q3);q0/=norm;q1/=norm;q2/=norm;q3/=norm;欧拉角：直观但危险
欧拉角（横滚roll、俯仰pitch、偏航yaw）最直观，但万向锁是绕不开的坑。当pitch接近±90度时，roll和yaw的旋转轴重合，丢失一个自由度。在嵌入式里，如果你用欧拉角做姿态控制，必须限制pitch范围，或者检测到接近90度时切换控制策略。
别这样写：直接拿欧拉角做PID的输入。因为欧拉角不是向量，不能直接加减。正确的做法是：把期望姿态和当前姿态的误差转换成旋转向量（轴角形式），再映射到控制量。或者用四元数误差，更稳定。
平移与旋转的耦合：齐次变换矩阵
刚体运动是平移+旋转，分开处理容易乱。齐次变换矩阵T是4x4矩阵，把旋转和平移打包在一起。形式是：
T = [R t] [0 1]其中R是3x3旋转矩阵，t是3x1平移向量。这个矩阵的好处是：连续变换可以连乘。比如从世界系到相机系，再相机系到机械臂末端，直接T_world_to_end = T_world_to_camera * T_camera_to_end。
嵌入式实现时注意：4x4矩阵乘法比3x3更费资源。如果只是纯旋转或纯平移，别用齐次矩阵，分开算。只有需要链式变换时才用齐次矩阵，而且尽量在PC端预计算好参数，MCU只做查表和简单插值。
刚体运动学：速度与角速度
刚体运动的速度分为线速度v和角速度ω。线速度是位置对时间的导数，角速度是姿态对时间的导数。但注意：角速度不是欧拉角的导数。欧拉角对时间求导得到的是欧拉角速率，和角速度之间有个非线性变换矩阵。
这个变换矩阵在pitch接近90度时会奇异（又是万向锁）。所以实际控制中，直接用角速度做反馈，别用欧拉角速率。IMU输出的就是角速度，直接拿来用，省去转换的麻烦。
经验之谈
坐标系定义写在代码最前面，用宏定义或枚举，注释里画个ASCII图。我见过一个项目，三个人用了三种坐标系定义，最后联调时炸了三天。
所有旋转相关的计算，统一用四元数。只在人机交互时转成欧拉角显示。这样既避免万向锁，又减少计算量。
调试时打印原始传感器数据，别只打印解算后的姿态。有一次IMU的Z轴陀螺仪坏了，输出一直为零，但姿态解算算法用加速度计补偿了，导致偏航角缓慢漂移。如果只看姿态，根本发现不了。
坐标系转换矩阵写单元测试。用几个已知向量（比如[1,0,0]转到[0,1,0]）验证，确保矩阵没写反。我习惯在初始化时做一次自检，如果转换结果不对，直接报错停机。
别迷信“通用库”。很多开源运动学库为了通用性，做了大量动态内存分配和虚函数调用，在实时性要求高的嵌入式系统里就是灾难。自己写一个精简版，只保留需要的坐标系和变换，性能能提升30%以上。
刚体运动基础看起来是数学，实际是工程。坐标系定义错了，后面所有算法都是错的。旋转矩阵算慢了，控制周期就上不去。这些坑，踩过一次就记住了。
