机器人逆运动学:从SVD到IK算法

优质文章学习记录

— 13 min read
机器人逆运动学:从SVD到IK算法

引言

在这里插入图片描述


最近接触的机器人项目需要实现在特定约束下的逆运动学解算,而直接套用目前大多数开源IK算法(如KDL/TRAC IK等)或多或少都存在一些问题,因此需要自行实现迭代IK并添加特定的约束.然而,目前普遍教科书都倾向于介绍解析解IK,对数值IK更多是从网络博客收集整理获得,因此决定写下本文将这部分的知识作一个总结.
本文将从基础的线性代数SVD开始介绍,逐步过渡到数值IK的求解中,并以KDL的源码为例进一步剖析实际应用时的写法.

SVD与最小二乘法

SVD是工程数学常用的工具,数学上看起来只是对矩阵做分解,但其在工程上可应用的地方很多,如奇异值和特征向量可以作为降维(PCA),除此以外分解后的矩阵有良好的性质方便求逆. 下面将详细介绍SVD如何用于求逆.
线性代数常见的一个问题:
A x = b (1.1) Ax=b \tag{1.1} Ax=b(1.1)
求 x x x最直观的一个理解是对矩阵 A A A求逆,其解就是 A − 1 b A^{-1}b A−1b,但可能存在以下问题:

  • A A A不可逆;
  • 即使 A A A可逆,求逆的效率很低,对于工程应用 A A A的维度往往很大,直接求逆不显实;
  • 工程应用中数据往往含有噪声,这意味着上式往往没有准确解,只能拟合出一个最优解.

综上原因,我们经常不直接求解原问题,而是将上面的求解方程的问题转换成一个最小二乘问题,具体而言, A x = b Ax=b Ax=b可看成一个拟合问题:
m i n x ∣ ∣ A x − b ∣ ∣ 2 (1.2) \underset{x}{min}||Ax-b||^2 \tag{1.2} xmin​∣∣Ax−b∣∣2(1.2)

那么这时候可以使用SVD来求解上面的最小二乘问题,这里给出简单的数学推导
上式是二次型,所以其导数为0处即取得极值:
A T ( A x − b ) = 0 (1.3) A^T(Ax-b)=0 \tag{1.3} AT(Ax−b)=0(1.3)
易得:
x = ( A T A ) − 1 A T b (1.4) x=(A^T A )^{-1} A^T b \tag{1.4} x=(ATA)−1ATb(1.4)
这么看 ( A T A ) − 1 A T (A^T A )^{-1} A^T (ATA)−1AT就是原问题 A x = b Ax=b Ax=b中的 A A A的逆,这种形式的"逆"有一个专有名字,称为 伪逆,记作 A + A^+ A+ ,放在机器人逆运动学也经常出现,下一节将涉及. 回到当前问题,现在似乎还是不可避免需要求逆,因此接下来先对 A A A 作SVD分解:
A = U Σ V T (1.5) A = U \Sigma V^T \tag{1.5} A=UΣVT(1.5)
因为 U U U 和 V V V 是正交矩阵,有良好的性质,代入 式1.4 将得出以下简单的结果
x = V Σ − 1 U T b (1.6) x = V \Sigma^{-1} U^T b \tag{1.6} x=VΣ−1UTb(1.6)
经过上述推导,将SVD与最小二乘联系起来. 除上面的推导以外,还可以参考SVD分解与最小二乘,文章提供了另一种推导过程.

数值IK

有了上面的关于SVD和最小二乘的铺垫,我们再回来机器人学的逆运动学求解问题.
不同于大多数机器人学教科书教授的解析解,可以直接得到目标位姿与角度的关系.然而,解析解一般需要满足特定的条件,因此只有特殊的构型才能使用解析解.而数值解一般只需要提供机械臂的模型即可求解.
数值IK的核心思想是迭代优化,因此先有一个初始值 q 0 q_0 q0​,通过正运动学(FK)容易得出当前的位姿与目标位姿的偏差 Δ T \Delta T ΔT,假设我们期望机器人沿着目标位姿的方向移动,那么可以将位姿的偏差除以单位时间即可求出目标方向 x ˙ \dot x x˙,已知任务空间(笛卡尔空间)速度下,通过雅可比矩阵容易得到关节空间的速度:
x ˙ = J q ˙ (2.1) \dot x = J \dot q \tag{2.1} x˙=Jq˙​(2.1)
得到关节空间速度后对关节位置进行更新:
q t = q t − 1 + q ˙ Δ t (2.2) q_{t} = q_{t-1} + \dot q \Delta t \tag{2.2} qt​=qt−1​+q˙​Δt(2.2)
注意雅可比矩阵是随着不同位姿变化的,因此理论通过上 式2.1 和 式2.2 求关节空间速度 q ˙ \dot q q˙​其实并不能使问题一步到位得出,而是需要一直迭代,更新雅可比矩阵,更新关节位置,使得 Δ T \Delta T ΔT(等同于 x ˙ \dot x x˙) 趋近于0.
细心的同学可能早就发现 x ˙ = J q ˙ \dot x = J \dot q x˙=Jq˙​ 不就是第一节的 A x = b Ax = b Ax=b 问题吗?因此
q ˙ = ( J T J ) − 1 J T x ˙ (2.3) \dot q = (J^T J)^{-1} J^T \dot x \tag{2.3} q˙​=(JTJ)−1JTx˙(2.3)
机器人学中常说的雅可比矩阵伪逆就是 J + = ( J T J ) − 1 J T J^+ = (J^T J)^{-1} J^T J+=(JTJ)−1JT, 同样地可以通过SVD来提高计算效率和稳定性,所以进一步地有
q ˙ = V Σ − 1 U T x ˙ (2.4) \dot q = V \Sigma ^ {-1} U^T \dot x \tag{2.4} q˙​=VΣ−1UTx˙(2.4)

阻尼最小二乘IK

实际上,单纯的数值IK仍然是无法应用在真实问题当中.因为机器人存在很多奇异位,直观描述是机器人在某些特殊的位姿下会丢失自由度,包括我们人体手臂本身,在某些姿态下我们无法使得手部往特定方向平移或旋转;这用数学语言描述就是,雅可比矩阵缺秩.这时候,对雅可比矩阵进行奇异值分解,所损失的自由度对应其奇异值极小(甚至为0)的特征向量.
奇异值为极小或者为0存在什么问题?注意 式2.4 有一项 Σ − 1 \Sigma ^ {-1} Σ−1, 这意味着即使是任务空间的小幅运动将会导致关节空间有极大的增量.如果将上述IK直接应用于控制中,机器人将在这些奇异位出现不稳定的抖动现象,对于逆运动学求解而言,将会导致迭代效率下降甚至无法收敛,可以想象一下一个梯度下降的优化问题,在接近目标时突然使优化变量增加了一个极大值.
为了解决上述问题,阻尼最小二乘IK在数值IK基础上引入一个阻尼项(惩罚项):
L = min ⁡ q ˙ ∥ J q ˙ − x ˙ ∥ 2 + λ 2 ∥ q ˙ ∥ 2 (3.1) L = \min_{\dot{q}} \left\| J\dot{q} - \dot{x} \right\|^2 + \lambda^2 \left\| \dot{q} \right\|^2 \tag{3.1} L=q˙​min​∥Jq˙​−x˙∥2+λ2∥q˙​∥2(3.1)
同理,对上式求导并使其等于0有:
q ˙ = ( J T J + λ 2 I ) − 1 J T x ˙ (3.2) \dot{q} = (J^T J + \lambda^2 I) ^{-1} J^T \dot{x} \tag{3.2} q˙​=(JTJ+λ2I)−1JTx˙(3.2)
再结合SVD有:
q ˙ = V D − 1 U T x ˙ (3.3) \dot q = V D ^ {-1} U^T \dot x \tag{3.3} q˙​=VD−1UTx˙(3.3)
其中 D D D 仍然是对角矩阵,只是在每一项奇异值的基础上增加了阻尼系数
d i = σ 2 + λ 2 σ (3.4) d_i = \frac{\sigma^2 + \lambda^2}{\sigma} \tag{3.4} di​=σσ2+λ2​(3.4)
从上式可知,由于引入了 λ \lambda λ 项,使奇异值 > λ \lambda λ ,这样 q ˙ \dot q q˙​ 的值更稳定.如果 λ \lambda λ 接近0,则其效果等效于普通的数值IK;如果 λ \lambda λ 取值较大容易导致迭代方向偏离目标,因此实际需要权衡其值的大小.

著名的pinocchio动力学库官方提供了阻尼最小二乘IK的python实现

import numpy as np import pinocchio from numpy.linalg import norm, solve model = pinocchio.buildSampleModelManipulator() data = model.createData() JOINT_ID =6 oMdes = pinocchio.SE3(np.eye(3), np.array([1.0,0.0,1.0])) q = pinocchio.neutral(model) eps =1e-4 IT_MAX =1000 DT =1e-1 damp =1e-12 i =0whileTrue: pinocchio.forwardKinematics(model, data, q) iMd = data.oMi[JOINT_ID].actInv(oMdes) err = pinocchio.log(iMd).vector # in joint frameif norm(err)< eps: success =Truebreakif i >= IT_MAX: success =Falsebreak J = pinocchio.computeJointJacobian(model, data, q, JOINT_ID)# in joint frame J =-np.dot(pinocchio.Jlog6(iMd.inverse()), J) v =-J.T.dot(solve(J.dot(J.T)+ damp * np.eye(6), err)) q = pinocchio.integrate(model, q, v * DT)ifnot i %10:print(f"{i}: error = {err.T}") i +=1if success:print("Convergence achieved!")else:print("\n""Warning: the iterative algorithm has not reached convergence ""to the desired precision")

KDL-IK

接下来再对比一个moveit2常用的IK插件,KDL-IK.
KDL-IK在数值IK基础上加入了一个实际应用很实用的权重,包括任务空间权重 W x W_x Wx​ 和 关节空间权重 W q W_q Wq​,两个权重矩阵均为对角阵.
对于任务空间权重,其目的就是使得求解IK时能够根据实际需求优先考虑目标姿态的特定方向,比如某些需求中只要求机械臂的末端能够到达指定位置,对姿态没有特定要求等,引入任务空间权重后,最小二乘的问题变成如下:
L = min ⁡ q ˙ ∥ W x ( J q ˙ − x ˙ ) ∥ 2 (4.1) L = \min_{\dot{q}} \left\| W_x(J\dot{q} - \dot{x}) \right\|^2 \tag{4.1} L=q˙​min​∥Wx​(Jq˙​−x˙)∥2(4.1)
显然,权重矩阵 W x W_x Wx​ 对目标位姿进行缩放处理进而达到对位姿优先级的控制.
而对于关节空间权重,其目的则是在求解IK的同时优先考虑部分关节,尤其适合冗余自由度下,根据实际需求优先使用指定关节,或压制指定关节的运动.
为了引入关节空间权重,需要定义新的优化变量 y y y
y = W q − 1 q ˙ (4.2) y = W_q^{-1} \dot q \tag{4.2} y=Wq−1​q˙​(4.2)
y y y 的意义也很明确,就是未被加权的虚拟关节速度,那么最小二乘问题演变为:
L = min ⁡ y ∥ W x ( J W q y − x ˙ ) ∥ 2 (4.3) L = \min_{y} \left\| W_x(J W_q y - \dot{x}) \right\|^2 \tag{4.3} L=ymin​∥Wx​(JWq​y−x˙)∥2(4.3)
整理一下,令 J ^ = W x J W q \hat J = W_x J W_q J^=Wx​JWq​, v ^ = W x x ˙ \hat v = W_x \dot x v^=Wx​x˙, J ^ \hat J J^ 看作加权后的雅可比矩阵, v ^ \hat v v^ 看作加权后的速度
L = min ⁡ y ∥ J ^ y − v ^ ∥ 2 (4.4) L = \min_{y} \left\| \hat J y - \hat v \right\|^2 \tag{4.4} L=ymin​​J^y−v^​2(4.4)
由此,上面的形式和普通的数值IK类似,同样可以使用SVD求解.

// weight Jacobian auto& jac = jac_reduced_.data; const Eigen::Index rows = svd_.rows(); // only operate on position rows? jac.topRows(rows) *= joint_weights.asDiagonal(); jac.topRows(rows).transpose() *= cartesian_weights.topRows(rows).asDiagonal();

上面的操作就是求解加权后的雅可比矩阵 J ^ \hat J J^

// transform v_in to 6D Eigen::Vector Eigen::Matrix<double, 6, 1> vin; vin.topRows<3>() = Eigen::Map<const Eigen::Array3d>(v_in.vel.data, 3) * cartesian_weights.topRows<3>().array(); vin.bottomRows<3>() = Eigen::Map<const Eigen::Array3d>(v_in.rot.data, 3) * cartesian_weights.bottomRows<3>().array();

上面的操作就是求解加权后的速度 v ^ \hat v v^

// Do a singular value decomposition: J = U*S*V^t svd_.compute(jac.topRows(rows)); qdot_out.data.noalias() = svd_.solve(vin.topRows(rows)); qdot_out.data.array() *= joint_weights.array();

上面代码便是求解SVD,注意这里svd的解是 y y y, 但我们的目标是 q ˙ \dot q q˙​,因此需要根据其定义恢复:
q ˙ = W q y (4.5) \dot q = W_q y \tag{4.5} q˙​=Wq​y(4.5)

下面附上moveit2中KDL的加权数值IK的源码实现:

// NOLINTNEXTLINE(readability-identifier-naming) int ChainIkSolverVelMimicSVD::CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out, const Eigen::VectorXd& joint_weights, const Eigen::Matrix<double, 6, 1>& cartesian_weights) { // Let the ChainJntToJacSolver calculate the Jacobian for the current joint positions q_in. if (num_mimic_joints_ > 0) { jnt2jac_.JntToJac(q_in, jac_); // Now compute the actual jacobian that involves only the active DOFs jacToJacReduced(jac_, jac_reduced_); } else jnt2jac_.JntToJac(q_in, jac_reduced_); // weight Jacobian auto& jac = jac_reduced_.data; const Eigen::Index rows = svd_.rows(); // only operate on position rows? jac.topRows(rows) *= joint_weights.asDiagonal(); jac.topRows(rows).transpose() *= cartesian_weights.topRows(rows).asDiagonal(); // transform v_in to 6D Eigen::Vector Eigen::Matrix<double, 6, 1> vin; vin.topRows<3>() = Eigen::Map<const Eigen::Array3d>(v_in.vel.data, 3) * cartesian_weights.topRows<3>().array(); vin.bottomRows<3>() = Eigen::Map<const Eigen::Array3d>(v_in.rot.data, 3) * cartesian_weights.bottomRows<3>().array(); // Do a singular value decomposition: J = U*S*V^t svd_.compute(jac.topRows(rows)); if (num_mimic_joints_ > 0) { qdot_out_reduced_.noalias() = svd_.solve(vin.topRows(rows)); qdot_out_reduced_.array() *= joint_weights.array(); for (unsigned int i = 0; i < chain_.getNrOfJoints(); ++i) qdot_out(i) = qdot_out_reduced_[mimic_joints_[i].map_index] * mimic_joints_[i].multiplier; } else { qdot_out.data.noalias() = svd_.solve(vin.topRows(rows)); qdot_out.data.array() *= joint_weights.array(); } return 0; }

小结

本文从经典问题 A x = b Ax=b Ax=b展开,引入了最小二乘的解法,同时结合奇异值分解(SVD)来避免矩阵求逆;在以上铺垫上回到机器人逆运动学问题上,讲解了如何利用雅可比矩阵通过迭代方式求目标位姿下的关节坐标;再结合实际应用中遇到的奇异位(缺秩)问题,引入阻尼最小二乘法IK,展示了pinocchio的官方例子;最后还简单家介绍了数值IK还会考虑的任务空间权重和关节空间权重问题,结合moveit2的KDL-IK展开说明.

参考文献

Read more

无人机视觉语言导航从入门到精通（一）：什么是无人机视觉语言导航

无人机视觉语言导航从入门到精通（一）：什么是无人机视觉语言导航 摘要 视觉语言导航（Vision-Language Navigation, VLN）是人工智能领域的前沿研究方向，它使智能体能够根据自然语言指令，在视觉环境中自主导航至目标位置。当这一技术应用于无人机平台时，便形成了无人机视觉语言导航（UAV Vision-Language Navigation）这一新兴研究领域。本文作为系列博客的开篇，将系统介绍视觉语言导航的基本概念、问题形式化定义、核心挑战、应用场景，并对整个系列的内容进行导读。 关键词：视觉语言导航、无人机、多模态学习、具身智能、自然语言处理 一、引言 1.1 从一个场景说起 设想这样一个场景：你站在一个陌生城市的街头，手中拿着一架小型无人机。你对无人机说："飞到前方那栋红色建筑的左侧，然后沿着河边向北飞行，在第二座桥附近降落。"无人机收到指令后，自主起飞，识别周围环境中的建筑、河流、桥梁等地标，规划路径，最终准确到达你所描述的位置。

基于单片机的喷漆机器人自动控制系统

1. 系统总体概述 点击下载protues仿真设计：https://download.ZEEKLOG.net/download/qq_39020934/92091236 基于单片机的喷漆机器人自动控制系统是一种将嵌入式控制技术与工业自动化理念相结合的典型应用系统，主要面向教学实验、小型自动化设备设计以及喷涂工艺流程模拟等应用场景。喷漆机器人在现代制造业中被广泛应用于汽车制造、机械设备表面处理和家电外壳喷涂等领域，其核心目标是通过自动化控制提高喷漆均匀性、降低人工劳动强度并提升作业安全性。 本系统以单片机作为核心控制单元，通过程序对喷漆动作、运行模式以及执行机构进行统一调度和管理。系统采用气动传动方式完成喷漆动作，通过继电器模拟气动阀的开关状态，实现对喷漆过程的精确控制。同时，引入手动模式与自动模式两种控制方式，以适应调试、维护和自动运行等不同应用需求。在系统运行过程中，喷嘴的位置信息以坐标形式实时显示在数码管上，便于操作人员直观掌握喷漆位置和运动状态。 整个系统结构清晰，模块划分明确，既体现了单片机在自动控制系统中的核心作用，又反映了工业控制系统中常见的执行机构驱动、模式切换
机器人系统SLAM讲解

机器人系统SLAM讲解

考点 1：SLAM 基本概念与目标 1. 核心定义 SLAM 全称是 Simultaneous Localization and Mapping，翻译为 同时定位与建图。 * 定位：机器人知道自己在环境中的 位置和姿态（位姿），比如 “我在教室第 3 排桌子左边，面朝黑板”。 * 建图：机器人根据传感器数据，画出周围环境的 地图，比如教室的桌子、墙壁的位置。 * 同时：定位和建图不是分开做的 —— 机器人一边用传感器 “看” 环境画地图，一边根据画好的部分地图判断自己的位置。 2. 最终输出 * 一张 环境地图（比如栅格地图、点云地图）。 * 机器人在地图上的 位姿轨迹（比如从起点到终点，每一刻的位置和朝向）。 3. 与 “纯定位” 的区别 对比项SLAM纯定位（
深度解析英伟达最新“瓦力”机器人：物理AI时代的开发者红利与技术突破

深度解析英伟达最新“瓦力”机器人：物理AI时代的开发者红利与技术突破

2026年CES展会上，黄仁勋牵着那款酷似《机器人总动员》“瓦力”的Reachy Mini机器人完成流畅互动时，全场的欢呼不仅是对萌系设计的认可，更是对一个新时代的致敬——英伟达用这套全新机器人系统，正式宣告物理AI从实验室走向产业化。对于咱们ZEEKLOG的开发者而言，这波技术浪潮带来的不只是视觉震撼，更是可落地的开发工具、开源生态和商业机遇。今天就从技术内核、开发价值、行业对比三个维度，深度拆解英伟达最新机器人的核心竞争力，帮大家找准入局切入点。 一、不止“萌出圈”：英伟达新机器人的技术内核拆解 很多人被“瓦力”的外形圈粉，但真正让行业震动的是其背后的全栈技术体系。不同于传统机器人“硬件堆砌+单一功能编程”的模式，英伟达这套系统是“大脑-身体-训练场”的全链路协同，每一个环节都为开发者预留了创新空间。 1. 核心大脑：GR00T N1.6模型的双系统突破 作为全球首个开源人形机器人基础模型，最新的Isaac GR00T N1.6堪称“机器人界的GPT-4o”，其最核心的创新是双系统架构设计，完美复刻了人类“本能反应+深度思考”