最近小虎在课程设计上用到了机器人学正运动学的仿真学习，利用D-H参数法求解正运动学问题。在利用MATLAB仿真中，通过设置不同的D-H参数以及位置的数量，可以实现多关节不同的位姿位置变换。另外，通过参数连续化，可以近似拟合出动态的运动。

结果

静态的四位置图：

动态仿真图：

代码分析

代码部分分为了6个程序，分别是设置初始参数、D-H求解、D-H参数绘图、3D圆柱关节绘制、3D杆件绘制、移动主程序。代码分析无删减。

D-H参数表

objects	θ\thetaθ	ddd	aaa	α\alphaα
1	000	0	50	90090^0900
2	20020^0200	100	0	90090^0900
3	30030^0300	120	0	000

比如关节2到3的变换可以就简化为：

在这里的实际例子是：
2T3=A3=[0.939700.342000.34200−0.939700−101000001]^2T_3=A_3=\begin{bmatrix} 0.9397 & 0 & 0.3420& 0\\ 0.3420 & 0 & -0.9397& 0\\ 0 & -1 & 0& 100\\ 0 & 0 & 0& 1 \end{bmatrix}2T3=A3=⎣⎢⎢⎡0.93970.34200000−100.3420−0.939700001001⎦⎥⎥⎤

Build_3DOFRobot.m

参数设置，D-H参数表。
有些变换要进行角度弧度的转变，因此设有ToDeg和ToRad。

ToDeg = 180/pi;
ToRad = pi/180;
UZ = [0 0 1]';

构建结构变量，以便设置D-H参数表，θ\thetaθ在调用DH求解时候设置，这里暂且设置为零。

Link= struct('name','Body' , 'th', 0, 'dz', 0, 'dx', 0, 'alf',90*ToRad,'az',UZ);
Link(1)= struct('name','Base' , 'th', 0*ToRad, 'dz', 0, 'dx', 0, 'alf',0*ToRad,'az',UZ);
Link(2) = struct('name','J1' , 'th', 0*ToRad, 'dz', 50, 'dx', 0, 'alf',90*ToRad,'az',UZ);
Link(3) = struct('name','J2' , 'th', 0*ToRad, 'dz', 0, 'dx', 100, 'alf',90*ToRad,'az',UZ);
Link(4) = struct('name','J3' , 'th', 0*ToRad, 'dz', 0, 'dx', 120, 'alf',0*ToRad,'az',UZ);

DHfk3Dof.m

D-H参数绘图。
调用D-H参数表，设置绘图尺寸（关节圆柱），将角度代入。

function DHfk3Dof(th1,th2,th3,fcla)
global Link;
Build_3DOFRobot;
radius = 10;
len= 30;
joint_col = 0;
plot3(0,0,0,'ro');
Link(2).th=th1*pi/180;
Link(3).th=th2*pi/180;
Link(4).th=th3*pi/180;

求解D-H参数矩阵。

for i=1:4 DH_Matrix(i);
end

绘图。

for i=2:4 Link(i).A=Link(i-1).A*Link(i).A; Link(i).p= Link(i).A(:,4); Link(i).n= Link(i).A(:,1); Link(i).o= Link(i).A(:,2); Link(i).a= Link(i).A(:,3);Link(i).R=[Link(i).n(1:3),Link(i).o(1:3),Link(i).a(1:3)]; Connect3D(Link(i-1).p,Link(i).p,'b',2); hold on;
DrawCylinder(Link(i-1).p, Link(i-1).R * Link(i).az, radius,len, joint_col);
hold on;
end

DH_Matrix.m

D-H矩阵求解。

function DH_Matrix(i)
global Link
C=cos(Link(i).th);
S=sin(Link(i).th);
Ca=cos(Link(i).alf);
Sa=sin(Link(i).alf);
a=Link(i).dx;
d=Link(i).dz; Link(i).n=[C,S,0,0]';
Link(i).o=[-1*S*Ca,C*Ca,Sa,0]';
Link(i).a=[S*Sa, -1*C*Sa,Ca,0]';
Link(i).p=[a*C,a*S,d,1]';
Link(i).R=[Link(i).n(1:3),Link(i).o(1:3),Link(i).a(1:3)];
Link(i).A=[Link(i).n,Link(i).o,Link(i).a,Link(i).p];

DrawCylinder.m

画关节圆柱。

function h = DrawCylinder(pos, az, radius,len, col)
%rotation matrix
az0 = [0;0;1];
ax  = cross(az0,az);
ax_n = norm(ax);
if ax_n < eps rot = eye(3);
elseax = ax/ax_n;ay = cross(az,ax);ay = ay/norm(ay);rot = [ax ay az];
end%********** make cylinder
% col = [0 0.5 0];  % cylinder colora = 20;    % number of side faces
theta = (0:a)/a * 2*pi;x = [radius; radius]* cos(theta);
y = [radius; radius] * sin(theta);
z = [len/2; -len/2] * ones(1,a+1);
cc = col*ones(size(x));for n=1:size(x,1)xyz = [x(n,:);y(n,:);z(n,:)];xyz2 = rot * xyz;x2(n,:) = xyz2(1,:);y2(n,:) = xyz2(2,:);z2(n,:) = xyz2(3,:);
end%************* draw
% side faces
h = surf(x2+pos(1),y2+pos(2),z2+pos(3),cc);for n=1:2patch(x2(n,:)+pos(1),y2(n,:)+pos(2),z2(n,:)+pos(3),cc(n,:));
end

Connect3D.m

画连杆。

function Connect3D(p1,p2,option,pt)
h = plot3([p1(1) p2(1)],[p1(2) p2(2)],[p1(3) p2(3)],option);
set(h,'LineWidth',pt)
end

Mov_3DOF_Rob.m

移动主程序，这里还设了三个θ\thetaθ的值。

close all;
th1=0;
th2=20;
th3=30;
DHfk3Dof(th1,th2,th3,0);
view(134,12);

动态分析：

pause;
stp=5;
for i=0:stp:180 DHfk3Dof(th1+i,th2,th3,1);
end

完整代码——已存github

见我的github:Robotics-motion-simulation

Robotics正运动学求解仿真（附代码和解释）相关推荐

【现代机器人学——螺旋理论】UR5机器人正运动学求解
系列文章目录 [现代机器人学--螺旋理论]UR5机器人正运动学求解 [现代机器人学--螺旋理论]UR5机器人逆运动学求解提示:本文介绍基于螺旋理论分析的UR5机器人正运动学求解思路,仅供参考. 文章 ...
aubo-i5机械臂（1）-正运动学求解
目录 1.DH表建立 2.Matlab实现正运动学求解 1.DH表建立注:使用改进DH表建立的根据公式求出相邻连杆的变换矩阵 2.Matlab实现正运动学求解 function [H, H_i] ...
智能优化算法：正余弦优化算法-附代码
智能优化算法:正余弦优化算法-附代码文章目录智能优化算法:正余弦优化算法-附代码 1.算法原理 2.算法流程 3.算法结果 4.参考文献 5.MATLAB代码 6.Python代码摘要:正余 ...
Modern Robotics正运动学（forward kinematics）
机器人学之正运动学(forward kinematics) 关键概念对于一个开链机器人,给定一个固定参考系{s}和一个固定于连杆的连杆的坐标系{b}, 该坐标系表示机器人末端.正运动学(forwar ...
标准DH模型正运动学求解程序编写
依据Peter Croke所建立的机器人工具箱模型. 标准DH模型,其第j个关节,第j个连杆与第j个坐标系建立关系如下所示.即j坐标系是建立在关节j+1处. 改进DH模型,其第j个关节,第j个连杆与第 ...
2020A炉温曲线第一题pytorch求解（附代码）
模型使用牛顿冷却模型画出外界温度曲线两端温度不确定,假定为其中h等于外界温度25°,c根据温度计算即可 def T(t,v,T15,T6,T7,T89):a=[0,25,30.5,5,30.5, ...
原根的求解证明附代码
求解方法: 枚举从2开始枚举,然后暴力判断g^(P-1) = 1 (mod P)是否当且当指数为P-1的时候第一次成立而由于原根一般都不大大多都在200以内,所以可以暴力得到. 方法例如求任何一 ...
6轴机械臂（拟人臂+球形腕（三轴相交于一点））正逆运动学求解
1.DH坐标下机械臂参数 theta=[pi/10,pi/2,pi/4,-pi/4,pi/4,-pi/8];%关节角度 a=[0, 0.260, 0.025, 0, 0, 0];%连杆长度 d=[0, ...
中正平和的机器人学笔记——1. 机械臂正运动学（附MATLAB代码）
中正平和的机器人学笔记--1.正运动学 0. 何为运动学?何为正运动学? 运动学主要研究的就是机械臂的运动特性(位置.速度.加速度等),但是不考虑使机械臂产生运动时施加的力和力矩(这部分是动力学).而 ...
【Matlab 六自由度机器人】基于蒙特卡罗方法(Monte Carlo Method)构建机器人工作空间（附MATLAB建模仿真完整代码）
[Matlab 六自由度机器人]基于蒙特卡洛法构建工作空间往期回顾前言正文一.蒙特卡洛方法及机器人工作空间的概述 1. 蒙特卡洛法 2. 机器人工作空间二.rand函数的应用 1. rand ...

Robotics正运动学求解仿真（附代码和解释）

目录

结果