算法求解过程

参数说明：

aia_iai：{0}-{i}之间的矢量距离
eie_iei：在base系下描述的关节轴向量
ppp：{0}-{p}之间的矢量距离
rir_iri：{i}-{p}之间的矢量距离

代码及解析（参考Robotics Toolbox）

注意：以下代码求的是J˙q˙\dot J\dot qJ˙q˙，而不是单纯求的J˙\dot JJ˙！

以下内容转载请联系！

% SerialLink.jacob_dot Derivative of Jacobian
%
% JDQ = R.jacob_dot(Q, QD) is the product (6x1) of the derivative of the Jacobian (in the world frame) and the joint rates.
% Notes::
% - This term appears in the formulation for operational space control XDD = J(Q)QDD + JDOT(Q)QD
%function Jdot = jacob_dot(robot, q, qd)n = robot.n;links = robot.links;% 使用Angeles的表示法：%   [Q,a] ~ [R,t] the per link transformation                                       相邻连杆间的变换%   P ~ R   the cumulative rotation t2r(Tj) in world frame                          世界坐标系的累积旋转变换t2r(Tj)%   e       the last column of P, the local frame z axis in world coordinates       P的最后一列，世界坐标中的局部坐标系z轴%   w       angular velocity in base frame                                          base坐标系中的角速度 %   ed      deriv of e                                                              e的导数,其中e为关节轴向量在base下的表示%   r       is distance from final frame                                            {i}距最后坐标系的距离%   rd      deriv of r                                                              r的导数%   ud      temporary variable                                                      ud = e × rd%            e1        e2      ...      en        %   J = [----------------------------------------]           %          e1 x r1   e2 x r2   ...    en x rn     %%          w %   t = [-----]%          v%%   t = J * dot_q%   robert.h.x.s% 提取数据，为算法计算做准备for i=1:nT = links(i).A(q(i));Q{i} = t2r(T);          % 旋转矩阵a{i} = transl(T)';      % 平移矩阵end% P为累积变换，e{i}为{i}系的旋转轴z轴变换到base系下的表示,P195P{1} = Q{1};e{1} = [0 0 1]';for i=2:nP{i} = P{i-1}*Q{i};e{i} = P{i}(:,3);end% step 1 求{i}在base系下的角速度w{i}，有速度叠加（注意点：旋转矩阵求逆==求转置）,P207-P209w{1} = qd(1)*e{1};for i=1:(n-1)w{i+1} = qd(i+1)*[0 0 1]' + Q{i}'*w{i};end% step 2 求dot_e,P207-P209ed{1} = [0 0 0]';for i=2:ned{i} = cross(w{i}, e{i});end% step 3 求dot_r,方法同step2（注意点：有矢量叠加）,P207-P209rd{n} = cross(w{n}, a{n});for i=(n-1):-1:1rd{i} = cross(w{i}, a{i}) + Q{i}*rd{i+1};end% 求{i}-{p}的距离,P198r{n} = a{n};for i=(n-1):-1:1r{i} = a{i} + Q{i}*r{i+1};end% 求d(e x rd)/dt,其中d(e x rd)/dt = dot_e x r + e x dot_r ,P207-P209ud{1} = cross(e{1}, rd{1});for i=2:nud{i} = cross(ed{i}, r{i}) + cross(e{i}, rd{i});end% step 4 swap ud and ed，目的：为了统一工具箱的描述形式v{n} = qd(n)*[ud{n}; ed{n}];for i=(n-1):-1:1Ui = blkdiag(Q{i}, Q{i});                               % blkdiag 分块对角矩阵v{i} = qd(i)*[ud{i}; ed{i}] + Ui*v{i+1};endJdot = v{1};    % 结果得到的是 dot_J * dot_q,即公式的后半部分

详细参考《Fundamentals of Robotic Mechanical Systems Theory, Methods, and Algorithms》，着重参考5.1-5.5部分，具体公式推导有空再补。

机器人学中雅可比矩阵求导计算（Angeles法）相关推荐

机器学习中的矩阵向量求导(二) 矩阵向量求导之定义法
在机器学习中的矩阵向量求导(一) 求导定义与求导布局中,我们讨论了向量矩阵求导的9种定义与求导布局的概念.今天我们就讨论下其中的标量对向量求导,标量对矩阵求导, 以及向量对向量求导这三种场景的基本求解 ...
神经网络中矩阵求导术的应用
神经网络中矩阵求导术的应用序记法约定常用公式和定理举例多层神经网络(MLP)的梯度推导 Batch Normalization Layer的推导序本文假设读者熟悉一元微积分,线性代数,并 ...
对 RNN 中 BPTT 求导过程的解析尝试
(转载一次对 RNN 中 BPTT 求导过程的解析尝试 - 知乎其中关于雅可比的内容参考: 学习笔记之--Jacobian matrix(雅可比矩阵)学习笔记之--Jacobian matrix( ...
sigmod 求导计算
sigmod 求导计算浅层神经网络损失函数和成本函数
matlab对多项式求导,matlab中多项式求导
1 0.5 0 -0.5 -1 -1.5 -2 -2 -1.5 -1 -0.5 0 0.5 1 1.5 2 4.对比用多项式函数的 polyder 函数及符号函数中的 diff 函数,求导 x2+2x ...
神经网络反向传播的矩阵复合求导计算
以前一直以为矩阵的复合求导和可微函数的链式求导是一样的,但是在推导神经网络梯度公式的时候往往会出现一些符号次序不对的问题,我这里借用吴恩达编程作业中给出的浅层神经网络的案例来计算反向传播的梯度.关于矩 ...
Theano2.1.6-基础知识之在thenao中的求导
来自:http://deeplearning.net/software/theano/tutorial/gradients.html Derivatives in Theano 一.计算梯度现在,让 ...
matlab中函数求导求反,关于一个函数的反函数求导问题，一个超复杂函数……急啊！...
该楼层疑似违规已被系统折叠隐藏此楼查看此楼我在做课设,现在时间很紧了-- 关于一个函数的反函数求导问题,用matlab弄出来的超复杂函数-- 函数的大概形式是:L_bc=f(k); 我想求的是:k ...
用神经网络学习Fe原子光谱并反向求导计算权重
首先构造一个1*26*26的神经网络根据铁的发射光谱强度 * 波长10^-10m * 归一化 1000 P 2483.2708 y0 0.566498 600 P 2488.1426 y1 0.5 ...

机器人学中雅可比矩阵求导计算（Angeles法）

文章目录

算法求解过程

代码及解析（参考Robotics Toolbox）

机器人学中雅可比矩阵求导计算（Angeles法）相关推荐

最新文章

热门文章