**1. Introduction**

Nowadays, manufacturing industry, the most common type of robotic manipulator has six serial axes (degrees of freedom—DoFs) that are arranged in a so-called universal kinematic structure, e.g., the robots of ABB, KUKA, Fanuc, Yaskawa, and many others. In a very simplified way, the typical process for the deployment of a robot is to analyse the manufacturing process and workplace area first, followed by the choosing of a universal robotic manipulator and simulating the process. If the manipulator can reach the desired poses and fulfil the given task, further deployment can be considered.

However, using this universal kinematic structure is not always necessary, when a manipulator with fewer axes is suitable to perform the given task, or even possible, if the universal manipulator can face unavoidable collisions in an already existing environment. Additionally, they might not fulfil the desireed advanced operation conditions, such as manipulability [1] and kinematic reliability [2]. Therefore, researchers are focused on the topic of the synthesis of the kinematic structure of manipulators, which means finding such a kinematic structure that is optimal for a given task. This approach of deployment of highly customised manipulators may lead to benefits like lowered energy consumption, accelerated manufacturing process cycles, or deploying manipulators in highly dense-built workplaces. An example of such a general structure is presented by Brandstötter et al., who delivered the so-called curved manipulator (CuMa) [3] with possible modifications of its structure during the operational process [4]. This is achieved by changing the temperature in the links so they become flexible. A different approach to a deformable manipulator was taken by Xu et al. [5], where the links are composed of a few components and it is possible to change the orientation between them. Clark et al. [6] uses air pressure to change the kinematic structure of the presented malleable robot. The custom design of manipulators is

**Citation:** Huczala, D.; Kot, T.; Pfurner, M.; Heczko, D.; Ošˇcádal, P.; Mostýn, V. Initial Estimation of Kinematic Structure of a Robotic Manipulator as an Input for Its Synthesis. *Appl. Sci.* **2021**, *11*, 3548. https://doi.org/10.3390/app11083548

Academic Editor: Giuseppe Carbone

Received: 19 March 2021 Accepted: 13 April 2021 Published: 15 April 2021

**Publisher's Note:** MDPI stays neutral with regard to jurisdictional claims in published maps and institutional affiliations.

**Copyright:** © 2021 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (https:// creativecommons.org/licenses/by/ 4.0/).

desired not only in the manufacturing industry, but for example, in healthcare for helping with human upper limb rehabilitation [7] and shoulder joint rehabilitation [8].

There are two approaches to the synthesis of the kinematic structure of a manipulator. Analytically, it was solved by Hauenstein et al. [9] in the synthesis of three-revolute spacial chain for five poses. However, once a path becomes more complex and requires more degrees of freedom (more manipulator axes), the numerical approach is applied utilising optimisation algorithms. Among them, evolutionary robotics with genetic algorithms (GA) are probably the most common. Chocron et al. [10] used an adaptive multi-chromosome evolutionary algorithm to build a modular manipulator. Furthermore, GA can be used to build a manipulator from adaptive modules to perform a desired task [11]. Pastor et al. [12] compared straight, rounded, and curved mechanism links synthesised using GA. Valsamos et al. created so-called pseudo-joints (links which can be modified) and proposed a GA that tries to find the kinematic structure with the best manipulability [13]. It was later verified in an experiment with a real manipulator [14]. The synthesis of a parallel manipulator is addressed in [15]. As an example of non-industrial application, the work by Zeiaee et al. [16] deals with the optimisation of an eight-DoFs upper-limb exoskeleton.

In addition to GAs, the global optimum of an objective function can be searched with Simulated Annealing algorithm [17] or by a heuristic-guided tree search algorithm [18]. Another numerical method for finding the optimal manipulator for a given task is to search for a local minimum of an objective function. To solve this, a nonlinear programming (NLP) can be used, as it was implemented by Dogra et al. [19] for the design of a modular manipulator. In the paper, an optimal kinematic structure was proposed based on the minimisation of the joint torques. Another usage of NLP is trying to find an optimal design minimizing the path length in joint space [20].

The results of the already described papers [19,20] seem promising in their application of nonlinear programming to solve task-based custom manipulator design, however, there is one unanswered question in their work. In general, nonlinear programming traditionally requires that a starting point is given as part of the problem data, and comparative numerical testing is done using these traditional starting points [21]. In the case of robotic manipulators, if there is a given random path for a robotic manipulator, how should the starting point (initial values, initial estimation) of its kinematic structure look like?

The work [19] mentions a set of input values without any detailed explanation of how those values were determined. In [20], eight initial seeds are applied, but they are random values, which might be a cause of why no solution was found at all in some cases. In [17], they searched for a global optimum; however, the input values are also random, which may extend the time of solving the objective function. Even in a book by Ghafil et al. [22] which serves as an introduction to the optimisation of kinematic structures, the initial values for all described methods are obtained randomly without any detailed discussion. Therefore, a possible answer to the previously stated question will be addressed in this paper using multiple approaches.

In this paper, a geometric analysis to estimate kinematic structures and related calculations are proposed and discussed. The outcomes may avoid relying on randomness, which in the case of local-optimum-search algorithms may more frequently lead to convergence, making them reliable but still much faster than global-optimum-search methods. Moreover, the results can also be used in the previously mentioned algorithms using GAs, where they can serve as an optional first generation input, and in global-optimum-search algorithms, where they can reduce the time needed for the optimisation. In addition to that, the procedure may serve as an input for other custom manipulator design challenges such as collision avoidance [23].

A Denavit–Hartenberg notation [24] (standard DH parameters) was applied in the presented algorithms to obtain a general kinematic structure of a robot. The DH parameters were widely used, however, they also bring some disadvantages in the case of general structures. These are discussed later. Some algorithms behind the automatic placement of DH parameters have already been presented. In [25], a vector-algebra is applied to extract the parameters. Corke [26] creates a string of elementary translations and rotations from the user-defined base coordinate to the end-effector and factorises the string afterwards. An approach by Rajeevlochana et al. [27] is using line geometry to obtain the parameters. In addition to that, some researchers proposed their algorithms and they also identified (and verified) the DH parameters with an external sensor device in simulation [28] or experiments [29]. However, all of these algorithms were applied to already existing robots or similar devices. On the other hand, the algorithms presented in this paper are here to determine and "create" a non-existing manipulator (its kinematic structure) that can achieve a freely given pose.

#### **2. Materials and Methods**

When an algorithm is searching for a local minima of a function, the results may differ upon the choice of initial values. To find the optimal solution for two initial values with different outcomes, the cost function has to be compared afterwards. More accurate initial values can lead to fewer iterations that the algorithm needs, which can also save computing time. In this section, four algorithms of automatic assignment of DH parameters are presented, so they can serve as initial values for the synthesis of kinematic structure. The inputs are the position of the base of a robot, its tool-center point (TCP) pose, and the number of joints.

In this paper, we denote the transformation matrix between two frames as **J** with axis vectors and position coordinates as shown in Equation (1). The *n* (normal) is the X axis vector, the*o* (orientation) is Y axis vector, the*a* (approach) is Z axis vector, and the*t* (translation) is the position coordinate vector of a pose. **J** is a special Euclidean group of rigid body displacements in three dimensions (SE3) representing 3D rigid-body motion:

$$\mathbf{J} = \begin{bmatrix} \vec{n} & \vec{o} & \vec{a} & \vec{t} \\\\ 0 & 0 & 0 & 1 \end{bmatrix} \tag{1}$$

We also use unit vectors of the X, Y, and Z axes. They are denoted as*i*,*j*, and*k*: 

$$
\vec{a} = \begin{bmatrix} 1 \\ 0 \\ 0 \end{bmatrix}; \quad \vec{j} = \begin{bmatrix} 0 \\ 1 \\ 0 \end{bmatrix}; \quad \vec{k} = \begin{bmatrix} 0 \\ 0 \\ 1 \end{bmatrix} \tag{2}
$$

For visualisation and work with SE3 groups, we used the MATLAB®, and Robotics Toolbox that was made by P. Corke. It is described in his book [30] and accessible as open source Github repository [31].

DH parameters are the most suitable and easily applicable technique for kinematic structures that have parallel or orthogonal axes. The typical procedure is that one has a robot and places the coordinate frames following the convention [24]. However, what to do when there is a given pose that is needed to be reached while no robot is chosen yet? This is the problem for the synthesis of kinematic structure. There is one important issue related to DH parameters. Between two general poses (right-hand rule following coordinate frames), it is uncertain if the transformation matrix **J***<sup>i</sup>*−1,*<sup>i</sup>* from (*i* − 1)th pose to *i*th pose can be achieved following the typical procedure as the multiplication of a rotation matrix of *θi* around the *zi*−1 axis, the translation matrix of *di* along the *zi*−1 axis, the translation matrix of *ai* along hte *xi* axis, and the rotation matrix of *αi* around the *xi* axis, as shown in the following equations:

$$\text{Rot}(z\_{i-1}, \theta\_i) = \begin{bmatrix} \cos \theta & -\sin \theta & 0 & 0 \\ \sin \theta & \cos \theta & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \tag{3}$$

$$\text{Trans}(z\_{i-1}, d\_i) = \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & d\_i \\ 0 & 0 & 0 & 1 \end{bmatrix} \tag{4}$$

$$\text{Trans}(x\_i, a\_i) = \begin{bmatrix} 1 & 0 & 0 & a\_i \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \tag{5}$$

$$\text{Rot}(\mathbf{x}\_i, \mathbf{a}\_i) = \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & \cos \alpha & -\sin \alpha & 0 \\ 0 & \sin \alpha & \cos \alpha & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \tag{6}$$

$$\mathbf{J}\_{i-1,i} = \text{Rot}(z\_{i-1}, \theta\_i) \operatorname{Trans}(z\_{i-1}, d\_i) \operatorname{Trans}(x\_i, a\_i) \operatorname{Rot}(x\_i, a) \tag{7}$$

It is clear that rotation and translation around and along the *y* axis are missing to achieve all possible poses. In DH convention, it is mitigated by placing the joint coordinate frames in a specific way and applying Equation (7), as explained in [24]. However, if there are two general poses (*i* − 1)th and *i*th which do not follow DH convention, it is possible to find a common perpendicular between their two *zi*−1 and *zi* axes. Please see Figure 1. Rotation around *zi*−1 to the direction of the perpendicular is *θi*. *di* is the distance from the *xi*−1 axis to the intersection point *P* of the perpendicular and *zi*−1 axis. The distance along the perpendicular is equal to the *ai* distance between these two frames. Finally, the rotation around the *xi* axis to the direction of *zi* is *αi*. If another displacement of *di*+<sup>1</sup> is added and a rotation *θi*+<sup>1</sup> is applied, the previously unreachable (by four DH parameters) general pose *i*th becomes achievable by four DH parameters of (*i* − 1)th joint and two DH parameters *di*+<sup>1</sup> and *θi*+<sup>1</sup> of the *i*th joint. It can also represent an end-effector coordinate frame if the (*i* − 1)th joint was the last joint. This approach is presented in detail in [27,29]. For the following calculations, we enhanced a script made by Brodsky [32] to find a common perpendicular and intersection points *P* and *Q*. This can be found in the Supplementary Material of this paper.

We used 3 poses to demonstrate the strong and weak points of the four presented algorithms to synthesise manipulators guiding their end-effector through the given position or pose, so everyone can choose the right solution for its implementation. They are also compared in Section 3 by manipulability and arm length. The first pose is a general one. The second pose is also general, but with a small offset between its Z axis and the Z axis of the base frame. The third pose has the parallel Z axis with the base Z axis. The poses are visualised in Figure 2:

$$Pose(1) = \begin{bmatrix} -0.50 & -0.18 & -0.84 & -1.0 \\ -0.06 & 0.98 & -0.17 & 0.9 \\ 0.86 & -0.02 & -0.50 & 0.8 \\ 0 & 0 & 0 & 1 \end{bmatrix} \tag{8}$$

$$Pose(2) = \begin{bmatrix} 0.81 & -0.34 & -0.46 & -0.4\\ 0.48 & -0.02 & 0.87 & 0.6\\ -0.30 & -0.93 & 0.14 & 0.7\\ 0 & 0 & 0 & 1 \end{bmatrix} \tag{9}$$

$$Pose(3) = \begin{bmatrix} 0 & 1 & 0 & 0.4 \\ 1 & 0 & 0 & 0.6 \\ 0 & 0 & -1 & 0.7 \\ 0 & 0 & 0 & 1 \end{bmatrix} \tag{10}$$

**Figure 1.** The principle of obtaining DH parameters between two general poses. Standard DH parameters are green; additional parameters are marked with blue colour.

**Figure 2.** The poses (red) chosen for the demonstration of the working principle. Base coordinate frame has a black colour. *Pose*(1) is on the left, *Pose*(2) is in the middle, and *Pose*(3) is on the right.

Two of the four presented algorithms utilised Bézier curves (splines) which are easy to implement between two given coordinate frames. For the presented calculations, only four control points are required to define a Bézier curve. We used a script by Bai [33] to calculate the curve. The control points *P*1−<sup>4</sup> were calculated using the following equations, where*tb* is the base point coordinate,*tp* is the pose point coordinate, *ab* is the rotational vector of the Z axis of the base,*ap* is the rotational vector of the Z axis of the pose, and *p* is

the parameter related to the Euclidean distance between the base and the pose. The Bézier splines are visualised in Figure 3 for the chosen poses.

$$P\_1 = \vec{t}\_b \tag{11}$$

$$P\_2 = \vec{t}\_b + p\vec{a}\_b\tag{12}$$

$$P\_{\mathbb{S}} = \vec{t}\_{\mathbb{P}} - p\vec{a}\_{\mathbb{P}} \tag{13}$$

$$P\_4 = \tilde{t}\_p \tag{14}$$

$$p = \frac{||\vec{t}\_p - \vec{t}\_b||}{2} \tag{15}$$

**Figure 3.** Bézier splines (blue curves) between the poses and base; control points are shown as red circles.

The generated kinematic structures that have served as examples in this paper have 4 joints; however, the presented algorithms are general and can provide a solution from 3 to an unlimited number of joints. In the following subsections, all 4 procedures are presented. The types A and B only deal with the position (translational part) of a given pose, so they do not fulfil the given orientation. However, this might be enough in some cases. The other two types C and D are able to achieve a pose including orientation using the common perpendicular approach, but in some specific poses it generates structures with joints in collision. The A and C types are obtained using vector algebra only, and the B and D types use Bézier's curve approximation.

Three variables are input for all presented algorithms. It is the transformation of the robot base, the transformation of the TCP pose, both with respect to the world coordinate frame, and the desired number of joints. In our case, the base is an identity matrix. We used 3 transformations of the poses presented before, and the number of joints is four, as already said.

#### *2.1. Type A—The Nearest Distance to Achieve a Position*

This simple structure is obtained by finding the distance between the poses projected into the XY base plane. Only a positional vector of a pose is reached while the orientation is not taken into account. The implementation of such a structure is easy and straightforward. The idea is presented in Figure 4:

**Figure 4.** The schematic of type A estimation; the base frame is black, the pose frame is red, and the end-effector frame is green and does not fulfil the orientation of the pose.

The first step is to find the length of *<sup>a</sup>*0..*n*—the distance between joints in the *X* axis direction. The length is the projection of*tp*, the pose position vector, in the XY plane of the base, so only the X and Y coordinates are applied in Equation (16). *n* is the number of joints:

$$a\_{1\dots n} = \frac{\sqrt{(\vec{t}\_{p,x} - \vec{t}\_{b,x})^2 + (\vec{t}\_{p,y} - \vec{t}\_{b,y})^2}}{n} \tag{16}$$

Then, find the length of *d*1—the offset of the joint along the *Z* axis. Only the Z axis coordinates of the two position vectors are applied:

$$d\_1 = \vec{t}\_{p,z} - \vec{t}\_{b,z} \tag{17}$$

The *θ*1..*n* are joint variables, and their offset is set to 0 degrees. The other parameters, *d*2..*n* and *α*1..*n* can be set either to zero or they can be freely defined as ±values, for example. We chose *α*1 = *π*/4, *α*2 = −*<sup>π</sup>*/4, etc. One must be careful in the case of an even/odd number of joints—the sum of such tweaks needs to be equal to zero.

#### *2.2. Type B—Joints on Bézier Curve to Achieve a Position*

This method places joints between the base and the pose on a Bézier curve. To be able to obtain DH parameters, the proposed algorithm is orienting the (*i* − 1)th joint (its rotational matrix) in a way that the *i*th joint lies in the XZ plane of the previous joint. The schematic is shown in Figure 5:

Using Equations (11)–(15), the Bézier spline is approximated between a given base and a pose. The number of approximated points is equal to the number of joints *n*. *Q*1..*n* is the set of these points–coordinates of each point with respect to the base frame.

Let us define a set **J**1..*n* of SE3 objects representing the translation and orientation of the joints in the manipulator's default position. As a first step, all **J**1..*n* are set to be equal to the given base (in our case, an identity matrix). We also define an SE3 object **J***n*+<sup>1</sup> representing the given end-effector pose. Now, for joints **J**2..*n*, the following procedure is done.

**Figure 5.** The schematic of type B estimation; the base frame is black, the pose frame is red, and the end-effector frame is green and does not fulfil the orientation of the pose. Bézier spline is shown as a light blue curve.

The *i*th joint is equal to the previous (*i* − 1)th joint:

$$\mathbf{J}\_{i} = \mathbf{J}\_{i-1} \tag{18}$$

The frame **J***i* is translated on the Bézier curve changing its translation vector*ti*:

$$
\vec{t}\_i = \mathbb{Q}\_i \tag{19}
$$

The position vector*ti* of **J***i* is expressed in the coordinate frame of the previous **J***<sup>i</sup>*−<sup>1</sup> using its inverse matrix:

$$
\vec{t'}\_i = \mathbf{J}\_{i-1}^{-1} \vec{t}\_i \tag{20}
$$

A projection of the*ti* vector in the XY plane of the **J***<sup>i</sup>*−<sup>1</sup> frame is determined:

$$
\vec{t}^{\prime}\vec{t}^{\prime} = \vec{t}^{\prime}\vec{i} - \begin{bmatrix} 0\\0\\\vec{i}^{\prime}\_{:,z} \end{bmatrix} \tag{21}
$$

The angle *θi* (DH parameter) between the unit vector *i* of the **J***<sup>i</sup>*−<sup>1</sup> frame and the projection of the*ti* vector is calculated as

$$\theta\_{\vec{i}} = \tan^{-1} \left( \frac{||\vec{i} \times \vec{t'}\_{\vec{i}}||}{\vec{i} \cdot \vec{t'}\_{\vec{i}}} \right) \tag{22}$$

While using a right-handed coordinate frame, it is necessary to check if an angle is rotating around an axis in the positive (counter clockwise) or negative (clockwise) direction. To determine this, we used the projection property of the dot product between the two vectors. In this case, if the dot product of the X*i* axis is in the negative direction of the Y*i*−<sup>1</sup> axis, the angle *θi* has to be multiplied by −1:

$$\theta\_{\vec{i}} = \begin{cases} -\theta\_{\vec{i}\prime} & \text{if } \vec{j} \cdot \vec{t}\prime\_{\vec{i}} < 0 \\ \theta\_{\vec{i}\prime} & \text{otherwise} \end{cases} \tag{23}$$

Now, **J***i* can be updated using the matrix multiplication:

$$\mathbf{J}\_{i} = \mathbf{J}\_{i} \text{Rot}(z\_{i-1}, \theta\_{i}) \tag{24}$$

Thanks to the known translations, *i* and *ai*, between the frames (from the Bézier curve approximation), and the calculated *θi*, only the angle *αi* is missing among the DH parameters. The steps to determine it are similar as in the case of *θ*. Following the DH convention, the *i*th and (*i* + 1)th frames are involved.

The position vector of **J***<sup>i</sup>*+<sup>1</sup> on the Bézier curve is given:

$$
\vec{t}\_{i+1} = Q\_{i+1} \tag{25}
$$

Position vector *ti*+<sup>1</sup> of **J***<sup>i</sup>*+<sup>1</sup> is expressed in the coordinate frame of the currently determining frame **J***i* using its inverse matrix:

$$
\vec{t}\_{i+1} = \mathbf{J}\_i^{-1} \vec{t}\_{i+1} \tag{26}
$$

A projection of the*ti*+<sup>1</sup> vector in the YZ plane of the **J***i* frame is calculated:

$$
\vec{t}'\_{i+1} = \vec{t}'\_{i+1} - \begin{bmatrix} \vec{t}'\_{i+1,x} \\ 0 \\ 0 \end{bmatrix} \tag{27}
$$

The angle *αi* is between the unit vector *k* of the **J***i* frame and the projection of the*ti*+<sup>1</sup> vector, calculated as the inverse tangent fraction of the cross and dot products of those two vectors: 

$$\alpha\_{i} = \tan^{-1} \left( \frac{||\vec{k} \times \vec{t'}\_{i+1}||}{\vec{k} \cdot \vec{t'}\_{i+1}} \right) \tag{28}$$

Using a right-hand rule for coordinate frames, if the dot product of the Z*i* axis is in the negative direction of the Y*i*+<sup>1</sup> axis, the angle *θi* has to be multiplied by −1. Y*i*+<sup>1</sup> is calculated as a cross product of *t i*+<sup>1</sup> and *i* vectors:

$$a\_{i} = \begin{cases} -a\_{i\prime} & \text{if } (\vec{t}^{\prime}|\_{i+1} \times \vec{t}) \cdot \vec{k} < 0\\ a\_{i\prime} & \text{otherwise} \end{cases} \tag{29}$$

Now, the final form of **J***i* that fulfils the DH convention between (*i* − 1)th and *i*th can be obtained:

$$\mathbf{J}\_{i} = \mathbf{J}\_{i} \text{Rot}(\boldsymbol{x}\_{i}, \boldsymbol{a}\_{i}) \tag{30}$$

This procedure works smoothly for all joints. However, it will probably not be possible to obtain such DH parameters between the last joint **J***n* and the given pose **J***n*+<sup>1</sup> to reach the pose with the right orientation. Therefore, this algorithm is extended as the type D estimation in Section 2.4.

#### *2.3. Type C—Achieving a Pose with Common Perpendicular*

This algorithm finds a common perpendicular between the Z axis of the base and the Z axis of the pose. The joints are placed on this perpendicular line, and the last joint is oriented in the direction of the Z axis. Both the position and orientation can be achieved using this approach, as Figure 6 shows.

At first, a common perpendicular and intersection points *P* and *Q* are determined between the Z*b* and Z*p* axes using the script made by Brodsky [32]. However, his algorithm was not providing good results if the 2 lines were parallel, so we enhanced it and added some functionality to mitigate this issue.

The next step is to calculate the angle *αsum* between those two axes:

$$\alpha\_{sum} = \tan^{-1} \left( \frac{||\vec{a\_b} \times \vec{a\_p}||}{\vec{a\_b} \cdot \vec{a\_p}} \right) \tag{31}$$

**Figure 6.** The schematic of the type C estimation; the base frame is black, the pose frame is red, and the end-effector frame is green and coincident with the pose.

Again, when using a right-handed coordinate system, it is necessary to determine whether the angle is positive or negative. We check the pose Z*i*+<sup>1</sup> axis as a projection in the base Y*i* axis:

$$\alpha\_{sum} = \begin{cases} -\alpha\_{sum} & \text{if } \vec{\sigma\_b} \cdot \vec{a\_p} < 0\\ \alpha\_{sum} & \text{otherwise} \end{cases} \tag{32}$$

*α*1..*n* is the angle between the Z axes of particular joints:

$$
\alpha\_{1\dots n-1} = \frac{\alpha\_{sum}}{n-1} \tag{33}
$$

$$
\mathfrak{a}\_{\mathfrak{n}} = 0 \tag{34}
$$

Now, we can calculate the rest of the DH parameters. *a*1..*n* is the distance between the joints. *l* is the length of a common perpendicular, and *n* is the number of joints:

$$a\_{1\dots n-1} = \frac{l}{n-1} \tag{35}$$

$$a\_n = 0\tag{36}$$

*d*0 is the distance from the base coordinate frame to the P-intersection point of the *ab* and common perpendicular. *dn* is the distance from the *Q*, the intersection point of the *ap* and a common perpendicular, to the pose coordinate frame. *dn* is also the translation of the end-effector from the last joint:

$$d\omic{o} = \left(P - \overrightarrow{t\_b}\right) \cdot \vec{a\_b} \tag{37}$$

$$d\_{1\dots n-1} = 0\tag{38}$$

$$d\_n = (\vec{t\_p} - \mathbb{Q}) \cdot d\_p^\* \tag{39}$$

*2.4. Type D—Joints on Bézier Curve while the Last Lies on Common Perpendicular to Achieve a Pose*

This method extends type B estimation by adding a common perpendicular approach, used in type C, between the two last joints. This assures reaching the pose including orientation, as shown in Figure 7. The beginning steps are the same as in type C, but only from **J**1 to **J***<sup>n</sup>*−1. The transformation of the last joint is determined using the following procedure.

**Figure 7.** The schematic of the type D estimation; the base frame is black, the pose frame is red, the end-effector frame is green and coincident with the pose. Bézier spline is shown as a light blue curve.

Again, a common perpendicular and the intersection points *P* and *Q* are found between the joint **J***<sup>n</sup>*−<sup>1</sup> and the TCP pose **J***n*+<sup>1</sup> using the already presented ways. **J***n* is set equal to **J***<sup>n</sup>*−1:

$$\mathbf{J}\_n = \mathbf{J}\_{n-1} \tag{40}$$

Angle *θn* between X*<sup>n</sup>*−<sup>1</sup> and X*n* axes is obtained:

$$\theta\_{\rm ll} = \tan^{-1} \left( \frac{||\vec{n}\_{n-1} \times \vec{PQ}||}{\vec{n}\_{n-1} \cdot \vec{PQ}} \right) \tag{41}$$

Right-hand rule check is performed:

$$\theta\_n = \begin{cases} -\theta\_{n\prime} & \text{if } \vec{\sigma}\_{n-1} \cdot \vec{PQ} < 0 \\ \theta\_{n\prime} & \text{otherwise} \end{cases} \tag{42}$$

**J***n* is rotated around Z*<sup>n</sup>*−<sup>1</sup> axis afterwards:

$$\mathbf{J}\_n = \mathbf{J}\_n \text{Rot}(z\_{n-1}, \theta\_n) \tag{43}$$

Translation of the **J***n* along the Z*n* and X*n* is obtained by changing its translational vector *tn*, and it is equal to the coordinates of point *Q*:

$$
\vec{t}\_n = \mathbb{Q} \tag{44}
$$

Angle *αn* between Z*n* and Z*n*+<sup>1</sup> axes is calculated:

$$\alpha\_n = \tan^{-1} \left( \frac{||\vec{a\_n} \times \vec{a\_{n+1}}||}{\vec{a\_n} \cdot \vec{a\_{n+1}}} \right) \tag{45}$$

Right-hand rule check, if the dot product of Z*i*+<sup>1</sup> axis is in the positive direction of the Y*i* axis, the angle *αn* has to be multiplied by −1:

$$\alpha\_{\boldsymbol{n}} = \begin{cases} -\alpha\_{\boldsymbol{n}\prime} & \text{if } \boldsymbol{\phi\_{n}^\*} \cdot \vec{\boldsymbol{a}}\_{n+1} > 0 \\ \alpha\_{\boldsymbol{n}\prime} & \text{otherwise} \end{cases} \tag{46}$$

The final transformation matrix of the last joint **J***n* is obtained:

$$\mathbf{J}\_n = \mathbf{J}\_n \text{Rot}(\mathbf{x}\_n, \mathbf{a}\_n) \tag{47}$$

From this point, when all frames **J**1..*n* representing the joints are known, it is possible to derive the DH parameters between them.
