精华内容
下载资源
问答
  • 用Matlab实现直线插补计算程序讲解学习
    2021-04-20 07:29:29

    精品文档

    姓名:学号:班级:

    用Matlab实现直线插补计算程序

    clear;F=0;x=0;y=0;dx=0.5;dy=0.6;xe=-5;ye=-6;K=ye/xe;xx(1)=x;yy(1)=y; index=2;(1)while(F>=0) if(xe>0) if x=x+dx;else

    x=x-dx;end y=y; F=abs(xe*y)-abs(ye*x);else

    (ye>0)if y=y+dy;else y=y-dy;end

    x=x; F=abs(xe*y)-abs(ye*x);end(abs(F)<=0.01)if F=0;end xx(index)=x; yy(index)=y; index=index+1;

    ((abs(x)+abs(y))>=(abs(xe)+abs(ye))) if;break end end(xe>0)if xxx=0:0.01:xe;else xxx=0:-0.001:xe;end yyy=K*xxx;(xe>0)if);'b'

    plot(xx,yy,'g>',xxx,yyy,else

    plot(xx,yy,'g-

    grid on;

    精品文档.

    精品文档

    b329f0db4f18587eecd7f031a79eda26.png

    -1-2轴-3y----0.-1.---3.--4.---2.5轴x

    1

    图表

    所示。时,结果如图表当dx=0.5;dy=0.6;xe=-5;ye=-6;1精品文档.

    更多相关内容
  • 基于抛物线过渡(梯形加减速)的空间直线插补算法与空间圆弧插补算法(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.

    展开全文
  • 直线插补MATLAB程序

    2019-05-24 16:35:13
    平面/空间直线插补;适用在机器人或其他领域,代码测试可能或多或少需要优化
  • 写在前面 机械臂用的是五自由度的,我测试时发现逆解精度存在一些问题,...假设机器人末端由P1点沿直线运动到P2点,(x1,y1,z1)\left(x_{1}, y_{1}, z_{1}\right)(x1​,y1​,z1​)和(α(1,β1,γ1)\left(\alpha(_{...

    写在前面

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

    Matlab机器人工具箱版本9.10

    机械臂用的是五自由度的,我测试时发现逆解精度存在一些问题,目前还没找到是求解析解时出错还是编程过程有问题,还是算法本身考虑不周到,欢迎有研究过的大神们批评指正!感谢~

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

    常规插补算法

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

    各个插值点位置坐标向量和RPY角度向量可以表示为:
    { x = x 1 + λ Δ x y = y 1 + λ Δ y z = z 1 + λ Δ z ( 1 ) \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.(1) x=x1+λΔxy=y1+λΔyz=z1+λΔz1

    { α = α 1 + λ Δ α β = β 1 + λ Δ β γ = γ 1 + λ Δ γ ( 2 ) \left\{\begin{array}{l}{\alpha=\alpha_{1}+\lambda \Delta \alpha} \\ {\beta=\beta_{1}+\lambda \Delta \beta} \\ {\gamma=\gamma_{1}+\lambda \Delta \gamma}\end{array}\right.(2) α=α1+λΔαβ=β1+λΔβγ=γ1+λΔγ2

    其中, ( x , y , z ) (x, y, z) (x,y,z) ( α , β , γ ) (\alpha, \beta, \gamma) (α,β,γ)表示插值点的位置坐标向量和RPY角度向量, λ \lambda λ为归一化因子,采用抛物线过渡的线性函数(即梯形加减速)以保证整段轨迹上的位移和速度都连续。 ( Δ x , Δ y , Δ z ) (\Delta x, \Delta y, \Delta z) (Δx,Δy,Δz) ( Δ α , Δ β , Δ γ ) (\Delta \alpha, \Delta \beta, \Delta \gamma) (Δα,Δβ,Δγ)分别表示首末位置间的位置和RPY角度的增量,其求解为:
    { Δ x = x 2 − x 1 Δ y = y 2 − y 1 Δ z = z 2 − z 1 Δ α = α 2 − α 1 Δ β = β 2 − β 1 Δ γ = γ 2 − γ 1 ( 3 ) \left\{\begin{array}{l}{\Delta x=x_{2}-x_{1}} \\ {\Delta y=y_{2}-y_{1}} \\ {\Delta z=z_{2}-z_{1}} \\ {\Delta \alpha=\alpha_{2}-\alpha_{1}} \\ {\Delta \beta=\beta_{2}-\beta_{1}} \\ {\Delta \gamma=\gamma_{2}-\gamma_{1}}\end{array}\right.(3) Δx=x2x1Δy=y2y1Δz=z2z1Δα=α2α1Δβ=β2β1Δγ=γ2γ13
    所以问题被转变为对归一化因子 λ \lambda λ的求解,求解方法后面会讲。下面先了解记录两种常见的插补算法:直线插补和空间圆弧插补。

    直线插补算法

    一般最简单的做法为,已知空间直线的起点S位置坐标为 ( x 1 , y 1 , z 1 ) (x_{1},y_{1}, z_{1}) (x1,y1,z1),终点D位置坐标为 ( x 2 , y 2 , z 2 ) (x_{2}, y_{2}, z_{2}) (x2,y2,z2)和插补次数N,则有
    { Δ x = ( x 2 − x 1 ) / ( N + 1 ) Δ y = ( y 2 − y 1 ) / ( N + 1 ) Δ z = ( z 2 − z 1 ) / ( N + 1 ) ( 4 ) \left\{\begin{array}{l}{\Delta x=\left(x_{2}-x_{1}\right) /(N+1)} \\ {\Delta y=\left(y_{2}-y_{1}\right) /(N+1)} \\ {\Delta z=\left(z_{2}-z_{1}\right) /(N+1)}\end{array}\right.(4) Δx=(x2x1)/(N+1)Δy=(y2y1)/(N+1)Δz=(z2z1)/(N+1)4
    对于该直线上任一点i(1<= i <= N)有
    { x i = x 1 + Δ x ⋅ i y i = y 1 + Δ y ⋅ i z i = z 1 + Δ z ⋅ i ( 5 ) \left\{\begin{array}{l}{x_{i}=x_{1}+\Delta x \cdot i} \\ {y_{i}=y_{1}+\Delta y \cdot i} \\ {z_{i}=z_{1}+\Delta z \cdot i}\end{array}\right.(5) xi=x1+Δxiyi=y1+Δyizi=z1+Δzi5
    这种方法很简单,对于机械臂的运动而言只需要加上RPY角的变化即可,此处不详述。

    基于抛物线过渡(梯形加减速)的空间直线插补的核心在于归一化参数 λ \lambda λ的计算,同时加入速度规划的物理条件和约束。因此得到的直线插补算法需要初始化的参数有机械臂末端线速度 v s v_{s} vs、加速度 a a a_{a} aa、减速度 a d a_{d} ad,需要计算得到的插值参数有总位移S和插值点数N。

    对于空间直线插补而言,总位移计算公式如下:
    S e = ( x 2 − x 1 ) 2 + ( y 2 − y 1 ) 2 + ( z 2 − z 1 ) 2 ( 6 ) S_{e}=\sqrt{\left(x_{2}-x_{1}\right)^{2}+\left(y_{2}-y_{1}\right)^{2}+\left(z_{2}-z_{1}\right)^{2}}(6) Se=(x2x1)2+(y2y1)2+(z2z1)2 6
    插值点数的计算公式如下:
    N = P n S e V s ( 7 ) \mathrm{N}=P_{n} \frac{S_{e}}{V_{s}}(7) N=PnVsSe7
    其中, P n P_{n} Pn为插值参数,可以根据情况进行调整。从公式(7)可以看出,插值点数N和总位移 S e S_{e} Se成正比,与机械臂末端的线速度 v s v_{s} vs成反比。也就是说,如果总位移越大,线速度越小,则插值点数应该越多。原因在于,如果位移越大,在速度不变的情况下,一定要保证更多的插值点才能对规划曲线进行有效的拟合。同样的,如果位移不变,为了得到大的运动速度,则要降低插值点数,因为插值过程是一个等时间间隔的过程,插值点数越多,意味着所需时间越长。

    空间圆弧插补算法

    1.圆心和半径

    在这里插入图片描述

    如图所示,{O-XYZ}为机器人基座坐标系。P1、P2、P3为空间中任意不共线的三个点,其中P1 ( x 1 , y 1 , z 1 ) (x_{1}, y_{1}, z_{1}) (x1,y1,z1)、P2 ( x 2 , y 2 , z 3 ) (x_{2}, y_{2}, z_{3}) (x2,y2,z3)、P3 ( x 3 , y 3 , z 3 ) (x_{3}, y_{3}, z_{3}) (x3,y3,z3)为三点在基座标系中的坐标。那么这三个点可以唯一确定一个外接圆,该园所在平面 M 1 M_{1} M1的方程如下:
    ∣ x − x 3 y − y 3 z − z 3 x 1 − x 3 y 1 − y 3 z 1 − z 3 x 2 − x 3 y 2 − y 3 z 2 − z 3 ∣ = 0 ( 8 ) \left|\begin{array}{ccc}{x-x_{3}} & {y-y_{3}} & {z-z_{3}} \\ {x_{1}-x_{3}} & {y_{1}-y_{3}} & {z_{1}-z_{3}} \\ {x_{2}-x_{3}} & {y_{2}-y_{3}} & {z_{2}-z_{3}}\end{array}\right|=0 (8) xx3x1x3x2x3yy3y1y3y2y3zz3z1z3z2z3=08
    该外接圆方程的一般形式如下:
    A 1 x + B 1 y + C 1 z + D 1 = 0 ( 9 ) A_{1} x+B_{1} y+C_{1} z+D_{1}=0(9) A1x+B1y+C1z+D1=09
    其中,

    A 1 = ( y 1 − y 3 ) ( z 2 − z 3 ) − ( y 2 − y 3 ) ( z 1 − z 3 ) A_{1} = (y_{1} - y_{3})(z_{2} - z_{3}) - (y_{2} - y_{3})(z_{1} - z_{3}) A1=(y1y3)(z2z3)(y2y3)(z1z3)

    B 1 = ( x 2 − x 3 ) ( z 1 − z 3 ) − ( x 1 − x 3 ) ( z 2 − z 3 ) B_{1} = (x_{2}-x_{3})(z_{1} - z_{3}) - (x_{1} - x_{3})(z_{2} - z_{3}) B1=(x2x3)(z1z3)(x1x3)(z2z3)

    C 1 = ( x 1 − x 3 ) ( y 2 − y 3 ) − ( x 2 − x 3 ) ( y 1 − y 3 ) C_{1} = (x_{1} - x_{3})(y_{2} - y_{3}) - (x_{2} - x_{3})(y_{1} - y_{3}) C1=(x1x3)(y2y3)(x2x3)(y1y3)

    D 1 = − ( A 1 ⋅ x 3 + B 1 ⋅ y 3 + C 1 ⋅ z 3 ) D_{1} = -(A_{1}\cdot x_{3} + B_{1} \cdot y_{3} + C_{1} \cdot z_{3}) D1=(A1x3+B1y3+C1z3)

    P1P2的垂直平分面M2的方程如下:
    A 2 x + B 2 y + C 2 z + D 2 = 0 ( 10 ) A_{2} x+B_{2} y+C_{2} z+D_{2}=0(10) A2x+B2y+C2z+D2=010
    其中,

    A 2 = x 2 − x 1 A_{2} = x_{2} - x_{1} A2=x2x1

    B 2 = y 2 − y 1 B_{2} = y_{2} - y_{1} B2=y2y1

    C 2 = z 2 − z 1 C_{2} = z_{2} - z_{1} C2=z2z1

    D 2 = [ ( x 2 2 − x 1 2 ) + ( y 2 2 − y 1 2 ) + ( z 2 2 − z 1 2 ) ] / 2 D_{2} = \left[\left(x_{2}^{2}-x_{1}^{2}\right)+\left(y_{2}^{2}-y_{1}^{2}\right)+\left(z_{2}^{2}-z_{1}^{2}\right)\right] /2 D2=[(x22x12)+(y22y12)+(z22z12)]/2

    P2P3的垂直平分面M3的方程如下:
    A 3 x + B 3 y + C 3 z + D 3 = 0 ( 11 ) A_{3} x+B_{3} y+C_{3} z+D_{3}=0(11) A3x+B3y+C3z+D3=011
    其中,

    A 3 = x 3 − x 2 A_{3} = x_{3} - x_{2} A3=x3x2

    B 3 = y 3 − y 2 B_{3} = y_{3} - y_{2} B3=y3y2

    C 3 = z 3 − z 2 C_{3} = z_{3} - z_{2} C3=z3z2

    D 3 = − [ ( x 3 2 − x 2 2 ) + ( y 3 2 − y 2 2 ) + ( z 3 2 − z 2 2 ) ] / 2 D_{3} = -\left[\left(x_{3}^{2}-x_{2}^{2}\right)+\left(y_{3}^{2}-y_{2}^{2}\right)+\left(z_{3}^{2}-z_{2}^{2}\right)\right] /2 D3=[(x32x22)+(y32y22)+(z32z22)]/2

    综上,联立M1、M2、M3三个平面的方程,可得到如下算式:
    ∣ A 1 B 1 C 1 A 2 B 2 C 2 A 3 B 3 C 3 ∣ ∣ x y z ∣ = ∣ − D 1 − D 2 − D 3 ∣ ( 12 ) \left|\begin{array}{lll}{A_{1}} & {B_{1}} & {C_{1}} \\ {A_{2}} & {B_{2}} & {C_{2}} \\ {A_{3}} & {B_{3}} & {C_{3}}\end{array}\right|\left|\begin{array}{l}{x} \\ {y} \\ {z}\end{array}\right|=\left|\begin{array}{c}{-D_{1}} \\ {-D_{2}} \\ {-D_{3}}\end{array}\right|(12) A1A2A3B1B2B3C1C2C3xyz=D1D2D312
    公式(12)有且仅有唯一一组解,可解得三点外接圆圆心P0的位置坐标为 ( x 0 , y 0 , z 0 ) (x_{0}, y_{0}, z_{0}) (x0,y0,z0),根据圆心坐标可求得外接圆半径为
    r = ( x 1 − x 0 ) 2 + ( y 1 − y 0 ) 2 + ( z 1 − z 0 ) 2 ( 13 ) r=\sqrt{\left(x_{1}-x_{0}\right)^{2}+\left(y_{1}-y_{0}\right)^{2}+\left(z_{1}-z_{0}\right)^{2}}(13) r=(x1x0)2+(y1y0)2+(z1z0)2 13

    2.坐标变换

    在以圆心P0为原点的基础上建立新的坐标系{P0-X0Y0Z0}。新坐标系中的Z0为平面M1的法矢量,从上文可得Z0轴的方向余弦为:
    a x = A 1 / L , a y = B 1 / L , a z = C 1 / L ( 14 ) a_{x}=A_{1} / L, \quad a_{y}=B_{1} / L, \quad a_{z}=C_{1} / L(14) ax=A1/L,ay=B1/Laz=C1/L14
    其中, L = ( A 1 2 + B 1 2 + C 1 2 ) 1 / 2 L=\left(A_{1}^{2}+B_{1}^{2}+C_{1}^{2}\right)^{1 / 2} L=(A12+B12+C12)1/2

    此处,规定新坐标系{P0-X0Y0Z0}的X0为P0P1的矢量方向,故可得X0轴的方向余弦如下:
    n i = ( i 1 − i 0 ) ( x 1 − x 0 ) 2 + ( y 1 − y 0 ) 2 + ( z 1 − z 0 ) 2 ( 15 ) n_{i}=\frac{\left(i_{1}-i_{0}\right)}{\sqrt{\left(x_{1}-x_{0}\right)^{2}+\left(y_{1}-y_{0}\right)^{2}+\left(z_{1}-z_{0}\right)^{2}}}(15) ni=(x1x0)2+(y1y0)2+(z1z0)2 (i1i0)15
    其中,i= x, y, z。

    确定Z0和X0的方向后,根据右手定则即可确定Y0的方向,如下:
    Y 1 = X 1 × Z 1 ( 16 ) \boldsymbol{Y}_{1}=\boldsymbol{X}_{1} \times \boldsymbol{Z}_{1}(16) Y1=X1×Z116
    在得到新坐标系三个轴的方向余弦后,根据旋转矩阵的定义,可得到新坐标系{P0-X0Y0Z0}相对于基座标系{O-XYZ}的齐次变换矩阵
    T = [ X 1 Y 1 Z 1 P 1 0 0 0 1 ] ( 17 ) T=\left[\begin{array}{cccc}{X_{1}} & {Y_{1}} & {Z_{1}} & {P_{1}} \\ {0} & {0} & {0} & {1}\end{array}\right](17) T=[X10Y10Z10P11]17
    因此,可通过齐次变换矩阵T将基座标系内的空间圆弧变换到新坐标系内的平面圆弧,简化求解过程。

    3.转换为平面圆弧问题

    根据齐次变换矩阵,P1、P2、P3在新坐标系下的坐标Q1、Q2、Q3可通过下面公式计算得到:
    ∣ Q i , 1 ∣ T = T − 1 ∣ P i , 1 ∣ T ( i = 1 , 2 , 3 ) ( 18 ) \left|Q_{i}, 1\right|^{\mathrm{T}}=T^{-1}\left|P_{i}, 1\right|^{\mathrm{T}}(i=1,2,3)(18) Qi,1T=T1Pi,1T(i=1,2,3)18
    在平面内进行圆弧插补,实际上就是对圆心角进行插补,因此核心就是求出圆弧的圆心角,同时还需要注意顺逆时针的问题。

    4.圆心角和插值点数N

    由于atan2函数返回的是原点至点(x,y)的方位角,即与 x 轴的夹角。也可以理解为复数 x+yi 的辐角。返回值的单位为弧度。在数学坐标系中,结果为正表示从 X 轴逆时针旋转的角度,结果为负表示从 X 轴顺时针旋转的角度。此时为了判断圆弧是顺时针还是逆时针,需要将角度值转换成0~2 π \pi π的范围。

    假设 ∠ P 1 P 0 P 2 \angle P_{1} P_{0} P_{2} P1P0P2 P 0 P 2 P_{0}P_{2} P0P2 P 0 X 0 P_{0}X_{0} P0X0正向的夹角,有
    ∠ P 1 P 0 P 2 = atan ⁡ 2 ( P 2 y , P 2 x ) + n ∗ 2 π ( 19 ) \angle P_{1} P_{0} P_{2}=\operatorname{atan} 2\left(P_{2 y}, P_{2 x}\right)+n * 2 \pi(19) P1P0P2=atan2(P2y,P2x)+n2π19
    其中 P 2 y 、 P 2 x P_{2y}、P_{2x} P2yP2x是P2在新坐标系下的坐标值。此时存在如下两种情况:

    1. atan ⁡ 2 ( P 2 y , P 2 x ) < 0 \operatorname{atan} 2\left(P_{2 y}, P_{2 x}\right)<0 atan2(P2y,P2x)<0时, n = 1 n=1 n=1
    2. 否则, n = 0 n=0 n=0

    同理,假设 ∠ P 1 P 0 P 3 \angle P_{1} P_{0}P_{3} P1P0P3 P 0 P 3 P_{0}P_{3} P0P3 P 0 X 0 P_{0}X_{0} P0X0正向的夹角,有
    ∠ P 1 P 0 P 3 = atan ⁡ 2 ( P 3 y , P 3 x ) + n ∗ 2 π ( 20 ) \angle P_{1} P_{0} P_{3}=\operatorname{atan} 2\left(P_{3 y}, P_{3 x}\right)+n * 2 \pi(20) P1P0P3=atan2(P3y,P3x)+n2π20
    其中 P 3 y 、 P 3 x P_{3y}、P_{3x} P3yP3x是P2在新坐标系下的坐标值。此时存在如下两种情况:

    1. atan ⁡ 2 ( P 3 y , P 3 x ) < 0 \operatorname{atan} 2\left(P_{3 y}, P_{3 x}\right)<0 atan2(P3y,P3x)<0时, n = 1 n=1 n=1
    2. 否则, n = 0 n=0 n=0

    通过比较 ∠ P 1 P 0 P 2 \angle P_{1} P_{0} P_{2} P1P0P2 ∠ P 1 P 0 P 3 \angle P_{1} P_{0} P_{3} P1P0P3的大小可以确定圆弧的方向,同时可以求出圆弧圆心角 Ω \Omega Ω

    1. ∠ P 1 P 0 P 2 < ∠ P 1 P 0 P 3 \angle P_{1} P_{0} P_{2}<\angle P_{1} P_{0} P_{3} P1P0P2<P1P0P3时,圆弧为逆时针,圆心角 Ω = ∠ P 1 P 0 P 3 \Omega=\angle P_{1} P_{0} P_{3} Ω=P1P0P3;
    2. ∠ P 1 P 0 P 2 ≥ ∠ P 1 P 0 P 3 \angle P_{1} P_{0} P_{2} \geq \angle P_{1} P_{0} P_{3} P1P0P2P1P0P3时, Ω = 2 π − ∠ P 1 P 0 P 3 \Omega=2\pi - \angle P_{1} P_{0} P_{3} Ω=2πP1P0P3;

    对应基于抛物线过渡(梯形加减速)的空间圆弧插补而言,需要初始化的运动参数有机械臂末端角速度 ω s \omega_{s} ωs和角加速度a,需要计算的插值参数为圆心角 Ω \Omega Ω和插值点数N。上文已经求得圆心角,插值点数计算公式如下:
    N = P n Ω ω s ( 21 ) \mathrm{N}=P_{n} \frac{\Omega}{\omega_{s}}(21) N=PnωsΩ21

    归一化因子 λ \lambda λ的求解

    预备知识

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

    设机械臂在匀速运动时的线速度为 v s v_{s} vs,抛物线段的加减速度均为 a a a,可以计算出加速阶段和减速阶段的时间和位移为
    T 1 = V s a ( 22 ) T_{1}=\frac{V_{s}}{a}(22) T1=aVs22

    S 1 = 1 2 a T 1 2 ( 23 ) S_{1}=\frac{1}{2} a T_{1}^{2}(23) S1=21aT1223

    设机械臂末端运动的总位移为 S e S_{e} Se,总时间为:
    T e = 2 T 1 + s e − 2 S 1 V s ( 24 ) T_{e}=2 T_{1}+\frac{s_{e}-2 S_{1}}{V_{s}}(24) Te=2T1+Vsse2S124
    将位移、速度和加速度进行归一化处理得到上述计算量的归一化参数:
    S 1 λ = s 1 s e ( 25 ) S_{1 \lambda}=\frac{s_{1}}{s_{e}}(25) S1λ=ses125

    T 1 λ = T 1 T e ( 26 ) T_{1 \lambda}=\frac{T_{1}}{T_{e}}(26) T1λ=TeT126

    T 2 λ = 1 − T 1 λ ( 27 ) T_{2 \lambda}=1-T_{1 \lambda}(27) T2λ=1T1λ27

    a λ = 2 s 1 λ T 1 λ 2 ( 28 ) a_{\lambda}=\frac{2 s_{1 \lambda}}{T_{1 \lambda}^{2}}(28) aλ=T1λ22s1λ28

    结合梯形加减速的位移公式,可以推导出归一化因子 λ \lambda λ的计算公式如下:
    λ = { 1 2 a λ t 2 ( 0 ≤ t ≤ T 1 λ ) 1 2 a λ T 1 λ 2 + a λ T 1 λ ( t − T 1 λ ) ( T 1 λ < t ≤ T 2 λ ) 1 2 a λ T 1 λ 2 + a λ T 1 λ ( t − T 1 λ ) − 1 2 a λ ( t − T 2 λ ) 2 ( T 2 λ < t ≤ 1 ) ( 29 ) \lambda=\left\{\begin{array}{cc}{\frac{1}{2} a_{\lambda} t^{2}} & {\left(0 \leq t \leq T_{1 \lambda}\right)} \\ {\frac{1}{2} a_{\lambda} T_{1 \lambda}^{2}+a_{\lambda} T_{1 \lambda}\left(t-T_{1 \lambda}\right)} & {\left(T_{1 \lambda}<t \leq T_{2 \lambda}\right)} \\ {\frac{1}{2} a_{\lambda} T_{1 \lambda}^{2}+a_{\lambda} T_{1 \lambda}\left(t-T_{1 \lambda}\right)-\frac{1}{2} a_{\lambda}\left(t-T_{2 \lambda}\right)^{2}} & {\left(T_{2 \lambda}<t \leq 1\right)}\end{array}\right.(29) λ=21aλt221aλT1λ2+aλT1λ(tT1λ)21aλT1λ2+aλT1λ(tT1λ)21aλ(tT2λ)2(0tT1λ)(T1λ<tT2λ)(T2λ<t1)29
    式中,t表示时间, t = i / N ( 0 < = t < = 1 ) t=i/N(0<=t<=1) t=i/N(0<=t<=1),其中N表示总的插值点数,i = 1、2、3、……、N-1、N。每个插值点的时间值,都有一个 λ \lambda λ与之对应,结合公式(1)(2)(3)可以得到各个插值点位置坐标向量和RPY角度向量,最后就是解逆运动学方程,得到每个插值点对应的关节变量,生成轨迹。

    Matlab实现代码

    预备知识:

    五自由度机械臂正逆运动学算法(C语言+Matlab)

    模型是我买的五自由度机械臂,之前已经推导过逆运动学的解析解,这里直接拿来用。

    先是预备程序:

    • 高斯列主元消去法,解圆心方程;
    • 五自由度逆运动学解析解推导(前面博客有);
    • 归一化因子推导。
    %% 高斯列主元消去法
    function x = Gauss_lie(n, A, b)
    for k = 1: n-1
        a_max = abs(A(k, k));
        cnt = k;
        for i = k: n
            if (abs(A(i, k)) > a_max)
                a_max = abs(A(i, k));
                cnt = i; % 确定下标i
            end
        end
        if (A(cnt, k) == 0)
            fprintf('Gauss_lie: no unique solution\n');
            return;
        end
        % 换行
        if (cnt ~= k)
            t = 0; s = 0;
            for j = k: n
                t = A(k, j);
                A(k, j) = A(cnt, j);
                A(cnt, j) = t;
                s = b(cnt);
                b(cnt) = b(k);
                b(k) = s;
            end
        end
        % step 5
        for i = k+1: n
            L(i) = A(i, k) / A(k, k);
            for j = k+1: n
                A(i, j) = A(i, j) - L(i)*A(k, j);
            end
            b(i) = b(i) - L(i)*b(k);
        end
    end
    for i = 1: n
        if (A(i, i) == 0)
            fprintf('Gauss_lie no unique solution\n');
            return;
        end
    end
    % 回代
    x(n) = b(n) / A(n, n);
    for i = n-1: -1: 1
        sum_a = 0;
        for j = i+1: n
            sum_a = sum_a + A(i, j)*x(j);
        end
        x(i) = (b(i) - sum_a) / A(i, i);
    end
    
    end 
    
    % 归一化处理
    % 梯形加减速曲线
    % 输入参数:机械臂末端运动总位移(角度)pos 
    %          机械臂末端线速度(角速度)vel
    %          加速度减速度accl(设定加减速段accl相同)
    %          插值点数N
    function lambda = Normalization(pos, vel, accl, N)
    % 加减速段的时间和位移
    T1 = vel / accl;
    S1 = (1/2) * accl * T1^2;
    % 总时间
    Te = 2*T1 + (pos - 2*S1) / vel;
    % 归一化处理
    S1_ = S1 / pos;
    T1_ = T1 / Te;
    T2_ = 1 - T1_;
    accl_ = 2*S1_ / T1_^2;
    % lambda求解公式
    for i = 0: N
        t = i / N;
        if (t >= 0 && t <= T1_)
            lambda(i+1) = (1/2) * accl_ * t^2;
        elseif (t > T1_ && t <= T2_)
            lambda(i+1) = (1/2)*accl_*T1_^2 + accl_*T1_*(t - T1_);
        elseif (t > T2_ && t <= 1)
            lambda(i+1) = (1/2)*accl_*T1_^2 + accl_*T1_*(T2_ - T1_) + (1/2)*accl_*power(t - T2_, 2);
        end
    end
                
    end
    

    核心程序:

    % 空间直线位置插补与RPY角姿态插补 + 梯形加减速归一化处理
    % 参数:起点S位置, 终点D位置, 末端线速度vs, 加减速度a
    %      起点S的RPY角、终点D的RPY角
    % 返回值:插值点(不包括起点S和终点D)
    function [x y z alp beta gama N] = SpaceLine(S, D, S_, D_, vs, a)
    x1 = S(1); y1 = S(2); z1 = S(3);
    x2 = D(1); y2 = D(2); z2 = D(3);
    alp1 = S_(1); beta1 = S_(2); gama1 = S_(3);
    alp2 = D_(1); beta2 = D_(2); gama2 = 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)
    delta_x = x2 - x1;
    delta_y = y2 - y1;
    delta_z = z2 - z1;
    delta_alp = alp2 - alp1;
    delta_beta = beta2 - beta1;
    delta_gama = gama2 - gama1;
    for i = 1: N+1
        x(i) = x1 + delta_x*lambda(i);
        y(i) = y1 + delta_y*lambda(i);
        z(i) = z1 + delta_z*lambda(i);
        alp(i) = alp1 + delta_alp*lambda(i);
        beta(i) = beta1 + delta_beta*lambda(i);
        gama(i) = gama1 + delta_gama*lambda(i);
    end
    end
    
    % 空间圆弧位置插补与RPY角姿态插补 + 梯形加减速归一化处理
    % 参数: 起点S位置和RPY角, 中间点M位置, 终点D位置和RPY角,末端角速度,角加减速度
    % 方便起见,角速度和角加速度均为角度制
    function [x y z alp beta gama N] = SpaceCircle(S, M, D, S_, D_, ws, a)
    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);
    alp1 = S_(1); beta1 = S_(2); gama1 = S_(3);
    alp2 = D_(1); beta2 = D_(2); gama2 = 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 = 2; %插补参数,增加插值点数,避免过小
    ws = ws*pi / 180; % 角度换成弧度
    a = a*pi / 180;
    N = P*theta / ws;
    % 求归一化参数
    lambda = Normalization(theta, ws, a, N);
    
    % 插补原理: 在新平面上进行插补(简化)
    % 在新坐标系下z1_,z2_,z3_均为0,即外接圆在新坐标系的XOY平面内
    % 此时转化为平面圆弧插补问题
    delta_ang = theta;
    delta_alp = alp2 - alp1
    delta_beta = beta2 - beta1;
    delta_gama = gama2 - gama1;
    for i = 1: N+1
        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);
        alp(i) = alp1 + delta_alp*lambda(i);
        beta(i) = beta1 + delta_beta*lambda(i);
        gama(i) = gama1 + delta_gama*lambda(i);
    end
    % % figure(1)
    % % plot(x_, y_)
    % 插补原理: 在原圆弧上进行插补
    % 圆弧上任一点处沿前进方向的切向量
    % x(1) = x1; y(1) = y1; z(1) = z1;
    % for i = 1: N+1
    %     m(i) = flag*(ay*(z(i) - z0) - az*(y(i) - y0));
    %     n(i) = flag*(az*(x(i) - x0) - ax*(z(i) - z0));
    %     l(i) = flag*(ax*(y(i) - y0) - ay*(x(i) - x0));
    %     delta_s = delta_ang * r;
    %     E = delta_s / (r*sqrt(ax^2 + ay^2 + az^2));
    %     G = r / sqrt(r^2 + delta_s^2);
    %     x(i+1) = x0 + G*(x(i) + E*m(i) - x0);
    %     y(i+1) = y0 + G*(y(i) + E*n(i) - y0);
    %     z(i+1) = z0 + G*(z(i) + E*l(i) - z0);
    % end 
    
    end
    

    测试主程序

    % Standard DH
    % five_dof robot
    % 在关节4和关节5之间增加一个虚拟关节,便于逆运动学计算
    clear;
    clc;
    th(1) = 0; d(1) = 0; a(1) = 0; alp(1) = pi/2;
    th(2) = 0; d(2) = 0; a(2) = 1.04;alp(2) = 0;
    th(3) = 0; d(3) = 0; a(3) = 0.96; alp(3) = 0;
    th(4) = 0; d(4) = 0; a(4) = 0; alp(4) = 0;
    th(5) = pi/2; d(5) = 0; a(5) = 0; alp(5) = pi/2;
    th(6) = 0; d(6) = 0; a(6) = 0; alp(6) = 0;
    th(7) = 0; d(7) = 1.63; a(7) = 0.28; alp(7) = 0;
    % DH parameters  th     d    a    alpha  sigma
    L1 = Link([th(1), d(1), a(1), alp(1), 0]);
    L2 = Link([th(2), d(2), a(2), alp(2), 0]);
    L3 = Link([th(3), d(3), a(3), alp(3), 0]);
    L4 = Link([th(4), d(4), a(4), alp(4), 0]);
    L5 = Link([th(5), d(5), a(5), alp(5), 0]); 
    L6 = Link([th(6), d(6), a(6), alp(6), 0]);
    L7 = Link([th(7), d(7), a(7), alp(7), 0]);
    robot = SerialLink([L1, L2, L3, L4, L5, L6, L7]); 
    robot.name='MyRobot-5-dof';
    robot.display() 
    
    % 起点关节角[0 90 30 60 90 0 0]*pi/180
    % 终点关节角[0 90 60 60 90 0 0]*pi/180
    theta_S = [0 120 100 40 90 0 0]*pi/180;
    theta_M = [60 100 100 30 90 0 0]*pi/180;
    theta_D = [100 120 100 40 90 0 0]*pi/180;
    % robot.teach();
    % robot.plot(theta_D); 
    % hold on
    T_S = robot.fkine(theta_S)    %起点末端执行器位姿
    T_M = robot.fkine(theta_M)
    T_D = robot.fkine(theta_D)
    % ik_T = five_dof_ikine(T_D)
    [S_RPY(1) S_RPY(2) S_RPY(3)] = RPY_angle(T_S); % 起点对应的RPY角
    [D_RPY(1) D_RPY(2) D_RPY(3)] = RPY_angle(T_D); % 终点对应的RPY角
    S = T_S(1: 3, 4); % 起点对应的位置坐标
    M = T_M(1: 3, 4);
    D = T_D(1: 3, 4); % 终点对应的位置坐标
    vs = 0.06; a = 0.02; % 直线插补速度参数
    ws = 5; a = 2.5; % 空间圆弧插补速度参数
    % [x y z alp beta gama N] = SpaceLine(S, D, S_RPY, D_RPY, vs, a); % 直线插补,得到插值点(不包括起点和终点)
    [x y z alp beta gama N] = SpaceCircle(S', M', D', S_RPY, D_RPY, ws, a);
    plot3(x, y, z)
    hold on
    th1(1) = theta_S(1); th2(1) = theta_S(2); th3(1) = theta_S(3);
    th4(1) = theta_S(4); th5(1) = theta_S(5); th6(1) = theta_S(6); th7(1) = theta_S(7);
    % t = [0 0 0 0 0];
    T = {1, N};
    for i = 1: N+1
        R = RPY_rot(alp(i), beta(i), gama(i));
        R(1, 1:3);
        R(2, 1:3);
        R(3, 1:3);
        T{i} = [R(1, 1:3), x(i);
                R(2, 1:3), y(i);
                R(3, 1:3), z(i);
                0 0 0 1];
         theta = five_dof_ikine(T{i});
         th = theta(2, 1:5); % 简单取1行逆解
         th1(i+1) = th(1);
         th2(i+1) = th(2);
         th3(i+1) = th(3);
         th4(i+1) = th(4);
         th5(i+1) = pi/2;
         th6(i+1) = th(5);
         th7(i+1) = 0;
    end
    for i = 1: N+1
        T_ = robot.fkine([th1(i), th2(i), th3(i), th4(i), th5(i), th6(i), th7(i)]);
        traj = T_(1: 3, 4);
        a(i) = traj(1);
        b(i) = traj(2);
        c(i) = traj(3);
        plot3(traj(1), traj(2), traj(3), 'r*');
        hold on
        robot.plot([th1(i), th2(i), th3(i), th4(i), th5(i), th6(i), th7(i)])
    end
    

    参考文献

    [1]杨淞. 一种六自由度机械臂的运动控制系统设计[D].上海交通大学,2014.

    [2]陈国良,黄心汉,王敏.机械手圆周运动的轨迹规划与实现[J].华中科技大学学报(自然科学版),2005(11):69-72.

    [3]卓扬娃,白晓灿,陈永明.机器人的三种规则曲线插补算法[J].装备制造技术,2009(11):27-29.

    [4]林威,江五讲.工业机器人笛卡尔空间轨迹规划[J].机械工程与自动化,2014(05):141-143.

    [5]Chen Z, Wang H, Lu X, et al. Kinematics analysis and application of 5-DOF manipulator with special joint[C]//2017 Chinese Automation Congress (CAC). IEEE, 2017: 7421-7426.

    展开全文
  • 六轴机器人轨迹规划之空间直线插补

    万次阅读 多人点赞 2018-07-26 14:32:10
    直线插补时,指定起止坐标与速度。 确定要插直线的周期增量,分解到xyz轴。 2.matlab代码 clear; clc; p0=[1,2,3]; pf=[2,4,5]; %指定起止位置 v=0.1; %指定速度 x=[p0(1)];y=[p0(2)];z=[p0(3)]; L=((pf(1)-p0(1)...

    1.原理简述
    直线插补时,指定起止坐标与速度。
    确定要插直线的周期增量,分解到xyz轴。
    2.matlab代码

    clear;
    clc;
    p0=[1,2,3];
    pf=[2,4,5];         %指定起止位置
    v=0.1;              %指定速度
    x=[p0(1)];y=[p0(2)];z=[p0(3)];
    L=((pf(1)-p0(1))^2+(pf(2)-p0(2))^2+(pf(3)-p0(3))^2)^0.5;%直线长度
    N=L/v;              %插补次数
    dx=(pf(1)-p0(1))/N; %每个周期各轴增量
    dy=(pf(2)-p0(2))/N;
    dz=(pf(3)-p0(3))/N;
    for t=1:1:N         %插补
    x(t+1)=x(t)+dx;
    y(t+1)=y(t)+dy;
    z(t+1)=z(t)+dz;
    end
    plot3(x,y,z,'r'),xlabel('x'),ylabel('y'),zlabel('z'),hold on,plot3(x,y,z,'o','color','g'),grid on;

    3.运行结果
    这里写图片描述

    展开全文
  • 直线插补程序

    2013-04-19 10:55:13
    利用逐点比较法实现直线插补运算,对初学者有用
  • 本帖最后由 CK345 于 2016-6-24 17:16 编辑 X0 = input('请输入起点横坐标 X\n X0 = '); Y0 = input('请输入... %延时程序形参为每走一步所用时间 end xlabel('X') ylabel('Y') title(['四象限直线插补']) hold off;
  • 运动规划——空间圆弧和直线插补及姿态平滑

    万次阅读 多人点赞 2019-01-31 10:44:20
    线性插值(Lerp/Linear Interpolation),即沿着一条直线(也就是圆上的一个弦)进行插值,此种插值方式所得结果并非单位四元数(只有单位四元数才能表示旋转)。 正规化线性插值 正规化线性插值(Normalized ...
  • 直线插补应用

    2015-09-15 22:28:48
    用于PLC程序的运算,常用的一些编写程序的方法。
  • 工业机械臂直线插补相关记录

    千次阅读 2020-04-02 15:12:03
    在笛卡尔空间机械臂的末端轨迹规划中,就直线插补来说,我们实验室试过了几种姿态规划方法,最早使用的是等效轴角坐标系表示法,主要原理如下(摘自机器人学导论): 实验室机械臂的直线插补思路大致是这样的...
  • 直线插补算法

    千次阅读 2021-05-20 12:15:59
    直线插补算法,就是刀具或绘笔每走一步都要和给定的数据进行比对,看该点在次点的上方或者是下方,从而决定下一步该怎么走。即机床数控系统依照一定方法确定刀具运动轨迹的过程。也可以说,已知曲线上的某些数据,...
  • 《用Matlab实现直线插补计算程序》由会员分享,可在线阅读,更多相关《用Matlab实现直线插补计算程序(5页珍藏版)》请在人人文库网上搜索。1、姓名: 学号: 班级: 用Matlab实现直线插补计算程序clear;F=0;x=0;y=0;...
  • 今天抽空把二月份宅家学习的空间插补算法进行整理,这里要感谢CSDN中的一些大咖们,有些算法是完全借鉴,有些算法是在之基础上发挥综合的,在这里先一并感谢!切入正题! 四元数基础 注意这里使用单位四元数来进行...
  • ECI3808系列控制卡支持最多达12轴直线插补、任意圆弧插补、空间圆弧、螺旋插补、电子凸轮、电子齿轮、同步跟随、虚拟轴、机械手指令等;采用优化的网络通讯协议可以实现实时的运动控制。 ECI3808系列运动控制卡支持...
  • 数字积分法第二象限直线插补程序设计数字积分法是利用数字积分的方法,计算刀具沿各坐标轴的位移,使得刀具沿着所加工的轮廓曲线运动利用数字 积分原 理构成的插 补装置 称为数字积分 器,又称数 字微分 分析器...
  • 直线插补和圆弧插补的区别

    万次阅读 多人点赞 2019-08-17 10:03:31
    插补(Interpolation),即机床数控系统依照一定方法确定刀具运动轨迹的过程。也可以说,已知曲线上的某些数据,按照某种算法计算已知点之间的中间点...直线插补就是数控系统,在起点和终点之间按照直线来密化点群,...
  • * 简单的空间插补程序:目前只更新空间直线插补和空间圆弧插补程序 * 传统插补方法+梯形加减速归一化处理 * 归一化因子采用抛物线过渡的线性函数(梯形加减速), * 以保证整段轨迹上的位移和速度都...
  • 为了提高Stewart型并联机床运动学精度,文章研究并联机床末端运动轨迹的直线插补步长和非线性插补误差的关系。提出了采用曲率法度量非线性运动误差和插补步长之间的关系,基于以上方法,建立了度量并联机床非线性...
  • 采用空间等长小直线段首尾连接来拟合空间焊缝轨迹曲线,在保证误差的条件下进行离散点的选取,从而完成焊枪的轨迹跟踪运动,实现空间曲线焊缝的焊接。最后通过MATLAB软件的仿真验证了该插补算法的正确性和实效性。

空空如也

空空如也

1 2 3 4 5 ... 20
收藏数 828
精华内容 331
关键字:

空间直线插补