A dual-arm motion planning method based on speed constraint space

Abstract

The application discloses a kind of double-arm motion planning methods based on speed constraint space, belong to robot technical field.Method includes: first give the starting point and target point of double-arm carrying object;Then the speed constraint space of object is calculated to carry out uniform sampling, obtain the sampling speed set of object;Then distance deviation, pose deviation, joint torque and task operability are used as index to establish evaluation function, the sampling speed is scored by evaluation function, selects the highest score sampling speed as the speed of object next time;Finally, the optimal sampling speed of object each time is constantly selected until the complete trajectory from starting point to target point is obtained.The application realizes the motion optimization of double-arm coordination under the kinematic and dynamic constraint conditions, improves the task operability of double-arm, reduces the joint torque of robot arm, guarantees the stability and safety of double-arm carrying process.

Description

Technical Field

[0001] This invention relates to the field of robotics, and in particular to a method for planning the motion of a dual-arm system based on velocity-constrained space. Background Technology

[0002] Compared to single-arm robots, dual-arm robots can perform more complex tasks. They have received increasing attention in current research. The advantages of dual-arm robots are particularly evident in complex maneuvering tasks, such as healthcare and lifting heavy objects. Therefore, dual-arm robots can perform complex tasks, improving flexibility and efficiency.

[0003] Current technologies for dual-arm robots to handle objects have certain limitations. The motion trajectory of dual-arm objects is usually assumed to be known or independently generated, and the optimization of the object's posture is not considered. In the prior art, Chinese Patent Publication No. CN116587263A discloses a hydraulic dual-arm trajectory planning and control method considering multiple physical constraints. This method adaptively updates the object's inertial parameters for estimating physical constraint boundaries, ensuring the object does not deviate from the desired path; however, this method does not consider optimizing the object's motion trajectory. Chinese Patent Publication No. CN108638057A discloses a motion planning method for a humanoid robot's dual arms. This method enumerates the standard quadratic programming problem expression for the humanoid robot, sets three motion planning decision methods for the humanoid robot, and outputs the optimal solution by combining these three motion planning decision methods; however, this method does not consider optimizing the object's posture. Chinese Patent Publication No. CN115107023A discloses a method for independent control of the two arms of a vacuum dual-arm robot. This method parses motion commands through a robot controller to extract commands for the upper arm and the lower arm, and adopts a dual-process independent control method for the two arms. However, this method does not consider the kinematic and dynamic constraints of the robotic arms and cannot guarantee the safety of the robotic arm system. Summary of the Invention

[0004] To overcome the shortcomings of existing technologies, this invention proposes a dual-arm motion planning method based on velocity-constrained space, thereby improving the safety and stability of dual-arm handling. To achieve the above objective, this invention employs the following technical solution, including:

[0005] A dual-arm motion planning method based on velocity constraint space is proposed. The two arms move an object from the starting point to the ending point. Taking into account the kinematic and dynamic constraints of the two robotic arms, a velocity constraint space for the object is established. An evaluation function is established based on distance deviation, posture deviation, joint torque, and task operability as indicators to select the optimal object velocity.

[0006] Preferably, the established evaluation function is as follows:

[0007] G0(v,w)=α·pos(v,w)+β·dis(v,w)+γ·m(v,w)+δ·tor(v,w);

[0008] pos(v,w)=||R-E3||;

[0009]

[0010] tor(v,w)=||τ T τ||;

[0011] Where G0(v,w) is the evaluation function; v is the current linear velocity, w is the current angular velocity; pos(v,w) is the attitude difference between the simulated predicted trajectory and the endpoint under the current velocity vector condition (v,w); dis(v,w) is the distance between the simulated predicted trajectory and the endpoint under the current velocity vector condition; m(v,w) is the task operability; tor(v,w) is the infinite norm torque of the two arms; α, β, γ and δ are all weighting coefficients; R is the rotation matrix of the object's current attitude; E3 is the third-order identity matrix. It is the distance between the object's current position p1 and the endpoint p2; det(·) calculates the determinant of the matrix; W is the grasping matrix; W T It is the transpose of the capture matrix; J + It is the pseudo-inverse of the Jacobian matrix; J +T It is the transpose of the pseudoinverse of the Jacobian matrix; τ is the torque vector of the two arm joints; τ T It is the transpose of the torque vector of the two arm joints.

[0012] Preferably, a velocity constraint space for the object is established, and the optimal object velocity is selected based on the criteria of minimizing joint torque and maximizing task operability. The specific method is as follows:

[0013] Step 1: The object is limited by its maximum and minimum velocities. The linear and angular velocities that the object can achieve in Cartesian space are:

[0014] V m ={v∈(v min ,v max ),w∈(w min ,w max )};

[0015] Among them, V m The set of velocities that meet the maximum and minimum speed limits, where v and w are the current linear velocity and angular velocity, respectively. min v max w min w max These are the minimum linear velocity, maximum linear velocity, minimum angular velocity, and maximum angular velocity, respectively.

[0016] Step 2: The object is limited by acceleration; the linear velocity and angular velocity it can reach within one sampling period are:

[0017] V d ={v∈(v c -v b T,v c +v a T), w∈(w c -w b T,w c +w a T)};

[0018] Among them, V d For a velocity set that meets acceleration limits, v b and w b These are the current maximum linear deceleration and maximum angular deceleration, respectively, v a and w a These are the current maximum linear acceleration and maximum angular acceleration, respectively, where T is the sampling period; v c and w c These are the initial linear velocity and initial angular velocity within the sampling period, respectively.

[0019] Step 3: The velocity constraint space is the space of velocity sets that simultaneously satisfy both the velocity constraints in Step 1 and Step 2.

[0020] V T =V m ∩V d ;

[0021] Among them, V T A velocity set in a velocity-constrained space;

[0022] Step 4: Within one sampling period, for the velocity set V T The linear velocity and angular velocity within the range are sampled, and sampling points that exceed the torque limit are removed to obtain a velocity set V that conforms to kinematic and dynamic constraints;

[0023] The torque is limited as follows:

[0024]

[0025] Where k = 1, 2 represents the k-th robotic arm; Let be the torque of the i-th joint on the robotic arm k; Let be the torque limit of the i-th joint on the robotic arm k;

[0026] Step 5: In order to obtain the optimal result, set an evaluation function to evaluate the quality of the velocity set V, and select the velocity with the highest score as the velocity at the next moment according to the evaluation function.

[0027] A readable storage medium having a computer program stored thereon, which, when executed, implements the above-described method for bi-arm motion planning based on velocity constraint space.

[0028] An electronic device includes a processor, a memory, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, it implements the above-described method for dual-arm motion planning based on velocity constraint space.

[0029] A computer program product comprising a computer program / instructions that, when executed by a processor, implement the aforementioned dual-arm motion planning method based on velocity constraint space.

[0030] The advantages of this invention are:

[0031] (1) This invention proposes a dual-arm motion planning method based on velocity constraint space. Based on the velocity constraint space of the object, an evaluation function is established to select the optimal object velocity, which improves the operability of the dual arms, reduces the joint torque of the robotic arms, improves the stability of dual-arm handling, and reduces energy consumption.

[0032] (2) This invention takes into account the kinematic and dynamic constraints of the robotic arm, establishes a velocity constraint space based on the kinematic limitations of the robotic arm, and performs dynamic constraints by discarding sampling points where the joint torque exceeds the limit, thus ensuring the safety of the robotic arm's operation. Attached Figure Description

[0033] Figure 1 This is a flowchart of the method of the present invention.

[0034] Figure 2 This is a torque comparison diagram of the straight trajectory and the optimized trajectory of the present invention.

[0035] Figure 3 This is a comparison chart of the task operability of the straight trajectory and the optimized trajectory of the present invention. Detailed Implementation

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

[0037] Example 1

[0038] Depend on Figure 1As shown, a dual-arm motion planning method based on velocity constraint space is presented. The specific process is as follows:

[0039] (1) Determine the starting and ending points of the object being moved by both arms.

[0040] (2) Establish the velocity constraint space of the object. The velocity constraint space is the space of velocities that simultaneously satisfy the following two constraint conditions:

[0041] V T =V m ∩V d ;

[0042] Among them, V m V is the set of velocities that an object can reach in Cartesian space, limited by its maximum and minimum velocities. d V is the set of velocities that an object can reach within one sampling period under acceleration constraints. T It is the set of velocities of an object in a velocity-constrained space, V T It is the velocity set in the velocity-constrained space.

[0043] The object is limited by its maximum and minimum velocities. The linear and angular velocities that the object can achieve in Cartesian space are:

[0044] V m ={v∈(v min ,v max ),w∈(w min ,w max )};

[0045] Among them, V m The set of velocities that meet the maximum and minimum speed limits, where v and w are the current linear velocity and angular velocity, respectively. min v max w min w max These are the minimum linear velocity, maximum linear velocity, minimum angular velocity, and maximum angular velocity, respectively.

[0046] The object is limited by acceleration; the linear velocity and angular velocity it can reach within one sampling period are:

[0047] V d ={v∈(v c -v b T,v c +v a T), w∈(w c -w b T,w c +w a T)};

[0048] Among them, V dFor a velocity set that meets acceleration limits, v b and w b These are the current maximum linear deceleration and maximum angular deceleration, respectively, v a and w a These are the current maximum linear acceleration and maximum angular acceleration, respectively, where T is the sampling period; v c and w c These represent the initial linear velocity and initial angular velocity within the sampling period, respectively.

[0049] (3) Perform uniform sampling on the velocity constraint space and discard sampling points that exceed the torque limit.

[0050] Within one sampling period, for the velocity set V T The linear velocity and angular velocity within the range are sampled, and sampling points that exceed the torque limit are removed to obtain a velocity set V that conforms to kinematic and dynamic constraints.

[0051] The torque is limited as follows:

[0052]

[0053] Where k = 1, 2 represents the k-th robotic arm; Let be the torque of the i-th joint on the robotic arm k; Let be the torque limit of the i-th joint on the robotic arm k.

[0054] (4) Set the evaluation function G0(v,w). Based on the evaluation function, select the velocity (v,w) with the highest score from the velocity set V as the velocity of the object at the next moment.

[0055] G0(v,w)=α·pos(v,w)+β·dis(v,w)+γ·m(v,w)+δ·tor(v,w);

[0056] pos(v,w)=||R-E3||;

[0057]

[0058] tor(v,w)=||τ T τ||;

[0059] Where v is the current linear velocity, w is the current angular velocity; pos(v,w) is the attitude difference between the simulated predicted trajectory and the endpoint under the current velocity vector condition (v,w); dis(v,w) is the distance between the simulated predicted trajectory and the endpoint under the current velocity vector condition; m(v,w) is the task operability; tor(v,w) is the infinite norm torque of the two arms; α, β, γ and δ are all weighting coefficients; R is the rotation matrix of the object's current attitude; E3 is the third-order identity matrix. It is the distance between the object's current position p1 and the endpoint p2; det(·) calculates the determinant of the matrix; W is the grasping matrix; W T It is the transpose of the capture matrix; J + It is the pseudo-inverse of the Jacobian matrix; J +T It is the transpose of the pseudoinverse of the Jacobian matrix; τ is the torque vector of the two arm joints; τ T It is the transpose of the torque vector of the two arm joints.

[0060] (5) Trajectory generation. Continuously select the optimal velocity of the object at each moment until a complete trajectory from the starting point to the target point is obtained.

[0061] (6) Admittance control. Given the desired contact force between the end effector of the robotic arm and the object, the displacement correction of the end effector of the robotic arm is calculated based on the admittance controller.

[0062] (7) Robotic arm motion control. The displacement correction calculated by the admittance controller is superimposed on the end position of the robotic arm to obtain the desired position of the robotic arm. The desired angles of each joint of the two robotic arms are obtained by inverse kinematics solution. Finally, the motion control of the robotic arm is realized by the position controller.

[0063] Figure 2 and Figure 3 The graphs show a comparison of the torque (tor(v,w)) and task operability (m(v,w)) of the straight-line trajectory and the optimized trajectory from the starting point to the ending point. Blue represents the straight-line trajectory, and orange represents the optimized trajectory. It can be seen that the torque of the optimized trajectory is less than that of the straight-line trajectory, and the task operability of the optimized trajectory is greater than that of the straight-line trajectory.

[0064] Example 2

[0065] An electronic device includes a processor, a memory, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, it implements a dual-arm motion planning method based on velocity constraint space as described in Embodiment 1 above.

[0066] The electronic device in this application embodiment may be the mobile device itself, or a standalone device independent of it. The standalone device may communicate with the mobile device to receive the collected input signals from it and send the selected target decision behavior to it.

[0067] An electronic device includes one or more processors and memory. The processor may be a central processing unit (CPU) or other processing unit with data processing capabilities and / or instruction execution capabilities, and may control other components in the electronic device to perform desired functions. The memory may include one or more computer program products, which may include various forms of computer-readable storage media, such as volatile memory and / or non-volatile memory. The volatile memory may, for example, include random access memory (RAM) and / or cache memory. The non-volatile memory may, for example, include read-only memory (ROM), hard disk, flash memory, etc. One or more computer program instructions may be stored on the computer-readable storage medium, and the processor may execute the program instructions to implement the decision-making behavior and decision-making methods of the various embodiments of this application described above, and / or other desired functions.

[0068] Electronic devices may also include input devices and output devices.

[0069] Example 3

[0070] In addition to the methods and devices described above, embodiments of this application may also be computer program products, which include computer program instructions that, when executed by a processor, cause the processor to perform the steps in the decision-making behavior method according to various embodiments of this application as described in Embodiment 1 above.

[0071] The computer program product can be written in any combination of one or more programming languages ​​to perform the operations of the embodiments of this application. The programming languages ​​include object-oriented programming languages ​​such as Java and C++, as well as conventional procedural programming languages ​​such as C or similar languages. The program code can be executed entirely on the user's computing device, partially on the user's computing device, as a standalone software package, partially on the user's computing device and partially on a remote computing device, or entirely on a remote computing device or server.

[0072] Example 4

[0073] Embodiments of this application may also be computer-readable storage media storing computer program instructions thereon, which, when executed by a processor, cause the processor to perform the steps in the decision-making behavior decision-making method according to various embodiments of this application described in Embodiment 1 above.

[0074] The computer-readable storage medium may be any combination of one or more readable media. A readable medium may be a readable signal medium or a readable storage medium. A readable storage medium may, for example, include, but is not limited to, electrical, magnetic, optical, electromagnetic, infrared, or semiconductor systems, apparatuses, or devices, or any combination thereof. More specific examples of readable storage media (a non-exhaustive list) include: electrical connections having one or more wires, portable disks, hard disks, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM or flash memory), optical fibers, portable compact disk read-only memory (CD-ROM), optical storage devices, magnetic storage devices, or any suitable combination thereof.

[0075] The above are merely preferred embodiments of the present invention and are not intended to limit the scope of the present invention. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A dual-arm motion planning method based on velocity constraint space, characterized in that, The two arms move an object from the starting point to the ending point. Taking into account the kinematic and dynamic constraints of the two robotic arms, a velocity constraint space for the object is established. An evaluation function is established based on distance deviation, posture deviation, joint torque, and task operability as indicators to select the optimal object velocity. The established evaluation function is shown below: ； ； ； ； ； in, G 0( v , w ) is the evaluation function; v It is the current linear velocity. w It is the current angular velocity; pos ( v , w ) is under the current velocity vector condition, i.e. ( v , w Under these conditions, the attitude difference between the simulated predicted trajectory and the endpoint; dis ( v , w () represents the distance between the simulated predicted trajectory and the endpoint under the current velocity vector conditions; m ( v , w () represents the task's operability. tor ( v , w ) is the infinite norm torque of the two arms; α , β , γ and δ These are all weighting coefficients; R It is the rotation matrix of the object's current orientation; E 3 is a third-order identity matrix; The current position of the object p 1 and the finish line p The distance between 2; det(·) calculates the determinant of the matrix; W It is a matrix capture; W T It is the transpose of the capture matrix; J + It is the pseudo-inverse of the Jacobian matrix; J +T It is the transpose of the pseudo-inverse of the Jacobian matrix; τ This represents the torque vector of the two arm joints; τ T It is the transpose of the torque vector of the two arm joints.

2. The dual-arm motion planning method based on velocity constraint space according to claim 1, characterized in that, Establish a velocity constraint space for the object, and select the optimal object velocity based on the criteria of minimizing joint torque and maximizing task operability. The specific method is as follows: Step 1: The object is limited by its maximum and minimum velocities. The linear and angular velocities that the object can achieve in Cartesian space are: ； in, V m For a set of speeds that meet the maximum and minimum speed limits, v and w These are the current linear velocity and angular velocity, respectively. v min , v max , w min , w max These are the minimum linear velocity, maximum linear velocity, minimum angular velocity, and maximum angular velocity, respectively. Step 2: The object is limited by acceleration; the linear velocity and angular velocity it can reach within one sampling period are: ； in, V d To meet the acceleration limit set, v b and w b These are the current maximum linear deceleration and maximum angular deceleration, respectively. v a and w a These are the current maximum linear acceleration and maximum angular acceleration, respectively. T The sampling period; v c and w c These are the initial linear velocity and initial angular velocity within the sampling period, respectively. Step 3: The velocity constraint space is the space of velocity sets that simultaneously satisfy both the velocity constraints in Step 1 and Step 2. V T = V m ∩ V d ； in, V T A velocity set in a velocity-constrained space; Step 4: Within one sampling period, for the velocity set V T The linear and angular velocities within the range are sampled, and sampling points exceeding the torque limit are removed to obtain a velocity set that conforms to kinematic and dynamic constraints. V ; The torque is limited as follows: ； in, k =1,2 indicates the first k One robotic arm; For robotic arms k Upper i The torque of each joint; For robotic arms k Upper i The torque limit of each joint; Step 5: To obtain the optimal result, set an evaluation function to evaluate the velocity set. V Based on the quality, the speed with the highest score is selected as the speed for the next moment according to the evaluation function.

3. A readable storage medium, characterized in that, It stores a computer program, which, when executed, implements the dual-arm motion planning method based on velocity constraint space as described in any one of claims 1 to 2.

4. An electronic device, characterized in that, It includes a processor, a memory, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, it implements the dual-arm motion planning method based on velocity constraint space as described in any one of claims 1 to 2.

5. A computer program product, characterized in that, It includes a computer program / instruction that, when executed by a processor, implements the dual-arm motion planning method based on velocity constraint space as described in any one of claims 1 to 2.

Images