多体系统听起来像是个纯理论的名词我第一次接触它也不是在课本上而是在一次机器人联调现场。当时机械臂末端一直在抖怎么调PID都压不住最后发现是运动学解算里把两个相邻关节坐标系的旋转顺序写反了计算出的末端位姿经过坐标变换后在仿真里显示正常实际落到真机上就差了几个毫米。那次之后我彻底明白了一个道理多体系统里所有看似高深的动力学推导、控制算法根基都是坐标系描述。坐标系没拧对后面全是白搭。这篇文章我想把多体系统里最基础、也最容易忽视的一块内容梳理清楚——坐标系到底怎么建、怎么描述、怎么在不同刚体之间做变换。适合刚入门的同学也适合做了一段时间仿真和算法、想回头补基本功的人。读完你会对自由度的含义、旋转矩阵和四元数的取舍、惯性张量为什么要定义在体坐标系里有根深的理解后面再去啃多体动力学方程就不会觉得是在看天书。1. 先把“多体系统”这个词拆开看1.1 什么才算多体系统和单体区别在哪多体系统的字面意思很清楚多个物体组成的系统。但工程上说的多体系统通常更严格——这些物体被近似为刚体也就是在外力作用下形状不变而且物体之间通过运动副铰链、滑轨、球铰、万向节等相互连接。你可以想象一台挖掘机。动臂和大臂之间是转动副大臂和铲斗之间有连杆机构底盘和履带之间还有滑动关系。如果只看单独一个臂那就是一个刚体姿态用三个旋转角就能描述但把液压缸、各节臂、铲斗串成一条运动链之后各刚体之间就有约束关系动臂一动铲斗的姿态就得通过坐标变换一路传下去。这就不是“单个刚体”能解决的问题了。和单个刚体相比多体系统有几个本质变化每个刚体都有自己的位置和姿态必须用各自独立的坐标系去描述。刚体之间存在约束这些约束会减少系统的独立变量个数。要计算某个刚体的速度、加速度不能只看它自己还要考虑上游刚体的运动传递。动力学方程往往是高度耦合的一个关节的驱动力矩会通过耦合项影响其他关节。所以多体系统分析的第一步就是先把“每个刚体处于什么位置、什么姿态”这件事用数学语言表达清楚。这一步做不对后面的速度和力分析全是错的。1.2 自由度和位形空间理解系统状态的第一把钥匙自由度这个概念值得多说两句因为很多人把它和“能动的方向数”混在一起。严格来说一个系统的自由度是确定系统位形所需的最少独立坐标数。一个自由刚体在三维空间里有6个自由度3个平动沿x、y、z加3个转动绕x、y、z。但两个刚体用铰链连起来后如果铰链只允许绕一个轴转动那么第二个刚体相对第一个刚体就少了2个旋转自由度整个系统的自由度就是6第一个刚体 1相对转动 7而不是12。这个概念的作用在于定状态变量的个数。做动力学仿真时状态向量得和自由度数目匹配少了方程解不出来多了又会引入冗余约束数值求解时容易出问题。位形空间就是所有可能位形组成的集合。一个刚体的位形空间可以写成旋转部分和位置部分的组合数学上记作SE(3)意思是“刚体运动群”。SE(3)里面有6个独立参数但表达方式千变万化——可以用齐次矩阵可以用一个位置向量加一个旋转矩阵也可以用一组欧拉角加位置。选择哪种表达方式直接影响后续方程的复杂度和数值稳定性这一点后面会展开。1.3 建模时的坐标选择绝对坐标还是相对坐标坐标系选择不只是描述问题还决定了整个建模路线的走向。多体系统建模里大致有两条路线。第一条是相对坐标法也叫关节坐标法。每个刚体的位形用相对于上一级刚体的坐标来描述比如机械臂里每个关节的角度。优点是变量数目少等于系统自由度天然满足约束缺点是运动学关系嵌套需要逐个传递表达式比较长。这类方法特别适合树状结构的多体系统机器人运动学里基本都走这条路。第二条是绝对坐标法每个刚体都用相对于惯性系的坐标来描述。变量数目多但每个刚体的位形一目了然。引入约束方程来处理运动副适合闭环机构比如并联机器人、车辆悬架这类系统。缺点是约束方程维数高微分代数方程的数值求解比纯常微分方程麻烦。实际应用中两者经常混着用全局用绝对坐标建立约束局部用相对坐标降维。不管走哪条路核心都是把坐标系描述准确。我个人的经验是动笔推公式之前先明确哪些坐标系是绝对坐标系、哪些是连体坐标系、哪些是中间坐标系否则推到一半就懵了。2. 坐标系的建立方法不是数学作业而是工程约定2.1 惯性系、连体坐标系和关节坐标系怎么区分几乎所有多体系统分析里都会出现三类坐标系。惯性系也叫世界系是整个系统的大地基准。它固定在地面上假设满足牛顿定律的使用前提——在这个坐标系里看物体惯性力才成立。盖楼先打地基惯性系就是这个地基。连体坐标系和刚体固连随刚体一起运动。坐标原点通常取在刚体质心或某个特征点上坐标轴的方向和刚体自身绑定。连体坐标系存在的意义是刚体本身的属性比如惯性张量在连体坐标系下是不变的这样计算起来最省事。关节坐标系则绑定在运动副上。它用来说明两个刚体之间的相对位形。比如转动副的旋转轴通常就是关节坐标系的某个轴。不同刚体之间通过坐标变换关联起来关节坐标系就是这个关联的枢纽。画图的时候惯用的做法是世界系用W表示World连体坐标系用B表示Body关节坐标系用J表示Joint。符号随手一标但工程上脑子要清楚。2.2 连体坐标系的原点到底放哪有讲究连体坐标系的原点和朝向不是随便定的会影响后面的所有推导。我的建议是如果主要做动力学分析原点放在质心。因为牛顿-欧拉方程、拉格朗日方程里大量用到质心位置惯性张量在质心坐标系下是对角的如果刚体形状规则计算量小很多。如果主要做运动学分析原点放在关节轴线或参考点。机器人运动学里连杆坐标系通常放在关节处让相邻连杆之间的变换矩阵简单直观。两种放置方法可以互相转换靠的是坐标平移和惯性张量的平行轴定理。所以我一般建议在方案设计阶段就定好哪个坐标系负责运动学传递哪个坐标系负责动力学解算中间再加一个“质心坐标系”作为中转桥梁。没必要所有坐标系都挤在一起。2.3 建系之后第一件事命名规范这个细节看似不起眼但做过多体仿真的人都懂它的分量。坐标系多了之后p\left(1\right)和p\left(2\right)这种写法很快就会让人抓狂。我的做法是所有量都用“坐标 上标源坐标系 下标目标坐标系”的格式。比如p_{BC}^{W}表示点BC在世界坐标系W下的位置R_{WB}表示从世界系W到连体系B的旋转矩阵。这里的下标“WB”可以理解为“把世界系旋转到连体系”。群里看到过不少因为命名混乱导致的低级错误两个人各算各的坐标变换结果一个用R_{WB}一个用R_{BW}乘出来结果差一个转置。只要约定统一这类问题在代码里一眼就能发现。3. 姿态描述工具怎么选旋转矩阵、欧拉角与四元数3.1 旋转矩阵的优点和代价旋转矩阵是一个3乘3的正交矩阵行列式为1。它完整描述了一个坐标系相对另一个坐标系的旋转没有奇异性这是它最大的优点。任意两次旋转相叠加就是两个矩阵相乘逻辑非常直接。想要把体坐标系下的向量转换到世界系乘一个旋转矩阵就行。但旋转矩阵也有硬伤9个元素里只有3个独立量存储和计算开销大而且直接看起来非常不直观。如果你在调试程序时盯着一个旋转矩阵想“当前机械臂到底是抬头还是低头”大概率会疯掉。所以旋转矩阵适合作为计算内核不适合作为人机交互的描述方式。3.2 欧拉角直观但万向锁是避不开的坑欧拉角用三个角来描述旋转比如先绕z轴转航向角再绕新y轴转俯仰角最后绕新x轴转滚转角。这种描述非常符合人体直觉航空、车辆、机器人领域广泛使用。但欧拉角有个致命问题万向锁。当中间那个旋转角到90度时第一个和第三个旋转轴会重合系统丢失一个自由度表现为姿态解算出现奇异性某些角速度组合会导致数值爆炸。万向锁不是场景偶然触发而是欧拉角这种三参数描述的固有缺陷只要用欧拉角就躲不开。实际工程里怎么处理一个办法是精心定义欧拉角的旋转顺序保证常用工作区间避开奇异点另一个办法是尽量只在界面显示时用欧拉角内部计算用四元数或旋转矩阵。3.3 四元数计算和插值的均衡解四元数用四个数描述旋转一个标量加一个三维向量模长恒为1。它的好处首先是没奇异其次存储比旋转矩阵小组合两次旋转也比矩阵乘少几次运算。更重要的是四元数做插值非常自然。比如要生成一条从姿态A平滑过渡到姿态B的路径四元数做球面线性插值slerp得到的结果是几何上最短的旋转路径姿态过渡均匀。换成欧拉角做线性插值角速度经常忽快忽慢中间还会出现奇怪的扭转。多体仿真软件里关节电机轨迹规划经常用四元数插值。我早期用欧拉角做过一次末端姿态的圆弧插补结果姿态在路径中点附近扭了一下之后全部改成四元数才解决。3.4 不同场景的工具选型建议每种姿态描述方式都有适合的场景整理成表格方便对照描述方式参数个数优势劣势典型用途旋转矩阵DCM9个实际3个独立无奇异组合旋转方便存储多、不直观内部计算、坐标变换欧拉角3个直观易懂有万向锁、插值难人机交互显示、简化分析四元数4个带模长约束无奇异、插值平滑、运算量小不直观需归一化控制、仿真、姿态解算旋转矢量3个无冗余、小角度近似简单大角度时表达式复杂误差分析、小角度修正没有一种工具是万能的。我的习惯是内部统一用四元数或旋转矩阵对外输出用欧拉角并且明确标出旋转顺序。4. 位姿描述与坐标变换齐次矩阵是绕不开的枢纽4.1 一次变换搞定旋转加平移单靠旋转矩阵只能描述转动描述姿态还得再配上位置。把旋转和平移装进一个4乘4的齐次变换矩阵在很多工程场景下是最顺手的表达方式。齐次矩阵的结构是左上角一个旋转矩阵右上角一个三维平移向量最后一行是0001。这样写的好处是位置向量补一个1变成齐次坐标之后一次矩阵乘法就能完成旋转加平移多个刚体链式传递时只需要连乘多个齐次矩阵。比如从世界系到末端执行器系的变换T T_{01}·T_{12}·T_{23}直接把各个关节的局部变换乘起来末端位姿就出来了。这正是机械臂正运动学的基本套路。4.2 矩阵相乘顺序大多数人栽过的跟头齐次矩阵乘法不满足交换律顺序不同结果天差地别。问题是本来就容易搞混因为既要把向量变到新系又要描述坐标系之间的级联关系。我常用的判断方法如果矩阵T_{AB}表示“从A系到B系的变换”那要把A系中的点p^A变到B系中就直接左乘T_{BA}。如果要对同一个刚体先做一次变换再做另一次变换比如先绕当前坐标轴旋转再平移那么矩阵乘的顺序必须和变换执行的顺序一致。换句话说得问清楚“每次旋转是在哪个坐标系里发生的”——是一个固定的世界系还是一个随刚体运动的局部系。固定系下做变换是左乘局部系下做变换是右乘。这个区别困惑了很久之后我给团队定了一条规矩任何写坐标变换的地方必须注释中写明是“相对固定系”还是“相对当前局部系”杜绝只写一句“这里做个旋转变换”的做法。5. 关键物理量怎么在坐标系之间切换5.1 惯性张量为什么必须待在连体坐标系里惯性张量是描述刚体质量分布的量是一个3乘3矩阵。它对体坐标轴的选择非常敏感坐标系平移或者旋转惯性张量都会变化。绝大多数情况下动力学方程用惯性张量最顺手的坐标系是连体坐标系。因为刚体在运动过程中连体坐标系和刚体没有相对运动惯性张量在连体系下是常数。如果非要在惯性系下用那惯性张量会随着刚体旋转不停变化每个时间步都要做相似变换计算量大且容易出错。把惯性张量从质心坐标系搬到任意参考点的操作叫平行轴定理。实际使用三维CAD软件导出的质量属性时经常遇到导出结果的原点不在质心的情况这个定理就派上用场了。我遇到过好多次模型导出的惯性张量看着没问题一加进来就发散排查半天发现参考点给错了。5.2 角速度和线速度在不同坐标系中的表示速度也一样同一条机械臂关节空间里的角速度和笛卡尔空间里的末端速度需要的坐标系完全不同。角速度通常是相对于连体坐标系更容易计算因为我们关心的旋转轴往往就定义在关节坐标系里。线速度则要看具体需求正运动学里经常关心末端在世界系里的位置导数。不同坐标系下的速度变换本质上还是那几个旋转矩阵在起作用。关键是搞清楚速度向量是在哪个坐标系里表达的。工程上很容易在这一点上栽跟头比如仿真出的角速度明明符合预期控制器的角速度反馈却反向往往就是没有做坐标系变换。6. 实操中避不开的三个坑6.1 旋转顺序和参考系约定不一致场景A模块的姿态接口定义的是Z-Y-X欧拉角B模块为了省事用了X-Y-Z顺序上位机界面显示正常但数据一对接末端轨迹立刻发散。排查方法其实很简单——先在离线环境下让两个模块执行同一组标准旋转比如绕x轴旋转90度对比各自输出的中间结果就能马上看出是谁的顺序约定不对。提醒一点任何多体系统交互一定要在接口文档里白纸黑字写清楚欧拉角顺序、参考坐标系、旋转是绕固定轴还是绕动轴。不要嫌繁琐。6.2 欧拉角直接做插值导致姿态扭曲做抓取动作规划时机械臂末端要从姿态A平滑转到姿态B。起初直接用欧拉角做线性插值结果中间出现了明显的非预期摇摆。换成四元数slerp插值后姿态过渡就非常平滑了。如果项目里已经用了欧拉角接口也不想大改结构退而求其次的办法是先把欧拉角转成四元数插值完再转回来同样能规避大部分问题。6.3 惯性张量参考系不统一造成控制发散有一次做某个多体平台的动力学仿真模型加进去之后系统一直震荡发散。后来把CAD导出的惯性张量参考点从模型原点改到质心用平行轴定理重算之后仿真立刻稳定。这个问题在刚接触仿真的人身上特别常见也是我每次检查模型时都会优先排查的项目。问题现象常见原因排查思路仿真姿态剧烈跳变欧拉角序列或旋转矩阵转置错误用单位旋转测试逐步核对变换矩阵运动轨迹存在扭摆欧拉角插值导致路径非最短改用四元数slerp插值动力学仿真相继发散惯性张量参考点不在质心核对CAD导出参数用平行轴定理修正多模块结果不一致命名规范和坐标系定义不统一建立坐标系和姿态约定的文档统一接口这几类问题我基本都遇到过而且每一项排查起来都不轻松。如果从一开始就按规范建系、按约定写变换、按统一格式定义姿态描述能省掉一大半麻烦。小结之外的一点体会写了这么多绕不开一个核心多体系统的基础就是坐标系描述。每一项动力学方程、每一次运动学解算本质上都是在各个坐标系之间做变换。坐标选择合理、变换准确、量纲一致后面的路会好走很多前面欠下的债迟早要在调试时加倍偿还。这也是这个系列的第001篇我特意把坐标系放在最前面讲因为后面的内容——运动学递推、动力学方程、控制器设计——都建立在这套描述方式之上。如果你正打算开始做多体仿真或者机器人算法开发建议先花时间把今天这部分内容彻底想明白亲手推一遍简单的两连杆转动模型把坐标变换、姿态描述在纸上和代码里各过一遍效果比看十篇文章都好。