In the modeling of serial robotic manipulators, the Denavit–Hartenberg (DH) convention is a widely adopted method for systematically describing the geometric relationships between consecutive links. The original formulation introduced by Denavit and Hartenberg is commonly referred to as the classical DH convention. A modified version, later proposed by John Craig, is known as the MDH convention. It is essential to clearly distinguish between these two conventions, as even minor differences in parameter definitions can result in significant discrepancies in the derived kinematic equations and their subsequent analysis.
2.2. Universal Frame Assignment Methodology
To assign frames correctly, a general method is proposed based on the relationship between consecutive joints. This method is applied from joint to i and repeated until the last joint is reached. Some preliminary concepts are introduced as follows: first, the assignment of the direction vector is defined, followed by the determination of the joint position, after which the relationships between consecutive joints are explained.
Each joint has a rotational or translational axis. To express a joint in Cartesian space, its direction and position must be defined with respect to a common coordinate frame. This coordinate frame is the reference for each joint and is defined as the world frame, which is denoted by the symbol .
Definition 1 (World Frame). The world frame, denoted by the symbol , is a common coordinate frame that serves as a universal reference for all objects in the workspace, including the position and direction of each joint.
The world frame is arbitrary and can be selected in any orientation. Throughout this study, the convention is as follows: the z-axis points up, the x-axis points right, and the y-axis is the cross product of the z- and x-axes.
Definition 2 (Direction vector of a joint). Let be a free unit vector; it is the direction vector of joint i if it is parallel to joint axis i.
The direction vector can be in either direction of the joint axis. The chosen direction determines the positive rotation or translation direction of the joint; however, it does not affect the resulting kinematic structure.
Definition 3 (Position of a joint). Let be a fixed vector with its origin at the world frame. This vector is the joint position if it lies on any point of the joint axis.
The joint position can be an arbitrary point on a joint axis because the computation of the consequent frame assignment and DH parameters will depend on the relationship between consecutive joint axes and not the joint positions. The joint position can easily be inferred by looking at a manufacturer’s datasheet of a robot, as the exact positions of the translational or rotational axes are always given.
From here on in this paper, whenever a reference frame is not specifically written in vector notation, it is always with respect to the world frame, for example,
. Finally, a joint’s axis can be defined as a parametric function of the form
where
k is a parametric variable and
is a vector with its origin on the world frame and its tip at a point on the joint axis. Note that when the parametric variable
, the vector becomes the joint position.
An example is given to provide a better understanding of the definitions. Two joints are defined and shown in
Figure 2, with their corresponding parametric functions being
where
k and
s are parametric variables,
and
are the positions of joint 1 and 2, respectively, and
and
are the direction vectors of joint 1 and 2, respectively.
The direction vector of a joint can be defined in several ways, the most intuitive of which is to define it in terms of the world frame axes. In
Figure 2, the unit vector
is aligned with the
z-axis of the world frame; thus, it can be written as
. The unit vector
is in the x–z plane of the world frame and has an angle of 45° with respect to the
x-axis. Thus, it has equal components in both x and z directions, which can be written as
.
Another method to define the direction vector of a joint
is to express it with a series of rotations. For example, the vector
can be expressed as the
vector rotated along the
axis by
or as
rotated along the
by
. This is shown in the following equation:
Throughout this work, the unit vector approach will be used to define the direction vector of a joint, since the authors believe it is more intuitive. The joint position and direction vectors will now be used to compute the DH frames and DH parameters. However, before placing the frames, the relationship between consecutive frames defines a framework in terms of how the origin and x-axis should be placed. In 3D space, subsequent joint axes can be in four states with each other, as follows: skew, intersecting, parallel, or collinear.
To check how consecutive joint axes are related to each other, some cases are introduced.
Characterization 1 (Geometric Conditions Between Joint Axes).
Let and be the direction vectors of joints i and , respectively, and letdenote the position vector from joint i to joint , expressed in the world frame.The following three geometric conditions were defined:
Condition C1 (Axis Alignment): This condition holds when the joint axes are parallel or collinear.
Condition C2 (Coplanarity): This condition holds true if the joint axes lie in the same plane.
Condition C3 (Axis Coincidence): This condition holds if the origin of joint lies along the axis of joint i.
These conditions are used throughout this study to categorize the spatial relationships between consecutive joint axes.
Now that the conditions are characterized, the state of the consecutive joints can be determined by combining these conditions. The results are presented in
Table 1.
The frame placement framework based on the geometric relationship between consecutive joint axes is introduced. The process of defining the MDH frames for the intersecting joint axes is illustrated in
Figure 3.
Figure 3 consists of four steps from left-to-right. After the position and direction vectors of joints
i and
are determined as shown in step one of
Figure 3, the origin must be placed at the intersection point, which is shown in step two. This intersection point is labeled as
and represents the origin of the MDH frame. Then, the already determined direction vector
is placed with its origin at
, fixing the vector in 3D space, as inferred from step three. The
unit vector is placed normal to the plane defined by the basis vectors
and
in either direction in step four. Although not used in computations, the
axis is defined as
. To mathematically express this process, the parametric function of the joint axes is defined as
where
k and
s are parametric variables. Substracting (
8) from (
7) yields the following matrix equality
Since the equality is overdetermined, a pseudoinverse can be taken that minimizes the least square error as follows
Now the parametric variables
k and
s are found, which can be used in either (
7) or (
8) to find the origin of the frame as
Since the joint axes are coplanar, the
x-axis is placed along the normal to this plane either positively or negatively, which fully defines the MDH frame of joint i.
The next geometric relationship involves the skew axes, which are defined as axes that do not intersect and are not parallel. The process of assigning a frame to joint
i is shown in
Figure 4 in a similar order as the previous case.
When the position and direction vectors of joints
i and
are determined, a common orthogonal line between these axes is drawn, indicated by the brown line segment in the second step of
Figure 4. The origin
is placed at the intersection of this common orthogonal line segment and joint axis
i. The origin of the
axis is fixed to
and the
axis is placed along the common orthogonal line.
The parametric functions of the joint axes are defined similarly to (
7) and (
8). The vector connecting these axes is defined as the multivariable function
To find the common orthogonal line segment, which is also the unique shortest line segment for skew axes, the minimum of (
13) must be found.
Writing (
13) in (
14) yields the following equation
The partial derivates with respect to
k and
s are written and set equal to 0 to find the parametric variables that minimize
.
This yields the following matrix equation to find the parametric variables
k and
s.
The common orthogonal line segment
is now known and the frame origin can now be obtained by substituting
s into the joint axis as follows
The
x-axis is placed along the common orthogonal line segment, which was defined as
; therefore, it can be written as follows
Next, the parallel axes were considered. A distinction between the parallel and collinear axes is made because both require different procedures to assign frames. The process for the parallel frames is shown in
Figure 5.
The process for parallel axes is similar to that of skew axes; however, the common orthogonal line segment is not unique, leading to an infinite amount of common line segments. Therefore, one must be chosen to minimize the number of additional parameters. This is commonly chosen as the position of joint i. The following steps are similar to those for skew axes: the vector’s origin is fixed at and the vector is placed along the common orthogonal.
The parametric functions are defined similarly to the previous steps and (
7) and (
8). Since the origin of the frame is chosen at the position of joint i as
the parameter
k is zero and
. This means the line segment equation can be written as follows:
Since the line segment must be orthogonal to the direction vector of joint
, the following equality can be derived
From this equality,
s can be computed by substituting (
25) in (
26) and is obtained as follows:
This yields the following expression for the
x-axis:
Collinear axes are the final geometric relationships and are the most obscure. These axes are defined as being aligned and coincident. The process of assigning the frame to joint
i is illustrated in
Figure 6.
Since the axes coincide, the origin is placed on an arbitrary point on the axes. As in the parallel axis case, the position of joint
i is preferred.
The z-axis was placed along the joint axis. The axis is placed anywhere on the plane normal to the axis, which makes it impossible to determine exactly. Therefore, for collinear axes, a secondary framework must be used for logical assignment.
The framework for assigning the axis in collinear frames is divided into the following two parts: joint i being the first joint in the robot or joint i being any joint other than joint 1 in the robot. In the former case, is checked to determine whether it is perpendicular to any of the main axes of the world frame. If it is perpendicular to any, the is chosen as that axis. If it is not perpendicular to any of the world frame axes, the axis with the smallest angle to is chosen as the axis.
In the latter case, namely, when the iteration is at a joint other than the first, the axis is chosen in the same direction as the previous axis .
The MDH frames for joints 1 through N-1 have been established, but the base frame and frame of the last joint remain undefined. To address this issue, two assumptions were introduced. The first assumption is that the base frame coincides with the MDH frame of joint 1 such that the origins and x- and z-axes are identical. The second assumption is that the final MDH frame N coincides with the position of the last joint while sharing the same x-axis orientation as the MDH frame of joint N-1.
Now that the MDH frames are obtained, the MDH parameters can be extracted using the following equations:
2.4. Workspace Analysis and Volume Computation
To provide a comprehensive kinematic analysis, the toolbox integrates functionalities for evaluating a robot’s workspace, including the computation of performance indices and the estimation of its total volume. The workspace is defined as the set of all reachable end-effector positions.
The analysis begins with the generation of a point-cloud representation of the workspace using the Monte Carlo method. This approach, implemented in C++, involves generating a large number of random joint configurations using a pseudorandom generator std::mt19937. For each configuration, the joint angles were sampled from a uniform distribution within their predefined limits. The forward kinematics based on the MDH are then computed to determine the resulting Cartesian position of the end-effector. This process yields a dense point cloud that effectively represents the robot’s reachable volume.
At each point in this cloud, the robot’s geometric Jacobian matrix
is calculated to evaluate the local performance characteristics. The Jacobian is computed column by column, where each column can be found using
where the vector
is the direction vector of joint i with respect to the base frame and can be found using the forward kinematics as the third column of the transformation matrix
, and the vector
is the position of the end-effector if it is defined. If not defined, this is taken as the position of the last frame
. The vector
is the origin of frame i with respect to the base frame, and
is an operator defined as follows
Using the defined Jacobian, two of the kinematic performance metrics can be determined as follows:
The manipulability index (
), based on Yoshikawa’s work, quantifies the robot’s ability to move its end-effector in any direction. It is calculated as the square root of the determinant of
.
A higher value indicates greater ease of motion, whereas a value of zero corresponds to a singularity.
The dexterity index, which measures the isotropy of the manipulator’s motion, is calculated as the ratio of the minimum singular value (
) to the maximum singular value (
) of the Jacobian matrix. This is equivalent to the inverse of the Jacobian condition number.
A value close to 1 indicates that the end-effector can move with equal ease in all directions, whereas a value close to 0 suggests that the robot is near a singularity.
The novel contribution of this study is a numerical method for computing the workspace volume. Because the workspace of a serial manipulator is often non-convex with potential voids, a simple convex hull would yield an inaccurate overestimation. To address this, our method employs a slicing technique combined with alpha-shapes. The procedure was as follows:
Slicing: The 3D workspace point cloud is partitioned into a series of thin, parallel slices of a uniform thickness, , along the z-axis.
2D Projection and Boundary Finding: For each slice, the points contained within it are projected onto the x–y plane. An alpha shape is then generated from the 2D point set. The alpha-shape algorithm creates a tight, potentially non-convex boundary that accurately envelops the points, effectively capturing the shape of a specific cross-section, including any internal holes. The alphaRadius is a critical parameter that controls the tightness of the boundary.
Area and Volume Summation: The area (
) of the alpha shape for each slice
i is calculated. The volume of the slice (
) is then approximated as
. The total workspace volume is estimated by summing the volumes of all slices as follows:
This method provides an accurate estimation of the true workspace volume, accommodating for the complex geometries characteristic of multi-link manipulators.
Using the MDH parameters obtained from Algorithm 3, the joint types for each joint and an optional joint angle or displacement parameter
, forward kinematics can be performed, as presented in Algorithm 4.
Algorithm 4 ForwardKinematics (MDH, cumulative) |
- Require:
with columns , , - Ensure:
, each - 1:
- 2:
for to N do - 3:
- 4:
if then - 5:
- 6:
else - 7:
- 8:
end if - 9:
- 10:
- 11:
- 12:
Append T to - 13:
end for - 14:
return
|
The input
represents the angles at the zero position. Using the obtained forward kinematics algorithm, the Jacobian can be obtained using Algorithm 5.
Algorithm 5 Jacobian computation |
- Require:
, - Ensure:
- 1:
translation of - 2:
for to N do - 3:
third column of - 4:
translation of - 5:
if then - 6:
; - 7:
else - 8:
; - 9:
end if - 10:
Set and - 11:
end for - 12:
return
J
|
The sampling of the workspace is performed as defined in Algorithm 6, where the input S represents the number of sampled joint configurations.
The computation time for Algorithm 6 greatly depends on the number of sampled joint configurations
S and the number of joints in the robot
N. For the fastest computation time, Algorithms 4–6 were implemented in C++ and compiled with MinGW-w64 into MEX files for use in MATLAB. Using the obtained end-effector positions and performance indices for each joint configuration, a visualization of the workspace can be obtained. Slices can also be viewed along the
xy,
yz, and
xz planes by indexing the positions from the algorithm at an interval of the axis normal to the sliced plane.
Algorithm 6 Workspace sampling with MDH kinematics |
- Require:
, , , - Ensure:
, , - 1:
Initialize RNG - 2:
for to S do - 3:
Sample with - 4:
- 5:
translation of last transform in - 6:
- 7:
▹ - 8:
- 9:
- 10:
- 11:
- 12:
end for - 13:
return
|
The algorithm for computing the workspace volume is presented in Algorithm 7.
Algorithm 7 Workspace volume computation |
- Require:
, , - Ensure:
- 1:
- 2:
, - 3:
- 4:
- 5:
for to do - 6:
, - 7:
- 8:
- 9:
if then - 10:
- 11:
- 12:
- 13:
end if - 14:
end for - 15:
- 16:
return
|
The proposed algorithms were directly implemented in the toolbox for workspace analysis and workspace volume computation.