CN105856240A - Single-joint fault mechanical arm model rebuilding method based on projection geometric method - Google Patents

Abstract

The invention discloses a single-joint fault mechanical arm model rebuilding method based on a projection geometric method. The method mainly includes that a kinematic model of a mechanical arm is characterized on the basis of rotation coordinates, different fault joints are different in axial direction in a base coordinate system, and fault angles of the fault joints influence different parameters of joint rotation coordinates, so that the fault joints are classified according to the axial direction; aiming at different joints, a projection method is adopted to construct a projection plane in a common perpendicular face of axes of the joints, and kinematic model plane projection drawings are acquired; according to the plane projection drawing of each fault joint, a relational expression of the fault angles, length of connecting rods and joint rotation coordinates is established through geometric analysis, and then rebuilding of a single-joint fault mechanical arm model is completed. By using the method, the problem that movement cannot be controlled due to the fact that an original kinematic model is invalid after a single joint of the mechanical arm is in a fault is solved; a rotation theory is used as a base, so that compared with a D-H parameter method, the method has the advantages of high efficiency, high universality and suitability for mechanical arms different in structure.

Description

Single-joint fault mechanical arm model reconstruction method based on projection geometry method

Technical Field

The invention relates to a single-joint fault mechanical arm model reconstruction method based on a projection geometry method, and belongs to the technical field of mechanical arm fault tolerance.

Background

Because the space environment has the characteristics of ultra-vacuum, high temperature difference and strong radiation, the joint failure phenomenon is likely to occur in the long-term service process of the space manipulator by comprehensively considering the badness of the working environment of the space manipulator and the structural complexity in the joint. Once a space manipulator fails, timely repair cannot be performed usually, which leads to delay and even delay of space tasks, thereby affecting implementation of the whole space plan. At present, aiming at the situation that a single joint suddenly fails in the track operation process of a space manipulator, the locking of a fault joint is a simple and effective processing method. After the joint is locked, the mechanical arm cannot complete the on-orbit operation task according to the original planned path due to the damage of the motion model, so that the task fails. The invention relates to a single-joint fault manipulator model reconstruction method based on a projection geometry method, which can meet the requirement of completing an in-orbit task to the maximum extent under the condition that a single joint of a manipulator is damaged, and has very important theoretical significance and practical significance for space exploration in the future, particularly for in-orbit application of a space manipulator.

At present, the mechanical arm model reconstruction method aiming at single joint faults is few in research results, the mainstream research result is the kinematic model reconstruction of the SSRMS mechanical arm based on a D-H parameter method, the problem of model reconstruction when any joint of a mechanical arm with a special configuration is locked is solved, but the reconstruction process of a D-H coordinate system is complex, the reconstruction methods aiming at the failure condition of each joint are different, the calculated amount is large, and meanwhile, the solution method aiming at the mechanical arms with different configurations does not have a universal expression, so the universality is low. In order to solve the problems, the invention provides a method for reconstructing a single-joint fault mechanical arm model based on a projection geometry method on the basis of a momentum theory, and the method has the advantages of high efficiency, strong universality and suitability for mechanical arms with different configurations.

Disclosure of Invention

The invention aims to provide a method for reconstructing a single-joint fault mechanical arm model based on a projection geometry method aiming at the defects of the model reconstruction method, and solves the problem that the motion control cannot be carried out due to the change of a mechanical arm motion model in a single-joint fault state.

The technical scheme adopted by the invention is as follows: a single-joint fault mechanical arm model reconstruction method based on a projection geometry method comprises the following steps:

1) characterizing a kinematics model of the mechanical arm based on a rotation coordinate, classifying fault joints along the axial direction according to different axial directions of the fault joints in a basic coordinate system and different parameters of rotation coordinates of each joint influenced by fault angles, and constructing a projection plane on a common vertical plane of the axis of the fault joints with different axial directions by adopting a projection method aiming at the fault joints with different axial directions to obtain a kinematics model plane projection diagram;

2) based on a plane projection diagram, determining the rotation coordinate of the mechanical arm by adopting a geometric analysis method, analyzing the transformation matrix of a basic coordinate system and a tool coordinate system and the relationship between the rotation coordinate of each joint in a reconstructed kinematic model and a fault angle after the fault joint is locked at a fixed angle according to the condition that different joints have faults, and realizing the reconstruction of the kinematic model of the single-joint fault mechanical arm.

Step 1) the two-dimensional projection of the mechanical arm kinematics model by the projection method comprises the following steps:

11) defining rules for labels in the failure state of the joint, when the joint J_fWhen locked, the joint J is removed_fBesides, the relative signs of the other joints are increased by "-" and the labels of the joints are decreased by one on the original basis, i.e. the joint is a straight-line jointAnd the degree of freedom of the mechanical arm is degenerated into m-n-1;

12) classifying fault joints, namely X-axis joints, Y-axis joints and Z-axis joints, aiming at different axial directions of the joints in a basic coordinate system and different influences of fault angles on rotation amount coordinates of the joints;

13) representing a kinematic model of the mechanical arm based on a rotation coordinate, and specifying a fault-removing joint J_fLocked at a fault angle, and the other joints are fixedAccording to fault joint J_fDetermine a two-dimensional projection plane of its kinematic model in the axial directionDough making: the two-dimensional graph can be used for determining the rotation coordinate of each joint except the fault joint in the axial direction, and the rotation coordinate along the fault joint in the axial direction needs to be obtained by a reconstructed kinematic model.

Step 2) determining the rotation coordinate by a geometric analysis method comprises the following steps:

21) if the fault joint is an X-axis joint, after the joint is in fault, the transformation relation between the basic coordinate system and the tool coordinate system is as follows:

$g_{st}\left(0\right)=\left[\begin{array}{cccc}1& 0& 0& {\mathrm{\Σl}}_x\\ 0& {\mathrm{c\θ}}_f& -{\mathrm{s\θ}}_f& {\mathrm{\Σl}}_{yr}+{\mathrm{\Σl}}_{zf}{\mathrm{s\θ}}_f+{\mathrm{\Σl}}_{yf}{\mathrm{c\θ}}_f\\ 0& {\mathrm{s\θ}}_f& {\mathrm{c\θ}}_f& {\mathrm{\Σl}}_{zr}+{\mathrm{\Σl}}_{zf}{\mathrm{c\θ}}_f+{\mathrm{\Σl}}_{yf}{\mathrm{s\θ}}_f\\ 0& 0& 0& 1\end{array}\right]$

wherein, theta_fAngle indicating faulty Joint Lock, ∑ l_x∑ l representing the sum of the lengths of the links in the X-axis direction_yrAnd ∑ l_zr∑ l each representing the sum of the lengths of the Y-axis and Z-axis links before the failed joint_yfAnd ∑ l_zfRespectively representing the sum of the lengths of the Y-axis and Z-axis links after the failed joint.

The rotation coordinate of the joint before the fault joint is labeled keeps the original value unchanged, and the rotation coordinate of the joint after the fault joint is labeled as follows:

${\widetilde{\mathrm{\ω}}}_y=\left[\begin{array}{ccc}0& {\mathrm{c\θ}}_f& -{\mathrm{s\θ}}_f\end{array}\right]$

${\widetilde{\mathrm{\ω}}}_z=\left[\begin{array}{ccc}0& {\mathrm{s\θ}}_f& {\mathrm{c\θ}}_f\end{array}\right]$

${\tilde{q}}_i=\left(\begin{array}{ccc}{x}_i& y_i+{\mathrm{\Σl}}_{y_f}({\mathrm{cos\θ}}_f-1)+{\mathrm{\Σl}}_{z_f}{\mathrm{sin\θ}}_f& z_i+{\mathrm{\Σl}}_{y_f}{\mathrm{sin\θ}}_f+{\mathrm{\Σl}}_{z_f}({\mathrm{cos\θ}}_f-1)\end{array}\right)$

${\widetilde{\mathrm{\ξ}}}_i=\left[\begin{array}{c}-{\mathrm{\ω}}_i\×q_i\\ {\mathrm{\ω}}_i\end{array}\right]$

wherein,respectively represent unit vectors in the direction of the motion vector axes of the Y-axis joint and the Z-axis joint, q_i＝(x_iy_iz_i) Representing a point on the pre-fault axis.

22) If the fault joint is a Y-axis joint, after the joint is in fault, the transformation relation between the basic coordinate system and the tool coordinate system is as follows:

$g_{st}\left(0\right)=\left[\begin{array}{cccc}{\mathrm{c\θ}}_f& 0& {\mathrm{s\θ}}_f& {\mathrm{\Σl}}_{xr}+{\mathrm{\Σl}}_{xf}{\mathrm{c\θ}}_f+{\mathrm{\Σl}}_{zf}{\mathrm{s\θ}}_f\\ 0& 1& 0& {\mathrm{\Σl}}_y\\ -{\mathrm{s\θ}}_f& 0& {\mathrm{c\θ}}_f& {\mathrm{\Σl}}_{zr}+{\mathrm{\Σl}}_{xf}{\mathrm{s\θ}}_f+{\mathrm{\Σl}}_{zf}{\mathrm{c\θ}}_f\\ 0& 0& 0& 1\end{array}\right]$

of these, ∑ l_y∑ l representing the sum of the lengths of the links in the Y-axis direction_xr∑ l representing the sum of the lengths of the X-axis links before the failed joint_xfThe sum of the lengths of the X-axis direction links after the failed joint is indicated.

The rotation coordinate of the joint before the fault joint is labeled keeps the original value unchanged, and the rotation coordinate of the joint after the fault joint is labeled as follows:

${\widetilde{\mathrm{\ω}}}_x=\left[\begin{array}{ccc}{\mathrm{c\θ}}_f& 0& {\mathrm{s\θ}}_f\end{array}\right]$

${\widetilde{\mathrm{\ω}}}_y=\left[\begin{array}{ccc}0& {\mathrm{c\θ}}_f& -{\mathrm{s\θ}}_f\end{array}\right]$

${\tilde{q}}_i=\left(\begin{array}{ccc}{x}_i+{\mathrm{\Σl}}_{x_f}({\mathrm{cos\θ}}_f-1)+{\mathrm{\Σl}}_{z_f}{\mathrm{sin\θ}}_f& y_i& z_i+{\mathrm{\Σl}}_{x_f}{\mathrm{sin\θ}}_f+{\mathrm{\Σl}}_{z_f}({\mathrm{cos\θ}}_f-1)\end{array}\right)$

${\widetilde{\mathrm{\ξ}}}_i=\left[\begin{array}{c}-{\mathrm{\ω}}_i\×q_i\\ {\mathrm{\ω}}_i\end{array}\right]$

wherein,a unit vector in the direction of the motion vector axis of the X-axis joint,representing the kinematic rotation of the joint i.

23) If the fault joint is a Z-axis joint, after the joint is in fault, the transformation relation between the basic coordinate system and the tool coordinate system is as follows:

$g_{st}\left(0\right)=\left[\begin{array}{cccc}{\mathrm{c\θ}}_f& -{\mathrm{s\θ}}_f& 0& {\mathrm{\Σl}}_{xr}+{\mathrm{\Σl}}_{yf}{\mathrm{s\θ}}_f+{\mathrm{\Σl}}_{xf}{\mathrm{c\θ}}_f\\ {\mathrm{s\θ}}_f& {\mathrm{c\θ}}_f& 0& {\mathrm{\Σl}}_{yr}+{\mathrm{\Σl}}_{yf}{\mathrm{c\θ}}_f+{\mathrm{\Σl}}_{yf}{\mathrm{s\θ}}_f\\ 0& 0& 1& {\mathrm{\Σl}}_z\\ 0& 0& 0& 1\end{array}\right]$

the rotation coordinate of the joint before the fault joint is labeled keeps the original value unchanged, and the rotation coordinate of the joint after the fault joint is labeled as follows:

${\widetilde{\mathrm{\ω}}}_y=\left[\begin{array}{ccc}0& {\mathrm{c\θ}}_f& -{\mathrm{s\θ}}_f\end{array}\right]$

${\widetilde{\mathrm{\ω}}}_z=\left[\begin{array}{ccc}0& {\mathrm{s\θ}}_f& {\mathrm{c\θ}}_f\end{array}\right]$

${\tilde{q}}_i=\left(\begin{array}{ccc}{x}_i+{\mathrm{\Σl}}_{y_f}{\mathrm{sin\θ}}_f+{\mathrm{\Σl}}_{x_f}({\mathrm{cos\θ}}_f-1)& y_i+{\mathrm{\Σl}}_{x_f}{\mathrm{sin\θ}}_f+{\mathrm{\Σl}}_{y_f}({\mathrm{cos\θ}}_f-1)& z_i\end{array}\right)$

${\widetilde{\mathrm{\ξ}}}_i=\left[\begin{array}{c}-{\mathrm{\ω}}_i\×q_i\\ {\mathrm{\ω}}_i\end{array}\right]$

compared with the prior art, the invention has the following advantages:

(1) the invention represents the kinematics model of the mechanical arm through the rotation coordinate, realizes the universality of the kinematics model reconstruction after the single joint failure of the mechanical arms with different configurations, and has particularly obvious universality when the mechanical arm has complex configuration and high degree of freedom.

(2) The invention adopts a projection method to construct a projection plane on the common vertical plane of the axis of the fault joint, and can effectively analyze the transformation matrix of the basic coordinate system and the tool coordinate system after the joint is locked at the fault angle, and reconstruct the relationship between the rotation coordinate of each joint in the kinematic model and the fault angle.

(3) According to the method, the mathematical expressions among the rotation coordinate, the fault joint angle and the connecting rod length when various joints have faults are deduced by adopting a geometric method, and the reconstructed rotation coordinate can be directly obtained on the basis of the known rotation coordinate of the mechanical arm and the fault joint locking angle in a normal state, so that the reconstruction efficiency of a kinematic model is improved, and the real-time requirement of on-orbit application can be met.

Drawings

FIG. 1 is a flow chart of the method of the present invention.

Fig. 2 is a normal spin coordinate system.

Figure 3 is a reconstruction rotation system after joint triple locking.

Fig. 4 is a YZ plane projection view after the joint is locked.

Figure 5 is a reconstruction rotation system after joint two locking.

FIG. 6 is a projection of the XZ plane after the joint II has been locked.

Figure 7 is a reconstruction rotation system after joint-locking.

Figure 8 is an XY plane projection view of the joint after locking.

Detailed Description

1. Two-dimensional projection method for single-joint failure mechanical arm kinematics model

FIG. 1 shows a flow chart of the method of the present invention. The invention takes a spatial seven-degree-of-freedom mechanical arm as a research object, the length of each rod piece is shown in table 1, and the configuration of the mechanical arm is shown in fig. 2. For each joint J_i(i 1.., 7), the kinematic momentum of which is constructed, the momentum coordinates such asShown in table 2.

TABLE 1 connecting rod Length

TABLE 2 normal spin coordinates

X-axis joint J₃Locked at 30 DEG, J₃Posterior joint markingThe reconstructed rotation amount is shown in fig. 3, and the YZ projection view is shown in fig. 4. Y-axis joint J₂Locked at 30 DEG, J₂Posterior joint markingThe reconstructed vector is shown in FIG. 5, and the projection view in the XZ direction is shown in FIG. 6. Z-axis joint J₁Locked at 30 DEG, J₁Posterior joint markingThe reconstructed vector is shown in fig. 7, and the XY projection is shown in fig. 8.

2. Determining the rotation coordinate by geometric analysis

X-axis joint J₃When locked, the variable rotation parameter isAndare all theta₃The reconstructed spin parameters are shown in table 3, and the spin coordinates are shown in table 4. Base coordinate system S maintenanceThe original direction, the direction of the tool coordinate system T changes, and the transformation relation between the basic coordinate system and the tool coordinate system is as follows:

$g_{st}\left(0\right)=\left[\begin{array}{cccc}1& 0& 0& -1.5\\ 0& 0.866& -0.5& 2.076\\ 0& 0.5& 0.866& 6.046\\ 0& 0& 0& 1\end{array}\right]---\left(1\right)$

TABLE 3 reconstruction curl parameter after three joint locking

TABLE 4 reconstruction of the rotation coordinates after locking of the three joints

Y-axis joint J₂When locked, the variable rotation parameter isAndare all theta₂As a function of (c). Reconstructed spin parameterAs shown in table 5, the curl coordinates are shown in table 6. The basic coordinate system S keeps the original direction, the direction of the tool coordinate system T changes, and the transformation relation between the basic coordinate system and the tool coordinate system is as follows:

$g_{st}\left(0\right)=\left[\begin{array}{cccc}0.866& 0& 0.5& -7.782\\ 0& 1& 0& -1\\ -0.5& 0& 0.866& 8.943\\ 0& 0& 0& 1\end{array}\right]---\left(2\right)$

TABLE 5 reconstruction curl parameter after two-joint locking

Z-axis joint J₁When locked, the variable rotation parameter isAndare all theta₁As a function of (c). The reconstructed spin parameters are shown in table 7, and the spin coordinates are shown in table 8. The basic coordinate system S keeps the original direction, the direction of the tool coordinate system T changes, and the transformation relation between the basic coordinate system and the tool coordinate system is as follows:

$g_{st}\left(0\right)=\left[\begin{array}{cccc}0.866& -0.5& 0& -0.799\\ 0.5& 0.866& 0& -1.616\\ 0& 0& 1& 11.2\\ 0& 0& 0& 1\end{array}\right]---\left(3\right)$

TABLE 6 reconstruction screw coordinate after two-joint locking

TABLE 7 reconstruction curl parameter after Joint Lock-Up

TABLE 8 reconstruction screw coordinate after one-joint locking

Those skilled in the art will appreciate that those matters not described in detail in the present specification are well known in the art.

Claims (3)

1. A single-joint fault mechanical arm model reconstruction method based on a projection geometry method is characterized by comprising the following steps:

1) characterizing a kinematics model of the mechanical arm based on a rotation coordinate, classifying fault joints along the axial direction according to different axial directions of the fault joints in a basic coordinate system and different parameters of rotation coordinates of each joint influenced by fault angles, and constructing a projection plane on a common vertical plane of the axis of the fault joints with different axial directions by adopting a projection method aiming at the fault joints with different axial directions to obtain a kinematics model plane projection diagram;

2) based on a plane projection diagram, determining the rotation coordinate of the mechanical arm by adopting a geometric analysis method, analyzing the transformation matrix of a basic coordinate system and a tool coordinate system and the relationship between the rotation coordinate of each joint in a reconstructed kinematic model and a fault angle after the fault joint is locked at a fixed angle according to the condition that different joints have faults, and realizing the reconstruction of the kinematic model of the single-joint fault mechanical arm.

2. The reconstruction method of the single-joint fault mechanical arm model based on the projection geometry method as claimed in claim 1, wherein the step 1) of performing two-dimensional projection on the mechanical arm kinematics model by the projection method comprises the following steps:

11) classifying fault joints, namely X-axis joints, Y-axis joints and Z-axis joints, aiming at different axial directions of the joints in a basic coordinate system and different influences of fault angles on rotation amount coordinates of the joints;

12) representing a kinematic model of the mechanical arm based on a rotation coordinate, and specifying a fault-removing joint J_fLocked at a fault angle, and the other joints are fixed(i ═ 1.. said, m, and i ≠ f), according to faulty joint J_fDetermines the two-dimensional projection plane of its kinematic model: the two-dimensional graph can be used for determining the rotation coordinate of each joint except the fault joint in the axial direction, and the rotation coordinate along the fault joint in the axial direction needs to be obtained by a reconstructed kinematic model.

3. The reconstruction method of the single-joint fault mechanical arm model based on the projection geometry method as claimed in claim 1, wherein the step 2) of determining the rotation coordinate by the geometry analysis method comprises the following steps:

21) if the fault joint is an X-axis joint, after the joint is in fault, the transformation relation between the basic coordinate system and the tool coordinate system is as follows:

$g_{st}\left(0\right)=\left[\begin{array}{cccc}1& 0& 0& \Σl_x\\ 0& {\mathrm{c\θ}}_f& -{\mathrm{s\θ}}_f& \Σl_{yr}+\Σl_{zf}{\mathrm{s\θ}}_f+\Σl_{yf}{\mathrm{c\θ}}_f\\ 0& {\mathrm{s\θ}}_f& {\mathrm{c\θ}}_f& \Σl_{zr}+\Σl_{zf}{\mathrm{c\θ}}_f+\Σl_{yf}{\mathrm{s\θ}}_f\\ 0& 0& 0& 1\end{array}\right]$

wherein, theta_fAngle indicating faulty Joint Lock, ∑ l_x∑ l representing the sum of the lengths of the links in the X-axis direction_yrAnd ∑ l_zr∑ l each representing the sum of the lengths of the Y-axis and Z-axis links before the failed joint_yfAnd ∑ l_zfRespectively represent hairThe sum of the lengths of the Y-axis and Z-axis links behind the failed joint.

The rotation coordinate of the joint before the fault joint is labeled keeps the original value unchanged, and the rotation coordinate of the joint after the fault joint is labeled as follows:

${\widetilde{\mathrm{\ω}}}_y=\left[\begin{array}{ccc}0& {\mathrm{c\θ}}_f& -{\mathrm{s\θ}}_f\end{array}\right]$

${\widetilde{\mathrm{\ω}}}_z=\left[\begin{array}{ccc}0& {\mathrm{s\θ}}_f& {\mathrm{c\θ}}_f\end{array}\right]$

${\tilde{q}}_i=\left(\begin{array}{ccc}{x}_i& y_i+\Σl_{y_f}({\mathrm{cos\θ}}_f-1)+\Σl_{z_f}{\mathrm{sin\θ}}_f& z_i+\Σl_{y_f}{\mathrm{sin\θ}}_f+\Σl_{z_f}({\mathrm{cos\θ}}_f-1)\end{array}\right)$

wherein,respectively represent unit vectors in the direction of the motion vector axes of the Y-axis joint and the Z-axis joint, q_i＝(x_iy_iz_i) Representing a point on the pre-fault axis.

22) If the fault joint is a Y-axis joint, after the joint is in fault, the transformation relation between the basic coordinate system and the tool coordinate system is as follows:

$g_{st}\left(0\right)=\left[\begin{array}{cccc}{\mathrm{c\θ}}_f& 0& {\mathrm{s\θ}}_f& \Σl_{xr}+\Σl_{xf}{\mathrm{c\θ}}_f+\Σl_{zf}{\mathrm{s\θ}}_f\\ 0& 1& 0& \Σl_y\\ -{\mathrm{s\θ}}_f& 0& {\mathrm{c\θ}}_f& \Σl_{zr}+\Σl_{xf}{\mathrm{s\θ}}_f+\Σl_{zf}{\mathrm{c\θ}}_f\\ 0& 0& 0& 1\end{array}\right]$

of these, ∑ l_y∑ l representing the sum of the lengths of the links in the Y-axis direction_xr∑ l representing the sum of the lengths of the X-axis links before the failed joint_xfThe sum of the lengths of the X-axis direction links after the failed joint is indicated.

The rotation coordinate of the joint before the fault joint is labeled keeps the original value unchanged, and the rotation coordinate of the joint after the fault joint is labeled as follows:

${\widetilde{\mathrm{\ω}}}_x=\left[\begin{array}{ccc}{\mathrm{c\θ}}_f& 0& {\mathrm{s\θ}}_f\end{array}\right]$

${\widetilde{\mathrm{\ω}}}_y=\left[\begin{array}{ccc}0& {\mathrm{c\θ}}_f& -{\mathrm{s\θ}}_f\end{array}\right]$

${\tilde{q}}_i=\left(\begin{array}{ccc}{x}_i+\Σl_{x_f}({\mathrm{cos\θ}}_f-1)+\Σl_{z_f}{\mathrm{sin\θ}}_f& y_i& z_i+\Σl_{x_f}{\mathrm{sin\θ}}_f+\Σl_{z_f}({\mathrm{cos\θ}}_f-1)\end{array}\right)$

wherein,represents a unit vector in the direction of the motion vector axis of the X-axis joint.

23) If the fault joint is a Z-axis joint, after the joint is in fault, the transformation relation between the basic coordinate system and the tool coordinate system is as follows:

$g_{st}\left(0\right)=\left[\begin{array}{cccc}{\mathrm{c\θ}}_f& -{\mathrm{s\θ}}_f& 0& \Σl_{xr}+{\Σ}_{yf}{\mathrm{s\θ}}_f+\Σl_{xf}{\mathrm{c\θ}}_f\\ {\mathrm{s\θ}}_f& {\mathrm{c\θ}}_f& 0& \Σl_{yr}+\Σl_{yf}{\mathrm{c\θ}}_f+\Σl_{yf}{\mathrm{s\θ}}_f\\ 0& 0& 1& \Σl_z\\ 0& 0& 0& 1\end{array}\right]$

the rotation coordinate of the joint before the fault joint is labeled keeps the original value unchanged, and the rotation coordinate of the joint after the fault joint is labeled as follows:

${\widetilde{\mathrm{\ω}}}_y=\left[\begin{array}{ccc}0& {\mathrm{c\θ}}_f& -{\mathrm{s\θ}}_f\end{array}\right]$

${\widetilde{\mathrm{\ω}}}_z=\left[\begin{array}{ccc}0& {\mathrm{s\θ}}_f& {\mathrm{c\θ}}_f\end{array}\right]$

${\tilde{q}}_i=\left(\begin{array}{ccc}{x}_i+\Σl_{y_f}{\mathrm{sin\θ}}_f+\Σl_{x_f}({\mathrm{cos\θ}}_f-1)& y_i+\Σl_{x_f}{\mathrm{sin\θ}}_f+\Σl_{y_f}({\mathrm{cos\θ}}_f-1)& z_i\end{array}\right)$