Integrated torso height constraint three-arm serial space robot inverse kinematics solving method

Abstract

A method for solving the inverse kinematics of a three-armed serial space robot with integrated torso height constraints is disclosed, relating to the field of space robot technology. The process is as follows: establishing a geometry-based kinematic model of the three-armed serial space robot; calculating the end-effector pose of the middle branch arm; establishing a formula for the virtual torso position; calculating the virtual torso position based on the input torso height constraint; calculating the virtual torso pose; converting the input desired pose into the desired pose under a single arm; and calling a single-branch arm inverse kinematics solver to calculate and integrate the joint angles. This method transforms the torso height-constrained problem into an unconstrained single-arm inverse kinematics problem, reducing the dimensionality of the solution and enabling parallel computation, which improves the real-time performance of inverse kinematics calculations, reduces disturbances during robot movement, and enhances safety.

Description

Technical Field

[0001] This invention relates to the field of space robot technology, specifically a method for solving the inverse kinematics of a three-armed serial space robot with integrated torso height constraints. Background Technology

[0002] Reconfigurable multi-branch space robots can achieve a configuration with three branch arms and a torso connected in series through configuration transformation, enabling them to capture or transport long-distance space payloads. While the long arm span provides a larger workspace during mission execution, it also increases the risk of collisions and disturbances to the space platform.

[0003] By controlling the trunk height and the height of the branch arm connections in a three-arm tandem configuration, collisions between the robot and the space platform can be avoided, while simultaneously increasing the stability of the space platform during robot movement. Therefore, for three-arm tandem space robots, there is an urgent need for an inverse kinematics solution method that integrates trunk height constraints to improve the safety and reliability of space robots performing on-orbit tasks. Summary of the Invention

[0004] To address the shortcomings of the prior art, this invention provides a method for solving the inverse kinematics of a three-armed serial space robot with integrated torso height constraints. This method transforms the torso height constraint problem into an unconstrained single-arm inverse kinematics problem, reducing the dimensionality of the problem and enabling parallel computation. This improves the real-time performance of inverse kinematics calculations, reduces disturbances during robot movement, and enhances safety.

[0005] To achieve the above objectives, the present invention adopts the following technical solution: a method for solving the inverse kinematics of a three-armed serial space robot with integrated torso height constraints, comprising the following steps:

[0006] Step 1: Establish a geometry-based kinematic model of the three-armed serial space robot

[0007] Define a world coordinate system {O}. Establish a virtual torso coordinate system at the geometric center of the torso. The three branch arms are designated as arma, armb, and armc, with armb serving as the intermediate branch arm connecting armc to the torso. Establish base and end coordinate systems for the three branch arms. The poses of the end coordinate systems of arma, armb, and armc, as well as the virtual torso coordinate system, under the world coordinate system {O} are as follows: as well as Represented as: in, and Let x, y, and z represent the x, y, and z axis vectors of the end coordinate system of Arma, respectively. Represents the position vector of the end coordinate system of ARMA. and Let x, y, and z represent the x, y, and z axis vectors of the end coordinate system of armb, respectively. This represents the position vector of the end coordinate system of armb. and Let x, y, and z represent the x, y, and z axis vectors of the end coordinate system of armc, respectively. This represents the position vector of the end coordinate system of armc. and Let x, y, and z represent the vectors of the virtual torso coordinate system, respectively. Represents the position vector of the virtual torso coordinate system;

[0008] Step 2: Calculate the end pose of the middle branch arm.

[0009] Take a point P on the line connecting the ends of arma and armc. ac , By P ac point to vector Where, n pac for and The normal vector of the plane formed, n ac Let be the vector connecting the ends of arma and armc. in, for The unit vector, where α and β take values ​​in the range [0,1], h abs Given the height constraint of the torso relative to the space mechanism platform, let That will confirm

[0010] Step 3: Establish the virtual position formula for the torso

[0011] Make the geometric center of the torso located at the midpoint P of the line connecting the ends of arma and armb. mide Directly above, define n ab Let n be the vector connecting the ends of arma and armb. p for and If the plane normal vector formed is P, then P mide point to vector The unit vector is but Where h vtorso for Relative to the height of the connection between the ends of arma and armb, Composed of in, and They represent Position on the x, y, and z axes;

[0012] Step 4: Calculate the virtual position of the torso based on the input torso constraint height.

[0013] The input torso height constraint is: arma as a fixed arm, and P mide In the coordinate system at the end of the fixed arm, v is defined as... fixedee and in, and Let v represent the coordinates of the end of the fixed arm respectively. fixedee Positions along the x, y, and z axes, and Let P represent the coordinates of the end of the fixed arm respectively. mide The positions along the x, y, and z axes are derived as follows:

[0014] Step 5: Calculate the virtual posture of the torso

[0015] Suppose that three interfaces, A, B, and C, are installed on the side of the robot's torso for mounting the branch arms. Interface A is located at the x-axis position in the virtual torso coordinate system. Interfaces B and C are evenly distributed around the geometric center of the torso at 120° intervals. The value of β is determined based on the mounting positions of arma and armb. Let... For n p Let the unit vector be used to determine the x-axis of the virtual torso coordinate system using the Rodrigues rotation formula. Around Rotate β to obtain Then, the z-axis is determined based on the x-axis and y-axis of the virtual torso coordinate system;

[0016] Step 6: Convert the input desired pose into the desired pose under single-arm conditions.

[0017] The poses of the base coordinate systems of arma, armb, and armc in world coordinate system {O} are respectively and in and The positions overlap;

[0018] Let the base coordinate system of Arma be represented in the virtual torso coordinate system as follows: but The target pose of the arma end effector is: Then the pose of the ARMA endpoint in its base coordinate system

[0019] Let the base coordinate system of armb be represented in the virtual torso coordinate system as follows: but The target pose of the armb end effector is: Then the pose of the armb end in its base coordinate system

[0020] Let the base coordinate system of armc be represented in the end coordinate system of armb as follows: but The target pose of the armc end effector is: Then the pose of the armc end effector in its base coordinate system

[0021] Step 7: Call the single-branch arm inverse kinematics solver to calculate joint angles and integrate them.

[0022] Will and Substitute the values ​​into the inverse kinematics solver for each branch arm, and use a parallel computing strategy to calculate and output the joint angles of the three branch arms respectively. Integrate the joint angles of the three branch arms to obtain the inverse kinematics solution of the three-arm serial space robot that meets the specified torso height constraint requirements.

[0023] Furthermore, in step one, the x-axis of the base coordinate system is perpendicular to the side of the torso and points to the geometric center of the torso; the z-axis is parallel to the edge of the side of the torso and points in the same counterclockwise direction around the geometric center of the torso; the y-axis is determined according to the right-hand coordinate definition. The x-axis of the end effector coordinate system is located on the extension line and points to the outside of the end of the branch arm; the z-axis is perpendicular to the gripper tool plane of the end effector; the y-axis is determined according to the right-hand coordinate definition. The x-axis of the virtual torso coordinate system points to the base of ARMA; the y-axis is determined according to the cross product of the x-axis of the base coordinate systems of ARMA and ARMB; the z-axis is determined according to the right-hand coordinate definition.

[0024] Furthermore, in step five, when arma and armb are installed on interface A and interface B respectively, β = -120° is taken; when arma and armb are installed on interface B and interface C respectively, β = 0° is taken; and when arma and armb are installed on interface C and interface A respectively, β = 120° is taken.

[0025] Furthermore, in step six... and It is a constant that can be obtained through measurement.

[0026] Furthermore, in step seven, the inverse kinematics solver for the single branch arm is a Levenberg-Marquardt, Broyden-Fletcher-Goldfarb-Shanno, or a solver based on analytical methods.

[0027] Compared with the prior art, the beneficial effects of the present invention are:

[0028] 1. The method of the present invention enables the three-armed serial space robot to apply height constraints to the torso at the inverse kinematics level when performing tasks. At the same time, it can constrain the height of the connection between the middle and end branch arms of the serial connection, so that the robot always maintains a safe distance from the space platform. It can also reduce the disturbance to the space platform during the robot's movement, effectively ensuring the safety of the three-armed serial space robot when capturing or transporting space loads.

[0029] 2. The method of this invention transforms the inverse kinematics problem of a three-armed serial space robot with torso height constraints into an unconstrained single-arm inverse kinematics problem, reducing the dimensionality of the problem solution. By adopting a modular inverse kinematics solution approach, the three serial branch arms can be split into single-arm parallel solution operations. Parallel computing strategies can be used to simultaneously calculate the inverse kinematics solutions of the branch arms, making the method of this invention both versatile and able to guarantee the real-time performance of inverse kinematics calculation. Attached Figure Description

[0030] Figure 1 This is a flowchart of the method of the present invention;

[0031] Figure 2 This is a schematic diagram of the kinematic model of the three-armed serial space robot in the method of this invention;

[0032] Figure 3 This is a schematic diagram of the torso interface distribution of the three-armed serial space robot in the method of this invention. Detailed Implementation

[0033] The technical solutions of the present invention will be clearly and completely described below with reference to the accompanying drawings of the embodiments of the present invention. Obviously, the described embodiments are only some embodiments of the invention, not all embodiments. All other embodiments obtained by those skilled in the art based on the embodiments of the present invention without creative effort are within the scope of protection of the present invention.

[0034] like Figures 1-3 As shown, a method for solving the inverse kinematics of a three-armed serial space robot with integrated torso height constraints is presented, with the specific process combined with... Figure 1 As shown, it includes the following steps:

[0035] Step 1: Establish a geometry-based kinematic model of the three-armed serial space robot

[0036] The three-armed serial space robot consists of a torso and three branch arms, based on a geometric approach and combining... Figure 2 As shown, let {O} be the world coordinate system, and the three branch arms are designated as arma, armb and armc respectively. Among them, armb and armc are connected in series, armb is the middle branch arm connecting armc and the torso, and armc is the end branch arm.

[0037] Establish end-effector coordinate systems for the three branch arms at their respective end effectors, away from the torso. The x-axis lies on its extension line pointing outwards from the end of the branch arm; the z-axis is perpendicular to the gripper plane of the end effector; and the y-axis is determined according to the right-hand coordinate system definition. The end-effector poses of arma, armb, and armc in the world coordinate system {O} are respectively represented as follows: and

[0038] Establish base coordinate systems at the connection points of the three branch arms and the torso. The x-axis is perpendicular to the torso's side surface and points towards the torso's geometric center; the z-axis is parallel to the torso's side surface edge and points counterclockwise around the torso's geometric center; the y-axis is determined according to the right-hand coordinate system definition. The poses of the base coordinate systems arma, armb, and armc in the world coordinate system {O} are respectively represented as follows: and in, pose of the end coordinate system of armb Their positions overlap, their x-axis and y-axis directions are opposite, and their z-axis directions coincide.

[0039] A virtual torso coordinate system is established at the geometric center of the torso, where the x-axis points to the base of arma, the y-axis is determined according to the cross product of the x-axis of the base coordinate systems of arma and armb, and the z-axis is determined according to the right-hand coordinate definition. The pose of the virtual torso coordinate system under the world coordinate system {O} is represented as follows: It refers to the pose representation after the actual torso pose is offset to the center plane of the torso.

[0040] but and It is expressed as follows:

[0041]

[0042]

[0043] In the formula, and Let x, y, and z be the vectors of the terminal coordinate system of ARMA in the world coordinate system {O}, respectively. This represents the position vector of the terminal coordinate system of ARMA in the world coordinate system {O}. and Let x, y, and z be the x, y, and z axis vectors of the end coordinate system of armb in the world coordinate system {O}, respectively. This represents the position vector of the end coordinate system of armb in the world coordinate system {O}. and Let x, y, and z be the x, y, and z axis vectors of the end coordinate system of armc in the world coordinate system {O}, respectively. This represents the position vector of the end coordinate system of armc in the world coordinate system {O}. and Let x, y, and z be the vectors of the virtual torso coordinate system under the world coordinate system {O}, respectively. This represents the position vector of the virtual torso coordinate system under the world coordinate system {O}.

[0044] Step 2: Calculate the end pose of the middle branch arm.

[0045] Take a point P on the line connecting the ends of arma and armc. ac Defined as:

[0046]

[0047] In the formula, α takes values ​​in the range of [0,1], and the specific value is determined according to the task scenario.

[0048] Define n pac for and The normal vector of the plane formed, n ac Let the vector connecting the ends of arma and armc be:

[0049]

[0050] Then by P ac point to vector It is expressed as follows:

[0051]

[0052] definition for If the unit vector is , then Represented as:

[0053]

[0054] In the formula, β ranges from [0,1], and its specific value is determined according to the task scenario. abs Given the height constraint of the torso relative to the space mechanism platform.

[0055] make and They are respectively:

[0056]

[0057] because Based on the definition of the right-handed coordinate system, the end pose of the middle branch arm can be determined.

[0058] Step 3: Establish the virtual position formula for the torso

[0059] Define n ab P is the vector connecting the ends of arma and armb. mide Let P be the midpoint of the line connecting the ends of arma and armb, so that the geometric center of the torso of the three-armed serial space robot is located at P. mide Directly above:

[0060]

[0061] Let n p for and If the plane normal vectors formed are then:

[0062]

[0063] By P mide point to vector It is expressed as follows:

[0064]

[0065] definition for If the unit vector is , then It can be represented as:

[0066]

[0067] In the formula, h vtorso for Relative to the height of the connection between the ends of arma and armb, Composition is as follows:

[0068]

[0069] In the formula, and Representing the world coordinate system {O} Position on the x, y, and z axes.

[0070] Step 4: Calculate the virtual position of the torso based on the input torso constraint height.

[0071] The input torso height constraint is h. abs , will h abs This is represented as the value in the negative x-axis direction of the coordinate system at the end of the fixed arm. In the three-arm cascade configuration, the end of the arm is connected to the space platform. The arm is the fixed arm, and the pose of the end coordinate system of the fixed arm is set to... make Let the pose of the world coordinate system {O} in the end-effector coordinate system of the fixed arm be represented. Then:

[0072]

[0073] Will In the coordinate system at the end of the fixed arm, it is defined as v fixedee ,but:

[0074]

[0075] In the formula, and Let v represent the coordinates of the end of the fixed arm respectively. fixedee Position on the x, y, and z axes.

[0076] Further P mide This also means that in the coordinate system at the end of the fixed arm, it is defined as... but:

[0077]

[0078] In the formula, and Let P represent the coordinates of the end of the fixed arm respectively. mide Position on the x, y, and z axes.

[0079] According to the following formula:

[0080]

[0081] It can be found that:

[0082]

[0083] In the formula, To represent the coordinate system at the end of the fixed arm

[0084] Step 5: Calculate the virtual posture of the torso

[0085] Assume a three-armed serial space robot has three interfaces, A, B, and C, mounted on the side of its torso for mounting the branch arms. Figure 3As shown, interface A is located at the x-axis position of the virtual torso coordinate system. Interfaces B and C are evenly distributed around the geometric center of the torso at 120° intervals. The value of β is determined according to the installation positions of arma and armb. When arma and armb are installed on interface A and interface B respectively, β = -120° is taken; when arma and armb are installed on interface B and interface C respectively, β = 0° is taken; and when arma and armb are installed on interface C and interface A respectively, β = 120° is taken.

[0086] make For n p Let the unit vector be used to determine the x-axis of the virtual torso coordinate system using the Rodrigues rotation formula. Around Rotate β to obtain but:

[0087]

[0088] Then, based on the x-axis and y-axis of the virtual torso coordinate system, the z-axis is determined as follows:

[0089]

[0090] in, and These represent the x, y, and z axis vectors of the virtual torso coordinate system under the world coordinate system {O}.

[0091] Step 6: Convert the input desired pose into the desired pose under single-arm conditions.

[0092] Let the base coordinate system of Arma be represented in the virtual torso coordinate system as follows: The target pose of the arma end effector is: The pose of the ARMA endpoint in its base coordinate system can be determined. It is expressed as follows:

[0093]

[0094] Let the base coordinate system of armb be represented in the virtual torso coordinate system as follows: The target pose of the armb end effector is: The pose of the armb end effector in its base coordinate system can be determined. It is expressed as follows:

[0095]

[0096] Let the base coordinate system of armc be represented in the end coordinate system of armb as follows: The target pose of the armc end effector is: The pose of the armc end effector in its base coordinate system can be determined. It is expressed as follows:

[0097]

[0098]

[0099] in, and Both are constants and can be obtained through measurement.

[0100] Step 7: Call the single-branch arm inverse kinematics solver to calculate joint angles and integrate them.

[0101] Will and The inverse kinematics solver for each of the three branches is substituted into the Levenberg-Marquardt single-branch arm inverse kinematics solver, and a parallel computing strategy is employed for synchronous computation. This fully utilizes the computing performance of the current hardware platform, compressing the computation time cost of the inverse kinematics of the three branches to a level comparable to that of a single branch arm. The single-branch arm inverse kinematics solver is embedded into the method of this invention in a modular manner, and can be replaced with different solvers (such as the Broyden-Fletcher-Goldfarb-Shanno solver or a solver based on analytical methods) depending on the working scenario and branch arm configuration. The single-branch arm inverse kinematics solver will output the joint angles of the three branches separately. According to the task requirements, the joint angles of the three branches are integrated into the data format specified by the selected solver, thus obtaining the inverse kinematics solution of the three-arm serial space robot that meets the specified torso height constraint requirements.

[0102] It will be apparent to those skilled in the art that the present invention is not limited to the details of the exemplary embodiments described above, and that the invention can be implemented in other forms without departing from its spirit or essential characteristics. Therefore, the embodiments should be considered illustrative and non-limiting in all respects, and the scope of the invention is defined by the appended claims rather than the foregoing description. Thus, all variations falling within the meaning and scope of the equivalents of the claims are intended to be included within the present invention. No reference numerals in the claims should be construed as limiting the scope of the claims.

[0103] Furthermore, it should be understood that although this specification describes embodiments, not every embodiment contains only one independent technical solution. This narrative style is merely for clarity. Those skilled in the art should consider 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 method for solving the inverse kinematics of a three-armed serial space robot with integrated torso height constraints, characterized in that: Includes the following steps: Step 1: Establish a geometry-based kinematic model of the three-armed serial space robot Define the world coordinate system A virtual torso coordinate system is established at the geometric center of the torso. The three branch arms are designated as arma, armb, and armc, with armb serving as the intermediate branch arm connected to armc in series with the torso. A base coordinate system and an end coordinate system are established for each of the three branch arms, along with a world coordinate system. The poses of the end-effector coordinate systems (ARMA, ARMB, and ARMC) and the virtual torso coordinate system are as follows: , , as well as , is represented as: , , , ,in, , and Let x, y, and z represent the x, y, and z axis vectors of the end coordinate system of Arma, respectively. Represents the position vector of the end coordinate system of ARMA. , and Let x, y, and z represent the x, y, and z axis vectors of the end coordinate system of armb, respectively. This represents the position vector of the end coordinate system of armb. , and Let x, y, and z represent the x, y, and z axis vectors of the end coordinate system of armc, respectively. This represents the position vector of the end coordinate system of armc. , and Let x, y, and z represent the vectors of the virtual torso coordinate system, respectively. Represents the position vector of the virtual torso coordinate system; Step 2: Calculate the end pose of the middle branch arm. Take a point on the line connecting arma and armc. , ,Depend on point to vector ,in, for , and The normal vector of the plane formed. Let be the vector connecting the ends of arma and armc. ,in, for The unit vectors, α and β, take values ​​in the range [0,1]. Given the height constraint of the torso relative to the space mechanism platform, let , That will confirm ; Step 3: Establish the virtual position formula for the torso Make the geometric center of the torso located at the midpoint of the line connecting the ends of arma and armb. Directly above, definition This is the vector connecting the ends of arma and armb. for , and The plane normal vector formed by it, then point to vector , The unit vector is ,but ,in for Relative to the height of the connection between the ends of arma and armb, Composed of ,in, , and They represent Position on the x, y, and z axes; Step 4: Calculate the virtual position of the torso based on the input torso constraint height. The input torso height constraint is: Arma serves as a fixed arm, and In the coordinate system at the end of the fixed arm, they are respectively defined as... and , , ,in, , and Representing the coordinate system of the end of the fixed arm respectively Positions along the x, y, and z axes, , and Representing the coordinate system of the end of the fixed arm respectively The positions along the x, y, and z axes are derived as follows: ; Step 5: Calculate the virtual posture of the torso Suppose that three interfaces, A, B, and C, are installed on the side of the robot's torso for mounting the branch arms. Interface A is located at the x-axis position in the virtual torso coordinate system. Interfaces B and C are evenly distributed around the geometric center of the torso at 120° intervals. The value of β is determined based on the mounting positions of arma and armb. Let... , for Let the unit vector be used to determine the x-axis of the virtual torso coordinate system using the Rodrigues rotation formula. Around Rotate β to obtain Then, the z-axis is determined based on the x-axis and y-axis of the virtual torso coordinate system; Step 6: Convert the input desired pose into the desired pose under single-arm conditions. World coordinate system The poses of the base coordinate systems of arma, armb, and armc are respectively , and ,in and The positions overlap; Let the base coordinate system of Arma be represented in the virtual torso coordinate system as follows: ,but The ARMA terminal target pose is Then the pose of the ARMA end point in its base coordinate system ; Let the base coordinate system of armb be represented in the virtual torso coordinate system as follows: ,but The target pose of the armb end effector is Then the pose of the armb end in its base coordinate system ; Let the base coordinate system of armc be represented in the end coordinate system of armb as follows: ,but The target pose of the armc end effector is Then the pose of the armc end effector in its base coordinate system ; Step 7: Call the single-branch arm inverse kinematics solver to calculate joint angles and integrate them. Will , and Substitute the values ​​into the inverse kinematics solver for each branch arm, and use a parallel computing strategy to calculate and output the joint angles of the three branch arms respectively. Integrate the joint angles of the three branch arms to obtain the inverse kinematics solution of the three-arm serial space robot that meets the specified torso height constraint requirements.

2. The method for solving the inverse kinematics of a three-armed serial space robot with integrated torso height constraints according to claim 1, characterized in that: In step one, the x-axis of the base coordinate system is perpendicular to the side of the torso and points to the geometric center of the torso; the z-axis is parallel to the edge of the side of the torso and points in the same counterclockwise direction around the geometric center of the torso; the y-axis is determined according to the right-hand coordinate definition. The x-axis of the end effector coordinate system is located on the extension line and points to the outside of the end of the branch arm; the z-axis is perpendicular to the gripper tool plane of the end effector; the y-axis is determined according to the right-hand coordinate definition. The x-axis of the virtual torso coordinate system points to the base of ARMA; the y-axis is determined according to the cross product of the x-axis of the base coordinate systems of ARMA and ARMB; the z-axis is determined according to the right-hand coordinate definition.

3. The method for solving the inverse kinematics of a three-armed serial space robot with integrated torso height constraints according to claim 1, characterized in that: In step five, β = -120° is taken when arma and armb are installed on interface A and interface B respectively, β = 0° is taken when arma and armb are installed on interface B and interface C respectively, and β = 120° is taken when arma and armb are installed on interface C and interface A respectively.

4. The method for solving the inverse kinematics of a three-armed serial space robot with integrated torso height constraints according to claim 1, characterized in that: In step six , and It is a constant that can be obtained through measurement.

5. The method for solving the inverse kinematics of a three-armed serial space robot with integrated torso height constraints according to claim 1, characterized in that: In step seven, the inverse kinematics solver for the single branch arm can be Levenberg-Marquardt, Broyden-Fletcher-Goldfarb-Shanno, or a solver based on analytical methods.

Images