Generally, a robot calibration process consists of four steps: error modeling, data acquisition (DAQ), parameter identification, and validation. Error modeling is a key step in calibration. An error model is established to describe the relationship between the kinematic-parameter errors and the joint coordinate errors or end-effector pose errors. In this study, only geometric errors are considered in the error modeling. Thus, the non-geometric errors arising from backlash, gear transmission, thermal distortion, and compliance are ignored. We assume that all the universal joints and spherical joints in the mechanism are perfect joints; i.e., the rotation axes of a composite joint intersect at one point. Therefore, the kinematic errors comprise the positional deviations of the passive joints and the length errors of the linkages.
3.1 General Error Modeling Method Based on Inverse Kinematics
An objective function for the error model can be derived according to either inverse kinematics or forward kinematics. The closed-form forward kinematic solution of a parallel mechanism is difficult to derive. Hence, we formulate the objective function by comparing the actual joint coordinates with the values computed by the inverse kinematic model.
For brevity, we assume that the inverse kinematic solution of a parallel mechanism is represented as follows:
$${\varvec{q}} = I\left( {{{\varvec{\uprho}}},{{\varvec{\upchi}}}} \right),$$
(20)
where \({{\varvec{\uprho}}}\) is a vector including all the geometric parameters related to the kinematic model, \({{\varvec{\upchi}}}\) is a vector representing the pose of the end effector, and \({\varvec{q}}\) is a vector representing the joint coordinates.
For the kth calibration pose, the joint coordinate residuals of a parallel mechanism are formulated as follows:
$$\Delta {\varvec{q}}_{k} = {\varvec{q}}_{k} - I\left( {{{\varvec{\uprho}}} + \Delta {{\varvec{\uprho}}},{{\varvec{\upchi}}}_{mk} } \right),$$
(21)
where \({{\varvec{\uprho}}}\) represents the nominal kinematic parameters of the parallel mechanism, and \(\Delta {{\varvec{\uprho}}}\) represents the kinematic-parameter errors to be identified. \({{\varvec{\upchi}}}_{mk}\) represents the actual pose of the end effector, which can be determined using an external measurement device. \({\varvec{q}}_{k}\) represents the command joint coordinates, which are calculated by substituting the nominal kinematic parameters \({{\varvec{\uprho}}}\) and target calibration poses \({{\varvec{\upchi}}}_{tk}\) into the inverse kinematic solution. The subscript k denotes the calibration configuration number.
$${\varvec{q}}_{k} = I\left( {{{\varvec{\uprho}}},{{\varvec{\upchi}}}_{tk} } \right).$$
(22)
Ignoring the servo error of each actuator, \({\varvec{q}}_{k}\) is used as the actual joint coordinates. The mapping between the kinematic errors and the joint coordinate residuals is performed for each calibration configuration.
Typically, the dimension of \(\Delta {{\varvec{\uprho}}}\) is larger than the dimension of the end-effector pose. Thus, additional configurations are needed to construct sufficient equations for parameter identification. Considering all the calibration configurations, the overall joint coordinate residuals are expressed as a system of nonlinear equations for the kinematic errors \(\Delta {{\varvec{\uprho}}}\) as follows:
$$f\left( {\Delta {{\varvec{\uprho}}}} \right) = \left[ {\begin{array}{*{20}c} {\Delta {\varvec{q}}_{1} } \\ {\Delta {\varvec{q}}_{2} } \\ \vdots \\ {\Delta {\varvec{q}}_{n} } \\ \end{array} } \right],$$
(23)
where n represents the number of calibration configurations.
The kinematic-parameter identification process involves finding a set of geometric parameters that minimizes the inverse kinematic residuals given the actual end-effector poses. This is a nonlinear least-squares problem, which is expressed as follows:
$$\Delta {{\varvec{\uprho}}} = \arg \mathop {\min }\limits_{{\Delta {{\varvec{\uprho}}}}} \left\| {f\left( {\Delta {{\varvec{\uprho}}}} \right)} \right\|_{2}^{2} .$$
(24)
Eq. (20) is linearized as
$$\Delta {\varvec{q}}_{k} = {\varvec{J}}_{e,k} \Delta {{\varvec{\uprho}}},$$
(25)
where \({\varvec{J}}_{e,k}\) is referred to as the error Jacobian mapping of the kinematic errors to the joint coordinate errors.
Applying Eq. (25) to all the calibration configurations yields
$$\left[ {\begin{array}{*{20}c} {\Delta {\varvec{q}}_{1} } \\ {\Delta {\varvec{q}}_{2} } \\ \vdots \\ {\Delta {\varvec{q}}_{n} } \\ \end{array} } \right] = \left[ {\begin{array}{*{20}c} {{\varvec{J}}_{e,1} } \\ {{\varvec{J}}_{e,2} } \\ \vdots \\ {{\varvec{J}}_{e,n} } \\ \end{array} } \right]\Delta {{\varvec{\uprho}}} = {\varvec{J}}_{e} \Delta {{\varvec{\uprho}}}.$$
(26)
Here, \({\varvec{J}}_{e}\) is referred to as the identification Jacobian matrix. Eq. (24) can be solved using the Levenberg–Marquardt algorithm, in which the identification Jacobian matrix is used to perform iterative calculations [30, 31].
3.2 Error Model of Parallel Leg Mechanism
The inverse kinematic solution of the parallel leg mechanism is determined using Eqs. (3, 5, 11 and 12). The kinematic parameters involved in these equations are \({\varvec{u}}_{1j}\), \({{\varvec{\Theta}}}_{j}\), \({\varvec{u}}_{2j}\), \({\varvec{u}}_{3j}\), \({\varvec{s}}_{fj}\), \({\varvec{s}}_{2j}\), and \({\varvec{s}}_{3j}\).
Inspired by the IPS method proposed by Hu [24] and the two-step calibration method proposed by Chen [29], we divide these kinematic errors into three groups that can be identified sequentially.
3.2.1 Group 1: Pose Errors of the Hip Frame with Respect to the Body Frame
The kinematic parameters in Eq. (3) correspond to the position and orientation of the hip frame \(\{ H_{j} \}\) with respect to the body frame \(\{ B\}\), i.e., \({\varvec{u}}_{1j}\) and \({{\varvec{\Theta}}}_{j}\). The foot-tip is confined to a spherical surface, with its center coincident with \(U_{1j}\) when the actuator in the UP limb is locked. Hence, the actual value of \({\varvec{u}}_{1j}\), which is denoted as \({\tilde{\varvec{u}}}_{1j}\), can be directly identified via spherical fitting. A symbol with a tilde on top denotes the actual or identified value of this variable. This notation is used hereinafter. Thus, the position errors of the hip frame can be easily acquired as follows:
$$\Delta {\varvec{u}}_{1j} = {\tilde{\varvec{u}}}_{1j} - {\varvec{u}}_{1j} .$$
(27)
There are two methods for modeling the orientation error \(\Delta {{\varvec{\Theta}}}_{j}\) according to different definitions of the actual hip frame \(\{ \tilde{H}_{j} \}\), as shown in Figure 4. In the figure, the symbol with a tilde represents the actual value, whereas the symbol without a tilde represents the nominal value.
Method 1: The orientation error \(\Delta {{\varvec{\Theta}}}_{j}\) is considered, implying that the orientation of \(\{ \tilde{H}_{j} \}\) is determined by the actual positions of \(U_{2j}\) and \(U_{3j}\), according to the definition of \(\{ H_{j} \}\) in Section 2.1. Thus, the position errors of \(\tilde{U}_{2j}\) and \(\tilde{U}_{3j}\) with respect to the \(\{ \tilde{H}_{j} \}\) frame must satisfy the following constraints:
$$\begin{array}{*{20}l} {\Delta u_{2jx} = 0,} \hfill \\ {\Delta u_{3jx} = 0,} \hfill \\ {\Delta u_{2jz} = \Delta u_{3jz} .} \hfill \\ \end{array}$$
(28)
Accordingly, \(\Delta {\varvec{u}}_{2j}\) and \(\Delta {\varvec{u}}_{3j}\) have three total independent variables in the error model.
Method 2: The orientation error \(\Delta {{\varvec{\Theta}}}_{j}\) is ignored, implying that the orientation of \(\{ \tilde{H}_{j} \}\) is identical to the nominal value:
$$\Delta {{\varvec{\Theta}}}_{j} = {\varvec{0}},\;{\text{and}}\;\widetilde{{{\varvec{\Theta}}}}_{j} = {{\varvec{\Theta}}}_{j} .$$
(29)
Thus, frame \(\{ \tilde{H}_{j} \}\) is acquired by translating \(\{ H_{j} \}\) from \(U_{1j}\) to \(\tilde{U}_{1j}\) without rotation. As such, \(\tilde{U}_{2j}\) and \(\tilde{U}_{3j}\) may not be located in the yz-plane of the frame \(\{ \tilde{H}_{j} \}\), and the constraints in Eq. (28) are invalid. Hence, there are six independent variables among \({\varvec{u}}_{2j}\) and \({\varvec{u}}_{3j}\).
In both methods, there are a total of nine independent variables among \(\Delta {\varvec{u}}_{1j}\), \(\Delta {{\varvec{\Theta}}}_{j}\), \(\Delta {\varvec{u}}_{2j}\), and \(\Delta {\varvec{u}}_{3j}\). The definition of frame \(\{ \tilde{H}_{j} \}\) affects the values of \(\alpha_{1j}\) and \(\beta_{1j}\) in Eqs. (9) and (10) but does not affect the final solution of the joint coordinates. Method 2 is used to define the actual hip frame \(\{ \tilde{H}_{j} \}\), because it is easier to solve a least-squares problem without constraint equations. Thus, three independent variables, i.e., the actual coordinates of the hip center \({\tilde{\varvec{u}}}_{1j}\), are identified in this step.
3.2.2 Group 2: Remaining Kinematic Errors of the UP Limb
After \({\tilde{\varvec{u}}}_{1j}\) is identified in the last step, \({\tilde{\varvec{u}}}_{1j}\) and the measured foot-tip position \({}^{B}{\tilde{\varvec{p}}}_{fj}\) are substituted into Eq. (3) to update \({}^{H}{\tilde{\varvec{p}}}_{fj}\). Then, \({}^{H}{\tilde{\varvec{p}}}_{fj} (\tilde{x}_{j} ,\tilde{y}_{j} ,\tilde{z}_{j} )\) is substituted into Eq. (8), and the objective function of the UP limb is established by comparing the actual limb length \(l_{1j}\) and computed limb length, as follows:
$$\begin{aligned} \Delta l_{1j} \, = \, & l_{1j} + \left( {s_{fjx} + \Delta s_{fjx} } \right) \\ & - \sqrt {\tilde{x}_{j}^{2} + \tilde{y}_{j}^{2} + \tilde{z}_{j}^{2} - \left( {s_{fjy} + \Delta s_{fjy} } \right)^{2} - \left( {s_{fjz} + \Delta s_{fjz} } \right)^{2} } . \\ \end{aligned}$$
(30)
Thus, there are three kinematic errors to be identified in Group 2: \(\Delta s_{fjx}\), \(\Delta s_{fjy}\), and \(\Delta s_{fjz}\).
Squaring both sides of Eq. (5) yields
$$\left( {l_{1j} {\varvec{e}}_{1} + {\varvec{s}}_{fj} } \right)^{{\text{T}}} \left( {l_{1j} {\varvec{e}}_{1} + {\varvec{s}}_{fj} } \right) = \, {}^{H}{\varvec{p}}_{fj}^{{\text{T}}} {}^{H}{\varvec{p}}_{fj} .$$
(31)
Differentiating both sides of Eq. (31) and ignoring the measurement error of \({}^{H}{\varvec{p}}_{fj}\) yields
$$\left( {l_{1j} {\varvec{e}}_{1} + {\varvec{s}}_{fj} } \right)^{{\text{T}}} \left( {\Delta l_{1j} {\varvec{e}}_{1} + \Delta {\varvec{s}}_{fj} } \right) = 0,$$
(32)
where \(\Delta {\varvec{s}}_{fj} = \left[ {\begin{array}{*{20}c} {\Delta s_{fjx} } & {\Delta s_{fjy} } & {\Delta s_{fjz} } \\ \end{array} } \right]^{{\text{T}}}\).
By expanding Eq. (32), the relationship between \(\Delta l_{1j}\) and \(\Delta {\varvec{s}}_{fj}\) is derived as follows:
$$\Delta l_{1j} = - \left[ {\begin{array}{*{20}c} 1 & {\frac{{S_{fjy} }}{{l_{1j} + S_{fjx} }}} & {\frac{{S_{fjz} }}{{l_{1j} + S_{fjx} }}} \\ \end{array} } \right]\Delta {\varvec{s}}_{fj} .$$
(33)
The coefficient matrix in Eq. (33) is referred to as the error Jacobian of the UP limb.
3.2.3 Group 3: Kinematic Errors of the UPS Limbs
In the last step, the actual coordinates of \({\varvec{s}}_{fj}\) are identified. By substituting \({\tilde{\varvec{s}}}_{fj}\) into Eqs. (9) and (10), the actual value of \({\varvec{R}}_{1j}\) is calculated. The calibration index of the UPS limb is defined as the joint coordinate error, as follows:
$$\Delta l_{2j} = l_{2j} - \left\| {{\tilde{\varvec{R}}}_{1j} \left( {\tilde{l}_{1j} {\varvec{e}}_{1} + {\varvec{s}}_{2j} + \Delta {\varvec{s}}_{2j} } \right) - \left( {{\varvec{u}}_{2j} + \Delta {\varvec{u}}_{2j} } \right)} \right\|.$$
(34)
Hence, there are six kinematic errors to be identified in Group 3, i.e., the coordinates of \(\Delta {\varvec{s}}_{2j}\) and \(\Delta {\varvec{u}}_{2j}\). Squaring both sides of Eq. (11) yields
$$\left[ {{\varvec{R}}_{1j} \left( {l_{1j} {\varvec{e}}_{1} + {\varvec{s}}_{2} } \right) - {\varvec{u}}_{2} } \right]^{{\text{T}}} \left[ {{\varvec{R}}_{1j} \left( {l_{1j} {\varvec{e}}_{1} + {\varvec{s}}_{2} } \right) - {\varvec{u}}_{2} } \right] = l_{2}^{2} .$$
(35)
Because the actual values of \(l_{1j}\) and \({\varvec{R}}_{1j}\) have been identified, their errors are ignored. Differentiating both sides of Eq. (35) and substituting \(\tilde{l}_{1j}\) and \({\tilde{\varvec{R}}}_{1j}\) into the equation yields
$$\left[ {{\tilde{\varvec{R}}}_{1j} \left( {\tilde{l}_{1j} {\varvec{e}}_{1} + {\varvec{s}}_{2j} } \right) - {\varvec{u}}_{2j} } \right]^{{\text{T}}} \left( {{\tilde{\varvec{R}}}_{1j} \Delta {\varvec{s}}_{2j} - \Delta {\varvec{u}}_{2j} } \right) = l_{2j} \Delta l_{2j} .$$
(36)
We define \({\tilde{\varvec{e}}}_{2j}\) as follows:
$${\tilde{\varvec{e}}}_{2j} = \frac{{{\tilde{\varvec{R}}}_{1j} \left( {\tilde{l}_{1j} {\varvec{e}}_{1} + {\varvec{s}}_{2j} } \right) - {\varvec{u}}_{2j} }}{{l_{2j} }}.$$
(37)
Substituting Eq. (37) into Eq. (36), \(\Delta l_{2j}\) is derived as
$$\Delta l_{2j} = \left[ {\begin{array}{*{20}c} {{\tilde{\varvec{e}}}_{2j}^{{\text{T}}} {\tilde{\varvec{R}}}_{1j} } & { - {\tilde{\varvec{e}}}_{2j}^{{\text{T}}} } \\ \end{array} } \right]_{1 \times 6} \left[ {\begin{array}{*{20}c} {\Delta {\varvec{s}}_{2j} } \\ {\Delta {\varvec{u}}_{2j} } \\ \end{array} } \right]_{6 \times 1} .$$
(38)
The coefficient matrix in Eq. (38) is the error Jacobian of the UPS limb.
Finally, \(\Delta {\varvec{s}}_{2j}\) and \(\Delta {\varvec{u}}_{2j}\) can be identified using the least-squares method to minimize \(\Delta l_{2j}\). \(\Delta {\varvec{s}}_{3j}\) and \(\Delta {\varvec{u}}_{3j}\) can also be identified using the same procedure in this step, because both UPS limbs have the same kinematic architecture.
Therefore, there are a total of 18 independent kinematic errors for the 2-UPS & UP parallel leg mechanism.
3.3 Error Model of Parallel Manipulator
By combining Eqs. (15) and (18), a simplified representation of the inverse kinematic solution for the ith kinematic limb in the 6-PSU parallel manipulator is obtained:
$$q_{i} = I_{PSU} \left( {{\varvec{p}},{\varvec{R}},{\varvec{a}}_{i} ,{\varvec{c}}_{i} ,{\varvec{e}}_{i} ,L_{i} } \right).$$
(39)
Thus, the error model of each PSU limb is expressed as follows:
$$\Delta q_{i} = q_{i} - I_{PSU} \left( {{\tilde{\varvec{p}}},{\tilde{\varvec{R}}},{\varvec{a}}_{i} + \Delta {\varvec{a}}_{i} ,{\varvec{c}}_{i} + \Delta {\varvec{c}}_{i} ,{\varvec{e}}_{i} + \Delta {\varvec{e}}_{i} ,L_{i} + \Delta L_{i} } \right),$$
(40)
where \({\tilde{\varvec{p}}}\) and \({\tilde{\varvec{R}}}\) represent the position and orientation, respectively, of the frame \(\{ P\}\) with respect to the frame \(\{ M\}\).
The inverse kinematic residual of each limb is unrelated to the kinematic parameters of the other limbs; therefore, the error parameters of each limb can be identified independently.
Differentiating both sides of Eq. (17) and ignoring the measurement error of the pose yields
$$\left( {{\varvec{p}} + {\varvec{Ra}}_{i} - {\varvec{c}}_{i} - q_{i} {\varvec{e}}_{i} } \right)^{{\text{T}}} \left( {{\varvec{R}}\Delta {\varvec{a}} - \Delta {\varvec{c}} - {\varvec{e}}\Delta q - q\Delta {\varvec{e}}} \right) = l\Delta l.$$
(41)
We define \({\varvec{k}}_{i}\) as follows:
$${\varvec{k}}_{i} = {\varvec{p}} + {\varvec{Ra}}_{i} - {\varvec{c}}_{i} - q_{i} {\varvec{e}}_{i} .$$
(42)
Substituting Eq. (42) into (41) yields
$$\Delta q_{i} = \left[ {\begin{array}{*{20}c} {\frac{{{\varvec{k}}_{i}^{{\text{T}}} {\varvec{R}}}}{{{\varvec{k}}_{i}^{{\text{T}}} {\varvec{e}}_{i} }}} & { - \frac{{{\varvec{k}}_{i}^{{\text{T}}} }}{{{\varvec{k}}_{i}^{{\text{T}}} {\varvec{e}}_{i} }}} & { - \frac{{q_{i} {\varvec{k}}_{i}^{{\text{T}}} }}{{{\varvec{k}}_{i}^{{\text{T}}} {\varvec{e}}_{i} }}} & { - \frac{1}{{{\varvec{k}}_{i}^{{\text{T}}} {\varvec{e}}_{i} }}} \\ \end{array} } \right]\left[ {\begin{array}{*{20}c} {\Delta {\varvec{a}}_{i} } \\ {\Delta {\varvec{c}}_{i} } \\ {\Delta {\varvec{e}}_{i} } \\ {\Delta l_{i} } \\ \end{array} } \right].$$
(43)
As \({\varvec{e}}_{i}\) is a unit vector, the following constraint must be satisfied:
$$\left\| {{\varvec{e}}_{i} } \right\| = 1.$$
(44)
Hence there are only two independent variables in \(\Delta {\varvec{e}}_{i}\). We define \(\Delta {\varvec{e}}_{i}\) as follows:
$$\Delta {\varvec{e}}_{i} = \left[ {\begin{array}{*{20}c} {\Delta e_{ix} } & {\Delta e_{iy} } & {\Delta e_{iz} } \\ \end{array} } \right]^{{\text{T}}} .$$
(45)
The nominal value of \({\varvec{e}}_{i}\) is \(\left[ {\begin{array}{*{20}c} 0 & 0 & 1 \\ \end{array} } \right]^{{\text{T}}}\); accordingly, we have
$${\tilde{\varvec{e}}}_{i} = \left[ {\begin{array}{*{20}c} {\Delta e_{ix} } & {\Delta e_{iy} } & {\sqrt {1 - \Delta e_{ix}^{2} - \Delta e_{iy}^{2} } } \\ \end{array} } \right]^{{\text{T}}} .$$
(46)
By combining Eqs. (45) and (46), we have
$$\Delta e_{iz} = \sqrt {1 - \Delta e_{ix}^{2} - \Delta e_{iy}^{2} } - 1.$$
(47)
As \(\Delta e_{ix}\) and \(\Delta e_{iy}\) are small, \(\Delta e_{iz} \approx 0\). Therefore, \(\Delta {\varvec{e}}_{i}\) can be expressed as follows:
$$\Delta {\varvec{e}}_{i} = \left[ {\begin{array}{*{20}c} 1 & 0 \\ 0 & 1 \\ 0 & 0 \\ \end{array} } \right]\left[ {\begin{array}{*{20}c} {\Delta e_{ix} } \\ {\Delta e_{iy} } \\ \end{array} } \right].$$
(48)
Thus, the number of kinematic-error parameters is reduced, and the nonlinear constraint in Eq. (44) can be eliminated.
The parameters can be defined as follows:
$${\varvec{A}}_{e} = \left[ {\begin{array}{*{20}c} 1 & 0 \\ 0 & 1 \\ 0 & 0 \\ \end{array} } \right],\Delta {\varvec{e}}_{i}^{*} = \left[ {\begin{array}{*{20}c} {\Delta e_{ix} } \\ {\Delta e_{iy} } \\ \end{array} } \right].$$
(49)
Substituting Eq. (48) into Eq. (43) yields
$$\Delta q_{i} = \left[ {\begin{array}{*{20}c} {\frac{{{\varvec{k}}_{i}^{{\text{T}}} {\varvec{R}}}}{{{\varvec{k}}_{i}^{{\text{T}}} {\varvec{e}}_{i} }}} & { - \frac{{{\varvec{k}}_{i}^{{\text{T}}} }}{{{\varvec{k}}_{i}^{{\text{T}}} {\varvec{e}}_{i} }}} & { - \frac{{q{\varvec{k}}_{i}^{{\text{T}}} }}{{{\varvec{k}}_{i}^{{\text{T}}} {\varvec{e}}_{i} }}{\varvec{A}}_{e} } & { - \frac{1}{{{\varvec{k}}_{i}^{{\text{T}}} {\varvec{e}}_{i} }}} \\ \end{array} } \right]_{1 \times 9} \left[ {\begin{array}{*{20}c} {\Delta {\varvec{a}}_{i} } \\ {\Delta {\varvec{c}}_{i} } \\ {\Delta {\varvec{e}}_{i}^{*} } \\ {\Delta l_{i} } \\ \end{array} } \right]_{9 \times 1} .$$
(50)
Thus, each PSU limb has nine independent kinematic errors, and the error model of the parallel manipulator has 54 parameters to be identified.