MIT机器人操作 知识点笔记(5):运动学优化
- 2026-11-21 03:40:46
在第四讲中,我们得到了微分逆运动学的基本解: 经由 Jacobian 伪逆转换为关节速度 。但真实机器人有关节位置、速度、加速度和碰撞等限制,“先求伪逆,再做硬截断”通常无法获得约束条件下的最优解。
第五讲 Kinematics via Optimization 先对上一讲内容进行了回顾,同时进一步将伪逆本身视作一个最小二乘问题;那么结合“决策变量 + 代价 + 约束”的语言,就可以将微分IK(逆运动学)以及对整条轨迹的设计问题,纳入到同一套框架中。

图 1 从带约束的微分逆运动学,到整条轨迹的优化。
以上的四个方法都在回答“机器人该怎么动”,区别主要在于考虑了哪些目标/约束:
定义代价函数,然后设法找到一个,使得与目标的误差尽可能小。也即一个最简单的最小二乘优化问题:上述问题可以通过对代价函数求导:
在的情况下,得到:从上式可以看到,在b固定为1的情况下,响应系数越小,需要的越大,即:
图 2 系统对输入的响应系数越小,那产生同样的输出,就必须使用越大的 上面的标量问题:
可以对应到上一讲的机器人关节速度求解问题:
则可通过如下对应关系理解上一讲的奇异值问题:
则有:也即上一讲提到的:当机械臂接近奇异构型时(),很小的末端速度需要非常大的关节速度。
伪逆求解实际上是进行最小二乘优化:
它的解是:找一组关节速度,使实际末端速度与期望速度的误差尽可能小。

图 3 最小二乘代价的等高线:越靠近中心,速度跟踪误差越小
通过伪逆求解,有两个重要特性:
一种常见做法是先计算:
然后对每个分量分别截断:
这样确实保证了单个关节不超速,但截断后的整个向量已经变了,因此 可能不再指向原来的 。从图四的例子可以明显地看出,如果先求出无约束的最优点(黑点),然后保持的值不变,取约束可允许的最大值,则最终的等高线落在红色虚线圈上,相比一开始就带入约束优化得到的蓝色圆点,误差更大。

图 4 红色方块是逐分量截断后的结果,蓝色圆点是直接在可行域中寻找的约束最优解;二者一般不同。
因此更好的方法是直接求一个带约束的二次规划(QP):
其中 是一个控制周期。第二行用一步 Euler 预测防止下一时刻的关节位置越界;第三行限制相邻周期的速度变化。当原始目标无法在约束内完成时,优化器会直接在可行域中找跟踪误差最小的速度,而不是事后修剪一个已经失效的解。〔4〕
对7自由度的iiwa来说,夹爪的6维空间速度通常不会只有一个唯一的7维关节速度解。若向量 满足:
那么它位于 Jacobian 的零空间:关节虽然在动,却不产生一阶末端速度。于是通解可以写成:
第一项完成末端主任务,投影矩阵 则只保留 中属于零空间的部分。例如,取:
就会在尽量不影响夹爪瞬时运动的前提下,把关节拉回活动区间中部。
图 5 可以将期望向量方向投影到 Jacobian 的零空间中,作为不破坏主任务的次任务。
图5其实就是亚琛工业大学的机械臂冗余度展示背后的原理。〔12〕

微分 IK 求的是当前速度 ,完整 IK 则直接求一组关节位置 。课件中给出了一个代表性的描述:

图 6 把位姿、关节、碰撞、视线、接触和平衡等要求统一写入 IK 优化问题。
与前面的速度 QP 相比,这个问题往往是非凸的:旋转、正运动学和碰撞距离对 的关系都可能是非线性的。

图 7 相同的抓取任务可对应三组不同关节位形;末端目标不会自动指定肘部应该走哪一侧。〔3〕
Drake示例代码先向 InverseKinematics 加入末端位置与姿态约束,再使用二次代价让 尽量靠近给定的 ,同时也把 设为初始猜测。这里 扮演了两个不同角色:
机械臂一般有多根连杆。运动时,不仅末端需要避开障碍物,手肘、前臂、夹爪等任何部位都不能发生碰撞,因此直接在现实空间中分析会非常复杂。
构形空间提供了另一种解法:用一组关节位置表示机器人的一个完整姿态。构形空间中的一个点 (q),表示整个机器人所有关节的位置组合。只要给定一个点,就能通过正运动学确定每根连杆的位置。
可以把构形空间理解成一张“机器人姿态地图”:
机器人运动时,关节位置不断变化,相当于代表机器人姿态的点在这张地图上连续移动。因此,运动规划可以重新表述为:
在构形空间中,寻找一条从起始姿态 到目标姿态 的连续路径,并保证路径上的每一个姿态都位于自由空间,不进入碰撞禁区。
这样,现实世界中“多根连杆共同绕开障碍物”的问题,就转化成了“一个姿态点在地图上绕开禁区”的路径搜索问题。
课件用一个二维教学例子把轨迹离散成 :

图 8 离散轨迹优化示例:目标是缩短相邻样本间距离,同时让所有样本位于 球之外。

图 9 初始轨迹猜测。

图 10 在同一离散结构上得到的局部优化结果。
对于凸优化问题,好的初值主要影响收敛速度;而对于非凸问题,初值还可能决定优化器对障碍物的绕行方向,甚至决定它能否找到可行路径。一般实现方式是“先生成一条大致可行的路,再交给局部优化器进一步打磨”。〔9〕

图 11 局部优化器可能无法穿过必须暂时增大代价才能离开的“拐角”。
局部求解器通常只利用当前点附近的函数值和导数。若到达更好的通道必须暂时让代价变差,它可能不会主动“绕远”,而是停在局部解或报告失败。
若碰撞约束只在 个离散姿态上检查,相邻姿态之间的连续曲线仍可能穿过障碍物。因此优化后还应对轨迹进行更密的碰撞检查。若需要严格连续时间保证,还需要更强的连续碰撞检测或具备可证性的规划方法。〔5〕
本讲的核心是 Kinematics via Optimization。它可以限制关节位置、速度和加速度,但仅靠这些并不会自动计算执行所需的力矩,也不会自动满足接触动力学。优化得到的轨迹可能在几何上无碰撞,却要求机器人产生过大的速度、加速度或力矩,真实机器人无法实现。
MultibodyPlant.CalcJacobianSpatialVelocity()numpy.linalg.lstsq() / numpy.linalg.pinv() | ||
DifferentialInverseKinematicsParametersDoDifferentialInverseKinematics() | ||
InverseKinematicsMathematicalProgram + Solve() | ||
KinematicTrajectoryOptimizationSolve() |
MultibodyPlant + 线性代数Drake 先根据当前 Context 计算几何 Jacobian,再由 NumPy 求最小二乘解:
import numpy as np from pydrake.multibody.tree import JacobianWrtVariable J = plant.CalcJacobianSpatialVelocity( context=plant_context, with_respect_to=JacobianWrtVariable.kV, frame_B=gripper_frame, p_BoBp_B=[0,0,0], frame_A=plant.world_frame(), frame_E=plant.world_frame(), ) v = np.linalg.lstsq(J, V_desired, rcond=None)[0] # 教学上也常写成:v = np.linalg.pinv(J) @ V_desired kV 表示 Jacobian 的列对应 MultibodyPlant 中的广义速度 。lstsq() 直接求最小二乘问题,通常比先显式构造伪逆再相乘更合适。这里 Drake 负责生成与机器人模型一致的 ,伪逆或最小二乘则由数值线性代数完成。〔10〕
DoDifferentialInverseKinematics()from pydrake.multibody.inverse_kinematics import( DifferentialInverseKinematicsParameters, DoDifferentialInverseKinematics, ) params = DifferentialInverseKinematicsParameters( plant.num_positions(), plant.num_velocities() ) params.set_joint_position_limits((q_min, q_max)) params.set_joint_velocity_limits((v_min, v_max)) params.set_joint_acceleration_limits((a_min, a_max)) params.set_nominal_joint_position(q_nominal) result = DoDifferentialInverseKinematics(..., parameters=params) DoDifferentialInverseKinematics() 输出一个控制周期内的关节速度解。若要把它接入 Drake Diagram,还可以选择:
DifferentialInverseKinematicsIntegrator:反复求解关节速度,并积分成关节位置指令;DifferentialInverseKinematicsSystem:结合任务坐标系、活动自由度和碰撞检查器构成模块化微分 IK 系统;DifferentialInverseKinematicsController:把上述系统进一步封装成控制器。因此,核心求解层是 Parameters + DoDifferentialInverseKinematics(),Integrator/System/Controller 则属于连续执行和系统集成层。〔11〕
InverseKinematicsfrom pydrake.multibody.inverse_kinematics import InverseKinematics from pydrake.solvers import Solve ik = InverseKinematics(plant, plant_context) q = ik.q() prog = ik.prog() ik.AddPositionConstraint(...) ik.AddOrientationConstraint(...) ik.AddMinimumDistanceLowerBoundConstraint(d_safe) prog.AddQuadraticErrorCost(Q, q_nominal, q) prog.SetInitialGuess(q, q_initial) result = Solve(prog) q_solution = result.GetSolution(q) 可以把 InverseKinematics 理解为“机器人约束生成器”:它把位置、姿态和碰撞距离等要求添加到内部 MathematicalProgram,再由 Solve() 选择可用求解器并求出 。〔6、7〕
KinematicTrajectoryOptimizationfrom pydrake.planning import KinematicTrajectoryOptimization from pydrake.solvers import Solve trajopt = KinematicTrajectoryOptimization(...) trajopt.AddPositionBounds(q_min, q_max) trajopt.AddVelocityBounds(v_min, v_max) trajopt.AddAccelerationBounds(a_min, a_max) trajopt.AddPathLengthCost() trajopt.AddDurationCost() trajopt.SetInitialGuess(initial_bspline) result = Solve(trajopt.prog()) trajectory = trajopt.ReconstructTrajectory(result) ReconstructTrajectory() 返回 BsplineTrajectory。需要特别注意:该类中的速度是 ;对于包含四元数浮动关节的模型, 不一定等于 MultibodyPlant 定义的广义速度 。〔8〕
以上方法并非互相替代。一个实用系统可能用全局规划器给出粗路径,用轨迹优化将它平滑化,最后在执行期间用带约束的微分IK降低偏差。