写在前面

动力学问题，简单来看，主要由两个问题组成：

已知一个轨迹点theta、theta_d、theta_dd，希望求出期望的关节力矩tau，这个问题及相应公式对机械臂的控制问题非常有用；

已知一个力矩矢量tau，计算出机械臂各关节的瞬态运动变量theta、theta_d、theta_dd，这个问题对机械臂的运动仿真及控制非常有用。

准备知识：

运动学 机器人学回炉重造(1)：正运动学、标准D-H法与改进D-H法的区别与应用(附ABB机械臂运动学建模matlab代码)

雅克比矩阵 机器人学回炉重造(2)：雅克比矩阵(附matlab实现)

下面先搬上笔记，最后面是平面三连杆动力学仿真代码(已通过函数验证)以及六自由度机械臂动力学仿真代码(未验证)(2019.11.29已通过函数验证)。

动力学

另外，在高自由度的情况下，牛顿-欧拉算法更容易通过编程实现，核心如下：

平面三连杆机械臂瞬态运动的牛顿-欧拉递归逆动力学求解Matlab

问题描述

自写Matlab实现代码

% 三连杆机械臂瞬态运动的牛顿-欧拉递归逆动力学求解：

% 参数：(运动指令)各关节运动角度， 关节速度， 关节加速度(3*1矩阵)

% 返回值：各关节力矩(3*1矩阵)

function tau = three_dof_dynamics(theta, theta_d, theta_dd)

% 改进D-H参数

th(1) = theta(1)*pi/180; d(1) = 0; a(1) = 0; alp(1) = 0;

th(2) = theta(2)*pi/180; d(2) = 0; a(2) = 4; alp(2) = 0;

th(3) = theta(3)*pi/180; d(3) = 0; a(3) = 3; alp(3) = 0;

% base_link的各项初始值

w00 = [0; 0; 0]; v00 = [0; 0; 0]; w00d = [0; 0; 0]; v00d = [0; 9.8; 0];

% 各关节p及各link质心pc的距离(假设质心在几何中心)

p10 = [0; 0; 0]; p21 = [4; 0; 0];

p32 = [3; 0; 0]; p43 = [2; 0; 0];

pc11 = [2; 0; 0]; pc22 = [1.5; 0; 0];

pc33 = [1; 0; 0];

z = [0; 0; 1];

% 各连杆质量

m1 = 20; m2 = 15; m3 = 10;

% 惯性张量

I1 = [0 0 0; 0 0 0; 0 0 0.5]; I2 = [0 0 0; 0 0 0; 0 0 0.2]; I3 = [0 0 0; 0 0 0; 0 0 0.1];

% 各齐次矩阵function T = MDHTrans(alpha, a, d, theta)

% 旋转矩阵

T01 = MDHTrans(alp(1), a(1), d(1), th(1));

T12 = MDHTrans(alp(2), a(2), d(2), th(2));

T23 = MDHTrans(alp(3), a(3), d(3), th(3));

R01 = T01(1:3, 1:3); R12 = T12(1:3, 1:3); R23 = T23(1:3, 1:3);

R10 = R01'; R21 = R12'; R32 = R23';

R34 = [1 0 0; 0 1 0; 0 0 1];

%% Outward iterations: i: 0->2

% i = 0 连杆1

w11 = R10*w00 + theta_d(1)*z;

w11d = R10*w00d + cross(R10*w00, z*theta_d(1)) + theta_dd(1)*z;

v11d = R10*(cross(w00d, p10) + cross(w00, cross(w00, p10)) + v00d);

vc11d = cross(w11d, pc11) + cross(w11, cross(w11, pc11)) + v11d;

F11 = m1*vc11d;

N11 = I1*w11d + cross(w11, I1*w11);

% i = 1 连杆2

w22 = R21*w11 + theta_d(2)*z;

w22d = R21*w11d + cross(R21*w11, z*theta_d(2)) + theta_dd(2)*z;

v22d = R21*(cross(w11d, p21) + cross(w11, cross(w11, p21)) + v11d);

vc22d = cross(w22d, pc22) + cross(w22, cross(w22, pc22)) + v22d;

F22 = m2*vc22d;

N22 = I2*w22d + cross(w22, I2*w22);

% i = 2 连杆3

w33 = R32*w22 + theta_d(3)*z;

w33d = R32*w22d + cross(R32*w22, z*theta_d(3)) + theta_dd(3)*z;

v33d = R32*(cross(w22d, p32) + cross(w22, cross(w22, p32)) + v22d);

vc33d = cross(w33d, pc33) + cross(w33, cross(w33, pc33)) + v33d;

F33 = m3*vc33d;

N33 = I3*w33d + cross(w33, I3*w33);

%% Inward iterations: i: 3->1

f44 = [0; 0; 0]; n44 = [0; 0; 0];

% i = 3

f33 = R34*f44 + F33;

n33 = N33 + R34*n44 + cross(pc33, F33) + cross(p43, R34*f44);

tau(3) = n33'*z;

% i = 2

f22 = R23*f33 + F22;

n22 = N22 + R23*n33 + cross(pc22, F22) + cross(p32, R23*f33);

tau(2) = n22'*z;

% i =1

f11 = R12*f22 + F11;

n11 = N11 + R12*n22 + cross(pc11, F11) + cross(p21, R12*f22);

tau(1) = n11'*z;

end

运行结果：

验证代码

clear;

clc;

% 改进D-H

th(1) = 0; d(1) = 0; a(1) = 0; alp(1) = 0;

th(2) = 0; d(2) = 0; a(2) = 4; alp(2) = 0;

th(3) = 0; d(3) = 0; a(3) = 3; alp(3) = 0;

% DH parameters th d a alpha

L1 = Link([th(1), d(1), a(1), alp(1)], 'modified');

L2 = Link([th(2), d(2), a(2), alp(2)], 'modified');

L3 = Link([th(3), d(3), a(3), alp(3)], 'modified');

L1.m = 20; L2.m = 15; L3.m = 10;% 连杆质量

% 连杆质心位置

L1.r = [2 0 0];

L2.r = [1.5 0 0];

L3.r = [1 0 0];

% 连杆惯性张量

L1.I = [0 0 0; 0 0 0; 0 0 0.5];

L2.I = [0 0 0; 0 0 0; 0 0 0.2];

L3.I = [0 0 0; 0 0 0; 0 0 0.1];

robot = SerialLink([L1, L2, L3]);

robot.name = 'Plan3R';

robot.comment = 'xuuyann';

robot.display()

% Forward Pose Kinematics

theta=[10, 20, 30]*pi/180;

robot.teach()

robot.plot(theta);

t0=robot.fkine(theta) %末端执行器位姿

% TAU = R.rne(Q, QD, QDD, GRAV)

tau = robot.rne(theta, [1, 2, 3], [0.5, 1, 1.5], [0 9.8 0])

运算结果：

验证完毕。

六自由度机械臂瞬态运动的牛顿-欧拉递归逆动力学求解Matlab(未验证)(2019.11.29已通过函数验证)

前情提要： 机器人学回炉重造(1)：正运动学、标准D-H法与改进D-H法的区别与应用(附ABB机械臂运动学建模matlab代码)

自写代码

% 六自由度机械臂瞬态运动的牛顿-欧拉递归逆动力学求解：

% 参数：(运动指令)各关节运动角度， 关节速度， 关节加速度(6*1矩阵)

% 返回值：各关节力矩(6*1矩阵)

% 由于我不清楚机械臂实际质量参数，因此下面的惯性张量及质心位置均为假设杜撰

function tau = myNewtonEuler(theta, theta_d, theta_dd)

%% 初始化

% 改进D-H参数

th(1) = theta(1)*pi/180; d(1) = 0; a(1) = 0; alp(1) = 0;

th(2) = theta(2)*pi/180; d(2) = 0; a(2) = 0.320; alp(2) = pi/2;

th(3) = theta(3)*pi/180; d(3) = 0; a(3) = 0.975; alp(3) = 0;

th(4) = theta(4)*pi/180; d(4) = 0.887; a(4) = 0.2; alp(4) = pi/2;

th(5) = theta(5)*pi/180; d(5) = 0; a(5) = 0; alp(5) = -pi/2;

th(6) = theta(6)*pi/180; d(6) = 0; a(6) = 0; alp(6) = pi/2;

w00 = [0; 0; 0]; v00 = [0; 0; 0]; w00d = [0; 0; 0]; v00d = [0; 0; 9.8];%base_link的各项初始值

z = [0; 0; 1];

% 各连杆质量L1.m = 7.36; L2.m = 36.26; L3.m = 12.44;

% L4.m = 1.21; L5.m = 1.21; L6.m = 1;

m1 = 7.36; m2 = 36.26; m3 = 12.44; m4 = 1.21; m5 = 1.21; m6 = 1;

% 惯性张量

I1 = [0 0 0; 0 0 0; 0 0 0.6]; I2 = [0 0 0; 0 0 0; 0 0 0.5]; I3 = [0 0 0; 0 0 0; 0 0 0.4];

I4 = [0 0 0; 0 0 0; 0 0 0.3]; I5 = [0 0 0; 0 0 0; 0 0 0.2]; I6 = [0 0 0; 0 0 0; 0 0 0.1];

% 各齐次矩阵function T = MDHTrans(alpha, a, d, theta)

T01 = MDHTrans(alp(1), a(1), d(1), th(1));

T12 = MDHTrans(alp(2), a(2), d(2), th(2));

T23 = MDHTrans(alp(3), a(3), d(3), th(3));

T34 = MDHTrans(alp(4), a(4), d(4), th(4));

T45 = MDHTrans(alp(5), a(5), d(5), th(5));

T56 = MDHTrans(alp(6), a(6), d(6), th(6));

% 各关节p及各link质心pc的距离(假设质心在几何中心)

p10 = T01(1: 3, 4); p21 = T12(1: 3, 4); p32 = T23(1: 3, 4);

p43 = T34(1: 3, 4); p54 = T45(1: 3, 4); p65 = T56(1: 3, 4); p76 = [0, 0, 0]';

pc11 = 0.5 * p21; pc22 = 0.5 * p32; pc33 = 0.5 * p43;

pc44 = 0.5 * p54; pc55 = 0.5 * p65; pc66 = 0.5 * p76;

% 旋转矩阵

R01 = T01(1:3, 1:3); R12 = T12(1:3, 1:3); R23 = T23(1:3, 1:3);

R34 = T34(1:3, 1:3); R45 = T45(1:3, 1:3); R56 = T56(1:3, 1:3);

R10 = R01'; R21 = R12'; R32 = R23';

R43 = R34'; R54 = R45'; R65 = R56';

R67 = [1 0 0; 0 1 0; 0 0 1]; R76 = R67';

%% Outward iterations: i: 0->5

% 连杆1到连杆6向外迭代

% i = 0

w11 = R10*w00 + theta_d(1)*z;

w11d = R10*w00d + cross(R10*w00, z*theta_d(1)) + theta_dd(1)*z;

v11d = R10*(cross(w00d, p10) + cross(w00, cross(w00, p10)) + v00d);

vc11d = cross(w11d, pc11) + cross(w11, cross(w11, pc11)) + v11d;

F11 = m1*vc11d;

N11 = I1*w11d + cross(w11, I1*w11);

% i = 1

w22 = R21*w11 + theta_d(2)*z;

w22d = R21*w11d + cross(R21*w11, z*theta_d(2)) + theta_dd(2)*z;

v22d = R21*(cross(w11d, p21) + cross(w11, cross(w11, p21)) + v11d);

vc22d = cross(w22d, pc22) + cross(w22, cross(w22, pc22)) + v22d;

F22 = m2*vc22d;

N22 = I2*w22d + cross(w22, I2*w22);

% i = 2

w33 = R32*w22 + theta_d(3)*z;

w33d = R32*w22d + cross(R32*w22, z*theta_d(3)) + theta_dd(3)*z;

v33d = R32*(cross(w22d, p32) + cross(w22, cross(w22, p32)) + v22d);

vc33d = cross(w33d, pc33) + cross(w33, cross(w33, pc33)) + v33d;

F33 = m3*vc33d;

N33 = I3*w33d + cross(w33, I3*w33);

% i= 3

w44 = R43*w33 + theta_d(4)*z;

w44d = R43*w33d + cross(R43*w33, z*theta_d(4)) + theta_dd(4)*z;

v44d = R43*(cross(w33d, p43) + cross(w33, cross(w33, p43)) + v33d);

vc44d = cross(w44d, pc44) + cross(w44, cross(w44, pc44)) + v44d;

F44 = m4*vc44d;

N44 = I4*w44d + cross(w44, I4*w44);

% i = 4

w55 = R54*w44 + theta_d(5)*z;

w55d = R54*w44d + cross(R54*w44, z*theta_d(5)) + theta_dd(5)*z;

v55d = R54*(cross(w44d, p54) + cross(w44, cross(w44, p54)) + v44d);

vc55d = cross(w55d, pc55) + cross(w55, cross(w55, pc55)) + v55d;

F55 = m5*vc55d;

N55 = I5*w55d + cross(w55, I5*w55);

% i = 5

w66 = R65*w55 + theta_d(6)*z;

w66d = R65*w55d + cross(R65*w55, z*theta_d(6)) + theta_dd(6)*z;

v66d = R65*(cross(w55d, p65) + cross(w55, cross(w55, p65)) + v55d);

vc66d = cross(w66d, pc66) + cross(w66, cross(w66, pc66)) + v66d;

F66 = m6*vc66d;

N66 = I6*w66d + cross(w66, I6*w66);

%% Inward iterations: i: 6->1

% 连杆6到连杆1向内迭代

f77 = [0; 0; 0]; n77 = [0; 0; 0];

% i = 6

f66 = R67*f77 + F66;

n66 = N66 + R67*n77 + cross(pc66, F66) + cross(p76, R67*f77);

tau(6) = n66'*z;

% i = 5

f55 = R56*f66 + F55;

n55 = N55 + R56*n66 + cross(pc55, F55) + cross(p65, R56*f66);

tau(5) = n55'*z;

% i = 4

f44 = R45*f55 + F44;

n44 = N44 + R45*n55 + cross(pc44, F44) + cross(p54, R45*f55);

tau(4) = n44'*z;

% i = 3

f33 = R34*f44 + F33;

n33 = N33 + R34*n44 + cross(pc33, F33) + cross(p43, R34*f44);

tau(3) = n33'*z;

% i = 2

f22 = R23*f33 + F22;

n22 = N22 + R23*n33 + cross(pc22, F22) + cross(p32, R23*f33);

tau(2) = n22'*z;

% i =1

f11 = R12*f22 + F11;

n11 = N11 + R12*n22 + cross(pc11, F11) + cross(p21, R12*f22);

tau(1) = n11'*z;

end

运行结果：

验证代码：

% Modified DH

% ABB robot

clear;

clc;

th(1) = 0; d(1) = 0; a(1) = 0; alp(1) = 0;

th(2) = 0; d(2) = 0; a(2) = 0.320; alp(2) = pi/2;

th(3) = 0; d(3) = 0; a(3) = 0.975; alp(3) = 0;

th(4) = 0; d(4) = 0.887; a(4) = 0.2; alp(4) = pi/2;

th(5) = 0; d(5) = 0; a(5) = 0; alp(5) = -pi/2;

th(6) = 0; d(6) = 0; a(6) = 0; alp(6) = pi/2;

% DH parameters th d a alpha sigma

L1 = Link([th(1), d(1), a(1), alp(1), 0], 'modified');

L2 = Link([th(2), d(2), a(2), alp(2), 0], 'modified');

L3 = Link([th(3), d(3), a(3), alp(3), 0], 'modified');

L4 = Link([th(4), d(4), a(4), alp(4), 0], 'modified');

L5 = Link([th(5), d(5), a(5), alp(5), 0], 'modified');

L6 = Link([th(6), d(6), a(6), alp(6), 0], 'modified');

L1.m = 7.36; L2.m = 36.26; L3.m = 12.44;

L4.m = 1.21; L5.m = 1.21; L6.m = 1;

% L1.r =

L1.I = [0 0 0; 0 0 0; 0 0 0.6];

L2.I = [0 0 0; 0 0 0; 0 0 0.5];

L3.I = [0 0 0; 0 0 0; 0 0 0.4];

L4.I = [0 0 0; 0 0 0; 0 0 0.3];

L5.I = [0 0 0; 0 0 0; 0 0 0.2];

L6.I = [0 0 0; 0 0 0; 0 0 0.1];

T01 = MDHTrans(alp(1), a(1), d(1), th(1));

T12 = MDHTrans(alp(2), a(2), d(2), th(2));

T23 = MDHTrans(alp(3), a(3), d(3), th(3));

T34 = MDHTrans(alp(4), a(4), d(4), th(4));

T45 = MDHTrans(alp(5), a(5), d(5), th(5));

T56 = MDHTrans(alp(6), a(6), d(6), th(6));

% 各关节p及各link质心pc的距离(假设质心在几何中心)

p10 = T01(1: 3, 4); p21 = T12(1: 3, 4); p32 = T23(1: 3, 4);

p43 = T34(1: 3, 4); p54 = T45(1: 3, 4); p65 = T56(1: 3, 4); p76 = [0, 0, 0]';

pc11 = 0.5 * p21

pc22 = 0.5 * p32

pc33 = 0.5 * p43

pc44 = 0.5 * p54

pc55 = 0.5 * p65

pc66 = 0.5 * p76

% 连杆质心位置

L1.r = pc11; L2.r = pc22; L3.r = pc33;

L4.r = pc44; L5.r = pc55; L6.r = pc66;

robot = SerialLink([L1, L2, L3, L4, L5, L6]);

robot.name='ABBRobot-6-dof';

robot.display()

theta = [-20, 120, -15, 30, 20, 10]*pi/180;

robot.teach();

robot.plot(theta);

% t = robot.fkine(theta) %末端执行器位姿

% [~, T] = My_Forward_Kinematics_MDH(theta, d, a, alp)

qd = [6 5 4 3 2 1]; qdd = [6 5 4 3 2 1];

tau = robot.rne(theta, qd, qdd, [0, 0, 9.8])

TAU = myNewtonEuler(theta*180/pi, qd', qdd')

运行结果：

qd=[1 1 1 1 1 1]’; qdd = [6 5 4 3 2 1]’;

qd=[1 1 1 1 1 1]’; qdd=[1 1 1 1 1 1]’;

2019.11.29

感谢vaultboy2020指出的错误，正文程序已改正并得到正确的验证结果

机械动力学瑞利法matlab程序,机器人学回炉重造（4）：动力学仿真（附牛顿-欧拉递归逆动力学算法matlab代码）...相关推荐

  1. 机器人学回炉重造（2-2）：雅可比矩阵的求法——矢量积法、微分变换法、Manipulator Jacobian（Jacobian for short）

    文章目录 写在前面 矢量积法--改进D-H法 微分变换法--改进D-H法 Manipulator for Jacobian(Jacobian for short)--标准D-H法 微分变换法--标准D ...

  2. 机器人学回炉重造（1-2）：各种典型机械臂的正运动学建模（标准D-H法）

    文章目录 写在前面 三连杆平面机械臂 平行四边形操作臂 闭链结构 例:平行四边形操作臂 球形臂 拟人臂 球腕 斯坦福机械臂 带球形手腕的拟人化机械臂 DLR机械臂 参考文献 写在前面 本文所有机械臂均 ...

  3. 机器人学回炉重造（5-2）：关节空间规划方法——梯形加减速（与抛物线拟合的线性函数）、S型曲线规划

    文章目录 写在前面 学习代码都记录在[个人github](https://github.com/xuuyann/RobotLearningCode)上,欢迎关注~ 梯形加减速(与抛物线拟合的线性函数) ...

  4. 机器人学回炉重造（2-4）：运动学奇异位型分析

    文章目录 什么是运动学奇异位型? 例子:平面二连杆机械手的奇异位型 奇异位型解耦 腕部奇异位型 手臂奇异位型 转载:6轴串联关节机器人的奇异点 参考文献 什么是运动学奇异位型? 在初步系统地了解了机器 ...

  5. 机器人学回炉重造（2-3）：基本雅可比矩阵与其他雅可比矩阵

    文章目录 基本雅可比矩阵 定义 求法 其他雅可比矩阵 定义 求法 补充:几何雅可比与解析雅可比 基本雅可比矩阵 定义 用笛卡尔坐标描述线速度(linear velocity)和角速度(angular ...

  6. matlab欧拉迭代,matlab机械臂正逆运动学求解问题，使用牛顿-欧拉迭代算法

    代码复制的有问题,详细见本楼,谢谢. clc;clear; DR=pi/180; %time j = 1; for i = 0 : 0.1 : 2 %input theta= 45 * DR *(1+ ...

  7. 【机器人学】牛顿-欧拉动力学方程迭代形式

    1 牛顿方程和欧拉方程 作者推导了牛顿欧拉动力学方程的迭代形式和矩阵,本文重点描述动力学方程的迭代形式,动力学方程的矩阵形式见博文. 假定机械臂的连杆均为刚体,若连杆质心的位置和惯性张量已知,那么它的 ...

  8. Vue回炉重造之封装防刷新考试倒计时组件

    你好,我是Vam的金豆之路,可以叫我豆哥.2019年年度博客之星.技术领域博客专家.主要领域:前端开发.我的微信是 maomin9761,有什么疑问可以加我哦,自己创建了一个微信技术交流群,可以加我邀 ...

  9. 《回炉重造 Java 基础》——集合（容器）

    整体框架 绿色代表接口/抽象类:蓝色代表类. 主要由两大接口组成,一个是「Collection」接口,另一个是「Map」接口. 前言 以前刚开始学习「集合」的时候,由于没有好好预习,也没有学好基础知识 ...

  10. 真人电影中的幻想生物迷墙：索尼克为什么被骂到回炉重造？

    <大侦探皮卡丘>上映在即,当网友们对着雷佳音配音的皮卡丘大呼好萌好萌时,我们仿佛又来到了大型真香现场--明明在几个月之前,当人们看到毛茸茸的大叔音皮卡丘时还每个细胞都充满了拒绝. 也有一种 ...

最新文章

  1. 求高精度幂(java)
  2. 苹果降价背后的代工厂之殇
  3. 单例模式应用场景_三、单例模式详解
  4. SQL技巧(多行合并一列)
  5. LeetCode 1984. 学生分数的最小差值
  6. 信奥中的数学：博弈论
  7. 任正非：6G华为也是领先世界 或在十年后投入使用
  8. html标签b规定粗体文本
  9. HBase简介及使用
  10. ETC风头已过，龙头林立，黔通智联此时上市还真缺点儿“想象力”
  11. 津津乐道播客网络 实习生/志愿者 招募
  12. linux系统的常用命令：在线下载数据文件解压缩
  13. js大数相加或相减（支持正数或负数）
  14. Centos6下Redis学习（一）——Java客户端Lettuce的使用、Springboot整合
  15. Data truncation: Data too long for column 'xxx' at row 1
  16. 人脸识别技术禁令再来！美国又一城市禁止面部识别软件
  17. PhotoBulk for Mac v2.3 图片批量调整重命名转换水印工具
  18. 雷军再次承诺：小米9下周二开放购买
  19. DMOZ编辑申请流程
  20. 公众号榜单 | 2020·8月公众号行业排行榜重磅发布

热门文章

  1. 制度罚则-- 日志规范
  2. Android Studio Menu item 的简单使用
  3. sar adc的常用指标（一）
  4. GitHub中国区前100名到底是什么样的人？向大佬们学习。
  5. Can‘t locate XXX/XXX.pm in @INC (you may need to install the XXX::XXX module)
  6. 云计算、网格计算和分布式计算区别
  7. jS 清除form表单
  8. LeetCode热门100之 无重复字符的最长子串
  9. glassfish java环境_GlassFish安装和配置详解
  10. Linux CentOS 大数据集群的基础配置(3) CHD分布式环境搭建