python计算机器人运动学分析_机器人学之逆运动学数值解法及SVD算法

机器人学之逆运动学数值解法及SVD算法

文章目录

前言

这半个月的业余时间研究了机器人逆运动学的解析解法和迭代数值解法，并用程序实现。由于解析法只适合于特定结构的机器人，不具有通用性，因此这里不讨论解析解，只讨论迭代数值解法。本文相关的程序及研究素材都可以从github下载–链接 。

数值逆运动学

机器人逆运动学的解析解虽然很优美，但是有时候获得解析解很困难，甚至以人类的智慧无法求得解析解，这时候就可以用迭代数值解法。即使存在解析解，数值解法也经常用来提高解的精度。例如PUMA类型的机器人，最后三个轴由于机械制造和装配误差，实际上可能没有相交与一点，肩关节的轴线也可能不垂直。这种情况下，不必抛弃任何可用的解析解，这些解可以作为迭代数值解法的初始值。

求解非线性方程的根有多种迭代方法，我们将使用一种非线性求根的基本方法，牛顿拉普森法。此外，在不存在精确解的情况下，我们需要寻找最接近的近似解，这就需要优化方法。因此，我们现在讨论非线性求根的牛顿拉普森方法，以及优化的一阶必要条件。

牛顿-拉普森方法

对于一个给定的可微函数g:R→Rg:\mathbb R\to \mathbb Rg:R→R, 假设θ0\theta_0θ0​是一个猜测的初值，求方程g(θ)=0g(\theta)=0g(θ)=0的数值解。对g(θ)=0g(\theta)=0g(θ)=0在θ0\theta_0θ0​处进行一阶截断，其泰勒展开式为

g(θ)=g(θ0)+∂g∂θ(θ0)(θ−θ0)+higher-order termsg(\theta)=g(\theta_0)+\frac{\partial g}{\partial \theta}(\theta_0)(\theta-\theta_0)+\text{higher-order terms}g(θ)=g(θ0​)+∂θ∂g​(θ0​)(θ−θ0​)+higher-order terms

只保留一阶项，设g(θ)=0g(\theta)=0g(θ)=0，那么可求得θ\thetaθ为

θ=θ0−(∂g∂θ(θ0))−1g(θ0)\theta=\theta_0-\left( \frac{\partial g}{\partial \theta}(\theta ^0)\right)^{-1}g(\theta^0)θ=θ0​−(∂θ∂g​(θ0))−1g(θ0)

使用θ\thetaθ值作为新的猜测的解，重复上面的计算，就得到下面的迭代式

θk+1=θk−(∂g∂θ(θk))−1g(θ0).\theta^{k+1}=\theta^k-\left( \frac{\partial g}{\partial \theta}(\theta^k) \right)^{-1}g(\theta^0).θk+1=θk−(∂θ∂g​(θk))−1g(θ0).

重复上式的迭代直到某些判断满足，例如对于用户预先设定的值ϵ\epsilonϵ,满足$ g(\thetak)-g(\theta{k+1}|/|g(\theta^{k}|\le \epsilon$。

当ggg是多维的时候，也使用同样的公式。例如g:Rn→Rng: \mathbb R^n\to \mathbb R^ng:Rn→Rn,

∂θ∂θ(θ)=[∂g1∂θ1(θ)…∂g1∂θn(θ)⋮⋱⋮∂gn∂θ1(θ)…∂gn∂θn(θ)]∈Rn×n\frac{\partial \theta}{\partial \theta}(\theta)=\left[ \begin{matrix}

\frac{\partial g_1}{\partial \theta_1 }(\theta)&\ldots&\frac{\partial g_1}{\partial \theta_n }(\theta)\\

\vdots &\ddots \vdots \\

\frac{\partial g_n}{\partial \theta_1 }(\theta)&\ldots&\frac{\partial g_n}{\partial \theta_n }(\theta)\\

\end{matrix}\right]\in \mathbb R^{n\times n}∂θ∂θ​(θ)=⎣⎢⎡​∂θ1​∂g1​​(θ)⋮∂θ1​∂gn​​(θ)​…⋱⋮…​∂θn​∂g1​​(θ)∂θn​∂gn​​(θ)​⎦⎥⎤​∈Rn×n

下面我们讨论该矩阵不可逆的情形。

数值逆运动学算法

假设我们用一个坐标向量xxx表示末端坐标系，由正运动学x=f(θ)x=f(\theta)x=f(θ)控制, 这是一个非线性向量方程，映射了nnn个关节坐标到mmm个末端坐标。假设f:Rn→Rmf: \mathbb R^n\to \mathbb R^mf:Rn→Rm可导，设xdx_dxd​是期望的末端坐标。那么g(θ)g(\theta)g(θ)的牛顿拉普森方法定义为g(θ)=xd−f(θ)g(\theta)=x_d-f(\theta)g(θ)=xd​−f(θ), 目标是寻找关节坐标θ\thetaθ使得

g(θd)=xd−f(θd).g(\theta_d)=x_d-f(\theta_d).g(θd​)=xd​−f(θd​).

给定初始值θ0\theta_0θ0​ “接近”于一个解θd\theta_dθd​, 所谓“接近”只是认为其接近，不一定是接近，但在迭代过程中会逐渐接近，如图所示。运动学方程可写成泰勒展开式

(6.3)xd=f(θd)=f(θ0)+∂f∂θ∣θ0⎵J(θ0)(θd−θ0)⎵Δθ+h.o.tx_d=f(\theta_d)=f(\theta^0)+\underbrace{\frac{\partial f}{\partial \theta}|\theta^0}_{J(\theta^0)}\underbrace{(\theta_d-\theta^0)}_{\Delta\theta}+\text{h.o.t}\tag{6.3}xd​=f(θd​)=f(θ0)+J(θ0)∂θ∂f​∣θ0​​Δθ(θd​−θ0)​​+h.o.t(6.3)

这里J(θ0)∈Rm×nJ(\theta^0)\in \mathbb R^{m\times n}J(θ0)∈Rm×n是在θ0\theta^0θ0处的雅可比坐标。一阶截断泰勒展开式，我们得到上式的近似方程

(6.4)J(θ0)Δθ=xd−f(θ0).J(\theta^0)\Delta\theta=x_d-f(\theta^0).\tag{6.4}J(θ0)Δθ=xd​−f(θ0).(6.4)

假设J(θ0)J(\theta^0)J(θ0)是方阵并且可导，我们可以得到

(6.5)Δθ=J−1(θ0)(xd−f(θ0)).\Delta\theta=J^{-1}(\theta^0)(x_d-f(\theta^0)).\tag{6.5}Δθ=J−1(θ0)(xd​−f(θ0)).(6.5)

如果正运动学方程是关于θ\thetaθ的线性方程，那么方程(6.3)的高阶项是0，新的猜测值θ1=θ0+Δθ\theta^1=\theta^0+\Delta\thetaθ1=θ0+Δθ确实满足xd=f(θ1)x_d=f(\theta^1)xd​=f(θ1). 如果正运动学不是关于θ\thetaθ是线性的，这是常见的情形，这性的猜测值θ1\theta^1θ1仍然是比θ0\theta^0θ0更接近真实解的，这个过程不断重复，就会产生一个序列{θ0,θ1,θ2,… }\{ \theta^0,\theta^1,\theta^2,\dots \}{θ0,θ1,θ2,…}收敛于 θd\theta_dθd​(如图所示).

**图示说明：**牛顿拉普森法求解非线性方程的根。在第一步，斜率−∂f∂θ-\frac{\partial f}{\partial \theta}−∂θ∂f​在点(θ0,xd−f(θ0)\theta^0,x_d-f(\theta^0)θ0,xd​−f(θ0))计算得到。第二步，斜率在(θ1,xd−f(θ1)\theta^1,x_d-f(\theta^1)θ1,xd​−f(θ1))处计算得到，最后这样的过程会收敛于θd\theta_dθd​。 注意到，初始值在函数xd−f(θ)x_d-f(\theta)xd​−f(θ)的极点左边时，很可能导致收敛于另外一个根，如果初始值在极点或靠近极点时，会导致Δθ\Delta\thetaΔθ的初始值很大，迭代过程可能根本不收敛。

像上图显示那样，如果你运动学存在多解，迭代过程倾向于收敛到“最靠近”初始值θ0\theta^0θ0的根。你可以认为每个根有自己的吸引力区域，如果初始值没有落在这些吸引力区域(例如初始值收敛于一个根的效率很低)，迭代过程可能是不收敛的。

实际应用中，由于计算效率的原因，求解方程(6.4)通常不直接计算逆矩阵J−1(θ0)J^{-1}(\theta^0)J−1(θ0)。 有更高效的方法求解线性方程组Ax=bAx=bAx=b的方法。例如，对于可逆矩阵AAA, 进行LU分解来求解xxx计算量更少。例如在MATLAB里，下面命令

x=A\b

求解Ax=bAx=bAx=b时不计算A−1A^{-1}A−1。

如果由于不是方阵或奇异，JJJ不可逆，那么方程(6.5)的J−1J^{-1}J−1不存在。通过使用广义逆(Moore-Penrose pseudoinverse) J†J^{\dagger}J†替换方程(6.5)的J−1J^{-1}J−1， 方程(6.4)仍然可以求解Δθ\Delta\thetaΔθ(或者说求近似解)。对于形式为Jy=zJy=zJy=z的方程，J∈Rm×n,y∈Rn,z∈RmJ\in \mathbb R^{m\times n}, y\in \mathbb R^n, z\in \mathbb R^mJ∈Rm×n,y∈Rn,z∈Rm ，它的解为

y∗=J†zy*=J^{\dagger}zy∗=J†z

需要分两种情况讨论得到的解

该解y∗y*y∗确实满足Jy∗=zJy*=zJy∗=z， 并且任意解yyy满足Jy=zJy=zJy=z，并且∣∣y∗∣∣≤∣∣y∣∣||y*||\le||y||∣∣y∗∣∣≤∣∣y∣∣。 也就是说，在所有的解中，y∗y*y∗的2范数最小。机器人的关节数n大于末端坐标m时，Jy=zJy=zJy=z有无穷多个解，这时称JJJ是“胖”的。

如果没有yyy满足Jy=zJy=zJy=z， 那么y∗y*y∗是误差的2范数最小的解，即对于任意y∈Rn,∣∣Jy∗−z∣∣≤∣∣Jy−z∣∣y\in \mathbb R^n ,||Jy*-z||\le ||Jy-z||y∈Rn,∣∣Jy∗−z∣∣≤∣∣Jy−z∣∣ . 这种情形对应于rank J≤mJ\le mJ≤m。例如机器人的关节数小于末端坐标数m("瘦"的雅可比JJJ)或者出现奇异。

有很多计算伪逆(广义逆)的方法，在matlab里是

y=pinv(J)*z

当JJJ是满秩(rank mmm，n>mn>mn>m 或秩 rank nnn, n<mn<mn

J†=JT(JJT)−1J^{\dagger}=J^T(JJ^T)^{-1}J†=JT(JJT)−1 如果JJJ是胖的, n>mn>mn>m (称为右逆，因为JJ†=IJJ^{\dagger}=IJJ†=I)

J†=(JTJ)−1JJ^{\dagger}=(J^TJ)^{-1}JJ†=(JTJ)−1J 如果JJJ是瘦的，n<mn<mn

使用伪逆计算雅可比的逆，方程(6.5)可以写成

(6.6)Δθ=J†(θ0)(xd−f(θ0)).\Delta \theta=J^{\dagger}(\theta^0)(x_d-f(\theta^0)).\tag{6.6}Δθ=J†(θ0)(xd​−f(θ0)).(6.6)

如果rank(J)<m\text {rank }(J)<mrank (J)m ，该解是满足方程(6.4)的关节变量改变最小的解(从2范数角度看)。

在牛顿-拉普森迭代算法中应用方程(6.6)求解θd\theta_dθd​分为两步

(1) 初始化：给定xd∈Rmx_d\in \mathbb R^mxd​∈Rm 以及初始θ0∈Rn\theta^0\in \mathbb R^nθ0∈Rn, 令i=0i=0i=0。

(2)设 e=xd−f(θi)e=x_d-f(\theta^i)e=xd​−f(θi). 当对于某个较小的ϵ\epsilonϵ，当∣∣e∣∣>ϵ||e||>\epsilon∣∣e∣∣>ϵ:

设置 θi+1=θi+J†(θI)e\theta^{i+1}=\theta^i+J^{\dagger}(\theta^I)eθi+1=θi+J†(θI)e

增加 iii.

适当修改该算法，就可以处理机器人逆运动学问题。机器人末端构型用Tsd∈SE(3)T_{sd}\in SE(3)Tsd​∈SE(3)表示, 替换坐标矢量xdx_dxd​, 坐标雅可比JJJ用末端的物体雅可比Jb∈R6×nJ_b\in \mathbb R^{6\times n}Jb​∈R6×n替换. 但是，向量e=xd−f(θi)e=x_d-f(\theta^i)e=xd​−f(θi)， 表示当前猜测值到末端期望构型的方向(通过正运动学计算)，不能简单地用Tsd−Tsb(θi)T_{sd}-T_{sb}(\theta^i)Tsd​−Tsb​(θi)代替。JbJ_bJb​的伪逆需要对在物体坐标系表示的旋量Vb∈R6\mathcal V_b\in \mathbb R^6Vb​∈R6进行变换. 为了正确类比，我们把e=xd−f(θi)e=x_d-f(\theta^i)e=xd​−f(θi)看成是一个速度矢量，如果经过一个单位时间，将会产生从f(θi)f(\theta^i)f(θi)向xdx_dxd​的运动。相似地，我们应该寻找一个物体坐标系旋量Vb\mathcal V_bVb​, 经过单位时间，将会产生一个从Tsb(θi)T_{sb}(\theta^i)Tsb​(θi)向期望构型TsdT_{sd}Tsd​的运动。

为了寻找这个Vb\mathcal V_bVb​， 我们首先计算期望构型在当前物体坐标系的表达

Tbd(θi)=Tsb−1(θi)Tsd=Tbs(θi)Tsd.T_{bd}(\theta^i)=T_{sb}^{-1}(\theta^i)T_{sd}=T_{bs}(\theta^i)T_{sd}.Tbd​(θi)=Tsb−1​(θi)Tsd​=Tbs​(θi)Tsd​.

然后Vb\mathcal V_bVb​通过矩阵对数计算

[Vb]=logTbd(θi)[\mathcal V_b]=\text {log}T_{bd}(\theta^i)[Vb​]=logTbd​(θi)

这样可以得到以下的逆运动学算法，类似于前面的坐标向量算法：

(1)初始化：给定TsdT_{sd}Tsd​ 和初始值θ0∈Rn\theta^0\in \mathbb R^nθ0∈Rn, 设置i=0i=0i=0.

(2)设置[Vb]=log(Tsb−1(θi))[\mathcal V_b]=\text{log}(T_{sb}^{-1}(\theta^i))[Vb​]=log(Tsb−1​(θi)), 对给定的误差ϵw,ϵv\epsilon_w ,\epsilon_vϵw​,ϵv​, 当$||\omega_b||>\epsilon_{\omega} $ 或者∣∣vb∣∣>ϵv||v_b||>\epsilon_{v}∣∣vb​∣∣>ϵv​:

设置 θi+1=θi+Jb†(θi)Vb\theta^{i+1}=\theta^i+J_b^{\dagger}(\theta^i)\mathcal V_bθi+1=θi+Jb†​(θi)Vb​.

增加 iii.

应用空间雅可比Js(θ)J_s(\theta)Js​(θ)和空间旋量Vs=[AdTsb]Vb\mathcal V_s=[\text{Ad}_{T_{sb}}]\mathcal V_bVs​=[AdTsb​​]Vb​, 可以推导在固定坐标系的等效形式。

为了数值逆运动学方法收敛，初始值θ0\theta^0θ0需要高效地收敛于θd\theta_dθd​. 从机器人初始构型开始，就可以满足条件。在初始位置末端的真实构型以及关节角度都是已知的，同时保证末端位置TsdT_{sd}Tsd​在逆运动学计算过程中是缓慢变化的。

补充 ：

以上的分析或许有人觉得有点难理解，例如TbdT_{bd}Tbd​的物理意义是啥？是在哪两个坐标系或矢量的变换？下面结合这个图分析一下，或许有助于理解。

我们知道，一个齐次矩阵，在不同场合可以有三种不同的含义，(1)它是坐标系的描述，或者说构型，例如TsdT_{sd}Tsd​表示坐标系{bd}\{b_d\}{bd​}在坐标系{s}\{s\}{s}中的描述，或者说坐标系{bd}\{b_d\}{bd​}的构型。(2)它是变换映射。例如TsdT_{sd}Tsd​可以看成是把在坐标系{bd}\{b_d\}{bd​}表示的物理量映射到在坐标系{s}\{s\}{s}中表示。(3)它是一个变换算子，或者说刚体运动。例如TsdT_{sd}Tsd​可以看成是把坐标系{s}\{s\}{s}变换的坐标系{bd}\{b_d\}{bd​}的变换算子，或者说{s}\{s\}{s}运动到{bd}\{b_d\}{bd​}所做的刚体运动。在我们的以上的迭代算法中，我们应该把M,Tsb(θi),TsdM, T_{sb}(\theta^i), T_{sd}M,Tsb​(θi),Tsd​理解为第一种含义，即坐标系的描述(或构型)，把TbdT_{bd}Tbd​理解为第三种含义，即变换算子(或者说刚体运动)。因此，机器人末端{b}\{b\}{b}的位姿(构型)初始状态在{b0}\{b_0\}{b0​}处，MMM表示机器人末端在坐标系{s}\{s\}{s}中的描述，在任意迭代步中，机器人末端{b}\{b\}{b}运动到 {bi}\{b_i\}{bi​}, Tsb(θi)T_{sb}(\theta^i)Tsb​(θi)表示此时机器人末端{bi}\{b_i\}{bi​}在坐标系{s}\{s\}{s}中的描述，这时末端当前位姿(构型)Tsb(θi)T_{sb}(\theta^i)Tsb​(θi)与期望位姿(构型)TsdT_{sd}Tsd​存在一个"差值" , 与xd−f(θi)x_d-f(\theta^i)xd​−f(θi)类比. 这个“差值”TbdT_{bd}Tbd​与向量(或者标量)xd−f(θi)x_d-f(\theta^i)xd​−f(θi)不同，该"差值"是一个变换算子，即刚体运动，表示末端从当前位姿Tsb(θi)T_{sb}(\theta^i)Tsb​(θi) (变换)运动到期望位姿TsdT_{sd}Tsd​所需做的(变换算子)刚体运动。那么这个关系可以表示为

TbdTsb(θi)=TsdT_{bd}{}T_{sb}(\theta^i)=T_{sd}Tbd​Tsb​(θi)=Tsd​

因此有

Tbd=Tbd(θi)=Tsb(θi)−1TsdT_{bd}=T_{bd}(\theta^i)=T_{sb}(\theta^i)^{-1}T_{sd}Tbd​=Tbd​(θi)=Tsb​(θi)−1Tsd​

那么这个刚体运动是一个螺旋运动, 指数坐标为VbV_bVb​，它是TbdT_{bd}Tbd​的对数。如图所示，当然TbdT_{bd}Tbd​也可以理解为前面所说的，期望构型在当前物体坐标系的表达。里面包含两个分量，角度位移wbw_bwb​和线位移vbv_bvb​ 。若对给定的误差ϵw,ϵv\epsilon_w ,\epsilon_vϵw​,ϵv​, 当$||\omega_b||\le \epsilon_{\omega} $ 且∣∣vb∣∣≤ϵv||v_b||\le \epsilon_{v}∣∣vb​∣∣≤ϵv​，那说明当前位姿与期望位姿非常接近，所需做的刚体运动非常小了。当小于给定误差，这时迭代停止，对应的θi\theta^iθi即为所需的解。因此有前面的逆运动学算法。

奇异值(SVD)分解算法

前面提到的J†=JT(JJT)−1J^{\dagger}=J^T(JJ^T)^{-1}J†=JT(JJT)−1 或J†=(JTJ)−1JJ^{\dagger}=(J^TJ)^{-1}JJ†=(JTJ)−1J 计算伪逆，都是要求解逆矩阵的。这里介绍一种应用广泛的方法–奇异值分解(SVD, Singular Value Decomposition ) ，求伪逆矩阵。

在许多情况下，高斯消去法和LU分解法都不能给出满意的结果。有一套非常有用的技术，用来该处理不论是奇异还是数值上非常接近奇异的方程组或矩阵。该方法称为奇异值值分解(SVD)。该方法会精确地告诉我们什么情况属于该类问题。某些情况下，SVD不仅可以判断该问题，还可以求解它，提供一个有用的数值解，尽管这个解不一定是我们想要的。SVD也可以用来求解多数的最小二乘问题。

SVD技术基于下面的线性代数定理。任意M×NM\times NM×N的矩阵AAA, 其行数MMM大于或等于列数NNN， 可以写成一个M×NM\times NM×N的列正交矩阵UUU， 一个N×NN\times NN×N的元素均为正数或零(奇异值)的对角阵WWW, 以及一个N×N