[Matlab six-degree-of-freedom robot] Construct workspace based on Monte Carlo method
Past review
[Matlab six degrees of freedom robot]
1. Establish robot model
2. Kinematics positive solution
foreword
This article mainly introduces how to implement the Monte Carlo random sampling method in MATLAB. Randomly select a large number of sampling points for each joint within its angle range, and perform forward kinematics calculations to obtain the corresponding end positions, and draw the end position points to visualize the robot workspace.
This article is divided into two parts. The first part explains the basic characteristics of the Monte Carlo Method and the robot workspace, and the second part analyzes the implementation of the code.
text
1. Overview of Monte Carlo method and robot workspace
The working space of the robot is an important indicator to evaluate the performance of the manipulator. There are three methods to analyze the working space of the manipulator: geometric construction method, analytical method and numerical calculation method.
The Monte Carlo method is one of the numerical calculation methods. This method is to randomly select a large number of sampling points to construct a complete working space of the robot as much as possible.
1. Monte Carlo method
The methods for solving the working space of the manipulator mainly includeGraphical method、analytical methodandnumerical method. However, both the graphical method and the analytical method are limited by the number of joints. For some manipulators, they cannot be accurately described. The numerical method is too computationally intensive, and the reliability of some boundary surfaces cannot be guaranteed. Therefore, an algorithm based on random probability derived from the numerical method is adopted: the Monte Carlo method.
The Monte Carlo method, also known as the statistical simulation method, is a numerical method that uses random sampling (pseudo-random numbers) to solve mathematical problems, and is widely used in engineering to describe certain random physical phenomena. This method is easy to realize the graphic display function, the calculation speed is fast and simple, and the complicated mathematical derivation and calculation process are omitted. It is suitable for solving the workspace of any articulated manipulator. There is no limit to the range of joint variables, and its error has nothing to do with the dimension.
The basic idea of applying Monte Carlo method to solve the working space of manipulator
- Each joint of the robotic arm works within its corresponding value range;
- All joints randomly traverse the value within the corresponding value range;
- The collection of all random values of the end points constitutes the workspace of the robotic arm.
2. Robot Workspace
The workspace of a robot is the total volume of space that its end effector sweeps when the robot performs all possible actions. The working space is limited by the geometry of the robot and the mechanical limits on each joint. For example, each joint of a six-degree-of-freedom robot is limited by different angles, as in previous articles: Building a robot modelfloorfunction, the different joints of the 6R robot are limited to varying degrees.
L1.qlim = [(-165/180)*pi,(165/180)*pi];
L2.qlim = [( -95/180)*pi, (70/180)*pi];
L3.qlim = [( -85/180)*pi, (95/180)*pi];
L4.qlim = [(-180/180)*pi,(180/180)*pi];
L5.qlim = [(-115/180)*pi,(115/180)*pi];
L6.qlim = [(-360/180)*pi,(360/180)*pi];
Workspaces can generally be divided into accessible workspaces and flexible workspaces .
The reachable workspace refers to the set of all points that the robot can reach, and the flexible workspace refers to the set of all points that the robot can reach with any posture. Obviously, flexible workspaces are a subset of reachable spaces. Therefore, the main content and programming focus of this article areaccessible workspace。
Second, the application of the rand function
1. rand function
For the randomness of the Monte Carlo method, we use rand()a function to represent it. The following is rand()a paraphrase of the function. For a more detailed explanation, please go to the official website of MATLAB. The syntax of the uniformly distributed random number
rand() function is as follows:
| grammar | illustrate |
|---|---|
| X = rand | Returns a uniform distribution in the interval (0,1)random number |
| X = rand(n) | returns an n×nrandom number matrix |
| X = rand(sz1,…,szN) | Returns a sz1×…×szN array of random numbers, where sz1,…,szN indicate the size of each dimension.For example: rand(3,4) returns a 3×4 matrix |
| X = rand(sz) | Returns an array of random numbers where the size vector sz specifies size(X).For example: rand([3 4]) returns a 3×4 matrix |
| X = rand(___,typename) | returnan array of random numbers of the typename data type. The typename input can be 'single' or 'double'. You can use any of the input parameters in the above syntax |
| X = rand(___,‘like’,p) | Returns an array of random numbers such as p; that is, the same object type as p. You can specify typename or 'like',但不能同时指定两者 |
| X = rand(s,___) | Generate numbers from random number stream s instead of the default global stream. To create a stream, use RandStream. Specify s followed by any combination of parameters in the syntax above, except combinations involving 'like'. This syntax does not support 'like' input |
注意！
The 'seed', 'state' and 'twister' inputs to the rand function are deprecated.
2. Randomly draw dots
After understanding the above rand()functions, the following is the random number selection of the joint angle of the robot within its angle range. The general code is as follows:
thetamin*(pi/180) + (thetamax-thetamin)*(pi/180)*rand;
The meaning of this code is: based on the minimum value of the angle range, plus a random number within the angle range, the current random angle value of the joint of the axis is obtained.
3. Constructing a workspace based on the Monte Carlo method using positive solutions of kinematics
1. Correct solution of kinematics
The code script is directly pasted here. If you are interested in knowing the correct solution of kinematics, please go to the previous article 【Matlab Six Degrees of Freedom Robot】The correct solution of kinematics
The script is as follows:
function [T06] = MODtransmatrix(theta1,theta2,theta3,theta4,theta5,theta6)
%%
%连杆偏移
d1 = 398;
d2 = -0.299;
d3 = 0;
d4 = 556.925;
d5 = 0;
d6 = 165;
%连杆长度
a1 = 0;
a2 = 168.3;
a3 = 650.979;
a4 = 156.240;
a5 = 0;
a6 = 0;
%连杆扭角
alpha1 = 0;
alpha2 = pi/2;
alpha3 = 0;
alpha4 = pi/2;
alpha5 = -pi/2;
alpha6 = pi/2;
MDH = [theta1 d1 a1 alpha1;
theta2+pi/2 d2 a2 alpha2;
theta3 d3 a3 alpha3;
theta4 d4 a4 alpha4;
theta5 d5 a5 alpha5;
theta6 d6 a6 alpha6];
T01=[cos(MDH(1,1)) -sin(MDH(1,1)) 0 MDH(1,3);
sin(MDH(1,1))*cos(MDH(1,4)) cos(MDH(1,1))*cos(MDH(1,4)) -sin(MDH(1,4)) -sin(MDH(1,4))*MDH(1,2);
sin(MDH(1,1))*sin(MDH(1,4)) cos(MDH(1,1))*sin(MDH(1,4)) cos(MDH(1,4)) cos(MDH(1,4))*MDH(1,2);
0 0 0 1];
T12=[cos(MDH(2,1)) -sin(MDH(2,1)) 0 MDH(2,3);
sin(MDH(2,1))*cos(MDH(2,4)) cos(MDH(2,1))*cos(MDH(2,4)) -sin(MDH(2,4)) -sin(MDH(2,4))*MDH(2,2);
sin(MDH(2,1))*sin(MDH(2,4)) cos(MDH(2,1))*sin(MDH(2,4)) cos(MDH(2,4)) cos(MDH(2,4))*MDH(2,2);
0 0 0 1];
T23=[cos(MDH(3,1)) -sin(MDH(3,1)) 0 MDH(3,3);
sin(MDH(3,1))*cos(MDH(3,4)) cos(MDH(3,1))*cos(MDH(3,4)) -sin(MDH(3,4)) -sin(MDH(3,4))*MDH(3,2);
sin(MDH(3,1))*sin(MDH(3,4)) cos(MDH(3,1))*sin(MDH(3,4)) cos(MDH(3,4)) cos(MDH(3,4))*MDH(3,2);
0 0 0 1];
T34=[cos(MDH(4,1)) -sin(MDH(4,1)) 0 MDH(4,3);
sin(MDH(4,1))*cos(MDH(4,4)) cos(MDH(4,1))*cos(MDH(4,4)) -sin(MDH(4,4)) -sin(MDH(4,4))*MDH(4,2);
sin(MDH(4,1))*sin(MDH(4,4)) cos(MDH(4,1))*sin(MDH(4,4)) cos(MDH(4,4)) cos(MDH(4,4))*MDH(4,2);
0 0 0 1];
T45=[cos(MDH(5,1)) -sin(MDH(5,1)) 0 MDH(5,3);
sin(MDH(5,1))*cos(MDH(5,4)) cos(MDH(5,1))*cos(MDH(5,4)) -sin(MDH(5,4)) -sin(MDH(5,4))*MDH(5,2);
sin(MDH(5,1))*sin(MDH(5,4)) cos(MDH(5,1))*sin(MDH(5,4)) cos(MDH(5,4)) cos(MDH(5,4))*MDH(5,2);
0 0 0 1];
T56=[cos(MDH(6,1)) -sin(MDH(6,1)) 0 MDH(6,3);
sin(MDH(6,1))*cos(MDH(6,4)) cos(MDH(6,1))*cos(MDH(6,4)) -sin(MDH(6,4)) -sin(MDH(6,4))*MDH(6,2);
sin(MDH(6,1))*sin(MDH(6,4)) cos(MDH(6,1))*sin(MDH(6,4)) cos(MDH(6,4)) cos(MDH(6,4))*MDH(6,2);
0 0 0 1];
T06 = T01*T12*T23*T34*T45*T56;
end
Notice! ! !
Here theta2+pi/2is because the robot has a joint offset of 90° at the second joint, so it needs to be theta2added to the above pi/2.
2. Build the workspace
MODtransmatrixCarry out an angle value limit for each joint, and then obtain the end position of the robot through the script in the kinematics positive solution in Chapter 3, Section 1, and finally plot3()draw the image through the function, and finally arrive at the approximate working space of the robot.
code show as below:
theta1min = -165;theta1max = 165;
theta2min = -95 ;theta2max = 70 ;
theta3min = -85 ;theta3max = 95 ;
theta4min = -180;theta4max = 180;
theta5min = -115;theta5max = 115;
theta6min = -360;theta6max = 360;
%
n = 30000;
x = zeros;y = zeros;z = zeros;
for i = 1:n
theta1 = theta1min*(pi/180) + (theta1max-theta1min)*(pi/180)*rand;
theta2 = theta2min*(pi/180) + (theta2max-theta2min)*(pi/180)*rand;
theta3 = theta3min*(pi/180) + (theta3max-theta3min)*(pi/180)*rand;
theta4 = theta4min*(pi/180) + (theta4max-theta4min)*(pi/180)*rand;
theta5 = theta5min*(pi/180) + (theta5max-theta5min)*(pi/180)*rand;
theta6 = theta6min*(pi/180) + (theta6max-theta6min)*(pi/180)*rand;
Tws = MODtransmatrix(theta1,theta2,theta3,theta4,theta5,theta6);
x(i) = Tws(1,4);
y(i) = Tws(2,4);
z(i) = Tws(3,4);
end
figure('color',[1 1 1]);
plot3(x,y,z,'b.','MarkerSize',0.5)
hold on
xlabel('x轴(millimeter)','color','k','fontsize',15);
ylabel('y轴(millimeter)','color','k','fontsize',15);
zlabel('z轴(millimeter)','color','k','fontsize',15);
grid on
3. Code running results
The result is as follows:
Summarize
This paper mainly introduces how to realize the method of Monte Carlo random sampling in MATLAB. A large number of sampling points are randomly selected for each joint within its angle range, and the corresponding end positions are obtained by calculating the forward kinematics. According to the drawing of the end position points, the robot workspace is constructed.
References
1. MATLAB implementation of Monte Carlo method
2. Monte Carlo method
3. What is the Monte Carlo algorithm?
4. Manipulator—Six-axis Manipulator Arm Operation Space Motion Analysis
5. Robot Workspace Visualization (Monte Carlo Method)
6. Seven Degrees of Freedom Anthropomorphic Manipulator Workspace Analysis Based on Monte Carlo Method