A Manipulator Pose Planning Algorithm Based on Matrix Information Geometry

. In an automatic ultrasonic testing system constituted by an ultrasonic probe and a six-axis manipulator, the manipulator needs to run from a static state to the target velocity. To prevent equipment damage caused by sudden acceleration or deceleration, it is necessary to plan the position and pose of the end effector of the manipulator at each detected point. In this manuscript, an algorithm for planning the position and pose of the manipulator is proposed based on the information geometry structure of special orthogonal groups. As the linear operation of the orthogonal matrix corresponding to the manipulator pose is not closed, the manipulator pose at each detected point was calculated using the straightness of the Lie algebra of the special orthogonal group. The matrix information geometry algorithm enabled not only the manipulator to accelerate and decelerate uniformly along the detection trajectory, but also the angular acceleration of the end effector to accelerate uniformly at ﬁ rst, then keep a uniform velocity, and ﬁ nally decelerate uniformly. The platform motion experiments with the Turin TKB070S six-axis manipulator are carried out to verify the effectiveness of the matrix information geometry algorithm for planning the pose of the manipulator.


Introduction
Ultrasonic testing is an important nondestructive testing approach frequently applied to detect the internal defects of composite materials with simple structures [1][2][3][4].However, the fast-growing composite manufacturing technology brings about the problem of complex shapes in many workpieces to be detected, such as curved shapes, variable thickness, and intricate rotating structures.Aircraft engine blades must have extremely high self-reliability as they serve in high-temperature and high-pressure environments.There are many methods for manufacturing blades, and we discuss diffusion welding of the aviation blades.Its manufacturing process first involves diffusion welding of flat alloy materials, followed by high-temperature torsional plastic forming, and finally, curved surface machining to obtain geometric contours consistent with the design drawings.Blades belong to the thin-walled parts, and after torsional deformation, the unloading of the forming load will cause a rebound phenomenon, which leads to the error of the formed blade surface.
Usually, after the blade is twisted and deformed, and before the curved surface is machined, it is still necessary to perform ultrasonic testing on the blade to ensure the quality of the diffusion welding interface.To achieve blade surface scanning, a line laser profilometer is first used in conjunction with a robotic multiaxis system to obtain blade point clouds, and then point cloud data are used to fit and obtain the scanning trajectory.Therefore, it is crucial to achieve the automatic detection of workpieces with the complex structures.
Figure 1 shows the automatic monitoring system for civil aviation engine blades.According to the requirements of blade structure and water immersion ultrasonic C-scan, Zhao et al. [5] designed a multiaxis detection system.The blade was fixed on the workpiece rotation axis, and the sixaxis manipulator stood upright on the side of the blade.The manipulator base could be translated parallel to the blade rotation axis.During ultrasonic scanning inspection, a sixaxis manipulator held the ultrasonic probe and incident vertically along the normal direction of the blade surface.The manipulator and the blade rotation axis achieved a seven-axis linkage profile scanning, and then the manipulator base stepped along the track and reversed the profile motion to achieve the next line of scanning.By analogy, it completed the overall scanning of this blade.A manipulator having six degrees of freedom can reach any position and make any pose within its working range.The position and pose of a six-axis manipulator equipped with an ultrasonic probe on its motion trajectory are simulated on an offline simulation platform.This method greatly improves the curved surface detection efficiency and accuracy [6][7][8][9][10][11].
In this manuscript, we mainly propose how to plan the space trajectory of the manipulator and present a concrete and feasible algorithm.The trajectory in space is a description of the expected motion of the manipulator, and the trajectory in this paper specifically refers to the position and pose of the flange at the manipulator end or the tool clamped on the flange.
The positions and poses at detected points should be subjected to interpolation treatment in trajectory planning.The purpose of interpolation is to facilitate the smooth transition of the manipulator from the initial position and pose to the desired position and pose.And it ensures that each axis of the manipulator can arrive at the specified position and angle within each interpolation period.Besides, interpolation prevents fast acceleration or deceleration of the manipulator during the detection process, helping the manipulator to accomplish the detection task smoothly and efficiently.
In the moving process of the manipulator, the position and pose of the end effector are described as the 3 × 1 vector and 3 × 3 orthogonal matrix in the three-dimensional (3D) real vector space, respectively [12].According to the linear relationship of the 3D real vector space, the positions at all detected points are planned at first, followed by the pose planning at each detected point based on the proportional relationship of position variations at the detected points.The linear operation of the orthogonal matrix corresponding to the pose is unclosed, so the Euler angle or quaternion transformation method is usually adopted for interpolation [13].However, the Euler angle method has multiple solutions, and the quaternion method may also encounter illegal situations caused by the rounding errors.To resolve the above problems, a matrix information geometry method [14][15][16] is used to plan the pose of the manipulator at each detected point in this paper.The pose of the manipulator in motion corresponds to an element in the special orthogonal group [17].Based on the linear property of the Lie algebra of the special orthogonal group, an algorithm is proposed to plan the pose of the manipulator at each detected point.The algorithm facilitates the steady movement of the probe at the manipulator end according to the planned position and pose.A matrix Lie group and its Lie algebra are transformed through the exponential map and the logarithmic map.In general, the information geometry algorithm does not need to discuss multiple solutions [18], and the resulting pose matrices are also legal.And, according to the physical meaning of the Lie algebra of special orthogonal groups, the angular acceleration of the end effector is a uniform acceleration and deceleration motion.
The matrix information geometry algorithm for the pose planning of the manipulator can address the issue of unclosed linear operation of the orthogonal matrices.In fact, due to the nonlinear relationship between orthogonal matrices of two different poses, it is difficult to determine the transition pose of these two poses by ratio.In this manuscript, the pose planning algorithm is constructed based on the geometric structure of the special orthogonal group, and the pose at the detected point is planned using the straightness of the Lie algebra.The proposed algorithm promotes the stable movement of the manipulator along the planned trajectory.
The geometric structure of the special orthogonal group will be introduced first in the following text.Then, the algorithm for planning the pose of the manipulator will be proposed based on the linear closure of the Lie algebra on the special orthogonal group.Finally, the algorithm will be verified through manipulator motion experiments.

Geometric Structure of the Special Orthogonal Group
The manipulator's position and pose in the space can be represented by 4 × 4 matrices.All the orthogonal matrices of the same order with the determinant of 1 form a special orthogonal group, which is a critical matrix compact Lie group.Next, a brief introduction is given to the concepts of special orthogonal groups and their Lie algebras, which are the theoretical basis for designing the pose-planning algorithm [18][19][20][21].
G becomes a Lie group. 2

Journal of Robotics
The tangent space at the identity element of the Lie group G is its Lie algebra g, which is a linear space.The exponential map from the Lie algebra g to the Lie group G is denoted as exp.Although, the exponential map is neither injective nor surjective, it is a diffeomorphism from the neighborhood of the identity element I of G to the neighborhood of the identity element 0 of g.The logarithmic map is indicated by log, which is the local inverse map of the exponential map.
Lie groups in which all elements are matrices, are described as the matrix Lie groups.Exponential and log maps enable the transformation between the matrix Lie group and Lie algebra, and play an essential role in the Lie group theories.The exponential map from the Lie algebra to the matrix Lie group G is represented as follows [22,23]: where V is an element in the Lie algebra g.The log map from the matrix Lie group G to its Lie algebra g is defined as follows: where X belongs to the neighborhood of the identity element I of G.
2.2.Special Orthogonal Group.The orthogonal group is expressed as follows: where < n×n denotes all the elements in the real matrix.Its Lie algebra represents all the elements in the antisymmetric matrix as follows: The special orthogonal group is as follows: Its Lie algebra is soðnÞ, and The pose variation of the rotating manipulator end effector is described by a 3 × 3 rotation matrix R, which is an element in the special orthogonal group SOð3Þ.A series of motion poses of the manipulator corresponds to the several elements in SOð3Þ.
The only expression for the antisymmetric matrix U in soð3Þ is determined as follows: where ω ¼ ðω x ; ω y ; ω z Þ T 2 < 3 and ω is the angular velocity of the manipulator end effector [24].In particular, the basis of the Lie algebra soð3Þ of SOð3Þ is as follows: where ½⋅;⋅ is the Lie bracket.The Lie bracket of U 1 ; U 2 2 soðnÞ is defined as follows: For any arbitrary U 2 soð3Þ, the exponential map has a close form [24]: with R 2 SOð3Þ and trðRÞ As a connected compact Lie group, the special orthogonal group SOðnÞ is a complete manifold, and any two points are connected by a geodesic curve.The geodesic curve on SOðnÞ is represented as follows: where γðtÞ begins with R and takes RU as its direction.The geodesic curve connecting the two points R 1 ; R 2 is expressed by: Journal of Robotics The geodesic distance between the two orthogonal matrices R 1 ; R 2 is calculated by: 3. Algorithm for Planning the Manipulator Trajectory 3.1.Generation of Aerial Paths.The basic description of trajectory planning is to move a manipulator from the initial position and pose fT s g to the desired position and pose fT e g.As shown in Figure 2, the tool coordinate system of the manipulator moves from the current value to the final target value.This process includes changes in the position and the pose of the tool.The detected points on the motion trajectory of the manipulator should be planned according to the motion period T of the manipulator and the external axis.
Owing to the introduction of the automatic control system, the manipulator velocity at the starting stationary state is zero.Therefore, the detected points should be planned to make sure that the manipulator can arrive at the next detected point within a motion period.Violent acceleration or deceleration give rise to the parts' depreciation and the sudden vibration of the manipulator.Therefore, the manipulator movement should conform to the acceleration or deceleration movement law and the movement path should be smooth.The velocity at the manipulator end is defined as a continuous function of time, which makes the displacement a function of time with a continuous first derivative.Figure 3 illustrates the relationship between the time and the velocity of the tool at the manipulator end on the whole planned path.From 0 to t 0 , the manipulator moves with a constant acceleration a.When the maximum velocity V max of the manipulator is reached, it continues to move at a constant velocity.From t 1 to t 2 , the manipulator moves at a constant deceleration − a, where t 2 − t 1 ¼ t 0 ; V max ¼ at 0 .Using the movement velocity function of the manipulator: the displacement function of the manipulator is obtained by: The above trajectory planned ensures the steady acceleration and deceleration of the manipulator end and prevents its rapid acceleration and deceleration in the specified motion period.

Algorithm for Planning the Position and Pose of the
Manipulator End.First, we introduce how to convert a motion trajectory segment of the manipulator into a matrix in SOð3Þ.
The detected point is described as the coordinate system of the tool position and pose relative to the operating platform.The position and pose at each detected point are expressed as ðx; y; z; α; β; γÞ.Here, x; y; z is the position of the manipulator end in the coordinate system of the operating platform, and α; β; γ are the angles by which the coordinate system at the detected point turns relative to the coordinate system of the operating platform.
By using the X − Y − Z Euler angle coordinate system method, the rotation matrix A is expressed as follows: In addition to equidistant points in the space, the motion velocity, acceleration a and motion period T of the manipulator end effector should also be planned in the trajectory planning process.The motion velocity should be smaller than the maximum rated velocity V max of the manipulator end, and the maximal motion velocity is generally fixed at 80% of the maximum rated velocity.In our experiments, the velocity is 100 mm/s, the acceleration is 100 mm/s 2 , and the motion interpolation period is 1 ms.If the motion velocity exceeds the maximum rated velocity of the manipulator end, the torque overload may occur to the manipulator in its moving process, resulting in abnormal sounds, increased wear, and even damage to the manipulator.The acceleration should also be lower than the maximum rated acceleration of the manipulator.The motion period should be set according to the German Beckhoff motion controller scan period of the integrated system.
The motion period of the manipulator is set to be the same as the scan period T (1 ms) of the automatic control system.Therefore, the distance between equidistant points should not be greater than the product of the maximum velocity V max and the period T. When the distance exceeds the product, the manipulator is unable to arrive at the next detected point from the current position within one period T even if it moves at the maximum velocity.In ultrasonic testing, the ultrasonic probe transmits and receives ultrasonic signals according to the position.Thus, a manipulator position deviation on the space trajectory will cause errors in the imaging results of ultrasonic testing.Due to the symmetry between the accelerated motion and decelerated motion of the manipulator, the distance and the time of the uniformly decelerated motion can be acquired in the same way.In ultrasonic testing, trajectory points refer to the position and pose of the probe held by the manipulator.In the experiments, the uniform acceleration, uniform velocity, and uniform deceleration motion trajectory are planned based on the probe detection speed of 100 mm/s, acceleration of 100 mm/s 2 , and motion interpolation motion period of 1 ms.According to the principle of trapezoidal acceleration and deceleration, the polynomial scanning trajectory is interpolated.The typical cubic curve interpolation data points are shown in Figure 4.
The algorithm for planning the position and the pose at each detected point is developed as follows.
3.2.1.Calculation of the Positions at Detected Points.The coordinates of scattered points on the workpiece collected by the laser are fitted to acquire the cross-sectional curve of the workpiece surface.Let the number of the detected points on the cross-sectional curve be N þ 1, and the detected point is represented as P 0 ; P 1 ; ⋯; P N .Their corresponding position coordinates and pose matrices are denoted by X 0 ; X 1 ; ⋯; X N and A 0 ; A 1 ; ⋯; A N , respectively.Assuming that the coordinates X 0 and pose matrix A 0 at the initial point P 0 and the coordinates X N and pose matrix A N at the target point P N are known, the acceleration a is adjusted so that the duration of accelerated, uniform, and decelerated motion is an integral multiple of the period T.
Due to the small distance between adjacent detected points, the arc length between two points approximates a straight-line segment.
If the coordinates X k of the detected point P k are known, the coordinates X kþ1 of the detected point P kþ1 can be obtained by using the cross-sectional curve equation and derivative function of the workpiece to be detected.The arc length between detected points P k−1 and P k is denoted by s k .As the manipulator end tool moves at a constant acceleration, uniformly and at a constant deceleration on the entire trajectory planned, the arc length s k is symmetrical.

Journal of Robotics
In fact, according to the displacement function (Equation ( 17)), we have T .The arc length s k satisfies as follows: 3.2.2.Calculation of the Poses at Detected Points.Given the pose matrices of the end effector of the manipulator at the starting and target points, using logarithmic mapping, their Lie algebras are obtained.By utilizing the proportional relationship of arc lengths between detected points, calculate the Lie algebra of all detected points.Finally, using exponential mapping, the pose matrices of all detected points can be obtained.
First, using the ratio relationship between the arc lengths of detected points, let Based on the linear property of the Lie algebra on the special orthogonal group, the Lie algebra at each detected point is described as follows: where According to the exponential map from the Lie algebra to the Lie group, the pose matrix at each detected point is further gotten as follows: Thus, the pose matrix at each detected point between the start and target points is acquired (Figure 5).
Figure 6 shows the flowchart of Algorithm 1.

The Rotation Equivalent Axes of the Poses at All
Detected Points Are Located on the Same Plane.The manipulator can make any pose by rotating a certain angle around an appropriate axis [25].Specifically, the coordinate system {B} first coincides with a known coordinate system {A}.Then based on the right-hand rule, the coordinate system rotates θ angle around a unit vector b K .b K is called the equivalent axis of a finite rotation.Let the pose matrix be R, and b K is the unitized angular velocity: Calculate the length Equation (19) of the motion trajectory segment between P k-1 and P k Calculate the Lie algebra at each detected point: Position coordinates of all detected points Detection path curve, initial position and pose of the manipulator, as well as the target position and pose

Pose matrices A k at all detected points
Calculate pose matrix at each detected point: Input: detection path curve, initial and target positions, and poses of the manipulator ðx; y; z; α; β; γÞ i .Output: position coordinates and pose matrices at all detected points.
(1) According to the length Equation ( 19) of the motion trajectory segment between P k−1 and P k as well as the curve equation of the detection trajectory, the position coordinates X k ðk ¼ 1; ⋯; NÞ of each detected point P k are calculated.
(2) Let the poses of the manipulator at the initial and target positions be A 0 and A N , respectively, and the Lie algebra at each detected point is expressed as Then the pose matrix at each detected point can be calculated by Algorithm 1: Algorithm for the position and the pose matrix at each detected point.
The pose at each detected point in Algorithm 1 is acquired by calculating the ratio of the Lie algebra of the initial pose to that of the target pose.From the one-to-one mapping relationship between the Lie algebra (Equation ( 7)) and the equivalent axis b K , it can be seen that the rotation equivalent axes of the poses at all detected points obtained by Algorithm 1 are located on the same plane.

The Manipulator End Effector Moves at a Constant
Angular Acceleration and Deceleration.According to Equation ( 22), the angular velocity ω k of the manipulator end effector at the detected point P k satisfies The angular acceleration a k at the detected point P k is approximated by the reverse difference of the angular velocity ω k as follows: Bringing Equation ( 22) into the above equation, we have Obviously, Equation (29) satisfies symmetry as follows: Hence, the manipulator end effector moves at a constant acceleration, uniformly and at a constant deceleration in the detection process.

Numerical Simulations of Pose Planning
In the simulations, a Turin TKB070S manipulator is applied to read the motion trajectory, and a Gocator-2350D laser is used to retrieve the coordinates of discrete points on a crosssection of the aircraft engine blade, where the step of the scattered points was 0.5 mm.
First, the spatial position of each detected point is calculated by Algorithm 1.
The scattered points are fitted with a quadratic curve according to the curve shape.The motion period T of the manipulator adopts 1 ms and the acceleration a is set to 100 m/s 2 .The coordinates of the start and target points obtained by the laser are X 0 and X N ðN ¼ 11049Þ, respectively.The arc between two adjacent points approximates to a straight-line segment.The coordinates X k of all detected points of the manipulator can be calculated by Equation (19).
Second, the pose matrices of the manipulator at all detected points are computed.In the experiment, the pose matrices at the start point P 0 and target point P N are as follows: respectively.To acquire the pose matrix A k at each detected point on the curve, the Lie algebra of each point is solved by Equation ( 23) based on its linear property, and then the Lie algebra is exponentially mapped to the Lie group by Equation (24) (Figure 7).According to the one-to-one mapping relationship between the Lie algebra Equation ( 7) and the equivalent axis b K Equation ( 25), the rotation equivalent axes of pose matrices are obtained, which are located on the same plane (Figure 8).
Third, the desired angle of each axis of the manipulator effector is calculated by solving the inverse kinematics.By arranging the effector angles at all detected points in order of arrival time, the functional relationship of the axis variation of the manipulator with time is obtained.Figure 9 shows the change in the axis angle when the tandem-type six-axis manipulator walks along a trajectory segment.The changes in the manipulator axis and external axis with time can be observed directly.
However, it is impossible to determine whether the axis velocity changes suddenly or the angular velocity of the rotation axis exceeds the motor limit merely according to the displacement of an axis in a time period.Figure 10 presents the functional relationship between the time and the axis angular velocity of the tandem-type six-axis manipulator.It can be seen that the function is continuous, which confirms that the axis variation is also continuous and no sudden velocity change occurs when the manipulator moves along this trajectory segment.
It can be seen that the function is continuous, which confirms that when the manipulator moves along this trajectory segment, the changes in the axis are also continuous and there is no sudden velocity change.
The calculation results of Algorithm 1 are compared with those of the classical Euler angle method and the quaternion  Journal of Robotics method.For this set of data, the geodesic distance between A k and the pose matrices gotten by the Euler angle method or the quaternion method is calculated by Equation (15).As shown in Figure 11, the maximum error of the geodesic distance at each detected point does not exceed 0.025.It indicates that Algorithm 1 is consistent with the two classical methods.At the 5,500th detected point, Algorithm 1 and the two classical methods show the largest difference.Figure 12 compares the poses obtained by Algorithm 1, the Euler angle method, and the quaternion method at the 5,500th detected point.It can be seen that the difference in pose matrices obtained by the three algorithms is very small.Finally, the angular accelerations of the manipulator end effector obtained by three algorithms are compared.show the angular accelerations of the manipulator end effector relative to the X, Y, and Z axes in the detection process.It shows that the end effector moves at a constant acceleration, uniformly, and at a constant deceleration.This motion pattern ensures stable and smooth rotation of the manipulator end effector and prevents rapid acceleration.
To sum up, the numerical experiments verify the effectiveness of the trajectory point planning algorithm based on the matrix information geometry.In addition, this algorithm also has the following advantages: all the rotation equivalent axes of the pose matrices are located on the same plane.During the detection process, the end actuator moves at a constant angular acceleration and a constant angular deceleration, which helps the manipulator move more stably.

Experiment of Trajectory Planning
In numerical simulations, discrete point coordinates of a cross-section of an aircraft engine blade are retrieved using a Gocator-2350D laser.After curve fitting, the coordinates of 11,050 detected points are obtained through interpolation.For the given poses Equations ( 31) and (32) of the starting and ending points, the poses at each detected point are obtained using Algorithm 1, the Euler angle method, and the quaternion method, respectively.
In this section, based on the inverse kinematics solutions of three sets of pose matrices, the Turing TKB070S six-axis manipulator is operated separately.First, the equivalent axis of the attitude matrix obtained by Algorithm 1 is verified to be located in the same plane.Second, for the three algorithms, the actual data and numerical simulation data of the end effector angular acceleration are compared.
Specifically, the pose matrices obtained from Algorithm 1 are transmitted to the controller.After the manipulator executes according to instructions, the timer captures the 170 real-time pose matrices corresponding to the end effectors of  Journal of Robotics the manipulator.Then, 170 pose matrices are obtained by forward solutions and their equivalent axes are calculated from Equation (25). Figure 16 shows that the rotation equivalent axes of the 170 pose matrices of the manipulator end effector are located in the same plane, which is consistent with Figure 8.
Then, compare the experimental data and numerical simulations of the angular acceleration of the end effector of the manipulator obtained from the three algorithms.Figure 17(a) shows that the data of Algorithm 1 is distributed near the trapezoidal curve.Figures 17(b) and 17(c) illustrate that the data from the Euler angle algorithm and the quaternion algorithm are also consistent with the numerical simulation curves.Therefore, the three algorithms are feasible in the motion experiments of the manipulator and consistent with numerical simulations.
In summary, the experimental results are consistent with the numerical simulation results, indicating the controllability of the detected point planning algorithm based on matrix information geometry.During the test, the angular acceleration of the end effector is a uniform acceleration and deceleration movement, which ensures a more stable movement of the manipulator.

Conclusion
(1) A manipulator detection trajectory planning algorithm is proposed based on the geometric structure of the special orthogonal group.The linear operation of the orthogonal matrix corresponding to the manipulator pose at each moment is unclosed.Thus, based on the mapping relationship between the special orthogonal group and its Lie algebra and the linear property of the Lie algebra, the manipulator pose at each detected point is acquired.(2) The trajectory planning algorithm based on the matrix information geometry achieves the same results as the typical Euler angle and quaternion transformation methods.The matrix information geometry algorithm does not have the problem of multiple solutions in the calculation process.Meanwhile, the rotation-equivalent axes of the pose matrices are located in the same plane.Moreover, the manipulator end effector moves with constant angular acceleration and deceleration in the detection process, enabling the effector to move stably along the given trajectory.

FIGURE 1 :
FIGURE 1: Automatic monitoring system for civil aviation engine blades.

FIGURE 2 :
FIGURE 2: Manipulator moving from the initial pose to the target pose.

a max a 0 ðbÞFIGURE 3 :
FIGURE 3: Velocity and acceleration of the manipulator end: (a) velocity and (b) acceleration.

FIGURE 5 :
FIGURE 5: Algorithm for the pose matrix at each detected point.
log mapping, the Lie algebras of the pose matrices A 0 and A N are obtained as follows: angular velocities of the pose matrices A 0 and A N are as follows:respectively.The equivalent axes of the pose matrices A 0 and A N are as follows:

FIGURE 12 :FIGURE 13 :
FIGURE 12: Poses obtained by three algorithms at the 5,500th detected point.

FIGURE 16 :ðaÞFIGURE 17 :
FIGURE 16: Rotation equivalent axis corresponding to each pose obtained from the experiment.