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

Abstract

The invention discloses a model reconstruction method of a single-joint fault mechanical arm based on a projection geometry method. Its core includes: characterize the kinematics model of the manipulator based on the screw coordinates. Since the axes of different faulty joints in the basic coordinate system are different, the fault angle affects the different parameters of the screw coordinates of each joint, so the faulty joints are divided according to their axis According to different types of joints, the projection method is used to construct the projection plane on the common vertical plane of its axis to obtain the plane projection diagram of the kinematic model; according to the plane projection diagram of each faulty joint, through geometric analysis, the fault angle, connecting rod The relationship between the length and the screw coordinates of each joint is used to complete the reconstruction of the single-joint failure manipulator model. The invention solves the problem that the original kinematics model becomes invalid after a single joint failure of the mechanical arm, and motion control cannot be performed. At the same time, it is based on the screw theory, and compared with the D-H parameter method, it has high efficiency and strong versatility. It is suitable for the characteristics of different configurations of robotic arms.

Description

A Model Reconstruction Method of Single-joint Fault Manipulator Based on Projective Geometry

technical field

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

Background technique

Since the space environment has the characteristics of ultra-vacuum, high temperature difference, and strong radiation, considering the harsh working environment of the space manipulator and the complexity of the internal structure of the joint, joint failure is very likely to occur during its long-term service. Once the space manipulator fails, it usually cannot be repaired in time, which will lead to the delay or even delay of the space mission, thereby affecting the implementation of the entire space program. At present, for the sudden failure of a single joint of the space manipulator during on-orbit operation, locking the faulty joint is a simple and effective solution. After the joint is locked, due to the damage of the motion model, the manipulator cannot complete the on-orbit operation task according to the original planned path, resulting in the failure of the task. The present invention is a single-joint failure manipulator model reconstruction method based on projective geometry method, which can meet the requirement of maximally completing on-orbit tasks in the state of single-joint damage of the manipulator, and is useful for future space exploration, especially space exploration. The in-orbit application of the manipulator has very important theoretical and practical significance.

At present, there are few research results on the reconstruction method of the manipulator model for single-joint faults. The mainstream research results are the kinematics model reconstruction of the SSRMS manipulator based on the D-H parameter method, which solves the problem of locking any joint of the special-configuration manipulator. However, the reconstruction process of the D-H coordinate system is complicated, and the reconstruction method for each joint failure is different, so the calculation is relatively large, and there is no general expression for the solution to different configurations of manipulators formula, so the versatility is low. In order to solve the above problems, the present invention provides a single-joint failure manipulator model reconstruction method based on the projection geometry method based on the screw theory. This method has high efficiency, strong versatility, and is suitable for different configurations of manipulators. The advantages.

Contents of the invention

The purpose of the present invention is to address the shortcomings of the above-mentioned model reconstruction method, to provide a single-joint failure manipulator model reconstruction method based on the projection geometry method, which solves the problem that the movement model of the manipulator under the single-joint failure state changes and motion control cannot be performed The problem.

The technical solution adopted in the present invention is: a method for reconstructing a model of a single-joint failure manipulator based on projective geometry, the steps are as follows:

1) Based on the screw coordinates to characterize the kinematics model of the manipulator, according to the different axes of the faulty joints in the basic coordinate system, the fault angle affects the different parameters of the screw coordinates of each joint, classify the faulty joints along the axial direction, and aim at For faulty joints with different axes, use the projection method to construct a projection plane on the common vertical plane of its axis, and obtain a kinematic model plane projection map;

2) Based on the planar projection diagram, use the geometric analysis method to determine the screw coordinates of the manipulator. For the failure of different types of joints, analyze the transformation matrix between the basic coordinate system and the tool coordinate system after the faulty joint is locked at a fixed angle, and the weight The relationship between the screw coordinates of each joint in the structural kinematics model and the fault angle is realized, and the kinematics model reconstruction of the single-joint fault manipulator is realized.

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

11) Specify the labeling rules in the joint failure state. When the joint J _f is locked, except for the joint J _f , the relevant symbols of the other joints are all increased by "~", and the subsequent joint labels are reduced by one on the original basis, that is, And the degree of freedom of the manipulator degenerates to m=n-1;

12) In view of the different axes of the joints in the basic coordinate system, the influence of the fault angle on the screw coordinates of each joint is different, and the faulty joints are classified into X-axis joints, Y-axis joints, and Z-axis joints;

13) Based on the screw coordinates to characterize the kinematics model of the manipulator, it is stipulated that except for the faulty joint _Jf locked at the faulty angle, the rest of the joints are fixed at Determine the two-dimensional projection plane of the kinematic model according to the axial direction of the faulty joint _Jf : X-axis joints correspond to the YZ plane projection diagram of the basic coordinate system, Y-axis joints correspond to the XZ plane projection diagram, Z-axis joints correspond to the XY plane projection diagram, The two-dimensional map can be used to determine the screw coordinates of each joint except the axial direction of the faulty joint, and the screw coordinates along the axial direction of the faulty joint need to be obtained from the reconstructed kinematics model.

Step 2) determining the screw coordinates by the geometric analysis method comprises the following steps:

21) If the faulty joint is an X-axis joint, after the joint is faulty, the transformation relationship between the basic coordinate system and the tool coordinate system is:

${\mathrm{<span\; class="notranslate">\; g</span>}}_{\mathrm{<span\; class="notranslate">\; the\; s</span>}\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 0</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>\left[\begin{array}{cccc}\mathrm{<span\; class="notranslate">\; 1</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& {\mathrm{<span\; class="notranslate">\; \&Sigma;l</span>}}_{\mathrm{<span\; class="notranslate">\; x</span>}}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& {\mathrm{<span\; class="notranslate">\; c\&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; f</span>}}& <span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; s\&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; f</span>}}& {\mathrm{<span\; class="notranslate">\; \&Sigma;l</span>}}_{\mathrm{<span\; class="notranslate">\; the\; y</span>}\mathrm{<span\; class="notranslate">\; r</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; \&Sigma;l</span>}}_{\mathrm{<span\; class="notranslate">\; z</span>}\mathrm{<span\; class="notranslate">\; f</span>}}{\mathrm{<span\; class="notranslate">\; s\&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; f</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; \&Sigma;l</span>}}_{\mathrm{<span\; class="notranslate">\; the\; y</span>}\mathrm{<span\; class="notranslate">\; f</span>}}{\mathrm{<span\; class="notranslate">\; c\&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; f</span>}}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& {\mathrm{<span\; class="notranslate">\; s\&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; f</span>}}& {\mathrm{<span\; class="notranslate">\; c\&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; f</span>}}& {\mathrm{<span\; class="notranslate">\; \&Sigma;l</span>}}_{\mathrm{<span\; class="notranslate">\; z</span>}\mathrm{<span\; class="notranslate">\; r</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; \&Sigma;l</span>}}_{\mathrm{<span\; class="notranslate">\; z</span>}\mathrm{<span\; class="notranslate">\; f</span>}}{\mathrm{<span\; class="notranslate">\; c\&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; f</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; \&Sigma;l</span>}}_{\mathrm{<span\; class="notranslate">\; the\; y</span>}\mathrm{<span\; class="notranslate">\; f</span>}}{\mathrm{<span\; class="notranslate">\; s\&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; f</span>}}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 1</span>}\end{array}\right]$

where θ _f represents the angle at which the faulty joint is locked, ∑l _x represents the sum of the lengths of the connecting rods along the X-axis direction, ∑l _yr and ∑l _zr represent the lengths of the connecting rods in the Y-axis and Z-axis directions before the faulty joint, respectively The sum of the lengths, ∑l _yf and ∑l _zf represent the sum of the lengths of the links in the Y-axis and Z-axis directions after the faulty joint, respectively.

The screw coordinates of the joints before the faulty joint label keep the original value unchanged, and the screw coordinates of the joints after the label are:

${\stackrel{<span\; class="notranslate">\; ~</span>}{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}}_{\mathrm{<span\; class="notranslate">\; the\; y</span>}}<span\; class="notranslate">\; =</span>\left[\begin{array}{ccc}\mathrm{<span\; class="notranslate">\; 0</span>}& {\mathrm{<span\; class="notranslate">\; c\&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; f</span>}}& <span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; s\&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; f</span>}}\end{array}\right]$

${\stackrel{<span\; class="notranslate">\; ~</span>}{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}}_{\mathrm{<span\; class="notranslate">\; z</span>}}<span\; class="notranslate">\; =</span>\left[\begin{array}{ccc}\mathrm{<span\; class="notranslate">\; 0</span>}& {\mathrm{<span\; class="notranslate">\; s\&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; f</span>}}& {\mathrm{<span\; class="notranslate">\; c\&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; f</span>}}\end{array}\right]$

${\stackrel{<span\; class="notranslate">\; ~</span>}{\mathrm{<span\; class="notranslate">\; q</span>}}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; =</span>\left(\begin{array}{ccc}{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}& {\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; \&Sigma;l</span>}}_{{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; f</span>}}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; cos\&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; f</span>}}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; \&Sigma;l</span>}}_{{\mathrm{<span\; class="notranslate">\; z</span>}}_{\mathrm{<span\; class="notranslate">\; f</span>}}}{\mathrm{<span\; class="notranslate">\; sin\&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; f</span>}}& {\mathrm{<span\; class="notranslate">\; z</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; \&Sigma;l</span>}}_{{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; f</span>}}}{\mathrm{<span\; class="notranslate">\; sin\&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; f</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; \&Sigma;l</span>}}_{{\mathrm{<span\; class="notranslate">\; z</span>}}_{\mathrm{<span\; class="notranslate">\; f</span>}}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; cos\&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; f</span>}}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; )</span>\end{array}\right)$

${\stackrel{<span\; class="notranslate">\; ~</span>}{\mathrm{<span\; class="notranslate">\; \&xi;</span>}}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; =</span>\left[\begin{array}{c}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; \&times;</span>{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}\\ {\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}\end{array}\right]$

in, represent the unit vectors in the direction of the axis of the motion screw of the Y-axis joint and the Z-axis joint respectively, and q _i =( _xi y _i z _i ) represents the point on the axis before the failure.

22) If the faulty joint is a Y-axis joint, after the joint is faulty, the transformation relationship between the basic coordinate system and the tool coordinate system is:

${\mathrm{<span\; class="notranslate">\; g</span>}}_{\mathrm{<span\; class="notranslate">\; the\; s</span>}\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 0</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>\left[\begin{array}{cccc}{\mathrm{<span\; class="notranslate">\; c\&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; f</span>}}& \mathrm{<span\; class="notranslate">\; 0</span>}& {\mathrm{<span\; class="notranslate">\; s\&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; f</span>}}& {\mathrm{<span\; class="notranslate">\; \&Sigma;l</span>}}_{\mathrm{<span\; class="notranslate">\; x</span>}\mathrm{<span\; class="notranslate">\; r</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; \&Sigma;l</span>}}_{\mathrm{<span\; class="notranslate">\; x</span>}\mathrm{<span\; class="notranslate">\; f</span>}}{\mathrm{<span\; class="notranslate">\; c\&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; f</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; \&Sigma;l</span>}}_{\mathrm{<span\; class="notranslate">\; z</span>}\mathrm{<span\; class="notranslate">\; f</span>}}{\mathrm{<span\; class="notranslate">\; s\&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; f</span>}}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 1</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& {\mathrm{<span\; class="notranslate">\; \&Sigma;l</span>}}_{\mathrm{<span\; class="notranslate">\; the\; y</span>}}\\ <span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; s\&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; f</span>}}& \mathrm{<span\; class="notranslate">\; 0</span>}& {\mathrm{<span\; class="notranslate">\; c\&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; f</span>}}& {\mathrm{<span\; class="notranslate">\; \&Sigma;l</span>}}_{\mathrm{<span\; class="notranslate">\; z</span>}\mathrm{<span\; class="notranslate">\; r</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; \&Sigma;l</span>}}_{\mathrm{<span\; class="notranslate">\; x</span>}\mathrm{<span\; class="notranslate">\; f</span>}}{\mathrm{<span\; class="notranslate">\; s\&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; f</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; \&Sigma;l</span>}}_{\mathrm{<span\; class="notranslate">\; z</span>}\mathrm{<span\; class="notranslate">\; f</span>}}{\mathrm{<span\; class="notranslate">\; c\&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; f</span>}}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 1</span>}\end{array}\right]$

Among them, ∑l _y represents the sum of the lengths of the connecting rods along the Y-axis direction, ∑l _xr represents the sum of the lengths of the connecting rods in the X-axis direction before the faulty joint, and ∑l _xf represents the X-axis direction after the faulty joint The sum of the lengths of the connecting rods.

The screw coordinates of the joints before the faulty joint label keep the original value unchanged, and the screw coordinates of the joints after the label are:

${\stackrel{<span\; class="notranslate">\; ~</span>}{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}}_{\mathrm{<span\; class="notranslate">\; x</span>}}<span\; class="notranslate">\; =</span>\left[\begin{array}{ccc}{\mathrm{<span\; class="notranslate">\; c\&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; f</span>}}& \mathrm{<span\; class="notranslate">\; 0</span>}& {\mathrm{<span\; class="notranslate">\; s\&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; f</span>}}\end{array}\right]$

${\stackrel{<span\; class="notranslate">\; ~</span>}{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}}_{\mathrm{<span\; class="notranslate">\; the\; y</span>}}<span\; class="notranslate">\; =</span>\left[\begin{array}{ccc}\mathrm{<span\; class="notranslate">\; 0</span>}& {\mathrm{<span\; class="notranslate">\; c\&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; f</span>}}& <span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; s\&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; f</span>}}\end{array}\right]$

${\stackrel{<span\; class="notranslate">\; ~</span>}{\mathrm{<span\; class="notranslate">\; q</span>}}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; =</span>\left(\begin{array}{ccc}{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; \&Sigma;l</span>}}_{{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; f</span>}}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; cos\&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; f</span>}}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; \&Sigma;l</span>}}_{{\mathrm{<span\; class="notranslate">\; z</span>}}_{\mathrm{<span\; class="notranslate">\; f</span>}}}{\mathrm{<span\; class="notranslate">\; sin\&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; f</span>}}& {\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}& {\mathrm{<span\; class="notranslate">\; z</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; \&Sigma;l</span>}}_{{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; f</span>}}}{\mathrm{<span\; class="notranslate">\; sin\&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; f</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; \&Sigma;l</span>}}_{{\mathrm{<span\; class="notranslate">\; z</span>}}_{\mathrm{<span\; class="notranslate">\; f</span>}}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; cos\&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; f</span>}}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; )</span>\end{array}\right)$

${\stackrel{<span\; class="notranslate">\; ~</span>}{\mathrm{<span\; class="notranslate">\; \&xi;</span>}}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; =</span>\left[\begin{array}{c}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; \&times;</span>{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}\\ {\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}\end{array}\right]$

in, Indicates the unit vector in the direction of the axis of the motion screw of the X-axis joint, Denotes the motion screw of joint i.

23) If the faulty joint is a Z-axis joint, after the joint is faulty, the transformation relationship between the basic coordinate system and the tool coordinate system is:

${\mathrm{<span\; class="notranslate">\; g</span>}}_{\mathrm{<span\; class="notranslate">\; the\; s</span>}\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 0</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>\left[\begin{array}{cccc}{\mathrm{<span\; class="notranslate">\; c\&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; f</span>}}& <span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; s\&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; f</span>}}& \mathrm{<span\; class="notranslate">\; 0</span>}& {\mathrm{<span\; class="notranslate">\; \&Sigma;l</span>}}_{\mathrm{<span\; class="notranslate">\; x</span>}\mathrm{<span\; class="notranslate">\; r</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; \&Sigma;l</span>}}_{\mathrm{<span\; class="notranslate">\; the\; y</span>}\mathrm{<span\; class="notranslate">\; f</span>}}{\mathrm{<span\; class="notranslate">\; s\&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; f</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; \&Sigma;l</span>}}_{\mathrm{<span\; class="notranslate">\; x</span>}\mathrm{<span\; class="notranslate">\; f</span>}}{\mathrm{<span\; class="notranslate">\; c\&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; f</span>}}\\ {\mathrm{<span\; class="notranslate">\; s\&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; f</span>}}& {\mathrm{<span\; class="notranslate">\; c\&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; f</span>}}& \mathrm{<span\; class="notranslate">\; 0</span>}& {\mathrm{<span\; class="notranslate">\; \&Sigma;l</span>}}_{\mathrm{<span\; class="notranslate">\; the\; y</span>}\mathrm{<span\; class="notranslate">\; r</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; \&Sigma;l</span>}}_{\mathrm{<span\; class="notranslate">\; the\; y</span>}\mathrm{<span\; class="notranslate">\; f</span>}}{\mathrm{<span\; class="notranslate">\; c\&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; f</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; \&Sigma;l</span>}}_{\mathrm{<span\; class="notranslate">\; the\; y</span>}\mathrm{<span\; class="notranslate">\; f</span>}}{\mathrm{<span\; class="notranslate">\; s\&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; f</span>}}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 1</span>}& {\mathrm{<span\; class="notranslate">\; \&Sigma;l</span>}}_{\mathrm{<span\; class="notranslate">\; z</span>}}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 1</span>}\end{array}\right]$

The screw coordinates of the joints before the faulty joint label keep the original value unchanged, and the screw coordinates of the joints after the label are:

${\stackrel{<span\; class="notranslate">\; ~</span>}{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}}_{\mathrm{<span\; class="notranslate">\; the\; y</span>}}<span\; class="notranslate">\; =</span>\left[\begin{array}{ccc}\mathrm{<span\; class="notranslate">\; 0</span>}& {\mathrm{<span\; class="notranslate">\; c\&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; f</span>}}& <span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; s\&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; f</span>}}\end{array}\right]$

${\stackrel{<span\; class="notranslate">\; ~</span>}{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}}_{\mathrm{<span\; class="notranslate">\; z</span>}}<span\; class="notranslate">\; =</span>\left[\begin{array}{ccc}\mathrm{<span\; class="notranslate">\; 0</span>}& {\mathrm{<span\; class="notranslate">\; s\&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; f</span>}}& {\mathrm{<span\; class="notranslate">\; c\&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; f</span>}}\end{array}\right]$

${\stackrel{<span\; class="notranslate">\; ~</span>}{\mathrm{<span\; class="notranslate">\; q</span>}}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; =</span>\left(\begin{array}{ccc}{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; \&Sigma;l</span>}}_{{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; f</span>}}}{\mathrm{<span\; class="notranslate">\; sin\&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; f</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; \&Sigma;l</span>}}_{{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; f</span>}}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; cos\&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; f</span>}}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; )</span>& {\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; \&Sigma;l</span>}}_{{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; f</span>}}}{\mathrm{<span\; class="notranslate">\; sin\&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; f</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; \&Sigma;l</span>}}_{{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; f</span>}}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; cos\&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; f</span>}}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; )</span>& {\mathrm{<span\; class="notranslate">\; z</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}\end{array}\right)$

${\stackrel{<span\; class="notranslate">\; ~</span>}{\mathrm{<span\; class="notranslate">\; \&xi;</span>}}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; =</span>\left[\begin{array}{c}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; \&times;</span>{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}\\ {\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}\end{array}\right]$

Compared with the prior art, the present invention has the following advantages:

(1) The present invention characterizes the kinematics model of the manipulator by screw coordinates, realizes the versatility of kinematics model reconstruction after a single joint failure of manipulators with different configurations, and when the manipulator has a complex configuration and a high degree of freedom, Its versatility is particularly obvious.

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

(3) The present invention adopts the geometric method to deduce the mathematical expression between the screw coordinates and the fault joint angle and the length of the connecting rod when all kinds of joints fail. Under the known normal state, the mechanical arm screw coordinates and the fault joint locking angle On the basis of , the reconstructed screw coordinates can be directly obtained, which improves the efficiency of kinematic model reconstruction and can meet the real-time requirements of on-orbit applications.

Description of drawings

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

Figure 2 is the screw coordinate system under the normal state.

Figure 3 is the reconstructed screw system after joint three locking.

Fig. 4 is a YZ plane projection diagram after three joints are locked.

Figure 5 is the reconstructed screw system after the second joint is locked.

Fig. 6 is the XZ plane projection diagram after the second joint is locked.

Figure 7 is the reconstructed screw system after the joint is locked.

Fig. 8 is an XY plane projection view after the joint is locked.

detailed description

1. The projection method is used to perform two-dimensional projection on the kinematic model of the single-joint failure manipulator

As shown in Figure 1 is the flow chart of the method of the present invention. The present invention takes a robot arm with seven degrees of freedom in space as the research object. The length of each bar is shown in Table 1, and the configuration of the robot arm is shown in Figure 2. For each joint J _i (i=1,...,7), construct its motion screw, and the screw coordinates are shown in Table 2.

Table 1 Length of connecting rod

Table 2 Spinor coordinates under normal state

When the X-axis joint J ₃ is locked at 30°, the joints behind J ₃ are marked as The reconstructed screw system is shown in Figure 3, and the YZ direction projection is shown in Figure 4. When the Y-axis joint J ₂ is locked at 30°, the joints behind J ₂ are marked as The reconstructed screw system is shown in Figure 5, and the XZ direction projection is shown in Figure 6. When the Z-axis joint J ₁ is locked at 30°, the joints behind J ₁ are marked as The reconstructed screw system is shown in Figure 7, and the XY direction projection is shown in Figure 8.

2. Geometric analysis method to determine screw coordinates

When the X-axis joint J ₃ is locked, the changed screw parameter is and Both are functions of θ ₃ , the reconstructed screw parameters are shown in Table 3, and the screw coordinates are shown in Table 4. The basic coordinate system S maintains the original direction, and the direction of the tool coordinate system T changes. The transformation relationship between the basic coordinate system and the tool coordinate system is:

${\mathrm{<span\; class="notranslate">\; g</span>}}_{\mathrm{<span\; class="notranslate">\; the\; s</span>}\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 0</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>\left[\begin{array}{cccc}\mathrm{<span\; class="notranslate">\; 1</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1.5</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0.866</span>}& <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 0.5</span>}& \mathrm{<span\; class="notranslate">\; 2.076</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0.5</span>}& \mathrm{<span\; class="notranslate">\; 0.866</span>}& \mathrm{<span\; class="notranslate">\; 6.046</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 1</span>}\end{array}\right]<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; )</span>$

Table 3 Reconstructed screw parameters after three-joint locking

Table 4 Reconstructed screw coordinates after three-joint locking

When the Y _- axis joint J2 is locked, the changed screw parameter is and _Both are functions of θ2. The reconstructed screw parameters are shown in Table 5, and the screw coordinates are shown in Table 6. The basic coordinate system S maintains the original direction, and the direction of the tool coordinate system T changes. The transformation relationship between the basic coordinate system and the tool coordinate system is:

${\mathrm{<span\; class="notranslate">\; g</span>}}_{\mathrm{<span\; class="notranslate">\; the\; s</span>}\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 0</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>\left[\begin{array}{cccc}\mathrm{<span\; class="notranslate">\; 0.866</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0.5</span>}& <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 7.782</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 1</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}\\ <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 0.5</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0.866</span>}& \mathrm{<span\; class="notranslate">\; 8.943</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 1</span>}\end{array}\right]<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; )</span>$

Table 5 Reconstructed screw parameters after two joints are locked

When the Z-axis joint J ₁ is locked, the changed screw parameter is and Both are functions of _θ1 . The reconstructed screw parameters are shown in Table 7, and the screw coordinates are shown in Table 8. The basic coordinate system S maintains the original direction, and the direction of the tool coordinate system T changes. The transformation relationship between the basic coordinate system and the tool coordinate system is:

${\mathrm{<span\; class="notranslate">\; g</span>}}_{\mathrm{<span\; class="notranslate">\; the\; s</span>}\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 0</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>\left[\begin{array}{cccc}\mathrm{<span\; class="notranslate">\; 0.866</span>}& <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 0.5</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 0.799</span>}\\ \mathrm{<span\; class="notranslate">\; 0.5</span>}& \mathrm{<span\; class="notranslate">\; 0.866</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1.616</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 1</span>}& \mathrm{<span\; class="notranslate">\; 11.2</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 1</span>}\end{array}\right]<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 3</span>}<span\; class="notranslate">\; )</span>$

Table 6 Reconstructed screw coordinates after joint locking

Table 7 - Reconstructed screw parameters after joint locking

Table 8 - Reconstructed screw coordinates after joint locking

The content that is not described in detail in the description of the present invention belongs to the well-known technology of those skilled 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& \&Sigma;l_x\\ 0& {\mathrm{c\&theta;}}_f& -{\mathrm{s\&theta;}}_f& \&Sigma;l_{yr}+\&Sigma;l_{zf}{\mathrm{s\&theta;}}_f+\&Sigma;l_{yf}{\mathrm{c\&theta;}}_f\\ 0& {\mathrm{s\&theta;}}_f& {\mathrm{c\&theta;}}_f& \&Sigma;l_{zr}+\&Sigma;l_{zf}{\mathrm{c\&theta;}}_f+\&Sigma;l_{yf}{\mathrm{s\&theta;}}_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{\&omega;}}}_y=\left[\begin{array}{ccc}0& {\mathrm{c\&theta;}}_f& -{\mathrm{s\&theta;}}_f\end{array}\right]$

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

${\tilde{q}}_i=\left(\begin{array}{ccc}{x}_i& y_i+\&Sigma;l_{y_f}({\mathrm{cos\&theta;}}_f-1)+\&Sigma;l_{z_f}{\mathrm{sin\&theta;}}_f& z_i+\&Sigma;l_{y_f}{\mathrm{sin\&theta;}}_f+\&Sigma;l_{z_f}({\mathrm{cos\&theta;}}_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\&theta;}}_f& 0& {\mathrm{s\&theta;}}_f& \&Sigma;l_{xr}+\&Sigma;l_{xf}{\mathrm{c\&theta;}}_f+\&Sigma;l_{zf}{\mathrm{s\&theta;}}_f\\ 0& 1& 0& \&Sigma;l_y\\ -{\mathrm{s\&theta;}}_f& 0& {\mathrm{c\&theta;}}_f& \&Sigma;l_{zr}+\&Sigma;l_{xf}{\mathrm{s\&theta;}}_f+\&Sigma;l_{zf}{\mathrm{c\&theta;}}_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{\&omega;}}}_x=\left[\begin{array}{ccc}{\mathrm{c\&theta;}}_f& 0& {\mathrm{s\&theta;}}_f\end{array}\right]$

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

${\tilde{q}}_i=\left(\begin{array}{ccc}{x}_i+\&Sigma;l_{x_f}({\mathrm{cos\&theta;}}_f-1)+\&Sigma;l_{z_f}{\mathrm{sin\&theta;}}_f& y_i& z_i+\&Sigma;l_{x_f}{\mathrm{sin\&theta;}}_f+\&Sigma;l_{z_f}({\mathrm{cos\&theta;}}_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\&theta;}}_f& -{\mathrm{s\&theta;}}_f& 0& \&Sigma;l_{xr}+{\&Sigma;}_{yf}{\mathrm{s\&theta;}}_f+\&Sigma;l_{xf}{\mathrm{c\&theta;}}_f\\ {\mathrm{s\&theta;}}_f& {\mathrm{c\&theta;}}_f& 0& \&Sigma;l_{yr}+\&Sigma;l_{yf}{\mathrm{c\&theta;}}_f+\&Sigma;l_{yf}{\mathrm{s\&theta;}}_f\\ 0& 0& 1& \&Sigma;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{\&omega;}}}_y=\left[\begin{array}{ccc}0& {\mathrm{c\&theta;}}_f& -{\mathrm{s\&theta;}}_f\end{array}\right]$

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

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