A solution method for the inverse kinematics of reconfiguration of a reconfigurable space manipulator

Abstract

A solution method for the inverse kinematics of a reconfigurable space manipulator, which relates to a solution method for the inverse kinematics of a manipulator. The kinematic model of the manipulator is established to obtain the position-level forward kinematic equation during the reconfiguration operation. Combining the idea of elementary matrix transformation, the active rotational joints and passive translational joints are separated to obtain the velocity-level forward and inverse kinematic equations during the reconfiguration operation. According to the characteristics of the velocity-level reconfiguration method, the position motion constraints of the end effector during the reconfiguration operation are simplified. The unit quaternion spherical linear interpolation method is used to plan the expected attitude trajectory of the end effector, and the high-order polynomial planning method is used to plan the expected trajectories of the two passive telescopic arm rods. Based on the velocity-level closed-loop feedback idea, a method for solving the inverse kinematics of reconfiguration is obtained, and the solution of the inverse kinematics of reconfiguration is obtained. It effectively increases the flexibility of the manipulator reconfiguration scheme selection, the joint motion is smooth, and the manipulator reconfiguration is safe and reliable.

Description

Technical Field

[0001] The present invention relates to a method for solving the inverse kinematics of a robotic arm, in particular to a method for solving the reconfigurable inverse kinematics of a reconfigurable space robotic arm, belonging to the technical field of robotic arm inverse kinematics research. Background Technique

[0002] Space robotic arms are widely used in on-orbit space services and are the core equipment for completing tasks such as the construction and maintenance of space stations, the recovery and repair of spacecraft, orbital debris cleaning, and assisting astronauts in extravehicular activities. Reconfigurable space robotic arms have strong adaptability to complex on-orbit tasks. Therefore, reconfigurable space robotic arms are an inevitable trend in the future development of space robotic arms.

[0003] Currently, scholars at home and abroad have proposed various design concepts for reconfigurable space robotic arms. The inventor has also proposed a novel reconfigurable space robotic arm with two passive telescopic boom rods in the literature "J. Zhao, Z. Zhao, X. Yang, et al., Inverse kinematics and workspace analysis of a novel SSRMS-type reconfigurable space manipulator with two lockable passive telescopic links. Mech. Mach. Theory [J], 2023, 180: 105152." It inherits the advantages of the space station robotic arm and has the function of reconfiguration, and has a simpler structural composition and a more reliable reconfiguration method than traditional reconfigurable robotic arms based on modular joints. The above-mentioned robotic arm changes the configuration of the robotic arm by changing the lengths of the two passive telescopic boom rods during the reconfiguration stage. To meet the requirements of structural simplification and lightweight space applications, the passive telescopic boom rods are not equipped with dedicated drivers for controlling the telescopic movement, that is, an underactuated method is adopted. To achieve the reconfiguration operation, the robotic arm first needs to form a kinematic closed chain with auxiliary facilities, and then the movement of the active rotary joints is transmitted to the passive telescopic boom rods through connecting rods by solving the closed-chain kinematics. However, its structural characteristics and reconfiguration method make it difficult to directly apply the existing method in the literature to the solution of its reconfigurable inverse kinematics, and only a position-level method for solving the reconfigurable inverse kinematics is proposed.

[0004] However, the position-level method for solving the reconfigurable inverse kinematics has the following disadvantages: First, the fixed handle in the end effector grasping auxiliary facility of the robotic arm forms a closed-chain structure, locking the position and attitude movements of the end effector. That is, during the reconfiguration process, both the position and attitude of the robotic arm end remain unchanged, resulting in a relatively small number of valid solutions that can be obtained for the reconfigurable inverse kinematics equation during the reconfiguration operation of the robotic arm, and the flexibility and reliability of the reconfigurable scheme selection are relatively low. Second, only the inverse kinematics solution at the position level can be obtained, and it cannot guarantee whether the joint movement speed is continuous and smooth, thus unable to ensure the safe and reliable implementation of the robotic arm reconfiguration operation.

[0005] In summary, for the reconfigurable space robotic arm, there is an urgent need to propose a method for solving the velocity-level reconfigurable inverse kinematics with high flexibility and reliability of the reconfiguration scheme, providing key technical support for promoting the development of space robotic arms. Summary of the Invention

[0006] To solve the deficiencies in the background technology, the present invention provides a method for solving the reconfigurable inverse kinematics of a reconfigurable space robotic arm. After the robotic arm forms a closed chain, it only needs to constrain the position of the robotic arm end. By flexibly planning the end attitude movement and jointly controlling the movement of the passive telescopic arm through the movement of the active rotation joints, the flexibility of the reconfigurable scheme selection of the robotic arm is effectively increased, the smoothness of the joint movement is increased, and the safety and reliability of the robotic arm during the reconfiguration operation are ensured.

[0007] To achieve the above object, the present invention adopts the following technical solution: A method for solving the reconfigurable inverse kinematics of a reconfigurable space robotic arm, for a robotic arm sequentially provided with a No. 1 joint, a No. 2 joint, a No. 3 joint, a No. 4 passive telescopic arm, a No. 5 joint, a No. 6 passive telescopic arm, a No. 7 joint, a No. 8 joint, a No. 9 joint, and an end effector from the root to the end, characterized in that: the solving method includes the following steps:

[0008] Step 1: Establish the kinematic model of the robotic arm to obtain the position-level forward kinematics equation during the reconfiguration operation

[0009] S11. When the robotic arm is in the reconfiguration operation, it altogether includes seven active rotation joints and two passive translation joints. The kinematic model of the robotic arm is established using Craig's D-H method. The base coordinate system is represented as {x0y0z0}, and the end effector coordinate system is represented as {x 10 y 10 z 10}. The coordinate system {x m y m z m} (m = 1, 2, 3, 5, 7, 8, 9) represents the m-th active rotation joint coordinate system, and the coordinate system {x n yn z n}(n = 4, 6) represents the coordinate system of the nth passive translation joint, θ m represents the joint variable of the mth active rotation joint, d n represents the joint variable of the nth passive translation joint;

[0010] S12. The forward kinematic equation of the manipulator at the position level is obtained according to the pose relationship between adjacent joint coordinate systems of the manipulator as follows:

[0011]

[0012] Among them, Φ represents the vector of joint variables of the manipulator, represents the pose of the end - effector coordinate system {x 10 y 10 z 10} expressed in the base coordinate system {x0y0z0}, represents the pose of the end - effector coordinate system {x 10 y 10 z 10} expressed in the coordinate system {x9y9z9}, represents the pose of the coordinate system {x m y m z m} expressed in the coordinate system {x m-1 y m-1 z m-1}, represents the pose of the coordinate system {x n y n z n} expressed in the coordinate system {x n-1 y n-1 z n-1};

[0013] Step 2: Combine the idea of matrix elementary transformation to separate the active rotation joints and passive translation joints, and obtain the forward and inverse kinematic equations at the velocity level during the reconstruction operation

[0014] S21. According to the mapping relationship between the velocities of the active rotation joints and passive translation joints of the manipulator during the reconstruction operation and the Cartesian velocity of the end - effector, the forward kinematic equation at the velocity level is:

[0015]

[0016] Among them, the superscript 0 indicates that the reference coordinate system is the base coordinate system {x0y0z0}, represents the Cartesian velocity vector of the end - effector, 0 v = 0 v x ,0 v y , 0 v z T and 0 ω= 0 ω x , 0 ω y , 0 ω z T represent the linear velocity vector and the angular velocity vector respectively, 0 J(Φ)= 0 J1 0 J2 0 J3 0 J4 0 J5 0 J6 0 J7 0 J8 0 J9] represents the Jacobian matrix, represents the velocity vector of the joint variables;

[0017] S22. Separate the contributions of the active rotational joints and passive translational joints to the Cartesian velocity of the end effector in the velocity-level forward kinematic equation. For the separation operation, perform elementary transformations on the Jacobian matrix 0 J(Φ) to obtain:

[0018]

[0019] where, 0 J(θ)= 0 J1 0 J2 0 J3 0 J8 0 J5 0 J9 0 J7] represents the Jacobian matrix corresponding to the active rotational joints, θ=[θ1 θ2 θ3 θ8 θ5 θ9 θ7] T represents the position vector of the active rotational joints, represents the velocity vector of the active rotational joints, 0 J(L)= 0 J4 0 J6] represents the Jacobian matrix corresponding to the passive translational joints, L=[d4d6] T represents the position vector of the passive translational joints, represents the velocity vector of the passive translational joints. According to formula (7), it can be obtained The calculation formula is the velocity-level inverse kinematic equation:

[0020] ​​

[0021] Among them, 0 J + (θ) represents 0 the pseudoinverse matrix of J(θ), 0 J + (θ) = 0 J T (θ)( 0 J(θ) 0 J T (θ)) -1 ;

[0022] Step 3: Simplify the position motion constraint of the end effector during the reconstruction operation according to the characteristics of the velocity level reconstruction method

[0023] When the manipulator performs the reconstruction operation, the object grasped by the end effector is set on the spherical joint to form a closed-chain structure. The whole formed by the end effector and the movable end of the spherical joint can rotate around the center of the spherical joint. The motion area of the end position is restricted in a conical spherical surface area. In order to simplify the solution process of the reconstruction inverse kinematics, the origin of the end coordinate system of the manipulator is moved to the center of the spherical joint. When the manipulator performs the reconstruction operation, only the motion trajectory of the posture needs to be planned, and there is no need to plan the motion trajectory of its end position;

[0024] Step 4: Use the unit quaternion spherical linear interpolation method to plan the desired attitude trajectory of the end effector

[0025] S401. Set the postures of the end of the manipulator corresponding to the start time of the manipulator reconstruction operation, the completion time of the reconstruction of the No. 4 passive telescopic arm rod, and the completion time of the reconstruction of the No. 6 passive telescopic arm rod, and represent them in Z-Y-X Euler angles as Γ j (j = 1, 2, 3);

[0026] S402. Convert the three postures in S401 into postures represented by rotation matrices R j (j = 1, 2, 3);

[0027] S403. Convert the three postures in S402 into postures represented by unit quaternions U j = [q j0 , q j1 , q j2 , q j3 T ;

[0028] S404. Calculate the angles between the postures U1 and U2 and between U2 and U3 in S403 respectively:

[0029]

[0030] ​

[0031] Among them, represents the included angle between U1 and U2, represents the included angle between U2 and U3;

[0032] S405. Use the unit quaternion spherical linear interpolation method to perform interpolation between the postures U1 and U2 and U2 and U3 in S404 respectively. The interpolation formula is as follows:

[0033]

[0034] Among them, h k ∈[0,1] represents the control parameter, and t represents the sampling time;

[0035] S406. Convert the posture at each sampling moment interpolated in S405 into a posture represented by a rotation matrix. The conversion formula is as follows:

[0036]

[0037] S407. Convert the posture represented by the rotation matrix at each sampling moment in S406 into a posture represented by Z - Y - X Euler angles. The conversion formula is as follows:

[0038]

[0039] S408. Take the derivative of formula (13) in S405 with respect to time t, and use the differentiated interpolation formula to perform interpolation between the postures U1 and U2 and U2 and U3 in S404. The interpolation formula is as follows:

[0040]

[0041] S409. Convert the derivative of the unit quaternion at each sampling moment obtained in S408 into the attitude angular velocity. The conversion formula is as follows:

[0042]

[0043] S410. So far, the desired posture Γ k (t) = [α k (t), β k (t), γ k (t)] T of the end effector at each sampling moment is planned in S407, and the desired angular velocity ω k (t) of the end effector at each sampling moment is planned in S409, and then the desired pose and the desired Cartesian velocity of the end effector are obtained;

[0044] Step 5: Use the high-order polynomial programming method to plan the expected trajectories of the two passive telescopic booms

[0045] Step 6: Obtain a method for solving the reconstructed inverse kinematics based on the velocity-level closed-loop feedback idea

[0046] S61. Use formula (1) to calculate the actual pose of the end effector at the current moment during the robotic arm reconstruction operation, and subtract the actual pose from the expected pose of the end effector at the current moment planned in Step 4 to obtain the end pose error as follows:

[0047] e = X d - X a (18)

[0048] where, X d represents the expected pose at the current moment, and X a represents the actual pose at the current moment;

[0049] S62. Use formula (4) to calculate the actual Cartesian velocity of the end effector at the current moment during the robotic arm reconstruction operation, and subtract the actual Cartesian velocity from the expected Cartesian velocity of the end effector at the current moment planned in Step 4 to obtain the end Cartesian velocity error as follows:

[0050]

[0051] where, represents the expected Cartesian velocity at the current moment, represents the actual Cartesian velocity at the current moment;

[0052] S63. Based on the velocity-level closed-loop feedback idea, introduce the end pose error in S61 and the end Cartesian velocity error in S62 as feedback terms into the gradient projection method to obtain the velocity-level closed-loop method for solving the reconstructed inverse kinematics equation, and its calculation formula is as follows:

[0053]

[0054] where, K p and K v are two symmetric positive definite matrices, representing the feedback coefficient matrices of the end pose error and the end Cartesian velocity error respectively, K represents a scalar form of the optimization coefficient, I represents the identity matrix, represents the gradient vector of the joint limit avoidance function H(θ);

[0055] Step 7: Combine Steps 4, 5, and 6 to solve for the solution of the reconstructed inverse kinematics

[0056] Substitute the expected attitude trajectory of the end effector obtained in Step 4 and the expected trajectories of the two passive telescopic boom arms obtained in Step 5 into the method for solving the reconstruction inverse kinematics in Step 6, and by adjusting the feedback coefficient matrices K p and K v as well as the optimization coefficient K, the end pose accuracy of the active rotating joints of the robotic arm is optimized under the premise of meeting the joint limits. At this time, the position trajectory and velocity trajectory of the active rotating joints obtained are the solutions of the reconstruction inverse kinematics during the reconstruction operation of the robotic arm.

[0057] Compared with the prior art, the beneficial effects of the present invention are as follows: During the reconstruction operation of the robotic arm, the position movement of the end of the robotic arm is restricted by a spherical joint, and the end of the robotic arm can move around the center of the spherical joint, releasing the movement constraint on the attitude of the end of the robotic arm, greatly increasing the flexibility and reliability of the reconstruction scheme selection of the robotic arm. Interpolating the attitude based on the quaternion method avoids the singularity problems encountered during interpolation of other attitude representation forms. After coordinating the movement of the end attitude with the movement of the active rotating joints, the movement of the passive telescopic boom arms can be effectively controlled. In addition, the relationship between the movement of the active and passive joints of the robotic arm and the movement of the end of the robotic arm established according to the characteristics of the velocity-level reconstruction method is extended to the velocity level. On this basis, a method for solving the reconstruction inverse kinematics based on velocity-level closed-loop feedback is proposed. Compared with the existing velocity-level inverse kinematics methods, it solves the problem of reduced end pose accuracy caused by inherent joint drift during the solution process, and the solutions obtained by solving the reconstruction inverse kinematics not only have smooth and continuous joint position trajectories, but also continuous and smooth joint velocity trajectories, greatly increasing the smoothness of joint movement, effectively ensuring the reliability and safety of the robotic arm during the reconstruction operation, and providing key technical support for promoting the development and application of reconfigurable robotic arms. BRIEF DESCRIPTION OF THE DRAWINGS

[0058] Figure 1 is a schematic structural diagram of the robotic arm to which the solution method of the present invention is applied;

[0059] Figure 2 is Figure 1 a schematic diagram of the coordinate systems of the joints of the robotic arm in

[0060] Figure 3 is a schematic structural diagram of the robotic arm during the reconstruction operation in the solution method of the present invention;

[0061] Figure 4 is a simplified schematic diagram of the position movement constraint of the end effector in the solution method of the present invention;

[0062] Figure 5 is a block diagram of the method for solving the reconstruction inverse kinematics in the solution method of the present invention;

[0063] Figure 6 It is a schematic structural diagram of reconstructing the No. 4 passive telescopic arm in the solution method of the present invention;

[0064] Figure 7 It is a schematic structural diagram of reconstructing the No. 6 passive telescopic arm in the solution method of the present invention;

[0065] Figure 8 It is a schematic diagram of the position of the active rotary joint obtained in the embodiment;

[0066] Figure 9 It is a schematic diagram of the speed of the active rotary joint obtained in the embodiment;

[0067] Figure 10 It is a schematic diagram of the position error of the end of the robotic arm obtained in the embodiment;

[0068] Figure 11 It is a schematic diagram of the attitude error of the end of the robotic arm obtained in the embodiment. Detailed implementation manners

[0069] Next, the technical solutions in the embodiments of the present invention will be clearly and completely described in conjunction with the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the invention, rather than all the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative efforts shall fall within the protection scope of the present invention.

[0070] A solution method for the inverse kinematics reconstruction of a reconfigurable space robotic arm, and the robotic arm referred to Figure 1 is shown as follows. Its structure is sequentially provided with a No. 1 joint, a No. 2 joint, a No. 3 joint, a No. 4 passive telescopic arm, a No. 5 joint, a No. 6 passive telescopic arm, a No. 7 joint, a No. 8 joint, a No. 9 joint, and an end effector from the root to the end. Among them, the No. 1 joint, the No. 2 joint, the No. 3 joint, the No. 5 joint, the No. 7 joint, the No. 8 joint, and the No. 9 joint are active rotary joints. The solution method includes the following steps:

[0071] Step 1: Establish the kinematic model of the robotic arm to obtain the position-level forward kinematic equation during the reconstruction operation

[0072] S11. Treat the two passive telescopic arms of the robotic arm as passive translation joints. During the reconstruction operation, the robotic arm altogether includes seven active rotary joints and two passive translation joints. Use Craig's D-H method to establish the kinematic model of the robotic arm. The base coordinate system is expressed as {x0y0z0}, and the end effector coordinate system is expressed as {x 10 y 10 z 10}, and the coordinate system {x m ym z m}(m = 1, 2, 3, 5, 7, 8, 9) represents the m-th active rotational joint coordinate system, the coordinate system {x n y n z n}(n = 4, 6) represents the n-th passive translational joint coordinate system, θ m represents the joint variable of the m-th active rotational joint, d n represents the joint variable of the n-th passive translational joint.

[0073] S12. The forward kinematic equation of the manipulator in the position level is obtained according to the pose relationship between adjacent joint coordinate systems of the manipulator as follows:

[0074]

[0075] Among them, Φ represents the joint variable vector of the manipulator, represents the pose of the end effector coordinate system {x 10 y 10 z 10} expressed in the base coordinate system {x0y0z0}, represents the pose of the end effector coordinate system {x 10 y 10 z 10} expressed in the coordinate system {x9y9z9}, represents the pose of the coordinate system {x m y m z m} expressed in the coordinate system {x m-1 y m-1 z m-1} expressed in the coordinate system {x represents the pose of the coordinate system {x n y n z n} expressed in the coordinate system {x n-1 y n-1 z n-1} expressed in the coordinate system {x and The calculation formulas of

[0076]

[0077]

[0078] Among them, s represents the sine function sin, c represents the cosine function cos, a m-1 and a n-1 represent the link lengths, α m-1 and α n-1 represent the link angles, d m represents the link offset;

[0079] Step 2: Combine the idea of elementary matrix transformation to separate the active rotational joints and passive translational joints, and obtain the forward and inverse kinematic equations at the velocity level during the reconstruction operation.

[0080] S21. According to the mapping relationship between the velocities of the active rotational joints and passive translational joints during the robotic arm reconstruction operation and the Cartesian velocity of the end effector, the forward kinematic equation at the velocity level is obtained as follows:

[0081]

[0082] where the superscript 0 in the upper left indicates that the reference coordinate system is the base coordinate system {x0y0z0}, represents the Cartesian velocity vector of the end effector, 0 v = 0 v x , 0 v y , 0 v z T and 0 ω = 0 ω x , 0 ω y , 0 ω z T represent the linear velocity vector and the angular velocity vector respectively, 0 J(Φ) = 0 J1 0 J2 0 J3 0 J4 0 J5 0 J6 0 J7 0 J8 0 J9] represents the Jacobian matrix, represents the velocity vector of the joint variables.

[0083] S22. The inverse kinematics at the velocity level means that, given and the desired trajectory of the passive translational joints, solve for the velocities corresponding to the active rotational joints in the velocity vector . Different from the processing method of traditional serial robotic arms, in order to obtain the velocities of the active rotational joints, the reconfigurable robotic arm needs to first separate the contributions of the active rotational joints and passive translational joints to the Cartesian velocity of the end effector in the forward kinematic equation at the velocity level. Specifically, to achieve the separation operation, perform elementary transformation on the Jacobian matrix 0 J(Φ). The first step is to swap 0 the 4th column and the 8th column of J(Φ), and the second step is to swap 0 ​​The 6th and 9th columns of J(Φ), the transformation process and results are expressed as:

[0084]

[0085] Among them, represents 0 the matrix obtained after J(Φ) undergoes elementary transformation, ET represents the elementary transformation symbol, represents swapping the f-th column and the g-th column.

[0086] To keep the equality in formula (4) valid, elementary transformation is performed on the velocity vector In the first step, swap the 4th row and the 8th row of , and in the second step, swap the 6th row and the 9th row of . The transformation process and results are expressed as:

[0087]

[0088] Among them, represents the matrix obtained after undergoing elementary transformation, represents swapping the f-th row and the g-th row.

[0089] Substitute formula (5) and formula (6) into formula (4) and expand to get:

[0090]

[0091] Among them, 0 J(θ) = 0 J1 0 J2 0 J3 0 J8 0 J5 0 J9 0 J7] represents the Jacobian matrix corresponding to the active rotating joints, θ = [θ1 θ2 θ3 θ8 θ5 θ9 θ7] T represents the position vector of the active rotating joints, represents the velocity vector of the active rotating joints, 0 J(L) = 0 J4 0 J6] represents the Jacobian matrix corresponding to the passive translational joints, L = [d4 d6] T represents the position vector of the passive translational joints, represents the velocity vector of the passive translational joints.

[0092] When the robotic arm undergoes reconstruction operation, L and are both given. According to formula (7), The calculation formula is the inverse kinematics equation of the velocity level:

[0093]

[0094] Among them, 0 J + (θ) represents 0 the pseudo-inverse matrix of J(θ), 0 J + (θ) = 0 J T (θ)( 0 J(θ) 0 J T (θ)) -1 ;

[0095] Step 3: Simplify the position motion constraint of the end effector during the reconstruction operation according to the characteristics of the velocity-level reconstruction method

[0096] When the manipulator is reconstructed, the object grasped by the end effector is set on the spherical joint to form a closed-chain structure. The whole formed by the end effector and the movable end of the spherical joint can move around the center of the spherical joint. The motion area of the end position is constrained in a conical spherical surface area. According to this characteristic, in order to simplify the solution process of the reconstruction inverse kinematics, the end effector and the movable end of the spherical joint are regarded as a whole for processing, that is, the origin of the end coordinate system of the manipulator is moved to the center of the spherical joint. When the manipulator is reconstructed, only the motion trajectory of the attitude needs to be planned, and there is no need to plan the motion trajectory of its end position, which can greatly reduce the workload of planning the motion trajectory of the end effector;

[0097] Step 4: Use the unit quaternion spherical linear interpolation method to plan the desired attitude trajectory of the end effector

[0098] S401. Set the attitudes of the end of the manipulator corresponding to the start time of the manipulator reconstruction operation, the completion time of the reconstruction of the No. 4 passive telescopic arm, and the completion time of the reconstruction of the No. 6 passive telescopic arm, and represent them in Z-Y-X Euler angles as Γ j = [α j , β j , γ j T (j = 1, 2, 3).

[0099] S402. Convert the three attitudes Γ j = [α j , β j , γ j T (j = 1, 2, 3) in S401 into the attitudes represented by the rotation matrix. The conversion formula is as follows:

[0100] ​​

[0101] S403. Convert the three postures R in S402 j (j = 1, 2, 3) into postures represented by unit quaternions. The conversion formula is as follows:

[0102]

[0103] where the quaternion U j = [q j0 , q j1 , q j2 , q j3 T represents the j-th (j = 1, 2, 3) posture, and q j0 , q j1 , q j2 , q j3 represent the four constituent elements of U j .

[0104] S404. Calculate the angles between the postures U1 and U2 and between U2 and U3 in S403 respectively:

[0105]

[0106]

[0107] where represents the angle between U1 and U2, and represents the angle between U2 and U3.

[0108] S405. Use the unit quaternion spherical linear interpolation method to perform interpolation between the postures U1 and U2 and between U2 and U3 in S404 respectively. The interpolation formula is as follows:

[0109]

[0110] where h k ∈ [0, 1] represents the control parameter, and t represents the sampling time.

[0111] S406. Convert the posture at each sampling moment interpolated in S405 into a posture represented by a rotation matrix. The conversion formula is as follows:

[0112]

[0113] S407. Convert the posture represented by the rotation matrix at each sampling moment in S406 into a posture represented by Z - Y - X Euler angles. The conversion formula is as follows:

[0114] ​

[0115] S408. Take the derivative of the formula (13) in S405 with respect to time t, and use the interpolated formula after taking the derivative to interpolate between the postures U1 and U2 and between U2 and U3 in S404. The interpolation formula is as follows:

[0116]

[0117] S409. Convert the derivative of the unit quaternion at each sampling moment obtained in S408 into an attitude angular velocity. The conversion formula is as follows:

[0118]

[0119] S410. So far, the desired attitude Γ k (t) = [α k (t), β k (t), γ k (t)] T of the end effector at each sampling moment is planned in S407. The desired angular velocity ω k (t) of the end effector at each sampling moment is planned in S409. Combining the two can obtain the desired attitude trajectory of the end effector. Combining the desired attitude obtained in this step with the position vector of the center of the spherical joint relative to the coordinate system {x0y0z0} in Step 3 gives the desired pose of the end effector. Combining the desired angular velocity obtained in this step with the vector [0, 0, 0] T gives the desired Cartesian velocity of the end effector;

[0120] Step Five: Plan the desired trajectories of the two passive telescopic arm rods using the high-order polynomial planning method

[0121] The desired trajectories of the two passive telescopic arm rods are the position changes and velocity changes of the two passive translation joints relative to the sampling time, and are preferably obtained by using the fifth-order polynomial planning introduced in John J. Craig. Introduction to Robotics, 3rd Edition [M]. China Machine Press, 2006. The high-order polynomial can ensure the continuity and smoothness of the trajectory curve;

[0122] Step Six: Obtain a method for solving the reconstructed inverse kinematics based on the idea of velocity-level closed-loop feedback

[0123] S61. Calculate the actual pose of the end effector at the current moment during the reconstructed operation of the robotic arm using formula (1), and subtract the actual pose from the desired pose of the end effector at the current moment planned in Step Four to obtain the following end pose error:

[0124] e = X d - X a (18)

[0125] Among them, X d represents the desired pose at the current moment, and X a represents the actual pose at the current moment.

[0126] S62. Calculate the actual Cartesian velocity of the end effector at the current moment during the robotic arm reconstruction operation using formula (4), and subtract the actual Cartesian velocity from the desired Cartesian velocity of the end effector at the current moment planned in step four to obtain the end Cartesian velocity error as follows:

[0127]

[0128] Among them, represents the desired Cartesian velocity at the current moment, and represents the actual Cartesian velocity at the current moment.

[0129] S63. To solve the problem that the gradient projection method commonly used to solve formula (8) has joint drift resulting in reduced end pose accuracy, based on the velocity-level closed-loop feedback idea, introduce the end pose error in S61 and the end Cartesian velocity error in S62 as feedback terms into the gradient projection method to obtain a velocity-level closed-loop method for solving the reconstruction inverse kinematic equation, and its calculation formula is as follows:

[0130]

[0131] Among them, K p and K v are two symmetric positive definite matrices, representing the feedback coefficient matrices of the end pose error and the end Cartesian velocity error respectively, K represents an optimization coefficient in scalar form, I represents the identity matrix, represents the gradient vector of the joint limit avoidance function H(θ). The gradient projection method is preferably the method introduced in Dubey R.V., Euler J.A., Babcock S.M., An efficient gradient projection optimization scheme for a seven-degree-of-freedom redundant robot with spherical wrist[C] / / Proceedings. 1988 IEEE International Conference on Robotics and Automation. IEEE, 1988: 28 - 36.

[0132] The gradient vector in formula (20) has the specific expression as:

[0133]

[0134] Gradient vector The calculation formula for the m-th row is as follows:

[0135]

[0136] where θ m_max and θ m_min respectively represent the upper and lower limits of the joint limit of the m-th active rotary joint;

[0137] Step Seven: Combine Steps Four, Five, and Six to solve for the solution of the reconstructed inverse kinematics

[0138] Substitute the desired pose trajectory of the end effector planned in Step Four and the desired trajectories of the two passive telescopic arm rods planned in Step Five into the method for solving the reconstructed inverse kinematics in Step Six. By adjusting the feedback coefficient matrices K p and K v and the optimization coefficient K, make the end pose accuracy of the active rotary joints of the robotic arm reach the optimal value on the premise of satisfying the joint limits. At this time, the position trajectory and velocity trajectory of the active rotary joints obtained are the solutions of the reconstructed inverse kinematics during the reconstruction operation of the robotic arm.

[0139] Embodiment

[0140] Step One: Combine Figure 1 the robotic arm configuration shown in, and establish the kinematic model of the robotic arm using Craig's method. Combine Figure 2 shown in, and solve to obtain the position-level forward kinematic equation of the robotic arm:

[0141]

[0142] where,

[0143] Combine Figure 2 shown in, the D-H parameters corresponding to the robotic arm are shown in Table 1 in detail. Substitute the numerical values of each parameter in the table into the position-level forward kinematic equation of formula (1) to calculate the specific value of 1 0 0T;

[0144] Table 1 D-H parameters of the reconfigurable space robotic arm

[0145]

[0146] Step Two: According to the definition of the velocity-level kinematics of the robotic arm, when the velocity of the active rotary joint and the velocity of the passive translation joint When known, combining the forward kinematic equation of the velocity level in Formula (4) and the D-H parameters in Table 1, the Cartesian velocity vector of the end effector can be calculated. When the above and the position vector L of the passive translation joint are both given, combining the inverse kinematic equation of the velocity level in Formula (8) and the D-H parameters in Table 1, the velocity vector of the active rotation joint required for the manipulator to complete the reconstruction operation can be calculated.

[0147] Step 3: Combining Figures 1 to 4 As shown, the manipulator and the auxiliary facilities of the spherical joint form a reconstruction system. The formed closed-chain structure combines Figure 3 As shown, during the reconstruction operation, the end effector can move around the center of the spherical joint. To prevent structural interference during movement, the movement of the end effector is constrained within a conical spherical surface area. When the end effector and the movable end of the spherical joint are regarded as a whole, the end of the combination is constrained at the center of the spherical joint. Therefore, the coordinate system {x Figure 2 y 10 y 10 z 10} of the end effector in 10 y' 10 z' 10} and the link offset d' 10 are obtained by moving the coordinate system of the end effector in Figure 4 to the center position of the spherical joint. As shown in the figure, the shaded part in the figure is the above conical spherical surface area. When the radius of the spherical joint is R, d' 10 = d 10 + R, simplifying the position constraint of the end effector. During the reconstruction operation, only the attitude motion trajectory of the manipulator end needs to be planned, reducing the workload of task trajectory planning;

[0148] Step 4: The planning of the desired attitude trajectory of the end effector includes:

[0149] Set the attitudes of the manipulator represented by Z-Y-X Euler angles at the initial, intermediate, and end states respectively;

[0150] Convert the three attitudes represented by Z-Y-X Euler angles into attitudes represented by rotation matrices;

[0151] Convert the three attitudes represented by rotation matrices into attitudes represented by unit quaternions;

[0152] Calculate the angle between the unit quaternion corresponding to the initial attitude and the unit quaternion corresponding to the intermediate attitude, and the angle between the unit quaternion corresponding to the intermediate attitude and the unit quaternion corresponding to the end attitude;

[0153] Interpolate between the unit quaternion corresponding to the initial attitude and the unit quaternion corresponding to the intermediate attitude using the unit quaternion spherical linear interpolation method, and then use the same interpolation method to interpolate between the unit quaternion corresponding to the intermediate attitude and the unit quaternion corresponding to the end attitude to obtain the attitude represented by the unit quaternion at each sampling moment;

[0154] Convert the attitude represented by the unit quaternion at each moment into the attitude represented by the rotation matrix;

[0155] Convert the attitude represented by the rotation matrix at each moment into the attitude represented by the Z - Y - X Euler angles, and the desired attitude of the end effector at each sampling moment can be obtained;

[0156] Take the derivative of the unit quaternion spherical linear interpolation formula with respect to time, use the interpolated formula after differentiation to interpolate between the unit quaternion corresponding to the initial attitude and the unit quaternion corresponding to the intermediate attitude, and then interpolate between the unit quaternion corresponding to the intermediate attitude and the unit quaternion corresponding to the end attitude to obtain the derivative of the unit quaternion at each sampling moment;

[0157] Convert the derivative of the unit quaternion at each sampling moment into the attitude angular velocity, and the desired angular velocity of the end effector at each sampling moment can be obtained;

[0158] Step Five: The high - order polynomial used is a fifth - degree polynomial. The planning range of the 4th passive telescopic arm is [0.560, 0.860] (m), and the planning range of the 6th passive telescopic arm is [0.500, 0.800] (m). According to the definition of the fifth - degree polynomial interpolation method, the position trajectories and velocity trajectories of the two passive translation joints can be planned;

[0159] Step Six: Combine Figure 5 is the block diagram of the reconstruction inverse kinematics solution method based on the velocity - level closed - loop feedback idea. Combine formula (1), formula (4), formula (18), formula (19), the D - H parameters in Table 1, and the trajectory X obtained in Step Four d 、 X a 、 e and Combine the trajectory L obtained in Step Five, and formula (20) to calculate the velocity of the active rotation joint Integrate this velocity to obtain the joint position θ;

[0160] Step Seven: Combine Figure 3 、 Figures 6 to 11 As shown, set Figure 3 The end attitude at the start moment of reconstruction shown is [π, 0, π] T, the position vector of the spherical joint center relative to the coordinate system is [-0.55, -0.95, -1.52] T (m), Figure 6 The end attitude at the completion of the reconstruction of the No. 4 passive telescopic arm shown is [-π / 3, π / 12, 5π / 7] T , Figure 7 The end attitude at the completion of the reconstruction of the No. 6 passive telescopic arm shown is [-π / 3, π / 4, 3π / 5] T , the total time for the reconstruction operation is 50 s, and the time for reconstructing each passive telescopic arm is 25 s. Solve in combination with steps four, five, and six Figures 6 to 7 The inverse kinematic solution during the reconstruction operation shown, the sampling time step set during the calculation process is 0.002 s, and the coefficient matrix K p = diag[450, 450, 450, 0.001, 0.001, 0.001], K v = diag[0.001, 0.001, 0.001, 0.001, 0.005, 0.003], the optimization coefficient K = -0.05, and the position-time curve of the active rotary joint is obtained by solving and combined with Figure 8 shown, the speed-time curve is combined with Figure 9 shown. It can be seen that both are smooth and continuous, ensuring the smooth movement of each joint during the reconstruction of the manipulator. The position error of the end of the manipulator obtained by solving is combined with Figure 10 shown, and the attitude error is combined with Figure 11 shown. The maximum position error is about 10 -6 m, and the maximum attitude error is about 10 -5 rad, both of which reach a very high precision.

[0161] It can be seen from the simulation results that the solution method provided by the present invention can obtain the inverse kinematic solution with continuous, smooth joint position trajectories and joint velocity trajectories and high end pose accuracy characteristics during the reconstruction operation stage of the manipulator. The method is reasonable and effective. Compared with the existing position-level reconstruction methods, the method of the present invention does not require strict fixation of the position and attitude of the end during the reconstruction operation of the manipulator. Instead, by forming a closed-chain structure with the spherical joint, the end of the manipulator can move around the spherical center, releasing the constraint on the end attitude movement, significantly increasing the flexibility and reliability of the manipulator reconstruction operation. Moreover, compared with the position-level reconfigurable methods, the solution obtained by solving the reconstruction inverse kinematics of the method of the present invention not only has smooth and continuous joint position trajectories, but also has continuous and smooth joint velocity trajectories, greatly increasing the smoothness of joint movement during the reconstruction process, effectively ensuring the reliability and safety during the reconstruction operation of the manipulator.

[0162] For those skilled in the art, it is obvious that the present invention is not limited to the details of the above-described exemplary embodiments, and the present invention can be implemented in other forms without departing from the spirit or basic characteristics of the present invention. Therefore, in any regard, the embodiments should be regarded as exemplary and non-limiting. The scope of the present invention is defined by the appended claims rather than the above description. Therefore, all changes falling within the meaning and scope of the equivalent conditions of the claims are intended to be embraced within the present invention. Any reference signs in the claims should not be construed as limiting the claims involved.

[0163] In addition, it should be understood that although this specification is described according to embodiments, not every embodiment only contains an independent technical solution. This narrative manner of the specification is only for clarity. Those skilled in the art should regard the specification as a whole, and the technical solutions in each embodiment can also be appropriately combined to form other embodiments that can be understood by those skilled in the art.

Claims

1. A solution method for the inverse kinematics of a reconfigurable space manipulator. The manipulator is successively provided with a No. 1 joint, a No. 2 joint, a No. 3 joint, a No. 4 passive telescopic arm rod, a No. 5 joint, a No. 6 passive telescopic arm rod, a No. 7 joint, an No. 8 joint, a No. 9 joint, and an end effector from the root to the end. It is characterized in that: The solution method includes the following steps: Step 1: Establish the kinematic model of the robotic arm to obtain the position-level forward kinematic equation during the reconstruction operation; Step 2: Combine the idea of elementary matrix transformation to separate the active rotational joints and passive translational joints, and obtain the velocity-level forward and inverse kinematic equations during the reconstruction operation; S21. According to the mapping relationship between the velocities of the active rotational joints and passive translational joints of the robotic arm during the reconstruction operation and the Cartesian velocity of the end effector, the velocity-level forward kinematic equation is obtained as follows: （4） Among them, the superscript 0 indicates that the reference coordinate system is the base coordinate system , represents the Cartesian velocity vector of the end effector, and represent the linear velocity vector and the angular velocity vector respectively, represents the Jacobian matrix, represents the velocity vector of the joint variables; S22. Separate the contributions of the active rotational joints and passive translational joints to the Cartesian velocity of the end effector in the velocity-level forward kinematic equation. For the separation operation, perform elementary transformations on the Jacobian matrix to obtain: （7） Among them, represents the Jacobian matrix corresponding to the active rotary joint, represents the position vector of the active rotary joint, represents the velocity vector of the active rotary joint, represents the Jacobian matrix corresponding to the passive translational joint, represents the position vector of the passive translational joint, represents the velocity vector of the passive translational joint, and can be obtained according to formula (7) , and the calculation formula is the inverse kinematics equation of the velocity level: （8） Among them, denotes the pseudo-inverse matrix of ; Step 3: Simplify the position motion constraints of the end effector during the reconstruction operation according to the characteristics of the velocity-level reconstruction method; During the reconstruction operation of the robotic arm, the object grasped by the end effector is set on the spherical joint to form a closed-chain structure. The whole formed by the end effector and the movable end of the spherical joint can move around the center of the spherical joint. The motion area of the end position is constrained in a conical spherical surface area. To simplify the solution process of the reconstruction inverse kinematics, the origin of the end coordinate system of the robotic arm is moved to the center of the spherical joint. During the reconstruction operation of the robotic arm, only the motion trajectory of the posture needs to be planned, and there is no need to plan the motion trajectory of its end position; Step 4: Use the unit quaternion spherical linear interpolation method to plan the desired posture trajectory of the end effector; S401. Set the postures of the end of the robotic arm corresponding to the start time of the robotic arm reconstruction operation, the completion time of the reconstruction of the No. 4 passive telescopic arm, and the completion time of the reconstruction of the No. 6 passive telescopic arm, and represent them in Z-Y-X Euler angles as ; S402. Convert the three postures in S401 into postures represented by rotation matrices ; S403. Convert the three postures in S402 into postures represented by unit quaternions ; S404. Calculate the postures in S403 respectively and and and the included angle between them: （11） （12） Among them, represents and the included angle between represents and the included angle between; S405. Use the unit quaternion spherical linear interpolation method to interpolate respectively between the postures in S404 and as well as and . The interpolation formula is as follows: （13） Among them, represents a control parameter, represents the sampling time; S406. Convert the posture at each sampling moment interpolated in S405 into a posture represented by a rotation matrix. The conversion formula is as follows: （14） S407. Convert the posture represented by the rotation matrix at each sampling moment in S406 into a posture represented by Z-Y-X Euler angles. The conversion formula is as follows: （15） S408. Take the derivative of the formula (13) in S405 with respect to time . Use the interpolating formula after taking the derivative to interpolate between the postures in S404 and as well as and . The interpolating formula is as follows: （16） S409. Convert the derivative of the unit quaternion at each sampling moment obtained in S408 into an angular velocity of the posture. The conversion formula is as follows: （17） S410. Thus, the desired pose of the end effector at each sampling moment is obtained in S407. , and the desired angular velocity of the end effector at each sampling moment is obtained in S409. , thereby obtaining the desired pose and the desired Cartesian velocity of the end effector. Step 5: Use the high-order polynomial planning method to plan the desired trajectories of the two passive telescopic arm rods; Step 6: Based on the idea of velocity-level closed-loop feedback, obtain the solution of the reconstruction inverse kinematic equation; S61. Use formula (1) to calculate the actual pose of the end effector at the current moment during the reconstruction operation of the robotic arm, and subtract the actual pose from the desired pose of the end effector at the current moment planned in Step 4 to obtain the following end pose error: （18） Among them, represents the desired pose at the current moment, represents the actual pose at the current moment; S62. Use formula (4) to calculate the actual Cartesian velocity of the end effector at the current moment during the reconstruction operation of the robotic arm, and subtract the actual Cartesian velocity from the desired Cartesian velocity of the end effector at the current moment planned in Step 4 to obtain the following end Cartesian velocity error: （19） Among them, represents the desired Cartesian velocity at the current moment, represents the actual Cartesian velocity at the current moment; S63. Based on the idea of velocity-level closed-loop feedback, introduce the end pose error in S61 and the end Cartesian velocity error in S62 as feedback terms into the gradient projection method to obtain the velocity-level closed-loop formula for solving the reconstruction inverse kinematic equation: （20） Among them, and are two symmetric positive definite matrices, representing the feedback coefficient matrices of the end - pose error and the end - Cartesian velocity error respectively, represents an optimization coefficient in scalar form, represents the identity matrix, represents the avoidance of joint - limit function of the gradient vector; Step 7: Combine Steps 4, 5, and 6 to solve for the solution of the reconstruction inverse kinematics; Substitute the desired attitude trajectory of the end effector planned in Step 4 and the desired trajectories of the two passive telescopic boom rods planned in Step 5 into the method for solving the reconstructed inverse kinematics in Step 6. By adjusting the feedback coefficient matrix and as well as the optimization coefficient the end pose accuracy of the active rotary joints of the robotic arm is optimized under the premise of meeting the joint limits. The position trajectory and velocity trajectory of the active rotary joints obtained at this time are the solutions of the reconstructed inverse kinematics during the reconstruction operation of the robotic arm.

2. A solution method for the inverse kinematics of reconfiguration of a reconfigurable space manipulator according to claim 1, characterized in that: The said Step 1 includes: S11. When performing the reconstruction operation, the robotic arm altogether includes seven active rotary joints and two passive translational joints. The kinematic model of the robotic arm is established using Craig's D-H method. The base coordinate system is denoted as , and the end-effector coordinate system is denoted as . The coordinate system represents the coordinate system of the -th active rotary joint, and the coordinate system represents the coordinate system of the -th passive translational joint. represents the joint variable of the -th active rotary joint, and represents the joint variable of the -th passive translational joint; S12. According to the pose relationship between adjacent joint coordinate systems of the robotic arm, obtain the following position-level forward kinematic equation of the robotic arm: （1） Among them, represents the joint variable vector of the robotic arm, represents the pose of the end effector coordinate system in the base coordinate system expressed. represents the pose of the end effector coordinate system in the coordinate system expressed. represents the pose of the coordinate system in the coordinate system expressed. represents the pose of the coordinate system in the coordinate system expressed.

3. A solution method for the inverse kinematics of reconfiguration of a reconfigurable space manipulator according to claim 1, characterized in that: in the S12 and The calculation formula is as follows: （2） （3） Among them, represents the sine function , represents the cosine function , and represent the connecting rod length, and represent the connecting rod rotation angle, represents the connecting rod offset.

4. A method for solving the inverse kinematics of reconfiguration of a reconfigurable space manipulator according to claim 1, characterized in that: The gradient vector in the S63 formula (20) has the following specific expression: （21） Gradient vector The formula for the line is: （22） Among them, and respectively represent the upper and lower limits of the joint limit of the No. active rotating joint.

Images