Inverse kinematics solving method for three-arm series space robot integrating trunk height constraint

Abstract

The invention discloses an inverse kinematics solving method for a three-arm series space robot integrating trunk height constraints, and relates to the technical field of space robots. The method comprises the following steps: establishing a geometry-based kinematics model of the three-arm series space robot, calculating the tail end pose of a branch arm of a middle section, establishing a trunk virtual position formula, calculating a trunk virtual position according to an input trunk constraint height, calculating a trunk virtual pose, and converting an input expected pose into an expected pose under a single arm. And calling a single branch arm inverse kinematics solver to calculate joint angles and performing integration. A trunk height constraint problem is converted into an unconstrained single-arm inverse kinematics problem, the problem solving dimension is reduced, parallel calculation can be carried out, the real-time performance of inverse kinematics calculation is facilitated, disturbance in the movement period of the robot is reduced, and safety is improved.

Description

Technical Field

[0001] The present invention relates to the technical field of space robots, and specifically to an inverse kinematics solution method for a three-arm series space robot with integrated torso height constraints. Background Art

[0002] The reconfigurable multi-branch space robot can achieve the configuration of three branch arms and a series-connected torso through configuration transformation, and can be used to capture or carry space payloads at a long distance. During the execution of tasks, although the long arm span brings a larger working space, it also increases the risk of collision and the disturbance to the space platform.

[0003] By controlling the torso height and the height at the connection of the branch arms in the three-arm series configuration, it helps to avoid the collision between the robot and the space platform, and at the same time increases the stability of the space platform during the movement of the robot. Therefore, for the space robot in the three-arm series configuration, there is an urgent need for an inverse kinematics solution method with integrated torso height constraints to improve the safety and reliability of the space robot when performing on-orbit tasks. Summary of the Invention

[0004] To solve the deficiencies in the background art, the present invention provides an inverse kinematics solution method for a three-arm series space robot with integrated torso height constraints, which converts the torso height constraint problem into an unconstrained single-arm inverse kinematics problem, reduces the problem-solving dimension and can be calculated in parallel, helps the real-time performance of inverse kinematics calculation, reduces the disturbance during the movement of the robot, and improves safety.

[0005] To achieve the above purpose, the present invention adopts the following technical solutions: An inverse kinematics solution method for a three-arm series space robot with integrated torso height constraints, comprising the following steps:

[0006] Step 1: Establish a geometric-based kinematic model of the three-arm series space robot

[0007] Define the world coordinate system {O}, establish a virtual torso coordinate system at the geometric center of the torso, and the three branch arms are respectively designated as arma, armb, and armc, where armb is the middle-segment branch arm connecting armc and the torso. Establish the base coordinate system and the end coordinate system of the three branch arms. The poses of the end coordinate systems of arma, armb, and armc and the virtual torso coordinate system under the world coordinate system {O} are respectively and Expressed as: Among them, and respectively represent the x, y, and z-axis vectors of the end coordinate system of arma, The position vector representing the end coordinate system of arma, and respectively represent the x, y, and z-axis vectors of the end coordinate system of armb, The position vector representing the end coordinate system of armb, and respectively represent the x, y, and z-axis vectors of the end coordinate system of armi, The position vector representing the end coordinate system of armi, and respectively represent the x, y, and z-axis vectors of the virtual torso coordinate system, The position vector representing the virtual torso coordinate system;

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

[0009] Take a point P on the line connecting the ends of arma and armi ac , The vector from P ac pointing to is where n pac is and constitute the normal vector of the plane, and n ac is the vector connecting the ends of arma and armi, then where is 's unit vector, and the value ranges of α and β are [0,1], and h abs is the given height constraint of the torso relative to the spatial mechanism platform. Let to determine

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

[0011] Make the geometric center point of the torso located directly above the midpoint P of the line connecting the ends of arma and armb mide Define n ab as the vector connecting the ends of arma and armb, and n p is and constitute the normal vector of the plane, then the vector from P mide pointing to is 's unit vector is Then where h vtorso is 's height relative to the line connecting the ends of arma and armb, is composed of Among them, and respectively represent the positions on the x, y, and z axes;

[0012] Step 4: Calculate the virtual position of the torso according to the input torso constraint height

[0013] The input torso height constraint is Using arma as the fixed arm, and P mide are respectively defined as v in the end - coordinate system of the fixed arm fixedee and Among them, and respectively represent the positions of v fixedee on the x, y, and z axes in the end - coordinate system of the fixed arm, and respectively represent the positions of P mide on the x, y, and z axes in the end - coordinate system of the fixed arm, and it is deduced that

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

[0015] Suppose there are three interfaces A, B, and C installed on the side of the robot's torso for the installation of the branch arms. Interface A is located at the position pointed by the x - axis of the virtual torso coordinate system. Interfaces B and C are evenly distributed around the geometric center of the torso at 120°. Determine the β value according to the installation positions of arma and armb. Let be the unit vector of n p Use the Rodrigues rotation formula to determine the x - axis of the virtual torso coordinate system. Let rotate around by β to obtain Furthermore, determine the z - axis according to the x - axis and y - axis of the virtual torso coordinate system;

[0016] Step 6: Convert the input desired pose to the desired pose under a single arm

[0017] The poses of the base coordinate systems of arma, armb, and armc in the world coordinate system {O} are respectively and Among them coincides with in position;

[0018] Let the representation of the base coordinate system of arma in the virtual torso coordinate system be Then The end - target pose of arma is Then the pose of the arma end-effector in its base coordinate system

[0019] Let the representation of the base coordinate system of the armb in the virtual torso coordinate system be Then The target pose of the armb end-effector is Then the pose of the armb end-effector in its base coordinate system

[0020] Let the representation of the base coordinate system of the armc in the armb end-effector coordinate system be Then 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: Invoke the single-branch arm inverse kinematics solver to calculate the joint angles and integrate them

[0022] Substitute and into the single-branch arm inverse kinematics solver respectively, adopt a parallel computing strategy to perform calculations synchronously and output the joint angles of the three branch arms respectively. Integrate the joint angles of the three branch arms to obtain the inverse kinematic solution of the three-arm serial spatial robot that meets the specified torso height constraint requirements.

[0023] Furthermore, in the above Step 1, the x-axis of the base coordinate system is perpendicular to the torso side and points to the geometric center of the torso, the z-axis is parallel to the torso side edge and points in the same direction as the counterclockwise direction around the geometric center of the torso, and 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 branch arm end, the z-axis is perpendicular to the gripper tool plane of the end-effector, and 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 the arma, the y-axis is determined according to the cross product of the x-axes of the base coordinate systems of the arma and the armb, and the z-axis is determined according to the right-hand coordinate definition.

[0024] Furthermore, in the above Step 5, when the arma and the armb are installed at interfaces A and B respectively, take β = -120°, when the arma and the armb are installed at interfaces B and C respectively, take β = 0°, and when the arma and the armb are installed at interfaces C and A respectively, take β = 120°.

[0025] Furthermore, in the above Step 6 and are constants that can be obtained through measurement.

[0026] Further, in Step 7, the single-branch-arm inverse kinematics solver adopts Levenberg-Marquardt, Broyden-Fletcher-Goldfarb-Shanno, or a solver based on an analytical method.

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

[0028] 1. When the three-arm serial space robot executes tasks by the method of the present invention, a high constraint can be imposed on the torso at the inverse kinematics level. Meanwhile, the height at the connection of the middle-segment branch arm and the end-segment branch arm in series can be constrained, so that the robot can always maintain a safe distance from the space platform. At the same time, the disturbance brought to the space platform during the movement of the robot can be reduced, effectively ensuring the safety of the three-arm serial space robot when capturing or carrying a space load.

[0029] 2. The method of the present invention converts the inverse kinematics problem of a three-arm serial space robot with a torso height constraint into an unconstrained single-arm inverse kinematics problem, reducing the problem-solving dimension. Through the modular inverse kinematics solution idea, the three series-connected branch arms can be split into single-arm parallel solution operations, and the inverse kinematics solutions of the branch arms can be calculated simultaneously using a parallel computing strategy, making the method of the present invention have both good versatility and can ensure the real-time performance of inverse kinematics calculation. BRIEF DESCRIPTION OF THE DRAWINGS

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

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

[0032] Figure 3 is a schematic diagram of the torso interface distribution of the three-arm serial space robot in the method of the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0033] Next, the technical solutions in 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.

[0034] As Figures 1 to 3 shown, an inverse kinematics solution method for a three-arm serial space robot integrating torso height constraint, the specific process is combined with Figure 1 shown, and includes the following steps:

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

[0036] The three-arm series spatial robot includes a torso and three branch arms. Based on geometric methods and combined with Figure 2 As shown, let {O} be the world coordinate system. The three branch arms are respectively designated as arma, armb, and armc, where armb and armc are in series. Armb serves as the intermediate branch arm connecting armc to the torso, and armc serves as the end-segment branch arm.

[0037] At the end effectors of the three branch arms far from the torso, end coordinate systems are respectively established. Among them, the x-axis is located on the extension line and points to the outside of the branch arm end, the z-axis is perpendicular to the gripper tool plane of the end effector, and the y-axis is determined according to the right-hand coordinate definition. The pose of the end coordinate systems of arma, armb, and armc in the world coordinate system {O} are respectively expressed as and

[0038] At the connection positions of the three branch arms to the torso, base coordinate systems are respectively established. Among them, the x-axis is perpendicular to the side of the torso and points to the geometric center point of the torso, the z-axis is parallel to the edge of the side of the torso and points in the same direction as the counterclockwise direction around the geometric center of the torso, and the y-axis is determined according to the right-hand coordinate definition. The pose of the base coordinate systems of arma, armb, and armc in the world coordinate system {O} are respectively expressed as and Among them, coincides with the pose of the end coordinate system of armb in position. The x-axis and y-axis directions of the two are opposite, and the z-axis directions coincide.

[0039] A virtual torso coordinate system is established at the geometric center of the torso. Among them, the x-axis points to the base of arma, the y-axis is determined according to the cross product definition 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 in the world coordinate system {O} is expressed as It refers to the pose expression after offsetting the actual torso pose to the torso center plane.

[0040] Then and are expressed as follows:

[0041]

[0042]

[0043] In the formula, and respectively represent the x, y, and z-axis vectors of the end coordinate system of arma in the world coordinate system {O}, represents the position vector of the end coordinate system of arma in the world coordinate system {O}, and respectively represent the x, y, and z-axis vectors of the end coordinate system of armb in the world coordinate system {O}, and represents the position vector of the end coordinate system of armb in the world coordinate system {O}, and respectively represent the x, y, and z-axis vectors of the end coordinate system of armp in the world coordinate system {O}, and represents the position vector of the end coordinate system of armp in the world coordinate system {O}, and

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

[0045] Take a point P on the line connecting the ends of arma and armp ac , defined as:

[0046]

[0047] where α ranges from [0, 1], and the specific value is determined according to the task scenario.

[0048] Define n pac as and the normal vector of the plane formed by them, and n ac is the vector connecting the ends of arma and armp, then:

[0049]

[0050] Then the vector ac pointing from P to is expressed as:

[0051]

[0052] Define as the unit vector of, then is expressed as:

[0053]

[0054] where β ranges from [0, 1], and the specific value is determined according to the task scenario, and h abs is the given height constraint of the torso relative to the spatial mechanism platform.

[0055] Let and They are:

[0056]

[0057] because According to the right-hand coordinate system, the end position of the middle branch arm can be determined

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

[0059] Definition ab is the terminal connection vector of arma and armb, P mide is the midpoint of the line connecting the ends of arma and armb, so that the geometric center point of the torso of the three-arm tandem space robot is located at P mide Directly above, then:

[0060]

[0061] Let n p for and The plane normal vector is:

[0062]

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

[0064]

[0065] definition for The unit vector of Can be expressed as:

[0066]

[0067] In the formula, h vtorso for Relative to the height of the line connecting the ends of arma and armb, Composition:

[0068]

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

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

[0071] The input torso height is constrained to h abs , and h abs is expressed as the value in the negative x-axis direction of the end coordinate system of the fixed arm, Under the three-arm serial configuration, the end of arma is connected to the space platform. Arma is the fixed arm. The pose of the end coordinate system of the fixed arm is taken as Let represent the pose of the world coordinate system {O} in the end coordinate system of the fixed arm, then:

[0072]

[0073] Let be expressed in the end coordinate system of the fixed arm and defined as v fixedee , then:

[0074]

[0075] In the formula, and respectively represent the positions of v fixedee on the x, y, and z axes in the end coordinate system of the fixed arm.

[0076] Furthermore, P mide is also expressed in the end coordinate system of the fixed arm and defined as then:

[0077]

[0078] In the formula, and respectively represent the positions of P mide on the x, y, and z axes in the end coordinate system of the fixed arm.

[0079] According to the following formula:

[0080]

[0081] It can be obtained that:

[0082]

[0083] In the formula, is the

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

[0085] Suppose there are three interfaces A, B, and C installed on the side of the torso of the three-arm serial space robot for the installation of the branch arms. Combining Figure 3As shown, Interface A is located at the position pointed by the x-axis of the virtual torso coordinate system. Interfaces B and C are evenly distributed at 120° intervals around the geometric center of the torso in sequence. Determine the value of β according to the installation positions of arma and armb. When arma and armb are installed at Interfaces A and B respectively, take β = -120°. When arma and armb are installed at Interfaces B and C respectively, take β = 0°. When arma and armb are installed at Interfaces C and A respectively, take β = 120°.

[0086] Let be the unit vector of n p Use the Rodrigues rotation formula to determine the x-axis of the virtual torso coordinate system. Let rotate around by β to obtain Then:

[0087]

[0088] Furthermore, determine the z-axis according to the x-axis and y-axis of the virtual torso coordinate system. We have:

[0089]

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

[0091] Step Six: Convert the input desired pose to the desired pose under a single arm

[0092] Let the representation of the base coordinate system of arma in the virtual torso coordinate system be The end target pose of arma is The pose of the end of arma in its base coordinate system can be obtained It is expressed as follows:

[0093]

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

[0095]

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

[0097]

[0098]

[0099] Among them, and are both constants and can be obtained through measurement.

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

[0101] Substitute and into the Levenberg-Marquardt inverse kinematics solver of the single-branch arm respectively, and adopt a parallel operation strategy to perform calculations synchronously, making full use of the computing performance of the current hardware platform to compress the time cost of the inverse kinematics calculation of the three-branch arms to the same level as that of the single-branch arm. The inverse kinematics solver of the single-branch arm is embedded in the method of the present invention in a modular manner and can be replaced with different solvers according to the working scenario and the configuration of the branch arm (such as the Broyden-Fletcher-Goldfarb-Shanno solver or the solver based on the analytical method). The inverse kinematics solver of the single-branch arm will output the joint angles of the three branch arms respectively, and integrate the joint angles of the three branch arms into the data format specified by the selected solver according to the task requirements, and then the inverse kinematic solution of the three-arm serial space robot that meets the specified trunk height constraint requirements can be obtained.

[0102] For those skilled in the art, it is obvious that the present invention is not limited to the details of the above exemplary embodiments, and can be implemented in other forms without departing from the spirit or basic characteristics of the present invention. Therefore, from any point of view, 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 included in the present invention. Any reference signs in the claims should not be regarded as limiting the claims involved.

[0103] In addition, it should be understood that although this specification is described according to the embodiments, not every embodiment only contains an independent technical solution. This narrative way 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. An inverse kinematics solution method for a three-arm serial space robot integrating torso height constraint, characterized in that: It includes the following steps: Step 1: Establish a geometric-based kinematic model of the three-arm series space robot Define the world coordinate system {O}, establish a virtual torso coordinate system at the geometric center of the torso, and specify the three branch arms as arma, armb, and ar mc respectively. Among them, armb is the intermediate branch arm connecting armc in series with the torso. Establish the base coordinate systems and end coordinate systems of the three branch arms. The poses of the end coordinate systems of arma, armb, and armc and the virtual torso coordinate system under the world coordinate system {O} are respectively And Expressed as: Among them, And respectively represent the x, y, and z-axis vectors of the end coordinate system of arma, represents the position vector of the end coordinate system of arma, And respectively represent the x, y, and z-axis vectors of the end coordinate system of armb, represents the position vector of the end coordinate system of armb, And respectively represent the x, y, and z-axis vectors of the end coordinate system of armc, represents the position vector of the end coordinate system of armc, And respectively represent the x, y, and z-axis vectors of the virtual torso coordinate system, represents the position vector of the virtual torso coordinate system; Step 2: Calculate the end pose of the intermediate segment branch arm Take a point P on the connecting line between the ends of arma and armc ac , The vector ac pointing from P is where n pac is and the normal vector of the plane formed by, n ac is the vector connecting the ends of arma and armc, then where is 's unit vector, the value ranges of α and β are [0, 1], h abs is the given height constraint of the torso relative to the spatial mechanism platform. Let to determine Step 3: Establish a virtual position formula for the torso Place the geometric center point of the torso at the midpoint P of the line connecting the ends of arma and armb mide Directly above, define n ab as the vector of the line connecting the ends of arma and armb, n p is and the normal vector of the plane formed by, then the vector from P mide pointing to is the unit vector of is Then where h vtorso is the height relative to the line connecting the ends of arma and armb, is composed of Among them, and respectively represent the positions on the x, y, and z axes; Step 4: Calculate the virtual position of the torso according to the input torso constraint height The input torso height is constrained to With arma as the fixed arm, and P mide Are respectively defined as v in the end - coordinate system of the fixed arm fixedee and Among them, and Respectively represent the positions of v fixedee On the x, y, and z axes in the end - coordinate system of the fixed arm, and Respectively represent the positions of P mide On the x, y, and z axes in the end - coordinate system of the fixed arm. It is derived that Step 5: Calculate the virtual attitude of the torso Suppose that three interfaces, namely A, B, and C, are installed on the side of the robot's torso for the installation of the branch arms. Interface A is located at the position pointed by the x-axis of the virtual torso coordinate system. Interfaces B and C are evenly distributed around the geometric center of the torso at intervals of 120°. Determine the value of β based on the installation positions of arma and armb. Let be n p the unit vector of. Use the Rodrigues rotation formula to determine the x-axis of the virtual torso coordinate system. Let rotate around by β to obtain Furthermore, determine the z-axis 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 a single arm The poses of the base coordinate systems of arma, armb, and arnc in the world coordinate system {O} are respectively and where coincides with in position; Let the representation of the base coordinate system of ARMA in the virtual torso coordinate system be Then The end effector pose of ARMA is Then the pose of the end effector of ARMA in its base coordinate system Let the representation of the base coordinate system of armb in the virtual torso coordinate system be Then The end target pose of armb is Then the pose of the end of armb in its base coordinate system Let the representation of the base coordinate system of armc in the end coordinate system of armb be Then The end target pose of armc is Then the pose of the end of armc in its base coordinate system Step 7: Call the inverse kinematics solver of the single-branch arm to calculate the joint angles and integrate them Substitute and into the inverse kinematics solver of the single-branch arm respectively, adopt the parallel operation strategy to perform calculations synchronously and output the joint angles of the three branch arms respectively. Integrate the joint angles of the three branch arms to obtain the inverse kinematic solution of the three-arm serial space robot that meets the specified torso height constraint requirements.

2. The inverse kinematics solution method of a three-arm serial space robot with integrated torso height constraint according to claim 1, characterized in that: In Step 1, the x-axis of the base coordinate system is perpendicular to the side of the torso and points to the geometric center point of the torso, the z-axis is parallel to the edge of the torso side and points in the same direction as the counterclockwise direction around the geometric center of the torso, and the y-axis is determined according to the right-hand coordinate definition; the x-axis of the end coordinate system is on the extension line and points to the outside of the branch arm end, the z-axis is perpendicular to the gripper tool plane of the end effector, and 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-axes of the base coordinate systems of arma and armb, and the z-axis is determined according to the right-hand coordinate definition.

3. A method for solving the inverse kinematics of a three-arm serial space robot integrated with a torso height constraint according to claim 1, characterized in that: In Step 5, when arma and armb are installed at interface A and interface B respectively, take β = -120°, when arma and armb are installed at interface B and interface C respectively, take β = 0°, and when arma and armb are installed at interface C and interface A respectively, take β = 120°.

4. A method for solving the inverse kinematics of a three-arm serial space robot with integrated torso height constraint according to claim 1, characterized in that: In the sixth step described above and are constants that can be obtained through measurement.

5. A method for solving the inverse kinematics of a three-arm serial space robot with integrated torso height constraint according to claim 1, characterized in that: In Step 7, the inverse kinematics solver of the single-branch arm adopts the Levenberg-Marquardt, Broyden-Fletcher-Goldfarb-Shanno or a solver based on an analytical method.

Images