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

左逆矩阵与右逆矩阵

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

对于一个矩阵Am×nA_{m\times n}Am×n和矩阵Bn×mB_{{n\times m}}Bn×m，如果矩阵之间满足：
ABA=AABA=A ABA=A
则称BBB是AAA的广义逆矩阵。

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

若m>nm>nm>n，则使用左逆矩阵，若n>mn>mn>m，则使用右逆矩阵，下面直接给出左逆矩阵和右逆矩阵的形式：
Aleft−1=(ATA)−1ATAright−1=AT(AAT)−1A_{left}^{-1}=(A^TA)^{-1}A^T\\ A_{right}^{-1}=A^T(AA^T)^{-1} Aleft−1=(ATA)−1ATAright−1=AT(AAT)−1

SVD奇异值分解求逆

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

所以我们可以将矩阵进行奇异值分解再计算矩阵的逆，假设矩阵AAA是m×nm\times nm×n维矩阵，我们定义AAA矩阵的SVD为：
A=UΣVTA=U\Sigma V^T A=UΣVT
其中，U∈Rm×mU \in \mathbb{R_{m \times m}}U∈Rm×m，Σ∈Rm×n\Sigma \in\mathbb{R_{m\times n}}Σ∈Rm×n，V∈Rn×nV \in\mathbb{R_{n\times n}}V∈Rn×n，其中UUU、VVV都是酉矩阵，我们令ATA^TAT与AAA做矩阵乘法，得到一个n×nn\times nn×n的矩阵，对其求特征向量，有：
(ATA)vi=λivi(A^TA)v_i=\lambda_iv_i (ATA)vi=λivi
viv_ivi列向量的组合就是矩阵VVV，同样的道理，令AAA与ATA_TAT做矩阵乘法，有：
(AAT)ui=λiui(AA^T)u_i=\lambda_iu_i (AAT)ui=λiui
uiu_iui列向量的组合就是矩阵UUU，下面计算奇异值矩阵，有：
Σ=(σ1∣σ2∣...∣σr∣−−−−十−∣0)\Sigma = \begin{pmatrix} \sigma_1&&&&|&\\ &\sigma_2&&&|&\\ &&...&&|&\\ &&&\sigma_r&|&\\ -&-&-&-&十&-\\ &&&&|&0 \end{pmatrix} Σ=⎝⎛σ1−σ2−...−σr−∣∣∣∣十∣−0⎠⎞
对每一个σi\sigma_iσi，有：
Avi=σiuiAv_i=\sigma_iu_i Avi=σiui
不过用这个式子来计算是比较困难的，我们一般计算出矩阵AATAA^TAAT的特征值λi\lambda_iλi用：
σi=λi\sigma_i = \sqrt{\lambda_i} σi=λi
利用SVDSVDSVD计算矩阵的逆，得到的逆矩阵为：
A+=VΣ+UTA^+=V\Sigma^+U^T A+=VΣ+UT
在上式中
Σ+=(1σ1∣1σ2∣...∣1σr∣−−−−十−∣0)\Sigma^+ = \begin{pmatrix} \dfrac{1}{\sigma_1}&&&&|&\\ &\dfrac{1}{\sigma_2}&&&|&\\ &&...&&|&\\ &&&\dfrac{1}{\sigma_r}&|&\\ -&-&-&-&十&-\\ &&&&|&0 \end{pmatrix} Σ+=⎝⎛σ11−σ21−...−σr1−∣∣∣∣十∣−0⎠⎞
此时可以放弃阈值小于ϵ\epsilonϵ的奇异值，但会产生不连续性，利用阻尼最小二乘法计算伪逆的方式是在倒置奇异值之前为其设置一个下界：
A∗=AT(AAT+λI)−1A^*=A^T(AA^T+\lambda I)^{-1} A∗=AT(AAT+λI)−1
通过引入阻尼参数λ\lambdaλ，避免分母为0，来进行平滑过渡。
A∗=V(σ1σ12+λσ2σ22+λ...σrσr2+λ0000)UTA^*=V\begin{pmatrix} \dfrac{\sigma_1}{\sigma_1^2+\lambda}&&&\\ &\dfrac{\sigma_2}{\sigma_2^2+\lambda}&&\\ &&...&\\ &&&\dfrac{\sigma_r}{\sigma_r^2+\lambda}\\ 0&0&0&0\\ \end{pmatrix}U^T A∗=V⎝⎛σ12+λσ10σ22+λσ20...0σr2+λσr0⎠⎞UT