004 坐标系与刚体运动基础

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秒就开始侧翻。代码里加一行// 归一化四元数不归一化会飘到姥姥家floatnormsqrt(q0*q0q1*q1q2*q2q3*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%以上。刚体运动基础看起来是数学实际是工程。坐标系定义错了后面所有算法都是错的。旋转矩阵算慢了控制周期就上不去。这些坑踩过一次就记住了。