Table of Contents
1. Calculation of forward solution of Stewart parallel six degrees of freedom
(1) Overview
(2) Matlab positive solution calculation
1. Reference procedure one
2. Reference procedure two
(3) Calculation of correct solution of C# program
1. Project download link
2. Correct solution operation calculation
(4) Collection of main program packaged download programs
2. Inverse solution calculation
(1) Matlab inverse solution calculation
1. Matlab inverse solution calculation program 1
2. Matlab inverse solution calculation program 2
3. Matlab inverse solution calculation program three
(2) C# program inverse solution calculation
1. Project download link
2. Inverse solution operation calculation
(3) Collection of inverse solution program package download programs
Note: This test is only a demonstration of the calculation method and does not involve model coordinate system parameters, matrix parameters, etc. If you need to learn more about it, please refer to the blog: Stewart six-degree-of-freedom platform forward and reverse solution algorithm_six-degree-of-freedom Stewart platform kinematics forward solution-CSDN blog
1. Calculation of forward solution of Stewart parallel connection with six degrees of freedom
(1) Overview
Stewart platform, also known as a six-degree-of-freedom parallel mechanism, is a mechanical structure with six degrees of freedom (three translational degrees of freedom and three rotational degrees of freedom). It consists of a fixed base and a movable platform, and the base and platform are connected through six connecting rods.
To calculate the forward solution of the Stewart platform, the length of each connecting rod and the coordinates of the fixed point on the base are known, and the coordinates of the moving point on the platform are solved. The correct solution calculation can be completed through the following steps:
1. Define the coordinate system of the base and platform. Choose an appropriate coordinate system and associate the coordinate systems of the base and platform to it.
2. Determine the coordinates of the fixed point on the base. Assume that there are six fixed points on the base, marked P1, P2, P3, P4, P5, P6 respectively, and their coordinates in the base coordinate system are given.
3. Determine the length of each link. Suppose there are six connecting rods, denoted L1, L2, L3, L4, L5, L6 respectively, and give their lengths.
4. Determine the coordinates of the moving point on the platform. Suppose there is a moving point M on the platform and its coordinates in the platform coordinate system are given.
5. Use trigonometric relations to calculate the angle of each connecting rod. Based on the known coordinates of the fixed point of the base, the length of the link, and the coordinates of the moving point on the platform, the angle of each link can be calculated using trigonometric relations.
6. Use the rotation matrix to calculate the pose of the platform. According to the angle of each link, a rotation matrix can be constructed, and then the coordinates of the moving points on the platform are converted into the base coordinate system to obtain the pose of the platform.
The above steps are only a rough calculation process. The specific implementation requires appropriate mathematical derivation and calculation based on the specific connecting rod structure and coordinate system selection. In practical applications, issues such as error correction and singular postures also need to be considered.
Please note that the calculation of the forward solution of the Stewart platform is relatively complex and requires strong knowledge of mathematics and geometry. If you need more detailed and accurate calculation results, it is recommended to refer to relevant literature or professional robotics textbooks, or use specialized robot simulation software for calculation and analysis.
(2) Matlab positive solution calculation
1. Reference program one
The following is a sample program for calculating the forward solution of Stewart’s parallel six degrees of freedom written in MATLAB:
%This program calculates the forward solution of Stewart's parallel six degrees of freedom, where the length of the leg L and the rotation angle of the leg theta are the input parameters.
%The program calculates the end point coordinates of each leg through a loop and uses these coordinates to calculate the rotation matrix of the platform.
%Finally, the program prints out the coordinates of the end points of the legs and the rotation matrix of the platform.
%You can modify the length and rotation angle of the legs as needed, and then run the program to get the corresponding results.
% Input parameters
L = [1, 1, 1, 1, 1, 1]; % length of leg
theta = [0, 0, 0, 0, 0, 0]; % rotation angle of the leg (unit: radians)
% Calculate the coordinates of the end point of the leg
P = zeros(3, 6); % stores the end point coordinates
for i = 1:6
P(:, i) = [L(i)*cos(theta(i)); L(i)*sin(theta(i)); 0];
end
% Calculate the rotation matrix of the platform
R = zeros(3, 3); % store rotation matrix
for i = 1:6
R = R + cos(theta(i))*eye(3) + (1 - cos(theta(i)))*(P(:, i)*P(:, i)') - sin(theta( i))*skewSymmetricMatrix(P(:, i));
end
% print results
disp("Coordinates of the end points of the legs:");
disp(P);
disp("Rotation matrix of platform:");
disp(R);
% Auxiliary function: calculate the cross product matrix
function M = skewSymmetricMatrix(v)
M = [ 0 -v(3) v(2);
v(3) 0 -v(1);
-v(2) v(1) 0 ];
end
Calculation results
2. Reference program two
% input parameters
L = [0.5, 0.5, 0.5, 0.5, 0.5, 0.5]; % leg length
theta = [pi/6, pi/4, pi/3, pi/6, pi/4, pi/3]; % rotation angle of the leg
% Calculate the coordinates of the end point of the leg
P = zeros(3, 6); % stores the end point coordinates
for i = 1:6
P(:, i) = [L(i)*cos(theta(i)); L(i)*sin(theta(i)); 0];
end
% Calculate the rotation matrix of the platform
R = zeros(3, 3); % store rotation matrix
for i = 1:6
R = R + cos(theta(i))*eye(3) + (1 - cos(theta(i)))*(P(:, i)*P(:, i)') - sin(theta( i))*skewSymmetricMatrix(P(:, i));
end
% print results
disp("Coordinates of the end points of the legs:");
disp(P);
disp("Rotation matrix of platform:");
disp(R);
% draw graphics
figure;
hold on;
grid on;
axis equal;
xlabel('X');
ylabel('Y');
zlabel('Z');
% Draw the end points of the legs
scatter3(P(1,:), P(2,:), P(3,:), 'filled');
% drawing platform
platform = [0, 1, 1, 0, 0; 0, 0, 1, 1, 0; 0, 0, 0, 0, 0];
platform = R * platform;
patch(platform(1,:), platform(2,:), platform(3,:), 'r');
% draw connection line
for i = 1:6
line([0, P(1,i)], [0, P(2,i)], [0, P(3,i)], 'Color', 'b');
end
% Auxiliary function: calculate the cross product matrix
function M = skewSymmetricMatrix(v)
M = [ 0 -v(3) v(2);
v(3) 0 -v(1);
-v(2) v(1) 0 ];
end
Calculation results
(3) Calculation of correct solution of C# program
1. Project download link
Project download link:
https://download.csdn.net/download/panjinliang066333/88421740
2. Correct solution operation and calculation
Given the platform vertex coordinates, six connecting rod lengths and angles, calculate the rotation matrix and position matrix.
The following is a sample program written in C# for calculating the forward solution of Stewart’s parallel six degrees of freedom:
main running program
?
operation result
?
In the above example program, we define the fixed point coordinates on the base, the length of the link and the moving point coordinates on the platform, and call the `StewartForwardKinematics` method to calculate the position coordinates and attitude (rotation matrix) of the platform. Finally, we display the results in the console.
Please note that this is just a simplified example program, only used to demonstrate the basic idea of Stewart’s parallel six-degree-of-freedom forward solution calculation. In practical applications, more details and special situations may need to be considered, such as singular postures, error correction, etc. If more accurate and complete calculations are required, it is recommended to refer to relevant robotics literature or professional software.
(4) Collection of main program packaged download programs
Download link:
https://download.csdn.net/download/panjinliang066333/88421740
2. Inverse solution calculation
The inverse solution calculation of the Stewart platform refers to solving the length and angle of each actuator on the platform based on the given target position and attitude. Inverse solution calculations can be performed using numerical or analytical methods.
(1) Matlab inverse solution calculation
1. Matlab inverse solution calculation program 1
%Inverse solution calculation
L1=0.5;
L2=0.5;
L3=0.5;
L4=0.5;
L5=0.5;
L6=0.5;
R11=4.3322;
R12=0.2105;
R13=-1.5;
R21=0.2105;
R22=4.4237;
R23=1.366;
R31=1.5;
R32=-1.36;
R33=4.1463;
P1=[0.433;0.25;0];
P2=[0.3536;0.3536;0];
P3=[0.2500;0.4330;0];
P4=[0.433;0.25;0];
P5=[0.3536;0.3536;0];
P6=[0.2500;0.4330;0];
% Input parameters
L = [L1, L2, L3, L4, L5, L6]; % leg length
R = [R11, R12, R13; R21, R22, R23; R31, R32, R33]; % rotation matrix of the platform
P = [P1, P2, P3, P4, P5, P6]; % coordinates of the end point of the leg
% Calculate the transformation matrix from the base coordinate system to the platform coordinate system
T = [R, zeros(3,1); 0 0 0 1];
% Calculate the rotation angle of the leg
theta = zeros(1, 6); % stores the rotation angle of the leg
for i = 1:6
% Calculate the coordinates of the end point of the leg in the base coordinate system
P_base = T \ [P(:, i); 1];
P_base = P_base(1:3);
% Calculate the axis of rotation of the leg
a = L(i) * R(:, 3);
% Calculate the projection of the end point of the leg in the direction of the rotation axis in the base coordinate system
b = dot(P_base, a) * a;
% Calculate the projection of the end point of the leg in the vertical direction of the rotation axis in the base coordinate system
c = P_base - b;
% Calculate the rotation angle of the leg
theta(i) = atan2(norm(cross(a, c)), dot(a, c));
end
% print results
disp("Rotation angle of leg:");
disp(theta);
% Auxiliary function: calculate the cross product matrix
function M = skewSymmetricMatrix(v)
M = [ 0 -v(3) v(2);
v(3) 0 -v(1);
-v(2) v(1) 0 ];
end
In the program, you need to provide the length of the leg L, the rotation matrix of the platform R and the coordinates of the end point of the leg P. The program first calculates the transformation matrix T from the base coordinate system to the platform coordinate system, and then calculates the rotation angle theta of each leg based on the principle of inverse kinematics.
During the calculation process, the program first converts the coordinates of the leg’s end point to the base coordinate system, and then calculates the leg’s rotation axis and the projection of the end point in the direction of the rotation axis. Finally, based on the relationship between the rotation axis and the projection, the rotation angle of the leg is calculated.
Please note that this is just a simple example program and the specific implementation may vary based on your specific needs and the geometry of the mechanism. You may need to make appropriate modifications and extensions based on your application scenarios.
operation result
?
2. Matlab inverse solution calculation program 2
%Inverse solution calculation
R11=4.3322;
R12=0.2105;
R13=-1.5;
R21=0.2105;
R22=4.4237;
R23=1.366;
R31=1.5;
R32=-1.36;
R33=4.1463;
P1=[0.433;0.25;0];
P2=[0.3536;0.3536;0];
P3=[0.2500;0.4330;0];
P4=[0.433;0.25;0];
P5=[0.3536;0.3536;0];
P6=[0.2500;0.4330;0];
% Input parameters
R = [R11, R12, R13; R21, R22, R23; R31, R32, R33]; % rotation matrix of the platform
P = [P1, P2, P3, P4, P5, P6]; % coordinates of the end point of the leg
% Calculate the length of the leg
L = zeros(1, 6); % stores the length of the legs
for i = 1:6
fprintf("Calculate the length of the %dth leg:\
", i);
% Step 1: Calculate a_i
a = R(:, 3);
fprintf("Step 1: a_%d = R(:, 3) =\
", i);
disp(a);
% Step 2: Calculate b_i
b = P(:, i);
fprintf("Step 2: b_%d = P%d =\
", i, i);
disp(b);
% Step 3: Calculate L_i
L(i) = norm(b - a);
fprintf("Step 3: L_%d = norm(b_%d - a_%d) = %.4f\
", i, i, i, L(i));
fprintf("\
");
end
% print results
disp("Leg length:");
disp(L);
In the program, you need to provide the length of the leg L, the rotation matrix of the platform R and the coordinates of the end point of the leg P. The program will follow the steps to calculate the length of each leg and give the intermediate results for each step.
Please note that this is just a sample program and the exact implementation may vary based on your specific needs and the geometry of the mechanism. You can modify the program as needed to add additional calculation steps or output results.
Calculation results
?
3. Matlab inverse solution calculation program three
%Inverse solution calculation
R11=4.3322;
R12=0.2105;
R13=-1.5;
R21=0.2105;
R22=4.4237;
R23=1.366;
R31=1.5;
R32=-1.36;
R33=4.1463;
P1=[0.433;0.25;0];
P2=[0.3536;0.3536;0];
P3=[0.2500;0.4330;0];
P4=[0.433;0.25;0];
P5=[0.3536;0.3536;0];
P6=[0.2500;0.4330;0];
% Input parameters
R = [R11, R12, R13; R21, R22, R23; R31, R32, R33]; % rotation matrix of the platform
P = [P1, P2, P3, P4, P5, P6]; % coordinates of the end point of the leg
% Calculate the length of the leg
L = zeros(1, 6); % stores the length of the legs
for i = 1:6
fprintf("Calculate the length of the %dth leg:\
", i);
% Step 1: Calculate a_i
a = R(:, 3);
fprintf("Step 1: a_%d = R(:, 3) =\
", i);
disp(a);
% Step 2: Calculate b_i
b = P(:, i);
fprintf("Step 2: b_%d = P%d =\
", i, i);
disp(b);
% Step 3: Calculate L_i
L(i) = norm(b - a);
fprintf("Step 3: L_%d = norm(b_%d - a_%d) = %.4f\
", i, i, i, L(i));
fprintf("\
");
end
% print results
disp("Leg length:");
disp(L);
In the program, you need to provide the length of the leg L, the rotation matrix of the platform R and the coordinates of the end point of the leg P. The program will follow the steps to calculate the angle and length of each leg and give the intermediate results for each step.
Please note that this is just a sample program and the exact implementation may vary based on your specific needs and the geometry of the mechanism. You can modify the program as needed to add additional calculation steps or output results.
Program running process
?
result
?
(2) Inverse solution calculation of C# program
1. Project download link
Project download link:
https://download.csdn.net/download/panjinliang066333/88422020
2. Inverse solution operation calculation
Given the platform vertex coordinates, six connecting rod lengths and angles, calculate the rotation matrix and position matrix.
The following is a sample program written in C# for calculating the forward solution of Stewart’s parallel six degrees of freedom:
main running program
?
Program running results
?
(3) Inverse solution program package and download program collection
Download link:
https://download.csdn.net/download/panjinliang066333/88422020