机器人学笔记(3)矩阵的伪逆

左逆矩阵与右逆矩阵

对于机器人而言，求解雅可比的矩阵可以通过机器人各个关节的速度求得末端的速度与角速度，而求解逆运动学时，经常会出现已知末端机器人末端速度，求机器人各个关节速度的情况。

对于一个矩阵$A_{m\times n}$和矩阵$B_{n\times m}$，如果矩阵之间满足：
$$ABA=A$$
则称$B$是$A$的广义逆矩阵。

在机器人学之中，一般使用矩阵的左逆或者右逆，若$A$是满秩，则可以使用它的左逆或右逆矩阵。

若$m>n$，则使用左逆矩阵，若$n>m$，则使用右逆矩阵，下面直接给出左逆矩阵和右逆矩阵的形式：
$$\begin{gathered} A_{left}^{-1}=(A^{T}A{)}^{-1}{A}^T \\ {A}_{right}^{-1}=A^T(A{A}^T{)}^{-1} \end{gathered}$$

SVD奇异值分解求逆

上述的做法的问题在于，它要求矩阵的满秩，而在机器人正常运行时，有时会运动到奇异点附近，雅可比矩阵发生降秩，此时较小的关节角度要通过较大的速度实现，对机器人造成损害。

所以我们可以将矩阵进行奇异值分解再计算矩阵的逆，假设矩阵$A$是$m\times n$维矩阵，我们定义$A$矩阵的SVD为：
$$A=U\Sigma V^T$$
其中，$U\in {\mathbb{R}}_{m\times m}$​，$\Sigma \in {\mathbb{R}}_{m\times n}$​，$V\in {\mathbb{R}}_{n\times n}$，其中$U$、$V$都是酉矩阵，我们令$A^T$与$A$做矩阵乘法，得到一个$n\times n$的矩阵，对其求特征向量，有：
$$(A^{T}A)v_i={\lambda }_i{v}_i$$
$v_i$列向量的组合就是矩阵$V$，同样的道理，令$A$与$A_T$做矩阵乘法，有：
$$(A{A}^T)u_i={\lambda }_i{u}_i$$
$u_i$列向量的组合就是矩阵$U$，下面计算奇异值矩阵，有：
$$\Sigma =\left(\begin{array}{cccccc}{\sigma }_1& & & & |& \\ & {\sigma }_2& & & |& \\ & & ...& & |& \\ & & & {\sigma }_r& |& \\ -& -& -& -& 十& -\\ & & & & |& 0\end{array}\right)$$
对每一个${\sigma }_i$，有：
$$A{v}_i={\sigma }_i{u}_i$$
不过用这个式子来计算是比较困难的，我们一般计算出矩阵$A{A}^T$的特征值${\lambda }_i$用：
$${\sigma }_i=\sqrt{{\lambda }_i}$$
利用$SVD$计算矩阵的逆，得到的逆矩阵为：
$$A^{+}=V{\Sigma }^{+}{U}^T$$
在上式中
$${\Sigma }^{+}=\left(\begin{array}{cccccc}\frac{1}{{\sigma }_1}& & & & |& \\ & \frac{1}{{\sigma }_2}& & & |& \\ & & ...& & |& \\ & & & \frac{1}{{\sigma }_r}& |& \\ -& -& -& -& 十& -\\ & & & & |& 0\end{array}\right)$$
此时可以放弃阈值小于$ϵ$的奇异值，但会产生不连续性，利用阻尼最小二乘法计算伪逆的方式是在倒置奇异值之前为其设置一个下界：
$$A^{\ast }=A^T(A{A}^T+\lambda I{)}^{-1}$$
通过引入阻尼参数$\lambda$，避免分母为0，来进行平滑过渡。
$$A^{\ast }=V\left(\begin{array}{cccc}\frac{{\sigma }_1}{{\sigma }_1^2+\lambda }& & & \\ & \frac{{\sigma }_2}{{\sigma }_2^2+\lambda }& & \\ & & ...& \\ & & & \frac{{\sigma }_r}{{\sigma }_r^2+\lambda }\\ 0& 0& 0& 0\end{array}\right)U^T$$