机器人逆运动学:从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−1q˙(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(JWqy−x˙)∥2(4.3)
整理一下,令 J ^ = W x J W q \hat J = W_x J W_q J^=WxJWq, v ^ = W x x ˙ \hat v = W_x \dot x v^=Wxx˙, 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=yminJ^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˙=Wqy(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展开说明.
参考文献
- [1] “逆运动学”——从操作空间到关节空间（下篇）
- [2] pinocchio例子
- [3] 机器人的运动学解——逆向运动学解
- [4] 全面理解奇异值分解
- [5] SVD分解与最小二乘
