Inverse kinematics algorithm for trajectories with specified tool orientation

Introduction. During welding, coating application, and assembly or installation operations, the tool’s position is specified in conjunction with its orientation. A single specified pose may correspond to various sets of manipulator joint angles, and the solutions obtained for individual points do not always form a continuous trajectory. In such cases, the transition from one point to another involves a sudden change in joint angles, a factor that must be taken into account when setting movement speeds. The trajectory is defined either by joint angles or by an array of Cartesian tool poses. In the latter case, the input data consists of coordinates and orientations derived from a CAD model, teach points, or an external planner. Calculating the trajectory from this data requires reconciling inverse kinematics solutions at adjacent points; in the algorithm under consideration, this is achieved by passing the calculated angles to the computation for the next point. Methods and Materials. The object of study is a six-link manipulator with six degrees of freedom. Joint angles were determined based on specified Tool Center Point (TCP) positions and orientations using the pseudoinverse of the numerical Jacobian matrix, while the orientation error component was calculated using ZYX Euler angles. The solution obtained for a given point served as the initial approximation for the subsequent point. Verification was performed using trajectories comprising varying numbers of points. Results and Discussion. The procedure for formulating the six-dimensional error, the Jacobian calculation, and the iteration termination criteria are presented. Position and orientation errors, iteration counts, and computation times were determined for trajectories consisting of 5, 10, and 20 points; trajectories with up to 50 points were also examined. Calculation results for an identical geometric trajectory showed average iteration counts per point of 28, 18, and 12, respectively. Utilizing the previous solution reduced the number of iterations and, in the tests conducted, enabled the maintenance of the selected solution branch. Conclusion. The developed algorithm enables the derivation of a sequence of manipulator joint angles from a set of tool positions and orientations. In the calculations performed, the transfer of the solution between adjacent points ensured smooth angular variation; however, this mechanism alone does not guarantee the avoidance of a solution branch switch for every possible trajectory. The algorithm’s applicability is constrained by the convergence conditions of the numerical computation. When approaching a singular configuration or using sparse point spacing, it is necessary to additionally check the condition number of the Jacobian. Enhancing computational stability may require finer trajectory discretization and regularized pseudo-inversion.