机器人运动控制学习1——基础概念

先搞清机器人为什么能动 → 再搞清怎么让一个关节听话 → 再控制整条腿 → 最后才上全身控制和人形机器人。先建立“运控组到底干什么”的地图你可以把机器人看成一个闭环我想让机器人怎么动 ↓ 目标指令 ↓ 运动控制算法 ↓ 位置 / 速度 / 力矩命令 ↓ 电机 / 关节 ↓ 机器人真的动了 ↓ 编码器 / IMU / 足底 / 电流等传感器 ↓ 实际状态 ↓ 再和目标比较 └──────────↺这就是整个运动控制最核心的东西目标 → 控制 → 执行 → 测量 → 修正 → 再执行。比如你告诉机器人把膝盖弯到 60°。这叫Position Target 位置目标但是机器人现在只有30°于是目标 60° - 实际 30° 误差 30°控制器看到差得挺多要使劲往目标方向转。机器人转30° ↓ 40° ↓ 50° ↓ 58° ↓ 59° ↓ 60°这就是一个最简单的闭环控制过程。目标 ↓ 和实际比较 ↓ 有误差 ↓ 调整 ↓ 再读取实际状态 ↓ 再比较这就是Feedback Control反馈控制。开环就是发命令 ↓ 完事例如给电机固定 2V 电压持续 3 秒。你根本不管电机到底转多少 有没有被卡住 有没有负载 有没有达到目标这就是开环。所以开环 命令 → 机器人而闭环 命令 → 机器人 ↑ ↓ └──反馈───┘现代机器人核心控制基本离不开闭环。PIDP你离目标越远我越使劲假设目标 100° 实际 20°误差很大那使劲转。到了实际 95°误差很小那少使点劲。所以 P 的感觉特别简单离目标远 ↓ 力气大 离目标近 ↓ 力气小这就是Proportional比例控制。D可以理解成“刹车”如果机器人离目标还有一点但是速度特别快那么 D 会说你冲太猛了赶紧刹车。所以P 油门 D 刹车这是理解 PD 控制特别好的方式于是机器人常见PD就是根据位置误差给动力同时根据速度控制别冲过头。kp 位置误差有多重要 kd 速度误差有多重要三个控制模式1. Position Control你告诉机器人到 30°。例如target_position 30°底层控制器自己想办法过去。特点简单 直观 容易用2. Velocity Control你告诉以 10°/s 转。你关心的是转多快而不是马上到哪个位置。3. Torque Control你直接告诉电机现在输出这么大的力矩。这是更底层 更自由 也更危险很多高级机器人控制最终会落到Torque Command因为算法可以更直接控制机器人动力学行为。“轨迹规划”和“轨迹跟踪”这个以后一定分清。轨迹规划 我希望机器人怎么走例如0° ↓ 10° ↓ 30° ↓ 60° ↓ 80° ↓ 90°而轨迹跟踪 机器人实际有没有跟住以后你看机器人代码优先找这几个东西不要一上来从第一行看到最后一行。先找输入状态是什么 q dq IMU ↓ 目标是什么 q_des dq_des torque_des ↓ 控制器在哪 kp kd PID MPC ↓ 输出是什么 position velocity torque ↓ 控制频率多少 50Hz 500Hz 1kHzFK关节转了多少 ↓ 手会跑到哪里Forward Kinematics正运动学 FK。比如肩膀转30° 肘关节转45°问手现在在哪里就是 FK。反过来我想让手到这个XYZ ↓ 肩膀和肘关节各转多少这就是Inverse Kinematics逆运动学 IK。所以FK 关节 → 手的位置 IK 手的位置 → 关节这是最好记的方法。运动学只关心怎么动。它不太关心机器人多重 惯性多大 重力怎么影响 需要多少力动力学开始关心这些。比如机器人胳膊水平伸出去即使完全不动电机也得出力抗重力。这就是Gravity Compensation 重力补偿所以以后你会看到一种特别重要的结构PD反馈 重力前馈意思提前知道重力会往下拉 ↓ 先给一部分力顶住 然后PD ↓ 修正剩余误差这就叫Feedforward Feedback前馈 反馈。前馈反馈已经出现误差 ↓ 我再修前馈我提前知道会发生什么 ↓ 先补偿比如你拿一个很重的箱子。反馈式箱子把手压下去了我再加力。前馈式我知道它很重一拿起来就提前使劲。机器人里重力 惯性 期望加速度都可能用来算前馈。阻抗控制这是运控特别值得理解的东西纯位置控制可能非常“硬”我说手必须到这里 ↓ 就必须到这里如果突然碰到人还在死命往目标顶很危险。阻抗控制希望机器人表现得像弹簧 阻尼器你推它能让一点你松手它又回去所以可以简单理解既想保持目标位置又不是完全死硬。人形机器人、机械臂、交互控制都会大量遇到。质心 CoM假设一个人站着。虽然身体有头 躯干 腿 胳膊但从整体重量分布来看可以想象有一个代表整个身体重量集中位置的点。这就是Center of Mass CoM 质心人往前倾CoM向前蹲下CoM下降抬腿CoM也会变化所以人形机器人控制特别关心我的质心现在在哪里WBC Whole-Body Control全身控制机器人全身这么多关节到底怎么一起协调比如同时要求双脚别滑 ↓ 身体保持平衡 ↓ 左手到目标位置 ↓ 右手做另一个动作 ↓ 关节别超限 ↓ 力矩别太大很多任务 很多约束 ↓ 统一协调 ↓ 所有关节一起工作MPCModel Predictive Control不只看现在还提前想未来几步。普通控制可能我现在往前倒了赶紧救。MPC按现在这个趋势0.2 秒后可能会倒我现在提前调整。非常像预测未来 ↓ 比较几种动作 ↓ 挑一个更好的 ↓ 先执行第一步 ↓ 下一周期重新预测这就是 MPC 的核心感觉。第1阶段 单关节控制 │ ├── q / dq / torque ├── 开环与闭环 ├── P ├── PD ├── PID ├── Kp / Kd 到底怎么调 ├── 饱和 ├── 控制频率 └── 一个关节为什么会振荡 第2阶段 轨迹 │ ├── 目标点和轨迹 ├── 插值 ├── 速度 ├── 加速度 └── jerk 第3阶段 运动学 │ ├── FK ├── IK ├── Jacobian └── 关节空间 / 笛卡尔空间 第4阶段 动力学 │ ├── 重力 ├── 惯性 ├── torque ├── 重力补偿 ├── 逆动力学 └── Feedforward Feedback 第5阶段 机器人高级控制 │ ├── 阻抗 ├── LQR ├── MPC └── WBC 第6阶段 人形机器人 │ ├── CoM ├── Contact ├── ZMP ├── 足底力 ├── 单脚/双脚支撑 ├── 步态 └── 平衡而且我建议下一课就从最重要的“单关节 PD 控制”开始。我们会直接用一个非常简单的场景机器人膝关节 当前 0° 目标 30°然后一步一步看没有控制会怎样 ↓ 只有P会怎样 ↓ Kp太小会怎样 ↓ Kp太大会怎样 ↓ 为什么会振荡 ↓ 为什么需要Kd ↓ Kd太大又会怎样 ↓ 控制频率为什么会影响效果 ↓ 最终一段真正的C控制循环长什么样这个如果彻底学懂你就真正跨进“运动控制”的门了。好我们正式进入运动控制第一课单关节 PD 控制。这一部分我建议你反复看因为以后不管是宇树 G1/H1、机械臂、MuJoCo、人形机器人 WBC最底层都逃不开这些概念目标 q_des ↓ 当前 q ↓ 误差 ↓ 控制器 ↓ torque ↓ 电机 ↓ 新的 q、dq ↓ 再次计算先把一个关节控制明白再扩展到几十个关节。Kp假设误差10°到底使多大劲这就需要一个旋钮Kp你可以把Kp想成这个控制器有多着急。比如Kp 很小机器人差10度啊慢慢来。于是0° ↓ 2° ↓ 5° ↓ 8° ↓ ... ↓ 30°非常慢。如果Kp 比较大机器人差10度赶紧过去于是0° ↓ 15° ↓ 27° ↓ 30°快很多。但如果Kp 特别特别大机器人可能0° ↓ 40° ↓ 20° ↓ 36° ↓ 25° ↓ 33° ...开始来回震。Kd和 Kp 一样D 也有一个旋钮Kd你可以把Kd理解成刹车力度。所以Kd 太小 ↓ 刹不住 ↓ 容易来回晃而Kd 合适 ↓ 快到目标的时候减速 ↓ 平稳停下但如果Kd 太大机器人会动一下都要被强烈阻止。于是感觉非常沉 非常钝 响应慢所以 Kd 也不是越大越好。控制器这是非常关键的地方。PD 不是算一次 torque ↓ 结束而是一直循环。比如控制频率500 Hz那么每秒运行500次大约2ms 一次整个循环第1次 读取 q、dq ↓ 计算 torque ↓ 发送给电机 2ms以后 第2次 重新读取 q、dq ↓ 重新算 torque ↓ 重新发 2ms以后 第3次 ...所以控制器其实一直在问现在怎么样了那我现在应该出多少力编码器Encoder安装在电机/关节上的“角度尺”。它不断告诉控制器关节位置 q然后通过连续位置变化还可以获得速度 dq所以Encoder ↓ q / dq ↓ Controller ↓ Torque ↓ Motor形成闭环。Saturation 饱和这个对真实机器人非常重要。假设 PD 算torque 300 N·m但是电机最大只能安全输出50 N·m你怎么办当然不能控制器说300我就硬发300。需要限幅例如if (torque max_torque) { torque max_torque; } if (torque -max_torque) { torque -max_torque; }大白话太大 ↓ 砍到最大允许值 太小 ↓ 砍到最小允许值这就是Saturation / Clamp。位置控制和力矩 PD 是什么关系你以后可能看到机器人 SDK 里直接提供q dq kp kd tau_ff这种接口。这是什么意思可以理解成输出力矩 ≈ PD 前馈力矩即位置目标 q_des 速度目标 dq_des Kp Kd 额外前馈 torque底层执行器帮你完成计算这种接口在人形机器人运控里非常常见所以看到q dq kp kd tau你应该马上想到这是一个关节级控制命令。tau_ff以后可能看到tau_fffffeedforward前馈。假设机器人手臂水平举着──────重力一直往下拉。如果只靠 PD手臂先往下掉一点 ↓ 出现位置误差 ↓ P开始加力这是出现误差以后才救。但如果动力学模型提前知道现在需要 8 N·m 抗重力。可以直接tau_ff 8 N·m先顶住。然后 PD只负责修小误差。于是最终力矩 前馈 反馈这会成为你以后运控非常重要的结构。q_des dq_des kp kd tau_ff │ ↓ ┌───────────┐ │ Controller│ └───────────┘ │ torque ↓ Motor ↓ Joint ↓ ┌───────────┐ │ Encoder │ └───────────┘ ↓ ↓ q dq │ │ └──┬──┘ ↓ Controller但人形机器人绝不是“23个PD拼一起”这么简单这是接下来非常重要的升级因为左膝动 ↓ 身体姿态会变化 右腿动 ↓ 质心变化 手臂抬起来 ↓ 身体质量分布变化所有关节彼此耦合所以低层 关节PD上面还需要更高级控制器 ↓ 决定每个关节应该干什么比如IK 动力学 MPC WBC RL Policy最终这些高级算法通常不是直接让电机“神奇地运动”而是输出目标关节角 目标速度 目标力矩最后还是落到底层控制。位置、速度、加速度位置 q就是现在在哪。比如膝关节 30°这是位置。速度 dq就是位置变化有多快。例如1秒前20° 现在30°说明它正在转如果dq 0就是没在动。如果速度很大转得快。加速度 ddq就是速度变化有多快。比如汽车0 km/h ↓ 20 ↓ 40 ↓ 60速度越来越快有加速度。而刹车60 ↓ 40 ↓ 20 ↓ 0也是有加速度只不过方向相反机器人关节同样如此所以先建立q 在哪 dq 动多快 ddq 速度变化多快JerkJerk加加速度 / 加速度变化率加速度变得有多突然。例如坐电梯。情况 A静止 ↓ 突然猛地往上加速你会咯噔一下。情况 B静止 ↓ 慢慢开始加速 ↓ 再逐渐加速舒服很多虽然两种最后可能都达到相同速度但“冲击感”不同这个冲击感和 Jerk 密切相关。所以可以建立位置突变 最糟糕 速度突变 有冲击 加速度突变 也会不舒服 让加速度变化更平滑 ↓ 机器人运动更柔顺这就是为什么工业机器人经常谈Jerk Limit。最简单的轨迹——线性插值假设起点 0° 终点 90° 时间 3秒最简单想法每秒匀速走 30°。于是0秒 → 0° 1秒 → 30° 2秒 → 60° 3秒 → 90°图像像这样位置 90 | / | / 60 | / | / 30 | / | / 0 |____________ 时间 0 1 2 3这就是非常典型的Linear Interpolation线性插值。大白话起点和终点之间画条直线按时间走过去。梯形速度轨迹一个非常经典的机器人轨迹就是Trapezoidal Velocity Profile梯形速度轨迹。第一阶段 慢慢加速 第二阶段 保持速度 第三阶段 慢慢减速速度图速度 __________ / \ / \ _____/ \____ 时间看起来像一个梯形所以叫梯形速度轨迹。S-curveS 曲线核心思想就一句连加速度都不要突然变化。梯形速度加速度 突然 a ↓ 保持 ↓ 突然 0S 曲线加速度 慢慢增加 ↓ 再慢慢降低所以整个速度变化会更柔和。机器人感觉普通 冲一下 S曲线 柔和起步工业机械臂、数控机床、电机控制里非常常见这种思想。三次轨迹、五次轨迹Cubic Polynomial 三次多项式 Quintic Polynomial 五次多项式它们主要是在做一件事设计一条满足起点和终点要求而且比较平滑的曲线。比如你要求起点 位置 0° 速度 0 终点 位置 90° 速度 0三次轨迹可以设计出慢慢起步 ↓ 运动 ↓ 慢慢停下五次轨迹还能进一步约束起点 位置 速度 加速度 终点 位置 速度 加速度因此能设计得更加平滑。线性 最简单 三次 位置、速度更平滑 五次 还可以让加速度也更自然 S曲线 特别关注冲击/JerkTrajectory Planning 和 Interpolation 有什么区别Interpolation 插值你已经知道起点 终点 ↓ 中间帮我补一些点例如0° 和 90° ↓ 自动生成 10° 20° 30° ...这就是插值的直觉。Trajectory Planning 轨迹规划除了中间经过哪里还要考虑什么时候到 速度多少 加速度多少 关节限制 碰撞 平滑性所以插值可以是轨迹规划里的一个工具。路径 Path 和轨迹 Trajectory这个区别进入运控以后一定要搞清楚。假设机械臂手从A移动到B路径回答从空间里的哪条线走例如A ───────── B直线。或者A \ \ ───── B绕开障碍物。轨迹还多问一个问题什么时候走到哪里同一条路径A ───────── B可以2秒走完也可以10秒走完路径完全一样。但轨迹不同。所以Path 空间上的路线 Trajectory 路线 时间这个以后非常重要。dtdt 每个控制周期的时间比如500 Hz大约dt 0.002 s也就是2 ms假设轨迹 2 秒完成。那控制器大概要获得2 / 0.002 1000个目标点。也就是说第0周期 q_des ... 第1周期 q_des ... 第2周期 q_des ... ... 第1000周期 q_des 最终目标这就是数字控制真正工作的样子。轨迹跟踪 Tracking假设轨迹规划器给时间 q_des 0.0 0 0.5 20 1.0 45 1.5 70 2.0 90机器人真实时间 q 0.0 0 0.5 18 1.0 42 1.5 68 2.0 89控制器不断努力让q靠近q_des这就是Trajectory Tracking轨迹跟踪。所以再强调一次轨迹规划 ↓ 负责生成 q_des 轨迹跟踪 ↓ 负责让 q 跟住 q_des不能混。Joint Space 和 Cartesian Space现在提前引出下一阶段最重要的概念。假设机器人手臂有肩关节 q1 肘关节 q2 腕关节 q3如果我们规划q1: 0° → 30° q2: 20° → 60° q3: 0° → -20°这叫Joint Space关节空间。你直接规定每个关节怎么走。但有时候你不关心关节。你只想说让机器人右手从这里直线移动到杯子这里。那目标描述的是手的 XYZ这是Cartesian Space / Task Space笛卡尔空间 / 任务空间。于是问题变成手要走这条轨迹 ↓ 每个关节应该怎么动这就是后面 IK 开始登场的地方。假设机器人手 A → B如果你直接让各关节线性插值q1线性 q2线性 q3线性手在空间里可能走A \ \ ) ) B是一条弯曲路径。如果你的要求是手必须沿空间直线A ───────── B那就要先规划手的XYZ直线 ↓ 不断做IK ↓ 求每个时刻的关节角所以关节轨迹直 ≠ 末端轨迹直这个概念特别重要。