机器人运动控制学习6——状态进阶

上篇文章学习总结Base Position是机器人主体在某个世界/参考坐标系中的位置。Base Velocity 积分可以得到 Base Position但误差也会被一起积累。Odometry 更关注“从起点到现在移动了多少”本质上主要是相对运动估计。Leg Odometry 可以利用编码器、腿部运动学和支撑脚约束来估计 Base 的运动。即使脚不明显打滑模型误差、关节误差、接触误差等仍然会让位置逐渐漂移。Localization 解决“我在地图/世界哪里”通常需要视觉、LiDAR、GNSS、动捕等外部环境参考来长期纠偏。最终要形成IMU Encoder Contact ↓ Local Odometry ↓ 短期连续、但会漂 Vision / LiDAR / GNSS ↓ Global Correction ↓ 更完整的状态估计这节课的学习思路Roll / Pitch 为什么能靠重力纠正 ↓ Yaw 为什么重力帮不上忙 ↓ 陀螺仪积分为什么让 Yaw 越来越漂 ↓ 磁力计能不能救 ↓ 为什么机器人上经常不敢太信磁力计 ↓ 视觉 / LiDAR 怎么帮助修正 Yaw ↓ Yaw 错一点为什么走远以后位置会偏很多 ↓ 这和 VIO / SLAM / Odometry 有什么关系这一课讲完你对IMU姿态估计 → Odometry → Localization这条线就会真正连起来。Base Position ↓ Odometry 里程计 ↓ 为什么机器人走着走着地图位置会漂 ↓ 为什么脚没打滑也可能累计误差 ↓ IMU Leg Odometry 能不能得到绝对位置 ↓ 为什么 GPS / VIO / LiDAR 会进入机器人状态估计 ↓ localization 和 state estimation 到底什么关系link、joint、frame这三个词以后你每天都会见。如果这三个没搞清楚URDF、Pinocchio、TF、MuJoCo都会看得非常乱。今天先不写公式。1. 先把机器人想象成“骨头 关节”比如你的胳膊身体 │ 肩膀 │ 上臂 │ 肘 │ 前臂 │ 手机器人里可以理解成link │ joint │ link │ joint │ link因此最基本的规律是Link 是刚体。Joint 负责连接两个 Link并规定它们怎么相对运动。2. Link 是什么Link 可以先理解成一块不会自己变形的刚体零件。例如人形机器人pelvis torso left_thigh left_shank left_foot right_upper_arm right_forearm right_hand都可以是 Link。比如UpperArm Link它本身的长度 质量 质心 惯量 外形通常是固定的。它不会突然30 cm → 40 cm所以运动学里我们把它当成刚体。3. Joint 是什么Joint 是两个 Link 之间允许发生相对运动的连接。比如upper_arm │ elbow_joint │ forearm肘关节允许前臂相对上臂旋转因此Link 东西本身 Joint 东西之间怎么动这个区别一定要记住。Pose 位置 姿态4. Link、Joint、Frame 三者是什么关系来看一条机器人手臂torso_link │ shoulder_joint │ upper_arm_link │ elbow_joint │ forearm_link │ hand_link但我们还可以在这些 Link 上定义 Frametorso_link [torso_frame] │ shoulder_joint │ upper_arm_link │ elbow_joint │ forearm_link │ hand_link [hand_frame]所以Link 实际刚体 Joint 刚体之间的连接关系 Frame 我们放在某个位置用来描述空间关系的坐标系5. URDF以后你看到 URDFlink nameupper_arm/ joint nameelbow_joint ... /joint link nameforearm/脑子里不要想着XML配置文件好复杂。直接翻译upper_arm 一块骨头 elbow_joint 肘关节 forearm 另一块骨头URDF 本质上就是在告诉计算机机器人有哪些刚体 ↓ 这些刚体怎么连接 ↓ 关节能怎么运动 ↓ 尺寸是多少 ↓ 质量是多少 ↓ 惯量是多少Pinocchio 再读取这个模型。于是URDF ↓ Pinocchio Model ↓ 输入 q ↓ FK ↓ 得到所有 Frame 的 Pose1. Link 刚体 2. Joint 两个刚体之间如何运动 3. Frame 用来描述位置和方向的一套坐标轴 4. Position 只有位置 5. Pose 位置 姿态然后再加一句非常关键的所有位置和姿态都必须问一句相对于哪个 FrameTranslationTranslation 就是两个坐标系的原点相差多少。例如World O \ \ Robot ORobot Base 相对于 Worldx 1 m y 2 m z 0 m那么这个(1,2,0)就是一部分平移关系。以后你会经常看到translation或者Eigen::Vector3d translation;本质就是dx dy dzRotation假设有两个坐标系World y ↑ | O────→ x机器人转了90°Robot x ↑ | O────→ ?机器人自己的x轴已经不再和 World 的 x 轴平行。这时候一个点Robot frame中 (1,0)是什么意思意思是在机器人自己的 x 轴方向前方1米。但由于机器人已经转了90°从 World 看它可能是 (0,1)也就是说Robot里 (1,0) World里 (0,1)同一个点数字不同原因就是坐标轴旋转了假设肩膀 ↓ 上臂 ↓ 肘 ↓ 前臂 ↓ 手肩膀转30°肘相对于肩膀又转60°那么手的位置怎么算不能简单写上臂长度 前臂长度因为前臂已经不是沿原来的方向伸出去。而是Shoulder O / / O Elbow \ \ O Hand因此真正计算过程中会变成Base ↓ 肩膀的平移 旋转 ↓ UpperArm ↓ 肘的平移 旋转 ↓ Forearm ↓ Hand每经过一个 Joint坐标系都可能跟着改变。所以 FK 真正是在干什么上一课我们说FK 已知关节角求手脚在哪里。现在可以再深入一级FK其实是在从机器人根节点开始不断进行坐标变换。比如World ↓ Base ↓ Torso ↓ Shoulder ↓ UpperArm ↓ Elbow ↓ Forearm ↓ Hand程序实际上是在算World → Base Base → Torso Torso → Shoulder Shoulder → UpperArm UpperArm → Elbow Elbow → Forearm Forearm → Hand最后把这些关系串起来World ↓ ↓ ↓ Hand得到Hand相对于World的Pose这就是 FK 的底层结构。T_AB以后运动学里你可能看到T_AB或者^A T_B不同教材符号可能不一样现在不要死背先建立一个意识这种东西通常是在描述“B坐标系相对于A坐标系是什么关系”。比如World T Hand可以理解为Hand相对于World的变换它里面包含Hand在哪里 Hand朝哪里也就是完整 Pose。Noitom / Xsens ↓ Human Hand Pose ↓ Mocap坐标系 ↓ 坐标变换 ↓ Robot Base坐标系 ↓ 缩放 / 身体比例映射 ↓ Robot Target Hand Pose ↓ IK / QP ↓ q_target ↓ MuJoCo这里Mocap frame → Robot frame就是我们现在正在学的东西。“动作镜像”常见原因之一就是坐标系轴定义不同甚至还有左手系 / 右手系的问题。你现在只需要形成一个排错习惯当机器人方向明显不对时不要第一反应就说IK坏了。先检查目标数据到底在哪个Frame里一个空间点 / Pose ↓ 在 Frame A 中描述 ↓ 坐标变换 ↓ 在 Frame B 中重新描述而坐标变换本身Transform │ ├── Translation │ 平移 │ └── Rotation 旋转这就是今天最核心的东西。旋转矩阵旋转矩阵的本质一句话旋转矩阵负责把一个向量从一个方向定义转换到另一个坐标系里。比如Robot坐标系里的向量 ↓ 旋转矩阵 R ↓ World坐标系里的向量可以写成p_world R · p_robot你现在不用害怕这个式子它只是在说“把机器人眼里的坐标翻译成世界眼里的坐标。”二维旋转矩阵如果机器人绕 z 轴旋转角度 θR [ cosθ -sinθ sinθ cosθ ]你现在不要背。我们只拿θ 90°来看。因为cos90° 0 sin90° 1于是R [ 0 -1 1 0 ]现在机器人中的“前方1米”p_robot [1 0]乘起来p_world [0 1]也就是Robot 前方1米 World y方向1米和刚才直觉完全一致。列这是今天最值得你理解的地方刚才R [ 0 -1 1 0 ]看第一列[0 1]它表示Robot 的 x 轴在 World 坐标系里是什么方向。结果是World (0,1)说明 Robot x 朝 World y再看第二列[-1 0]它表示Robot 的 y 轴在 World 坐标系里是什么方向。也就是 Robot y 朝 World 的负 x。所以你以后看到旋转矩阵R [ | | | x y z | | | ]可以理解为三列分别告诉你新坐标系 x、y、z 三根轴在旧坐标系里长什么样。这是非常强的直觉。现实机器人是三维的一个 Frame 有x轴 y轴 z轴所以旋转矩阵变成3 × 3形式R [ r11 r12 r13 r21 r22 r23 r31 r32 r33 ]你暂时不用理解9个数字各自怎么计算先知道第一列 新坐标系x轴在旧坐标系中的方向 第二列 新坐标系y轴在旧坐标系中的方向 第三列 新坐标系z轴在旧坐标系中的方向为什么旋转矩阵里每一列长度都是1因为它们表示的是坐标轴方向方向不关心长度。例如 x 轴方向(1,0,0)长度就是1。旋转以后可能变成(0.707, 0.707, 0)它的长度仍然是1。所以旋转矩阵的每一列都是单位向量。而且三根轴互相垂直。所以x ⟂ y y ⟂ z x ⟂ z这就是为什么旋转矩阵不是随便一个 3×3 矩阵。放到机器人手臂上看假设上臂长度0.3 m在上臂自己的坐标系里它可能沿 x 轴[0.3 0 0]肩膀转了90°以后R · [0.3 0 0]可能变成[0 0.3 0]这就是说上臂长度没变只是方向变了。FK里干的事情本质就是不断做这种变换。一个关节接一个关节假设肩膀旋转30°再肘旋转60°那手的方向不是只看其中一个。而是肩膀旋转 ↓ 肘坐标系已经跟着变了 ↓ 再在肘坐标系里旋转所以最终会出现R_total R_shoulder · R_elbow你现在不用管乘法顺序细节。只要知道多个旋转可以通过矩阵乘法串起来。这就是为什么机器人 FK 里面到处都是矩阵乘法。旋转矩阵和欧拉角你可能已经见过roll pitch yaw也就是RPY它们是一种“描述旋转的方法”旋转矩阵也是一种比如yaw 90°最终可以转换成一个3×3 Rotation Matrix所以Euler Angle / RPY ↓ 可以转换 ↓ Rotation Matrix但它们不是同一个东西。四元数你以后还会看到quaternion例如x y z w它也是描述旋转的方法所以目前先建立旋转的表示方法 ├─ Rotation Matrix ├─ Euler Angle / RPY └─ Quaternion它们都在描述一个坐标系相对于另一个坐标系转了多少。只是表达方式不同。齐次变换矩阵因为现在我们已经有Rotation但还缺Translation下一步要把旋转 平移合成一个统一的4×4 Transform Matrix这一步之后你就真正能开始看懂 FK 里面最常见的T矩阵了。p_world R · p_base t你可以直接翻译机器人眼里的点 ↓ 先根据机器人朝向旋转 ↓ 再加上机器人在世界中的位置 ↓ 得到世界眼里的点所以R 方向关系 t 位置关系两个加起来就能完整描述两个 Frame 的关系。为什么还需要“齐次变换矩阵”既然已经有R · p t为什么还要搞一个 4×4 矩阵因为我们希望旋转和平移都能统一写成一次矩阵乘法。这样以后多个关节连起来就特别方便。于是我们把R和t塞进一个矩阵里。得到T [ R t 0 1 ]展开就是T [ r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz 0 0 0 1 ]这就是4×4 齐次变换矩阵真实机器人一般同时有旋转和平移比如肩关节坐标系相对于 torso位置 右边 0.2m 上方 0.3m 方向 绕某轴旋转那么完整关系就由T_torso_shoulder描述。里面同时存着shoulder在哪里 shoulder坐标轴朝哪里所以一个 T 实际上就是一个完整 Pose。Pose 和 Transform 的关系之前说Pose Position Orientation而齐次变换矩阵T也包含Translation Rotation所以在很多机器人库里Pose和Transform表达的是非常接近的空间关系只是数据结构和语义可能略有差异。例如你以后在 ROS2 里会见到geometry_msgs/Pose也会见到geometry_msgs/TransformPinocchio里则可能看到SE3本质都在处理三维位置 三维姿态。假设一个两关节机械臂Base ↓ Joint1 ↓ Link1 ↓ Joint2 ↓ Link2 ↓ Hand每一段都可以有一个变换矩阵T_base_1 T_1_2 T_2_hand那么手相对于 BaseT_base_hand T_base_1 · T_1_2 · T_2_hand这就是你以后天天会见到的变换矩阵连乘。人形机器人可以看成pelvis ├── torso │ ├── left_arm │ └── right_arm │ ├── left_leg │ └── left_foot │ └── right_leg └── right_foot如果你想算pelvis → right_hand就沿右臂这条链pelvis ↓ torso ↓ shoulder ↓ upper_arm ↓ elbow ↓ forearm ↓ hand把这些 T 一路乘下去。如果你想算pelvis → left_foot就走左腿链。所以FK本质就是在机器人树上向下传播变换。T_WB表示Base 相对于 World 的变换。T_HB表示Hand 相对于 Base 的变换。那么T_WH T_WB · T_BH就是Hand 相对于 World。你可以把它看成World → Base → Hand很自然。TF你以后学 ROS2 时TF/TF2其实就是在回答“任意两个坐标系之间现在是什么关系”例如right_hand相对于base_link在哪里或者left_foot相对于odom在哪里TF会利用整棵树odom ↓ base_link ↓ pelvis ↓ ... ↓ left_foot把中间变换串起来。本质还是今天这件事。Pinocchio里的 SE3以后你会看到pinocchio::SE3你可以把它理解成一个三维刚体变换。也就是Rotation Translation所以本质还是T只是 Pinocchio 不一定真的每次都用一个裸 4×4 矩阵来存。SE(3) 和 SO(3)SO(3) 纯三维旋转 SE(3) 三维旋转 三维平移所以Rotation Matrix 属于 SO(3)而Rigid Transform 属于 SE(3)q ↓ 每个Joint产生一个局部变换 ↓ 沿运动链不断相乘 ↓ 得到末端T也就是Joint Angle ↓ Rotation / Translation ↓ Transform ↓ Transform ↓ Transform ↓ End-Effector Pose这已经非常接近真正的机器人运动学实现了。