To start, the arm in use can be classified as the "Revolute" type due to each joint consisting of rotational motion as opposed to linear actuation. Using the standard Denavit-Hartenberg convention, a free body diagram of this arm can be created by representing each of the joints as a symbol and showing the links connecting each joint as a line. It is not necessary to include the end effector (gripper) because it is not a major factor in the mathematical process, but it has been shown in the adjoining initial FBD. Also noted are the measured lengths of each linkage as well as the designated name of each joint. It can be seen that the arm has a total of five degrees of freedom, not including the horizontal movement associated with the iRobot Create base or the gripping action of the end effector.
For the geometric representation, both the end effector and wrist rotate servos can be excluded as they do not effect the positioning of the arm. Also to be excluded from the initial system is the base rotation. These exclusions leave three joints to work with. These joints are first shown with each in its base position of zero degrees before be more generally represented.
Positioning as Θ1, Θ2, and Θ3 are equal to zero degrees.
This representation can be further simplified by a more generic drawing of the links and joints. In this final progression of the geomtric model, a few new variables are introduced to signify the height of the arm base (ho) as well as the height (H) and distance (D) from the base to an object in space.
If the end goal here was amanipulator capable of grasping any given object with the ease of a human being, this model would not be feasible, but for the practical purposes of this project, there are only two approaches that actually need to be considered. The first (and most likely) scenario is that the gripper will approach the object parallel to the ground. This means that Link L3 would also be parallel to the ground and can be omitted from the equations. The second scenario is that the gripper approaches an object from directly above such that link L3 would be perpendicular to the ground plane and can be again omitted from the equations.
The Parallel Assumption
At this point, a few new variables should be introduced. The coordinates (Da, Ha) represent the respective distance and height of the arm need to be positioned at the translated points while (D, H) represents the respective distance and height of the original grasping point.
While some may view trig as a dirty word, it is an awesome tool in solving these types of math problems without needing calculus or differential equations. To start, the previous geometric representation is shown again with every mentioned simplification as well as a few new variables.
If a line is drawn from the origin (arm base) to the point P, two triangles are created by the links L1 and L2, the imaginary line L, and the height (H) and distance (D) from the origin to the point P, as shown in the figure. The lower is a standard right angle triangle consisting of the legs L, Ha, and Da. The upper is formed by the legs L1, L2, and L. The solutions to Φ1 and L are given as Eq 3 and Eq 4.
The second and upper triangle is easily managed using the law of cosines.
Combining the results obtained in equations 5-7 with the variables associated with the previous figures yields a few new relationships as shown in Eq 8 and Eq 9. Finally, the entire system can be solved using only the original variables by combining everything obtained thus far. The final solutions to Θ1 and Θ2 are shown as Eq 10 and Eq 11.
To find the seemingly forgotten angle Θ3, a simple combination of the solved angles and a slight offset can be used. The amount of offset depends on the desired angle of the end effector with respect to ground.
Considering the ability of the wrist to rotate is not a factor in the positioning of the arm, it's function can be completely neglected in this system. The only remaining angle to be solved is the base rotation. This can also be easily solved by generating a second geometric model to represent the base as looked at from above. Now, the point P is not a factor of distance and height, but of the Cartesian coordinates (X, Y) where the center of the base is the origin point. The solution to the angle is given in Eq 14, and again in Eq 15 should Eq 14 result in a negative value. Lastly, the distance D is the same as that used previously.
Θ0 = arccos(Ybase / Xbase) Eq 14
Θ0 = arccos(Ybase / Xbase) + π Eq 15
D = | Ybase / sin(Θ0) | Eq 16
Obviously, this solution is not perfect. There are many restrictive factors which must be taken into consideration. For example, the formulas will work if a negative height Ha is used, but the arm cannot very well go below the ground plane unless the robot is on the edge of a cliff. Also, using a negative distance is not feasible given that only sensors are located in the front. However, there is a distinct possibility that a reasonable height and distance pair could lie outside of the operating range of the arm. For this reason, special precautions must be taken when determining the proper algorithms to control the arm in a useful manner.
With that, I will introduce a concept known as reachablilty. This is the estimated reachable area of the mounted arm, as shown in the adjacent image. While the gripper can be placed into any point in orange outlined area A, only the maroon outline area B is reachable while maintaining a ground parallel orientation in accordance to Eq 1.
The last piece of this puzzle has to be the way in which the coordinates of the object to be manipulated are supplied to the robot. This can be accomplished in many ways, but a pair of simple forward range finding sensors are be used in this project. Once again, simple procedure encompassing geometric modeling and trigonometric calculations is used.
Each sensor will return a voltage level related to the distance to an object which can be linearized to a very precise distance in millimeters. The angle between the sensor and object is also known because the robot will be in control of the servos supplying the sensor rotation. With this, the unknown Cartesian coordinates of the point P can be found with respect to each sensor.
XLET = DLcos(ΦL) Eq 14 ; YLET = DLsin(ΦL) Eq 15
XRET = DRcos(ΦR) Eq 16 ; YRET = DRsin(ΦR) Eq 17
With these coordinates found, the position of the object with respect to the base can be solved, and the resulting coordinates can be used to position the arm through the process previously described. Because the position of the sensors with respect to the base is known (I put them there) the following final equations for the object position can be obtained quite easily.
Xbase=XLET-Xdiff Eq 18 ; Ybase=YLET-Ydiff Eq 19
Xbase=Xdiff-XRET Eq 20 ; Ybase=Ydiff+YRET Eq 21
Placing these results into Equations 14 -16 will yield the values for D and Θ0. One important thing to note here is that the known value of ho can be used for the object height H. This is not because the arm is not capable of auto-positioning to any point within its reachability, but simply because the only object detection in use is not capapble of scanning vertically. Thus, the height of the object (H) becomes equal to the height of the sensors (ho). The calculated values of H and D can then be used to find the remaining unknown joint angles, Θ1, Θ2, and Θ3 using equations 1 - 13.
The actual robot arm can be translated to a two dimensional free body diagram for modeling.
Robotic arm joint angles with random positioning.
Generic Arm Modeling
The Perpendicular Assumption
(DA, HA)parallel = (D - L3, H - ho) Eq 1
(DA, HA)perpendicular = (D, H - ho + L3) Eq 2
Introducing the imaginary line 'L'
Φ1 = arctan( HA / DA ) Eq 3
L = DA / cos(Φ1) Eq 4
Θ1 = β + Φ1 Eq 8
Θ2 = ɣ - π / 2 Eq 9
Θ1 = arccos(L12 + L2 - L22)/(2L1L) + Φ1 Eq 10
Θ2 = arccos(L12 + L22 - L2)/(2L1 L2)-π/2 Eq 11
Θ3 (Parallel) = Θ1 + Θ2 Eq 12
Θ3 (Perpendicular) = Θ1 + Θ2 + π/2 Eq 13
A final model for the base angle
Reachability can be difficult to comprehend.
Object coordinates with respect to the sensors
Object coordinates with respect to the base
Insomniac Modeling and Kinematics
This page describes the system modeling and arm kinematics of
the Insomniac Robot built for my master's project in grad school.
Looking for the electo-mechanical design? Check it out!
One of the central objectives of this venture is solving the auto-positioning of the arm using as straightforward of mathematics equations as possible. There are two underlying reasons for which, the first being the most obvious - complicated math is not easy to follow. This project was meant to be repeatable and one of which something valuable can be learned. The second reason is less obvious but more practical - the controller in use does not have vast amounts of program memory, and most of what is available would be spent for all of the various functions of the running program. Here, a very straightforward process for the geometric modeling of an arm in two dimensional space is presented which should be reasonably easy to alter for various applications.