精华内容
下载资源
问答
  • 通过使用matlab编程,可用于计算任何类型串联机器人工作空间。
  • 串联机器人工作空间

    2013-11-26 12:38:57
    通过matlab画出5自由度串联机器人工作空间
  • 六轴串联机器人D-H法求解的工作空间,主要是数值解法求得的工作空间
  • 六自由度串联机器人运动优化与轨迹跟踪.pdf
  • 教程从加载不同工业机器人的设计参数开始: >> opw_参数 要计算史陶比尔 TX40 给定末端执行器姿势的所有解,请使用: >> theta = ik_6Ropw( staeubli_tx40, [40,400,500], [1, 0.3, 0.1], 0.25) 这里 [40,400,500]...
  • 牛顿迭代求解串联机器人运动学正解
  • 6自由度串联机器人D-H模型参数辨识及标定
  • 提出了一种基于改进PID控制算法的串联机器人轨迹跟踪控制策略,首先采用减聚类的方法和改进的Logistic映射对RBF神经网络进行聚类中心的优化,然后将改进RBF神经网络中的自适应学习机制和自调整能力应用于传统PID控制...
  • 针对一种四自由度工业串联机器人,为实现其在工作时的精确运动控制,对其进行运动学研究。建立空间坐标系,推导出运动学正解方程。根据正解方程使用Jacobian-迭代法,推导出机构的反解方程,用于运动控制的输入。通过...
  • 随着机器人技术的快速发展,机器人在各个领域中的作用越来越重要。通过对机器人运动控制系统的二次开发可以实现对机器人的远程控制,并在此...本文的研究对象为四自由度串联机器人,研究了其运动控制系统二次开发的问题.
  • 基于指数积公式的串联机器人运动学参数标定方法,王伟,王刚,标定运动学参数是提高机器人定位精度的有效方法之一。基于指数积公式,提出了一种仅需要测量末端位置坐标的运动学参数标定方法。
  • 以平面2R串联机器人机构为研究对象,运用拉格朗日方法建立了机构的动力学模型,并通过数值算例,运用Matlab、ADAMS两个软件对机 构的动力学模型进行了仿真,通过对比,证明了所建立的平面2R串联机器人机构动力学模型...
  • 提出了一种串联机器人的改进控制算法。采用一自适应模糊控制器,根据滑模到达条件对滑模切换增益进行估算,消除滑模控制中输出力矩的抖振现象,增强其对不确定性因素的适应能力。采用另一自适应模糊控制器对指数趋近...
  • 主要是讲串联机器人的奇异点的规避和振动抑制,里面分析的比较透彻,供大家参考
  • 六自由度串联机器人运动优化与轨迹跟踪控制研究_刘松国;知网下载,很全面的运动学资料;
  • Modern Robotics串联机器人常见的奇异构型 奇异点的数学定义 机器人末端执行器失去瞬间向一个或多个方向移动的能力时的姿态称为运动学奇异点(kinematic singularity), 或简称奇异点(singularity)。雅可比矩阵使...

    Modern Robotics串联机器人常见的奇异构型

    奇异点的数学定义

    机器人末端执行器失去瞬间向一个或多个方向移动的能力时的姿态称为运动学奇异点(kinematic singularity), 或简称奇异点(singularity)。雅可比矩阵使我们能够识别的奇异点。从数学角度看,一个奇异点在雅可比Jb(θ)J_b(\theta)不能达到最大秩的构型。考虑物体雅可比,列用Jbi,i=1,...,nJ_{bi},i=1,...,n表示,那么有
    Vb=[Jb1(θ)Jbn1(θ)Jbn][θ˙1θ˙n1θ˙n]=Jb1(θ)θ˙1++Jbn1(θ)θ˙n1+Jbnθ˙n \mathcal V_{b}=\left[\begin{matrix} J_{b1}(\theta)& \cdots&J_{bn-1}(\theta)&J_{bn}\end{matrix} \right] \left[\begin{matrix} \dot \theta_1\\ \vdots \\ \dot\theta_{n-1}\\ \dot\theta_n \end{matrix}\right]\\ =J_{b1}(\theta)\dot\theta_1+\cdots+J_{bn-1}(\theta)\dot\theta_{n-1}+J_{bn}\dot\theta_n
    因此末端坐标系的旋量是雅可比矩阵的列JbiJ_{bi}的线性组合.只有n6n\ge 6,雅可比Jb(θ)J_b(\theta)的最大秩就是6.

    奇异点与选择物体雅可比还是空间雅可比无关,与固定坐标系和物体坐标系的选择也无关。因为物体雅可比和空间雅可比之间是伴随映射段关系,Js(θ)=AdTsb(Jb(θ))=[AdTsb]Jb(θ)J_s(\theta)=Ad_{T_{sb}}(J_b(\theta))=[Ad_{T_{sb}}]J_b(\theta), 即
    Js(θ)=[Rsb0[psb]RsbRsb]Jb(θ). J_s(\theta)=\left[\begin{matrix} R_{sb}&0\\ [p_{sb}]R_{sb}&R_{sb} \end{matrix}\right]J_{b}(\theta).
    由于[AdTsb][Ad_{T_{sb}}]总是可逆的,因此Jb(θ)J_b(\theta)Jb(θ)J_b(\theta)的秩总是相等的。

    6自由度串联机器人常见的奇异构型

    下面我们列出由旋转关节(revolute joint)和平移关节(prismatic joint)组成的6自由度串联机械臂常见的奇异构型。

    (1)两个旋转关节共线

    在这里插入图片描述

    若存在两个关节轴线共线,我们把这两个关节编号为1和2,对应的雅可比的列为
    Js1(θ)=[ωs1ωs1×q1],Js2(θ)=[ωs2ωs2×q2]. J_{s1}(\theta)=\left[\begin{matrix} \omega_{s1}\\ -\omega_{s1}\times q_1 \end{matrix}\right], J_{s2}(\theta)=\left[\begin{matrix} \omega_{s2}\\ -\omega_{s2}\times q_2 \end{matrix}\right].
    由于两个关节轴线是共线的,必有ωs1=±ωs2\omega_{s1}=\pm \omega_{s2},我们不妨取ωs1=ωs2\omega_{s1}= \omega_{s2} 同时对任意i,ωsi×(q1q2)=0\omega_{si}\times (q_1-q_2)=0, 因此有Js1=Js2J_{s1}=J_{s2} 。列向量{Js1,Js2,,Js6}\{J_{s1},J_{s2},\dots,J_{s6}\}不可能线性无关,因此,Js(θ)J_s(\theta)的秩必定小于6. 对应后面的几种情形,同样可以用雅可比矩阵进行辨别出其奇异性。

    (2)3个旋转关节轴线共面且平行

    在这里插入图片描述

    (3)4个旋转关节轴线相交于同一点

    在这里插入图片描述

    (4)四个旋转关节轴线共面

    在这里插入图片描述

    (5)6个关节轴线相交于同一条线

    小结

    在机器人控制上来说,就意味着,一旦出现奇异,你就不能随意控制你的机器人朝着你想要的方向前进了。这也就是前面所谓的自由度退化、逆运动学无解。

    解决办法:

    1.在规划路径中尽可能的避免机器人经过奇异点。

    2.结合机器人运动学,优化机器人反解算法,确保在奇异点附近伪逆解的稳定性 。


    参考资料

    [1] Kenvin M. Lynch , Frank C. Park, Modern Robotics Mechanics,Planning , and Control. May 3, 2017

    [2] https://blog.csdn.net/qq_19390445/article/details/86480421

    展开全文
  • MatlabSimMechanics三自由度串联机器人正解反解-MyRobot_moveline.rar 上次我发了二自由度串联机器人的正解和反解的贴子,可能这个论坛的高手觉得太简单了,都问我做的是是不是并联机器人。 前几天我感到很羞愧,...
  • 串联机械臂的逆解是一个求解多元非线性方程解的过程。当然,市面上的众多商业臂也都是采用符合pieper准则的构型,可以用解析...串联机器人运动学方程可写为: p=f(Θ),Θ=[θ1,θ2...θn]T p=f(\Theta), \Theta=[\theta

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

    1、Newton Raphson迭代法
    把函数 f(x)f(x)x0x_0 点某邻域内进行泰勒展开:
    f(x)=f(x0)+f(x0)(xx0)1!++f(n)(x0)(xx0)nn!+Rn(x)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)=0f(x)=0 的近似方程:
    f(x0)+f(x0)(xx0)=0f(x_0)+f’(x_0)(x−x_0)=0
    f(x0)0f’(x_0)≠0, 则其解为:
    x1=x0f(x0)f(x0)x_1=x_0−\frac{f(x_0)}{f’(x_0)}
    得到迭代关系式:
    xn+1=xnf(xn)f(xn)x_{n+1}=x_n−\frac{f(x_n)}{f’(x_n)}
    - 举例
    f(x)=x36x,x0=1f(x)=x^3−6x, 取 x_0=1
    在这里插入图片描述

    x1=x0x036x03x026=23,f(x1)=10027x_1=x_0−x_0^3−\frac{6x_0}{3x_0^2}−6=−\frac{2}{3}, f(x_1)=\frac{100}{27}

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

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

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

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

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

    1. 求解 Θi\Theta^{i} 处雅克比矩阵:
      Θi=[θ1i,θ2i...θni]\Theta^{i}=[\theta_1^i,\theta_2^i...\theta_n^i]为单位角速度或线速度1时,每个关节对末端的速度旋量
      Ji=Twisti=[vxi,vyi,vzi,rxi,ryi,rzi]TJ(Θi)=[J1,J2...,Jn]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]
    2. 误差项 ee 的求取:

      e=[epeω]=[ΔxΔyΔzΔrxΔryΔrz]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}]
      位置误差: ep=PBPA=[xbxaybyazbza]e_p=P_B−P_A=[\begin{matrix}x_b−x_a&y_b−y_a&z_b−z_a\end{matrix}]
      姿态误差:以AA为参考,AABB的姿态误差
      erotA2B=Rbase2A1Rbase2Be_{rot_{A2B}}=R_{base2A}^{−1}∗R_{base2B}
      此时的 erotA2Be_{rot_{A2B}} 需要把旋转矩阵表示成轴角的形式:
      erotA2BeωA2Be_{rot_{A2B}} \Rightarrow e_{\omega_{A2B}}
      最后将eωA2Be_{ω_{A2B}}得转换到基于基座标 basebase 为参考的姿态误差:
      eω=Rbase2AeωA2Be_ω=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的求解方案进行优化,欢迎继续关注。

    - 最后
    欢迎关注Msnhnet框架,它是一个深度学习推理框架,也会慢慢变成一个机器人+视觉一体的框架
    Msnhnet
    知乎介绍

    机器人学建模、规划与控制.Bruno Siciliano
    现代机器人学:机构、规划与控制. Kevin M. Lynch.
    机器人操作的数学导论.李泽湘
    机器人学导论。John J. Craig

    展开全文
  • 将倍四元数的复指数形式应用于串联机构位置逆解分析中,提出了空间6R(R代表转动副)串联机构位置逆解新算法.基于倍四元数建立了空间6R串联机构位置逆解的数学模型;然后,使用线性消元和Dixon结式消元法,得到了6×6...
  • 使用Msnhnet实现串联机器人运动数值逆解vs KDL(一) 1.改进一 首先, 对于机械臂来说, 一般都会有角度限位, 那么可以对每次迭代之后的 \Theta 进行角度限制, 缩小搜索范围, 可以提高求解效率. 2. 改进二 使用最大迭代...

    上一篇文章对于串联机械臂逆解问题, 使用Newton Raphson法进行了简单的实现.但是其速度和准确率都存在比较大的问题.
    使用Msnhnet实现串联机器人运动数值逆解vs KDL(一)

    1.改进一

    首先, 对于机械臂来说, 一般都会有角度限位, 那么可以对每次迭代之后的 \Theta 进行角度限制, 缩小搜索范围, 可以提高求解效率.

    2. 改进二

    使用最大迭代次数作为求解结束的条件,那么对于最终迭代时间是无法确定的, 比如迭代7轴的臂和迭代4轴的臂,迭代次数一样的前提下, 迭代时间肯定不一样.那么, 就需要对迭代时间进行把控. 所以, 可以设置最大迭代时间, 不管有没有达到最大迭代次数, 只要达到了最大迭代时间, 就退出迭代.

    3. 改进三

    之前的实现, 在迭代过程中, 会出现困在局部最小值的情况. 所以需要对陷入局部最小值的情况进行处理. 那么我们可以通过以下条件进行判断.
    Θi+1Θi0,diff(Ti+1,Tdesire)>eps\Theta_{i+1} - \Theta_{i}\approx0, diff(T_{i+1},T_{desire})>eps
    出现以上情况时, 可以在最小限位和最大限位的角度中, 随机出一个点, 让其跳出局部最小.

    int Chain::ikNewtonRR(const Frame &desireFrame, VectorXSDS &outJoints, const Twist &bounds, const bool &randomStart, const bool &wrap, int maxIter, double eps, double maxTime)
    {
    
        if(outJoints.mN != _numOfJoints)
        {
            return -2;
        }
    
        Frame frame;
        Twist dtTwist;
        auto joints     = outJoints;
    
        double timeM = maxTime * 1000.0;
        double timeNow = 0;
    
        MatSDS jac(_numOfJoints,6);
    
        auto st = std::chrono::high_resolution_clock::now();
    
        for (int i = 0; i < maxIter; ++i)
        {
    
            auto so = std::chrono::high_resolution_clock::now();
    
            timeNow = std::chrono::duration <double,std::milli> ((so-st)).count();
    
            if(timeNow > timeM)
            {
                return -6;
            }
    
            frame = fk(joints);
    
            dtTwist = Frame::diffRelative(desireFrame,frame);  
    
            if (std::abs(dtTwist.v[0]) <= std::abs(bounds.v[0]))
                dtTwist.v[0] = 0;
    
            if (std::abs(dtTwist.v[1]) <= std::abs(bounds.v[1]))
                dtTwist.v[1] = 0;
    
            if (std::abs(dtTwist.v[2]) <= std::abs(bounds.v[2]))
                dtTwist.v[2] = 0;
    
            if (std::abs(dtTwist.omg[0]) <= std::abs(bounds.omg[0]))
                dtTwist.omg[0] = 0;
    
            if (std::abs(dtTwist.omg[1]) <= std::abs(bounds.omg[1]))
                dtTwist.omg[1] = 0;
    
            if (std::abs(dtTwist.omg[2]) <= std::abs(bounds.omg[2]))
                dtTwist.omg[2] = 0;
    
            if(dtTwist.closeToEps(eps))
            {
    
                outJoints = joints;
                return i;
            }
    
            dtTwist = Frame::diff(frame,desireFrame);
    
            jacobi(jac,joints);
    
            auto UDVt = jac.svd();
            double sum = 0;
            VectorXSDS tmp(jac.mWidth);
    
            VectorXSDS dtJoints(jac.mWidth);
    
            for (int i = 0; i < jac.mWidth; ++i)
            {
                sum = 0;
                for (int j = 0; j < jac.mHeight; ++j)
                {
                    sum +=  UDVt[0](i,j)*dtTwist[j];
                }
    
                if(fabs(UDVt[1][i])<eps)
                {
                    tmp[i] = 0;
                }
                else
                {
                    tmp[i] = sum/UDVt[1][i];
                }
            }
    
            for (int i = 0; i < jac.mWidth; ++i)
            {
                sum = 0;
                for (int j=0;j<jac.mWidth;j++)
                {
                    sum+=UDVt[2](i,j)*tmp[j];
                }
    
                dtJoints[i] = sum;
            }
    
            VectorXSDS currJoints(joints.mN);
    
            for (int i = 0; i < joints.mN; ++i)
            {
                currJoints[i] = joints[i] + dtJoints[i];
    
                auto mtype      = _jointMoveTypes[i];
                auto minJoint   = _minJoints[i];
                auto maxJoint   = _maxJoints[i];
    
                if( mtype == Joint::ROT_CONTINUOUS_MOVE)
                {
                    continue;
                }
    
                if(currJoints[i] < minJoint)
                {
                    if(!wrap ||  mtype == Joint::TRANS_MOVE)
                    {
                        currJoints[i] = minJoint;
                    }
                    else
                    {
                        double diffAngle = fmod(minJoint - currJoints[i], MSNH_2_PI);
                        double currAngle = minJoint - diffAngle + MSNH_2_PI;
                        if (currAngle > maxJoint)
                            currJoints[i] = minJoint;
                        else
                            currJoints[i] = currAngle;
                    }
                }
    
                if(currJoints[i] > maxJoint)
                {
                    if(!wrap ||  mtype == Joint::TRANS_MOVE)
                    {
                        currJoints[i] = maxJoint;
                    }
                    else
                    {
                        double diffAngle = fmod(currJoints[i] - maxJoint, MSNH_2_PI);
                        double currAngle = maxJoint + diffAngle - MSNH_2_PI;
                        if (currAngle < minJoint)
                            currJoints[i] = maxJoint;
                        else
                            currJoints[i] = currAngle;
                    }
                }
    
                joints[i] = joints[i] - currJoints[i];
            }
    
            if(joints.isFuzzyNull(MSNH_F32_EPS))
            {
                if(randomStart)
                {
                    for (int j = 0; j < currJoints.mN; j++)
                    {
                        if (_jointMoveTypes[j] == Joint::ROT_CONTINUOUS_MOVE)
                            currJoints[j] = fRand(currJoints[j] - MSNH_2_PI, currJoints[j] + MSNH_2_PI);
                        else
                            currJoints[j] = fRand(_minJoints[j], _maxJoints[j]);
                    }
                }
            }
    
            joints = currJoints;
        }
        return -1;
    }
    
    • 测试

    使用puma560机器人模型,分别测试约20万组点,在0点开始迭代,与改进之前的效果对比, 在成功率上提升不大, 但是在速度上获得了约1.3倍提升(Win10 I7 10700KF).
    在这里插入图片描述
    在这里插入图片描述

    4. 改进四(思路来源于Trac_IK)

    理论上来说, 关节是无约束运动, 采用Newton法这类无约束最优化方法可以获得比较好的结果. 但是在实际机器人求解中, 许多机器人的关节都有严格的关节限制, 通过Newton Raphson法迭代的结果, 肯定会超出关节极限. 所以每次在在超过极限位置后, 需要把结果约束到最大值和最小值之间, 故而其搜索空间不再平滑.

    为解决这一问题, 可以采用有约束的优化方法用于求解. 这里采用序列二次规划的方法(SQP)用来求解, SQP算法是目前公认的求解约束非线性优化问题最有效的方法之一,具有收敛性好、计算效率高、边界搜索能力强的优点.

    SQP算法思想

    对于一般约束最优化问题::
    minf(x),s.t.ci(x)=0,iE,ci(x)0,iIminf(x),\\s.t. c_i(x)=0, i\in \mathcal{E},\\ c_i(x)\ge 0, i\in\mathcal{I}
    利用泰勒展开将非线性约束问题的目标函数在迭代点 xkx_k 处简化成二次函数,将约束条件简化成线性函数后得到如下二次规划问题来求解.
    min12dTWkd+gkTd+fk,s.t.ci(xk)+ai(xk)T=0,iE,ci(xk)+ai(xk)Td0,iImin\frac{1}{2}d^TW_kd+g_k^Td+f_k,\\s.t. c_i(x_k)+a_i(x_k)^T=0, i\in \mathcal{E},\\ c_i(x_k)+a_i(x_k)^Td\ge 0, i\in\mathcal{I}
    其中对于上式中的拉格朗日函数的Hessian矩阵 WkW_k ,使用拟牛顿法进行修正. 拟牛顿法可以使用BFGS公式或者DFP公式进行计算.

    具体原理可以参看numerical optimization

    这里SQP方法采用nlopt库中的SLSQP算子, SLSQP采用BFGS拟牛顿法对二次规划方法中的Hessian矩阵进行拟合, 同样, BFGS也存在局部最小值问题, 一样需要在局部最小时加入随机项.

    目标函数选取,

    误差项ee的计算详见上一篇文章:
    minf=eeTminf=e\cdot e^T

    // =============================== 目标函数 ==========================================
    inline double minfuncSumSquared(const std::vector<double>& x, std::vector<double>& grad, void* data)
    {
        Chain *chain = (Chain *) data;
    
        std::vector<double> vals(x);
    
        double jump = 0.000001;
        double result[1];
        chain->cartSumSquaredErr(vals, result);
    
        if (!grad.empty())
        {
            double v1[1];
            for (unsigned int i = 0; i < x.size(); i++)
            {
                double original = vals[i];
    
                vals[i] = original + jump;
                chain->cartSumSquaredErr(vals, v1);
    
                vals[i] = original;
                grad[i] = (v1[0] - result[0]) / (2.0 * jump);
            }
        }
    
        return result[0];
    }
    
    void Chain::cartSumSquaredErr(const std::vector<double> &x, double error[])
    {
        if(_optStatus != -3)
        {
            _opt.force_stop();
            return;
        }
    
        Frame frame = fk(x);
    
        if(std::isnan(frame.trans[0]))
        {
            error[0] = std::numeric_limits<float>::max();
            _optStatus = -1;
            return;
        }
    
        Twist dtTwist = Frame::diffRelative(_desireFrameSQP, frame);
    
        if (std::abs(dtTwist.v[0]) <= std::abs(_boundsSQP.v[0]))
            dtTwist.v[0] = 0;
    
        if (std::abs(dtTwist.v[1]) <= std::abs(_boundsSQP.v[1]))
            dtTwist.v[1] = 0;
    
        if (std::abs(dtTwist.v[2]) <= std::abs(_boundsSQP.v[2]))
            dtTwist.v[2] = 0;
    
        if (std::abs(dtTwist.omg[0]) <= std::abs(_boundsSQP.omg[0]))
            dtTwist.omg[0] = 0;
    
        if (std::abs(dtTwist.omg[1]) <= std::abs(_boundsSQP.omg[1]))
            dtTwist.omg[1] = 0;
    
        if (std::abs(dtTwist.omg[2]) <= std::abs(_boundsSQP.omg[2]))
            dtTwist.omg[2] = 0;
    
        error[0] = Vector3DS::dotProduct(dtTwist.v,dtTwist.v) + Vector3DS::dotProduct(dtTwist.omg,dtTwist.omg);
    
        if(dtTwist.closeToEps(_epsSQP))
        {
            _optStatus = 1;
            _bestXSQP  = x;
            return;
        }
    }
    // ===============================================================================
    
    
    // SQP迭代
    int Chain::ikSQPSumSqr(const Frame &desireFrame, VectorXSDS &outJoints, const Twist &bounds, int maxIter, double eps, double maxTime)
    {
    
        if(outJoints.mN != _numOfJoints)
        {
            return -2;
        }
    
        _boundsSQP = bounds;
    
        _epsSQP    = eps;
    
        _opt.set_maxtime(maxTime);
    
        double minf; /* the minimum objective value, upon return */
    
        _desireFrameSQP = desireFrame;
    
        std::vector<double> x(_numOfJoints);
    
        for (unsigned int i = 0; i < x.size(); i++)
        {
            x[i] = outJoints[i];
    
            if (_jointMoveTypes[i] == Joint::ROT_CONTINUOUS_MOVE)
                continue;
    
            if (_jointMoveTypes[i] == Joint::TRANS_MOVE)
            {
                x[i] = std::min(x[i], _maxJoints[i]);
                x[i] = std::max(x[i], _minJoints[i]);
            }
            else
            {
    
                if (x[i] > _maxJoints[i])
                {
    
                    double diffangle = fmod(x[i] - _maxJoints[i], MSNH_2_PI);
    
                    x[i] = _maxJoints[i] + diffangle - MSNH_2_PI;
                }
    
                if (x[i] < _minJoints[i])
                {
    
                    double diffangle = fmod(_minJoints[i] - x[i], MSNH_2_PI);
    
                    x[i] = _minJoints[i] - diffangle + MSNH_2_PI;
                }
    
                if (x[i] > _maxJoints[i])
                    x[i] = (_maxJoints[i] + _minJoints[i]) / 2.0;
            }
        }
    
        _bestXSQP   = x;
        _optStatus  = -3;
    
        std::vector<double> artificialLowerLimits(_minJoints.size());
    
        for (unsigned int i = 0; i < _minJoints.size(); i++)
        {
            if (_jointMoveTypes[i] == Joint::ROT_CONTINUOUS_MOVE)
            {
                artificialLowerLimits[i] = _bestXSQP[i] - MSNH_2_PI;
            }
            else if (_jointMoveTypes[i] == Joint::TRANS_MOVE)
            {
                artificialLowerLimits[i] = _minJoints[i];
            }
            else
            {
                artificialLowerLimits[i] = std::max(_minJoints[i], _bestXSQP[i] - MSNH_2_PI);
            }
        }
    
        _opt.set_lower_bounds(artificialLowerLimits);
    
        std::vector<double> artificialUpperLimits(_minJoints.size());
    
        for (unsigned int i = 0; i < _maxJoints.size(); i++)
        {
            if (_jointMoveTypes[i] == Joint::ROT_CONTINUOUS_MOVE)
            {
                artificialUpperLimits[i] = _bestXSQP[i] + MSNH_2_PI;
            }
            else if (_jointMoveTypes[i] == Joint::TRANS_MOVE)
            {
                artificialUpperLimits[i] = _maxJoints[i];
            }
            else
            {
                artificialUpperLimits[i] = std::min(_maxJoints[i], _bestXSQP[i] + MSNH_2_PI);
            }
        }
    
        _opt.set_upper_bounds(artificialUpperLimits);
    
        try
        {
            _opt.optimize(x, minf);
        }
        catch (...)
        {
        }
    
        if (_optStatus == -1) 
    
        {
            _optStatus = -3;
        }
    
        int q = 0;
    
        if (!(_optStatus < 0)) 
    
        {
    
            for (int z = 0; z < 100; ++z)
            {
    
                q = z;
                if(!(_optStatus < 0)) 
    
                {
                    break;
                }
    
                for (unsigned int i = 0; i < x.size(); i++)
                {
                    x[i] = fRand(artificialLowerLimits[i], artificialUpperLimits[i]);
                }
    
                try
                {
                    _opt.optimize(x, minf);
                }
                catch (...) {}
    
                if (_optStatus == -1) 
    
                {
                    _optStatus = -3;
                }
    
            }
        }
    
        if(q == (maxIter-1))
        {
            return -1;
        }
    
        for (unsigned int i = 0; i < x.size(); i++)
        {
            outJoints[i] = _bestXSQP[i];
        }
    
        return q;
    }
    
    • 测试

    使用puma560机器人模型,分别测试约20万组点,在0点开始迭代,可以看出使用SQP方法可以极大提高准确率, 但是由于计算量的关系, 计算速度也大幅度下降(Win10 I7 10700KF).
    在这里插入图片描述
    在这里插入图片描述
    5. 改进五(不开源)

    基于SQP和Newton的方法各有优势, 可以将这二者进行结合, 进行并行优化, 多个求解器同时计算, 以最快的结果为准同时停止其他求解器. 同时相关计算进行深度优化, Simd指令集优化, 矩阵乘法优化等等, 最后得到最终优化版本, 并与trac_ik进行比较(ubuntu20.04 I7 10700KF).

    • 测试

    使用puma560机器人模型,分别测试约300万组点,在0点开始迭代,可以看出Msnhnet Fast版本在相同的求解准确率下, 其求解速度比Trac_Ik快了约2.5倍.
    在这里插入图片描述
    在这里插入图片描述

    • 最后
      欢迎关注Msnhnet框架,它是一个深度学习推理框架,也会慢慢变成一个机器人+视觉一体的框架
      Msnhnet
      知乎介绍

    机器人学建模、规划与控制.Bruno Siciliano
    现代机器人学:机构、规划与控制. Kevin M. Lynch.
    机器人操作的数学导论.李泽湘
    机器人学导论。John J. Craig
    Numerical Optimization. Jorge Nocedal Stephen J. Wright

    展开全文
  • 二、具体步骤 2.1机器人三维模型保存为x_t格式,导入ADAMS软件 2.2 进行关节设置 2.3 添加运动规划 2.4 运行仿真 3 总结 一、本文介绍 本文以六自由度工业机器人为例,介绍利用ADAMS软件进行串联机器人运动学仿真的...


    一、本文介绍

    本文以六自由度工业机器人为例,介绍利用ADAMS软件进行串联机器人运动学仿真的具体步骤,实现机器人末端走出一条长方形轨迹。
    在这里插入图片描述

    二、具体步骤

    2.1机器人三维模型保存为x_t格式,导入ADAMS软件

    首先将在三维建模软件(如SolidWorks、Proe等)中建立好的机器人三维模型,另存为x_t格式,并导入ADAMS软件。

    提示:
    1 模型导入前建议对模型进行适当简化,去掉螺纹孔、螺钉等不重要的特征或零部件;
    2 导入时尽量选择全英文路径;
    3 导入时建议勾选【Explode Assembly】,方便后续对模型进行整理。

    在这里插入图片描述

    2.2 进行关节设置

    根据机器人关节进行设置,在机器人基座与groud之间添加固定约束;其余各关节依据机器人实际情况添加转动关节或移动关节。
    在这里插入图片描述
    本例中涉及一个移动关节和5个转动关节,设置如下:
    在这里插入图片描述
    提示:
    1 关节设置完成后建议检查关节轴线设置是否正确;

    2.3 添加运动规划

    本例要实现机器人末端运行长方形轨迹,首先在机器人末端连杆与ground之间添加motion
    这里选择General point motion

    在这里插入图片描述
    注意,在ADAMS中的motion和关节等运动关系,其实是两个坐标系间的相互运动,坐标系在ADAMS中被叫作MAKER,如本例中建立的连杆末端与groud之间的运动关系,实质是MAEKER_44和MARKER_45之间的相互运动
    在这里插入图片描述

    可以在相互运动的部件中找到相应的MARKER,查看其坐标轴方向
    在这里插入图片描述
    可以看到坐标轴方向为向上为+Z方向,向右为+Y方向,在此规划机器人完成如下所示长方形轨迹,计划轨迹长度长和宽均为200mm;
    在这里插入图片描述
    规划仿真时间为20s,具体运动规划如下:
    【0~5s】机器人末端从起点开始,沿+Z方向移动200mm
    【5~10s】机器人末端沿-Y方向移动200mm
    【10~15s】机器人末端沿-Z方向移动200mm
    【15~20s】机器人末端沿+Y方向移动200mm,回到起点

    然后再本步骤建立的motion中进行具体设置,机器人末端仅有Y、Z方向的移动,其余四个自由度设置为0在这里插入图片描述
    采用step函数对Y和Z方向的运动进行实现

    在这里插入图片描述
    可以点击下方Plot按钮,查看函数曲线是否正确
    在这里插入图片描述

    Z方向函数为

    STEP( time , 0 , 0 , 5 , 200 )+STEP( time , 10 , 0 , 15 , -200 )
    

    Y方向函数为

    STEP( time , 5 , 0 , 10 , -200 )+STEP( time , 15 , 0 , 20 , 200 )
    

    2.4 运行仿真

    设置仿真时间为20s,仿真步数为1000步,仿真类型选择kinematic
    在这里插入图片描述

    在这里插入图片描述
    至此,已实现了机器人末端运行长方形轨迹。

    提示:第一次仿真时不会显示长方形的轨迹线,需要在仿真完成后,选择results栏的trace按钮,即可显示上一仿真过程中末端的运行轨迹。
    在这里插入图片描述

    3 总结

    以上就是本文要分享的内容,可以利用已有的机器人模型,利用ADAMS仿真功能,对机器人的运动过程进行仿真和动画演示。

    展开全文
  • 主要就是关于机器人控制的一款软件的编写和调试的过程!
  • 6自由度串联机器人D-H建模方法

    万次阅读 2017-12-30 14:23:10
    1. 建立各个连杆的坐标轴 关节i的轴向方向为坐标系i的Zi...6R机器人连杆坐标系示意图 D-H参数表如下表: 欢迎关注知乎专栏: Jungle是一个用Qt的工业Robot 欢迎关注Jungle的微信公众号:Jungle笔记
  • 求解机器人学雅克比矩阵的方法以及速度仿真的方法
  • RobCoGen相关的文献介绍: : 1)论文a code generator for efficient kinematics and dynamics of articulated robots based on domain specific languages 下载链接: ...
  • 新型并串联复合机器人KED研究

空空如也

空空如也

1 2 3 4 5 ... 10
收藏数 198
精华内容 79
关键字:

串联机器人