Trajectory planning in Cartesian space is divided into linear trajectory planning and arc trajectory planning. This article is a matlab simulation analysis of arc interpolation method in Cartesian space.
Table of Contents
1 Experimental purpose
2 Experimental content
2.1 Standard D-H parameter method
2.2 Matlab functions used in experiments
3 all codes
4 Simulation results
1 Experiment purpose
Based on the theoretical knowledge of robotics, the standard D-H parameter method is used to establish a mathematical model of the articulated robot, and the Robotics Toolbox toolkit of Matlab is used to build the model.
2 Experimental Content
2.1 Standard D-H parameter method
The standard D-H parameter method is often used to establish mathematical models of articulated robots. The D-H parameter method is a coordinate description of connecting rods, and an articulated robot is essentially a series of connecting rods connected through joints space open kinematic chain.
As for the connecting rod itself, its function is to maintain a fixed geometric relationship between the joint axes at both ends. The characteristics of the connecting rod are determined by the axis, and are usually described by four connecting rod parameters:Connecting rod length, connecting rod torsion angle, Link offset and joint angles.
Matlab functions used in the 2.2 experiment
Link function
Used to define an axis of a six-axis robot.
It includes the kinematic parameters, dynamic parameters, rigid body moment of inertia parameters, motor and transmission parameters of the robot;
The DH method can be used to build a model, which includes parameters: joint angle, joint distance, connecting rod length, connecting rod angle, joint type (0 rotation, 1 movement).
% defines an axis of the six-axis robot L(1) = Link([theta1, D1, A1, alpha1, offset1], 'standard')
SerialLink function
Used to build robotic arms.
It has many class functions, including display robot, dynamics, inverse dynamics, Jacobian, etc.;
% 'six' is the name of the robotic arm robot = SerialLink(L,'name','six');
fkine positive solution function
Used to solve the end pose p.
theta = [0.1,0,0,0,0,0]; %specified joint angle p=robot.fkine(theta) %fkine positive solution function, based on the joint angle theta, solves the end pose p
ikine inverse solution function
Used to solve for joint angle q.
q=ikine(robot,p) %ikine inverse solution function, based on the end pose p, solves the joint angle q
Trajectory Planning
(1)jtraj
Knowing the initial and final joint angles, use a fifth-order polynomial to plan the trajectory;
T1=transl(0.5,0,0); %According to the given starting point, get the starting point pose T2=transl(0,0.5,0); %According to the given end point, get the end point pose init_ang=robot2.ikine(T1);%According to the starting point pose, get the starting point joint angle targ_ang=robot2.ikine(T2);%According to the end point pose, get the end point joint angle step = 20; [q,qd, qdd]=jtraj(init_ang,targ_ang,step); %Five degree polynomial trajectory, get joint angle, angular velocity, angular acceleration, 50 is the number of sampling points
(2)ctraj
The initial and final end joint poses are known, and the trajectory is planned using uniform acceleration and uniform deceleration motion.
T0 = robot2.fkine(init_ang);% correct solution of kinematics T1 = robot2.fkine(targ_ang);% correct solution of kinematics Tc = ctraj(T0,T1,step); %Get the T matrix of each step tt = transl(Tc);
3 all codes
%% MATLAB quality triple link https://www.guyuehome.com/34853
clear;
close all;
clc;
%% Experiment 1 Simulation of an articulated six-axis manipulator based on MATLAB
%% parameter definition
%The robotic arm is a six-degree-of-freedom robotic arm
clear L;
%Angle conversion
angle=pi/180; % degree
%D-H parameter table
theta1 = -pi/2; D1 = 89.2; A1 = 0; alpha1 = -pi/2; offset1 = 0;
theta2 = 0; D2 = 0; A2 = 425; alpha2 = 0; offset2 = 0;
theta3 = 0; D3 = 0; A3 = 392; alpha3 = 0; offset3 = 0;
theta4 = pi/2; D4 = 109.3; A4 = 0; alpha4 = pi/2; offset4 = 0;
theta5 = -pi/2; D5 = 94.75; A5 = 0; alpha5 = -pi/2; offset5 = 0;
theta6 = 0; D6 = 82.5; A6 = 0; alpha6 = 0; offset6 = 0;
%% DH method is used to establish the model, joint angle, joint distance, connecting rod length, connecting rod angle, joint type (0 rotation, 1 movement)
L(1) = Link([theta1, D1, A1, alpha1, offset1], 'standard')
L(2) = Link([theta2, D2, A2, alpha2, offset2], 'standard')
L(3) = Link([theta3, D3, A3, alpha3, offset3], 'standard')
L(4) = Link([theta4, D4, A4, alpha4, offset4], 'standard')
L(5) = Link([theta5, D5, A5, alpha5, offset5], 'standard')
L(6) = Link([theta6, D6, A6, alpha6, offset6], 'standard')
% Define joint range
L(1).qlim =[-180*angle, 180*angle];
L(2).qlim =[-180*angle, 180*angle];
L(3).qlim =[-180*angle, 180*angle];
L(4).qlim =[-180*angle, 180*angle];
L(5).qlim =[-180*angle, 180*angle];
L(6).qlim =[-180*angle, 180*angle];
%% shows the robot arm
robot0 = SerialLink(L,'name','ur5');
f = 1% is drawn on the first picture
theta = [0 pi/2 0 0 pi 0]; % initial joint angle
figure(f)
robot0.plot(theta);
title('Six-axis robotic arm model');
%% Add the teach command to adjust the angles of each joint
robot1 = SerialLink(L,'name','ur5');
f=2
figure(f)
robot1.plot(theta);
robot1.teach
title('The six-axis robotic arm model is adjustable');
%% Experiment 2 Simulation of six-axis robot arm trajectory planning based on MATLAB
%% 2.2 Solving the positive solution of kinematics
robot2 = SerialLink(L,'name','ur5');
theta2 = [0.1,0,0,0,0,0]; % joint angle specified in experiment 2
p=robot2.fkine(theta2) %fkine positive solution function, based on the joint angle theta, solves the end pose p
q=ikine(robot2,p) %ikine inverse solution function, based on the end pose p, solves the joint angle q
robot2.plot(q,'movie','circleCHAZHI.gif');%Save
%% 2.3 jtraj knows the initial and final joint angles, and uses a fifth-order polynomial to plan the trajectory
% T1=transl(0.5,0,0); %According to the given starting point, get the starting point pose
% T2=transl(0,0.5,0); %According to the given end point, get the end point pose
T1=transl(400,-500,0); %According to the given starting point, get the starting point pose
T2=transl(0,400,600); %According to the given end point, get the end point pose
init_ang=robot2.ikine(T1); %According to the starting point pose, get the starting point joint angle
targ_ang=robot2.ikine(T2); %According to the end point pose, get the end point joint angle
step = 20;
f=3
%Trajectory planning method
figure(f)
[q,qd, qdd]=jtraj(init_ang,targ_ang,step); %Five degree polynomial trajectory, get joint angle, angular velocity, angular acceleration, 50 is the number of sampling points
grid on
T=robot2.fkine(q); %According to interpolation, obtain the end effector pose
nT=T.T;
plot3(squeeze(nT(1,4,:)), squeeze(nT(2,4,:)), squeeze(nT(3,4,:)),'LineWidth',2);% output end trajectory
title('Output end track');
robot2.plot(q); % animation demonstration
%% Solve the position, speed and acceleration curves
f=4
figure(f)
subplot(3,2,[1,3]); %subplot partitions the screen into three rows and two columns, occupying positions 1 to 3
plot3(squeeze(nT(1,4,:)),squeeze(nT(2,4,:)),squeeze(nT(3,4,:)));% output end trajectory
robot2.plot(q); % animation demonstration
figure(f)
subplot(3, 2, 2);
i = 1:6;
plot(q(:,1));
title('position');
grid on;
figure(f)
subplot(3, 2, 4);
i = 1:6;
plot(qd(:,1));
title('Speed');
grid on;
figure(f)
subplot(3, 2, 6);
i = 1:6;
plot(qdd(:,1));
title('Acceleration');
grid on;
t = robot2.fkine(q); %Correct solution of kinematics
rpy=tr2rpy(t); Extract position (xyz) in %t
figure(f)
subplot(3,2,5);
plot2(rpy);
%% ctraj planning trajectory considers the end effector moving between two Cartesian poses
f=5
T0 = robot2.fkine(init_ang); %Correct solution of kinematics
T1 = robot2.fkine(targ_ang); %Kinematics correct solution
Tc = ctraj(T0,T1,step); %Get the T matrix of each step
tt = transl(Tc);
figure(f)
plot2(tt,'r');
title('Straight line trajectory');
4 Simulation Results
Reprinted in The trajectory planning simulation of the articulated six-axis manipulator based on MATLAB (complete code measured in 2021) – Gu Yueju The trajectory planning simulation of the articulated six-axis manipulator based on MATLAB (complete code measured in 2021) 
https://www.guyuehome.com/34853