递归牛顿欧拉(正)动力学仿真

  递归牛顿-欧拉方法(Recursive Newton-Euler Method)是一种高效的动力学计算方法,尤其适用于串联多刚体系统,例如串联机械臂。递归牛顿-欧拉方法有正和逆两种形式,本文我们先来看正动力学。正动力学又称为前向动力学,如果给你机器人各关节的力矩后,通过正动力学可以求出机器人的运动。在这里,机器人的运动指各个关节(或者各连杆)的位置、速度和加速度。

  下面的算法来自于论文《Lie Group Formulation of Articulated Rigid Body Dynamics》,我们更正了原文中的一处小错误。该算法使用了李群和李代数表示机器人连杆的坐标、速度和受力,其优点是形式简洁、线速度和角速度可以合写在一块,力也是如此,并有清晰的数学含义。该算法适用于三维空间,每一步正动力学计算过程包含三个递归子过程:1. 前向计算连杆的位姿和速度;2. 反向计算连杆系的广义惯量和偏置力;3. 前向计算加速度。在定义连杆的记号时,通常将固定的基座记为 0,与基座相连的连杆记为1,与连杆1相连的下一个连杆记为2,依次类推。这里把从基座开始向机械臂末端的方向称为前向(ii+1),把机械臂末端到基座的方向称为反向ii-1)

具体实现(Mathematica代码)

    (*Initialization 部分参数初始化*)
    time = 2000; dt = 0.005;  
    Table[mass[i] = 1; Gravity[i] = grav*mass[i]*{0, 0, -1, 0, 0, 0}, {i, 0, n, 1}];  
    Table[g[i, i + 1, 0] = RPToH[Id[3], {0, 0, (La[i] + La[i + 1])/2}], {i, 0, n - 1, 1}];  
    q = dq = ddq = ConstantArray[0, n];  
    Table[V[i] = dV[i] = ConstantArray[0, 6], {i, 0, n, 1}];  
    Table[M[i] = Id[6]; \[Tau][i] = 0, {i, n}];  
    F[n + 1] = ConstantArray[0, 6];  
    g[n, n + 1] = g[0, 0] = Id[4];  
    q = ConstantArray[Pi/2, n];  
    \[CapitalPi][n + 1] = Id[6]*0.0;  
    \[Beta][n + 1] = ConstantArray[0, 6];  
    Table[  
       qList = {qList, q};  
       gList = {gList, g[0, 4]};  
       (*Forward 前向递归*)  
       dq = dq + ddq*dt;   (*欧拉积分*)
       q = q + dq*dt;  
       For[i = 1, i <= n, i++,  
        g[i - 1, i] = TwistExp[\[Xi]r[i], q[[i]]].g[i - 1, i, 0];  
        g[0, i] = g[0, i - 1].g[i - 1, i];  
        V[i] = Ad[Iv[g[i - 1, i]]].V[i - 1] + \[Xi]s[i]*dq[[i]];  
        \[Eta][i] = ad[V[i] - \[Xi]s[i]*dq[[i]]].\[Xi]s[i]*dq[[i]];  
        ];  
       (*Backward 反向递归*)  
       For[i = n, i >= 1, i--,  
        \[Tau][i] = 0;  
        Mh[i] = M[i] + T[Ad[Iv[g[i, i + 1]]]].\[CapitalPi][i + 1].Ad[Iv[g[i, i + 1]]];  
        Fext[i] = T[Ad[RPToH[R[g[0, i]], {0, 0, 0}]]].Gravity[i];  
        \[ScriptCapitalB][i] = -T[ad[V[i]]].M[i].V[i] - Fext[i] + T[Ad[Iv[g[i, i + 1]]]].\[Beta][i + 1];  
        \[CapitalPsi][i] = 1/(\[Xi]s[i].Mh[i].\[Xi]s[i]);  
        \[CapitalPi][i] = Mh[i] - \[CapitalPsi][i]*KroneckerProduct[Mh[i].\[Xi]s[i], \[Xi]s[i].Mh[i]];  
        \[Beta][i] = \[ScriptCapitalB][i] + Mh[i].(\[Eta][i] + \[Xi]s[i]*\[CapitalPsi][i]*(\[Tau][i] - \[Xi]s[i].(Mh[i].\[Eta][i] + \[ScriptCapitalB][i])));  
        ];  
       (*Forward 前向递归*)  
       For[i = 1, i <= n, i++,  
        ddq[[i]] = \[CapitalPsi][i]*(\[Tau][i] - \[Xi]s[i].Mh[i].(Ad[Iv[g[i - 1, i]]].dV[i - 1] + \[Eta][i]) - \[Xi]s[i].\[ScriptCapitalB][i]);  
        dV[i] = Ad[Iv[g[i - 1, i]]].dV[i - 1] + \[Xi]s[i]*ddq[[i]] + \[Eta][i]];  
       , {t, time}]; 

仿真结果

  我们首先以简单的 4 个连杆组成的机械臂为例进行仿真试验,连杆之间用转动关节连接,机器人初始时刻处于水平的静止状态,所有关节的力矩设为0。理论上机器人应该在重力作用下自由下落,我们看看用正向动力学步骤计算得到的仿真结果是什么样的。结果如下动画所示(只显示了Y-Z平面)。从结果看好像是对的,但是我还不敢100%保证。


正确性验证

  为了验证算法的正确性,我和第三方的仿真软件进行对比。这里我选取了Working Model软件(是一款商业的二维动力学仿真软件)。在Working Model中设置相同的参数和初始状态,仿真过程如下图所示。


  我们选择末端连杆(也就是第4个连杆)质心的Y坐标(重力加速度的反方向)进行对比,结果如下图左所示。二者的误差(下图右)在-0.001m~0.001m之间。考虑到每个连杆的长度为10cm,误差<1mm 说明我们算法基本是正确的。因为都是数值计算,有误差应该是正常的。至于为什么误差在0.5mm这个量级,这可能和我们采用的积分方法有关。在上面的算法中,我们采用了最简单的欧拉积分。而在Working Model中,因为这个软件不是开源的,我们也看不到它的积分方法是怎样实现的。不过我们倒是可以设置,在Edit菜单下的Accuracy选项中即可选择积分方法并设置积分步长。

  

  下图中的例子是10自由度的连杆同样只在重力作用下(关节力矩为0)的运动,只是初始状态不同,而且每相邻的两个关节旋转轴相互垂直。有没有闻到一丝混沌(Chaos)的味道 大笑



  这时要对比验证可以借助三维动力学仿真软件,例如MSC Adams。但是太繁琐,我懒得做了。机械臂是怎么画的呢?其它类型的关节怎么定义呢?这些我会随后在另外的博客中给出来。
阅读更多
换一批

没有更多推荐了,返回首页