继续我们之前机器人运动控制学习1——基础概念的学习q_des ↓ PD ↓ torque ↓ Motor ↓ q,dq反馈现在在它上面再加一层最终目标 ↓ Trajectory Generator ↓ q_des dq_des ddq_des ↓ PD / Controller ↓ torque ↓ Motor ↓ Robot ↓ q / dq └──────↺肩膀 ●──────● 肘关节 \ \ ● 手你只要先把三个问题搞懂已知关节角 → 手在哪里 → FK 正运动学 已知手想去哪 → 关节该转多少 → IK 逆运动学 手想移动一点点 → 每个关节该怎么配合 → Jacobian 雅可比这三者就是运动学的主线。自由度 DOF_Degree of Freedom这个机器人有多少个“独立可以动的方向”。一个简单机械臂肩 ●────● 肘肩膀能转q1肘关节也能转q2那么2 DOF人形机器人可能左腿 6 DOF 右腿 6 DOF 左手若干 右手若干 腰 头 ...最后二十多个、三十多个自由度都很正常。所以以后看到num_dof dof nq大概率都和机器人有多少可控制的运动变量有关。Joint Space关节空间描述机器人也就是机器人状态 每个关节分别是多少例如q [30°, 60°]这就是一个关节空间状态。Task Space / Cartesian Space但人操作机器人时往往不会想肩膀 31.7°肘 54.2°。我们更容易想手放到这里。比如X 0.5 m Y 0.3 m这就是Cartesian Space笛卡尔空间或者Task Space任务空间。所以Joint Space 关节角是多少 Task Space 手、脚、身体在空间哪里这两个世界以后会不停互相转换。正运动学 FKForward Kinematics我知道所有关节怎么弯求手在哪里。比如肩膀 q1 30° 肘关节 q2 60°机械臂长度也知道大臂 0.4m 小臂 0.3m那么q1 q2 机械臂结构 ↓ 计算 ↓ 手的位置 X,Y这就是 FK所以关节 → 末端叫正运动学。输入 q ↓ 输出末端 Pose末端 EE End Effector机器人真正用来干活的末端部位。比如机械臂夹爪 吸盘 焊枪 机械手都可能是 End Effector人形机器人里左手 右手 左脚 右脚也经常作为重要末端所以EE position就是末端位置。EE pose就是末端的位置 朝向。Kinematic Chain 运动链机器人通常不是一个关节而是一条链Base ↓ 肩关节 ↓ 大臂 ↓ 肘关节 ↓ 小臂 ↓ 腕关节 ↓ 手每一级都可以理解成上一段 ↓ 经过一个关节变换 ↓ 下一段所以正运动学本质就是从根部一路算到末端。例如Base ↓ 肩 ↓ 肘 ↓ 腕 ↓ Hand一步一步把坐标变换乘起来。T 旋转 平移现在机器人每个关节之间都有T1 T2 T3比如Base ↓ T1 Shoulder ↓ T2 Elbow ↓ T3 Hand如果想Base → Hand就把一路上的变换组合起来所以 FK 本质可以理解为从机器人根部出发 ↓ 一个关节一个关节往外走 ↓ 最终得到末端坐标这就是运动学链。逆运动学 IK现在反过来你不告诉机器人肩30° 肘60°而是说手给我放到桌子上这个杯子旁边。目标X 0.5m Y 0.2m问题变成肩关节应该多少肘关节应该多少也就是末端位置 ↓ 求关节角这就是Inverse Kinematics逆运动学 IK。所以最核心FK q → 手的位置IK 手的位置 → qWorkspace 工作空间这个机器人末端实际能够到达的空间范围。例如一个二维机械臂可能· · · · · · · ●肩 · · · · · · ·它只能覆盖某个区域。机器人外面的点IK 无解。所以以后规划目标前必须考虑目标能不能到不然控制器再强也没用。JacobianJacobian 先理解成关节动一点末端会怎么动例如肩膀动一点 ↓ 手可能移动很多 手腕动一点 ↓ 手的位置可能只变化一点Jacobian 就是在描述关节运动和末端运动之间的瞬时关系。假设你不直接说手去 X0.5而是说手现在向右移动 1 cm。也就是末端想产生一点速度Jacobian 可以帮助算每个关节该以多快速度配合所以你可以先记dq ↓ Jacobian ↓ 末端速度或者反过来我想要末端速度 ↓ 利用 Jacobian ↓ 求关节速度 dq这就是它非常重要的用途。Jacobian 是 FK 和 IK 之间的桥FKq ↓ 末端位置IK末端目标 ↓ q而 Jacobian关节的“小变化” ↔ 末端的“小变化”所以很多数值 IK 实际上就是看手还差多少 ↓ 通过 Jacobian 算关节该改多少 ↓ 更新 q ↓ 重新 FK ↓ 再看还差多少 ↓ 反复和ICP 很像差一点 ↓ 修一点 ↓ 重新看误差 ↓ 继续修这就是很多迭代算法共同的思维。当前手位置 ↓ 和目标比较 ↓ 还差一点 Jacobian ↓ 算关节应该改多少 q 更新 ↓ 重新做 FK 手更接近目标 ↓ 再算 不断循环 ↓ 到目标附近所以数值 IK 本质可以理解通过很多次小修正把末端慢慢推到目标位置。Singular 奇异位形想象手臂完全伸直肩 ●────────●────────● 手这时候某些方向的运动会变得很麻烦例如你要求手继续沿着某个方向再伸。机械臂已经伸直了根本没法通过小幅关节转动继续完成。或者为了产生很小的末端动作需要关节速度突然特别大这种特殊姿态附近就可能是Singularity奇异位形。不是机械卡死而是运动学意义上某些方向的控制能力变差。比如正常弯曲 肩 ●────● \ ● 手关节有很多调整空间。完全伸直肩 ●────●────● 手某些方向就很尴尬所以以后 IK 出现关节速度爆大 数值不稳定 抖动 解突然跳就要考虑是不是靠近 singularity 了J_hand J_foot J_com可认为J_hand 关节运动对手的影响关系 J_foot 关节运动对脚的影响关系 J_com 关节运动对质心的影响关系这一下就不神秘了。关节空间控制 vs 任务空间控制关节空间控制你直接规定左膝 30° 右膝 30° 髋关节 ...控制的是q优点直接 简单 底层好实现任务空间控制你规定右手去这个 XYZ 左脚保持这里 身体保持竖直控制的是手 脚 身体然后控制器自己算关节怎么配合这更符合“任务”。所以高级人形控制会大量使用Task Space。Null Space在不破坏主要任务的前提下剩下还能怎么动。例如主要任务 右手必须保持这个位置但机器人肘部还有余地肘可以稍微抬高 或者降低只要手不动这些“剩余动作空间”就和Null Space有关即Primary Task 最重要任务 Null Space 不影响主任务情况下还能做的动作以后层级控制特别重要。Joint Space q / dq │ │ FK ↓ End Effector Pose X Y Z ... │ │ IK ↓ Joint q 速度层 Joint Velocity dq │ │ Jacobian ↓ End Effector Velocity所以FK 关节 → 末端 IK 末端 → 关节 Jacobian 关节速度 ↔ 末端速度假设任务右手去拿桌上的杯子。系统可能视觉 ↓ 杯子位置 XYZ ↓ 生成手的轨迹 ↓ 每个时刻得到 hand_des ↓ IK / Jacobian / WBC ↓ 得到 q_des / dq_des ↓ 关节 PD ↓ tau ↓ Motor ↓ 机器人运动假设视觉系统得到杯子在相机坐标系 X,Y,Z先Camera Frame ↓ 坐标变换 ↓ Robot Frame得到目标杯子在机器人坐标系的位置然后运控目标 XYZ ↓ 生成手部轨迹 ↓ IK ↓ q_des ↓ PD ↓ Motor于是OpenCV / Open3D 告诉机器人目标在哪 运动学 决定关节怎么过去 控制器 让真实电机跟上整个机器人系统就串起来了。运动学和动力学运动学 Kinematics关心位置 速度 加速度 几何关系不太关心质量 力 力矩比如肩30°、肘60°手在哪里动力学 Dynamics关心机器人有多重 惯性多大 重力多大 需要多少力矩例如想让肩膀以这个加速度运动需要电机输出多少 N·m所以Kinematics 怎么动 Dynamics 为什么这么动、需要多大力这是非常重要的区分。运动学 关节怎么动 → 手脚会去哪 动力学 想让机器人这样动 → 到底需要多大的力/力矩先记住一句最重要的话运动学负责“去哪”动力学负责“怎么出力才能去”。力矩 Torque力矩就是“拧东西的劲”。单位以后常见N·m 牛·米同样的重量离旋转轴越远对肩膀造成的“拧劲”越大。所以机器人里同样一个负载姿态不同需要的关节力矩也可能完全不同。这就是为什么只看关节角不够。Gravity Compensation 重力补偿假设现在手臂水平伸着重力需要8 N·m往下拽。那么电机如果提前8 N·m往上顶两边大概抵消重力 ↓ ↕ 电机 ↑手臂就可以比较轻松保持住你可以把它理解成我提前知道重力会搞事情所以先把它抵消掉。M(q) C(q,dq) G(q)M 惯性相关 机器人想加速有多难 C 运动过程中互相带来的影响 G 重力所以控制器在考虑想产生这个加速度需要克服哪些东西大概就是克服惯性 处理运动耦合 抵抗重力Coriolis / CentrifugalCoriolis 科氏项 Centrifugal 离心项肩膀动 ↓ 会影响肘 肘高速动 ↓ 又会影响肩所以一个关节的运动会通过整个机械结构影响其他关节。动力学模型需要把这些影响算进去这就是你先理解C(q,dq)的意义。髋关节突然加速 ↓ 整条腿都会被带动 ↓ 膝关节受力变化 ↓ 足底力也变化这叫Coupling耦合。所以高级运控不能永远每个关节自己PD 互不管别人Forward Dynamics 前向动力学这个名字也很容易和 FK 搞混。我知道机器人现在的状态 我给了多少力矩 ↓ 接下来机器人会怎么加速这就是Forward Dynamics前向动力学。大白话输入 tau 输出 ddq也就是我这么使劲机器人会怎么动Inverse Dynamics 逆动力学反过来我希望机器人 按照这个加速度运动 ↓ 那每个关节需要多少力矩这叫Inverse Dynamics逆动力学。大白话输入 我想要的 q / dq / ddq ↓ 动力学模型 输出 tau所以特别好记Forward Dynamics 力 → 运动 Inverse Dynamics 想要的运动 → 力假设轨迹规划器已经告诉你当前时刻 q_des dq_des ddq_des也就是我要在哪 我要多快 我要怎么加速逆动力学可以问按机器人质量、惯性、重力来看要实现这个动作理论上需要多少力矩得到tau_ff然后再加 PDtau tau_ff PD修正这就是一种很典型的Model-based Feedforward Feedback假设Trajectory Generator ↓ q_des dq_des ddq_des然后Inverse Dynamics ↓ tau_ff同时真实机器人返回q dqPD 计算tau_feedback最后tau_command tau_ff tau_feedback整个流程轨迹 ↓ 目标 q/dq/ddq ↓ 动力学前馈 ─────────┐ │ 真实 q/dq → PD ─────┤ ↓ tau ↓ MotorComputed Torque Control尽量用机器人动力学模型把本来复杂的机械系统“提前补偿掉”然后再用比较简单的反馈控制追踪。可以想机器人本来 重力 惯性 耦合 非常复杂 ↓ 模型先算一遍 ↓ 提前补偿 ↓ 剩下的问题 让PD之类去修这是模型控制非常重要的一类思想。External Force 外力机器人不只受自己的重力还会受到地面对脚的力 人推它的力 手拿物体的力 碰撞力这些都叫External Force外力。例如机器人站着 ↓ 重力 ──────────── 地面 ↑ 地面反作用力机器人没有穿过地面掉下去是因为地面在向上顶它。这个力以后非常重要Ground Reaction Force地面反作用力。Contact 接触以后人形机器人会天天出现contact contact state contact forceContact 就是机器人某个部位正在跟环境接触。比如站立左脚contact 右脚contact走路时某个阶段 左脚contact 右脚离地再过一会左脚离地 右脚contact所以走路本质上还有一个接触状态不断切换的问题。