复现KDL:使用Msnhnet实现串联机器人运动数值逆解(一）

最新推荐文章于 2024-08-12 08:31:03 发布

穆士凝魂

最新推荐文章于 2024-08-12 08:31:03 发布

阅读量1k

点赞数 3

分类专栏： ROS 文章标签：机械臂人工智能逆解数值解

本文链接：https://blog.csdn.net/MSNH2012/article/details/117378915

版权

ROS 专栏收录该内容

24 篇文章 35 订阅

订阅专栏

串联机械臂的逆解是一个求解多元非线性方程解的过程。当然，市面上的众多商业臂也都是采用符合pieper准则的构型，可以用解析法或者几何法直接推导出解析式，从而得到封闭解。
符合peiper准则的puma560机械臂
而对于不符合peiper准则的臂或者运动学冗余的臂，数值解也是不错的方法。当然，速度肯定是比不上解析法了，而且每次的解只有一个，且每次求解的结果都不一样，当然，好处就是在通用平台上，可以快速搭建机器人系统。
7轴冗余机械臂kuka iiwa
串联机器人运动学方程可写为：
$p=f(\Theta), \Theta=[\theta_1,\theta_2...\theta_n]^T$
现已知 $p$ ,求每个关节的转角 $\Theta$ ,由于 $f(\Theta)$ 带有三角函数，是一个典型的多元非线性问题。可选用Newton Raphson迭代法作为求解方案。

1、Newton Raphson迭代法
把函数 $f (x)$ 在 $x_0$ 点某邻域内进行泰勒展开：
$f(x)=f(x_0)+\frac{f’(x_0)(x−x_0)}{1!}+⋯+\frac{f^{(n)}(x_0)(x−x_0)^n}{n!}+R_n(x)$
取线性部分(泰勒展开前两项),令其等于0作为 $f (x) = 0$ 的近似方程:
$f(x_0)+f’(x_0)(x−x_0)=0$
若 $f’(x_0)≠0$ , 则其解为:
$x_1=x_0−\frac{f(x_0)}{f’(x_0)}$
得到迭代关系式:
$x_{n+1}=x_n−\frac{f(x_n)}{f’(x_n)}$
- 举例
$f(x)=x^3−6x, 取 x_0=1$
在这里插入图片描述

$x_1=x_0−x_0^3−\frac{6x_0}{3x_0^2}−6=−\frac{2}{3}, f(x_1)=\frac{100}{27}$

$x_2=x_1−x_1^3−\frac{6x_1}{3x_1^2}−6=\frac{8}{63}, f(x_2)=0.759857$

$x_3=x_2−x_2^3−\frac{6x_2}{3x_2^2}−6=−0.000688086,f(x_3)=0.00412852$

$x_4=x_3−x_3^3−\frac{6x_3}{3x_3^2}−6=1.08594×10^{−10},f(x_4)=−6.51564×10^{−10}$

- 应用在机器人运动学
给定 $p∈ℝ^m$ 和初始猜测角度 $\Theta^0∈ℝ^n$ ( $m$ 为空间自由度, $n$ 为关节数):
$p=f(\Theta)=f(\Theta^i)+\frac{\partial{f}}{\partial{\Theta}}|_{\Theta^i}(\Theta−\Theta^i)+R_n(\Theta)$
可写成：
$p−f(\Theta^i)=J(\Theta^i)\Delta\Theta , J(\Theta^i)=[J_1,J_2...,J_n]$
$J(\Theta^i)$ 为一阶偏导，即为雅可比矩阵。
左右同时左乘 $J^{−1}(\Theta^i)$ :
$J^{−1}(\Theta^i)(p−f(\Theta^i))=\Delta\Theta$
设 $‖ e ‖ < ε$ ( $ε$ 为迭代最小误差,如0.001):
$e=(p−f(\Theta^i))$

$\Delta\Theta=\Theta^{i+1}-\Theta^{i}$
得到：
$\color{red}{\Theta^{i+1}=\Theta^i+J^{-1}(\Theta^i)e }$
如果雅可比矩阵不是方阵或机器人位于奇异点，此时用雅可比矩阵的伪逆 $J^†(\Theta^i)$ .伪逆进行计算.可写成：
$\color{red}{\Theta^{i+1}=\Theta^i+J^†(\Theta^i)e }$
- 实现：

求解 $\Theta^{i}$ 处雅克比矩阵：
令 $\Theta^{i}=[\theta_1^i,\theta_2^i...\theta_n^i]$ 为单位角速度或线速度1时，每个关节对末端的速度旋量
$J_i=Twist_i=[v_{x_i},v_{y_i},v_{z_i},r_{x_i},r_{y_i},r_{z_i}]^T \\J(\Theta^i)=[J_1,J_2...,J_n]$
误差项 $e$ 的求取：

$e=[\begin{matrix}e_p&e_\omega\end{matrix}]=[\begin{matrix}\Delta x&\Delta y&\Delta z&\Delta rx&\Delta ry&\Delta rz\end{matrix}]$
位置误差: $e_p=P_B−P_A=[\begin{matrix}x_b−x_a&y_b−y_a&z_b−z_a\end{matrix}]$
姿态误差：以 $A$ 为参考， $A$ 到 $B$ 的姿态误差
$e_{rot_{A2B}}=R_{base2A}^{−1}∗R_{base2B}$
此时的 $e_{rot_{A2B}}$ 需要把旋转矩阵表示成轴角的形式：
$e_{rot_{A2B}} \Rightarrow e_{\omega_{A2B}}$
最后将 $e_{ω_{A2B}}$ 得转换到基于基座标 $b a s e$ 为参考的姿态误差:
$e_ω=R_{base2A}∗e_{ω_{A2B}}$

- 代码实现：源码

int Chain::ikNewton(const Frame &desireFrame, VectorXSDS &outJoints, int maxIter, double eps)
{

    if(outJoints.mN != _numOfJoints)
    {
        return -2;
    }

    Frame frame;
    Twist dtTwist;
    MatSDS jac(_numOfJoints,6);

    for (int i = 0; i < maxIter; ++i)
    {

        frame = fk(joints);

        dtTwist = Frame::diff(frame, desireFrame);  

        if(dtTwist.closeToEps(eps))
        {

            outJoints = joints;
            return i;
        }

        jacobi(jac,joints);

        MatSDS dtJoints = jac.pseudoInvert()*dtTwist.toMat(); 

        for (int i = 0; i < joints.mN; ++i)
        {
            joints[i] = joints[i] + dtJoints[i];
        }
    }
    return -1;
}

- 结果:
对比Msnhnet实现和KDL实现，使用puma560机器人模型，分别测试约20万组点，在0点开始迭代，结果如下。Msnhnet成功率为96.02%, 单次求解速率为114us，KDL成功率为96.11%,单次求解速率为135us。(Win10 I7 10700KF)
在这里插入图片描述

- 后续：
后面会采用trac_ik的求解方案进行优化，欢迎继续关注。