机器人运动控制学习2——基础进阶

继续我们之前机器人运动控制学习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
=
手、脚、身体在空间哪里

这两个世界以后会不停互相转换。

正运动学 FK Forward 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：

手的位置 → q

Workspace 工作空间

这个机器人末端实际能够到达的空间范围。

例如一个二维机械臂可能：

复制代码

        · · · ·
     ·           ·
   ·      ●肩      ·
     ·           ·
        · · · ·

它只能覆盖某个区域。

机器人外面的点：

IK 无解。

所以以后规划目标前必须考虑：

复制代码

目标能不能到？

不然控制器再强也没用。

Jacobian

Jacobian 先理解成：

关节动一点，末端会怎么动？

例如：

复制代码

肩膀动一点
↓
手可能移动很多

手腕动一点
↓
手的位置可能只变化一点

Jacobian 就是在描述：

关节运动和末端运动之间的瞬时关系。

假设你不直接说：

复制代码

手去 X=0.5

而是说：

手现在向右移动 1 cm。

也就是：

复制代码

末端想产生一点速度

Jacobian 可以帮助算：

复制代码

每个关节该以多快速度配合

所以你可以先记：

复制代码

dq
↓
Jacobian
↓
末端速度

或者反过来：

复制代码

我想要末端速度
↓
利用 Jacobian
↓
求关节速度 dq

这就是它非常重要的用途。

Jacobian 是 FK 和 IK 之间的桥

FK：

复制代码

q
↓
末端位置

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°
髋关节 = ...

控制的是：

复制代码

优点：

复制代码

直接
简单
底层好实现

任务空间控制

你规定：

复制代码

右手去这个 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 / Centrifugal

复制代码

Coriolis
科氏项

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

然后再加 PD：

复制代码

tau =
tau_ff
+
PD修正

这就是一种很典型的：

Model-based Feedforward + Feedback

假设：

复制代码

Trajectory Generator
↓
q_des
dq_des
ddq_des

然后：

复制代码

Inverse Dynamics
↓
tau_ff

同时真实机器人返回：

复制代码

q
dq

PD 计算：

复制代码

tau_feedback

最后：

复制代码

tau_command
=
tau_ff
+
tau_feedback

整个流程：

复制代码

轨迹
↓
目标 q/dq/ddq
↓
动力学前馈 ─────────┐
                    │
真实 q/dq → PD ─────┤
                    ↓
                  tau
                    ↓
                  Motor

Computed Torque Control

尽量用机器人动力学模型，把本来复杂的机械系统"提前补偿掉"，然后再用比较简单的反馈控制追踪。

可以想：

复制代码

机器人本来：
重力 + 惯性 + 耦合
非常复杂

↓

模型先算一遍

↓

提前补偿

↓

剩下的问题
让PD之类去修

这是模型控制非常重要的一类思想。

External Force 外力

机器人不只受：

复制代码

自己的重力

还会受到：

复制代码

地面对脚的力
人推它的力
手拿物体的力
碰撞力

这些都叫：

External Force，外力。

例如机器人站着：

复制代码

      🤖
      ↓ 重力

──────────── 地面
      ↑ 地面反作用力

机器人没有穿过地面掉下去，是因为地面：

在向上顶它。

这个力以后非常重要：

Ground Reaction Force，地面反作用力。

Contact 接触

以后人形机器人会天天出现：

复制代码

contact
contact state
contact force

Contact 就是：

机器人某个部位正在跟环境接触。

比如站立：

复制代码

左脚：contact
右脚：contact

走路时：

复制代码

某个阶段：

左脚：contact
右脚：离地

再过一会：

复制代码

左脚：离地
右脚：contact

所以走路本质上还有一个：

接触状态不断切换

的问题。