Innovative inverse kinematics algorithm for 6-DOF robotic manipulators with offset wrists

Abstract

This paper presents a novel numerical algorithm for inverse kinematics (IK) in robotic arms with offset wrists to address the challenges in IK solutions caused by biased parameters. Initially, the problem is simplified by categorizing robotic arms into two standard types and applying rotation or translation transformations to the terminal link. This approach is used for a robotic arm with a biased wrist, allowing for the acquisition of joint angles that approximate the desired solution. Subsequently, the initial Hessian matrix is corrected by incorporating the Jacobian matrix and regularization terms. Furthermore, the step size of the scaled memoryless augmented Broyden-Fletcher-Goldfarb-Shanno (SMABFGS) algorithm is dynamically adjusted to avoid convergence to local optima by integrating a momentum-based gradient descent (GD) method. The joint angles are iteratively refined to facilitate the convergence of the robotic arm’s end effector toward the desired pose. Finally, the algorithm’s accuracy in solving discrete end-effector poses is validated through a simulation experiment of IK with randomly sampled end poses. Additionally, a trajectory-tracking experiment is conducted on a physical robotic arm with an offset wrist to demonstrate its effectiveness and real-time performance in practical robotic operations.

Introduction

Robotic arms play a crucial role in advanced industries such as industrial manufacturing¹, medicine², and aerospace³, as well as in everyday life⁴. As a fundamental and critical issue in robotics, IK solutions are recognized as essential for precisely planning and controlling robotic arm joints.

Solution strategies for robot inverse kinematics can be broadly classified into two categories: position-based and rate-based. Classic rate-based methods, such as Resolved Motion Rate Control (RMRC) proposed by Whitney, control joint velocities by solving the differential kinematics Equation⁵. These methods are widely applied in the field of real-time rate control. However, such first-order methods typically exhibit linear convergence. Their flexibility is also limited when integrating complex, nonlinear constraints. In contrast, position-based optimization methods are used to directly solve for the joint configuration that corresponds to a target pose. This paper is aimed at the development of an efficient, position-based inverse kinematics solver. Faster convergence speeds and higher solution accuracy than first-order methods are achieved. Complex constraints, such as joint limits and singularities, can be handled flexibly within a unified framework. This makes the proposed method more suitable for high-precision, automated trajectory planning tasks.

Currently, six-degree-of-freedom (6-DOF) robotic arms are primarily designed in accordance with the Pieper criterion to ensure that the wrist structure is spherical^6,7. Industrial robotic arms, such as those produced by KUKA and FANUC, are characterized by a common feature in which the axes of the last three joints either converge at a single point or are arranged in parallel. Due to the decoupling of the robotic arm joints, the problems can be divided into position and orientation components, facilitating the analytical solution of the robotic arm’s IK. However, as application domains expand, spherical wrist robotic arms may struggle to meet the production requirements of specific industries^8,9. In tasks such as welding, painting, caregiving, and surgical procedures, robotic arms are required to handle high loads while maintaining a long horizontal reach and appropriate joint angles. Robots designed with non-spherical wrist structures, such as the Motoman EPX2050, FANUC P-250iB, and UR10, are meant for tasks like painting to facilitate the installation of spray hoses^10,11,12. Furthermore, 6-DOF serial manipulators with offset wrists exhibit higher payload capacity and flexibility than those with Euler wrist joints, where the axes of three consecutive joints intersect at a common point¹³.

Current research on offset robotic arms primarily focuses on the intersection of adjacent axes, while special cases such as three-axis non-coplanarity may also exist in offset wrist joints. This situation results in the nonlinearity of the mathematical model governing the last three axes being significantly higher than that of spherical wrists, making direct solutions using traditional algebraic and geometric methods challenging⁹. Moreover, in practical implementations, errors arising from the production and assembly of joints can prevent the complete realization of an optimal three-axis decoupled configuration. Even decoupled robotic arms may still exhibit couplings after calibration. However, the complexity, high nonlinearity, and coupling of IK equations make it challenging to compute the IK solution for 6-DOF manipulators with wrist offsets accurately and efficiently^13,14,15. Consequently, it is necessary to consider numerical methods for obtaining solutions that satisfy convergence criteria.

Numerous scholars have explored the use of numerical methods to compute IK solutions, employing iterative estimates that converge the current end-effector pose to the target pose by minimizing end-effector pose error. Commonly employed iterative methods include numerical approaches based on the inverse Jacobian matrix or the least squares method, such as the Jacobian pseudo-inverse method and its variants^16,17, as well as damped least squares methods^18,19. While their convergence performance depends on the damping factor, certain studies have proposed algorithms for selecting damping coefficients to improve the least squares method. Nonetheless, these parameters remain primarily empirical. Recently, some researchers have investigated the application of neural networks to learn the relationship between joint angles and corresponding poses, thereby facilitating the resolution of IK problems^20,21,22. However, extensive network training and the establishment of corresponding databases are required prior to utilization.

Additionally, the Broyden-Fletcher-Goldfarb-Shanno (BFGS) algorithm has also been applied to the computation of IK, employing continuous gradient assessments to derive solutions^23,24. As a second-order algorithm, BFGS exhibits rapid computation speed. It achieves quadratic convergence as it approaches the desired solution. This process effectively doubles the number of significant digits with each iteration. Nevertheless, the approximation of the Hessian matrix relies on the previous Hessian values, necessitating substantial operational memory²⁵. Some researchers have also investigated alternative approaches for replacing the Hessian matrix based on the numerical computation of the existing Jacobian. These methods implicitly construct the Hessian matrix tensor to reduce the memory and computational complexity required. However, a high level of complexity still remains²⁶.

Currently, the IK algorithms for robotic arms with offset wrists predominantly focus on specific configurations, with few algorithms available for various types of 6-DOF robotic arms featuring offset wrists^10,27,28,29. For example, the configurations involving the intersection of adjacent axes have been simplified in studies conducted by scholars such as Zhou et al²⁸. The designs of non-spherical wrists proposed by Li and Kucuk are constrained to specific configurations^8,9. Therefore, the spatial line in mathematics is considered to simulate the axes of the three joints at the end-effector of the robotic arm. An analysis is conducted on the possible types of offset linkages. This approach synthesizes the potential configurations that may exist for a 6-DOF robotic arm with an offset wrist. The transformation of virtual links is utilized to accommodate configurations involving both translational and rotational transformations, while modifications to the Hessian matrix are made to enhance convergence efficiency. A systematic classification is performed, and the issues associated with heterogeneous offsets are addressed, thereby expanding the range of applicable scenarios.

This paper proposes a novel IK algorithm for 6-DOF robotic arms with offset wrists. The algorithm effectively calculates the impact of the first three consecutive joints on the end-effector position. It also assesses the influence of the last three consecutive joints on the end-effector orientation by incorporating simplified robotic structures, an improved BFGS algorithm, and a momentum-based search strategy. Specifically, this algorithm is capable of solving the corresponding IK for robotic arms with all offset configurations. Section 2 presents the problem formulation for robotic arms with offset wrists. Section 3 proposes a simplification method for various robotic configurations, categorizing the simplified configurations into two types and deriving analytical solutions for both types of robotic arms. Section 4 provides a detailed introduction to the SMABFGS algorithm, including enhancements to the initial Hessian matrix and a momentum-based GD method. Section "Conclusion" applies the proposed algorithm to solve the IK for four categorized robotic arms with offset wrists, presenting simulated end-effector errors and computation times. Additionally, experiments involving continuous trajectory motion of the robotic arm SR4 with offset wrist are conducted to validate the algorithm’s effectiveness and real-time performance. Finally, Sect. 6 summarizes the insightful conclusions derived from the proposed algorithm for robotic arms with offset wrists.

Description of the offset wrist structures

This paper investigates robotic arms with offset wrists, in which the end-effector’s position is influenced by the positions of the first three consecutive joints, while the remaining three consecutive joints control the orientation. It is demonstrated that when a 6-DOF robotic arm satisfies the Pieper criterion, analytical solutions for IK can be obtained relatively easily. Consequently, the relationships among the three adjacent joints at the end of the robotic arm are analyzed by modeling their axes as three spatial lines.

It is well known that two spatial lines can exhibit relationships of intersection, parallelism, or being skew. Furthermore, intersecting or parallel spatial lines can define a plane. Additionally, a spatial line can be related to a plane in three ways: intersection, parallelism, or lying within the plane. When two spatial lines intersect or are parallel, they collectively define a spatial plane. In the scenario involving three spatial lines, where two lines intersect, the potential relationships may include: (1) intersecting at a single point, (2) crossing without a coincident intersection, and (3) the third spatial line being either skew to or parallel with the plane formed by the other two lines. Additionally, when two spatial lines are parallel, the relationships among the three lines include the third being skew to or parallel with the plane defined by the first two lines or intersecting with the other two. There are additional considerations in the context of two skew lines in space. Beyond the possibility of one of these lines being parallel to or intersecting with a third line, there exists a scenario where all three lines are mutually skew. In this case, the lines do not intersect and are not coplanar.

Thus, in the configuration of a 6-DOF robotic arm, when considering the spatial lines as axes of rotation, the relationships among three adjacent rotational axes may exhibit the following six forms, as illustrated in Fig. 1. The angles between the rods in the figure are not necessarily right angles. The particular positions related to right angles have been indicated in the figure using right angle symbols.

• Three adjacent joint axes intersect at a single point, as shown in Fig. 1(a);

• The three adjacent joint axes are mutually parallel, as depicted in Fig. 1(b);

• When two adjacent joint axes intersect, the remaining adjacent joint axis intersects with the previous two, but these three adjacent joint axes do not meet at a single point, as illustrated in Fig. 1(c);

• In the case where two adjacent joint axes intersect, the remaining adjacent joint axis is parallel to the plane formed by the first two adjacent joint axes, as shown in Fig. 1(d);

• When two adjacent joint axes are parallel, the remaining adjacent joint axis is parallel to the plane formed by the first two adjacent joint axes, as demonstrated in Fig. 1(e);

• The three adjacent joint axes do not intersect at all, as depicted in Fig. 1(f).

Fig. 1

Six configurations formed by three adjacent rotational axes.

Robotic arm configurations that include configuration 1 or 2 comply with the Pieper criterion, allowing for the derivation of analytical solutions for the arm’s IK. However, when any of the other four configurations occurs within the robotic arm, the severe coupling of joint parameters complicates the derivation of analytical solutions in their mathematical models^12,14,15. Therefore, a numerical approach is considered for the IK solution.

The forward kinematics equations for all the aforementioned robotic arms can be expressed as follows:

$$_{6}^{0} T = \prod\limits_{i = 1}^{6} {{}_{i}^{i - 1} T\left( {\theta_{i} } \right)}$$

(1)

where ${}_{i}^{i - 1} T\left( {\theta_{i} } \right)$∈SE(3) represents the homogeneous transformation matrix of the joint coordinate system $O_{i} - X_{i} Y_{i} Z_{i}$ with respect to the previous joint coordinate system $O_{i - 1} - X_{i - 1} Y_{i - 1} Z_{i - 1}$. This matrix can be obtained through the transformation matrix and translation vector derived from the Denavit-Hartenberg (DH) parameters²⁹.

$${}_{i}^{i - 1} {\mathbf{T}}\left( {\theta_{i} } \right) = Rot_{{X_{i - 1} }} \left( {\alpha_{i} } \right) \cdot Trans_{{X_{i - 1} }} \left( {a_{i} } \right) \cdot Rot_{{Z_{i} }} \left( {\theta_{i} } \right) \cdot Trans_{{Z_{i} }} \left( {d_{i} } \right)$$

(2)

The pose of the end effector of the robotic arm relative to the base coordinate system ${\text{O}}_{0}-{\text{X}}_{0}{{\text{Y}}}_{0}{{\text{Z}}}_{0}$ is represented by the homogeneous transformation matrix as follows:

$${\mathbf{T}}_{end} = {}_{6}^{0} {\mathbf{T}} = \left[ {\begin{array}{*{20}c} {{}_{6}^{0} {\mathbf{R}}} & {{}_{6}^{0} {\mathbf{P}}} \\ 0 & 1 \\ \end{array} } \right] = \left[ {\begin{array}{*{20}c} {\mathbf{n}} & {\mathbf{o}} & {\mathbf{a}} & {\mathbf{p}} \\ 0 & 0 & 0 & 1 \\ \end{array} } \right] = \left[ {\begin{array}{*{20}c} {n_{x} } & {o_{x} } & {a_{x} } & {p_{x} } \\ {n_{y} } & {o_{y} } & {a_{y} } & {p_{y} } \\ {n_{z} } & {o_{z} } & {a_{z} } & {p_{z} } \\ 0 & 0 & 0 & 1 \\ \end{array} } \right]$$

(3)

where ${}_{6}{}^{0}{\text{R}}$∈SO(3) denotes the rotation matrix, while its columns n, o, a∈${\mathbb{R}}$³ represent the cosine vectors of the axes of the end effector coordinate system relative to the axes of the base coordinate system. ${}_{6}{}^{0}{\text{P}}$∈${\mathbb{R}}$³ signifies the translation vector, which indicates the translation of the origin of the end effector coordinate system with respect to the origin of the base coordinate system along the respective axes.

All configurations discussed in this paper are exemplified using revolute joints, as illustrated in Figs. 1(c) to (f). Due to the potential presence of non-zero values for a_i, d_i, and cos(α_i) or sin(α_i) between two adjacent joints, the mathematical model of the robotic arm becomes complex. As a result, a direct analytical solution for the robotic arm cannot be obtained.

Simplification of offset wrist structure

Simplification of offset wrist robotic arms

To address the four configurations of the offset wrist introduced in Sect. 1, the derivation of the IK solution for the simplified structure is facilitated by employing the virtual wrist concept, as Wu et al. proposed for a 6-DOF robotic configuration¹⁰. This approach involves altering the original links’ shape and adjusting the rotational axes’ positions. Consequently, the axes of three adjacent joints are configured to either converge at a single point or remain parallel. This results in a simplified link model that adheres to the kinematic Pieper criteria for robotic systems.

Link transformations can take two forms: translation and rotation. For the transformation of the same link, the degree of transformation may vary, resulting in different degrees of similarity between the new virtual and original links. Therefore, preference is given to those with higher similarity when selecting the transformation method. When the degrees of transformation are consistent, priority is given to links closer to the end effector as well as to translation transformations. This phenomenon is attributed to the fact that a simplified configuration, derived solely from translation, maintains pose consistency with the original configuration under the same degree of transformation. The only difference noted is in positional accuracy. Thus, the criteria of minimal transformation and preference for translation transformations establish a foundational basis for the proposed novel numerical method for solving the IK problem.

Additionally, it is important to recognize that the geometric simplification methodology presented herein relies on the DH parameter framework. A core tenet of this framework is the convention that joint rotation axes are aligned with the Z-axis. While this assumption is advantageous for simplifying kinematic derivations, many robot description formats, including the widely used Unified Robot Description Format (URDF), do not strictly follow this rule. In order to ensure compatibility with robots whose kinematics are not natively described by DH parameters, a pre-processing step can be implemented to convert the original kinematic model into an equivalent DH representation.

(1) For configuration 3, since the rotational axes of links l₅ and l₆ do not coincide with the intersection point of the rotational axes of links l₄ and l₅, a translation or rotation transformation can be applied to link l₆ to modify the original robotic arm configuration. The new configuration ensures the three rotational axes intersect at a single point, as illustrated in Fig. 2.

Fig. 2

Two transformations of configuration 3.

Rotation transformation: the rotational axes Z₅ and Z₆ lie within the same plane, with the axis X₆ being perpendicular to this plane. A line segment is drawn from the origin O₆ that is perpendicular to the rotational axis Z₅, connecting the origin O₆ and the origin O₄.

$$\alpha_{{6^{1} }} = \angle O_{6} O_{4} O_{5} = \arctan \left( {\frac{{d_{6} \cdot \sin \left( {\user2{\pi - }\alpha_{{6}} } \right)}}{{d_{5} - d_{6} \cdot \cos \left( {\user2{\pi - }\alpha_{{6}} } \right)}}} \right)$$

(4)

The angle of rotation $\alpha_{{6^{2} }}$ about the axis Z₆ is defined as follows:

The angle between the representations before and after the Z₆ transformation is denoted as $\alpha_{{6^{2} }}$.

$$\alpha_{{6^{2} }} = \angle O_{5} O_{6} O_{4} = \alpha_{6} - \alpha_{{6^{1} }}$$

(5)

The link l₆ is rotated around the axis X₆ by an angle $\alpha_{{6^{2} }}$, resulting in a virtual new link denoted as ${\text{l}}_{{{6}^{{1}} }}$. Let its coordinate system be defined as ${\text{O}}_{{{6}^{{1}} }} {\text{ - X}}_{{{6}^{{1}} }} {\text{Y}}_{{{6}^{{1}} }} {\text{Z}}_{{{6}^{{1}} }}$. The rotation axis of the new link ${\text{l}}_{{6}^{1}}$ intersects the link l₄ at the origin O₄, thus satisfying the Pieper criterion for the new configuration, with the corresponding coordinate systems defined as shown in Fig. 2(a).

Additionally, along the rotation axis ${\text{Z}}_{{6}^{1}}$ of the virtual new rod’s coordinate system, a displacement of ${\text{d}}_{{6}^{1}}$ is applied.

$$d_{{6^{1} }} = d_{6} \cdot \frac{{\sin \left( {\alpha_{6} } \right)}}{{\sin \left( {\alpha_{{6^{1} }} } \right)}}$$

(6)

Therefore, assuming that the pose matrix of the end effector ${}_{6}{}^{0}{{\text{T}}}_{\text{d}}$ is known, the transformed pose matrix ${}_{{6}^{1}}{}^{0}{{\text{T}}}_{\text{d}}$ can be expressed as follows:

$${}_{{6^{1} }}^{0} {\mathbf{T}}_{d} = {}_{6}^{0} {\mathbf{T}}_{d} \cdot Rot_{{X_{6} }} \left( { - \alpha_{{6^{2} }} } \right)$$

(7)

Translation transformation: The rotational axes Z₅ and Z₆ lie within the same plane, with the axis X₆ being perpendicular to this plane. A line segment is drawn from the origin O₆ that is perpendicular to the axis Y₆, connecting the origin O₆ with the origin O₄.

$$O_{6} O_{{6^{1} }} = b_{6} = d_{5} \cdot \sin \left( {\alpha_{6} } \right)$$

(8)

By translating link l₆ along the negative direction of axis Y₆ by a distance b₆, we obtain a new virtual link denoted as ${\text{l}}_{{6}^{1}}$. Let its coordinate system be defined as ${\text{O}}_{{6}^{1}}-{\text{X}}_{{6}^{1}}{\text{Y}}_{{6}^{1}}{\text{Z}}_{{6}^{1}}$. The rotational axis of the new link ${\text{l}}_{{6}^{1}}$ intersects link l₄ at the original point O₄, thus satisfying the Pieper criteria for the new configuration, where the respective coordinate systems can be defined as illustrated in Fig. 2(b).

The distance moved along the rotational axis ${\text{Z}}_{{6}^{1}}$ of the virtual new link is denoted as ${\text{d}}_{{6}^{1}}$.

$$d_{{6^{1} }} = d_{6} - a_{5} \cdot \cos \left( {{\varvec{\pi}} - \alpha_{{6}} } \right)$$

(9)

Therefore, assuming that the pose matrix of the coordinate system ${\text{O}}_{6}-{\text{X}}_{6}{{\text{Y}}}_{6}{{\text{Z}}}_{6}$, denoted as ${}_{6}{}^{0}{{\text{T}}}_{\text{d}}$, is known, the transformed pose matrix ${}_{{6}^{1}}{}^{0}{{\text{T}}}_{\text{d}}$ can be expressed as follows:

$${}_{{6^{1} }}^{0} {\mathbf{T}}_{d} = {}_{6}^{0} {\mathbf{T}}_{d} \cdot Trans\left( {\begin{array}{*{20}c} 0 & { - b_{6} } & 0 \\ \end{array} } \right)$$

(10)

(2) For configuration 4, the rotational axis Z₆ is parallel to the plane O₃O₄O₅, which contains the rotational axes Z₄ and Z₅. Consequently, the projection of the rotational axis Z₆ onto this plane must intersect with either the rotational axis Z₄ or Z₅. Assuming that the projection of Z₆ onto the plane O₃O₄O₅ intersects the rotational axis Z₅ at point O₅, a translation transformation is applied to link l₆ as illustrated in Fig. 3, thereby transforming the original robotic arm configuration into configuration 3. Ultimately, following the transformation of configuration 3 will yield the final configuration, in which the three rotational axes intersect at a single point.

Fig. 3

Preliminary transformation of configuration 4.

There are two approaches to transforming configuration 4 into the final configuration: a translation-rotation transformation and a translation-translation transformation.

Translation-Rotation Transformation: Link l₆ is translated along the axis X₆ by a distance a₆ to intersect the rotational axis Z₅ at point O₅, resulting in a new virtual link denoted as $l_{{6^{2} }}$. The projection of the original link l₆ onto the plane O₃O₄O₅ intersects link l₅ at point O₅, forming an angle $\alpha_{6}$. This preliminary transformation of configuration 4 is depicted in Fig. 3. Since the preliminary transformed configuration conforms to configuration 3, the rotational transformation scheme of configuration 3 can be applied, as outlined in Fig. 2(a). The new virtual link ${\text{l}}_{{6}^{2}}$ is then rotated around the virtual link origin ${\text{O}}_{{{6}^{{1}} }}$ by an angle $\alpha_{{6^{2} }}$, intersecting link l₄ at O₄ and forming a new virtual link ${\text{l}}_{{{6}^{{1}} }}$.

Assuming the pose matrix of the coordinate system ${\text{O}}_{6}-{\text{X}}_{6}{{\text{Y}}}_{6}{{\text{Z}}}_{6}$, denoted as ${}_{6}{}^{0}{{\text{T}}}_{\text{d}}$, is known, the transformed pose matrix ${}_{{6}^{1}}{}^{0}{{\text{T}}}_{\text{d}}$ can be expressed as follows:

$${}_{{6^{1} }}^{0} {\mathbf{T}}_{d} = {}_{6}^{0} {\mathbf{T}}_{d} \cdot Trans\left( {\begin{array}{*{20}c} { - a_{6} } & 0 & 0 \\ \end{array} } \right) \cdot Rot_{{X_{6} }} \left( {\alpha_{{6^{2} }} } \right)$$

(11)

Translation-Translation Transformation: The link l₆ is translated along the axis X₆ by a distance a₆ to intersect the rotational axis Z₅ at point O₅, resulting in a new virtual link denoted as ${\text{l}}_{{{6}^{{2}} }}$. The projection of the original link l₆ onto the plane O₃O₄O₅ intersects link l₅ at point O₅, forming an angle $\alpha_{6}$. This preliminary transformation of configuration 4 is illustrated in Fig. 3. Since the preliminary transformed configuration aligns with configuration 3, the translation transformation scheme for configuration 3 can be applied, as depicted in Fig. 2(b). The new virtual link ${\text{l}}_{{{6}^{{2}} }}$ is then translated along the axis ${\text{Y}}_{{{6}^{{1}} }}$ by a distance b₆, intersecting link l₄ at point O₄. This is equivalent to directly translating link l₆ along a vector ${\text{b}}_{{{6}^{{1}} }}$ in the plane defined by ${\text{X}}_{{6}} {\text{O}}_{{6}} {\text{Y}}_{{6}}$, resulting in a new virtual link denoted as ${\text{l}}_{{{6}^{{1}} }}$.

Assuming the pose matrix of the end effector ${}_{{6}}^{{0}} {\text{T}}_{{\text{d}}}$ is known, the transformed pose matrix ${}_{{{6}^{{1}} }}^{{0}} {\text{T}}_{{\text{d}}}$ can be expressed as follows:

$${}_{{6^{\prime}}}^{0} {\mathbf{T}}_{d} = {}_{6}^{0} {\mathbf{T}}_{d} \cdot Trans\left( {\begin{array}{*{20}c} { - a_{6} } & 0 & 0 \\ \end{array} } \right) \cdot Trans\left( {\begin{array}{*{20}c} 0 & { - b_{6} } & 0 \\ \end{array} } \right)$$

(12)

(3) In the case of configuration 5, the rotational axes Z₄ and Z₅ are parallel to each other, while the rotational axes Z₆, Z₄, and Z₅ lie in different planes. If a translation transformation is applied to the rotational axis Z₆ to intersect it with either the rotational axis Z₄ or Z₅, it would necessitate a transformation of the other rotational axis. However, by applying a rotational transformation to the axis Z₆ to align it parallel to Z₅, the three rotational axes can be made parallel directly.

Considering the established parallel orientation of the rotational axes Z₄ and Z₅, the rotational axis Z₆ is rotated around the axis X₅ by an angle $\alpha_{6}$. This rotation achieves the parallel alignment of all three axes, as illustrated in Fig. 4.

Fig. 4

Rotational transformation of configuration 5.

Assuming the pose matrix of the end effector ${}_{6}{}^{0}{{\text{T}}}_{\text{d}}$ is known, the transformed pose matrix ${}_{{6}^{1}}{}^{0}{{\text{T}}}_{\text{d}}$ can be expressed as follows:

$${}_{{6^{\prime}}}^{0} {\mathbf{T}}_{d} = {}_{6}^{0} {\mathbf{T}}_{d} \cdot Tran\left( {\begin{array}{*{20}c} 0 & 0 & { - d_{6} } \\ \end{array} } \right) \cdot Rot_{{X_{6} }} \left( { - \alpha_{6} } \right) \cdot Tran\left( {\begin{array}{*{20}c} 0 & 0 & {d_{6} } \\ \end{array} } \right)$$

(13)

(4) For configurations that do not comply with the Pieper criteria and are not included in the three configurations mentioned above, such as those where the three rotational axes are mutually non-coplanar, it is not possible to directly transform them into configurations that satisfy the Pieper criteria using the previous transformation methods. It is necessary to consider simultaneously rotating the last two links around the axis X_i-1 to achieve mutual parallelism among the three rotational axes.

In this context, the rotational axes Z₄, Z₅, and Z₆ are mutually non-coplanar. If the rotational axis Z₅ is rotated around the axis X₄ to align it parallel with another rotational axis, this would resemble the processing of configuration 5. However, the severe coupling of axis X₄ with the other rotational axes complicates establishing a direct relationship with the end-effector pose. As a result, the application of the aforementioned handling methods is rendered infeasible.

It is proposed to reduce the angle between the rotational axes Z₄ and Z₅ to zero, i.e., α₅ = 0. The resulting configuration after transformation will be approximately the same as the initial configuration, with the primary requirement being compensation for the variation around the axis X₄ using other joint angles associated with that axis. Although the analytical solutions derived may deviate from the actual kinematic inverse solutions of the mechanism, the simplification in this section aims to provide approximate initial joint angles for subsequent algorithms. Consequently, a high precision requirement on the inverse solution of the simplified mechanism is unnecessary. After reducing the angle to zero, the mechanism can be processed according to configuration 5, resulting in a simplified mechanism where the three adjacent rotational axes are mutually parallel. The transformation process for configuration 6 is depicted in Fig. 5.

Fig. 5

Transformation processing of configuration 6.

Simplified inverse kinematics for robotic arms with offset wrist

By transforming the aforementioned different types of robotic arms with offset wrist, the resulting configurations that comply with the Pieper criteria can be categorized into two types of non-offset wrist robotic arms. The first type includes simplified configurations where three adjacent rotational axes intersect at a single point, while the second type consists of simplified configurations where three adjacent rotational axes are mutually parallel³⁰.

According to the analysis in Sect. 2.1, configurations 3 and 4 can be transformed into the first type of non-offset wrist robotic arm. In contrast, configurations 5 and 6 can be converted into the second type of non-offset wrist robotic arm.

The forward kinematics of both types of non-offset wrist robotic arms can be expressed as in (1), and the pose of the end effector is represented by (3).

Inverse kinematics for the first type of non-offset wrist robotic arm

The first type of non-offset wrist robotic arm obtained through the transformations mentioned above is represented in two virtual link forms, as illustrated in Figs. 2(a) and 2(b). When defining the coordinate systems for the new configuration, the origins of the coordinate systems ${\text{O}}_{4}-{\text{X}}_{4}{{\text{Y}}}_{4}{{\text{Z}}}_{4}$ and ${\text{O}}_{5}-{\text{X}}_{5}{{\text{Y}}}_{5}{{\text{Z}}}_{5}$ coincide. The coordinate system for the virtual link is denoted as ${\text{O}}_{{6}^{1}}-{\text{X}}_{{6}^{1}}{\text{Y}}_{{6}^{1}}{\text{Z}}_{{6}^{1}}$, with the negative direction of ${\text{Z}}_{{6}^{1}}$ intersecting the coordinate system ${\text{O}}_{4}-{\text{X}}_{4}{{\text{Y}}}_{4}{{\text{Z}}}_{4}$ at the origin O₄. Thus, the distance moved along the axis ${\text{Z}}_{{6}^{1}}$ in the DH convention is represented by ${\text{d}}_{{6}^{1}}$.

In these two virtual link forms, the final configuration allows the rotational axes of the three adjacent links at the end effector to intersect at a single point. The distinction between these two forms lies in the differing values of the movement distance ${\text{d}}_{{6}^{1}}$. When defining the coordinate system ${\text{O}}_{6}-{\text{X}}_{6}{{\text{Y}}}_{6}{{\text{Z}}}_{6}$, the origin of the virtual link coordinate system ${\text{O}}_{{6}^{1}}-{\text{X}}_{{6}^{1}}{\text{Y}}_{{6}^{1}}{\text{Z}}_{{6}^{1}}$ can be established at the intersection of the three rotational axes. For example, the first type of non-offset wrist robotic arm can be redefined to align with the traditional configuration, whereby the three axes are in accordance with the Pieper criteria, as depicted in the simplified form shown in Fig. 6. The DH parameters for the redefined robotic arm are detailed in Table 1.

Fig. 6

Schematic of the overall robotic arm with three adjacent intersecting rotational axes at the end-effector.

Table 1 DH parameters for the overall robotic arm with three adjacent intersecting rotational axes at the end-effector.

Thus, the pose matrix of the coordinate system ${\text{O}}_{{6}^{1}}-{\text{X}}_{{6}^{1}}{\text{Y}}_{{6}^{1}}{\text{Z}}_{{6}^{1}}$ at the virtual rotation center is represented as follows:

$${\mathbf{T}}_{{end^{\prime}}} = {\mathbf{T}}_{end} \cdot Trans\left( {\begin{array}{*{20}c} 0 & 0 & { - d_{{6^{\prime}}} } \\ \end{array} } \right)$$

(14)

As indicated by (14) and Fig. 6, the position of the virtual rotation center is determined by the first three rotational angles and can be expressed as follows:

$${}^{0}{\mathbf{P}}_{4org} = \prod\limits_{i = 1}^{3} {{}_{i}^{i - 1} {\mathbf{T}}} \left( {\theta_{i} } \right){}^{3}{\mathbf{P}}_{4org}$$

(15)

Similarly, as indicated by (15) and Fig. 6, the orientation of the virtual link relative to the virtual rotation center is determined by the last three rotational angles. Therefore, the relationship between the kinematics of the robotic arm and the pose of the end effector can be expressed as follows:

$${}_{6}^{3} {\mathbf{R}} = {}_{3}^{0} {\mathbf{R}}^{ - 1} \cdot {}_{6}^{0} {\mathbf{R}}$$

(16)

From (15) and (16), the values of $\theta_{i} ,(i = 1, \cdots ,6)$ can be obtained.

Inverse kinematics for the second type of non-offset wrist robotic arm

The second type of non-offset wrist robotic arm obtained through the transformation exhibits a configuration in which the virtual link is parallel to the two adjacent rotational axes. Figure 7 illustrates the coordinate system definitions according to the Modified DH (MDH) convention, and Table 2 details the DH parameters for each link.

Fig. 7

Schematic of the overall robotic arm with three adjacent parallel rotational axes at the end-effector.

Table 2 DH parameters for the overall robotic arm with three adjacent parallel rotational axes at the end-effector.

The pose matrix of the coordinate system ${\text{O}}_{{6}^{1}}-{\text{X}}_{{6}^{1}}{\text{Y}}_{{6}^{1}}{\text{Z}}_{{6}^{1}}$ relative to the base coordinate system ${\text{O}}_{0}-{\text{X}}_{0}{{\text{Y}}}_{0}{{\text{Z}}}_{0}$ is presented in (3). There exists a geometric relationship between the initial coordinate system ${\text{O}}_{3}-{\text{X}}_{3}{{\text{Y}}}_{3}{{\text{Z}}}_{3}$ of the three parallel rotational axes and the terminal coordinate system ${\text{O}}_{{6}^{1}}-{\text{X}}_{{6}^{1}}{\text{Y}}_{{6}^{1}}{\text{Z}}_{{6}^{1}}$:

1) the positional difference along the Z-axis is fixed, while the three parallel rotational axes are aligned along the Y-axis of the coordinate system. Consequently, the distance from the origin of the coordinate system to the origin in the Y-direction is given by d₄ + d₅ + d₆; 2) ${\text{Z}}_{{6}^{1}}$ remains perpendicular to Z₃, indicating that the cosine components of these two axes are zero; 3) ${\text{Y}}_{{6}^{1}}$ is always perpendicular to X₃; and ${\text{X}}_{{6}^{1}}$ is consistently perpendicular to Y₃.

Therefore, by separating the links l₄, l₅, and l₆ and utilizing the aforementioned geometric characteristics in conjunction with the kinematics of the robotic arm, the relative homogeneous transformation matrix of the coordinate system ${\text{O}}_{{6}^{1}}-{\text{X}}_{{6}^{1}}{\text{Y}}_{{6}^{1}}{\text{Z}}_{{6}^{1}}$ with respect to the coordinate system ${\text{O}}_{3}-{\text{X}}_{3}{{\text{Y}}}_{3}{{\text{Z}}}_{3}$ can be calculated as follows:

$$\left( {{}_{1}^{0} {\mathbf{T}} \cdot {}_{2}^{1} {\mathbf{T}} \cdot {}_{3}^{2} {\mathbf{T}}} \right)^{ - 1} \cdot {}_{{6^{\prime}}}^{0} {\mathbf{T}} = {}_{4}^{3} {\mathbf{T}} \cdot {}_{5}^{4} {\mathbf{T}} \cdot {}_{{6^{\prime}}}^{5} {\mathbf{T}}$$

(17)

Based on the aforementioned geometric relationships and the relationships between the elements of the matrix in (17), the values of $\theta_{i} ,(i = 1, \cdots ,6)$ can be resolved.

Improvement of the numerical algorithm for inverse kinematics of the robotic arm based on the SMABFGS method

Due to the simplification of the offset wrist robotic arm, discrepancies arise between the computed joint angles and the corresponding joint angles of the original robotic arm, leading to deviations in the calculated end-effector pose. Consequently, it is not feasible to directly derive the actual joint angles from the analytical solution of the simplified mechanism based on the known end-effector pose.

Analysis of the IK processes for two types of non-offset wrist robotic arms reveals that simplifying the robotic arm to the first non-offset wrist configuration keeps the end-effector orientation unchanged while modifying only its position. This is corroborated by comparing the joint angles from the analytical solution of the simplified robotic arm with the end-effector pose of the original arm through forward kinematics, as shown in Fig. 8a.

Fig. 8

Desired pose versus actual pose.

For the second non-offset wrist robotic arm type, the analytical solution for the joint angles does not produce the same effect on the end-effector pose as in the first type. Instead, the calculated pose is close to the desired pose, indicating that the analytical inverse solution of the simplified mechanism approximates the actual inverse solution, as illustrated in Fig. 8b. Therefore, the derived inverse solution can serve as a reference for the numerical algorithm, reducing the iteration time of the numerical computation. When obtaining a reference joint configuration close to the true solution, we assume each joint angle lies within the principal range [−π, π]. This assumption provides a reasonable initial search space for the subsequent numerical optimization.

This section introduces a numerical algorithm that employs the SMABFGS method to solve the IK of robotic arms. This algorithm integrates the kinematics of both the actual and simplified mechanisms, facilitating iterative calculations for both non-offset wrist robotic arms. For the first type, since the analytical inverse solution aligns the poses of the simplified and actual mechanisms, it suffices to determine the joint angles that allow the origin of the coordinate system ${\text{O}}_{4}-{\text{X}}_{4}{{\text{Y}}}_{4}{{\text{Z}}}_{4}$ to reach the desired pose. In contrast, for the second type, the strong coupling between the joint angles and the end-effector pose precludes the use of the previous method for iterative compensation of the joint angles. Therefore, the final inverse solution is obtained through iterative compensation of the overall joint angles.

Inverse kinematics solving based on the SMABFGS method

Let ${\text{T}}_{\text{end}}^{\text{t}}$ and ${\text{T}}_{\text{end}}^{\text{a}}$ denote the target end-effector pose and the actual end-effector pose, respectively. The actual end-effector pose can be obtained through forward kinematics by a series of joint angles $\theta = [\theta_{1} \theta_{2} \theta_{3} \theta_{4} \theta_{5} \theta_{6} ]^{T}$∈${\mathbb{R}}$⁶. The residual r(θ)∈${\mathbb{R}}$⁶ between the desired and actual end-effector poses can be expressed using the axis-angle representation³¹ as follows:

$${\mathbf{r}}\left( {{\varvec{\uptheta}}} \right) = \left[ {\begin{array}{*{20}c} {{\mathbf{p}}^{t} - {\mathbf{p}}^{a} \left( {{\varvec{\uptheta}}} \right)} \\ {\frac{1}{2} \cdot \left( {{\mathbf{n}}^{a} \left( {{\varvec{\uptheta}}} \right) \times {\mathbf{n}}^{t} + {\mathbf{o}}^{a} \left( {{\varvec{\uptheta}}} \right) \times {\mathbf{o}}^{t} + {\mathbf{a}}^{a} \left( {{\varvec{\uptheta}}} \right) \times {\mathbf{a}}^{t} } \right)} \\ \end{array} } \right]$$

(18)

The vector r(θ) is expressed using the axis-angle representation. The first three components of r(θ) represent the position error, and the last three represent the orientation error. The solution for the desired joint angles θ*∈${\mathbb{R}}$⁶ can be obtained when the actual end-effector pose coincides with the desired end-effector pose. The traditional iterative method for IK based on the Newton iteration approach is founded on three assumptions. If any of these assumptions are violated, it may lead to a failure in the iterative solution process³².

The equations above can be transformed into a minimization problem to explore the IK solution for the robotic arm. Given that the initial values obtained from the IK solution of the simplified mechanism exhibit some degree of residual error between the end-effector pose and the actual pose, it is necessary to optimize the poses with more significant residuals. A weighting matrix is introduced to reflect the priority of various constraints. The optimization objective function is thus defined as:

$$\mathop {arg\min }\limits_{{ - {\varvec{\pi}} \le {{\varvec{\uptheta}}} < {\varvec{\pi}}}} \;G\left( {{\varvec{\uptheta}}} \right) = \mathop {arg\min }\limits_{{ - {\varvec{\pi}} \le {{\varvec{\uptheta}}} < {\varvec{\pi}}}} \;{\mathbf{r}}\left( {{\varvec{\uptheta}}} \right)^{T} \cdot {\mathbf{r}}\left( {{\varvec{\uptheta}}} \right)$$

(19)

With r(θ) denotes the six-dimensional pose residual vector.

Since the function G(θ) contains quadratic and trigonometric components, it becomes nonlinear and can only be addressed through nonlinear programming. Existing literature indicates that most nonlinear problems should be reformulated as linear equations for computational solutions^33,34. The BFGS method is employed to optimize the aforementioned objective function, where the update rule for the quasi-Newton method is given by:

$$\Delta {{\varvec{\uptheta}}} = - \nabla^{2} G\left( {{{\varvec{\uptheta}}}_{k} } \right)^{ - 1} \cdot \nabla G\left( {{{\varvec{\uptheta}}}_{k} } \right)^{T}$$

(20)

With $\nabla G\left( {{{\varvec{\uptheta}}}_{k} } \right) = 2{\mathbf{r}}\left( {{{\varvec{\uptheta}}}_{k} } \right)^{T} \cdot \nabla {\mathbf{r}}\left( {{{\varvec{\uptheta}}}_{k} } \right)$, $\nabla G\left( {{{\varvec{\uptheta}}}_{k} } \right) \in {\mathbb{R}}^{1 \times 6}$;

$\nabla^{2} G\left( {{{\varvec{\uptheta}}}_{k} } \right) = 2 \cdot \nabla {\mathbf{r}}\left( {{{\varvec{\uptheta}}}_{k} } \right)^{T} \cdot \nabla {\mathbf{r}}\left( {{{\varvec{\uptheta}}}_{k} } \right) + 2 \cdot \sum\limits_{i = 1}^{{dim\left( {{\mathbf{r}}\left( {{\varvec{\uptheta}}} \right)} \right)}} {{\mathbf{r}}\left( {{{\varvec{\uptheta}}}_{k} } \right)_{i}^{T} \cdot \nabla^{2} {\mathbf{r}}\left( {{{\varvec{\uptheta}}}_{k} } \right)_{i} }$, $\nabla^{2} G\left( {{{\varvec{\uptheta}}}_{k} } \right) \in {\mathbb{R}}^{6 \times 6}$.

However, in classical Newton’s method one must compute the Hessian of the objective, $\nabla^{2} G\left( {\theta_{k} } \right)$, which in turn requires evaluating second-order partial derivatives of the residual vector r(θ), namely $\nabla^{2} r\left( {\theta_{k} } \right)$. Due to the complexity of computing the second derivative $\nabla^{2} G\left( {\theta_{k} } \right)$ involving $\nabla^{{2}} {\text{r}}\left( {{\uptheta }_{{\text{k}}} } \right)$ at a computational complexity of ${\mathcal{O}}\left( {d^{3} } \right)$, where $d$ represents the degrees of freedom of the robotic arm, the computational complexity of $\nabla^{2} G\left( {\theta_{k} } \right)$ also reaches ${\mathcal{O}}\left( {d^{3} } \right)$. Therefore, to reduce computational complexity, the SMABFGS method is introduced, utilizing the matrix B_k∈${\mathbb{R}}$^6×6 as an approximation for the Hessian matrix $\nabla^{2} G\left( {\theta_{k} } \right)$. In reference³⁵, Saman Babaie-Kafaki replaced B_k with $\vartheta_{k} \cdot B_{k}$ and proposed a more suitable and effective $\vartheta_{k}$. The matrix B_k is updated using the following recursive formula:

$${\mathbf{B}}_{k + 1} = \frac{1}{{\vartheta_{k} }} \cdot {\mathbf{I}} + \frac{{{\mathbf{y}}_{k} \cdot {\mathbf{y}}_{k}^{T} }}{{{\mathbf{y}}_{k}^{T} \cdot {\mathbf{s}}_{k} }} - \frac{1}{{\vartheta_{k} }} \cdot \frac{{{\mathbf{s}}_{k} \cdot {\mathbf{s}}_{k}^{T} }}{{{\mathbf{s}}_{k}^{T} \cdot {\mathbf{s}}_{k} }} + \tau_{k} \cdot \frac{{{\mathbf{s}}_{k} \cdot {\mathbf{s}}_{k}^{T} }}{{{\mathbf{s}}_{k}^{T} \cdot {\mathbf{s}}_{k} }}$$

(21)

where B_k denotes the approximate substitute for the second derivative of the function G(θ); $\vartheta_{k}$ is the scaling parameter; τ, C and ${\varrho }$ are non-negative natural numbers³⁶.

With ${\varrho } _{k} = \frac{{{\mathbf{s}}_{k}^{T} \cdot {\mathbf{y}}_{k} }}{{\left\| {{\mathbf{y}}_{k} } \right\|^{2} }}$; ${\mathbf{s}}_{k} = {{\varvec{\uptheta}}}_{k + 1} - {{\varvec{\uptheta}}}_{k}$, ${\mathbf{s}}_{k} \in {\mathbb{R}}^{6}$;$\eta_{k} = \max \left\{ {\tilde{\eta },0} \right\}$; $\tau_{k} = \tau \cdot \frac{{\eta_{k} }}{{\left\| {{\mathbf{s}}_{k} } \right\|^{2} }} + C \cdot \left\| {\nabla G\left( {{{\varvec{\uptheta}}}_{k} } \right)} \right\|^{{\varrho }}$;

$\tilde{\eta }_{k} = 2 \cdot \left( {G\left( {\theta_{k} } \right) - G\left( {\theta_{k + 1} } \right)} \right) + s_{k}^{T} \cdot \left( {\nabla G\left( {\theta_{k} } \right) + \nabla G\left( {\theta_{k + 1} } \right)} \right)$; ${\mathbf{y}}_{k} = \nabla G\left( {{{\varvec{\uptheta}}}_{k + 1} } \right)^{T} - \nabla G\left( {{{\varvec{\uptheta}}}_{k} } \right)^{T}$, $y_{k} \in R^{6 \times 1}$.

By updating B_k using the aforementioned recursive formula, the formula for calculating the search direction is simplified, while avoiding the direct computation of the high-dimensional Hessian matrix, resulting in good computational performance³⁷. The Sherman-Morrison formula is then applied for transformation, allowing the inverse of the matrix to be expressed as follows:

$${\mathbf{B}}_{k + 1}^{ - 1} = \vartheta_{k} \cdot {\mathbf{I}} - \vartheta_{k} \cdot \frac{{{\mathbf{s}}_{k} \cdot {\mathbf{y}}_{k}^{T} + {\mathbf{y}}_{k} \cdot {\mathbf{s}}_{k}^{T} }}{{{\mathbf{s}}_{k}^{T} \cdot {\mathbf{y}}_{k} }} + \left( {I + \vartheta_{k} \cdot \frac{{{\mathbf{y}}_{k}^{T} \cdot {\mathbf{y}}_{k} }}{{{\mathbf{s}}_{k}^{T} \cdot {\mathbf{y}}_{k} }}} \right) \cdot \frac{{{\mathbf{s}}_{k} \cdot {\mathbf{s}}_{k}^{T} }}{{{\mathbf{s}}_{k}^{T} \cdot {\mathbf{y}}_{k} }} - \frac{{\tau_{k} }}{{\gamma_{k} }} \cdot \frac{{{\mathbf{z}}_{k} \cdot {\mathbf{z}}_{k}^{T} }}{{{\mathbf{s}}_{k}^{T} \cdot {\mathbf{y}}_{k} }}$$

(22)

With $\gamma_{k} = \tau_{k} + \frac{{{\mathbf{s}}_{k}^{T} \cdot {\mathbf{y}}_{k} }}{{\left\| {{\mathbf{s}}_{k} } \right\|^{2} }} + \tau_{k} \cdot \vartheta_{k} \cdot \left( {\frac{{\left\| {{\mathbf{y}}_{k} } \right\|^{2} }}{{{\mathbf{s}}_{k}^{T} \cdot {\mathbf{y}}_{k} }} - \frac{{{\mathbf{s}}_{k}^{T} \cdot {\mathbf{y}}_{k} }}{{\left\| {{\mathbf{s}}_{k} } \right\|^{2} }}} \right)$, ${\mathbf{z}}_{k} = - \vartheta_{k} \cdot {\mathbf{y}}_{k} + \left( {{\mathbf{I}} + \vartheta_{k} \cdot \frac{{\left\| {{\mathbf{y}}_{k} } \right\|^{2} }}{{{\mathbf{s}}_{k}^{T} \cdot {\mathbf{y}}_{k} }}} \right) \cdot {\mathbf{s}}_{k}$.

If B_k is initially positive definite and the curvature is more significant than zero, then the updated value at time k + 1 will also be positive definite. Consequently, the initialized value B₀∈${\mathbb{R}}$^6×6 must be positive definite³⁸. Moreover, since $\nabla^{2} G\left( {\theta_{k} } \right)$ includes the square term of the robotic arm’s Jacobian matrix, and B_k approximates $\nabla^{2} G\left( {\theta_{k} } \right)$, it is necessary to incorporate the square term of the Jacobian matrix in the initialization of B_k.

$${\mathbf{B}}_{0} = {\mathbf{J}}_{0}^{T} \cdot {\mathbf{J}}_{0} + \lambda \cdot {\mathbf{I}}$$

(23)

where $J_{0} = \nabla r(\theta_{0} )$∈${\mathbb{R}}$^6×6 represents the initial Jacobian matrix; λ denotes the regularization factor. In the initialization of B_k, a regularization term is employed to fill the diagonal. This approach prevents B₀ from degenerating due to the proximity of J₀ to a singular point. To enhance the handling of constraint boundaries, adaptive regularization is introduced in the above equation:

$$\lambda = \lambda_{0} + \sum\limits_{i = 1}^{6} {\frac{\gamma }{{d_{i}^{2} + \varepsilon }}}$$

(24)

In this context, $d_{i} = \min \left( {\left| {\theta_{i} - \theta_{i,\min } } \right|,\left| {\theta_{i} - \theta_{i,\max } } \right|} \right)$ represents the distance from the current joint angle to the nearest boundary, where θ_i denotes the i-th joint angle of θ_k. According to the description by Tomomichi Sugihara in reference³², λ₀ can be set as λ₀ = max(λ₁,G(θ)), with an empirical value of λ₁ = 1e−3. The constraint sensitivity coefficient is defined as γ = 1e−6, and ϵ = 1e−9 is introduced to prevent division by zero. When any joint angle approaches the constraint boundary, the regularization coefficient λ increases sharply, resulting in a numerical barrier effect.

Thus, the update rule for the Newton method is modified to:

$$\Delta {{\varvec{\uptheta}}}_{k} = - {\mathbf{B}}_{k}^{ - 1} \cdot \nabla G\left( {{{\varvec{\uptheta}}}_{k} } \right)^{T}$$

(25)

During the SMABFGS iteration process, joint angle constraints are addressed using the projected gradient method³⁹. The joint angles of the robotic arm at the next time step can be updated as follows:

$${{\varvec{\uptheta}}}_{k + 1} = \prod\limits_{\Omega } {\left( {{{\varvec{\uptheta}}}_{k} + \Delta {{\varvec{\uptheta}}}_{k} } \right)}$$

(26)

The projection operator for θ_i is defined as follows:

$$\prod\limits_{\Omega } {\left( {\theta_{i} } \right)} = \left\{ {\begin{array}{*{20}l} {\theta_{i,\min } } \hfill & {\;\theta_{i} < \theta_{i,\min } } \hfill \\ {\theta_{i} } \hfill & {\;\theta_{i,\min } \le \theta_{i} \le \theta_{i,\max } } \hfill \\ {\theta_{i,\max } } \hfill & {\;\theta_{i} > \theta_{i,\max } } \hfill \\ \end{array} } \right.$$

(27)

A projected-gradient scheme projects each joint angle θ_i onto its prescribed bounds [θ_i,min, θ_i,max], enabling the algorithm to accommodate asymmetric limits, thereby enhancing its general applicability.

Adaptive step size optimization search algorithm

It is widely acknowledged that the Newton–Raphson method typically requires an initial estimate close to the operator equation’s solution in most cases. Numerous scholars have introduced damping parameters to improve the lack of global convergence associated with the Newton method^40,41. However, determining the precise value of the damping parameter significantly increases computational costs. Attempting to achieve accuracy in this calculation may lead to excessively small step sizes. As a result, this can slow down the convergence of the iterations. Therefore, this paper employs Wolfe line search to establish the SMABFGS method’s step sizes that satisfy Wolfe’s conditions⁴².

Most theories related to Wolfe line search are based on the properties of auxiliary functions⁴³. The Wolfe conditions can be expressed as in (28) of the auxiliary function, as illustrated in Fig. 9.

$$\left\{ {\begin{array}{*{20}l} {\Phi \left( \alpha \right) = G\left( \alpha \right) - \left( {G\left( 0 \right) + \alpha \cdot \eta_{A} \cdot G^{\prime } \left( 0 \right)^{T} } \right)} \hfill \\ {\Phi^{\prime } \left( \alpha \right) = G^{\prime } \left( \alpha \right)^{T} - \eta_{W} \cdot G^{\prime } \left( 0 \right)^{T} } \hfill \\ \end{array} } \right.$$

(28)

where α > 0 represents the descent step size; $0 < \eta_{A} < \eta_{W} < 1$ is a set of pre-assigned scalar values that modify the precision of the step size through different selections. Generally, $\eta_{A}$ approaches zero, with $\eta_{A} = 1e - 4$ and $\eta_{W} = 0.9$⁴⁴.

Fig. 9

Auxiliary function for Wolfe conditions.

The above approach delineates the range of step sizes that satisfy Wolfe conditions, thereby reducing the search area. Nonetheless, even under Wolfe conditions, there is still the potential to become trapped in regions of local extrema, particularly in some low-lying areas depicted in Fig. 9. In such regions, the distance to the global minimum remains substantial, which may lead to line searches that fall within these zones, confirming relatively small step sizes and consequently increasing the number of iterations required.

For regions with relatively shallow depressions, as depicted in Fig. 9, it is considered beneficial to introduce momentum in the calculation of the step size α. This approach aims to eliminate certain local minima and thus reduce the search scope while enhancing the optimization speed, ultimately leading to a more suitable step size. Within the range of step sizes that satisfy the Wolfe conditions, a GD algorithm is employed to conduct a search in the specified area, utilizing a decay factor and the gradient from the previous iteration to update the current gradient. The GD equation incorporating momentum can be expressed as follows⁴⁵:

$$\left\{ \begin{gathered} \alpha_{k + 1} = \alpha_{k} + \alpha^{GD} \cdot d_{k + 1} \hfill \\ d_{k + 1} = - \Phi^{\prime}\left( {\alpha_{k} } \right) + \beta_{k + 1} \cdot d_{k} \hfill \\ \beta_{k + 1} = \frac{{\left\| {{{\left( {\Phi \left( {\alpha_{k} } \right) - \Phi \left( {\alpha_{k - 1} } \right)} \right)} \mathord{\left/ {\vphantom {{\left( {\Phi \left( {\alpha_{k} } \right) - \Phi \left( {\alpha_{k - 1} } \right)} \right)} {\left( {\alpha_{k} - \alpha_{k - 1} } \right)}}} \right. \kern-0pt} {\left( {\alpha_{k} - \alpha_{k - 1} } \right)}}} \right\|^{2} }}{{d_{k}^{T} \cdot \left( {{{\left\| {\Phi^{\prime}\left( {\alpha_{k} } \right)} \right\|^{2} } \mathord{\left/ {\vphantom {{\left\| {\Phi^{\prime}\left( {\alpha_{k} } \right)} \right\|^{2} } {\left( {\Phi^{\prime}\left( {\alpha_{k} } \right) + \Phi^{\prime}\left( {\alpha_{k - 1} } \right)} \right) - \Phi^{\prime}\left( {\alpha_{k - 1} } \right)}}} \right. \kern-0pt} {\left( {\Phi^{\prime}\left( {\alpha_{k} } \right) + \Phi^{\prime}\left( {\alpha_{k - 1} } \right)} \right) - \Phi^{\prime}\left( {\alpha_{k - 1} } \right)}}} \right)}} \hfill \\ \end{gathered} \right.$$

(29)

where $\beta \in [0,1][0,1]$ is the momentum decay factor. $\upsilon_{0} = 0.9$ is commonly chosen for momentum. α^GD represents the iteration step size in the GD method.

In traditional GD methods, a fixed step size is utilized for the iterative search. Appropriate step size can accelerate convergence. However, the dynamic and unknown distance between the extremum point and the current iteration point may result in challenges such as oscillations during iterations, slow convergence speeds, and low convergence precision when a fixed step size is employed⁴⁶. Consequently, a suitable dynamic step size promotes rapid convergence of the optimization objective. This paper proposes a dynamic step size approach for iterative searching to derive a step size that meets the Wolfe conditions for the SMABFGS method.

$$\alpha_{k + 1}^{GD} = \gamma \cdot \left\| {\alpha_{k + 1} - \alpha_{k} } \right\|^{2} \cdot \left\| {{\Phi }\left( {\alpha_{k + 1} } \right)} \right\|$$

(30)

With γ represents a coefficient.

The dynamic adaptive step size mechanism is established in this study, whereby the Wolfe conditions are integrated with a momentum factor. A line search method is employed to determine the step size for the SMABFGS algorithm, effectively circumventing local extrema. Compared to traditional fixed step size BFGS or simple line search methods, an improvement in convergence speed is observed. Furthermore, the initial Hessian matrix is regularized by incorporating information from the robotic arm’s Jacobian matrix, thereby enhancing the effectiveness of the initial direction. An adaptive regularization factor is added to the initial Hessian matrix to avoid divergence often caused by inappropriate initial values in conventional BFGS methods. By combining geometric simplification with numerical optimization, initial values are generated using virtual links, achieving high-precision compensation and reducing the dependence of numerical methods on precisely defined initial values⁹.

Numerical example

This paper utilizes the proposed IK algorithm to solve for the four configurations of robotic arms previously mentioned, thereby validating the robustness and effectiveness of the algorithm. First, 1000 random points are selected within the workspace of each type of robotic arm. The proposed algorithm is employed to compute the joint angles θ_i, ${\text{i = 1,}} \cdots {,6}$ within the range $\left( { - \pi ,\pi } \right.]$. Forward kinematics are then used to obtain the end-effector poses corresponding to each set of joint angles, allowing for a comparison of the discrepancies between the desired and actual poses. Importantly, the numerical optimization stage of our algorithm can accommodate arbitrary joint limits; the range used in these experiments was selected solely for convenience in simulation-based comparisons.

Subsequently, the robotic arm SR4 from ROKAE Robotics is chosen as the subject for experimentation. The proposed algorithm is applied to calculate a continuous trajectory within a segment of its workspace, facilitating a discussion on the algorithm’s real-time performance and accuracy.

The proposed algorithm has been implemented using MATLAB to study the comparative performance of various robotic arms. The algorithm was executed on a laptop running MATLAB 2023a on a laptop with an i7-11800H processor and 16 GB of RAM.

Simulation of robotic arms with offset wrists

Due to the offset or rotation of certain linkages in the four configurations, discrepancies arise in the treatment and solution processes. To facilitate comparisons, the DH parameters of the robotic arms have been configured to be as identical as possible. Consequently, the DH parameters for all four configurations are set as follows: a₁ = 0, a₂ = 0, a₃ = 0.40311, a₄ = −0.05, a₅ = 0, a₆ = 0, d₁ = 0.327, d₂ = 0, d₃ = 0, d₄ = 0.399, d₅ = 0.0725, d₆ = 0.1025, α₁ = 0, α₂ = π/2, α₃ = 0, α₄ = −π/2, α₅ = −π/2, and α₆ = −3π/4. The offsets and rotations of certain linkages result in different DH parameters for the configurations: for configuration 4, a₆ = 0.1; for configuration 5, a₅ = 0.125, a₆ = 0.1, α₅ = 0; and configuration 6, a₅ = 0.125, a₆ = 0.1, α₅ = π/2, α₆ = 0. The convergence precision of the function is set to 1e-12.

Initially, 1000 randomly selected end-effector pose points are identified for simplification and IK solutions across the four configurations, resulting in the corresponding end-effector poses. An error analysis is conducted by comparing the desired end-effector poses with the computed poses. As depicted in Fig. 10, the distribution of end-effector position errors is shown, with parts (a) and (b), (c) and (d) illustrating two different treatment methods for configuration 3 and configuration 4, respectively, and parts (e) and (f) representing the methods for configuration 5 and configuration 6. Notably, the density distribution of sample points indicates that over 97% of the samples achieve position errors converging within the range [0,1e−6], with only a small fraction falling within [1e−6,1e−5].

Fig. 10

End-effector position errors computed by four configuration algorithms.

As shown in Fig. 11, the distribution of end-effector orientation errors is illustrated. Specifically, (a) and (b), (c) and (d) represent different processing approaches for configurations 3 and 4, respectively. Similarly, (e) and (f) depict the handling methods for configurations 5 and 6. It can be readily observed that the distribution of end-effector orientation errors exhibits patterns closely resembling those of the position error distribution. The orientation errors for all four configurations are primarily concentrated within the range [0,5 × 1e-5], indicating a narrower convergence range relative to the position errors. It is important to note that the results obtained from the two different treatment methods for configurations 3 and 4 indicate distinct outcomes. The second treatment method, which incorporates only translational transformations, is associated with a more concentrated sample distribution for both position and orientation errors. In contrast, the first method, which includes rotational transformations, results in a broader sample distribution.

Fig. 11

End-effector pose errors computed by four configuration algorithms.

Table 3 presents the mean operation time and standard deviation for the 1,000 end-effector pose points subjected to IK. The solution times are concentrated around 0.005 s across various configurations. The standard deviation of 0.003 suggests minimal fluctuation in the distribution of values, thereby validating the stability of the algorithm.

Table 3 Error analysis of end-effector computation times for four configurations.

In addition, a convergence precision range of [1e−4,1e−15] was established with 100 precision levels. For each convergence precision, inverse solutions were obtained for 1000 randomly generated end-effector poses using the six aforementioned inverse kinematics algorithms. The average computation time for each algorithm at the current precision level was calculated.

Figure 12 illustrates the relationship between convergence precision and computation time. It is evident from Fig. 12 that as the convergence precision increases, the computation time also rises, which is directly related to the number of iterations. However, the overall variation observed is not significant. This indicates that the proposed algorithm can reduce the number of iterations through an adaptive step size mechanism, thereby decreasing computation time.

Fig. 12

The relationship between convergence precision and computation time.

By comparing the SMABFGS-NIK algorithm designed in this paper with the widely used inverse kinematics algorithms BFGSGP and LM, 1,000 desired end-effector poses were randomly generated within the workspace, using randomly obtained joint angles as initial values. The inverse kinematics solutions were obtained using these three algorithms, and the resulting joint angles were employed to compute the real-time end-effector poses through forward kinematics. Table 4 presents the average computation times for inverse kinematics solutions, average end-effector position errors, and average end-effector orientation errors for the three algorithms concerning six configurations.

Table 4 Average end-effector pose errors and average computation times for the three inverse kinematics solution algorithms.

From the observations for configurations 3–1, it was found that the SMABFGS-NIK algorithm exhibited superior performance in terms of average position error and average orientation error compared to the LM and BFGSGP algorithms. The accuracy of the SMABFGS-NIK algorithm was found to reach 1E−7 m and 1E−6°, while the accuracy obtained by the BFGSGP algorithm was slightly lower. The average computation time for the SMABFGS-NIK algorithm was 0.0049 s, significantly less than the average times for the LM and BFGSGP algorithms, accounting for approximately 7.9% and 13.6% of their respective times.

In comparing the performance of the three algorithms across different configurations, all three algorithms were found to converge to relatively small ranges for the kinematic inverse solutions concerning the four configurations. However, the LM and BFGSGP algorithms exhibited large average position and orientation errors for configuration 5, particularly with the LM algorithm showing values as high as 7.787E−3 m and 0.9954°. In contrast, the SMABFGS-NIK algorithm attained average position and orientation errors of only 7.79E−8 m and 1.4E−6°, respectively. Furthermore, the average computation time for the LM algorithm in configuration 5 reached 0.117 s, significantly higher than the 0.00516 s required by the SMABFGS-NIK algorithm.

Overall, the comparative analysis indicated that the SMABFGS-NIK algorithm outperforms the other two algorithms in both the accuracy of the kinematic inverse solutions and computation time for the four configurations, demonstrating faster convergence without reliance on initial angles.

Experiment on a robotic arm with offset wrist

To further demonstrate the practicality of the proposed algorithm, a series of IK solutions for continuous trajectories is conducted using the robotic arm SR4 from Rock Stone Robotics. The DH parameters are defined as follows: a₁ = 0, a₂ = 0, a₃ = 0.40311, a₄ = -0.05, a₅ = 0, a₆ = 0, d₁ = 0.327, d₂ = 0, d₃ = 0, d₄ = 0.399, d₅ = 0.135, d₆ = 0.1025, α₁ = 0, α₂ = π/2, α₃ = 0, α₄ = −π/2, α₅ = −π/2, and α₆ = −π/2. An end-effector offset d₅ is considered, and the second treatment method from configuration 3 is used to perform the IK solution. The supplementary experimental video concerning the actual robotic arm is provided in the Supplementary Information.

First, a circular trajectory is interpolated to obtain a complete end-effector path. Subsequently, using the second treatment method for configuration 3, a set of joint angles that meet the convergence criteria is computed, and the Robot Operating System (ROS) is employed to control the SR4’s motion. Finally, Inertial Measurement Unit (IMU) data captures changes in the robotic arm’s end-effector pose.

Figure 13a illustrates the desired end-effector trajectory alongside the actual end-effector trajectory. A comparison of the expected and actual end-effector trajectories reveals a high degree of overlap between the desired and actual poses. Figure 13b depicts the trajectory error resulting from the IK solution, showing that the errors remain within the interval [0.4556, 0.45562] along the circular path, with a low distribution of error values. This indicates that the proposed IK algorithm achieves excellent tracking performance within a comprehensive error range of 2 × 1e−5. Figure 13c illustrates the circular trajectory experiment conducted with the robotic arm SR4 and related descriptions of the components involved.

Fig. 13

Circular trajectory experiments of end-effector poses.

Figure 14 displays the changes in joint angles as the algorithm resolves the test trajectory. The trajectories of all joint angle changes are continuous and smooth, thereby validating the algorithm’s effectiveness and stability in maintaining angular continuity. Figure 15 showcases the measured variations in joint angles of the physical robotic arm, indicating a remarkable consistency between the computed joint angle changes and those obtained through actual measurements. Figure 16 illustrates the time required to calculate IK solutions for specific end-effector path points. The computation time for these path points stabilizes at approximately 0.002 s, while the overall computation times remain below 0.0055 s. Additionally, the communication time for the robotic arm is approximately 0.02 s. This affirms that the algorithm meets the requirements for real-time performance.

Fig. 14

Joint angles from IK.

Fig. 15

Actual joint errors.

Fig. 16

IK computation times.

Figures 17 and 18 present the position error and orientation error, respectively, between the desired end-effector trajectory and the actual end-effector trajectory for the specified circular arc path. The error variation curves indicate that both translational and rotational movements exhibit varying degrees of oscillation, predominantly fluctuating around zero. This observation confirms that the actual end-effector trajectory is centered around the desired trajectory.

Fig. 17

The position error.

Fig. 18

The orientation error.

In Fig. 17, the position fluctuations are confined within a range of ± 0.01 mm. Figure 18 indicates that the orientation error remains within a range of ± 0.08°, with only a limited number of instances exceeding this threshold. Such outliers may be attributed to sudden external disturbances or measurement noise. In summary, these findings validate that the proposed algorithm exhibits a high degree of robustness.

Notably, when comparing the IK solution times for discrete points in Sect. 4.1 with those for continuous path points in this section, it is evident that the computation time for continuous path points is reduced by approximately 0.003 s. This reduction is attributable to reference values from the preceding path points in continuous trajectories, whereas discrete points are randomly generated. As a result, the preceding end-effector pose may differ significantly from the subsequent pose, which not only precludes the latter from serving as a valid reference but also tends to increase the offset in the computed joint angles for the next iteration, thereby slowing convergence. The proposed algorithm exhibits high efficiency and real-time capabilities when solving IK for continuous trajectories.

Conclusion

This paper presents a hybrid approach for solving the IK problem, integrating geometric simplification with numerical optimization. Conventional methods typically employ these two techniques in isolation. Our method first utilizes virtual links to rapidly generate an initial solution. This solution is then refined using an improved SMABFGS algorithm. The algorithm incorporates a dynamic step size and momentum gradient descent for faster convergence. Furthermore, joint limits are handled by a projected gradient method. This integrated approach achieves both high-precision compensation and rapid convergence for IK solutions. The introduction of Jacobian matrix information reduces oscillations in early iterations compared to traditional BFGS, while the adaptive selection of λ helps avoid singularities. Experimental comparisons have demonstrated that the convergence time of the SMABFGS-NIK algorithm is significantly faster than that of the BFGSGP and LM algorithms.

This paper presents a numerical method for solving the IK of arbitrary 6-DOF serial robotic arms. The method compensates for actual pose errors to obtain accurate joint angle values iteratively.

• (1) The concept of virtual links is employed to transform various biased link configurations of the robotic arm, simplifying four biased links into two categories of traditional robotic arms. This allows for the acquisition of reference joint angles that approximate the desired solutions of the actual robotic arm.

• (2) By utilizing reliable initial reference angles and values of the Hessian matrix, along with a momentum-based GD method, the step size of the SMABFGS algorithm is dynamically adjusted to satisfy Wolfe conditions, achieving fast convergence rates and reduced pose errors.

• (3) By combining the approaches outlined in (1) and (2), the IK problems for most robotic arms with offset wrists can be effectively solved. This addresses the limitations of existing IK methods, which are restricted to specific types of arms with offset wrists.

The selection of initial angles has been identified as the primary factor contributing to the shorter computation times for IK during trajectory tracking experiments compared to random sampling. Therefore, future research should focus on determining the analytical solutions for simplified structural robotic arms that closely resemble the desired solutions. Furthermore, more complex joint constraints will be addressed in future work. The use of a Quadratic Programming (QP) solver will be explored for the integrated handling of joint limits. The resulting impact on the algorithm’s real-time performance will also be evaluated.