精华内容
下载资源
问答
  • 关节型机器人路径插补控制的研究.pdf
  • 直线插补和圆弧插补的区别

    万次阅读 2019-08-17 10:03:31
    插补(Interpolation),即机床数控系统依照一定方法确定刀具运动轨迹的过程。也可以说,已知曲线上的某些数据,按照某种算法计算已知点之间的中间点的方法,也称为“数据点的密化”;数控装置根据输入的零件程序的...

    插补(Interpolation),即机床数控系统依照一定方法确定刀具运动轨迹的过程。也可以说,已知曲线上的某些数据,按照某种算法计算已知点之间的中间点的方法,也称为“数据点的密化”;数控装置根据输入的零件程序的信息,将程序段所描述的曲线的起点、终点之间的空间进行数据密化,从而形成要求的轮廓轨迹,这种“数据密化”机能就称为“插补”。

    一、
    直线插补就是数控系统,在起点和终点之间按照直线来密化点群,然后驱动伺服按照这个点群来运动
    圆弧插补就是这个点群是按照起点终点还有半径或者是圆心坐标来建立方程,运算,建立点群。
    二、
    插补是零件轮廓上的已知点之间,用通过有规律地分配各个轴的单位运动面逼近零件廓形的过程。
    直线插补方式,给出两端点间的插补数字信息借此信息控制刀具的运动,使其按照规定的直线加工出理想的曲面。
    圆弧插补方式,给出两端点间的插补数字信息借此信息控制刀具的运动,使其按照规定的圆弧加工出理想的曲面。
    数控车床的运动控制中,工作台(刀具)X、Y、Z轴的最小移动单位是一个脉冲当量。因此,刀具的运动轨迹是具有极小台阶所组成的折线(数据点密化)。例如,用数控车床加工直线OA、曲线OB,刀具是沿X轴移动一步或几步(一个或几个脉冲当量Dx),再沿Y轴方向移动一步或几步(一个或几个脉冲当量Dy),直至到达目标点。从而合成所需的运动轨迹(直线或曲线)。数控系统根据给定的直线、圆弧(曲线)函数,在理想的轨迹上的已知点之间,进行数据点密化,确定一些中间点的方法,称为插补。
    【注意】由下图可知:不管是直线还是圆弧插补都是走的折线轨迹,只是所对应函数计算(算法)不同;

    数控系统本质上所有的插补运动,都是直线插补。如果目标的曲线不是直线,而是一条不规则的曲线,那么数控会根据这条曲线的点坐标,来确定系统允许的最小跨度来进行一次直线插补,直线插补完成后,比较当前位置的坐标与目标曲线上目标点的坐标,来确定下一条直线插补的起点终点坐标,一小段一小段的直线插补,来拟合用户需要的曲线。而拟合的准确度取决于最小跨度的设置和系统的精度(一个脉冲当量对应的伺服运动距离)
    当然,现在圆弧插补已经应用于各种数控系统,所以圆弧插补也就不需要依赖直线插补来拟合,不过底层的处理机制依然是相同的。

    下面是用逐点比较法的原理来对比直线插补和圆弧插补:
    在这里插入图片描述
    在这里插入图片描述
    在这里插入图片描述
    在这里插入图片描述

    展开全文
  • MATLAB机器人圆弧轨迹插补算法,可以学习相互探讨一下
  • 基于抛物线过渡(梯形加减速)的空间直线插补算法与空间圆弧插补算法(Matlab) 基于单位四元数的姿态插补(Matlab) 我使用了抛物线过渡(也就是梯形加减速)作为空间插补算法的速度规划方法,但是梯形曲线的缺点...

    写在前面

    学习代码都记录在个人github上,欢迎关注~

    Matlab机器人工具箱版本9.10

    在前面的博文中:

    基于抛物线过渡(梯形加减速)的空间直线插补算法与空间圆弧插补算法(Matlab)

    基于单位四元数的姿态插补(Matlab)

    我使用了抛物线过渡(也就是梯形加减速)作为空间插补算法的速度规划方法,但是梯形曲线的缺点也很明显,即加速度不连续,速度过渡不平滑,容易造成机械系统冲击。下面我尝试了S型加减速曲线的规划方法并结合到空间插补算法中,仿真效果还可以,加速度曲线连续,更柔和,适用于精度速度要求更高的场合。

    空间直线插补:

    在这里插入图片描述在这里插入图片描述在这里插入图片描述

    空间圆弧插补:

    在这里插入图片描述在这里插入图片描述在这里插入图片描述

    直观插补:

    在这里插入图片描述在这里插入图片描述

    带约束S型速度规划

    预备知识:

    机器人学回炉重造(6):关节空间规划方法——梯形加减速(与抛物线拟合的线性函数)、S型曲线规划

    参照基于抛物线过渡(梯形加减速)的空间直线插补算法与空间圆弧插补算法(Matlab)中归一化参数思想,将归一化参数计算函数中的梯形加减速方法换成S型加减速法。此时,定义归一化时间算子:
    λ ( t ) = s ( t ) − s ( 0 ) L \lambda( t ) = \frac { s ( t ) - s ( 0 ) } { L } λ(t)=Ls(t)s(0)
    其中, t ∈ [ 0 , T ] t \in [ 0 , T ] t[0,T], s ( t ) s(t) s(t)是S型曲线中位移在全局时间坐标下的函数, L L L是机器人执行器末端经过的距离。

    空间直线插补与空间圆弧插补

    位置插补与姿态插补需要分开讨论。

    位置插补

    假设机器人末端由P1点沿直线运动到P2点, ( x 1 , y 1 , z 1 ) \left(x_{1}, y_{1}, z_{1}\right) (x1,y1,z1)分别为起点P1的位置, ( x 2 , y 2 , z 2 ) \left(x_{2}, y_{2}, z_{2}\right) (x2,y2,z2)为终点P2的位置, ( x i , y i , z i ) \left(x_{i}, y_{i}, z_{i}\right) (xi,yi,zi)为中间插补点Pi的位置。

    各个插值点位置坐标向量为:
    { x = x 1 + λ Δ x y = y 1 + λ Δ y z = z 1 + λ Δ z \left\{\begin{array}{l}{x=x_{1}+\lambda \Delta x} \\ {y=y_{1}+\lambda \Delta y} \\ {z=z_{1}+\lambda \Delta z}\end{array}\right. x=x1+λΔxy=y1+λΔyz=z1+λΔz
    其中, λ \lambda λ是归一化时间因子, ( Δ x , Δ y , Δ z ) (\Delta x, \Delta y, \Delta z) (Δx,Δy,Δz)表示首末位置间的位置增量,其解为:
    { Δ x = x 2 − x 1 Δ y = y 2 − y 1 Δ z = z 2 − z 1 \left\{\begin{array}{l}{\Delta x=x_{2}-x_{1}} \\ {\Delta y=y_{2}-y_{1}} \\ {\Delta z=z_{2}-z_{1}} \end{array}\right. Δx=x2x1Δy=y2y1Δz=z2z1

    基于单位四元数的姿态插补

    预备知识:

    基于单位四元数的姿态插补(Matlab)

    已知起点S、终点D的齐次变换矩阵(位姿)分别为 T S T_S TS T D T_D TD,则姿态插补步骤如下:

    1. 从齐次变换矩阵中提取出各自的旋转矩阵 R S R_S RS R D R_D RD,并将其转换为四元数 Q s Q_s Qs Q d Q_d Qd;

    2. 姿态四元数插补公式为:
      Q k ( t ) = Slerp ⁡ ( Q s , Q d , t ) = sin ⁡ [ ( 1 − λ ( t ) ) θ ] sin ⁡ θ Q s + sin ⁡ [ λ ( t ) θ ] sin ⁡ θ Q d Q_{k}(t)=\operatorname{Slerp}\left(Q_{s}, Q_{d}, t\right)=\frac{\sin [(1-\lambda(t)) \theta]}{\sin \theta} Q_{s}+\frac{\sin [\lambda(t) \theta]}{\sin \theta} Q_{d} Qk(t)=Slerp(Qs,Qd,t)=sinθsin[(1λ(t))θ]Qs+sinθsin[λ(t)θ]Qd
      式中, θ = a r c c o s ( Q s ⋅ Q d ) \theta=arccos(Q_{s} \cdot Q_{d}) θ=arccos(QsQd)

    3. 将四元数 Q k ( t ) Q_{k}(t) Qk(t)转换为旋转矩阵 R k R_{k} Rk,将 R k R_{k} Rk和位置插补得到的 P k P_{k} Pk组合得到各插值点对应的齐次变换矩阵 T k T_{k} Tk

    4. 带入逆运动学计算,得到各关节角度变量,进行插补运动。

    规划插补流程

    基于带约束S型加减速曲线的插补算法流程图如下:

    在这里插入图片描述

    Matlab实现代码

    和前面博文的联系非常紧密,部分函数并未展出。

    % 归一化处理
    % S型加减速曲线
    % 输入参数:机械臂末端运动总位移(角度)pos 
    %          机械臂末端线速度(角速度)初始v0,终止v1,最大速度vmax
    %          最大加减速度amax、最大加加速度jmax
    %          插补周期t
    % 返回值: 插值点数N
    function [lambda, N] = Normalization_S(pos, v0, v1, vmax, amax, jmax, t)
    % S曲线参数计算(S型速度规划,又称七段式轨迹)
    % function para = STrajectoryPara(q0, q1, v0, v1, vmax, amax, jmax)
    q0 = 0; q1 = pos;
    para = STrajectoryPara(q0, q1, v0, v1, vmax, amax, jmax); % 获取S曲线参数
    % 总的规划时间
    T = para(1) + para(2) + para(3)
    % 等时插补
    N = ceil(T / t);
    j = 1;
    for i = 0 : t: T
       q(j) = S_position(i, para(1), para(2), para(3), para(4), para(5), para(6), para(7), para(8), para(9), para(10), para(11), para(12), para(13), para(14), para(15), para(16));
    %    qd(j) = S_velocity(i, para(1), para(2), para(3), para(4), para(5), para(6), para(7), para(8), para(9), para(10), para(11), para(12), para(13), para(14), para(15), para(16));
       lambda(j) = q(j) / pos;
       j = j + 1;
    end
    end
    
    % 空间单一直线位置插补与单元四元数姿态插补 + 梯形加减速归一化处理/S型加减速曲线归一化处理
    % 参数:起点S位置, 终点D位置
    %      起点S的单元四元数Qs,终点的单元四元数Qd
    %      起始速度v0,终止速度v1,最大速度vmax,最大加速度amax,最大加加速度jmax
    %      插补周期t
    % 返回值:插值点位置、单元四元数、插值点数
    function [x y z qk N] = SpaceLine_Q(S, D, Qs, Qd, v0, v1, vmax, amax, jmax, t)
    x1 = S(1); y1 = S(2); z1 = S(3);
    x2 = D(1); y2 = D(2); z2 = D(3);
    
    P = 1; % 插值参数,增加插值点数,避免过小
    % 总位移S
    s = sqrt(power(x2 - x1, 2) + power(y2 - y1, 2) + power(z2 - z1, 2))
    % 插值点数N
    % N = ceil(P*s / vs)
    % 求归一化参数:梯形加减速曲线
    % function lambda = Normalization(pos, vel, accl, N)
    % lambda = Normalization(s, vs, a, N);
    % 求归一化参数:S型加减速曲线
    % function lambda = Normalization_S(pos, v0, v1, vmax, amax, jmax, N)
    [lambda, N] = Normalization_S(s, v0, v1, vmax, amax, jmax, t);
    delta_x = x2 - x1;
    delta_y = y2 - y1;
    delta_z = z2 - z1;
    % 计算两个四元数之间的夹角
    dot_q = Qs.s*Qd.s + Qs.v(1)*Qd.v(1) + Qs.v(2)*Qd.v(2) + Qs.v(3)*Qd.v(3);
    if (dot_q < 0)
        dot_q = -dot_q;
    end
     
    for i = 1: N
        % 位置插补
        x(i) = x1 + delta_x*lambda(i);
        y(i) = y1 + delta_y*lambda(i);
        z(i) = z1 + delta_z*lambda(i);
        % 单位四元数球面线性姿态插补
        % 插值点四元数
        if (dot_q > 0.9995)
            k0 = 1-lambda(i);
            k1 = lambda(i);
        else
            sin_t = sqrt(1 - power(dot_q, 2));
            omega = atan2(sin_t, dot_q);
            k0 = sin((1-lambda(i)*omega)) / sin(omega);
            k1 = sin(lambda(i)*omega) / sin(omega);
        end
        qk(i) = Qs * k0 + Qd * k1;
    end
    
    end
    
    % 空间单一圆弧位置插补与单位四元数姿态插补 + 梯形加减速归一化处理/S型加减速曲线归一化处理
    % 参数: 起点S位置, 中间点M位置, 终点D位置
    %       起点S和终点D的单位四元数Qs_,Qd_
    %       起始速度v0,终止速度v1,最大速度vmax,最大加速度amax,最大加加速度jmax
    %       插值周期t
    % 返回值:插值点位置、单元四元数、插值点数
    % 方便起见,角速度和角加速度均为角度制
    function [x y z qk N] = SpaceCircle_Q(S, M, D, Qs_, Qd_, v0, v1, vmax, amax, jmax, t)
    x1 = S(1); x2 = M(1); x3 = D(1);
    y1 = S(2); y2 = M(2); y3 = D(2);
    z1 = S(3); z2 = M(3); z3 = D(3);
    
    A1 = (y1 - y3)*(z2 - z3) - (y2 - y3)*(z1 - z3);
    B1 = (x2 - x3)*(z1 - z3) - (x1 - x3)*(z2 - z3);
    C1 = (x1 - x3)*(y2 - y3) - (x2 - x3)*(y1 - y3);
    D1 = -(A1*x3 + B1*y3 + C1*z3);
    
    A2 = x2 - x1;
    B2 = y2 - y1;
    C2 = z2 - z1;
    D2 = -((x2^2 - x1^2) + (y2^2 - y1^2) + (z2^2 - z1^2)) / 2;
    
    A3 = x3 - x2;
    B3 = y3 - y2;
    C3 = z3 - z2;
    D3 = -((x3^2 - x2^2) + (y3^2 - y2^2) + (z3^2 - z2^2)) / 2;
    A = [A1, B1, C1; A2, B2, C2; A3, B3, C3]
    b = [-D1, -D2, -D3]'
    % 圆心
    C = Gauss_lie(3, A, b)
    x0 = C(1); y0 = C(2); z0 = C(3);
    plot3(x0, y0, z0, 'bo')
    hold on
    % 外接圆半径
    r = sqrt(power(x1 - x0, 2) + power(y1 - y0, 2) + power(z1 - z0, 2));
    % 新坐标系Z0的方向余弦
    L = sqrt(A1^2 + B1^2 + C1^2);
    ax = A1 / L; ay = B1 / L; az = C1 / L;
    % 新坐标系X0的方向余弦
    nx = (x1 - x0) / r;
    ny = (y1 - y0) / r;
    nz = (z1 - z0) / r;
    % 新坐标系Y0的方向余弦
    o = cross([ax, ay, az], [nx, ny, nz]);
    ox = o(1);
    oy = o(2);
    oz = o(3);
    % 相对于基座标系{O-XYZ}, 新坐标系{C-X0Y0Z0}的坐标变换矩阵
    T = [nx ox ax x0;
         ny oy ay y0;
         nz oz az z0;
          0  0  0  1]
    T_ni = T^-1
    % 求在新坐标系{C-X0Y0Z0}下S、M和D的坐标
    S_ = (T^-1)*[S'; 1]
    M_ = (T^-1)*[M'; 1]
    D_ = (T^-1)*[D'; 1]
    x1_ = S_(1) , y1_ = S_(2), z1_ = S_(3)
    x2_ = M_(1), y2_ = M_(2), z2_ = M_(3)
    x3_ = D_(1), y3_ = D_(2), z3_ = D_(3)
    % 判断圆弧是顺时针还是逆时针,并求解圆心角
    if (atan2(y2_, x2_) < 0)
        angle_SOM = atan2(y2_, x2_) + 2*pi;
    else
        angle_SOM = atan2(y2_, x2_);
    end
    if (atan2(y3_, x3_) < 0)
        angle_SOD = atan2(y3_, x3_) + 2*pi;
    else
        angle_SOD = atan2(y3_, x3_);
    end
    % 逆时针
    if (angle_SOM < angle_SOD)
        flag = 1;
        theta = angle_SOD % 圆心角
    end
    % 顺时针
    if (angle_SOM >= angle_SOD)
        flag = -1;
        theta = 2*pi - angle_SOD % 圆心角
    end
    % 插补点数N
    % P = 1; %插补参数,增加插值点数,避免过小
    % ws = ws*pi / 180; % 角度换成弧度
    % a = a*pi / 180;
    % N = ceil(P*theta / ws);
    % % 求归一化参数:梯形加减速曲线
    % lambda = Normalization(theta, ws, a, N);
    % 求归一化参数:S型加减速曲线
    % function lambda = Normalization_S(pos, v0, v1, vmax, amax, jmax, N)
    [lambda, N] = Normalization_S(theta, v0, v1, vmax, amax, jmax, t);
    
    % 插补原理: 在新平面上进行插补(简化)
    % 在新坐标系下z1_,z2_,z3_均为0,即外接圆在新坐标系的XOY平面内
    % 此时转化为平面圆弧插补问题
    delta_ang = theta;
    % 计算两个四元数之间的夹角
    dot_q = Qs_.s*Qd_.s + Qs_.v(1)*Qd_.v(1) + Qs_.v(2)*Qd_.v(2) + Qs_.v(3)*Qd_.v(3);
    if (dot_q < 0)
        dot_q = -dot_q;
    end
    
    for i = 1: N
        % 位置插补
        x_(i) = flag * r * cos(lambda(i)*delta_ang);
        y_(i) = flag * r * sin(lambda(i)*delta_ang);
        P = T*[x_(i); y_(i); 0; 1];
        x(i) = P(1);
        y(i) = P(2);
        z(i) = P(3);
        % 单位四元数球面线性姿态插补
        % 插值点四元数
        if (dot_q > 0.9995)
            k0 = 1-lambda(i);
            k1 = lambda(i);
        else
            sin_t = sqrt(1 - power(dot_q, 2));
            omega = atan2(sin_t, dot_q);
            k0 = sin((1-lambda(i))*omega) / sin(omega);
            k1 = sin(lambda(i)*omega) / sin(omega);
        end
        qk(i) = Qs_ * k0 + Qd_ * k1;
    end
    
    end
    

    不足之处

    1. 圆弧的姿态插补只严格通过了起始姿态和终点姿态,没有经过中间姿态;
    2. 无法达到时间最优。

    参考文献

    [1]刘鹏飞,杨孟兴,宋科,段晓妮.‘S’型加减速曲线在机器人轨迹插补算法中的应用研究[J].制造业自动化,2012,34(20):4-8+11.

    [2]李振娜,王涛,王斌锐,郭振武,陈迪剑.基于带约束S型速度曲线的机械手笛卡尔空间轨迹规划[J].智能系统学报,2019,14(04):655-661.

    [3]王添. 基于单位四元数机器人多姿态轨迹平滑规划[D].天津工业大学,2018.

    [4]季晨. 工业机器人姿态规划及轨迹优化研究[D].哈尔滨工业大学,2013.

    展开全文
  • 基于单位四元数的姿态插补算法 文章目录基于单位四元数的姿态插补算法摘要单位四元数空间与欧式空间的转化四元数的旋转变换表示空间定点旋转两个姿态间的插补仿真验证小结 摘要 现代制造领域对工业机器人的需求...

    基于单位四元数的姿态插补算法

    在这里插入图片描述

    摘要

    现代制造领域对工业机器人的需求越来越多,对工业机器人性能要求也越来越苛刻。机器人运动轨迹插补算法是决定机器人性能的核心技术之一,直接影响着机器人的运动精度、速度和平滑性等主要性能。在曲面、曲线加工,喷涂、弧焊等领域,机器人的轨迹插补,尤其是姿态插补对加工质量和加工效率起到决定性作用。目前对机器人的姿态描述主要使用欧拉法,此类方法存在奇异性以及角速度耦合等问题。单位四元数对姿态的描述更加自然,另外还有效避免了欧拉角旋转时奇异性的问题,且基于单位四元数的运动插补算法计算效率要比欧拉角和余弦矩阵高。目前,单位四元数已经在航天器姿态控,动画制作以及 CAD三维建模等多个领域有着广泛的应用。本文研究的算法等材料可以在gihub下载-链接

    单位四元数空间与欧式空间的转化

    构造姿态曲线的主要问题在于在单位四元数空间中的姿态曲线的构造问题。因为单位四元数存在于 S 3 \mathbf S^3 S3 空间中,然而欧式空间中的度量并不适用于 S 3 \mathbf S^3 S3 空间。同时在欧氏空间中位移的导数为速度,而 S 3 \mathbf S^3 S3 空间中四元数的导数不是角速度,而是角速度与四元数本身的乘积: q ′ = ω q / 2 q'=\omega q/2 q=ωq/2. 下面研究将单位四元数曲线构造问题转换为在欧氏空间中单位矢量的球面曲线构造问题。

    四元数的旋转变换表示空间定点旋转

    定理: 设 q q q R R R为非标量四元数,如果令
    q = ∣ E ∣ ( c o s α 2 + E ∣ E ∣ s i n α 2 ) q=|\mathbf E|(cos\frac{\alpha}{2}+\frac{\mathbf E}{|\mathbf E|}sin\frac{\alpha}{2}) q=E(cos2α+EEsin2α)

    R ′ = q ∗ R ∗ q − 1 R'=q*R*q^{-1} R=qRq1
    是四元数,其范数和标量与四元数 R R R相同,其矢量部分Vect R ′ R' R为Vect R R R绕欧拉轴 E \mathbf E E旋转 α \alpha α角。因此,绕定点矢量旋转可以用四元数变换来表示。

    注意q是单位四元数,可表示为
    q = q 0 + q 1 i + q 2 j + q 3 k = c o s α 2 + ω s i n α 2 q=q_0+q_1\mathbf i+q_2\mathbf j+q_3\mathbf k=cos\frac{\alpha}{2}+\mathbf \omega sin\frac{\alpha}{2} q=q0+q1i+q2j+q3k=cos2α+ωsin2α
    其中 ω \mathbf \omega ω是单位矢量。

    为什么四元数可以表示坐标系旋转变换?因为坐标系是由三个单位矢量构成的,上面定理发现,四元数可以对矢量进行旋转变换,对坐标系做旋转变换可以看成是对坐标系的三个单位矢量做旋转变换。

    因为表示一个旋转变换可以用一个欧拉轴的单位矢量 ω \omega ω和一个角度 α \alpha α来描述,有的书称为等效轴表示法。而这里 ω \omega ω是个定值,对该旋转变换进行插补时,只需对角度 θ \theta θ进行插补即可,而四元数又是联系这两个参数和旋转变换矩阵表示的桥梁,可以把中间插补姿态转化为旋转矩阵。

    四元数和旋转矩阵的关系如下:

    已知旋转矩阵计算四元数
    R = [ r 11 r 12 r 13 r 21 r 22 r 23 r 31 r 32 r 33 ] R=\left[\begin{matrix} r_{11}&r_{12}&r_{13}\\ r_{21}&r_{22}&r_{23}\\ r_{31}&r_{32}&r_{33} \end{matrix}\right] R=r11r21r31r12r22r32r13r23r33
    对应的等效轴参数为

    (1)若(trR-1)/2=1(即为单位阵),那么 α = 0 \alpha=0 α=0, ω \omega ω未定义。

    (2)若trR=-1,那么 α = π \alpha=\pi α=π, ω \omega ω的解为以下可行的向量
    ω = 1 2 ( 1 + r 33 ) [ r 13 r 23 1 + r 33 ] \omega=\frac{1}{\sqrt{2(1+r_{33})}} \left[\begin{matrix} r_{13}\\ r_{23}\\ 1+r_{33} \end{matrix}\right] ω=2(1+r33) 1r13r231+r33
    或者
    ω = 1 2 ( 1 + r 22 ) [ r 12 1 + r 22 r 32 ] \omega=\frac{1}{\sqrt{2(1+r_{22})}} \left[\begin{matrix} r_{12}\\ 1+r_{22}\\ r_{32} \end{matrix}\right] ω=2(1+r22) 1r121+r22r32
    或者
    ω = 1 2 ( 1 + r 11 ) [ 1 + r 11 r 21 r 31 ] \omega=\frac{1}{\sqrt{2(1+r_{11})}} \left[\begin{matrix} 1+r_{11}\\ r_{21}\\ r_{31} \end{matrix}\right] ω=2(1+r11) 11+r11r21r31
    (3)否则, α = c o s − 1 ( 1 2 ( t r R − 1 ) ∈ ( 0 , π ) \alpha=cos^{-1}(\frac{1}{2}(trR-1) \in(0,\pi) α=cos1(21(trR1)(0,π),且
    ω = 1 2 sin α [ r 32 − r 23 r 13 − r 31 r 21 − r 12 ] \omega=\frac{1}{2\text {sin}\alpha} \left[\begin{matrix} r_{32}-r_{23}\\ r_{13}-r_{31}\\ r_{21}-r_{12} \end{matrix}\right] ω=2sinα1r32r23r13r31r21r12
    那么对应的四元数为
    q 0 = cos α 2 , q 1 = r 32 − r 23 2 sin α , q 2 = r 13 − r 31 2 sin α , q 3 = r 21 − r 12 2 sin α q_0=\text{cos}\frac{\alpha}{2},q_1=\frac{r_{32}-r_{23}}{2\text {sin}\alpha},q_2=\frac{r_{13}-r_{31}}{2\text {sin}\alpha},q_3=\frac{r_{21}-r_{12}}{2\text {sin}\alpha} q0=cos2α,q1=2sinαr32r23,q2=2sinαr13r31,q3=2sinαr21r12
    已知四元数计算旋转矩阵
    R = [ q 0 2 + q 1 2 − q 2 2 − q 3 2 2 ( q 1 q 2 − q 0 q 3 ) 2 ( q 0 q 2 + q 1 q 3 ) 2 ( q 0 q 3 + q 1 q 2 ) q 0 2 − q 1 2 + q 2 2 − q 3 2 2 ( q 2 q 3 − q 0 q 1 ) 2 ( q 1 q 3 − q 0 q 2 ) 2 ( q 0 q 1 + q 2 q 3 ) q 0 2 − q 1 2 − q 2 2 + q 3 2 ] R=\left[\begin{matrix} q_0^2+q_1^2-q_2^2-q_3^2 &2(q_1q_2-q_0q_3) &2(q_0q_2+q_1q_3)\\ 2(q_0q_3+q_1q_2)&q_0^2-q_1^2+q_2^2-q_3^2&2(q_2q_3-q_0q_1)\\ 2(q_1q_3-q_0q_2)&2(q_0q_1+q_2q_3)&q_0^2-q_1^2-q_2^2+q_3^2 \end{matrix}\right] R=q02+q12q22q322(q0q3+q1q2)2(q1q3q0q2)2(q1q2q0q3)q02q12+q22q322(q0q1+q2q3)2(q0q2+q1q3)2(q2q3q0q1)q02q12q22+q32
    上面旋转矩阵与欧拉轴,转角,四元数之间的相互转换,我们写成以下三个函数模块,后面构建姿态插补算法将会用到它们。C语言实现代码如下:

    	#define			PI					3.14159265358979323846
    	//if the norm of vector is near zero(< 1.0E-6),regard as zero.
    	#define			ZERO_VECTOR			1.0E-6	
    	#define			ZERO_ELEMENT		1.0E-6		
    	#define			ZERO_ANGLE			1.0E-6
    	#define			ZERO_DISTANCE		1.0E-6
    	#define			ZERO_VALUE			1.0E-6
    /**
     * @brief 			Description: Computes the unit vector of Euler axis and rotation angle corresponding to rotation matrix.
     * @param[in]		R				A rotation matrix.
     * @param[out]		omghat			the unit vector of Euler axis .
     * @param[out]		theta			the rotation angle.
     * @return			No return value.
     * @retval			0
     * @note:			if  theta is zero ,the unit axis is undefined and set it as a zero vector [0;0;0].
     *@warning:
    */
    void RotToAxisAng(double R[3][3],double omghat[3],double *theta)
    {
    	double tmp;
    	double omg[3] = { 0 };
    	double acosinput = (R[0][0] + R[1][1] + R[2][2] - 1.0) / 2.0;
    	if (fabs(acosinput-1.0)<ZERO_VALUE)
    	{
    		memset(omghat, 0, 3 * sizeof(double));
    		*theta = 0.0;
    	}
    	else if (acosinput <= -1.0)
    	{
    		if ((1.0 + R[2][2]) >= ZERO_VALUE)
    		{
    			omg[0] = 1.0 / sqrt(2 * (1.0 + R[2][2]))*R[0][2];
    			omg[1] = 1.0 / sqrt(2 * (1.0 + R[2][2]))*R[1][2];
    			omg[2] = 1.0 / sqrt(2 * (1.0 + R[2][2]))*(1.0 + R[2][2]);
    		}
    		else if ((1.0 + R[1][1] >= ZERO_VALUE))
    		{
    			omg[0] = 1.0 / sqrt(2 * (1.0 + R[1][1]))*R[0][1];
    			omg[1] = 1.0 / sqrt(2 * (1.0 + R[1][1]))*(1.0 + R[1][1]);
    			omg[2] = 1.0 / sqrt(2 * (1.0 + R[1][1]))*R[2][1];
    		}
    		else
    		{
    			omg[0] = 1.0 / sqrt(2 * (1.0 + R[0][0]))*(1.0 + R[0][0]);
    			omg[1] = 1.0 / sqrt(2 * (1.0 + R[0][0]))*R[1][0];
    			omg[2] = 1.0 / sqrt(2 * (1.0 + R[0][0]))*R[2][0];
    		}
    		omghat[0] = omg[0];
    		omghat[1] = omg[1];
    		omghat[2] = omg[2];
    		*theta=PI;
    	}
    	else
    	{
    		*theta = acos(acosinput);
    		tmp = 2.0*sin(*theta);
    		omghat[0] = (R[2][1] - R[1][2]) / tmp;
    		omghat[1] = (R[0][2] - R[2][0]) / tmp;
    		omghat[2] = (R[1][0] - R[0][1]) / tmp;
    
    	}
    
    	return;
    }
    
    
    /**
     * @brief 			Description: Computes the unit quaternion corresponding to the Euler axis and rotation angle.
     * @param[in]		omg				Unit vector of Euler axis.
     * @param[in]		theta			Rotation angle.
     * @param[in]		q				The unit quaternion
     * @return			No return value.
     * @note:
     *@warning:
    */
    void AxisAngToQuaternion(double omg[3],double theta, double q[4])
    {
    	q[0] = cos(theta / 2.0);
    	q[1] = omg[0] * sin(theta / 2.0);
    	q[2] = omg[1] * sin(theta / 2.0);
    	q[3] = omg[2] * sin(theta / 2.0);
    	return;
    }
    
    
    /**
     * @brief 			Description:Computes the unit quaternion corresponding to a rotation matrix.
     * @param[in]		q				Unit quaternion.
     * @param[out]		R				Rotation matrix.
     * @return			No return value.
     * @note:
     * @warning:
    */
    void QuaternionToRot(double q[4], double R[3][3])
    {
    	R[0][0] = q[0] * q[0] - q[1] * q[1] - q[2] * q[2] - q[3] * q[3];
    	R[0][1] = 2.0*(q[1] * q[2] - q[0] * q[3]);
    	R[0][2] = 2.0*(q[0] * q[2] + q[1] * q[3]);
    	R[1][0] = 2.0*(q[0] * q[3] + q[1] * q[2]);
    	R[1][1] = q[0] * q[0] - q[1] * q[1] + q[2] * q[2] - q[3] * q[3];
    	R[1][2] = 2.0*(q[2] * q[3] - q[0] * q[1]);
    	R[2][0] = 2.0*(q[1] * q[3] - q[0] * q[2]);
    	R[2][1] = 2.0*(q[0] * q[1] + q[2] * q[3]);
    	R[2][2] = q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3];
    	return;
    }
    
    /**
     * @brief 			Description: Computes the unit quaternion corresponding to the rotation matrix.
     * @param[in]		R				The rotation matrix.
     * @param[out]		q				The unit quaternion.
     * @return			No return value.
     * @note:
     * @warning:
    */
    void RotToQuaternion(double R[3][3], double q[4])
    {
    	double omghat[3];
    	double theta;
    	RotToAxisAng(R, omghat, &theta);
    	AxisAngToQuaternion(omghat, theta, q);
    	return;
    }
    

    两个姿态间的插补

    有了前面的理论基础,接下来我们就可以构建基于单位四元数的插补算法了。根据个人经验,基本思路如下

    在这里插入图片描述

    这里显示了速度规划化等和插补算法间的关系,这里不打算研究速度规划,因此姿态插补和位置补中,直接用了等距的步长。

    首先用户输入姿态坐标一般是用欧拉角表示,即
    p = [ x , y , z , γ , β , α ] T p=[x,y,z,\gamma,\beta,\alpha]^T p=[x,y,z,γ,β,α]T
    因此需要把欧拉角转化为旋转矩阵。

    假设用户输入的起点和终点位姿对应的旋转矩阵分别为 R s , R e R_s,R_e Rs,Re,设这两个姿态的旋转变换矩阵为 R R R,则有
    R s R = R e R_sR=R_e RsR=Re
    注意这里是对矢量进行旋转,因此是右乘。因此旋转变换矩阵为
    R = R s − 1 R e R=R_s^{-1}R_e R=Rs1Re
    那么我们接下来实际是计算R的对应的欧拉参数,每次插补计算得到一个旋转矩阵 R i R_i Ri,机器人末端对应的姿态为 R s i = R s R i R_{si}=R_sR_i Rsi=RsRi

    姿态插补和位置插补均需要周期地循环计算,通过速度规划时就要保证姿态和位置的运动同步。

    逆运动学计算的输入一般是齐次矩阵,输出是关节坐标。

    下面仅针对直线路径的情形,把各个小模块写成C函数的形式,方便根据具体需求构建控制系统。

    欧拉角转化为旋转矩阵

    /**
    * @brief 			Description: Algorithm for Computing the rotation matrix of the roll-pitch-yaw angles.
    * @param[in]		roll			Angles for rotate around fix reference X axis.
    * @param[in]		pitch			Angles for rotate around fix reference Y axis.
    * @param[in]		yaw				Angles for rotate around fix reference Z axis.
    * @param[out]		R				Rotation matrix.
    * @return			No return value.
    * @note:
    *@warning:
    */
    void RPYToRot(double roll, double pitch, double yaw, double R[3][3])
    {
    	double alpha = yaw;
    	double beta = pitch;
    	double gamma = roll;
    	R[0][0] = cos(alpha)*cos(beta);
    	R[0][1] = cos(alpha)*sin(beta)*sin(gamma) - sin(alpha)*cos(gamma);
    	R[0][2] = cos(alpha)*sin(beta)*cos(gamma) + sin(alpha)*sin(gamma);
    	R[1][0] = sin(alpha)*cos(beta);
    	R[1][1] = sin(alpha)*sin(beta)*sin(gamma) + cos(alpha)*cos(gamma);
    	R[1][2] = sin(alpha)*sin(beta)*cos(gamma) - cos(alpha)*sin(gamma);
    	R[2][0] = -sin(beta);
    	R[2][1] = cos(beta)*sin(gamma);
    	R[2][2] = cos(beta)*cos(gamma);
    	return;
    }
    

    姿态插补相关函数

    
    	/**
    	* @brief 			Description: structure of LinePath interpolation parameters.
    	*/
    	typedef struct
    	{
    		double p1[3];
    		double p2[3];
    		double L;
    		double t[3];
    		double pi[3];
    		double Li;
    		int InpFlag;
    	}LineInpParam;
    
    
    	/**
    	* @brief 			Description: structure of arc interpolation parameters.
    	*/
    	typedef struct  
    	{
    		double p1[3];
    		double p2[3];
    		double p3[3];
    		double N[3];
    		double C[3];
    		double theta;
    		double R;
    	}ArcInpParam;
    
    
    	/**
    	* @brief 			Description: structure of orientation interpolation parameters.
    	*/
    	typedef struct
    	{
    		double Rs[3][3];//Start orientation Rotation matrix. 
    		double Re[3][3];//End orientation Rotation matrix.
    		double R[3][3];	//Matrix rotation from Start orientation to End orientation.
    		double omg[3];  //unit vector of Euler axis.
    		double theta;	//Total interpolation angle.
    		double Ri[3][3];//Current orientation rotation Matrix.
    		double thetai;  //Current angle.
    		int InpFlag;	//1:finish initial ,2;interpolating ,3;finish interpolation
    	}OrientInpParam;
    
    
    	/**
    	* @brief 			Description: structure of line Path and Orientation (PO) interpolation parameters.
    	*/
    	typedef struct  
    	{
    		LineInpParam Line;
    		OrientInpParam Orient;
    		double Ts[4][4];
    		double Te[4][4];
    		double Ti[4][4];
    		int InpFlag;
    	}LinePOParam;
    
    /**
     * @brief 			Description: Computes the parameters of  orientation interpolation between two orientations.
     * @param[in]		Rs				Start orientation.
     * @param[in]		Re				End orientation. 
     * @param[in]		Param			structure of orientation interpolation parameters..
     * @return			No return value.
     * @note:
     * @warning:
    */
    void InitialOrientInpParam(double Rs[3][3],double Re[3][3], OrientInpParam *Param)
    {
    	double InvR[3][3];
    	MatrixCopy((double *)Rs, 3, 3, (double *)Param->Rs);
    	MatrixCopy((double *)Re, 3, 3, (double *)Param->Re);
    	RotInv(Rs, InvR);
    	MatrixMult((double *)InvR, 3, 3, (double *)Re, 3, (double *)Param->R);
    	RotToAxisAng(Param->R, Param->omg, &Param->theta);
    	MatrixCopy((double *)Param->R, 3, 3, (double *)Param->Ri);
    	Param->thetai = 0.0;
    	Param->InpFlag = 1;
    	return;
    }
    
    
    /**
     * @brief 			Description: Computes orientations in each interpolation cycle.
     * @param[in]		Param			Interpolation parameter structure.
     * @param[out]		dtheta			angle  need to rotate from previous orientation to next orientation in next time step.
     * @return			Ri1				orientations in next interpolation cycle.
     * @retval			0
     * @note:
     * @warning:
    */
    void QuaternionOrientInp(OrientInpParam *Param, double dtheta, double Ri1[3][3])
    {
    	double q[4];
    	double R[3][3];
    	Param->InpFlag = 2;
    	Param->thetai = Param->thetai + dtheta;
    	if (Param->thetai >= Param->theta)
    	{
    		Param->thetai = Param->theta;
    		Param->InpFlag = 3;
    	}
    	AxisAngToQuaternion(Param->omg, Param->thetai, q);
    	QuaternionToRot(q, R);
    	MatrixMult((double *)Param->Rs, 3, 3, (double *)R, 3, (double *)Ri1);
    	MatrixCopy((double *)Ri1, 3, 3, (double *)Param->Ri);
    	return;
    }
    

    直线路径插补相关函数

    
    /**
     * @brief 			Description:Computes the parameters of line path for interpolation.
     * @param[in]		p1				Coordinates of start point.
     * @param[in]		p2				Coordinates of end point.
     * @param[out]		p				Line path parameters structure.
     * @return			No return value.
     * @note:
     * @warning:
    */
    void InitialLinePathParam(double p1[3],double p2[3], LineInpParam *p)
    {
    	int i;
    	for (i=0;i<3;i++)
    	{
    		p->p1[i] = p1[i];
    		p->p2[i] = p2[i];
    		p->pi[i] = p1[i];
    	}
    	p->L = sqrt((p2[0] - p1[0])*(p2[0] - p1[0]) + (p2[1] - p1[1])*(p2[1] - p1[1]) + (p2[2] - p1[2])*(p2[2] - p1[2]));
    
    	if (p->L<ZERO_DISTANCE)
    	{
    		p->t[0] = 0.0;
    		p->t[1] = 0.0;
    		p->t[2] = 0.0;
    	}
    	else
    	{ 
    		p->t[0] = (p2[0] - p1[0]) / p->L;
    		p->t[1] = (p2[1] - p1[1]) / p->L;
    		p->t[2] = (p2[2] - p1[2]) / p->L;
    	}
    	p->InpFlag = 1;
    	p->Li = 0;
    	return;
    }
    
    /**
     * @brief 			Description:Computes the line path interpolation coordinates in each interpolation cycle.
     * @param[in]		p				Line path parameters structure.
     * @param[in]		dL				step length in next interpolation cycle. 
     * @param[in]		pi1				coordinates in next interpolation cycle.
     * @return			No return value.
     * @note:
     * @warning:
    */
    void LinePathInp(LineInpParam *p, double dL, double pi1[3])
    {
    	p->InpFlag = 2;
    	if (p->Li + dL>=p->L)
    	{
    		pi1[0] = p->p2[0];
    		pi1[1] = p->p2[1];
    		pi1[2] = p->p2[2];
    		p->Li = p->L;
    		p->InpFlag = 3;
    	}
    	else if ( p->L- p->Li < 2.0*dL)
    	{
    		//avoid distance of  final step  is too small.
    		dL = 0.5*dL;
    		pi1[0] = p->pi[0] + p->t[0] * dL;
    		pi1[1] = p->pi[1] + p->t[1] * dL;
    		pi1[2] = p->pi[2] + p->t[2] * dL;
    		p->Li = p->Li+dL;
    	}
    	else
    	{
    		pi1[0] = p->pi[0] + p->t[0] * dL;
    		pi1[1] = p->pi[1] + p->t[1] * dL;
    		pi1[2] = p->pi[2] + p->t[2] * dL;
    		p->Li = p->Li + dL;
    	}
    
    	p->pi[0] = pi1[0];
    	p->pi[1] = pi1[1];
    	p->pi[2] = pi1[2];
    	return;
    }
    

    把姿态插补和直线路径插补封装在一起,得到直线的位姿插补函数

    
    /**
     * @brief 			Description: Computes the parameters of both line path and orientation for interpolation.
     * @param[in]		p1				Start coordinates,including x,y,z coordinates and orientation angles roll-pitch-yaw angles.
     * @param[in]		p2				End coordinates,including x,y,z coordinates and orientation angles roll-pitch-yaw angles.
     * @param[out]		LPO				Parameters of both line path and orientation for interpolation.
     * @return			No return value.
     * @note:
     * @warning:
    */
    void InitialLinePOInpParam( double p1[6], double p2[6], LinePOParam *LPO)
    {
    	double Rs[3][3];
    	double Re[3][3];
    	RPYToRot(p1[3], p1[4], p1[5], Rs);
    	RPYToRot(p2[3], p2[4], p2[5], Re);
    	InitialLinePathParam( p1, p2, &(LPO->Line));
    	InitialOrientInpParam(Rs, Re, &(LPO->Orient));
    	RpToTrans(Rs, p1, LPO->Ts);
    	RpToTrans(Rs, p1, LPO->Ti);
    	RpToTrans(Re, p2, LPO->Te);
    	LPO->InpFlag = 1;
    	return;
    }
    
    /**
     * @brief 			Description:Computes the line path interpolation coordinates and orientation in each interpolation cycle.
     * @param[out]		LPO				Line path and orientation parameters structure.
     * @param[in]		dL				Line path interpolation step length.
     * @param[out]		dtheta			angle interpolation step length for Orientation interpolation.
     * @return			No return value.
     * @note:
     * @warning:
    */
    void LinePOInp(LinePOParam *LPO,double dL,double dtheta,double Ti[4][4])
    {
    	double pi[3];
    	double Ri[3][3];
    	LPO->InpFlag = 2;
    	LinePathInp(&LPO->Line, dL, pi);
    	QuaternionOrientInp(&LPO->Orient, dtheta, Ri);
    	if (LPO->Line.InpFlag==3 && LPO->Orient.InpFlag==3)
    	{
    		LPO->InpFlag = 3;
    	}
    	RpToTrans(Ri, pi, Ti);
    	MatrixCopy((double *)Ti, 4, 4, (double *)LPO->Ti);
    	return;
    }
    

    仿真验证

    有了上面的函数,以及前面博客机器人学之逆运动学数值解法及SVD算法的逆运动学计算等模块,我们就可以集成一个直线路径的位姿插补程序。

    (1)下面以UR3机器人为例,我们对两条线段进行插补,路径的坐标为

    p1[6] = { 213.0,267.8,478.95,0,0,0 };

    p2[6] = { 10,425,200, -PI / 2,0 ,0 };

    p2[6] = { -10,525,200,-PI / 4,0,-PI / 6 };

    插补生成路径的位置和姿态序列,进行逆运动学计算,得到关节坐标序列。仿真结果如下(动画是循环播放,实际路径是两条线段)。

    在这里插入图片描述

    示例程序如下

    void test_LinePOInp()
    {
    	double p1[6] = { 213.0,267.8,478.95,0,0,0 };
    	double p2[6] = { 10,425,200, -PI / 2,0 ,0 };
    	//double p1[6] = { 10,425,200, -PI / 2,0 ,0 };
    	//double p2[6] = { -10,525,200,-PI / 4,0,-PI / 6 };
    	double Ti[4][4];
    	double dL = 1;
    	FILE *fp1;
    	int ret = fopen_s(&fp1, "LineTrajactory.txt","w");
    	if (ret)
    	{
    		printf("fopen_s error %d\n", ret);
    	}
    	LinePOParam pt;
    	InitialLinePOInpParam(p1, p2, &pt);
    	double dtheta = pt.Orient.theta/(pt.Line.L / dL);
    	int JointNum = 6;
    	double Slist[6][6] = {
    		0 ,        0,         0,         0 ,        0,         0,
    		0 ,   1.0000 ,   1.0000 ,   1.0000,         0,    1.0000,
    		1.0000,         0 ,        0 ,        0 ,   1.0000,         0,
    		0, -151.9000, -395.5500, -395.5500 , 110.4000 ,-478.9500,
    		0 ,        0 ,        0  ,       0 ,-213.0000,         0,
    		0  ,       0 ,        0,  213.0000 ,        0,  213.0000
    	};
    	double M[4][4] =
    	{
    		1.0000 ,        0,         0,  213.0000,
    		0 ,   1.0000 ,        0,  267.8000,
    		0 ,        0 ,   1.0000,  478.9500,
    		0 ,        0  ,       0,    1.0000,
    	};
    	double thetalist0[6] = { 0 };
    	//double thetalist0[6] = { 1.284569 ,0.488521, - 0.443200, 1.525477, - 1.570797, - 0.286227 };
    
    	double thetalist[6];
    	double eomg = 0.001;
    	double ev = 0.01;
    	while (pt.InpFlag!=3)
    	{
    		LinePOInp(&pt, dL, dtheta, Ti);
    		IKinSpaceNR(JointNum,(double *)Slist, M, Ti, thetalist0,eomg,ev,10,thetalist);
    		//MatrixCopy((double *)Ti, 4, 4, (double *)M);
    		MatrixCopy(thetalist, 6, 1, thetalist0);//当前关节坐标作为下次逆解计算的初值.
            //把关节坐标写入文件
    		fprintf(fp1, "%lf %lf %lf %lf %lf %lf\n", thetalist[0], thetalist[1], thetalist[2], thetalist[3], thetalist[4], thetalist[5]);
    	}
    	fclose(fp1);
    	return;
    }
    

    (2)定点姿态插补

    假设我们输入的两个坐标位置一样,只是姿态不同,那么应该能实现定点姿态插补。

    p1[6] = { 10,425,200, -PI / 2,0 ,0 };
    p2[6] = { 10,425,200, -PI *3/ 4,0 ,PI / 2 };

    类似上面插补的示例程序,对两个路径做直线插补,计算的关节坐标进行仿真,效果如下图

    在这里插入图片描述

    示例程序如下

    void test_LinePOInp()
    {
    	//double p1[6] = { 213.0,267.8,478.95,0,0,0 };
    	//double p2[6] = { 10,425,200, -PI / 2,0 ,0 };
    	//double p1[6] = { 10,425,200, -PI / 2,0 ,0 };
    	//double p2[6] = { -10,525,200,-PI / 4,0,-PI / 6 };
    	double p1[6] = { 10,425,200, -PI / 2,0 ,0 };
    	double p2[6] = { 10,425,200, -PI *3/ 4,0 ,PI / 2 };
    	double Ti[4][4];
    	double dL = 1;
    	FILE *fp1;
    	int ret = fopen_s(&fp1, "LineTrajactory.txt","w");
    	if (ret)
    	{
    		printf("fopen_s error %d\n", ret);
    	}
    	LinePOParam pt;
    	InitialLinePOInpParam(p1, p2, &pt);
    	//double dtheta =pt.Orient.theta/(pt.Line.L / dL);
    	double dtheta = PI / 100;
    	int JointNum = 6;
    	double Slist[6][6] = {
    		0 ,        0,         0,         0 ,        0,         0,
    		0 ,   1.0000 ,   1.0000 ,   1.0000,         0,    1.0000,
    		1.0000,         0 ,        0 ,        0 ,   1.0000,         0,
    		0, -151.9000, -395.5500, -395.5500 , 110.4000 ,-478.9500,
    		0 ,        0 ,        0  ,       0 ,-213.0000,         0,
    		0  ,       0 ,        0,  213.0000 ,        0,  213.0000
    	};
    	double M[4][4] =
    	{
    		1.0000 ,        0,         0,  213.0000,
    		0 ,   1.0000 ,        0,  267.8000,
    		0 ,        0 ,   1.0000,  478.9500,
    		0 ,        0  ,       0,    1.0000,
    	};
    	//double thetalist0[6] = { 0 };
    	double thetalist0[6] = { 1.284569 ,0.488521, - 0.443200, 1.525477, - 1.570797, - 0.286227 };
    
    	double thetalist[6];
    	double eomg = 0.001;
    	double ev = 0.01;
    	while (pt.InpFlag!=3)
    	{
    		LinePOInp(&pt, dL, dtheta, Ti);
    		IKinSpaceNR(JointNum,(double *)Slist, M, Ti, thetalist0,eomg,ev,10,thetalist);
    		//MatrixCopy((double *)Ti, 4, 4, (double *)M);
    		MatrixCopy(thetalist, 6, 1, thetalist0);
    		fprintf(fp1, "%lf %lf %lf %lf %lf %lf\n", thetalist[0], thetalist[1], thetalist[2], thetalist[3], thetalist[4], thetalist[5]);
    	}
    	fclose(fp1);
    	return;
    }
    

    逆解的关节坐标序列如下

    1.289932 0.496461 -0.458281 1.540103 -1.584799 -0.307921
    1.295334 0.504302 -0.472991 1.554231 -1.598923 -0.329481
    1.300770 0.512033 -0.487316 1.567856 -1.613161 -0.350925
    1.306238 0.519643 -0.501232 1.580967 -1.627510 -0.372268
    1.311735 0.527116 -0.514715 1.593553 -1.641965 -0.393525
    1.317257 0.534438 -0.527739 1.605603 -1.656523 -0.414710
    1.322802 0.541595 -0.540278 1.617102 -1.671178 -0.435840
    1.328368 0.548571 -0.552304 1.628036 -1.685928 -0.456928
    1.333951 0.555352 -0.563789 1.638391 -1.700769 -0.477991
    1.339549 0.561922 -0.574706 1.648150 -1.715695 -0.499044
    1.345159 0.568267 -0.585024 1.657297 -1.730704 -0.520101
    1.350778 0.574370 -0.594715 1.665811 -1.745792 -0.541180
    1.356404 0.580217 -0.603749 1.673676 -1.760954 -0.562295
    1.362033 0.585792 -0.612097 1.680872 -1.776185 -0.583462
    1.367662 0.591081 -0.619729 1.687377 -1.791483 -0.604697
    1.373289 0.596069 -0.626617 1.693172 -1.806842 -0.626016
    1.378910 0.600741 -0.632733 1.698236 -1.822257 -0.647437
    1.384522 0.605086 -0.638051 1.702547 -1.837724 -0.668974
    1.390121 0.609089 -0.642543 1.706084 -1.853237 -0.690645
    1.395705 0.612740 -0.646188 1.708826 -1.868790 -0.712467
    1.401271 0.616028 -0.648963 1.710753 -1.884378 -0.734457
    1.406813 0.618943 -0.650849 1.711842 -1.899994 -0.756633
    1.412329 0.621477 -0.651829 1.712076 -1.915631 -0.779012
    1.417815 0.623625 -0.651889 1.711433 -1.931281 -0.801613
    1.423267 0.625381 -0.651018 1.709895 -1.946939 -0.824454
    1.428681 0.626741 -0.649208 1.707445 -1.962594 -0.847553
    1.434053 0.627706 -0.646456 1.704065 -1.978238 -0.870930
    1.439378 0.628275 -0.642761 1.699741 -1.993862 -0.894604
    1.444653 0.628450 -0.638125 1.694456 -2.009455 -0.918595
    1.449873 0.628237 -0.632555 1.688197 -2.025008 -0.942922
    1.455034 0.627642 -0.626062 1.680950 -2.040508 -0.967606
    1.460131 0.626672 -0.618659 1.672705 -2.055945 -0.992667
    1.465159 0.625339 -0.610363 1.663451 -2.071304 -1.018126
    1.470113 0.623654 -0.601196 1.653176 -2.086573 -1.044004
    1.474990 0.621632 -0.591179 1.641872 -2.101738 -1.070322
    1.479784 0.619287 -0.580340 1.629531 -2.116783 -1.097101
    1.484490 0.616636 -0.568707 1.616144 -2.131693 -1.124362
    1.489104 0.613699 -0.556313 1.601706 -2.146451 -1.152127
    1.493620 0.610496 -0.543190 1.586210 -2.161040 -1.180417
    1.498035 0.607047 -0.529374 1.569651 -2.175441 -1.209252
    1.502342 0.603376 -0.514903 1.552024 -2.189637 -1.238653
    1.506538 0.599505 -0.499817 1.533327 -2.203606 -1.268639
    1.510617 0.595461 -0.484156 1.513557 -2.217329 -1.299229
    1.514576 0.591268 -0.467963 1.492712 -2.230784 -1.330442
    1.518409 0.586954 -0.451280 1.470795 -2.243950 -1.362294
    1.522112 0.582544 -0.434153 1.447806 -2.256802 -1.394800
    1.525680 0.578068 -0.416627 1.423751 -2.269320 -1.427975
    1.529110 0.573553 -0.398748 1.398637 -2.281477 -1.461829
    1.532398 0.569028 -0.380564 1.372472 -2.293250 -1.496373
    1.535539 0.564522 -0.362122 1.345270 -2.304615 -1.531613
    1.538529 0.560062 -0.343471 1.317049 -2.315546 -1.567553
    1.541366 0.555678 -0.324660 1.287827 -2.326017 -1.604192
    1.544046 0.551397 -0.305737 1.257632 -2.336004 -1.641529
    1.544046 0.551397 -0.305737 1.257632 -2.336004 -1.641529
    1.544046 0.551397 -0.305737 1.257632 -2.336004 -1.641529
    
    

    小结

    在学习过程中,我对机器人算法的程序不断更新迭代,完整程序在gihub可下载-链接

    经过本文的研究,对机械臂的姿态插补实现基本掌握了。两个姿态间插补算法可以总结为以下几步

    (1)把输入起点和终点欧拉角转换为旋转矩阵 R s , R e R_s,R_e Rs,Re

    (2)两个姿态间的变换矩阵 R = R s − 1 R e R=R_s^{-1}R_e R=Rs1Re

    (3)由旋转矩阵R计算等效轴和转角(即欧拉参数),并写出对应的四元数。

    (4)四元数转化为旋转矩阵 R i R_i Ri,计算中间姿态序列 R s i = R s R i R_{si}=R_sR_i Rsi=RsRi .

    (5)综合姿态和位置,得到表示位姿的齐次矩阵序列 T s i T_{si} Tsi


    参考文献

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

    [2]季晨. 工业机器人姿态规划及轨迹优化研究[D]. 哈尔滨工业大学, 2013.

    [3] 程国采.四元数法及其应用[M].湖南:国防科技大学出版社,1991.

    展开全文
  • 轨迹规划与插补

    千次阅读 2019-03-11 15:30:08
    原机械臂轨迹规划——空间圆弧和直线插补及姿态平滑 姿态插补 线性插值 普通线性插值 线性插值(Lerp/Linear Interpolation),即沿着一条直线(也就是圆上的一个弦)进行插值,此种插值方式所得结果并非单位四元数...

    原 机械臂轨迹规划——空间圆弧和直线插补及姿态平滑

    姿态插补 线性插值 普通线性插值 线性插值(Lerp/Linear Interpolation),即沿着一条直线(也就是圆上的一个弦)进行插值,此种插值方式所得结果并非单位四元数(只有单位四元数才能表示旋转)。 正规化线性插值 正规化线性插值(Normalized LinearInterpolat...

    2019-01-31 10:44:20

     

    阅读数 454

     

    评论数 15

    原 机械臂——六轴机械臂操作空间运动分析

    机械臂建模分析:https://blog.csdn.net/Kalenee/article/details/81990130 MoveIt规划下的关节空间运动分析:http://www.guyuehome.com/752   一、简介       在ROS平台下使用MoveIt进行机械臂控...

    2018-11-12 15:03:09

     

    阅读数 418

     

    评论数 0

    原 机械臂运动控制——robotics toolbox配置

    1、简介 Matlab_Robotic_Toolbox是一个功能强大的机器人工具箱,包含了机器人正、逆向运动学,正、逆向动力学,轨迹规划等等,其中的可视化仿真可使抽象的机器人学变得相对直观。   2、配置 此处安装robotics toolbox 10.3 下载链接:http://pet...

    2018-10-27 00:36:06

     

    阅读数 286

     

    评论数 0

    原 ROS Industrial——运动学规划算法STOMP

    环境:Ubuntu16.04+ROS Kinetic   一、简介 STOMP(随机轨迹优化运动规划算法)是基于PI^2算法的进行优化的运动规划器。它可以完成机器人手臂运动轨迹的平滑规划,同时避开障碍物和优化约束。 该算法不需要梯度,因此可以优化成本函数中的任意项,如电机工作。 注意:ST...

    2018-09-13 11:19:17

     

    阅读数 409

     

    评论数 2

    原 机械臂运动控制——三维空间刚体运动描述

    一、旋转矩阵 在机器人运动的过程当中,我们通常会设定一个惯性坐标系(或者叫世界坐标系),姑且认为这个坐标系是固定不动的。例如:,,是固定不动的世界坐标系,,,​​是机器人坐标系。存在一个向量,在世界坐标系下的坐标是​​,在移动机器人坐标系下的坐标是​​,通常情况下,我们通过传感器已知移动机器人坐...

    2018-09-12 11:08:41

     

    阅读数 862

     

    评论数 0

    原 机械臂——六轴机械臂逆解

    环境:MATLAB 2017B+Robotics Toolbox 9.10.0 前期准备:完成机械臂数学模型的建立+计算机械臂工作空间 https://blog.csdn.net/Kalenee/article/details/81990130 注意:这里采用几何法计算机械臂逆解,因此不一定...

    2018-09-01 23:57:32

     

    阅读数 2800

     

    评论数 0

    原 机械臂——六轴机械臂构型分析与MATLAB建模

    环境:MATLAB 2017B+Robotics Toolbox 10.3(版本不一致有可能会报错) Robotics Toolbox:http://petercorke.com/wordpress/toolboxes/robotics-toolbox 机械臂逆解计算:https://blog...

    2018-08-26 14:15:56

     

    阅读数 4599

     

    评论数 30

    原 运动控制——三点圆弧和平面圆弧插补

    设输入三点为圆弧上的三个点 ,) ,,三点按顺时针或者逆时针在圆弧上排列。 一、三点圆弧 1、判断三点是否共线及圆弧走向 求与的向量积   (1) 结果为正:圆弧是逆时针画    (2) 结果为负:圆弧是顺时针画   (3 )结果为零:三点在同一直线上 2、计算圆弧圆心及半径 ...

    2018-07-26 10:42:51

     

    阅读数 2161

     

    评论数 2

    原 ROS进阶——通过MoveIt!控制实体机械臂PC层

    环境:Ubuntu16.04+ROS Kinetic 硬件: Arduino mega2560(需拥有两个串口) 总线舵机(可采用dynamixel舵机,其带有ROS的功能包dynamixel_controllersTutorials) 前期准备: 完成MoveIt!配置 完成机器人在...

    2018-06-10 21:10:27

     

    阅读数 2360

     

    评论数 5

    展开全文
  • 写在前面 机械臂用的是五自由度的,我测试时发现逆解精度存在...常规插补算法 假设机器人末端由P1点沿直线运动到P2点,(x1,y1,z1)\left(x_{1}, y_{1}, z_{1}\right)(x1​,y1​,z1​)和(α(1,β1,γ1)\left(\alpha(_{...
  • * 简单的空间插补程序:目前只更新空间直线插补和空间圆弧插补程序 * 传统插补方法+梯形加减速归一化处理 * 归一化因子采用抛物线过渡的线性函数(梯形加减速), * 以保证整段轨迹上的位移和速度都...
  • 该方法采用了3根镗杆和1台直接驱动旋转电机,通过3个转动关节连接在一起,安装在专用机床上,实现对核电管道弯一次性装夹,完成对其弯孔的加工。文章对各插补参数进行的理论分析与计算,为下一步的联动编程提供了...
  • 2019/4/19

    2019-04-19 22:37:10
    明天任务 1、设计姿态不变走直线,应该可以。 2、空间关节插补 3、姿态插补
  • 本文分析空间二次旋转曲面的产品数学模型,通过参数方程求解二次曲面各点在笛卡尔坐标系中的位姿矩阵,从而计算出机械手臂每个关节的电机旋转角度.由MATLAB软件进行验证,观测此二次曲面的粗插补算法的准确度和精确...
  •  一 插补运动 插补是机床数控系统依照一定方法确定刀具运动轨迹的过程,插补是一个实时进行的数据密化的过程,不论是何种插补算法,运算原理基本相同,其作用都是根据给定的信息进行数据计算,不断计算出参与插补...
  • 今天在SIMWE 网上浏览,见有网友询问机器人轨迹规划和数控插补之间的关系。正好这段时间,做数控相关的项目,有些心得,便花点时间,做了回复。以下将回复内容抄录于此。 要说这个问题,有四个概念需要明确。头两个...
  • UR机器人(二):直线插补详解

    千次阅读 2019-04-23 22:20:28
    看过UR机器人脚本手册的都应该知道有这样一个直线插补函数: interpolate_pose(p_from, p_to, alpha); 参数:p_from表示初始pose,p_to表示目标pose,alpha通常为0-1之间的浮点数,如果alpha=0,则函数插值返回的...
  • 三次样条插补的实现

    2017-07-18 15:21:00
    机械臂的平滑运动需要确定各个轨迹点的位置、速度、各点间的运行时间,甚至还需要加速度。应对这种需求,一般驱动器设计会做三次样条插值(CubicSpline Interpolation),控制端需要发送PVT信息,没有加速度信息。...
  • 6轴机器人直线插补运动

    万次阅读 2015-10-24 21:27:24
    距离上一篇博客已经好长一段时间了,已知在思考一般6轴的工业机器人的逆向运动算法,有参考很多论文,里面讲到了D-H参数,然后买各种书籍,下载各种电子书,研究逆向运动算法。 慢慢地耳语目染,对D-H参数也熟悉起来...
  • 直线插补算法易于编程实现,且能使机器人手臂的运动轨迹以较高的精度逼近预定义轨迹。根据基于蓝牙通信的5DOF遥操作机器人的实际要求,采用绝对坐标值法计算各关节电机所需脉冲个数,并且使用MC9S12XDP512单片机的五个...
  • 基于S型曲线的连续多段曲线插补平滑过渡的规划算法(Matlab) 写在前面 前面的博客已经写了关于空间直线与空间圆弧的常用插补算法,而这些都是单一路径,实际中并不实用。对于连续多段路径,传统方法是将多段路径...
  • 实践三次样条插补 实践三次样条插补 以前有做过moveit控制舵机机械臂的测试,记了些水文。当时觉得moveit规划的点有点少,运行时有抖动,觉得反正是玩玩,实际应用应该插值处理,于是没有深入。前不久有网友看了...
  • 该项目以视频的方式展示,内容包括基于Qt OpenGL和旋量理论的史陶比尔6R机器人3D建模和运动学仿真,包括正运动学、逆运动学,速度雅可比矩阵算法,轨迹的绘制,关节插补,直线插补,直线、三点圆、直径圆、文字的...
  • 这里机械臂上所用的步进电机是常用的42步进电机,每个电机控制一个关节,步进角度1.8度,全步模式下,200个脉冲走360度。这里我们在步进电机上面加了减速器,减速比是1:10,即转子真正走一圈是要2000个脉冲,每个...
  • 知乎话题:多轴插补为什么普遍使用梯形速度曲线? 链接:https://www.zhihu.com/people/yu-tian-qi-76/activities 1-ICRA2014上 Joonyoung Kim分享了Trajectory Planning for Robots: The Challenges of ...
  • %第七章题目,单连杆机械手关节静止位置为θ0=-5.该机械手从静止位置开始在4s内平滑转动到θf=80°停止位置。试进行下列计算: %1)计算完成此运动并使机械臂停在目标点的三次曲线的系数 %2)计算带抛物线过渡的线型...
  • 通过ROS控制真实机械臂(7)---三次样条插补

    千次阅读 热门讨论 2019-08-05 13:40:48
    下图所示是用五次多项式插补的一条路径,可以看到速度加速都是平滑的,按照这样的插补运算,确实可以完成插补的任务,但是由于加速度曲线完全拟合,在实际的控制中表现成加速度抖动问题,也就是传说中的”龙格现象...
  • 对于MOVEJ的关节运动来说,我们只关心每个电机的角度(只需要考虑多个电机协同开始运动和结束运动,关键是对每个电机加速度均一化,从而一起跑一起停,这部分内容可以参考机器人学导论以获取更加详细的说明),我们...
  • 1.引言及本文简介 在上两篇博客里,Jungle介绍了Qt键盘...在之前的一篇博客(Qt设计机器人仿真控制器)Jungle结合Qt和Coin3D设计实现了机器人仿真控制器,鼠标拖拽控制器界面6个轴的滑条,分别控制机器人6个关节...

空空如也

空空如也

1 2 3 4 5 ... 16
收藏数 314
精华内容 125
关键字:

关节插补