精华内容
下载资源
问答
  • 2020-03-10 18:05:49

    %圆弧插补函数,输入三个坐标,以及插补点个数,得到一组坐标
    function [trajx,trajy,trajz]=Cir_interpol(N,pos1,pos2,pos3)
    x1=pos1(1);y1=pos1(2);z1=pos1(3);
    x2=pos2(1);y2=pos2(2);z2=pos2(3);
    x3=pos3(1);y3=pos3(2);z3=pos3(3);
    A1 = y1z2 - y1z3 -z1y2 + z1y3 + y2z3 - y3z2;
    B1 = -x1z2 + x1z3 + z1x2 - z1x3 - x2z3 + x3z2;
    C1 = x1y2 - x1y3 - y1x2 + y1x3 + x2y3 - x3y2;
    D1 = -x1y2z3 + x1y3z2 + x2y1z3 - x3y1z2 -x2y3z1 + x3y2z1;
    % fplane = @(x,y,z) A1x + B1y +C1z +D1; %三点确定的面的方程
    A2 = 2
    (x2-x1);
    B2 = 2*(y2-y1);
    C2 = 2*(z2-z1);
    D2 = x12+y12+z1^2 - x22-y22-z2^2;
    A3 = 2*(x3-x1);
    B3 = 2*(y3-y1);
    C3 = 2*(z3-z1);
    D3 = x12+y12+z1^2 - x32-y32-z3^2;
    T = [A1 B1 C1;A2 B2 C2;A3 B3 C3];
    D = [D1 D2 D3]’;
    r0 = -inv(T)D ;%圆心坐标向量
    R = sqrt((pos1(1)-r0(1))2+(pos1(2)-r0(2))2+(pos1(3)-r0(3))^2);%半径
    %step2:得到13两点间的间距d
    d13=sqrt((pos1(1)-pos3(1))2+(pos1(2)-pos3(2))2+(pos1(3)-pos3(3))^2);
    o13=(pos1+pos3)/2;%得到13两点的中点,当弧角180度时,圆心在该点,以该点为衡量,判定优劣弧
    %step3:得到13中点与2点间距do2
    do2=sqrt((o13(1)-pos2(1))2+(o13(2)-pos2(2))2+(o13(3)-pos2(3))^2);
    %ste4:判断优弧还是劣弧
    if(do2<d13/2)
    theta=2
    asin(d13/2/R);
    else
    theta=2pi-2asin(d13/2/R);
    end
    %step5:寻找该平面的法向量n,并将其单位化,将n1向量绕其旋转得到ni向量,加上圆心坐标即为圆弧插补点坐标
    n1=pos3-r0’;
    n2=pos1-r0’;
    n=cross(n1,n2);
    len=sqrt(n(1)2+n(2)2+n(3)^2);
    n=n/len;%得到单位法向量
    u=n(1);
    v=n(2);
    w=n(3);
    %插值个数为30个
    xx(N)=0;
    yy(N)=0;
    zz(N)=0;
    %计算每一个插值点坐标,向量绕旋转轴n的旋转矩阵T
    for c=1:N
    tmp=transT((c-1)/(N-1)*theta,u,v,w)*n2’;
    xx©=tmp(1);
    yy©=tmp(2);
    zz©=tmp(3);
    end
    trajx(N)=0;
    trajy(N)=0;
    trajz(N)=0;
    for t=1:1:N;
    trajx(t)=xx(t)+r0(1);
    trajy(t)=yy(t)+r0(2);
    trajz(t)=zz(t)+r0(3);
    end
    end

    更多相关内容
  • 圆弧生成算法;只画1/8圆其余点通过对称关系求得;考虑中心在原点半径为R 的第二个8分圆 构造判别式圆方程;1若 d, 则取P1 为下一象素而且再下一象素的判别式为 2若d>=0, 则应取P2 为下一象素而且下一象 素的判别式为 ...
  • 运动控制器圆弧插补算法研究。 介绍了关于运动控制器圆弧插补算法研究的详细说明,提供控制理论工程的技术资料的下载。
  • 运动控制器圆弧插补算法研究pdf,运动控制器圆弧插补算法研究
  • 利用相交多边形代替常用的内接多边形进行圆弧插补计算,给出了周期插补各坐标分量计算的递推公式。算法简便先进,能够提高插补精度与数控加工的效率。
  • 插补算法是数控技术的核心,最常见的算法是逐点比较法,主要是针对直线插补和圆弧插补.直线插补较为直观简单,而圆弧插补由于决定当前应该进给方向的因素很多,因而在程序编写时,需要用许多条件判断语句才能实现,程序...
  • 研究一种基于双极坐标的并联机械臂的圆弧插补算法。该算法采用逐点比较法原理,通过转动两并联机械臂转角的方式实现圆弧插补,重点给出转角转动的方向和条件,并给出了递推公式。分析方法和公式采用了双极坐标的方式。
  • 针对传统的逐点比较法圆弧插补时,机床进给速度不够快,数控...与逐点比较法圆弧插补算法相比,该插补算法使数控系统插补次数减少20%以上,进给速度提高20%以上;使插补误差小于0.5个脉冲当量,插补精度提高一倍。
  • 逐点比较法圆弧插补算法.doc
  • 写在前面 机械臂用的是五自由度的,我测试时发现逆解精度存在...常规插补算法 假设机器人末端由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.

    展开全文
  • 基于抛物线过渡(梯形加减速)的空间直线插补算法与空间圆弧插补算法(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.

    展开全文
  • 三菱PLC插补教程 QD75定位模块 LD75定位模块 圆弧插补 直线插补 画各种图形 频教程+程序案例+手册资料 教程共13集 第1集 Q系列与L系列 程序互相转换教程 第2集 G2000 G2004 G2006 参数用程序设置方式 LD75 QD75 教程...
  • 圆弧插补算法 前瞻算法 ...

                                                                        圆弧插补算法


                                                                              前瞻算法


                                                                              插补算法

    展开全文
  • 路径规划算法之直线圆弧插补算法
  • 逐点比较法直线和圆弧插补算法及实现

    万次阅读 多人点赞 2018-12-13 17:57:44
    一年前就用逐点比较法做过直线和圆弧插补,当时没做笔记,现在忘光了,这次好好的整理了一番。:) 如果你看到了这里,相信你已经对逐点比较法有所了解了,缺的是整个插补过程中需要用到的公式而已。 补充一点:...
  • 喜欢三菱PLC学习的朋友可以看看
  • 在CNC机床的G代码中,最常见的有G0、G1、G2、G3代码,分别表示...因此需要通过特定的圆弧插补算法来控制步进电机运动,圆弧插补算法比较多,常用的有逐点比较法、最小偏差法和数字积分法等等,本文使用的是逐点比较法
  • 教程共13集 视频+程序+手册 第1集 Q系列与L系列 程序互相转换教程 第2集 G2000 G2004 G2006 参数用程序设置方式 LD75 QD75 教程 第3集 三菱PLC程序 M代码使用教程 ...第13集 画个小鸭子 直线插补 圆弧插补 合并讲解例子
  • 圆弧插补MATLAB程序

    2019-05-24 16:39:46
    空间圆弧/平面圆弧插补,输入3个点的位置坐标(3个不在一条直线上的点确定一条直线)确定出相应的圆弧。
  • 插补算法——圆弧插补.pdf

    千次阅读 2021-04-22 15:02:37
    17-6-16 上午10:16 H:\Matlab Do...\demo.m 第 1 页,共 1 页%% 初始化clear,clcclose allwarning offfeature jit off%% 主程序% R代表所插补的圆的半径R=10;%ThetaAround代表所要插补的角度范围ThetaAround=[135,90...
  • 直线与圆弧插补计算

    2018-09-26 09:32:22
    直线与圆弧插补计算,介绍各种插补的计算方式和原理,设计多种插补原理
  • 在CNC机床的G代码中,最常见的有G0、G1、G2、G3代码,分别表示直线和圆弧插补,直线插补对于...因此需要通过特定的圆弧插补算法来控制步进电机运动,圆弧插补算法比较多,常用的有逐点比较法、最小偏差法和数字积分法等
  • Arduino的两轴插补算法实现,不调用已有的插补库,直接实现算法。含算法说明文件
  • 可进行圆弧插补,输入起止坐标圆弧半径顺逆时针以及步长就可以自动插补
  • 源文件:https://pan.baidu.com/s/17FQKqn3UaEPQHkmTcOXKOg提取码:atb2#include #include #include #include //运行时坐标结构体typedef ...//直线的逐点比较法的直线插补的坐标的结构体typedef struct{float ...
  • 中国 ·包头 职 大 学 报 2012年第 4期圆弧跨象限代数运算插补算法的C程序实现李艳玲(潍坊职业学院,山东省 潍坊市 261031)摘 要:文章论述了代数运算圆弧插补算法过程和判别方式,给出了圆弧四个象限的插补递推公式...
  • 原理讲解:https://blog.csdn.net/z893345329/article/details/25922857 c++模拟直线插补和圆弧插补(共5个实例)
  • mc_arc函数的原理就是将圆弧插补成足够多的小线段。arc_tolerance为圆弧上任意两点连接而成的小线段到圆弧的最大距离,该值是人为设定的。arc_tolerance越小,则圆弧微分成的小线段越多,插补精度也越高。由上图...
  • 三菱FX系列PLC直线圆弧插补SFC梯形图程序。用逐点比较法做的直线圆弧插补程序,1,2,3,4象限直线插补。圆弧插补:1,2,3,4,单象限顺逆圆弧,跨1象限,跨2象限,跨3象限,跨4象限顺逆圆弧,顺逆正圆等程序。自己动手...

空空如也

空空如也

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

圆弧插补算法

友情链接: leds-adp5520.rar