Formal Kinematic Analysis of a General 6 R Manipulator Using the Screw Theory

1Beijing Key Laboratory of Electronic System Reliability Technology, Capital Normal University, Beijing 100048, China 2College of Information Science and Engineering, Capital Normal University, Beijing 100048, China 3College of Information Science and Engineering, University of Chinese Academy of Sciences, Beijing 100049, China 4School of Mechanical Engineering and Automation, Beijing University of Aeronautics and Astronautics, Beijing 100191, China


Introduction
Kinematic analysis [1] is the study of motion of a machine or mechanism without considering the forces that cause the motion.The key point behind the kinematic analysis is to identify the parameters, like position, displacement, velocity, and acceleration of end effector relative to the base.In this respect, there are two types of problems: direct kinematics and the inverse one [2].For the former, the joint variables are given and the task is to find the location of the end effector.For the latter, the location of the end effector is instead given while the task is to find the joint variables necessary to bring the end effector to the desired location.Kinematic analysis allows us to extract useful information about the workspace, dexterity, and precision of a given robot [3].Generally, it is necessary to perform kinematic analysis during the design stage to verify that the designed robot is capable of serving the given purpose [4].
Traditional techniques, such as numerical methods and simulations, are commonly used to verify the kinematic performances of robots.For example, the famous mathematical software, like Maple and Mathematica, offer complete packages that can be applied in kinematic analyses.Despite being very efficient in computing numerical or symbolical solutions, these packages cannot be considered 100% reliable due to the fact that huge numerical and symbolic procedures in their cores are unverified themselves [5].Inaccuracies in kinematic analysis could lead to disastrous consequences, including a robot's breakdown, and thus developing more reliable kinematic verification techniques is an urgent need [6].In the past several decades, formal methods have emerged as a successful verification technique for both hardware and software systems.Thus applying this verification technique in the case of robotic kinematics could be a wise move.Basically, there are two kinds of formal verification methods: model checking and theorem proving.Specifically, theorem proving, based on higher-order-logic, can provide the ability to formally reason on the correctness of kinematic analysis and then guarantee the high reliable kinematic performances of robots.
In the present work, we perform the formal kinematic analysis on a general 6R manipulator by means of screw theory.The formalization of the direct kinematic equations and the singularity description are realized by extending the recently formalized analytical screw theory in HOL4.Screw theory has two main advantages in describing robotic kinematics [2].One is that it allows a global description of rigid body motion, which does not suffer from the singularities that are inevitable when one chooses to represent rotation via Euler angles.The other is that it provides a very simple geometric description of rigid motion, which greatly simplifies the analysis of mechanisms.Compared with the traditional methods based on the Denavit-Hartenberg parameters, the screw theory facilitates the kinematic analysis to a large extent.
The utilization of formal methods in ensuring the correctness of continuous and physical systems has been increasingly advocated these days.Meanwhile, in some safety-critical applications, the traditional methods of verification are helpful and easy but insufficient to be used because they cannot satisfy the security requirements in relevant standards such as IEC 61508 [7].As a complementary technique, formal methods have been highly recommended and frequently applied to enhance the accuracy of robotic analysis.For example, formal verification of the movements of a Samsung Home-Service Robot (SHR) was conducted by analyzing its discrete control software using the Esterel model checker [8].Li et al. verified the algorithm of the collision-free motion planning for a dual-arm robot in HOL4, and found an inconsistency in the planned motion [9].Täubig et al. verified the safety area algorithm of an autonomous robot in Isabelle/HOL [10].Farooq et al. applied high-order-logic theorem proving in kinematic analysis of a biped robot and verified the continuous mechanical system in HOL-light without using any abstractions and thus maintaining a higher accuracy [5].Enlightened by these works, we plan to apply high-orderlogic theorem proving for kinematic analysis based on screw theory [11], which is a more general way to analyze the three dimensional robotic kinematics and meanwhile easier to implement compared with Farooq et al. 's method.
The rest of the paper is organized as follows.In Section 2, we provide a brief introduction about kinematic analysis of a general 6R manipulator.The formalization of these foundations of kinematic analysis is provided in Section 3. We apply this formalization to conduct the formal kinematic analysis on a general 6R manipulator in Section 4. Finally, Section 5 concludes the paper.

Kinematic Analysis of Open-Loop 6-DOF Serial Manipulator
This section introduces kinematic analysis and the singular configuration of the 6R manipulator.

Direct Kinematics Based on Screw Theory.
A general 6R manipulator, depicted in Figure 2, has 6 rotary degrees of freedom (DOF).Point  denotes the origin, and u, k, and w are the three orthogonal unit vectors of the end effector.
Using the Denavit-Hartenberg (D-H) method, a loop closure equation can be written as follows to get the transform matrix between the end effector and the base: where    denote the D-H transformation matrix of each joint.The D-H method requires the designers to establish the local Cartesian coordinate system one by one for each joint at first and then to calculate the corresponding D-H transformation matrices [2].The computation of the these transformation matrices is a cumbersome work, if the robotic structure is complex.Thus the D-H method is not recommended in this paper.
Since the manipulator is composed of a group of rigid bodies.The rigid-body kinematics can be used to represent its motion.According to screw theory, each rigid body motion can be regarded as a rotation about an axis plus a translation parallel to that axis [1].Therefore, the movement of each joint of a general 6R manipulator can be denoted by a specific screw, which takes the form of a line vector such as  1¡  1 ,  2¡  2 , and  3¡  3 .
Moreover, the instantaneous motion of end effector relative to the inertial coordinate system can also be denoted as  ¡   .Thus, a loop closure equation of direct kinematics can be written as follows: where According to the direct kinematic equation of the general 6R manipulator, the screw motion parameters of the end effector can be calculated (see Table 1).
The parameters of the screw motion determine the position and velocity of the end effector, and thus the direct kinematic problem can be solved.

Manipulator Jacobian and Singular Configuration.
Traditionally, one describes the Jacobian of a manipulator by differentiating the forward kinematics map.For the general rigid body transformation, forward kinematics map can be represented as  : R 3 → SE(3).According to screw theory, the manipulator Jacobian can be written in terms of twists, which is the infinitesimal generator of a special Euclidean group (SE(3)).Because of the exponential properties, the Jacobian can be described in a very natural and explicit way, which not only highlights the geometry of the mechanism but also has none of the drawbacks of a local representation [1].For the general 6R manipulator, its Jacobian can be easily written in terms of screw as follows: Table 1: The screw motion parameters of the end effector.

Parameters Value
Pitch where ¡   is the screw coordinates of each joint.When the manipulator has a given configuration, the instantaneous velocity of the end-effector can be expressed as: where θ represents the angular velocity.When the manipulator is under a singular configuration, it is unable to achieve instantaneous movement in a certain direction, and its Jacobian is not invertible in this case.When designing the manipulator, the singular configurations should usually be avoided, if possible.

Formalization of Kinematic Analysis
Formalization of the screw theory and kinematic analysis are proposed in this section.This work is based on the formalization of the matrix and function matrix theories in HOL4 [12,13].
There are two significant requirements for the kinematic analysis based on screw theory in a high-order-logic (HOL) theorem prover: formal reasoning about geometrical principles and formalization of screw theory.The formalization of Cartesian-coordinate based geometrical theory along with its proof, which is based on -dimensional real vectors in the Euclidean space, has been realized in theorem provers like HOL-light and HOL4 [14][15][16].Based on the foundation of geometry theory, the function matrix and vector theory can be employed to describe the positions of the joints, and the set theory can be used to represent obstacles' boundaries in Cspace.These two theories are referred to as domain theories [9].Therefore, the contour of the robot can be formalized as a convex polygon while obstacles can be modeled as connected sets of points [9,10].After the geometrical principles are formalized, one can begin to formalize the screw theory of rigid bodies.The main formalization process of screw theory is depicted in Figure 2. Based on the domain theory, the library of screw theory in HOL4 is constructed by the basic libraries of screw data type, rigid body theory, and lie group theory, which is a powerful tool in analyzing the robot manipulator.

Formalization of Screw Theory.
A screw takes the form of + ∈ , where  and  are in the data type of (real 3 ), and ∈ is a Clifford factor, having the feature of ∈ ̸ = 0 and ∈ 2 = ∈ 3 = 0.The screw theory formalized in HOL4 lays the basic foundation for the formal kinematic analysis of the general 6R manipulator in this paper.
The function screw accepts two vectors  and  of data type (real 3 ), which are the two components of a 3D vector pair returned by the function.In HOL4, we define a new type (screw) to denote the type of 3D vector pair.Screw has the basic operations including addition and reciprocal product.They are defined in HOL4 as follows.
The function FST returns the first component of a vector pair, while the function SND returns the second component.Specifically, the reciprocal product remains constant during the coordinate transformation, which is an important property.
When performing kinematic analysis, the configuration space of the manipulator is an important parameter, which depicts the rotation and translation of manipulator.It is the set of all possible configurations of the manipulator.Each configuration of the manipulator that is free to rotate relative to the base can be identified by a unique kinematic map  ∈ SO (3).
The definition of SO(3) is written in HOL4 as follows.
Definition 4 (special orthogonal group).Consider It is important to note that the kinematic map of a revolute joint can be identified by an orthogonal matrix.
Generally, every configuration of the manipulator that is free to move relative to a fixed frame can be described by a unique kinematic map  ∈ SE(3).Thus, special Euclid group is the configuration space for the general manipulator.Similarly, we have the definition of SE(3) as follows.According to the rigid body kinematics, any orientation  ∈ SO(3) is equivalent to a rotation around a fixed axis  ∈ R 3 through an angle  ∈ [0, 2], which is the Euler's theorem representing the rotation of a rigid body.The theorem can be formalized as follows.( In the theorem the function so 3 has the date types of (matrix → matrix set), which is a special matrix set that describes the rotation of a rigid body around a fixed axis.
The function  exp represents the exponentials of matrix.
A general rigid body motion is a screw motion, which includes a rotation about an axis combined with a translation parallel to that axis.This kinematic property is commonly called the Chasles theorem, which is also the foundation of screw theory.It is formalized as follows.

Theorem 7 (Chasles theorem). Consider
It is important to note that the function se 3 has the date type of (matrix → matrix set), which is a special matrix set that represents the screw motion.

Direct Kinematics.
We formalized the general 6R manipulator, showed in Figure 1, as the following high-order-logic function in HOL4.
Definition 8 (joint := screw).As is depicted in Section 3, every joint can be denoted by a specific screw.So the definition of screw can be overloaded for joint.Particularly, for a general 6R manipulator, all the joints are revolute, which can be denoted as a line vector.A line vector is a special screw, of which the two components are orthogonal.For a prismatic joint, the first component of the 3D vector pair is a null vector.

Definition 9 (serial manipulator). Consider
|− ∀ , serial manipulator   = sc SUM (count ) . ( The function serial manipulator accepts two variables  and . denotes the number of joints, which has the data type of (num). is of the type of (num → screw), which means a series of screws, representing the joints of the manipulator one by one.Sc SUM(count ) is a special function defined in HOL4 to realize the summation of screw.
The general 6R manipulator is a special serial manipulator whose joints can be denoted as line vectors.The definition of a general 6R manipulator is specialized as follows.
A serial manipulator generally consists of several links in series through various types of joints such as the revolute joints and the prismatic ones [2].In screw theory, a serial manipulator can be regarded as the summation of screws of all the joints.

Jacobian Matrix and Singular Configuration.
When the screw coordinate of each joint is specified, the manipulator Jacobian can be defined in an easy way.
Definition 11 (screw Jacobian).Consider ∀, , , , , , sc jacobian       = screw 2        .(15) It is important to note that the function sc jacobian has the data type of (screw → matrix), which is a map from screw to matrix.This procedure is realized by a special function screw 2 m, which accepts 6 parameters and returns a 6 × 6 matrix.
After defining the manipulator Jacobian matrix, we can test its determinant to decide whether or not the manipulator is under a singular configuration.

Application: The General 6R Manipulator
The general 6R manipulator is used as our experimental platform.As is depicted in Figure 3, we conducted kinematic analysis on this general 6R manipulator as follows.First, a formal model is constructed for the manipulator in higherorder-logic.The definition of the manipulator is realized according to the method in Section 3. The second step is to calculate the screw coordinates of each joint according to its angular velocity.For a given joint,  and  are its regular velocity and position vector, respectively, relative to the inertial coordinate system.The screw has the form of a 3D vector pair: (;  × ).
Next, we have to test whether the manipulator is under a singular configuration.According to the Jacobian matrix defined in Section 4, the singular configuration is tested as follows: When the equation holds, the manipulator is under a singular configuration.Otherwise, the configuration is normal.If the manipulator is not under a singular configuration, the direct kinematic equation can be formalized in HOL4 as follows: Then, based on the screw's parameters, we can calculate the transformation map of the screw motion.Last but not the least, the manipulator's motion can be represented as a homogeneous 4×4 matrix called manipulator transformation map.Then, the manipulator transformation is used to make comparison with the screw motion transformation.When the two transformations are equivalent, it can be concluded that the design of the general 6R manipulator is formally correct according to the Chasles theorem.Otherwise, some design faults may exist in the manipulator.Last, the above method is employed to make verification on the 6R manipulator with normal configuration (see Figure 4) and that with singular configuration (see Figure 5), respectively.

Mathematical Problems in Engineering
In Figure 4, the screw coordinations of each rotary joint are specified by two steps.Firstly, the rotation axis of each joint, which is denoted as   , is specified one by one as follows: Secondly, the position vector of each joint is calculated as follows: According to these two steps, the screw coordinate of each joint can be specified as (  ;   ×   ).Then, the screw coordinates of all the joints can be applied to make verification on the direct kinematic equation by the above method.Specifically, if the rank of the manipulator Jacobian is reduced when the manipulator is under a certain configuration, it can be inferred that the manipulator has lost several degrees of freedom, which means that it is under a singular configuration.
As is shown in Figure 5, a 6R manipulator is singular which has the special parameters as follow: For this singular configuration of the 6R manipulator shown in Figure 5, the manipulator Jacobian takes the following form: Obviously, the determinant of this matrix is 0; that is to say, the manipulator is under a singular configuration.More generally, any two rotation axes of joints which are parallel have a chance to generate a singular configuration of a 6R manipulator.
The proof scripts for the definitions and theorems of screw theory and rigid body motion, including the verification of the general 6R manipulator, are composed of approximately 2,000 lines of codes.It takes about 800 hours for a new HOL4 user to accomplish the development.The formal method presented in this section is a sound supplement to the traditional verification methods.Meanwhile, the present formal method can be reused when dealing with other manipulator mechanisms, which extends the verification scope of theorem proving to a great extent.

Conclusion
This paper proposes a high-order-logic theorem proving method for kinematic analysis, which is an essential design step for all robotic manipulators.In terms of the high-orderlogic, we can formally model the geometrical relationships between the mechanical links and joints in their true form.With the help of the domain theory and screw theory HOL4, the formal analysis of robotic manipulator kinematics becomes much easier and intuitive in geometry.Meanwhile, the inherent soundness of theorem proving, which is not shared by any other existing computer-based kinematic analysis technique to the best of our knowledge, guarantees the correctness of analysis and ensures the availability of all preconditions of the analysis as assumptions of the formally verified theorems.Thus the proposed approach can be very useful for the kinematic analysis of safety-critical robots.The main challenge in the proposed approach is the enormous amount of user intervention required due to the undefined nature of the logic.When dealing with large-scale practical problems, the theorem proving method also has its drawback, which greatly limits the development of the method.We propose to overcome this limitation by formalizing kinematic analysis foundations and verifying their associated properties.We can also combine the theorem proving with other methods when dealing with large-scale problems.Due to the advantage of screw theory, formal reasoning support for other motion related parameters, such as displacement, angle, velocity, and acceleration, may also be developed by using our results along with the topological and analytic foundations for vectors available in HOL4.This verification will pave the way for conducting formal kinematic analysis for a much wider application range of theorem proving.Considering the complexity of inverse kinematics of the general 6R manipulator, this paper did not perform inverse kinematics analysis formally, which can be referred to as a promising future work [17].

Figure 2 :Figure 3 :
Figure 2: Overview of the formalization of screw theory.

Q
The joint in black is at a singular position which has the special parameters as follow: