 |
  |
|||||||
 |
 |
 |
 |
William Kirkwood |  |
|||
 |
 |
 |
 |
 |
 |
 |
|
|
 |
| |
 |
|||
|
|
|
| |||||||||||||||||||||||||||||||||||||
| |||||||||||||||||||||||||||||||||||||||
|
September 2002 |
|
Creating a Stewart Platform Model Using SimMechanics (continued) Defining the Geometry Using an M-file Script Using M-file scripts and custom library blocks is a powerful technique in SimMechanics that enables you to create complicated mechanical models. We demonstrate this technique in this example. After we have created the physical components of the Stewart Platform model with SimMechanics, we need to define the specific geometry in its initial configuration and the dynamic parameters for the Stewart Platform. These variable definitions will be written in a basic MATLAB script and referenced in the dialog boxes of the blocks that we use to build the SimMechanics model. |
First, we define basic angular unit conversions and axes.
x_axis = [1 0 0];
y_axis = [0 1 0];
z_axis = [0 0 1];
Now we define the connection points on the base and top plate with respect to the world frame of reference at the center of the base plate. The definitions below represent the offset angle of 60 degrees between the base and top plate, the radii for both the base and top plate, and the initial position height of the system.
pos_top = [];
alpha_b = 2.5*deg2rad;
alpha_t = 10*deg2rad;
height = 2.0;
radius_b = 3.0;
radius_t = 1.0;
for i = 1:3,
angle_m_b = (2*pi/3)* (i-1) - alpha_b;
angle_p_b = (2*pi/3)* (i-1) + alpha_b;
pos_base(2*i-1,:) = radius_b* [cos(angle_m_b), sin(angle_m_b), 0.0];
pos_base(2*i,:) = radius_b* [cos(angle_p_b), sin(angle_p_b), 0.0];
% top points (with a 60 degree offset)
angle_m_t = (2*pi/3)* (i-1) - alpha_t + 2*pi/6;
angle_p_t = (2*pi/3)* (i-1) + alpha_t + 2*pi/6;
pos_top(2*i-1,:) = radius_t* [cos(angle_m_t), sin(angle_m_t), height];
pos_top(2*i,:) = radius_t* [cos(angle_p_t), sin(angle_p_t), height];
end
The following permutes the array of the top points so that the index in the top points and the bottom points refers to connection points for a single leg. The points in the top plate coordinate system are defined as well. The leg vectors and unit leg vectors are calculated.
body_pts = pos_top' - height*[zeros(2,6);ones(1,6)]; legs = pos_top - pos_base;
leg_length = [ ];
leg_vectors = [ ];
for i = 1:6,
leg_vectors(i,:) = legs(i,:) / leg_length(i);
Below is a loop that calculates the revolute and cylindrical axes that will be input into the joint blocks in the physical plant model. There are two revolutes for the Universal Joint block that is connected to the top plate, one cylindrical and one revolute for the linear motion of the Cylindrical Joint block, and two revolutes for the Universal Joint block that is connected to the base plate.
for i = 1:6,
rev1(i,:) = rev1(i,:) / norm(rev1(i,:));
rev2(i,:) = - cross(rev1(i,:), leg_vectors(i,:));
rev2(i,:) = rev2(i,:) / norm(rev2(i,:));
cyl1(i,:) = leg_vectors(i,:);
rev3(i,:) = rev1(i,:);
rev4(i,:) = rev2(i,:);
Each Body block needs a defined coordinate system for the center of gravity.
upper_leg = struct('origin', [0 0 0], 'rotation', eye(3), 'end_point', [0 0 0]);
for i = 1:6,
lower_leg(i).end_point = pos_base(i,:) + (3/4)*legs(i,:);
lower_leg(i).rotation = [rev1(i,:)', rev2(i,:)', cyl1(i,:)'];
upper_leg(i).origin = pos_base(i,:) + (1-3/8)*legs(i,:);
upper_leg(i).end_point = pos_base(i,:) + (1/4)*legs(i,:);
upper_leg(i).rotation = [rev1(i,:)', rev2(i,:)', cyl1(i,:)'];
Now we will calculate the inertia and mass for the top plate, bottom plate, and the legs. The density of steel has been used for this calculation:
base_thickness = 0.05;
inner_radius = 0.03;
outer_radius = 0.05;
density = 76e3/9.81; % Kg/m^3
The leg inertia and mass are calculated here in a function called inertiaCylinder, which calculates the mass and inertia of a cylinder given the density, length, and inner and outer radius of the cylinder:
0.75*leg_length(1),outer_radius, inner_radius);
[upper_leg_mass, upper_leg_inertia] = inertiaCylinder(density, ...
0.75*leg_length(1),inner_radius, 0);
The top and base plate inertia and mass are calculated here using the same function as we used for the legs, accepting inputs of density, plate thickness, and plate radius:
top_thickness, radius_t, 0);
[base_mass, base_inertia] = inertiaCylinder(density, ...
base_thickness,radius_b, 0);
Controller Strategy
In typical serial robot applications, the forward kinematics problem is easy while the inverse kinematics problem is more difficult. The forward kinematics problem calculates the position and orientation of the end effector of the robot given the joint angles while the inverse kinematics problem calculates the joint angles (multiple) given the end effector position and orientation. In the Stewart Platform, it is easy to calculate the joint angles (leg lengths) given the position and orientation of the end effector (top plate).
The basic goal of this controller is to specify the desired trajectory of the top plate in both position and orientation. We then map this desired trajectory to the corresponding trajectory in the legs using inverse kinematics. Finally, we use a lower level controller for each leg to command the leg to follow the desired trajectory. In this way, we avoid solving the difficult forward kinematics problem for the Stewart Platform.
The controller consists of two sections: the leg trajectory and the controller. The leg trajectory generates the desired leg lengths for each time step. It starts with a desired rotation and position of the top plate and calculates the desired leg lengths to achieve this. The following equation calculates the leg lengths for each leg:
where R is the
rotation matrix relating the top plate orientation with respect to the
bottom plate, Pt,i
is the attachment point of leg i in the top
plate with respect to the top plate coordinate system, P is the
position of the top plate with respect to the bottom plate, Pb,i
is the attachment point of leg i in the bottom
plate with respect to the bottom plate coordinate system, and ln,i
is the nominal length of leg i. The leg
trajectory subsystem and all of the other blocks in this subsystem
implement this equation for each of the six legs.
Simple PID Low-Level Controller
We first implement a simple low-level
controller based on the classic PID design. The input to this controller
is the actual leg position and velocity and the desired leg position. The
leg trajectory subsystem does not generate leg velocities, although a more
complicated one could. We then form an error in the position and create a
force based on the gain and integral of the error. We also provide a
velocity feedback term.
We can develop more sophisticated low-level controllers but we first need to linearize about an equilibrium point.
Part 3
The MathWorks, Inc.