Formal Verification of Robot Rotary Kinematics

Abstract

With the widespread application of robots in aerospace, medicine, automation, and other fields, their motion safety is essential for the well-being of humans and the accomplishment of vital socially beneficial programs. Conventional robot hardware and software designs mainly rely on experiential knowledge and manual testing to ensure safety, but this fails to cover all possible testing paths and adds risks. Alternatively, formal, mathematically rigorous verifications can provide predictable and reliable guarantees of robot motion safety. To demonstrate the feasibility of this approach, we formalize the mathematical coordinate transformation of a robot’s rigid-body kinematics using the Coq Proof Assistant to verify the correctness of its theoretical design. First, based on record-type matrix formalization, we define and verify a robot’s spatial geometry by constructing formal expressions of the matrix’ Frobenius norm, trace, and inner product. Second, we divide rotary motion into revolution and rotation construct and provide their formal definitions. Next, we formally verify the rotational matrices of angle conventions (e.g., roll–pitch–yaw and Euler), and we complete the formal verification of the Rodriguez formula to formally verify the correctness of the motion theory in specific rotating kinematics problems. The formal work of this paper has a variety of essential applications and provides a generalizable kinematics analysis framework for robot control system verification. Moreover, it paves the way for automatic programming capabilities.

1. Introduction

In recent years, robots have been adopted in many operational areas that require high safety protocols (e.g., unmanned aerial vehicles (UAVs), surgery, and autonomous driving). Hence, the reliability and safety of robot development have become topics of considerable research interest [1]. Although most robots operate in structured environments with extensive testing and parametric fail-safes, manual testing programs often do not consider all high-risk scenarios, which compounds the risk of unforeseen mishaps. Formal methods [2,3] are crucial for ensuring the reliable design and development of complex robot systems, including their safety measures and fail-safes. Accordingly, our purpose in this research is to introduce a formal method for robot safety verification.

Our focus is on the formal verification of robot rotary kinematics. As an application of kinematic geometry, rotary kinematics [4] describes purely geometric problems (e.g., positions and orientations of robot motions). Thus, robot motion control [5] relies on kinematic descriptions and can be validated mathematically. Our objective is to formally verify the rotary problems of robot kinematics using the Coq Proof Assistant [6,7].

In a robot system, links and manipulators are considered rigid bodies; hence, vector-matrix theory [8,9] can be used to establish a general description of their positions and movements with respect to a global coordinate system, G. Robot manipulators can rotate and move around each other. Thus, the various robot components’ rigid-body coordinate systems ($B1$, $B2$, …, $Bn$) along each link are referred to in terms of the generalized local coordinate system, B. Let G be the global coordinate system; ${}^B{R}_G$ represents the transformation of vector r from G to B, and ${}^G{R}_B$ represents the transformation of vector r from B to G. Through ${}^B{R}_G$ and ${}^G{R}_B$, we can globally define the positions between connecting rods and between the rods and environment.

Formal verification relies on mathematical modeling and deductive reasoning, and it is more rigorous and reliable than empirical and heuristic methods [10,11]. Thus, by formally verifying robot rotary kinematics, we hope to ensure the safety of robot controls and their algorithmic designs. In summary, we apply a formal mathematical method of proof to the detection of “correctness’’ in robot rotary kinematics. The formal framework of robot rotary kinematics is illustrated in Figure 1. First, based on Matrix formalization, we formally define the matrix Frobenius norm, matrix trace, and matrix inner product, providing mathematical formal support for the formal verification of robot rotary kinematics. Then, we divide rotary motion into revolution and rotation, defining their formal representations. Based on this, we perform the formal verification of some common rotary problems, including the Euler angles and Rodriguez formula.

Our contributions can be summarized as follows. First, we expand the formal matrix library based on record type. Specifically, we add formal definitions and proofs of the matrix Frobenius norm, trace, and inner product using a verifiable mathematical process. We divide rotary motion into revolution and rotation, and formally verify the full system coordinate transformation. Moreover, we construct a rotation matrix of angles, which is conventionally used in engineering, to provide the formal definition and verification of Euler angles. Furthermore, we provide the formal definition and verification of the Rodriguez formula. Finally, we demonstrate the applicability of the proposed formal method through case analyses of robot rotary motion problems and demonstrate how to formally verify specific robot rotary problems.

The organization of the paper is as follows: In Section 2, we discuss related works. In Section 3, matrix formalization and rotary kinematics are briefly reviewed. In Section 4, formal spatial geometry verification methods are presented. In Section 5, we formally define “revolution” and “rotation” and verify their pertinent properties. In Section 6 and Section 7, we formally verify Euler angles and the Rodriguez Formula, respectively. In Section 8, we provide case analyses and verify the correctness of their rotary kinematics. Finally, in Section 9, we conclude and discuss future efforts.

2. Related Works

In the field of robot verification, many methods have been proposed in recent years. Ref. [12] focused on the safety of physical human–robot collaboration and proposed a formal verification-based risk analysis method for detecting and modifying hazardous situations early in a model’s design. From a multidisciplinary perspective, ref. [13] developed a two-step verification method by combining formal methods and schedulability analytics, which led to the design of a novel multi-resource locking mechanism for real-time robot services. A drone example was used to demonstrate the benefits of the above approach. Considering that camera pose estimation is a critical component of robot tasks, ref. [14] attempted to formalize and validate the unique and general solutions of pose estimation algorithms using an interactive theorem prover. Some researchers have combined formal validation with a robot operating system (ROS). Based on the formalization of ROS architectural models and node behaviors in Electrum, a formal specification language based on first-order linear temporal logic, ref. [15] integrated their proposed technique into the HAROS framework for the analysis and quality improvement of robotics software. Using a linear logic theorem prover, ref. [16] presented a new technique to formally describe and verify robotic systems to meet the need for an easy-to-use framework for expressing and verifying robot systems in a ROS. In summary, many researchers have applied formal verification methods to various real-world robot applications.

In the industrial domain, ref. [17] studied a representative safety-critical industrial paint robot system from ABB Inc. Through the transfer of hardware abstractions and verification results among tools, they formalized the convergence of a high-voltage system. To meet the demands of user-defined verification goals in robot manufacturing, a layer-based formal scheme was proposed by [18] to support state-space comprehensive verifications.

In healthcare settings, ref. [19] extended the application of formal methods to human modeling using hybrid automata formalism to capture the variability of human behaviors. This resulted in a user-friendly representation that used the Uppaal integrated tool environment for modeling, validation and verification of real-time systems to automatically generate and verify a formal model.

Notably, UAVs and autonomous vehicles are suitable for formal verification. In the context of a distributed UAV scenario representing urban air mobility, ref. [20] constructively provided some insightful ideas to accelerate the development cycle of transforming formally verified models to robotic simulations. To navigate a UAV inside a safety corridor, ref. [21] developed a genetic fuzzy system and demonstrated its adherence to behavior safety specifications. Considering the importance of the formal verification of robotics and autonomous vehicles, ref. [22] developed a binary-search method to extend timed automata models of robotic specifications with dynamic priority schedulers and demonstrated scalability improvements in a real robot case.

Unlike the abovementioned studies, we focus specifically on rotary kinematics problems and use the Coq Proof Assistant [23] to formally verify robot rotary kinematics.

3. Preliminaries

3.1. Matrix Formalization Based on Record Type

Because higher-order theorem provers are based on $\lambda$-type calculus, the basic data structure for traditional matrix realization is a $\lambda$ expression rather than an imperative language version [24]. Hence, the higher-order method is suitable for describing inductive data structures with infinite extensibility (e.g., lists). However, it does not work for data structures with fixed lengths (e.g., vectors and matrices) because there is no direct description method. To make it work, Ma et al. [25] proposed a formal $matrix$ method based on the Coq $Record$ type to verify matrices of any size. In Coq, the $Record$ type is a macro that enables the creation of definitions. From this, a $matrix$ definition is expressed as

$$\begin{array}{cc}\hfill Recor& dMarix(mn:nat):Set:=mkMat\{\hfill \\ \hfill & mat:list\left(listA\right);\hfill \\ \hfill & mat\_length:lengthmat=m;\hfill \\ \hfill & mat\_all\_len:all\_len\_nmatn;\}\hfill \end{array}$$

where $mat$ is a nested list that stores the data of the matrix, and $mat\_length$ and $mat\_all\_len$ are matrix attributes indicating that the length of $mat$ is m and its width is n.

3.2. Rotary Kinematics

Rotary kinematics studies most often use global coordinate system G or local (i.e., rigid-body) coordinate system B to describe robot motion [26]. To verify robot rotary kinematics, we describe the relationship between G and B.

For rigid body D in B, B initially coincides with G. We denote point O as the origin of the coordinate systems [27]. The motion of D around G is “revolution”. In this motion, rigid body D is stationary in B. Hence, for any vector, r, formed by point P and O in D, the equivalence relation of vector r in G and B is given by Formula (1). $B_r$ and $G_r$ are the algebraic representations of r in B and G, respectively, and $\mathit{Q}$ is the revolution matrix.

$$G_r=\mathit{Q}\ast B_r.$$

(1)

For rigid body D in G, the motion of D around B is “rotation”. In this motion, rigid body D is stationary in G. At this time, for any vector, r, formed by points P and O in D, the equivalence relation of r in G and B is given by Formula (2), with $\mathit{A}$ being the rotation matrix [28].

$$B_r=\mathit{A}\ast G_r.$$

(2)

4. Formalization of the Spatial Geometry

4.1. Formalization of the Matrix Trace

In linear algebra, the trace of matrix $\mathit{M}(m\times m)$ is defined as the sum of its diagonal elements, which is also equal to the sum of its eigenvalues, as shown in Formula (3):

$$tr\left(\mathit{M}\right)=\sum _{i=0}^{m-1}{m}_{ii}.$$

(3)

In Coq, we formally solve the matrix trace recursively. In function $tr$, m is an implicit parameter representing the width of the matrix, and $ma$ is a matrix with dimension $m\ast m$. In the recursive function body, $get\_Sum\_tr$, the elements along the diagonal of the matrix are obtained and summed using the $match$ strategy.

$$\begin{array}{cc}\hfill Def& initiontr\{m:nat\}(ma:MatRmm):=\hfill \\ \hfill & (fixget\_Sum\_tr(m0n:nat\left)\right(Zero:R\left)\right(ma0:MatRm0m0\left)\right\{structn\}:R:=\hfill \\ \hfill & matchnwith\hfill \\ \hfill & |0\Rightarrow Zero\hfill \\ \hfill & |S{n}^{\prime }\Rightarrow \hfill \\ \hfill & get\_Sum\_trm0n^{\prime }(Zero+mat\_nthma0(m0-n^{\prime }-1)(m0-n^{\prime }-1))ma0\hfill \\ \hfill & end)mm0ma.\hfill \end{array}$$

4.2. Formalization of the Matrix Frobenius Norm

The Frobenius norm, also known as the F-norm, represents the length of a matrix for an intuitive understanding of matrix theory. Its definition in matrix space ${\mathbb{R}}^{m\ast n}$ is given by Formula (4):

$$\begin{array}{c}\hfill \left\{\begin{array}{c}{\left|X\right|}_F=\sqrt{{\sum }_{i=0}^{m-1}{\sum }_{j=0}^{n-1}{x}_{ij}^2}\\ s.t.\left\{\begin{array}{c}{x}_{ij}\in X\hfill \end{array}\right.\end{array}\right.\end{array}$$

(4)

Accordingly, the formal definition of the F-norm is given next. The recursive function, $get\_Sum1$, inputs list h as a real-number type and outputs the sum of the squares of all elements in the list. Recursive function $get\_Sum2$ inputs nested list $ll$ as a real-number type and provides the sum of the squares of all elements by calling $get\_Sum1$. Function $Frobenius$ has an input matrix-type parameter, $ma$, with m rows and n columns, and function $mat$ provides the data elements as matrix types.

$$\begin{array}{cc}\hfill Def& initionFrobenius\{mn:nat\}(ma:MatRmn):=\hfill \\ \hfill & letll:=\left(matRmnma\right)in\hfill \\ \hfill & sqrt\left(\right(fixget\_Sum2(l:list(listR\left)\right):R:=matchlwith\hfill \\ \hfill & \left|\right[]=>0\hfill \\ \hfill & |h::t=>(fixget\_Sum1(l0:listR):R:=\hfill \\ \hfill & matchl0with\hfill \\ \hfill & \left|\right[]=>0|h0::t0=>h0\ast h0+get\_Sum1t0\hfill \\ \hfill & end)h+get\_Sum2t\hfill \\ \hfill & end\left)ll\right).\hfill \end{array}$$

In Coq, we use $Notation$ to define the Frobenius infix notation.

$$\begin{array}{cc}\hfill Notation“\left|ma\right|”:& =\left(Frobeniusma\right)\left(atlevel70\right).\hfill \end{array}$$

Through the definition of Frobenius, we can formally verify the properties of special matrices.

Lemma 1.

The F-norm of a zero matrix is zero.

$$\begin{array}{cc}\hfill Lemmalemma1:& forall\left\{mn:nat\right\}(ma:MatRmn),ma=RMatrix.MOmn\to \left|ma\right|=0.\hfill \end{array}$$

Lemma 2.

The F-norm of an n-dimensional identity matrix is $\sqrt{n}$.

$$\begin{array}{cc}\hfill Lemmalemma2:& forall\left\{n:nat\right\}(ma:MatRnn),ma=RMatrix.MIn\to \left|ma\right|=sqrtn.\hfill \end{array}$$

4.3. Formalization of the Matrix Inner Product

The inner product of matrices in matrix space ${\mathbb{R}}^{m\ast n}$ is given by Formula (5), where $x_{ij}\in X,y_{ij}\in Y$:

$$X·Y=\sum _{i=0}^{m-1}\sum _{j=0}^{n-1}{x}_{ij}\ast y_{ij}.$$

(5)

Accordingly, the matrix inner product is formally defined as

$$\begin{array}{cc}\hfill & DefinitionDot\_Product\{mn:nat\}(mamb:MatRmn):=\hfill \\ \hfill & letm{a}^{\prime }:=\left(matRmnma\right)inletm{b}^{\prime }:=\left(matRmnmb\right)in\hfill \\ \hfill & (fixget\_Sum2(laalbb:list\left(listR\right)\left)\right\{structlaa\}:R:=matchlaawith\hfill \\ \hfill & |\left[\right]=>0|haa::la{a}^{\prime }=>matchlbbwith\hfill \\ \hfill & \left|\right[]=>0\hfill \\ \hfill & |hbb::lb{b}^{\prime }=>(fixget\_Sum1(lalb:listR)\left\{structla\right\}:R:=matchlawith\hfill \\ \hfill & |\left[\right]=>0|ha::l{a}^{\prime }=>matchlbwith\hfill \\ \hfill & \left|\right[]=>0\hfill \\ \hfill & |hb::l{b}^{\prime }=>ha\ast hb+get\_Sum1l{a}^{\prime }l{b}^{\prime }\hfill \\ \hfill & endend)haahbb+get\_Sum2la{a}^{\prime }lb{b}^{\prime }\hfill \\ \hfill & endend)m{a}^{\prime }m{b}^{\prime }.\hfill \end{array}$$

Similar to the definition of $Frobenius$, $Dot\_Product$ also uses two recursive functions to calculate the matrix inner product. The infix notation of $Dot\_Product$ is expressed as

$$\begin{array}{cc}\hfill & Notation‘‘m1·m2":=(Dot\_Productm1m2)\left(atlevel70\right).\hfill \end{array}$$

We formally verify the commutative and associative laws of the matrix inner product in the following lemmas.

Lemma 3.

In matrix space ${\mathbb{R}}^{m\ast n}$, $ma·mb=mb·ma$.

$$\begin{array}{cc}\hfill Lemmalemma3:& forall\{mn:nat\}(mamb:MatRmn),\hfill \\ \hfill & (ma·mb)=(mb·ma).\hfill \end{array}$$

Lemma 4.

In matrix space ${\mathbb{R}}^{m\ast n}$, for k being a constant of type $\mathbb{R}$, $(k\ast ma)·mb=k\ast (ma·mb)$.

$$\begin{array}{cc}\hfill Lemmalemma4:& forall\{mn:nat\}(k:R)(mamb:MatRmn),\hfill \\ \hfill & \left(\right(k\times ma)·mb)=k\ast (ma·mb).\hfill \end{array}$$

Lemma 5.

In matrix space ${\mathbb{R}}^{m\ast n}$, $(ma+mb)·mc=ma·mc+mb·mc$.

$$\begin{array}{cc}\hfill Lemmalemma5:& forall\{mn:nat\}(mambmc:MatRmn),\hfill \\ \hfill & (ma+mb)·mc=(ma·mc)+(mb·mc).\hfill \end{array}$$

4.4. Vector Formalization

Because a vector is a special kind of matrix with a row or column of dimension 1, we formally define it as follows:

$$DefinitionR\_vector(n:nat):=MatRn1.$$

Let $m_1$ and $m_2$ be any vector in space and $\theta$ be the angle between $m_1$ and $m_2$. The equivalence relation between the inner product and F-norm of a vector is given by Formula (6):

$$m_1·m_2=|m_1{|}_F\ast {\left|m_2\right|}_F\ast cos\theta .$$

(6)

We formalize the equivalence relation in Formula (6), where $<<m1,m2>>$ represents the angle between $m_1$ and $m_2$.

Lemma 6.

Formal verification of Formula (6).

$$\begin{array}{cc}\hfill Lemmalemma6:& forall\left\{n:nat\right\}(m1m2:(R\_vectorn)),\hfill \\ \hfill & (m1·m2)=\left(\right|m1|\ast |m2\left|\right)\ast cos(<<m1,m2>>).\hfill \end{array}$$

5. Formalization of the Rotary Kinematics

5.1. Global and Local Coordinate Systems

Because the formalization of the coordinate system is the basis for the formal verification of robot kinematics, we provide relative definitions. We use $RAG$ to define global coordinate system G as a type with three constructors, $X,Y,\mathrm{and}Z$, which correspond to the X, Y, and Z axes, respectively.

$$InductiveRAG:Type:=\left|X\right|Y|Z.$$

Analogously, we use $RAB$ to define local coordinate system B, where the three constructors, x, y, and z, in B corresponds to the x, y, and z axes, respectively.

$$InductiveRAB:Type:=\left|x\right|y|z.$$

Figure 2 illustrates the two coordinate systems. Given any r, $G_r$ and $B_r$ are the representations of r in G and B, respectively. Additionally, $X_1$, $Y_1$, and $Z_1$ are the coordinates of r in G, whereas $x_1$, $y_1$, and $z_1$ are the coordinates of r in B. Therefore, the r in G and B can be expressed using Formula (7), where I, J, and K are the direction vectors of G, and i, j, and k are the direction vectors of B.

$$\begin{array}{c}\hfill \left\{\begin{array}{c}{G}_r=X_1\ast I+Y_1\ast J+Z_1\ast K\hfill \\ B_r=x_1\ast i+y_1\ast j+z_1\ast k\hfill \end{array}\right.\end{array}$$

(7)

We formally define Formula (7) as follows:

$$\begin{array}{cc}\hfill & VariableX1Y1Z1x1y1z1:R.\hfill \\ \hfill & DefinitionGr:=X1\times I+Y1\times J+Z1\times K.\hfill \\ \hfill & DefinitionBr:=x1\times i+y1\times j+z1\times k.\hfill \end{array}$$

Then, $axiom1$ and $axiom2$ describe the relations of r in G and B. Therefore, the geometric meanings of $Br·I$, $Br·J$, and $Br·K$ are expressed as the projection length of vector $Br$ on I, J, and K, respectively. Similarly, the geometric meanings of $Gr·i$, $Gr·j$, and $Gr·k$ are expressed as the projection length of vector $Gr$ on i, j, and k, respectively.

$$\begin{array}{cc}\hfill & Axiomaxiom1:X1=(Br·I)\wedge Y1=(Br·J)\wedge Z1=(Br·K).\hfill \\ \hfill & Axiomaxiom2:x1=(Gr·i)\wedge y1=(Gr·j)\wedge z1=(Gr·k).\hfill \end{array}$$

5.2. Formalization of Rotary Motion

5.2.1. Formalization of Revolution Motion

According to Formula (1), the formal definition of revolution matrix $\mathit{Q}$ is as follows. I, J, K, i, j, and k are unit vectors with F-norms of one (Formula (4)). The column vector of matrix $\mathit{Q}$ represents the cosine of the angle between each coordinate axis in B and G (Formula (6)).

$$\begin{array}{c}DefinitionQ:=mkMat\_3\_3\\ \begin{array}{ccc}(i·I)& (j·I)& (k·I)\\ (i·J)& (j·J)& (k·J)\\ (i·K)& (j·K)& (k·K).\end{array}\end{array}$$

Based on this definition, we formally verify Formula (1) as follows:

Theorem 1.

Formal verification of Formula (1)

$$Theoremtheorem1:Gr=Q\times Br.$$

According to the definition of $RAG$, it can be divided into three cases (see Figure 3). Figure 3a shows the position relation between the two coordinate systems after D (a rigid body) rotates around the X axis of G (global coordinate system), where P is any vector in B. Similarly, Figure 3b,c show the position relationship between the two coordinate systems after D rotates around the Y and Z axes of G.

According to Figure 3, we formally define the corresponding revolution matrix as follows:

$$\begin{array}{cc}\begin{array}{c}DefinitionQX(\gamma :R):=mkMat\_3\_3\\ \begin{array}{ccc}1& 0& 0\\ 0& \left(cos\gamma \right)& (-(sin\gamma \left)\right)\\ 0& \left(sin\gamma \right)& \left(cos\gamma \right)\end{array}\end{array}& \begin{array}{c}DefinitionQY(\beta :R):=mkMat\_3\_3\\ \begin{array}{ccc}\left(cos\beta \right)& 0& \left(sin\beta \right)\\ 0& 1& 0\\ (-(sin\beta \left)\right)& 0& \left(cos\beta \right)\end{array}\end{array}\end{array}$$

$$\begin{array}{c}\begin{array}{c}DefinitionQZ(\alpha :R):=mkMat\_3\_3\\ \begin{array}{ccc}\left(cos\alpha \right)& (-(sin\alpha \left)\right)& 0\\ \left(sin\alpha \right)& \left(cos\alpha \right)& 0\\ 0& 0& 1\end{array}\end{array}\end{array}$$

We use the following theorems to formally verify the equivalence of matrices $\mathit{Q}$ and $\mathit{QX}$ ($\mathit{QY}$ and $\mathit{QZ}$).

Theorem 2.

When B rotates by angle γ around the X axis of G, $\mathit{Q}=(\mathit{QX}\gamma$).

$$Theoremtheorem2:forall(axis:RAG),axis=X\to Q=\left(QX\gamma \right).$$

Theorem 3.

When B rotates by angle γ around the X axis of G, $Gr=\left(\mathit{QX}\gamma \right)\ast Br$.

$$Theoremtheorem3:forall(axis:RAG),axis=X\to Gr=\left(QX\gamma \right)\times Br.$$

Theorem 4.

When B rotates by angle β around the Y axis of G, $\mathit{Q}=\left(\mathit{QY}\beta \right)$.

$$Theoremtheorem4:forall(axis:RAG),axis=Y\to Q=\left(QY\beta \right).$$

Theorem 5.

When B rotates by angle β around the Y axis of G, $Gr=\left(\mathit{QY}\beta \right)\ast Br$.

$$Theoremtheorem5:forall(axis:RAG),axis=Y\to Gr=\left(QY\beta \right)\times Br.$$

Theorem 6.

When B rotates by angle α around the Z axis of G, $\mathit{Q}=\left(\mathit{QZ}\alpha \right)$.

$$Theoremtheorem6:forall(axis:RAG),axis=Z\to Q=\left(QZ\alpha \right).$$

Theorem 7.

When B rotates by angle α around the Z axis of G, $Gr=\left(\mathit{QZ}\alpha \right)\ast Br$.

$$Theoremtheorem7:forall(axis:RAG),axis=Z\to Gr=\left(QZ\alpha \right)\times Br.$$

After B undergoes a finite number of continuous revolutions, $\mathit{Q}\mathbf{1},\mathit{Q}\mathbf{2},\mathit{Q}\mathbf{3},\cdots ,\mathit{Qn}$, around $RAG$, the equivalence relation between r in G and B is given by Formula (8):

$$G_r=({\mathit{Q}}_{\mathit{n}}\ast \cdots \ast ({\mathit{Q}}_{\mathbf{2}}\ast ({\mathit{Q}}_{\mathbf{1}}\ast B_r))).$$

(8)

Therefore, we formally define Formula (8) by $CRG$ recursively. Thus, $rl$ is a list containing pair-type elements, with the first and last elements recording the revolution type and angle, respectively. $B{r}^{\prime }$ is the vector’s initial position.

$$\begin{array}{c}\hfill \begin{array}{c}\begin{array}{c}FixpointCRG(rl:list(RAG\ast R))(B{r}^{\prime }:R\_vector3):=\hfill \\ matchrlwith\hfill \\ |nil\Rightarrow B{r}^{\prime }\hfill \\ |h::l\Rightarrow letM=(get\_Q\left(fsth\right)\left(sndh\right))in\hfill \\ letB{r}^{\prime \prime }=M\times B{r}^{\prime }inCRGlB{r}^{\prime \prime }\hfill \\ end.\hfill \end{array}\end{array}\end{array}$$

In $CRG$, the recursive structure first judges $rl$. If $rl$ is empty, the function returns the received $B{r}^{\prime }$; otherwise, the function provides revolution matrix $\mathit{M}$ through function $get\_Q$ by multiplying $\mathit{M}$ with $B{r}^{\prime }$ while regarding the product as new position $B{r}^{″}$. Then, the recursive structure uses l and $B{r}^{″}$ to continue the recursion. $get\_Q$ is formally defined as follows:

$$\begin{array}{c}\hfill \begin{array}{c}\begin{array}{c}Definitionget\_Q(axis:RAG)(\theta :R):=\hfill \\ matchaxiswith\hfill \\ |X\Rightarrow (QX\theta \left)\right|Y\Rightarrow \left(QY\theta \right)|Z\Rightarrow (QZ\theta )\hfill \\ end.\hfill \end{array}\end{array}\end{array}$$

We synthesize the finite continuous revolutions around $RAG$ into one revolution around a specific line. The synthesized revolution matrix is the global revolution matrix, ${}^{\mathit{G}}{\mathit{Q}}_{\mathit{B}}$, given by Formula (9):

$$\begin{array}{c}\hfill \left\{\begin{array}{c}{}^{\mathit{G}}{\mathit{Q}}_{\mathit{B}}={\mathit{Q}}_{\mathit{n}}\ast \cdots {\mathit{Q}}_{\mathbf{2}}\ast {\mathit{Q}}_{\mathbf{1}}\hfill \\ G_r={}^{\mathit{G}}{\mathit{Q}}_{\mathit{B}}\ast B_r\hfill \end{array}\right.\end{array}$$

(9)

We formally define the global revolution matrix as follows, where function $Cml$ represents the left multiplication of the matrix in list $ml$:

$$\begin{array}{cc}\hfill Def& initionGQB(ml:list(RAG\ast R\left)\right):=\hfill \\ \hfill & \left(fixCml\right(matList:list(RAG\ast R)\left)\right(res:MatR33\left)\right\{structmatList\}:MatR33:=\hfill \\ \hfill & matchmatListwith\hfill \\ \hfill & \left|\right[]\Rightarrow res\hfill \\ \hfill & |h::l\Rightarrow Cmll(get\_Q\left(fsth\right)\left(sndh\right)\times res)\hfill \\ \hfill & end\left)ml\right(RMatrix.MI3).\hfill \end{array}$$

Based on this definition, we formally verify Formula (9) as $lemma7$:

Lemma 7.

When $rl=[Q_1{Q}_2\cdots Q_n]$ is an arbitrary rotation sequence, r is the initial location in B,$({\mathit{Q}}_{\mathit{n}}\ast \cdots \ast ({\mathit{Q}}_{\mathbf{2}}\ast ({\mathit{Q}}_{\mathbf{1}}\ast r)))=({\mathit{Q}}_{\mathit{n}}\ast \cdots \ast {\mathit{Q}}_{\mathbf{2}}\ast {\mathit{Q}}_{\mathbf{1}})\ast r$..

$$Lemmalemma7:forall(rl:list(RAG\ast R\left)\right)(r:R\_vector3),CRGrlr=\left(GQBrl\right)\times r.$$

5.2.2. Formalization of Rotation Motion

Similar to the definition of a rotation matrix, $\mathit{Q}$, according to Formula (2), we formally define the rotation matrix $\mathit{A}$ as follows:

$$\begin{array}{c}DefinitionA:=mkMat\_3\_3\\ \begin{array}{ccc}(I·i)& (J·i)& (K·i)\\ (I·j)& (J·j)& (K·j)\\ (I·k)& (J·k)& (K·k).\end{array}\end{array}$$

Accordingly, we formally verify Formula (2) as follows:

Theorem 8.

Formal verification of Formula (2).

$$Theoremtheorem8:Br=A\times Gr.$$

Figure 4 shows three cases in $RAB$, in which the rotation matrices in Figure 4a–c are defined as $Ax$, $Ay$, and $Az$, respectively.

$$\begin{array}{c}\begin{array}{c}DefinitionAx(\psi :R):=mkMat\_3\_3\\ \begin{array}{ccc}1& 0& 0\\ 0& \left(cos\psi \right)& \left(sin\psi \right)\\ 0& (-(sin\psi \left)\right)& \left(cos\psi \right)\end{array}\end{array}\end{array}\begin{array}{c}DefinitionAy(\theta :R):=mkMat\_3\_3\\ \begin{array}{ccc}\left(cos\theta \right)& 0& (-(sin\theta \left)\right)\\ 0& 1& 0\\ \left(sin\theta \right)& 0& \left(cos\theta \right)\end{array}\end{array}$$

$$\begin{array}{c}\begin{array}{c}DefinitionAz(\varphi :R):=mkMat\_3\_3\\ \begin{array}{ccc}\left(cos\varphi \right)& \left(sin\varphi \right)& 0\\ (-(sin\varphi \left)\right)& \left(cos\varphi \right)& 0\\ 0& 0& 1\end{array}\end{array}\end{array}$$

We use the following theorems to formally verify the equivalence of matrices $\mathit{A}$ and $\mathit{Ax}$ ($\mathit{Ay}$ and $\mathit{Az}$).

Theorem 9.

When D rotates by angle ψ around the x axis of B, $\mathit{A}=\left(\mathit{Ax}\psi \right)$.

$$Theoremtheorem9:forall(axis:RAB),axis=x\to A=\left(Ax\psi \right).$$

Theorem 10.

When D rotates by angle ψ around the x axis of B, $Br=\left(\mathit{Ax}\psi \right)\ast Gr$.

$$Theoremtheorem10:forall(axis:RAB),axis=x\to Br=\left(Ax\psi \right)\times Gr.$$

Theorem 11.

When D rotates by angle θ around the y axis of B, $\mathit{A}=\left(\mathit{Ay}\theta \right)$.

$$Theoremtheorem11:forall(axis:RAB),axis=y\to A=Ay\theta .$$

Theorem 12.

When D rotates by angle θ around the y axis of B, $Br=\left(\mathit{Ay}\theta \right)\ast Gr$.

$$Theoremtheorem12:forall(axis:RAB),axis=y\to Br=\left(Ay\theta \right)\times Gr.$$

Theorem 13.

When D rotates by angle ϕ around the z axis of B, $\mathit{A}=\left(\mathit{Az}\varphi \right)$.

$$Theoremtheorem13:forall(axis:RAB),axis=z\to A=\left(Az\varphi \right).$$

Theorem 14.

When D rotates by angle ϕ around the z axis of B, $Br=\left(\mathit{Az}\varphi \right)\ast Gr$.

$$Theoremtheorem14:forall(axis:RAB),axis=z\to Br=\left(Az\varphi \right)\times Gr.$$

After D performs a finite number of continuous rotations, $\mathit{A}\mathbf{1},\mathit{A}\mathbf{2},\mathit{A}\mathbf{3},\cdots ,\mathit{An}$, around $RAB$, the equivalence relation between r in B and G is given by Formula (10):

$$B_r=({\mathit{A}}_{\mathit{n}}\ast \cdots \ast ({\mathit{A}}_{\mathbf{2}}\ast ({\mathit{A}}_{\mathbf{1}}\ast G_r))).$$

(10)

Similar to the definition of $CRG$, we formally define Formula (10) in $CRB$, where $G{r}^{\prime }$ is the vector initial position, and function $get\_A$ provides the corresponding rotation matrix according to the rotation type and angle:

$$\begin{array}{c}\hfill \begin{array}{cc}\begin{array}{c}FixpointCRB(rl:list(RAB\ast R))(G{r}^{\prime }:R\_vector3):=\hfill \\ matchrlwith\hfill \\ |nil\Rightarrow G{r}^{\prime }\hfill \\ |h::l\Rightarrow letM=(get\_A\left(fsth\right)\left(sndh\right))in\hfill \\ letG{r}^{\prime \prime }=M\times G{r}^{\prime }inCRBlG{r}^{\prime \prime }\hfill \\ end.\hfill \end{array}& \begin{array}{c}Definitionget\_A\left(axis\right)\left(\theta \right):=\hfill \\ matchaxiswith\hfill \\ |x\Rightarrow (Ax\theta )\hfill \\ |y\Rightarrow (Ay\theta )\hfill \\ |z\Rightarrow (Az\theta )\hfill \\ end.\hfill \end{array}\end{array}\end{array}$$

We synthesize the finite continuous rotations around $RAB$ into one rotation around a specific line in B. The synthesized rotation matrix is called the local rotation matrix (${}^B{A}_G$), and is given by Formula (11):

$$\begin{array}{c}\hfill \left\{\begin{array}{c}{}^{\mathit{B}}{\mathit{A}}_{\mathit{G}}={\mathit{A}}_{\mathit{n}}\ast \cdots \ast {\mathit{A}}_{\mathbf{2}}\ast {\mathit{A}}_{\mathbf{1}}\hfill \\ B_r={}^{\mathit{B}}{\mathit{A}}_{\mathit{G}}\ast G_r\hfill \end{array}\right.\end{array}$$

(11)

We formally define ${}^B{A}_G$ as follows, where function $Cm{l}^{\prime }$ represents the left multiplication of the rotation matrix in list $ml$:

$$\begin{array}{cc}\hfill Def& initionBAG(ml:list(RAB\ast R\left)\right):=\hfill \\ \hfill & (fixCm{l}^{\prime }(matList:list(RAB\ast R))(res:MatR33)\left\{structmatList\right\}:MatR33:=\hfill \\ \hfill & matchmatListwith\hfill \\ \hfill & \left|\right[]\Rightarrow res\hfill \\ \hfill & |h::l\Rightarrow Cm{l}^{\prime }l(get\_A\left(fsth\right)\left(sndh\right)\times res)\hfill \\ \hfill & end\left)ml\right(RMatrix.MI3).\hfill \end{array}$$

Based on this definition, we use a formal method to verify Formula (15), as shown in $lemma8$:

Lemma 8.

When $rl=[A_1{A}_2\cdots A_n]$ is an arbitrary rotation sequence, r is the initial location in G,$({\mathit{A}}_{\mathit{n}}\ast \cdots \ast ({\mathit{A}}_{\mathbf{2}}\ast ({\mathit{A}}_{\mathbf{1}}\ast r)))=({\mathit{A}}_{\mathit{n}}\ast \cdots \ast {\mathit{A}}_{\mathbf{2}}\ast {\mathit{A}}_{\mathbf{1}})\ast r$.

$$Lemmalemma8:forall(rl:list(RAB\ast R\left)\right)(r:(R\_vector3\left)\right),CRBrlr=\left(BAGrl\right)\times r.$$

5.2.3. Relationship between ${}^B{A}_G$ and ${}^G{Q}_B$

From Formulas (9) and (11), the relationship between ${}^{\mathit{B}}{\mathit{A}}_{\mathit{G}}$ and ${}^{\mathit{G}}{\mathit{Q}}_{\mathit{B}}$ is given by Formula (12):

$$\begin{array}{c}\hfill \left\{\begin{array}{c}{B}_r={}^{\mathit{B}}{\mathit{A}}_{\mathit{G}}\ast {}^{\mathit{G}}{\mathit{Q}}_{\mathit{B}}\ast B_r\hfill \\ G_r={}^{\mathit{G}}{\mathit{Q}}_{\mathit{B}}\ast {}^{\mathit{B}}{\mathit{A}}_{\mathit{G}}\ast G_r\hfill \end{array}\right.\end{array}$$

(12)

For Formula (12), we use $lemma9$ and $lemma10$ to prove the inverse of ${}^{\mathit{G}}{\mathit{Q}}_{\mathit{B}}$ and ${}^{\mathit{B}}{\mathit{A}}_{\mathit{G}}$, where $rLB$ and $rLG$ are lists. The list order is the same as the matrix rotation order, and $RMatrix.MI3$ represents an identity matrix with dimension 3.

Lemma 9.

For any non-zero vector r, ${}^{\mathit{G}}{\mathit{Q}}_{\mathit{B}}$ is the revolution matrix of $B\to G$ and ${}^{\mathit{B}}{\mathit{A}}_{\mathit{G}}$ is the rotation matrix of $G\to B$, then ${}^{\mathit{B}}{\mathit{A}}_{\mathit{G}}\ast {}^{\mathit{G}}{\mathit{Q}}_{\mathit{B}}=\mathit{I}$.

$$\begin{array}{c}\hfill Lemmalemma9:\left|Br\right|<>0\to \left(BAGrLB\right)\times \left(GQBrLG\right)=RMatrix.MI3.\end{array}$$

Lemma 10.

For any non-zero vector r, ${}^{\mathit{G}}{\mathit{Q}}_{\mathit{B}}$ is the revolution matrix of $B\to G$ and ${}^{\mathit{B}}{\mathit{A}}_{\mathit{G}}$ is the rotation matrix of $G\to B$, then ${}^{\mathit{G}}{\mathit{Q}}_{\mathit{B}}\ast {}^{\mathit{B}}{\mathit{A}}_{\mathit{G}}=\mathit{I}$.

$$\begin{array}{c}\hfill Lemmalemma10:\left|Gr\right|<>0\to \left(GQBrLG\right)\times \left(BAGrLB\right)=RMatrix.MI3.\end{array}$$

Similar to the proof of the inverses of ${}^{\mathit{G}}{\mathit{Q}}_{\mathit{B}}$ and ${}^{\mathit{B}}{\mathit{A}}_{\mathit{G}}$, we verify the transpose of ${}^{\mathit{G}}{\mathit{Q}}_{\mathit{B}}$ and ${}^{\mathit{B}}{\mathit{A}}_{\mathit{G}}$. The formal definition of transpose is as follows, where $trans$ represents the transpose operation of the matrix, and $rev$ represents the inverse operation of the list:

Lemma 11.

${}^{\mathit{G}}{\mathit{Q}}_{\mathit{B}}$ is the revolution matrix of $B\to G$ and ${}^{\mathit{B}}{\mathit{A}}_{\mathit{G}}$ is the rotation matrix of $G\to B$, then ${}^{\mathit{G}}{\mathit{Q}}_{\mathit{B}}{=}^{\mathit{B}}{\mathit{A}}_{\mathit{G}}^{\mathit{T}}$.

$$\begin{array}{c}\hfill LemmaLemma11:GQBrLG=transR0\left(BAG\right(revrLB\left)\right).\end{array}$$

Lemma 12.

${}^{\mathit{G}}{\mathit{Q}}_{\mathit{B}}$ is the revolution matrix of $B\to G$ and ${}^{\mathit{B}}{\mathit{A}}_{\mathit{G}}$ is the rotation matrix of $G\to B$, then ${}^{\mathit{B}}{\mathit{A}}_{\mathit{G}}{=}^{\mathit{G}}{\mathit{Q}}_{\mathit{B}}^{\mathit{T}}$.

$$\begin{array}{c}\hfill LemmaLemma12:BAGrLB=transR0\left(GQB\right(revrLG\left)\right).\end{array}$$

6. Euler Angles

In a three-dimensional (3D) space, the orientation of any coordinate system can be represented by Euler angles. The global coordinate system is assumed to be stationary, whereas the local coordinate system rotates with the rigid body. Euler angles are described by three independent parameters that determine the position of a fixed-point rotating rigid body through its precession, nutation, and rotation angles. Figure 5 shows the geometric representation of the Euler angles in space, where X, Y, and Z are the axes of the global coordinate system, and x, y, and z are the axes of the local coordinate system. Additionally, N is the intersection line of $XY\_plane$ and $xy\_plane$.

There is no consensus on the convention for Euler angles in different fields. Hence, their use requires specifying one. In this paper, we formally define and verify commonly used Euler angle conventions.

6.1. Euler Angles per the $xyz$ Convention

The angles obtained from the $xyz$ convention are also called “Tait—Bryan’’ angles, and they often appear in engineering applications to describe the direction of mobile vehicles or missiles. The three angles reflect the roll about the rotation of the airframe axis, the pitch describing the rotation perpendicular to the airframe axis, and the yaw describing the rotation of the vertical axis. In Figure 6, the rotations around the x, y, and z axes are called “roll” ($\psi$), “pitch” ($\theta$), and “yaw” ($\varphi$), respectively.

Based on Formula (15), the roll–pitch–yaw rotation matrix is given by Formula (13):

$$\mathit{BAG}\_\mathit{rpy}={\mathit{A}}_{(\mathit{z},\varphi )}\ast {\mathit{A}}_{(\mathit{y},\theta )}\ast {\mathit{A}}_{(\mathit{x},\psi )}.$$

(13)

According to the physical meaning of roll, pitch, and yaw, we provide the following formal definition:

$$\begin{array}{cc}\hfill Variable& \psi \theta \varphi :R.\hfill \\ \hfill Definde& tionBAG\_rpy:=\left(Az\varphi \right)\times \left(\right(Ay\theta )\times (Ax\psi \left)\right).\hfill \end{array}$$

Next, we prove the equivalence of $BAG\_rpy$ in $lemma13$ so that the roll–pitch–yaw sequence can be transformed into a rotation matrix.

Lemma 13.

Formal verification of Formula (13).

$$\begin{array}{cc}\hfill & Lemmalemma13:BAG\_rpy=mkMat\_3\_3\hfill \\ \hfill & \begin{array}{ccc}(cos\varphi \ast cos\theta )& (sin\varphi \ast cos\psi +cos\varphi \ast sin\theta \ast sin\psi )& (sin\varphi \ast sin\psi -cos\varphi \ast sin\theta \ast cos\psi )\\ (-(sin\varphi \ast cos\theta \left)\right)& (cos\varphi \ast cos\psi -sin\varphi \ast sin\theta \ast sin\psi )& (cos\varphi \ast sin\psi +sin\varphi \ast sin\theta \ast cos\psi )\\ \left(sin\theta \right)& (-cos\theta \ast sin\psi )& (cos\theta \ast cos\psi ).\end{array}\hfill \end{array}$$

6.2. Euler Angles per the $zxz$ Convention

Using the $zxz$ convention, precession is the rotation around the z axis, nutation is the rotation around the x axis, and spin is another rotation around the z axis. Its rotation matrix is given by Formula (14), where $\alpha$ is the precession angle, $\psi$ is the nutation angle, and $\varphi$ is the rotation angle:

$$Euler\_zxz=A_{(z,\varphi )}\ast A_{(x,\psi )}\ast A_{(z,\alpha )}.$$

(14)

Our formal proof of the rotation matrix reflecting Euler angles is as follows:

Lemma 14.

Formal verification of Formula (14).

$$\begin{array}{cc}\hfill & Lemmalemma14:Euler\_zxz=mkMat\_3\_3\hfill \\ \hfill & \begin{array}{ccc}(cos\varphi \ast cos\alpha -sin\varphi \ast cos\psi \ast sin\alpha )& (cos\varphi \ast sin\alpha +sin\varphi \ast cos\psi \ast cos\alpha )& (sin\varphi \ast sin\psi )\\ -sin\varphi \ast cos\alpha -cos\varphi \ast cos\psi \ast sin\alpha )& (-sin\varphi \ast sin\alpha +cos\varphi \ast cos\psi \ast cos\alpha )& (cos\varphi \ast sin\psi )\\ (sin\psi \ast sin\alpha )& (-sin\psi \ast cos\alpha )& \left(cos\psi \right).\end{array}\hfill \end{array}$$

Based on precession angle $\alpha$, nutation angle $\psi$, and rotation angle $\varphi$, we can calculate the overall rotation matrix according to $lemma14$. Given a rotation matrix, we can recover the Euler angles. For example, Formula (15) describes the solution for precession angle $\alpha$ from a rotation matrix:

$$\alpha =\left\{\begin{array}{ccc}\hfill & -arctan(Euler\_zx{z}_{31}/Euler\_zx{z}_{32})-\pi ,\hfill & \hfill -\pi \le \alpha <-\pi /2,\\ \hfill & -arctan(Euler\_zx{z}_{31}/Euler\_zx{z}_{32}),\hfill & \hfill -\pi /2\le \alpha <\pi /2,\\ \hfill & -arctan(Euler\_zx{z}_{31}/Euler\_zx{z}_{32})+\pi ,\hfill & \hfill \pi /2\le \alpha <\pi .\end{array}\right.$$

(15)

We formally verify the precession angle solution as follows, where $lemma15$–$lemma18$ represent the verification of $\alpha$ in the first–fourth quadrants, respectively.

$$\begin{array}{cc}\hfill & Definitionr(ij:nat):=mat\_nthEuler\_zxz(i-1)(j-1).\hfill \end{array}$$

Lemma 15.

In the domain of definition $[0,\pi /2)$, $\alpha =-arctan(r_{31}/r_{32})$.

$$\begin{array}{cc}\hfill & Lemmalemma15:0<=\alpha <PI/2\to cos\alpha <>0\to \alpha =-atan\left(\right(r31)/(r32\left)\right).\hfill \end{array}$$

Lemma 16.

In the domain of definition $[\pi /2,\pi /2)$, $\alpha =-arctan(r_{31}/r_{32})+\pi$.

$$\begin{array}{cc}\hfill & Lemmalemma16:PI/2<=\alpha <PI\to cos\alpha <>0\to \alpha =-atan\left(\right(r31)/(r32\left)\right)+PI.\hfill \end{array}$$

Lemma 17.

In the domain of definition $[-\pi ,-\pi /2)$, $\alpha =-arctan(r_{31}/r_{32})-\pi$.

$$\begin{array}{cc}\hfill & Lemmalemma17:-PI<=\alpha <-(PI/2)\to cos\alpha <>0\to \alpha =-atan\left(\right(r31)/(r32\left)\right)-PI.\hfill \end{array}$$

Lemma 18.

In the domain of definition $[-\pi /2,0)$, $\alpha =-arctan(r_{31}/r_{32})$.

$$\begin{array}{cc}\hfill & Lemmalemma18:-(PI/2)<=\alpha <0\to cos\alpha <>0\to \alpha =-atan\left(\right(r31)/(r32\left)\right).\hfill \end{array}$$

Formula (16) describes the solution for nutation angle $\psi$, given a rotation matrix:

$$\psi =\left\{\begin{array}{ccc}\hfill & -arccos(Euler\_zx{z}_{33}),\hfill & \hfill -\pi \le \psi <0,\\ \hfill & arccos(Euler\_zx{z}_{33}),\hfill & \hfill 0\le \psi <\pi .\end{array}\right.$$

(16)

Then, the formal verification of the nutation angle solution is expressed as

Lemma 19.

In the domain of definition $[0,\pi )$, $\psi =arccos\left(r_{33}\right)$.

$$\begin{array}{cc}\hfill & Lemmalemma19:0<=\psi <PI\to \psi =acos\left(r33\right).\hfill \end{array}$$

Lemma 20.

In the domain of definition $[-\pi ,0)$, $\psi =-arccos\left(r_{33}\right)$.

$$\begin{array}{cc}\hfill & Lemmalemma20:-PI<=\psi <0\to \psi =-acos\left(r33\right).\hfill \end{array}$$

Finally, Formula (17) describes the solution for rotation angle $\varphi$, given a rotation matrix:

$$\varphi =\left\{\begin{array}{ccc}\hfill & arctan(Euler\_zx{z}_{13}/Euler\_zx{z}_{23})-\pi ,\hfill & \hfill -\pi \le \varphi <-\pi /2,\\ \hfill & arctan(Euler\_zx{z}_{13}/Euler\_zx{z}_{23}),\hfill & \hfill -\pi /2<\varphi <\pi /2,\\ \hfill & arctan(Euler\_zx{z}_{13}/Euler\_zx{z}_{23})+\pi ,\hfill & \hfill \pi /2<\varphi <\pi .\end{array}\right.$$

(17)

Accordingly, the formal verification of the rotation angle solution is as follows:

Lemma 21.

In the domain of definition $[0,\pi /2)$, $\varphi =arctan(r_{13}/r_{23})$.

$$\begin{array}{cc}\hfill & Lemmalemma21:0<=\varphi <PI/2\to cos\varphi <>0\to \varphi =atan\left(\right(r13)/(r23\left)\right).\hfill \end{array}$$

Lemma 22.

In the domain of definition $[\pi /2,\pi )$, $\varphi =arctan(r_{13}/r_{23})+\pi$.

$$\begin{array}{cc}\hfill & Lemmalemma22:PI/2<\varphi <PI\to cos\varphi <>0\to \varphi =atan\left(\right(r13)/(r23\left)\right)+PI.\hfill \end{array}$$

Lemma 23.

In the domain of definition $[-\pi ,-\pi /2)$, $\varphi =arctan(r_{13}/r_{23})-\pi$.

$$\begin{array}{cc}\hfill & Lemmalemma23:-PI<=\varphi <-(PI/2)\to cos\varphi <>0\to \varphi =atan\left(\right(r13)/(r23\left)\right)-PI.\hfill \end{array}$$

Lemma 24.

In the domain of definition $[-\pi /2,0)$, $\varphi =arctan(r_{13}/r_{23})$.

$$\begin{array}{cc}\hfill & Lemmalemma24:-(PI/2)<\varphi <0\to cos\varphi <>0\to \varphi =atan\left(\right(r13)/(r23\left)\right).\hfill \end{array}$$

7. Rodriguez Formula

For a rigid body with fixed point O, a revolution by angle, $\varphi$, around $\xi$ in G can be decomposed into revolutions around three specific non-coplanar axes. On the contrary, after a rigid body rotates for a finite number of times, its revolution effect is equivalent to a specific revolution around a specific axis. The Rodriguez formula is a simple and effective way to describe such revolutions.

7.1. Proof of the Existence of the Rodriguez Formula

Let ${\xi }^{\prime }$ be a line passing through origin O in G and the rigid body that rotates by angle $\varphi$ around ${\xi }^{\prime }$. We denote the direction vector of $\xi$ as u. The revolution of the rigid body around u is equivalent to the following process: (1) Rotate one axis in B to coincide with u; (2) B rotates $\varphi$ around u; (3) B performs a revolution inverse to that in Step 1. We rotate the z axis in B to coincide with u, as shown in Figure 7.

In Figure 7, the rigid body first rotates by angle $\vartheta$ around the z axis and then by angle $\phi$ around the y axis, such that the z axis coincides with u. Then, it rotates by angle $\varphi$ around the z axis and finally rotates in the reverse sequence. The corresponding revolution is shown in Formula (18), where ${}^{\mathit{G}}{\mathit{R}}_{\mathit{B}}$ is the revolution matrix of $B\to G$:

$$\begin{array}{cc}\hfill & {}^{\mathit{G}}{\mathit{R}}_{\mathit{B}}{=}^{\mathit{G}}{\mathit{Q}}_{\mathit{B}}{=}^{\mathit{B}}{\mathit{A}}_{\mathit{G}}^{\mathit{T}}\hfill \end{array}$$

$$\begin{array}{cc}\hfill & ={(\left(\mathit{Az}(-\phi )\right)\times \left(\mathit{Ay}(-\vartheta )\right)\times \left(\mathit{Az}\varphi \right)\times \left(\mathit{Ay}\vartheta \right)\times \left(\mathit{Az}\phi \right))}^T\hfill \end{array}$$

(18)

$$\begin{array}{cc}\hfill & s.t.\left\{\begin{array}{cc}sin\phi =\frac{u_2}{\sqrt{u_1^2+u_2^2}}& cos\phi =\frac{u_1}{\sqrt{u_1^2+u_2^2}}\\ sin\vartheta =\sqrt{u_1^2+u_2^2}& cos\vartheta =u_3\end{array}\right.\hfill \end{array}$$

With this corollary, we can assume that the z axis coincides with u after two rotations. We use a formalization to verify this assumption.

Lemma 25.

There are angles φ and ϑ such that direction vector $\left[001\right]$ of the z axis coincides with u in G after two rotations.

$$\begin{array}{cc}\hfill Lemmalemma25:& Br=mkMat\_3\_1001\to exists({\varphi }^{\prime }{\theta }^{\prime }:R),\hfill \\ \hfill & u=transR0(\left(Ay{\theta }^{\prime }\right)\times \left(Az{\varphi }^{\prime }\right))\times Br.\hfill \end{array}$$

According to Formula (18), we assume the equivalence of $GRB$ and solve the matrix form of $GRB$ under constraints:

$$\begin{array}{c}Variable{\varphi }^{\prime }{\theta }^{\prime }:R.\hfill \\ HypothesisGRB\_Hy:GRB=transR0(\left(Az(-{\varphi }^{\prime })\right)\times \left(Ay(-{\theta }^{\prime })\right)\times \hfill \\ \left(Az\varphi \right)\times \left(Ay{\theta }^{\prime }\right)\times \left(Az{\varphi }^{\prime }\right)).\hfill \\ Hypothesisu\_len:\left|u\right|=1.\hfill \\ Definitionvers(x:R):=1-\left(cosx\right).\hfill \end{array}$$

Lemma 26.

Formal verification of Formula (18)

$$\begin{array}{cc}\hfill & Lemmalemma26:sin{\varphi }^{\prime }=u2/sqrt(u{1}^2+u{2}^2)\to cos{\varphi }^{\prime }=u1/sqrt(u{1}^2+u{2}^2)\to \hfill \\ \hfill & sin{\theta }^{\prime }=sqrt(u{1}^2+u{2}^2)\to cos{\theta }^{\prime }=u3\to GRB=mkMat\_3\_3\hfill \\ \hfill & \begin{array}{cc}(u{1}^2\ast \left(vers\varphi \right)+cos\varphi )& (u1\ast u2\ast (vers\varphi )-u3\ast sin\varphi )\\ (u1\ast u2\ast (vers\varphi )+u3\ast sin\varphi )& (u{2}^2\ast \left(vers\varphi \right)+cos\varphi )\\ (u1\ast u3\ast (vers\varphi )-u2\ast sin\varphi )& (u2\ast u3\ast (vers\varphi )+u1\ast sin\varphi )\end{array}\begin{array}{c}(u1\ast u3\ast (vers\varphi )+u2\ast sin\varphi )\\ (u2\ast u3\ast (vers\varphi )-u1\ast sin\varphi )\\ (u{3}^2\ast \left(vers\varphi \right)+cos\varphi )\end{array}.\hfill \end{array}$$

7.2. Formal Definition of the Rodriguez Formula

In $lemma25$, we proved that for any revolution axis, u, passing through the origin, it is always possible to match the u and z axes after two rotations. In $lemma26$, we verified the equivalent form of revolution matrix $GRB$ under constraints. Using $lemma26$, the Rodriguez formula can be deduced as shown in Formula (19) for direction vector u:

$$\begin{array}{c}\hfill \begin{array}{c}\mathit{I}=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]\mathit{u}=\left[\begin{array}{c}u1\\ u2\\ u3\end{array}\right]\widehat{\mathit{u}}=\left[\begin{array}{ccc}0& -u3& u2\\ u3& 0& -u1\\ -u2& u1& 0\end{array}\right]\hfill \\ {\mathit{R}}_{(u,\varphi )}={}^{\mathit{G}}{\mathit{R}}_{\mathit{B}}=\left(cos\varphi \right)\ast \mathit{I}+(1-cos\varphi )\ast \mathit{u}\ast {\mathit{u}}^T+\left(sin\varphi \right)\ast \widehat{\mathit{u}}.\hfill \end{array}\end{array}$$

(19)

Generally, for any non-zero vector, $\xi$, its direction vector is $e_u=\frac{\xi }{\left|\xi \right|}$.

According to Formula (19), we formally define the Rodriguez formula as $ELR$. In the definition of $ELR$, ${\varphi }^{\prime }$ represents the revolution angle, and ${\xi }^{\prime }$ represents any non-zero revolution axis:

$$\begin{array}{cc}\hfill Definition& I:=RMatrix.MI3.\hfill \\ \hfill Definition& \xi \_tilde({\xi }^{\prime }:MatR31):=mkMat\_3\_3\hfill \\ \hfill & \begin{array}{ccc}0& (-(mat\_nth{\xi }^{\prime }20))& (mat\_nth{\xi }^{\prime }10)\\ (mat\_nth{\xi }^{\prime }20)& 0& (-(mat\_nth{\xi }^{\prime }00))\\ (-(mat\_nth{\xi }^{\prime }10))& (mat\_nth{\xi }^{\prime }00)& 0.\end{array}\hfill \\ \hfill Definition& ELR({\varphi }^{\prime }:R)({\xi }^{\prime }:MatR31):=\hfill \\ \hfill match& Req\_EM\_T\left(\right|{\xi }^{\prime }\left|\right)1with\hfill \\ \hfill & |left\_=>\hfill \\ \hfill & [\left(cos{\varphi }^{\prime }\right)\times I]+[\left(vers{\varphi }^{\prime }\right)\times [{\xi }^{\prime }\times \left(transR0{\xi }^{\prime }\right)]]+[\left(sin{\varphi }^{\prime }\right)\times (\xi \_tilde{\xi }^{\prime })]\hfill \\ \hfill & |right\_=>let{\xi }^{\prime \prime }:=\left[\right(1/|{\xi }^{\prime }\left|\right)\times {\xi }^{\prime }]in\hfill \\ \hfill & [\left(cos{\varphi }^{\prime }\right)\times I]+[\left(vers{\varphi }^{\prime }\right)\times [{\xi }^{″}\times \left(transR0{\xi }^{″}\right)]]+[\left(sin{\varphi }^{\prime }\right)\times (\xi \_tilde{\xi }^{″})]\hfill \\ \hfill end.& \hfill \end{array}$$

7.3. Equivalence between the Rodriguez Formula and a Revolution around $RAG$

The Rodriguez formula applies to a rigid body rotating around $\xi$, a vector in G. When $\xi$ coincides with the X axis (Y or Z axes), $ELR$ is equivalent to revolution matrix $\mathit{Q}$ around $RAG$.

Lemma 27.

When $\xi =[x,0,0]$ and $u=\xi /\left|\xi \right|$, ${\mathit{R}}_{(u,\varphi )}=QX\varphi$.

$$\begin{array}{c}\hfill Lemmalemma27:forallx:R,x<>0\to \xi =mkMat\_3\_1x00\to ELR\varphi \xi =QX\varphi .\end{array}$$

Lemma 28.

When $\xi =[0,y,0]$ and $u=\xi /\left|\xi \right|$, ${\mathit{R}}_{(u,\varphi )}=QY\varphi$.

$$\begin{array}{c}\hfill Lemmalemma28:forally:R,y<>0\to \xi =mkMat\_3\_10y0\to ELR\varphi \xi =QY\varphi .\end{array}$$

Lemma 29.

When $\xi =[0,0,z]$ and $u=\xi /\left|\xi \right|$, ${\mathit{R}}_{(u,\varphi )}=QZ\varphi$.

$$\begin{array}{c}\hfill Lemmalemma29:forallz:R,z<>0\to \xi =mkMat\_3\_100z\to ELR\varphi \xi =QZ\varphi .\end{array}$$

$lemma27$ indicates that when the revolution axis coincides with the X axis, the Rodriguez formula describes a revolution around the X axis. Similarly, $lemma28$ and $lemma29$ show that when the revolution axis coincides with the Y and Z axes, respectively, the Rodriguez formula describes revolutions around $RAG$.

7.4. Non-Uniqueness of the Revolution Axis and Angle

Using $ELR$, we can obtain the corresponding revolution matrix, $GRB$, given revolution angle $\varphi$ and revolution axis $\xi$. Conversely, for the given $GRB$, we can obtain $\varphi$ and u (i.e., the direction vector of $\xi$). This is shown in Formula (20), where ${}^G{R}_{B_{10}}$ denotes the element at position (1,0) in $GRB$:

$$\begin{array}{c}\hfill \left\{\begin{array}{c}\widehat{u}=\frac{1}{2sin\varphi }{(}^{\mathit{G}}{\mathit{R}}_{\mathit{B}}{-}^{\mathit{G}}{\mathit{R}}_{\mathit{B}}^T)\hfill \\ sin\varphi =\frac{{(}^G{R}_{B_{10}}-{}^G{R}_{B_{01}})+{(}^G{R}_{B_{02}}-{}^G{R}_{B_{20}})+{(}^G{R}_{B_{21}}-{}^G{R}_{B_{12}})}{2(u1+u2+u3)}\hfill \\ cos\varphi =\frac{1}{2}(tr{(}^{\mathit{G}}{\mathit{R}}_{\mathit{B}})-1)\hfill \end{array}\right.\end{array}$$

(20)

From Formula (20), the solutions of $\varphi$ and $\xi$ are non-unique for the given ${}^G{R}_B$. We formally verify the non-uniqueness of the solution in $lemma30$ and $lemma31$:

Lemma 30.

When ξ is any non-zero vector and $u=\xi /\left|\xi \right|$, ${\mathit{R}}_{(u,\varphi )}={\mathit{R}}_{(-u,-\varphi )}$

$$\begin{array}{cc}\hfill & Lemmalemma30:forall({\varphi }^{\prime }:R)({\xi }^{\prime }:MatR31),ELR{\varphi }^{\prime }{\xi }^{\prime }=ELR(-{\varphi }^{\prime })(-{\xi }^{\prime }),\hfill \end{array}$$

Lemma 31.

When ξ is any non-zero vector and $u=\xi /\left|\xi \right|$, ${\mathit{R}}_{(u,\varphi )}={\mathit{R}}_{(u,\varphi +2\pi )}$

$$\begin{array}{cc}\hfill & Lemmalemma31:forall({\varphi }^{\prime }:R)({\xi }^{\prime }:MatR31),ELR{\varphi }^{\prime }{\xi }^{\prime }=ELR({\varphi }^{\prime }+2\ast PI)\left({\xi }^{\prime }\right).\hfill \end{array}$$

$lemma30$ shows that when the rigid body revolution by angle ${\varphi }^{\prime }$ around ${\xi }^{\prime }$, the revolution is equivalent to a revolution by angle $-{\varphi }^{\prime }$ around $-{\xi }^{\prime }$. This equivalence relation is obvious in a geometric proof. $lemma30$ proves this equivalence through the formal method. Similarly, $lemma31$ shows that when the revolution angle increases by $2\pi$, the revolution matrix is equivalent.

7.5. Generalized Rodriguez Formula

The formal verification of the Rodriguez formula assumed that B and G coincide initially. That is, in the initial state, r has an equivalence in G and B. More generally, B and G need not coincide initially, as shown in Formula 22, where ${}^B{R}_G^{\prime }$ is the revolution matrix.

$$B_r={}^{\mathit{B}}{\mathit{R}}_{\mathit{G}}^{\prime }\ast G_r.$$

(21)

When the initial state coordinate systems do not coincide, to obtain the Rodriguez formula, we assume a new global coordinate system, $G_0$, which coincides with B initially. Therefore, transformation $B\to G$ of r is decomposed into $B\to G_0$ and $G_0\to G$.

To obtain the revolution matrix of $B\to G_0$, we apply the Rodriguez formula. The revolution axis should be represented in $u_0$, as shown in Formula (22), where $u\left(\right|u|=1)$ is the revolution axis in G, ${}^0{R}_B$ is the revolution matrix of $B\to G_0$, and $\varphi$ is the revolution angle.

$$\begin{array}{c}\hfill \left\{\begin{array}{c}{}^{\mathbf{0}}{\mathit{R}}_{\mathit{G}}={}^{\mathit{B}}{\mathit{R}}_{\mathit{G}}^{\prime }\hfill \\ {\mathit{u}}_{\mathbf{0}}={}^{\mathbf{0}}{\mathit{R}}_{\mathit{G}}\ast u\hfill \\ {}^{\mathbf{0}}{\mathit{R}}_{\mathit{B}}=cos\varphi \ast \mathit{I}+\left(vers\varphi \right)\ast u_0\ast u_0^T+\left(sin\varphi \right)\ast \widehat{{\mathit{u}}_{\mathbf{0}}}\hfill \\ =cos\varphi \ast \mathit{I}+\left(vers\varphi \right)\ast {(}^{\mathbf{0}}{\mathit{R}}_{\mathit{G}}\ast u)\ast {{(}^{\mathbf{0}}{\mathit{R}}_{\mathit{G}}\ast u)}^T+\left(sin\varphi \right)\ast {}^{\mathbf{0}}{\mathit{R}}_{\mathit{G}}\ast \widehat{u}\ast {}^{\mathbf{0}}{\mathit{R}}_{\mathit{G}}^{\mathit{T}}\hfill \\ =cos\varphi \ast \mathit{I}+\left(vers\varphi \right)\ast {(}^{\mathbf{0}}{\mathit{R}}_{\mathit{G}}\ast u)\ast u^T\ast {}^{\mathbf{0}}{\mathit{R}}_{\mathit{G}}^{\mathit{T}}+\left(sin\varphi \right)\ast {}^{\mathbf{0}}{\mathit{R}}_{\mathit{G}}\ast \widehat{\mathit{u}}\ast {}^{\mathbf{0}}{\mathit{R}}_{\mathit{G}}^{\mathit{T}}\hfill \end{array}\right.\end{array}$$

(22)

Therefore, the revolution matrix of $B\to G$ in the final state is given by Formula (23), where ${}^G{R}_0={}^0{R}_G^T$ can be obtained from $lemma11$ and $lemma12$. Similarly, ${}^G{R}_0\ast {}^0{R}_G^T=I$ can be obtained from $lemma9$ and $lemma10$.

$$\begin{array}{c}\hfill \left\{\begin{array}{c}{}^{\mathit{G}}{\mathit{R}}_{\mathbf{0}}={}^{\mathbf{0}}{\mathit{R}}_{\mathit{G}}^{\mathit{T}}\hfill \\ {}^{\mathit{G}}{\mathit{R}}_{\mathit{B}}={}^{\mathit{G}}{\mathit{R}}_{\mathbf{0}}\ast {}^{\mathbf{0}}{\mathit{R}}_{\mathit{B}}\hfill \\ =cos\varphi \ast {}^{\mathit{G}}{\mathit{R}}_{\mathbf{0}}+\left(vers\varphi \right)\ast u\ast u^T\ast {}^{\mathit{G}}{\mathit{R}}_{\mathbf{0}}+\left(sin\varphi \right)\ast \widehat{\mathit{u}}\ast {}^{\mathit{G}}{\mathit{R}}_{\mathbf{0}}\hfill \\ ={\mathit{R}}_{(u,\varphi )}\ast {}^{\mathit{G}}{\mathit{R}}_{\mathbf{0}}\hfill \end{array}\right.\end{array}$$

(23)

We use the formal methods to infer and verify Formula (23). First, we formally define revolution axis u, revolution angle $\varphi$, and revolution matrix $\_0RG$ of $G\to B$ (in the initial state) and set the length of u to one.

$$\begin{array}{cc}\hfill & Variableu:MatR31.Variable\varphi :R.\hfill \\ \hfill & Variable\_0RG:MatR33.\hfill \\ \hfill & Hypothesisu\_len:\left|u\right|=1.\hfill \end{array}$$

Second, according to $lemma11$, we define revolution matrix $GR0$ of $B\to G$ in the initial state.

$$\begin{array}{cc}\hfill & LetGR0:=transR0\_0RG.\hfill \end{array}$$

Again, according to Formula (22), we redefine revolution axis $\_0u$ in $G_0$:

$$\begin{array}{cc}\hfill & Let\_0u:=0RG\times u.\hfill \\ \hfill & Let\_0RB:=ELR\varphi \_0u.\hfill \end{array}$$

Finally, we formally define revolution matrix $GRB$ for $B\to G_0\to G$:

$$\begin{array}{cc}\hfill & LetGRB:=GR0\times \_0RB.\hfill \end{array}$$

Based on this definition, a formal proof of the equivalence relation in Formula (23) is provided in $lemma32$:

Lemma 32.

Formal verification of Formula (23).

$$\begin{array}{cc}\hfill & Lemmalemma32:GRB=ELR\varphi u\times GR0.\hfill \end{array}$$

8. Case Analysis and Verification

In this section, we analyze and prove several cases related to robot rotations to show the applicability of our formal verification of designed robots.

8.1. Revolution around the Global Coordinate System

Case 1: In the initial state, G coincides with B. Let rigid body D, represented in B, rotate continuously around the X axis of G at an angular velocity of $0.6\pi rad/s$ along with point $P_B={[10,20,30]}^T$ in rigid body D. We illustrate the calculation of position $P_G$ of point $P_B$, represented in G, at $t=5$ s.

$$\begin{array}{cc}\hfill & SectionExample1.\hfill \\ \hfill & Variable\delta t\delta \theta :R.Variableaxis:RAG.\hfill \\ \hfill & ExampleEg\_1:Br=mkMat\_3\_1R102030\to \delta t=5\to \hfill \\ \hfill & \omega \delta t\delta \theta =0.6\ast PI\to axis=X\to Gr=mkMat\_3\_1R(-10)20(-30).\hfill \\ \hfill & Proof.\hfill \\ \hfill & \cdots \hfill \\ \hfill & Qed.\hfill \\ \hfill & EndExample1.\hfill \end{array}$$

For this case, we obtain solution $P_G={[-10,20,-30]}^T$ via the informal method. For the formal method, we apply the formal verification shown above, where $\delta t$ is the time variable, $\delta \theta$ is the revolution angle variable, and $axis$ is the revolution axis variable. From $Eg\_1$, we observe that the solution to this case is ${[-10,20,-30]}^T$, as expected.

8.2. Rotation around the Local Coordinate System

Case 2: In the initial state, G coincides with B. Let rigid body D, represented in G, first rotate by $\pi /4$ around the z axis and then rotate by $\pi /4$ around the x axis to finally rotate by $\pi /4$ around the y axis in B along with point $P_G={[10,20,30]}^T$ in rigid body D. We illustrate the calculation of position $P_B$ of point $P_G$, represented in B, after rotation.

Similar to case 1, we obtain solution [ $5\sqrt{2}/2$, $5+15\sqrt{2}$, $30-5\sqrt{2}/2$ ] ${}^T$ using the informal method. For the formal method, we show the formal verification below, where $rotList$ is a list corresponding to the rotation order. From $Eg\_2$, the solution to this case is ${\left[5\sqrt{2}/2,5+15\sqrt{2},30-5\sqrt{2}/2\right]}^T$, as expected.

$$\begin{array}{cc}\hfill & SectionExample2.\hfill \\ \hfill & VariablerotList:list(RAB\ast R).\hfill \\ \hfill & ExampleEg\_2:\hfill \\ \hfill & rotList=\left(pairz\right(PI/4\left)\right)::\left(pairx\right(PI/4\left)\right)::\left(pairy\right(PI/4\left)\right)::nil\to \hfill \\ \hfill & CRBrotList(mkMat\_3\_1R102030)=\hfill \\ \hfill & mkMat\_3\_1R(5\ast sqrt2/2)(5+15\ast sqrt2)(30-5\ast sqrt2/2).\hfill \\ \hfill & Proof.\hfill \\ \hfill & \cdots \hfill \\ \hfill & Qed.\hfill \\ \hfill & EndExample2.\hfill \end{array}$$

8.3. Euler Angle Cases

Case 3: Given precession angle $\alpha =\pi /4$, nutation angle $\psi =\pi /4$, and rotation angle $\varphi =\pi /4$, we determine the corresponding Euler angle rotation matrix.

To obtain the rotation matrix from the given Euler angles, we can use $lemma14$ to complete the formal verification with the following script:

$$\begin{array}{c}\hfill \begin{array}{c}ExampleEg\_3:forall\alpha \psi \varphi :R,\alpha =PI/4\to \psi =PI/4\to \varphi =PI/4\to \hfill \\ Euler\_zxz\alpha \psi \varphi =mkMat\_3\_3\hfill \\ \begin{array}{ccc}(1/2-sqrt2/4)& (1/2+sqrt2/4)& (1/2)\\ (-1/2-sqrt2/4)& (-1/2+sqrt2/4)& (1/2)\\ (1/2)& (-1/2)& (sqrt2/2).\end{array}\hfill \\ Proof.\hfill \\ \cdots \hfill \\ Qed.\hfill \end{array}\end{array}$$

Our analysis of the Euler angles for a given rotation matrix is shown below.

Case 4: Given the rotation matrix in Formula (24), we determine the corresponding Euler angles:

$$Euler\_zxz=\left[\begin{array}{ccc}\frac{1}{2}-\frac{\sqrt{2}}{4}& \frac{1}{2}+\frac{\sqrt{2}}{4}& \frac{1}{2}\\ -\frac{1}{2}-\frac{\sqrt{2}}{4}& -\frac{1}{2}+\frac{\sqrt{2}}{4}& \frac{1}{2}\\ \frac{1}{2}& -\frac{1}{2}& \frac{\sqrt{2}}{2}\end{array}\right].$$

(24)

For this type of case, we can use $lemma15$–$lemma26$ for verification, as follows:

$$\begin{array}{c}\hfill \begin{array}{c}ExampleEg\_4:forall\alpha \psi \varphi :R,0<=\alpha <PI/2\to 0<=\psi <PI/2\to \hfill \\ 0<=\varphi <PI/2\to Euler\_zxz\alpha \psi \varphi =mkMat\_3\_3\hfill \\ \begin{array}{ccc}(1/2-sqrt2/4)& (1/2+sqrt2/4)& (1/2)\\ (-1/2-sqrt2/4)& (-1/2+sqrt2/4)& (1/2)\\ (1/2)& (-1/2)& (sqrt2/2)\end{array}\hfill \\ \to \alpha =PI/4\wedge \psi =PI/4\wedge \varphi =PI/4.\hfill \\ Proof.\hfill \\ \cdots \hfill \\ Qed.\hfill \end{array}\end{array}$$

8.4. Rodriguez Revolutions

Case 5: Let local coordinate system B rotate by three Euler angles ($\frac{\pi }{4},\frac{\pi }{4},\frac{\pi }{4}$) with respect to global coordinate system G. We determine revolution axis u and revolution angle $\varphi$ equivalent to that revolution.

$$\begin{array}{cc}\hfill Exa& mpleEg\_5:sin\varphi <>0\to GRB=ELR\varphi u\to \theta 1=PI/4\to \hfill \\ \hfill & \theta 2=PI/4\to \theta 3=PI/4\to BRG=Euler\theta 1\theta 2\theta 3\to \hfill \\ \hfill & \begin{array}{c}\begin{array}{c}u=mkMat\_3\_1\hfill \\ \begin{array}{c}(sqrt2/sqrt(5+2\ast sqrt2\left)\right)\\ 0\\ \left(\right(4\ast sqrt2+4)/(4\ast sqrt(5+2\ast sqrt2)\left)\right)\end{array}\hfill \end{array}\vee \begin{array}{c}u=mkMat\_3\_1\hfill \\ \begin{array}{c}(-(sqrt2/sqrt(5+2\ast sqrt2)\left)\right)\\ 0\\ (-((4\ast sqrt2+4)/(4\ast sqrt(5+2\ast sqrt2\left)\right)\left)\right)\end{array}\hfill \end{array}\end{array}\hfill \\ \hfill Pro& of.\hfill \\ \hfill & \cdots \hfill \\ \hfill Qed& .\hfill \end{array}$$

Case 6: Let a rigid body rotate by $\pi /6$ around the X axis, and suppose the rigid body continues to rotate by $phi=\pi /2$ around $u=[\sqrt{3}/3,\sqrt{3}/3,\sqrt{3}/3]$. We determine the corresponding revolution matrix, $GRB$.

For this type of case, we can use $lemma32$ for verification, as follows:

$$\begin{array}{cc}\hfill & Letu:=mkMat\_3\_1\begin{array}{ccc}(sqrt3/3)& (sqrt3/3)& (sqrt3/3)\end{array}.Let\varphi :=PI/2.\hfill \\ \hfill & Let0RG:=QX(PI/6).LetGR0:=transR00RG.\hfill \\ \hfill & Let0u:=0RG\times u.Let0RB:=ELR\varphi 0U.\hfill \\ \hfill & LetGRB:=GR0\times 0RB.\hfill \\ \hfill & ExampleEg\_6:GRB=mkMat\_3\_3\hfill \\ \hfill & \begin{array}{ccc}(1/3)& (-(2/3\left)\right)& (2/3)\\ (sqrt3/3+1/3)& (sqrt3/3-1/6)& (sqrt3/6-1/3)\\ (-(sqrt3/3)+1/3)& (sqrt3/6+1/3)& (sqrt3/3+1/6).\end{array}\hfill \\ \hfill & Proof.\hfill \\ \hfill & \cdots \hfill \\ \hfill & Qed.\hfill \end{array}$$

9. Conclusions

As one of the most basic theories in motion control systems, rotary kinesthetics is widely used in different research topics. In this paper, the formal technology was applied to verify the correctness of robot rotary motion theory. We divided rotary motion into two types (i.e., revolution and rotation) and defined them formally. Specifically, we established a 3D coordinate system model in the Coq Proof Assistant and formally defined the matrix trace, Frobenius norm, and inner product in the model. We formalized the definition of Euler angle and verified its transformation relationship with the rotation matrix. Moreover, we completed the machine proof of the Rodriguez formula. In this paper, all proofs and verifications were implemented using the Coq Proof Assistant. All source files are accessible at https://github.com/GuojunXie123/RFV.git (accessed on 7 January 2023).

Overall, the proofs of the formal verification consist of about 3200 lines of code. The code has been tested and should compile under Coq 8.13.2.

Table 1. Overview of the formal verification of robot rotary kinesthetics.

File	Reference	Specification	Proof
Trace.v	Section 4.1	33	0
Norm.v	Section 4.2	88	20
DotProduct.v	Section 4.3	115	30
DotAngle.v	Section 4.4	100	47
Coordinate_Basics.v	Section 5.1	376	350
Rotate_Around_G.v	Section 5.2.1	290	260
Rotate_Around_G_Lemma.v	Section 5.2.1	320	300
Rotate_Around_G_Example.v	Section 5.2.1	148	130
Rotate_Around_B.v	Section 5.2.2	270	240
Rotate_Around_B_Lemma.v	Section 5.2.2 and Section 6.1	320	300
Rotate_Around_B_Exaplme.v	Section 5.2.2	150	130
General_Rotate.v	Section 5.2.3	70	60
Relation_GB.v	Section 5.2.3	130	120
Euler_Angle.v	Section 6.2	180	170
RodriguezDef.v	Section 7.2	260	230
RodriguezPf.v	Section 7.1	115	100
RodriguezLem.v	Section 7.3 and Section 7.4	310	300

provides a detailed account of the formalization in terms of script files. To help navigate through them, we indicate the related sections in the paper. The count in terms of lines of code distinguishes between specifications and proofs.

In the future, we will complete the verification of more robot control technologies based on the formal verification framework of this paper. With more complex kinematic logic, we will improve the formal verification framework of more types of robot kinematics. Our goal is to complete the formal verification of a robot control system. We also plan to develop some automatic tactics that can be used to increase the automation of formal proofs. This will be a meaningful exploration and a worthwhile attempt in the field of automated control system verification. Additionally, we will also try to combine the technologies of code generation and formal verification. We plan to design a robot control algorithm and automatically generate the C code using Coq Proof Assistant. This is expected to vastly improve the safety assurance of robot control systems.