精华内容
下载资源
问答
  • 雅各比矩阵
    千次阅读
    2020-04-02 18:27:19

    前端扫描匹配

    LOAM系统通过提取激光点云中的线特征和面特征,匹配前后两帧点云的线面特征,从而恢复运动估计进行航位推算。所以观测方程变成了最小化点到线/面的距离之和的函数:

    算法步骤:
    在这里插入图片描述

    1. Pk+1点云根据位姿内插转换到tk+1时刻坐标系下
    2. tk+1时刻坐标系下,和上一帧点云进行匹配对准,获得位姿估计结果
    3. 再将Pk+1点云根据刚刚估计的位姿转换到tk+2时刻坐标系下,准备和下一时刻的点云进行匹配

    注意:LOAM中定义的载体坐标系是X向左、Y向上、Z向前,旋转矩阵的旋转顺序是YXZ,转角顺时针为正。

    R = R z R x R y R = R_{z}^{}R_{x}^{}R_{y}^{} R=RzRxRy
    去除点云畸变,把运动过程中的点云投影到开始时刻:
    X ~ L = R − 1 ( X L − T ) \tilde X_{}^L = R_{}^{{\rm{ - }}1}\left( {X_{}^L - {T_{}}} \right) X~L=R1(XLT)
    把运动过程中的点云统一投影到结束时刻:
    X ~ L = R X L + T \tilde X_{}^L = RX_{}^L{\rm{ + }}{T_{}} X~L=RXL+T

    matlab公式推导代码

    clc;clear;
    %% 线特征点
    % % 计算af/ap'
    syms P0 P1 P2 P1P0 P2P0 P2P1 B  P A B
    syms x0 x1 x2 y0 y1 y2 z0 z1 z2 m11 m22 m33 d 
    P0=[x0;y0;z0];
    P1=[x1;y1;z1];
    P2=[x2;y2;z2];
    P1P0=P0-P1;
    P2P0=P0-P2;
    P2P1=P1-P2;
    cross(P1P0,P2P0)
    d=norm(cross(P1P0,P2P0))/norm(P2P1)
     
    la=diff(d,x0);
    lb=diff(d,y0);
    lc=diff(d,z0);
    
    %% 面特征点
    % 计算af/ap'
    % syms P0 P1 P2 P3 P1P0 P2P1 P3P1 x0 y0 z0 x1 x2 x3 y1 y2 y3 z1 z2 z3
    % syms tmp
    % P1P0=[x0-x1;y0-y1;z0-z1];
    % P2P1=[x1-x2;y1-y2;z1-z2];
    % P3P1=[x1-x3;y1-y3;z1-z3];
    % tmp=cross(P2P1,P3P1)
    % d=norm(dot(P1P0,tmp))/norm(tmp)
    % pa=diff(d,x0)
    % pb=diff(d,y0)
    % pc=diff(d,z0)
    
    %% 计算ap'/aX
    syms x y z xx yy zz s X tx ty tz thetax thetay thetaz
    syms rx ry rz C1 C2 C3 T
    %X=[thetax thetay thetaz tx ty tz]';
    X=[rx ry rz tx ty tz]';
    % rx=s*thetax;
    % ry=s*thetay;
    % rz=s*thetaz;
    rx=thetax;
    ry=thetay;
    rz=thetaz;
    p=[x y z]';
    %T=s*[tx ty tz]'
    T=[tx ty tz]';
    %%里程计部分***************************************
    % C1=[cos(rz) sin(rz) 0; -sin(rz) cos(rz) 0; 0 0 1];
    % C2=[1 0 0; 0 cos(rx) sin(rx); 0 -sin(rx) cos(rx)];
    % C3=[cos(ry) 0 -sin(ry); 0 1 0; sin(ry) 0 cos(ry)];
    % P=C3*C2*C1*(p-T);
    %%里程计部分***************************************
    % %%图优化部分***************************************
    C1=[cos(rz) -sin(rz) 0; sin(rz) cos(rz) 0; 0 0 1];
    C2=[1 0 0; 0 cos(rx) -sin(rx); 0 sin(rx) cos(rx)];
    C3=[cos(ry) 0 sin(ry); 0 1 0; -sin(ry) 0 cos(ry)];
    P=C3*C2*C1*p+T;
    % %%图优化部分***************************************
    xx=P(1,1);
    yy=P(2,1);
    zz=P(3,1);
    arx1=diff(xx,thetax);
    arx2=diff(yy,thetax);
    arx3=diff(zz,thetax);
    ary1=diff(xx,thetay);
    ary2=diff(yy,thetay);
    ary3=diff(zz,thetay);
    arz1=diff(xx,thetaz);
    arz2=diff(yy,thetaz);
    arz3=diff(zz,thetaz);
    atx1=diff(xx,tx);
    atx2=diff(yy,tx);
    atx3=diff(zz,tx);
    aty1=diff(xx,ty);
    aty2=diff(yy,ty);
    aty3=diff(zz,ty);
    atz1=diff(xx,tz);
    atz2=diff(yy,tz);
    atz3=diff(zz,tz);
    
    %% 链式法则
    syms la lb lc arx ary arz atx aty atz
    J=[la lb lc]*[arx1 ary1 arz1 atx1 aty1 atz1;arx2 ary2 arz2 atx2 aty2 atz2;arx3 ary3 arz3 atx3 aty3 atz3];
    arx=J(1,1)
    ary=J(1,2)
    arz=J(1,3)
    atx=J(1,4)
    aty=J(1,5)
    atz=J(1,6)
    
    更多相关内容
  • 数值分析常用几种方法,三湾矩阵雅各比迭代法等算法
  • 雅各比矩阵

    2015-08-30 10:22:40
    3自由度球铰并联机构的运动学优化设计,外文
  • 梯度下降法,常被用来求解算法的最佳参数,举个多元线性回归问题的例子来进行解释雅各比矩阵的应用

    梯度下降法,常被用来求解算法的最佳参数(例如求解神经网络中的权重和偏置),举个多元线性回归问题的例子,来解释一下梯度下降法的原理

    预测城市的平均房价y与该城市的GDP(x1),人口数(x2),交通便利程度(x3)等有关,X的权重矩阵用W来表示。

    其中求解预测的最佳参数W的过程可以用梯度下降法进行参数的最优化。

    如果用最小二乘法来计算误差J=(XW-Y)^2 ,令J对W偏导数=0,求解误差J最小时W的值,解如下,这种情况下有局限性的前提是X为可逆矩阵(也就是说X不含有多重共线性)。

     

    而梯度下降法则是一种更广泛的求解权重W的方法,很多资料中讲误差J比作是一个山谷,在山顶向山谷里丢一个人进去,其下降到无法下降的时候我们也就找到了山谷(也就是误差)的最低点,

    在最低点的梯度(导数值)也就为0。将人不断下降的过程看作是按照一定横向的步长a(也叫做学习率)迭代的过程,为了方便后续求解我们在误差J前面加一个1/2m的常数,由于最后梯度会变为0,就会停止迭代了。

    那么其中的梯度到底是什么?

    梯度其实叫雅各比矩阵(Jacobian matrix) ,实际上就是多变量微分的一般化,在多变量函数中,梯度是一个向量,可以理解成多元函数在某一点的各个方向导数所构成的一个矩阵,它体现了一个可微方程与给出点的最优线性逼近。

    如何应用梯度下降法求解多元线性回归问题呢?

    把初始问题中的权重矩阵进行设置,其中gradient_descen这个函数是求解雅各比矩阵。

    import numpy as np
     
    # 数据的个数
    m = 5
     
    
    X0 = np.ones((m, 1))
    X1 = np.array([
        90,89,80, 100,100,140,100,100,120, 100,120, 150, 112,150,200
    ]).reshape(m, 3)
    X = np.hstack((X0, X1))
    #房价的三个特征
    
    y = np.array([
        171,236,217,256,328
    ]).reshape(m, 1)
     
    #学习的步长 学习率
    alpha = 0.000001
    #误差函数
    def error_function(theta, X, y):
        diff = np.dot(X, theta) - y
        return (1./2*m) * np.dot(np.transpose(diff), diff)
    #偏导数函数 雅各比矩阵
    def gradient_function(theta, X, y):
        diff = np.dot(X, theta) - y
        return (1./m) * np.dot(np.transpose(X), diff)
    #梯度下降函数求解权重的函数
    def gradient_descent(X, y, alpha):
        new_inter=0
        theta = np.array([1,1, 1,1]).reshape(4, 1)
        gradient = gradient_function(theta, X, y)
        while (np.all(np.absolute(gradient) >= 1e-5) ):
            #print(theta)
            theta = theta - alpha * gradient
            new_inter=new_inter+1
            gradient = gradient_function(theta, X, y)
        return theta
    optimal = gradient_descent(X, y, alpha)
    print('optimal:', optimal)
    print('error function:', error_function(optimal, X, y)[0,0])

     

    b和三个权重的权重分别为,0.99 0.44 0.65 0.89 误差项在10^1的数量级,在使用的时候应该注意调整学习率的参数,防止过拟合或无法完成迭代,会报出现缺失值的error

    展开全文
  • 包含线速度v和角速度w 图像特征速度向量和相机速度向量,存在下面关系: 矩阵L被称作交互矩阵或图像的雅各比矩阵,严格来说,交互矩阵并不是图像雅各比矩阵,因为相机速度ξ 并不是位姿参数的偏导 :表示相机坐标在...

    如何将图像像素速度转变到相机速度?

    导入

    在基于图像的视觉伺服中,我们可以通过相机观察到图像在相机中的位置发生变化 ,针对于每个像素来说呢?便是图像中像素发生了位置变化,通过求导我们可以将位置的变化转化成像素的速度变化。那有没有一种东西可以将像素的速度变化转变到相机坐标系下呢?答案是有的。它就是图像的雅各比矩阵或者严格意义上来说是交互矩阵。那交互矩阵又是怎么将像素速度的变化与相机速度的变化联系到一起的呢?下面想通过推导来说明一下这个问题。

    符号定义

    在这里插入图片描述
    根据相机成像模型,我们做如下定义:

    话外:像素是什么呢?通过百度和wiki我们可以看到定义:

    像素,为影像显示的基本单位,译自英文“pixel”,每个这样的消息元素不是一个点或者一个方块,而是一个抽象的取样。照片是一个个取样点的集合,在影像没有经过不正确的/有损的压缩或相机镜头合适的前提下,单位面积内的像素越多代表分辨率越高,所显示的影像就会接近于真实物体。

    相机所说的像素,其实是最大像素的意思,像素是分辨率的单位,这个像素值仅仅是相机所支持的有效最大分辨率。30万 640×480=307,200像素 我懵了。

    像素坐标系 u,v:位于像素平面上,其原点位于图像左上角,单位为像素
    图像坐标系 x,y:位于成像平面上,原点在在成像平面的中心点附近,单位为物理单位(如 mm)。
    相机坐标系 X,Y,Z:原点为光心,单位为物理单位(如 mm)。
    世界坐标系 :实际三维空间中的坐标系,根据使用需要进行定义,是用于表示系统中绝对位置的坐标系,单位为物理单位(如 mm)。

    为了表达图像中被测量的特征(单一点特征):
    在这里插入图片描述:图像中可以被测量的图像特征值向量
    在这里插入图片描述:图像中被测量的图像特征速度向量

    如果是单一的图像点,我们将有
    在这里插入图片描述
    图像特征速度和相机速度线性相关,为此定义相机速度:
    在这里插入图片描述:代表相机速度向量,包含线速度v和角速度w

    在这里插入图片描述
    图像特征速度向量和相机速度向量,存在下面关系:
    在这里插入图片描述
    矩阵L被称作交互矩阵或图像的雅各比矩阵,严格来说,交互矩阵并不是图像雅各比矩阵,因为相机速度ξ 并不是位姿参数的偏导


    :表示相机坐标在时间t时刻相对于固定坐标系o的旋转量

    :
    :表示相机坐标在时间t时刻相对于固定坐标系o的位置变化

    在这里插入图片描述:空间中一个固定点

    在这里插入图片描述
    :对应空间中的点p投影到图像中的特征向量

    接下来我们的目标便是通过一系列代数推导获得相机速度ξ 与图像特征点(空间中的点投影到图像平面点的偏导)速度˙s的交互矩阵L。最终得出˙s = Lξ。

    固定点相对于移动相机的速度

    我们定义
    在这里插入图片描述
    作为点P相对于世界坐标系o的坐标。记住P并不会随时间变化,因此它相对于世界坐标系是固定的。

    如果我们定义
    在这里插入图片描述
    作为在t时刻,P相对于移动相机坐标系的坐标。

    结合之前的定义,便有
    在这里插入图片描述
    因此,在t时刻,P相对于移动相机的坐标便可以表示为
    在这里插入图片描述
    现在我们对这个公式求导
    在这里插入图片描述
    其中
    在这里插入图片描述
    从移动坐标系到固定坐标点P的向量。表达了相对于固定坐标系的坐标,因此

    在这里插入图片描述
    从移动坐标系到固定坐标点P的向量。表达了相对于移动坐标系的坐标。

    利用角速度与旋转矩阵偏导的关系,这里的S(ω(t))是斜对称矩阵,这一关系的推导,这里暂时不做说明。
    在这里插入图片描述

    我们可以转化公式为
    在这里插入图片描述
    其中变量ω代表了移动坐标系相对于固定坐标系的角速度。即ω = ω0,因此,说明一下这一块多了一个R^T
    在这里插入图片描述
    给出移动坐标系相对于移动坐标系的角速度。
    类似的
    在这里插入图片描述
    利用上面等式,我们可以立刻得到P相对于移动相机坐标系的速度。
    在这里插入图片描述
    很有趣的一个结论是,一个固定坐标点相对于移动坐标系的速度是一个移动相对于固定坐标点速度的-1倍。

    构建交互矩阵

    为了简化说明。定义P点坐标相对于相机坐标为
    在这里插入图片描述
    P相对于移动坐标系的速度是
    在这里插入图片描述
    定义角速度向量
    在这里插入图片描述
    进一步简化
    在这里插入图片描述
    使用上面的定义,我们可以将等式
    在这里插入图片描述
    转变成
    在这里插入图片描述
    利用向量叉乘,可以写成
    在这里插入图片描述

    假设图像投影可以通过透视投影被建模,
    在这里插入图片描述
    通过三角形相似的原理,我们可以表达空间中的点P的x和y坐标与图像平面的u,v和深度z的表达式为
    在这里插入图片描述
    其中λ为相机的焦距。

    带入到上一个等式中,我们可以得到
    在这里插入图片描述
    这些等式可以用来表达速度˙p 对于图像平面的u,v以及点深度z和相机角速度和线速度的关系。这里的目的是为了将图像平面速度( ˙u, ˙v)T(转置)到相机坐标的角速度和线速度联系到一起。下面我们要将( ˙u, ˙v)T 与上面的等式进行联系。

    对上面的投影等式求偏导,我们可以得到
    在这里插入图片描述
    然后将x,y,z的偏导公式带入到上式中,我们可以得到
    在这里插入图片描述
    采用同样的方法,我们可以得到
    在这里插入图片描述
    在这里插入图片描述
    将上面两个等式写到一起,我们便可以得到
    在这里插入图片描述
    通过这个矩阵等式,我们可以得到一个点的交互矩阵。它将相机的速度与点在图像平面的投影速度联系到了一起。注意这个交互矩阵是关于图像点坐标u,v以及点在相机坐标下的深度z。因此,这个等式可以简化为
    在这里插入图片描述
    到这里,我们已经将图像平面的像素速度与相机速度进行关联的交互矩阵进行了推导。

    展开全文
  • 六轴六自由度机械臂轨迹规划的matlab实现(基于速度雅各比矩阵方法)

    六轴六自由度机械臂轨迹跟踪的matlab实现(基于速度雅各比矩阵方法)

    对于六轴六自由度机械臂进行轨迹规划,并针对其设计滑模控制器,实现机械臂的末端轨迹跟踪。(完整代码链接见文章末尾)
    本文所用机械臂为innfos-Gluon-6L3,通过standard DH方法建模得到参数如下:
    在这里插入图片描述

    本文利用速度雅各比矩阵的方法来实现轨迹跟踪。这一方法的优点在于可以完全避免逆运动学求解,更加节省时间。

    1.轨迹跟踪的控制结构图设计

    在这里插入图片描述
    控制系统的输入,同样也是系统的期望输出,是机械臂的目标位姿 x d = ( x , y , z , α , β , γ ) x_d=(x,y,z,\alpha,\beta,\gamma) xd=(x,y,z,α,β,γ)。其刚好对应机械臂的六个自由度。前三者是机械臂末端坐标系相对于世界坐标系的位置,后三者是机械臂末端坐标系相对于世界坐标系的旋转角度。每一时刻的机械臂期望位姿都是已知的,它是通过轨迹规划得到的,这一点将在后文详细讲解。
    系统的控制器采用滑模控制器,其输入为位姿误差 e = x d − x e=x_d-x e=xdx,输出为角度增量 q ˙ \dot q q˙
    系统的被控对象是利用速度雅各比矩阵来建模的,其关系为 v = J ( q ) q ˙ v=J(q)\dot q v=J(q)q˙。其中 J ( q ) J(q) J(q)为速度雅各比矩阵。
    系统的输出为机械臂的实际末端位置 x = ∫ v d t x=\int vdt x=vdt

    2.系统的输入:轨迹规划

    为了使得机械臂的末端运动平滑,常对机械臂进行规划,对末端速度、加速度等进行一定约束。
    工业上常使用七段式S型曲线来进行轨迹规划,其示意图图如下:
    七段S型曲线
    其有七个等间距时间段,分别为加加速段、加速度恒定段、加减速段、匀速段、减减速段、加速度恒定段、减加速段。
    各段速度的计算表达式如下,其中 J J J为加加速度。
    七段S型曲线速度表达式
    式中的 v m a x v_{max} vmax等参数的计算推导如下:
    △ T = t i + 1 − t i = t f − t 0 7 L = 4 v m a x △ T v 2 = v 1 + a m a x △ T a m a x = 2 v 1 △ T v m a x = v 1 + v 2 = 2 v 1 + a m a x △ T = 4 v 1 L = 16 v 1 △ T → v 1 = L 16 △ T \bigtriangleup T=t_{i+1}-t_{i}=\frac{t_f-t_0}{7} \\ L=4v_{max} \bigtriangleup T \\ v_2=v_1+a_{max}\bigtriangleup T\\ a_{max}=\frac{2v_1}{\bigtriangleup T} \\ v_{max}=v_1+v_2=2v_1+a_{max}\bigtriangleup T=4v_1\\ L=16v_1 \bigtriangleup T \rightarrow v_1=\frac{L}{16\bigtriangleup T} T=ti+1ti=7tft0L=4vmaxTv2=v1+amaxTamax=T2v1vmax=v1+v2=2v1+amaxT=4v1L=16v1Tv1=16TL
    所以最终只需知道末端位移 L L L和所需时间 T T T,即可计算得到整个轨迹规划曲线。其matlab代码实现如下。
    其中的 x _ s x\_s x_s y _ s y\_s y_s z _ s z\_s z_s分别为每时的机械臂末端位置,即期望期望位姿 x _ d x\_d x_d的前三行。 x _ d x\_d x_d的后三行为末端位置的姿态信息,在本次仿真中我们默认机械臂的姿态始终为0,所以 x _ d x\_d x_d的后三行总是为0。

    %% 0.设定初始参数
    % xyz_start=[0,-120.02,533.96];       %轨迹起点(关节角为0)的末端坐标,单位mm;
    xyz_start=[0,-120.02,533.96];
    xyz_end=[-30,-45,385];     %轨迹终点的末端坐标;
    T=10;       %完成轨迹规划的时间;
    
    %% 1.轨迹规划
    L=sqrt((xyz_end(1)-xyz_start(1))^2+(xyz_end(2)-xyz_start(2))^2+(xyz_end(3)-xyz_start(3))^2);
    dt=T/7;        %每段的时间长度
    v1=L/(16*dt);   %第一次加速度拐点
    J=2*v1/(dt*dt); %加加速度
    amax=dt*J;      %最大加速度
    v2=v1+dt*amax;  %第二次加速度拐点
    vmax=v2+v1;     %第三次速度拐点
    
    t1 = 1*dt;
    t2 = 2*dt;
    t3 = 3*dt;
    t4 = 4*dt;
    t5 = 5*dt;
    t6 = 6*dt;
    t7 = 7*dt;
    
    t=0:0.1:T;
    
    vt1=1/2*J*t.^2.*(t>=0 & t<t1);
    vt2=(v1+amax*(t-t1)).*(t>=t1 & t<t2);
    vt3=(vmax-1/2*J*(t3-t).^2).*(t>=t2 & t<t3);
    vt4=vmax.*(t>=t3 & t<t4);
    vt5=(vmax-1/2*J*(t-t4).^2).*(t>=t4 & t<t5);
    vt6=(v2-amax*(t-t5)).*(t>=t5 & t<t6);
    vt7=(1/2*J*(t7-t).^2).*(t>=t6 & t<t7);
    
    vt=vt1+vt2+vt3+vt4+vt5+vt6+vt7;     %各时刻速度
    
    S=zeros(1,length(t));       %各时刻位移
    for i=2:length(t)
        S(i)=trapz(t(1:i),vt(1:i));
    end 
    
    %各时刻xyz的位移
    x_s=xyz_start(1)+(xyz_end(1)-xyz_start(1))/L*S;     
    y_s=xyz_start(2)+(xyz_end(2)-xyz_start(2))/L*S;
    z_s=xyz_start(3)+(xyz_end(3)-xyz_start(3))/L*S;
    
    %各时刻xyz轴的速度分量
    v_x=(xyz_end(1)-xyz_start(1))/L*vt;
    v_y=(xyz_end(2)-xyz_start(2))/L*vt;
    v_z=(xyz_end(3)-xyz_start(3))/L*vt;
    

    3.被控对象:速度雅各比矩阵

    速度雅各比矩阵方法的关系表达式如下:
    v = J ( q ) q ˙ v=J(q)\dot q v=J(q)q˙
    前者 v v v是末端执行器的速度,后者 q ˙ \dot q q˙是关节角速度。表达式的物理意义是:当关节角度发生一个微小的变化 △ q \bigtriangleup q q,末端执行器也会相应产生一个微小的位姿变化 △ x \bigtriangleup x x
    速度的雅各比矩阵的求解方法有多种,如1.向量积方法 2.微分法 等等…
    本文采用向量积的方法,求解方法如下。
    在这里插入图片描述

    J ( q ) = [ J v J w ] = [ J 1 J 2 J 3 J 4 J 5 J 6 ] J i = [ Z i − 1 × r i − 1 Z i − 1 ] = [ Z i − 1 × ( P n − P i − 1 ) Z i − 1 ] J(q)=\begin{bmatrix}J_v\\J_w\end{bmatrix}=\begin{bmatrix}J_1&J_2&J_3&J_4&J_5&J_6\end{bmatrix}\\ J_i=\begin{bmatrix}Z_{i-1} \times r_{i-1}\\Z_{i-1}\end{bmatrix}=\begin{bmatrix}Z_{i-1} \times (P_n-P_{i-1})\\Z_{i-1}\end{bmatrix} J(q)=[JvJw]=[J1J2J3J4J5J6]Ji=[Zi1×ri1Zi1]=[Zi1×(PnPi1)Zi1]
    本文采用的机械臂有6个关节角,因此其速度雅各比矩阵有6列,分别为 J i J_i Ji。本文的机械臂有6个自由度,因此对应的矩阵为6行。
    每个雅各比矩阵分量 J i J_i Ji的后三行为 { i − 1 } \{i-1\} {i1}坐标系相对于世界坐标系的 Z Z Z轴分量;分量 J i J_i Ji的前三行为 Z i − 1 Z_{i-1} Zi1 r i − 1 r_{i-1} ri1的差乘, r i − 1 r_{i-1} ri1是末端坐标系与 { i − 1 } \{i-1\} {i1}坐标系的相对位置在世界坐标系中的表示。
    这种方法求解只适用于standard DH方法建模的模型,若使用modify DH方法建模,则需对上式的下标做一定修改。
    此方法的matlab代码实现如下:

    function [ J ] = Jacob_cross_SDH( q )
    %JACOB_CROSS_SDH 函数摘要
    %   输入q0为逼近角,单位为弧度,矩阵大小1*6;
    %   输出J为速度雅各比矩阵,矩阵大小6*6%   说明:利用向量积的方法求解系统的雅各比矩阵,方法1和方法2任选一种
    %   说明:此求解方法基于SDH参数建模,若MDH方法建模,需进行一定的下标改动
    
    d=[105.03,0,0,75.66,80.09,44.36];
    a=[0,-174.42,-174.42,0,0,0];
    alp=[pi/2,0,0,pi/2,-pi/2,0];
    offset=[0,-pi/2,0,-pi/2,0,0];
    thd=q+offset;
    
    % 求各个关节间的变换矩阵
    T0=trotz(0)*transl(0,0,0)*trotx(0)*transl(0,0,0);
    T1=trotz(thd(1))*transl(0,0,d(1))*trotx(alp(1))*transl(a(1),0,0);
    T2=trotz(thd(2))*transl(0,0,d(2))*trotx(alp(2))*transl(a(2),0,0);
    T3=trotz(thd(3))*transl(0,0,d(3))*trotx(alp(3))*transl(a(3),0,0);
    T4=trotz(thd(4))*transl(0,0,d(4))*trotx(alp(4))*transl(a(4),0,0);
    T5=trotz(thd(5))*transl(0,0,d(5))*trotx(alp(5))*transl(a(5),0,0);
    T6=trotz(thd(6))*transl(0,0,d(6))*trotx(alp(6))*transl(a(6),0,0);
    
    % 求各个关节相对于惯性坐标系的变换矩阵
    T00 = T0;
    T01 = T1;
    T02 = T1*T2;
    T03 = T1*T2*T3;
    T04 = T1*T2*T3*T4;
    T05 = T1*T2*T3*T4*T5;
    T06 = T1*T2*T3*T4*T5*T6;
    
    % 求各个关节相对于末端坐标系的变换矩阵
    T06 = T1*T2*T3*T4*T5*T6;
    T16 = T2*T3*T4*T5*T6;
    T26 = T3*T4*T5*T6;
    T36 = T4*T5*T6;
    T46 = T5*T6;
    T56 = T6;
    
    % 提取各变换矩阵的旋转矩阵
    R00 = t2r(T00);
    R01 = t2r(T01);
    R02 = t2r(T02);
    R03 = t2r(T03);
    R04 = t2r(T04);
    R05 = t2r(T05);
    R06 = t2r(T06);
    
    % 取旋转矩阵第3列,即Z轴方向分量
    Z0 = R00(: , 3);
    Z1 = R01(: , 3);
    Z2 = R02(: , 3);
    Z3 = R03(: , 3);
    Z4 = R04(: , 3);
    Z5 = R05(: , 3);
    Z6 = R06(: , 3);
    
    %% Method.1
    % 求末端关节坐标系相对于前面各个坐标系的位置,即齐次变换矩阵的第四列
    % pi6为坐标系i和末端坐标系的相对位置在坐标系i下的表示
    P06 = T06(1:3, 4);
    P16 = T16(1:3, 4);
    P26 = T26(1:3, 4);
    P36 = T36(1:3, 4);
    P46 = T46(1:3, 4);
    P56 = T56(1:3, 4);
    P66 = [0; 0; 0];
    
    % 使用向量积求出雅可比矩阵
    % R0i为坐标系0到坐标系i的旋转矩阵
    % R0i*Pi6指坐标系i和末端坐标系的相对位置在0坐标系下的表示
    J1 = [cross(Z0, R00*P06); Z0];
    J2 = [cross(Z1, R01*P16); Z1];
    J3 = [cross(Z2, R02*P26); Z2];
    J4 = [cross(Z3, R03*P36); Z3];
    J5 = [cross(Z4, R04*P46); Z4];
    J6 = [cross(Z5, R05*P56); Z5];
    
    %% Method.2
    
    % % pi为坐标系i与世界坐标系0的相对位置
    % p0=transl(T00);
    % p1=transl(T01);
    % p2=transl(T02);
    % p3=transl(T03);
    % p4=transl(T04);
    % p5=transl(T05);
    % p6=transl(T06);
    % 
    % % p6-pi为i坐标系指向末端坐标系的向量
    % % p6-pi即为末端坐标系与i坐标系相对位置在世界坐标系中的表示
    % % Ji=[Jv;Jw]    对应六自由度的速度分量和旋转分量
    % J1 = [cross(Z0, p6-p0); Z0];
    % J2 = [cross(Z1, p6-p1); Z1];
    % J3 = [cross(Z2, p6-p2); Z2];
    % J4 = [cross(Z3, p6-p3); Z3];
    % J5 = [cross(Z4, p6-p4); Z4];
    % J6 = [cross(Z5, p6-p5); Z5];
    
    
    J = [J1, J2, J3, J4, J5, J6];
    
    end
    

    4.控制器:等速率趋近的滑模控制器

    控制器的输入为位姿误差 e = x d − x e=x_d-x e=xdx,输出为关节角的增量 q ˙ \dot q q˙,因此控制器满足关系:
    u = q ˙ = f _ S M C ( e ) u=\dot q=f\_SMC(e) u=q˙=f_SMC(e)
    为此需求设计滑模控制器 f _ S M C f\_SMC f_SMC
    设计滑模面:
    s = c e e = x d − x s=ce\\ e=x_d-x s=cee=xdx
    设计趋近率为等速趋近率:
    s ˙ = − ξ s g n s \dot s=-\xi sgns s˙=ξsgns
    推导得到控制器输出 u u u
    s ˙ = c e ˙ = − ξ s g n s e ˙ = x ˙ d − x ˙ = − 1 c ξ s g n s v = v d + 1 c ξ s g n s u = q ˙ = J − 1 ( q ) v = J − 1 ( v d + 1 c ξ s g n s ) \dot s=c\dot e=-\xi sgns\\ \dot e=\dot x_d-\dot x=-\frac{1}{c}\xi sgns\\ v=v_d+\frac{1}{c}\xi sgns\\ u=\dot q=J^{-1}(q)v=J^{-1}(v_d+\frac{1}{c}\xi sgns) s˙=ce˙=ξsgnse˙=x˙dx˙=c1ξsgnsv=vd+c1ξsgnsu=q˙=J1(q)v=J1(vd+c1ξsgns)
    matlab代码实现如下:

    dth = [0; 0; 0; 0; 0; 0];
    th = [0; 0; 0; 0; 0; 0];
    
    x=[xyz_start';0;0;0];        %其实时刻的位姿
    
    lamda=1;		%阻尼矩阵的系数
    
    k = 0.1;
    ita = 0.0002;
    c = 5;
    
    e = [0; 0; 0; 0; 0; 0];
    de = [0; 0; 0; 0; 0; 0];
    for i = 1 : length(t)
        xd=[x_s(i);y_s(i);z_s(i);0;0;0];     %期望位姿
        dxd=[v_x(i);v_y(i);v_z(i);0;0;0];	%期望速度
    
        q=th(:, i);
    
        Jac = Jacob_cross_SDH(q');	%求解当前角度下的雅可比矩阵
    
        e(:, i) = xd - x(:,i);      %误差
        s = c*e(:, i);      %滑模面
        v=dxd + (1/c)*ita*sign(s);    %机械臂的末端实际速度
        
        de(:, i) = dxd - v;     %误差的微分
        dth(:, i) = inv(Jac+lamda.*diag(ones(1,6)))*v;      %关节角的增量
        th(:, i + 1) = th(:, i) + dth(:, i)*0.1;    %下一时刻的关节角度
        x(:, i+1) = x(:, i) + v*0.1;    %机械臂末端实际位姿
    end
    

    更多更高级的滑模控制器设计请点击博主的控制器设计github仓库

    5.结果展示

    设定轨迹跟踪起始点和终点:
    [ 0 − 120.02 533.96 ] → [ − 30 − 45 385 ] \begin{bmatrix}0\\-120.02\\533.96\end{bmatrix} \rightarrow \begin{bmatrix}-30\\-45\\385\end{bmatrix} 0120.02533.963045385
    10 s 10s 10s内的末端轨迹位置在各坐标轴的映射:
    在这里插入图片描述
    10 s 10s 10s内系统跟踪末端轨迹位置的误差:
    在这里插入图片描述
    10 s 10s 10s内各个关节角的角度:
    在这里插入图片描述


    源代码下载链接:https://github.com/Fantasty9413/Trajectory-tracking-

    展开全文
  • 文章目录 《机器人动力学与控制》第五章——速度运动学之机械臂的雅各比矩阵 5.0 导言 参考文献 《机器人动力学与控制》第五章——速度运动学之机械臂的雅各比矩阵 5.0 导言 前面的章节我们推导了用来描述关节空间与...
  • 来回顾一下雅各比矩阵和黑塞矩阵: 雅各比矩阵是对多个 f f f 分别求多个维度的偏导,或者说 f f f 由 f 1 、 f 2 f_1、f_2 f1​、f2​ 等多个子函数构成。 例如,有3维向量 X ⃗ = ( x 1 , x 2 , x 3 ) \vec{X}=(x...
  • 机械手运动控制-雅各比矩阵程序,可以计算各个自由度的转动角度。
  • 这时这种表达方法就不合适了,因此在下一节我们需要用到反对称矩阵的知识帮我们解决三维情况下角速度线速度的表达。 参考引用 翻译自《robot dynamics and control》 作者:Mark W Spong,Seth Hutchinson, and M. ...
  • 一、雅各比 雅各比矩阵式多元形式的导数,假设有以下3个函数,每个函数有6个自变量: y 1 = f 1 ( x 1 , x 2 , x 3 ) y 2 = f 2 ( x 1 , x 2 , x 3 ) y 3 = f 3 ( x 1 , x 2 , x 3 ) \begin{aligned} &y_1 = f_1(x_1...
  • VINS 预积分 雅各比矩阵和协方差矩阵推导过程 文章目录VINS 预积分 雅各比矩阵和协方差矩阵推导过程根据中值积分旋转角度误差推导过程速度误差推导过程位置误差推导过程根据以上的计算结果整理成矩阵形式 根据中值...
  • 输入雅各比矩阵时分式使用\dfrac命令上下排列拥挤,不美观: 分式使用\frac命令又感觉太小: 解决方法是在每行后面添加一个垂直间隔\vspace{1.5ex},效果如图: \begin{equation*} DF(x) = \begin{bmatrix} \...
  • BA Jacobian 矩阵推导

    2019-03-01 10:39:48
    SALM 中BA 算法的 雅可比(Jacobian)矩阵推导 适用于所有SLAM新手,了解雅可比矩阵的原理作用。
  • 可以算出每次迭代的雅可比矩阵
  • 简单的理解就是:根据工作需求,我们必须在工作空间内,给机械手分配速度,而机械手的速度是由关节速度控制的,他们之间存在雅各比矩阵J的关系,某些位置会出现雅各比矩阵的行列式值为0,这样雅各比矩阵的逆不存在,...
  • 雅各比矩阵–像方点对物方点求导 像方点对物方点求导 即像方的x,y对物方点X,Y,Z求导(全微分,对X,Y,Z求全微分)
  • 5.3节我们会推导一些矩阵的性质,这些性质可以被用来计算坐标系之间相对速度转化关系,这些转化关系里面包括了旋转矩阵的推导。而通过介绍反对称矩阵可以简化其中的一些计算。 定义一. 当一个矩阵S有且仅有 ...
  • 雅可比矩阵matlab代码

    2017-03-19 11:54:01
    算法研究,matlab雅可比矩阵
  • syms r l f x=r*cos(l)*cos(f); y=r*cos(l)*sin(f); z=r*sin(l); J=jacobian([x;y;z],[r l f]) 结果: J = [ cos(l)*cos(f), -r*sin(l)*cos(f), -r*cos(l)...[ sin(l), r*cos(l), 0 ]
  • 1.雅可比矩阵 由一阶偏导数构成的矩阵,发明它的目的主要是为了简化求导公式。 假设有这样一个函数可以把n维的向量x映射为k维的向量y。,其中每个和每个都是相关的,也就是每个是单独从映射过来的函数,它的...
  • APM/PX4中的EKF2中有涉及磁偏角的量测融合,就是以当前位置的磁偏角(可以查表,也可以预先输入...其量测雅各比求解如下,主要求解两项: 1、相对地磁北magN分量的雅各比; 2、相对地磁东magE分量的雅各比: ...
  • BAJacobian矩阵推导.pdf

    2021-05-01 11:43:58
    雅可比矩阵推导
  • 用matlab计算雅可比矩阵

    千次阅读 2022-03-15 16:27:12
    如何用matlab轻松算出雅可比矩阵
  • 雅可比矩阵:“Jacobian“矩阵

    千次阅读 2022-05-23 16:25:24
    1. 雅可比矩阵:"Jacobian"矩阵 在向量微积分中,雅可比矩阵是一阶偏导数以一定方式排列成的矩阵,雅可比矩阵类似于多元函数的导数,其行列式称为雅可比行列式;雅可比矩阵的重要性在于体现了一个可微方程与给定点...
  • 简介雅可比矩阵(Jacobian)

    万次阅读 2020-04-09 14:09:54
    雅可比矩阵,有时简称为雅可比矩阵,是一个一阶偏导矩阵(在某些情况下,术语“雅可比矩阵”也指雅可比矩阵的行列式)。 注意,在某些约定中,雅可比矩阵是上述矩阵的转置。 其中m=n为方阵,常用于坐标变换,...
  • 用途: 求解未知表达式情况下的雅克比矩阵。 clear,clc; % f1(x,y) = ye^x + xy % f2(x,y) = xe^y + xy % (0.5,1)处的Jacobian x = 0.5; y = 1; J = calJ(x,y) dt=0.000001; df1x = (cal1(x+dt,y)-cal1(x,y))/dt; ...
  • 此函数在有限元分析中找到八节点砖单元的雅可比和变形矩阵 (B) 的行列式: 函数 [J_det, B]=brick8(V,r,s,t) %输入---------- V: (8*3) 顶点坐标矩阵。 行代表每个节点和列 x 坐标、y 坐标和 z 坐标。 r、s 和 t...
  • 数值计算之 梯度向量,雅可比矩阵,海森矩阵前言梯度向量梯度矩阵雅可比矩阵海森矩阵总结 前言 非线性最小二乘中的函数求导内容,主要涉及梯度向量、雅可比矩阵和海森矩阵。因此提前做一个辨析。实际上之前在矩阵...

空空如也

空空如也

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

雅各比矩阵

友情链接: student-system.rar