Double-arm space robot inverse kinematics solving method integrating trunk height and attitude constraint

Abstract

The invention discloses a double-arm space robot inverse kinematics solving method integrating trunk height and attitude constraint, and relates to the technical field of space robots. The method sequentially comprises the steps that a geometry-based kinematics model of the double-arm space robot is established, a trunk virtual position formula is established, a trunk virtual pose is calculated according to trunk height and pose constraints, a mobile end expected pose is converted into expected poses under all single arms, and a single-branch-arm inverse kinematics solver is called to calculate joint angles and integrate the joint angles. A trunk height and attitude constraint problem is converted into an unconstrained two-single-arm inverse kinematics problem, the problem solving dimension is reduced, a parallel computing strategy can be adopted, when the double-arm space robot executes a task, the trunk attitude is adjusted while the trunk and a space mechanism keep a safe height, disturbance of the robot and the space mechanism is reduced, and the task execution efficiency is improved. And task field image information can be obtained more flexibly through the trunk camera.

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 dual-arm space robot integrating trunk height and attitude constraints. Background Art

[0002] When a dual-arm space robot performs tasks such as on-orbit assembly or maintenance on a large space platform, it needs to perform actions such as dual-arm handling and crawling, which will cause disturbances to the large space platform. If not controlled, it will increase the risk of performing on-orbit tasks. By controlling the height and attitude of the trunk of the dual-arm space robot, the disturbances brought to the large space platform during the movement of the robot can be reduced. In addition, by controlling the attitude of the trunk, the camera on the trunk can obtain a better field of view, thereby assisting in the successful execution of the task.

[0003] However, existing inverse kinematics methods are difficult to directly impose height and attitude constraints on the trunk. Therefore, there is an urgent need for an inverse kinematics solution method for a dual-arm space robot integrating trunk height and attitude constraints to improve the safety and reliability of the dual-arm 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 dual-arm space robot integrating trunk height and attitude constraints, which converts the trunk height and attitude constraint problem into two unconstrained single-arm inverse kinematics problems, reduces the problem-solving dimension, and can adopt a parallel computing strategy. When the dual-arm space robot performs a task, the trunk and the space mechanism adjust the trunk attitude while maintaining a safe height, reducing the disturbance between the robot and the space mechanism, and more flexibly obtaining task site image information through the trunk camera.

[0005] To achieve the above object, the present invention adopts the following technical solution: An inverse kinematics solution method for a dual-arm space robot integrating trunk height and attitude constraints, comprising the following steps:

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

[0007] Define the world coordinate system {O}, establish a virtual trunk coordinate system at the geometric center of the trunk, respectively designate the two branch arms as arma and armb, establish the base coordinate systems and end coordinate systems of the two branch arms, and the poses of the end coordinate systems of arma and armb and the virtual trunk coordinate system in the world coordinate system {O} are respectively and and Expressed as: Wherein, and respectively represent the x, y, and z axis vectors of the end coordinate system of arma, represent 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, represent the position vector of the end coordinate system of armb, and respectively represent the x, y, and z axis vectors of the virtual torso coordinate system, represent the position vector of the virtual torso coordinate system;

[0008] Step 2: Establish the virtual position formula of the torso

[0009] Make the geometric center point of the torso of the dual-arm space robot be directly above the midpoint P of the line connecting the ends of the two branch arms mide Define n ab as the vector of the line connecting the ends of the two branch arms, and n p be and constitute the normal vector of the plane, then the vector mide pointing from P to has a unit vector of Then where h vtorso is the height relative to the line connecting the ends of the two branch arms, is composed of where, and respectively represent the positions on the x, y, and z axes;

[0010] Step 3: Calculate the virtual pose of the torso according to the torso height and pose constraints

[0011] The input torso height constraint is h abs , and the input torso pose constraints are and Select arma or armb as the fixed arm, and express h abs as the value of the negative x-axis direction of the end coordinate system of the fixed arm, Express and P mide in the end coordinate system of the fixed arm and define them as v fixedee and where, 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 in the x, y, and z axes of the end coordinate system of the fixed arm. It is derived that mide and then it can be calculated that Furthermore, it can be calculated that Combined with the input torso pose constraints, we can obtain

[0012] Step 4: Convert the desired pose of the mobile end to the desired poses of each single arm

[0013] The poses of the base coordinate systems of arma and armb in the world coordinate system {O} are respectively and

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

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

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

[0017] Substitute and into the single-branch arm inverse kinematics solver respectively. Adopt a parallel operation strategy to calculate synchronously and output the joint angles of the two branch arms respectively. Integrate the joint angles of the two branch arms to obtain the inverse kinematic solution of the dual-arm space robot that meets the requirements of the specified torso height and pose constraints.

[0018] Furthermore, in the above Step 1, 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 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 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, 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.

[0019] Furthermore, in the above Step 4 and It is a constant that can be obtained through measurement.

[0020] Furthermore, in the fifth step, the single-branch arm inverse kinematics solver adopts the Levenberg-Marquardt, Broyden-Fletcher-Goldfarb-Shanno or a solver based on an analytical method.

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

[0022] 1. The method of the present invention enables the dual-arm space robot to impose height and attitude constraints on the torso at the inverse kinematics level when performing tasks, so that the torso can adjust its attitude while maintaining a specified safe height with the space mechanism, and can more efficiently achieve functions such as camera field of view adjustment, reducing the disturbance to the space mechanism during the movement of the robot, and obstacle avoidance, providing more flexible and reliable kinematic technical support for the robot to perform space tasks;

[0023] 2. The method of the present invention converts the inverse kinematics problem of the dual-arm space robot with torso height and attitude constraints into an unconstrained single-arm inverse kinematics problem, reduces the problem-solving dimension, and also provides a modular inverse kinematics solution idea, and can flexibly replace the single-arm inverse kinematics solver with existing or more matching methods according to different branch arm configurations, so that the method of the present invention can be used for dual-arm space robot platforms with different branch arm configurations;

[0024] 3. After the inverse kinematics problem of the dual-arm space robot integrating torso height and attitude constraints is decomposed into two single-arm inverse kinematics problems by the method of the present invention, a parallel computing strategy can be adopted to calculate the inverse kinematic solutions of the two branch arms simultaneously, thereby effectively ensuring the real-time performance of the inverse kinematics calculation of the dual-arm space robot. Description of the Drawings

[0025] Figure 1 It is a flow block diagram of the method of the present invention;

[0026] Figure 2 It is a schematic diagram of the kinematic model of the dual-arm space robot in the method of the present invention;

[0027] Figure 3 It is a schematic diagram of the simulation scene when the dual-arm space robot in the method of the present invention performs tasks. Detailed Embodiments

[0028] 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 work fall within the scope of protection of the present invention.

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

[0030] Step 1: Establish a geometric-based kinematic model for the dual-arm space robot

[0031] The dual-arm space robot includes a torso and two branch arms. Based on the geometric method, combined with Figure 2 shown, let {O} be the world coordinate system, and the two branch arms are respectively designated as arma and armb. Base coordinate systems are established at the connection positions of the two branch arms and the torso. 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 and armb in the world coordinate system {O} is respectively expressed as and End coordinate systems are established at the end effectors of the two branch arms far from the torso. Among them, the x-axis is 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, and the y-axis is determined according to the right-hand coordinate definition. The pose of the end coordinate systems of arma and armb in the world coordinate system {O} is respectively expressed as and 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 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. 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.

[0032] Then and are expressed as follows:

[0033]

[0034]

[0035] 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}, 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 virtual torso coordinate system in the world coordinate system {O}, represents the position vector of the virtual torso coordinate system in the world coordinate system {O}.

[0036] Step 2: Establish the virtual position formula of the torso

[0037] Define n ab as the vector connecting the ends of the two branch arms, and P mide as the midpoint of the line connecting the ends of the two branch arms, so that the geometric center point of the torso of the dual-arm space robot is located directly above P mide Then:

[0038]

[0039] Let n p be the normal vector of the plane formed by and Then:

[0040]

[0041] The vector mide pointing from Pto is expressed as follows:

[0042]

[0043] Define as the unit vector of Then can be expressed as:

[0044]

[0045] Here h vtorso is the height relative to the line connecting the ends of the two branch arms, is composed of:

[0046]

[0047] In the formula, and respectively represent the positions on the x, y, and z axes in the world coordinate system {O}

[0048] Step 3: Calculate the virtual pose of the torso according to the torso height and pose constraints

[0049] Define the input torso height constraint as h abs , which refers to the height constraint of the torso relative to the spatial mechanism platform. The input torso pose constraint is and

[0050] During the crawling process of the dual-arm space robot, the two branch arms move and fix alternately. Therefore and both may remain in a fixed state. Select arma or armb as the fixed arm, and express h abs as the value of the negative x-axis direction of the end coordinate system of the fixed arm

[0051] According to the actual crawling situation, take the pose value of the end coordinate system of the fixed arm for pose transformation, and let represent the pose of the world coordinate system {O} in the end coordinate system of the fixed arm. Then

[0052]

[0053] Express in the end coordinate system of the fixed arm and define it as v fixedee . Then

[0054]

[0055] 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

[0056] Further express P mide in the end coordinate system of the fixed arm and define it as . Then

[0057]

[0058] 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

[0059] According to the following formula

[0060] ​

[0061] It can be obtained that:

[0062]

[0063] In the formula, represents the

[0064] under the end coordinate system of the fixed arm. Then, by calculating according to formula (7), can be obtained. At the same time, combined with the input torso pose constraint and by calculating according to formula (3),

[0065] Step 4: Convert the desired pose of the mobile end to the desired poses of each single arm

[0066] Let the representation of the base coordinate system of arma in the virtual torso coordinate system be which is a constant and can be obtained by measurement. The target pose at the end of arma is The pose of the end of arma in its base coordinate system can be obtained The representation is as follows:

[0067]

[0068] Let the representation of the base coordinate system of armb in the virtual torso coordinate system be which is a constant and can be obtained by measurement. The target pose at the end of armb is The pose of the end of armb in its base coordinate system can be obtained The representation is as follows:

[0069]

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

[0071] Put and They are respectively substituted into the Levenberg-Marquardt single-branch-arm inverse kinematics solver, and a parallel operation strategy is adopted for synchronous calculation. By making full use of the computing performance of the current hardware platform, the computational time cost of the inverse kinematics of the two branch arms is compressed to a level equivalent to that of the single-branch-arm computational time cost. The single-branch-arm inverse kinematics solver is embedded in the method of the present invention in a modular manner and can be replaced with different solvers (such as the Broyden-Fletcher-Goldfarb-Shanno solver or the solver based on the analytical method) according to the working scenario and the configuration of the branch arm. The single-branch-arm inverse kinematics solver will respectively output the joint angles of the two branch arms. By integrating the joint angles of the two branch arms into the data format specified by the selected solver according to the task requirements, the inverse kinematic solution of the dual-arm space robot that meets the requirements of the specified torso height and attitude constraints can be obtained.

[0072] In summary, through the method of the present invention, when the dual-arm space robot executes tasks, height and attitude constraints can be imposed on the torso at the inverse kinematics level, simulating the state scenario of the dual-arm space robot executing tasks. Figure 3 As shown, by imposing height and attitude constraints on the torso, the torso and the space mechanism can adjust the torso attitude while maintaining the specified safe height, so as to facilitate the adjustment of the field of view of the torso camera, and also help to reduce the disturbance to the space mechanism and obstacle avoidance functions during the movement of the robot, providing more flexible and reliable kinematic technical support for the robot to execute space tasks.

[0073] 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 the present invention can be implemented in other specific forms without departing from the spirit or basic characteristics of the present invention. Therefore, in any aspect, 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.

[0074] 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 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. A method for solving inverse kinematics of a dual-arm space robot integrating trunk height and posture constraints, characterized in that: The following steps are involved: Step 1: Establish a geometry-based kinematic model of the dual-arm space robot Define the world coordinate system {O}, establish a virtual torso coordinate system at the geometric center of the torso, designate the two branch arms as arma and armb, establish the base coordinate system and end coordinate system of the two branch arms, and the poses of the end coordinate systems of arma and armb and the virtual torso coordinate system in the world coordinate system {O} are and as well as It is expressed as: in, and Represents the x, y and z axis vectors of the terminal coordinate system of arma, Represents the position vector of the end coordinate system of arma, and 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 virtual torso coordinate system, The position vector representing the virtual torso coordinate system; Step 2: Establish the formula for the virtual position of the torso Make the geometric center point of the trunk of the dual-arm space robot located at the midpoint P of the line connecting the ends of the two branch arms. mide Just above, define n ab is the connecting line vector between the two branch arms, n p for and The plane normal vector formed by P mide point to Vector The unit vector is but where h vtorso for Relative to the height of the line connecting the ends of the two branch arms, Composition in, and Respectively Position on the x, y and z axes; Step 3: Calculate the virtual pose of the torso based on the torso height and posture constraints Enter the torso height constraint as h abs , the input torso pose constraint is and Select arma or armb as the fixed arm and set h abs It is expressed as the value of the negative x-axis of the end coordinate system of the fixed arm. Will and P mide Indicates that in the end coordinate system of the fixed arm, they are defined as v fixedee and in, and They represent the end coordinate system of the fixed arm v fixedee The position of the x, y and z axes, and They represent the end coordinate system of the fixed arm P mide In terms of the x, y, and z axis positions, we can deduce Then we can calculate Combined with the input torso posture constraint, we can get Step 4: Convert the desired posture of the mobile terminal to the desired posture of each arm The poses of the base coordinate system of arma and armb in the world coordinate system {O} are and Let the base coordinate system of arma be expressed in the virtual torso coordinate system as but The target pose at the end of arma is Then the position of the arma terminal in its base coordinate system is Let the base coordinate system of armb be expressed in the virtual torso coordinate system as but The target pose at the end of armb is Then the pose of the armb end in its base coordinate system is Step 5: Call the single-branch arm inverse kinematics solver to calculate the joint angles and integrate them Will and The inverse kinematics solver of the single branch arm is substituted respectively, and the parallel computing strategy is used to synchronously calculate and output the joint angles of the two branch arms respectively. The joint angles of the two branch arms are integrated to obtain the inverse kinematic solution of the dual-arm space robot that meets the specified torso height and posture constraints.

2. The inverse kinematics solution method for a dual-arm space robot with integrated trunk height and posture constraints according to claim 1, characterized in that: In the step one, the x-axis of the base coordinate system is perpendicular to the side of the trunk and points to the geometric center of the trunk, the z-axis is parallel to the side edge of the trunk and points in the same counterclockwise direction around the geometric center of the trunk, and the y-axis is determined according to the right-hand coordinate definition; the x-axis of the end 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 clamping 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 definition of the x-axis of the base coordinate system of arma and armb, and the z-axis is determined according to the right-hand coordinate definition.

3. The inverse kinematics solution method for a dual-arm space robot with integrated trunk height and posture constraints according to claim 1, characterized in that: In step 4 and It is a constant that can be obtained through measurement.

4. The inverse kinematics solution method for a dual-arm space robot with integrated trunk height and posture constraints according to claim 1, characterized in that: In the step 5, the single-branch arm inverse kinematics solver adopts Levenberg-Marquardt, Broyden-Fletcher-Goldfarb-Shanno or a solver based on an analytical method.

Images