Dual-arm collaborative planning method, device and storage medium based on dynamic system

Abstract

The dual-arm collaborative planning method, device and storage medium based on a dynamic system belong to the field of robotics technology and solve the real-time planning problem under collaborative constraints and dynamic obstacle interference. The method of the present invention includes: given the target posture of an object, the planning method will combine the current actual state of the two arms, obstacle information and motion boundary constraints to generate a dual-arm end motion trajectory that meets the target following and copes with dynamic obstacles; then, through the centralized inverse solution, the end motion trajectory is mapped to the joint space to drive the movement of the two arms; finally, the robot perceives the surrounding environment and obstacle information in real time, and feeds it back to the planner, thereby forming a data flow loop. The present invention is suitable for dual-arm collaborative planning based on a dynamic system.

Description

Technical Field

[0001] The present application relates to the field of robotics technology, and in particular to dual-arm collaborative planning based on a dynamic system. Background Art

[0002] Dual arms can give robots greater operational flexibility and capabilities. Their coordinated motion, considered a key component of dual-arm manipulation, is crucial for ensuring stable grasping, handling, and placement of objects. However, their coordinated motion is subject to multiple coupled constraints, including kinematics, dynamics, and forces. These constraints make it difficult for robots to flexibly respond to dynamic targets and environmental disturbances.

[0003] The coordinated motion of the two arms is subject to coupled constraints, such as kinematics, making flexible planning difficult. Furthermore, since real-world environments are often dynamic, and actual motion requires comprehensive consideration of the geometric properties (shape, position, and posture) and dynamic interference (relative motion) between the two arms and obstacles, coordinated motion of the two arms must address the real-time planning challenges inherent in collaborative constraints and the interference of dynamic obstacles. Summary of the Invention

[0004] The purpose of the present invention is to solve the problem of real-time planning of dual-arm collaborative motion under collaborative constraints and dynamic obstacle interference, and to provide a dual-arm collaborative planning method, device and storage medium based on a dynamic system.

[0005] The present invention is achieved through the following technical solutions. In one aspect, the present invention provides a dual-arm collaborative planning method based on a dynamic system, the method comprising:

[0006] Step 1: Establish a simplified model of the two arms, which is established based on the geometric attributes and dynamic properties of the two arms and obstacles;

[0007] Step 2: Multiple control points and their corresponding weights are introduced into the simplified dual-arm model. The motion of each control point is considered as point motion, which is used for local guidance and obstacle avoidance. Dynamic obstacle avoidance and target convergence of the dual arms are achieved through motion synthesis.

[0008] The motion synthesis specifically includes: obtaining the modulated motion of each control point; synthesizing the velocity of each control point into the object, and then mapping it to the two arms using the centralized inverse solution;

[0009] The velocity of each control point is synthesized to satisfy the following equation:

[0010]

[0011] Where, v obj is the linear velocity of the object before modulation, is the linear velocity and angular velocity of the object after synthesis, is the linear velocity of the object and each control point after modulation, is the angular velocity of the object after modulation, x obj.pos 、x i.pos is the current position of the object and each control point, ω obj 、ω i is the weight of the object and each control point.

[0012] Furthermore, in step 1, the establishment of a simplified dual-arm model specifically includes: using a rectangular bounding box to minimally enclose the ends of the dual arms and the operated object, simplifying the kinematic chain between the ends of the dual arms into a connecting rod, and then simplifying the connecting rod into a line segment by extending the obstacle boundary.

[0013] Furthermore, when the distance between each control point and the object is less than a preset value, the angular velocity of the object will satisfy the following formula:

[0014]

[0015] Among them, κ R，i is the angular velocity gain coefficient.

[0016] Furthermore, the weight ω of each control point i The distance Γ(x i ) and satisfy the following formula:

[0017]

[0018] ω i =ω i / ∑ω i

[0019] Among them, Γ min is the minimum threshold of the control point distance from the obstacle boundary, satisfying 1≤Γ min <Γ cut , in particular, if all control points satisfy Γ(x i )>Γ cut , Γ cut To determine whether the end point is close to the distance threshold of the obstacle boundary, the weights ω of all control points i Both are 1.

[0020] Furthermore, the distance function formula from each control point to the obstacle boundary is:

[0021]

[0022] in, The control point is the boundary point of the obstacle o, satisfying Where b>0,κ distis the distance magnification factor (κ dist ≥1);

[0023] In a multi-obstacle scenario, the weight of each control point ω i The shortest distance Γ(x i ) min Calculate and satisfy the following formula:

[0024] T(x i ) min =MinΓ o＝1:N (x i )

[0025]

[0026] Among them, Γ o＝1：N (x i ) is the set of distances between the control point and all obstacles.

[0027] Furthermore, for the coordinated movement of both arms, when the speeds of the control points on both sides form a symmetrical distribution, let is the expression of the left and right control points in the obstacle coordinate system. When the motion of the control points on both sides satisfies the following formula, the virtual target will replace the global target for local guidance. The following formula is:

[0028]

[0029] in, is the expression of each control point in the obstacle coordinate system, v i，x 、v i，y is the projection velocity of the control point i on the x and y axes, i∈{left, right}, Γ(x left ),Γ(x right ) are both in Γ min With Γ cut Between, that is, within the influence range of the obstacle, where x left and x right are the control points on the left and right sides respectively;

[0030] The virtual target is set to the side of the obstacle close to the current object, corresponding to the position:

[0031]

[0032] Among them, x obs，x 、x obs，y is the coordinate of the obstacle center, l obs，x The length of the obstacle in the x direction. The default horizontal axis is x, the vertical axis is y, and r 1 / r，x The length between the center of the object and the left or right side control point.

[0033] Furthermore, the centralized inverse solution specifically includes:

[0034] The two arms work together to manipulate the same object. In this system, the number of degrees of freedom of a single arm is n. i , where i = [l, r], representing the subscripts of the left and right arms; x = [x1, x r ] T is the generalized coordinate system of the robot end effector; if there is no superscript, the coordinate system is expressed in the global base system ∑base by default;

[0035] In the dual-arm coordinated motion, the dual arms satisfy the motion coupling constraints at the position level and velocity level. The position level constraint is expressed as:

[0036]

[0037] Among them, T obj , T1, T r is the homogeneous transformation matrix of the object, left arm and right arm relative to the global basis system, is the homogeneous transformation matrix of the left arm and the right arm relative to the object coordinate system;

[0038] The grasping matrix of the coordinated motion of the two arms is expressed as:

[0039]

[0040] in, To capture the matrix, I3, is the three-dimensional unit matrix and the three-dimensional zero matrix, are antisymmetric factors, r1, r r is the position vector between the left and right arm end coordinate systems and the contact point of the object;

[0041] According to rigid body kinematics, the velocity level constraint of the coordinated motion of the two arms can be obtained as:

[0042]

[0043] in, is the Jacobian matrix, is the linear velocity and angular velocity matrix of the double-arm end coordinate system, is the joint angular velocity of both arms, is the linear velocity and angular velocity matrix of the object;

[0044] According to the position-level constraint expression and the velocity-level constraint expression of the coordinated motion of the two arms, when the motion trajectory of the object in the task space is known, the trajectory of the two arms is calculated through the velocity-level constraint, and then the inverse kinematics solution is performed based on the damped least squares method, so that the motion of the object is mapped to the joint space of the two arms, thereby forming an object-oriented centralized inverse kinematics solution model.

[0045] In a second aspect, the present invention provides a computer device comprising a memory and a processor, wherein the memory stores a computer program, and when the processor runs the computer program stored in the memory, the steps of a dual-arm collaborative planning method based on a dynamic system as described above are executed.

[0046] In a third aspect, the present invention provides a computer-readable storage medium, wherein a plurality of computer instructions are stored in the computer-readable storage medium, and the plurality of computer instructions are used to enable a computer to execute a dual-arm collaborative planning method based on a dynamic system as described above.

[0047] In a fourth aspect, the present invention provides an electronic device, comprising:

[0048] at least one processor; and,

[0049] a memory communicatively connected to the at least one processor; wherein,

[0050] The memory stores instructions that can be executed by the at least one processor, and the instructions are executed by the at least one processor to enable the at least one processor to execute the dual-arm collaborative planning method based on a dynamic system as described above.

[0051] Beneficial effects of the present invention:

[0052] This paper proposes a new and effective method for dynamic collaborative planning of dual arms. This method, while satisfying the closed-chain coupling kinematic constraints of the dual arms, can dynamically follow a potentially changing target pose. Furthermore, by considering the geometric influence between the dual arms and obstacle shapes, the dual arms can collaboratively respond to dynamic obstacle interference.

[0053] The present invention provides a data flow for the planning method: given the target posture of an object, the planning method will combine the current actual state of the two arms, obstacle information and motion boundary constraints to generate a two-arm end motion trajectory that satisfies the target following and copes with dynamic obstacles; then, through the centralized inverse solution of the two arms (also known as the centralized inverse solution), the end motion trajectory is mapped to the joint space to drive the movement of the two arms; finally, the robot perceives the surrounding environment and obstacle information in real time and feeds it back to the planner, thereby forming a data flow loop.

[0054] The present invention is applicable to dual-arm collaborative planning based on a dynamic system. BRIEF DESCRIPTION OF THE DRAWINGS

[0055] In order to more clearly illustrate the technical solution of the present application, the following is a brief introduction to the drawings required for use in the embodiments. Obviously, for ordinary technicians in this field, other drawings can be obtained based on these drawings without any creative work.

[0056] Figure 1 A dual-arm collaborative planning framework based on dynamic systems;

[0057] Figure 2 Schematic diagram of obstacle definition and motion planning, where a is the definition of obstacles and motion area, and b is the 2D scene motion streamline diagram;

[0058] Figure 3 It is a model of coordinated movement of both arms;

[0059] Figure 4 Schematic diagram of dual-arm collaborative obstacle avoidance;

[0060] Figure 5 This is the streamline diagram of the double-arm motion in a single obstacle scenario;

[0061] Figure 6 This is the streamline diagram of the dual-arm motion in a multi-obstacle scenario;

[0062] Figure 7 The two-arm motion in the scenario of symmetrical velocity distribution, where a represents the velocity symmetry on both sides and b represents the interference between the resultant motion and the obstacle.

[0063] Figure 8 Schematic diagram of local guidance based on virtual targets, where a is local guidance based on virtual targets and b is global target guidance. DETAILED DESCRIPTION

[0064] The embodiments of the present invention are described in detail below, examples of which are shown in the accompanying drawings, wherein the same or similar reference numerals throughout represent the same or similar elements or elements having the same or similar functions. The embodiments described below with reference to the accompanying drawings are exemplary and are intended to be used to explain the present invention, but are not to be construed as limiting the present invention.

[0065] Embodiment 1: A dual-arm collaborative planning method based on a dynamic system, the method comprising:

[0066] Step 1: Establish a simplified model of the two arms, which is established based on the geometric attributes and dynamic properties of the two arms and obstacles;

[0067] Step 2: Multiple control points and their corresponding weights are introduced into the simplified dual-arm model. The motion of each control point is considered as point motion, which is used for local guidance and obstacle avoidance. Dynamic obstacle avoidance and target convergence of the dual arms are achieved through motion synthesis.

[0068] The motion synthesis specifically includes: obtaining the modulated motion of each control point; synthesizing the velocity of each control point into the object, and then mapping it to the two arms using the centralized kinematics inverse solution;

[0069] The velocity of each control point is synthesized to satisfy the following equation:

[0070]

[0071] Where, is the linear velocity and angular velocity of the object after synthesis, is the linear velocity of the object and each control point after modulation, is the angular velocity of the object after modulation, x obj.pos 、x i.pos is the current position of the object and each control point, ω obj 、ω i is the weight of the object and each control point.

[0072] In this implementation, given the target pose of an object, the planning method combines the current actual state of the two arms, obstacle information, and motion boundary constraints to generate a two-arm end motion trajectory that satisfies the target and copes with dynamic obstacles. The end motion trajectory is then mapped to the joint space through a centralized inverse solution (also known as a centralized inverse solution) of the two arms to drive the movement of the two arms. Finally, the robot perceives the surrounding environment and obstacle information in real time and feeds it back to the planner, thus forming a data flow loop.

[0073] Implementation method 2: This implementation method further limits the dual-arm collaborative planning method based on a dynamic system described in implementation method 1. In this implementation method, the establishment of a simplified dual-arm model in step 1 is further limited, specifically including:

[0074] In step 1, the establishment of a simplified dual-arm model specifically includes: using a rectangular bounding box to minimize the encirclement of the dual-arm ends and the manipulated object, simplifying the kinematic chain between the dual-arm ends into a connecting rod, and then simplifying the connecting rod into a line segment by extending the obstacle boundary.

[0075] In this implementation, the planning model is first simplified to reduce the computational dimension, and then a dual-arm collaborative planning method is proposed based on the simplified model.

[0076] Implementation method 3: This implementation method further limits the dual-arm collaborative planning method based on a dynamic system described in implementation method 1. In this implementation method, the angular velocity synthesis of the object when the distance between each control point and the object is less than a preset value is further limited, specifically including:

[0077] When the distance between each control point and the object is less than the preset value, the object's angular velocity synthesis will satisfy the following formula:

[0078]

[0079] Among them, κ R，i is the angular velocity gain coefficient.

[0080] In this embodiment, the posture of the object is determined by its angular velocity However, due to the small distance between each control point and the object, the synthetic motion has little effect on the object's posture, and there may be a problem of not being flexible enough to deal with obstacles. Therefore, the angular velocity gain coefficient κ is introduced R，i , to enhance the flexibility of the object's posture response in collaborative motion.

[0081] Implementation method 4: This implementation method further limits the dual-arm collaborative planning method based on a dynamic system described in implementation method 1. In this implementation method, the weight ω of each control point is i , further defined, including:

[0082] The weight ω of each control point i The distance Γ(x i ) and satisfy the following formula:

[0083]

[0084] ω i =ω i ∑ω i

[0085] Among them, Γ min is the minimum threshold of the control point distance from the obstacle boundary, satisfying 1≤Γ min <Γ cut , in particular, if all control points satisfy Γ(x i )>Γ cut , the weights ω of all control points i Both are 1.

[0086] In this embodiment, during the movement, since the relative distances between each control point and the obstacle are different, each control point will be calculated by its corresponding weight ω i Determine the size of the impact.

[0087] Implementation 5: This implementation further limits the dual-arm collaborative planning method based on a dynamic system described in Implementation 4. In this implementation, the distance function formula from each control point to the obstacle boundary is further limited, specifically including:

[0088] The distance function formula from each control point to the obstacle boundary is:

[0089]

[0090] in, The control point is the boundary point of the obstacle o, satisfying Where b>0,κ dist is the distance magnification factor (κ dist ≥1);

[0091] In a multi-obstacle scenario, the weight of each control point ω i The shortest distance Γ(x i ) min Calculate and satisfy the following formula:

[0092] Γ(x i ) min =MinΓ o＝1:N (x i )

[0093]

[0094] Among them, Γ o＝1:N (x i ) is the set of distances between the control point and all obstacles.

[0095] In this embodiment, when the two arms are in a coordinated motion mode, the motion coupling constraint will reduce the working space of the two arms. Therefore, in order to avoid obstacles while getting as close to the obstacles as possible and utilizing more working space, this embodiment uses an exponential form to calculate the distance between the control point and the boundary. The amplification factor κ dist The role of is to determine the range of influence of obstacles. When the magnification factor is small, the movement of the control point will be close to the obstacle boundary. However, once the control point approaches the obstacle boundary, the boundary distance function will increase exponentially, and vice versa.

[0096] In a multi-obstacle scenario, the weight of each control point still depends on the distance between it and the obstacle.

[0097] Implementation 6: This implementation further limits the dual-arm collaborative planning method based on a dynamic system described in Implementation 1. In this implementation, the guidance method for the coordinated movement of the dual arms when the speeds of the control points on both sides form a symmetrical distribution is further limited, specifically including:

[0098] For the coordinated movement of both arms, when the speeds of the control points on both sides form a symmetrical distribution, let is the expression of the left and right control points in the obstacle coordinate system. When the motion of the control points on both sides satisfies the following formula, the virtual target will replace the global target for local guidance. The following formula is:

[0099]

[0100] in, is the expression of each control point in the obstacle coordinate system, v i，x 、v i，y is the projection velocity of the control point i on the x and y axes, i∈{left, right}, Γ(x left ),Γ(x right ) are both in Γ min With Γ cut Between, that is, within the influence range of the obstacle;

[0101] The virtual target is set to the side of the obstacle close to the current object, corresponding to the position:

[0102]

[0103] Among them, x obs，x 、x obs，y is the coordinate of the obstacle center, l obs，x The length of the obstacle in the x direction. The default horizontal axis is x, the vertical axis is y, and r 1 / r，x The length between the center of the object and the left or right side control point.

[0104] In this implementation, when the velocities of the control points on both sides of the dual-arm coordinated motion are symmetrically distributed, a special case occurs where the resulting motion of the object will still interfere with the obstacle. This velocity symmetry phenomenon means that the resulting motion will drive both arms toward the obstacle until interference occurs. This implementation analyzes this special case and provides a corresponding solution, employing a strategy of localized guidance using a virtual target to unify the motion streamlines on both sides of the obstacle.

[0105] Implementation method seven: This implementation method further limits the dual-arm collaborative planning method based on a dynamic system described in implementation method one. In this implementation method, the centralized inverse solution is further limited, specifically including:

[0106] The centralized inverse solution specifically includes:

[0107] The two arms work together to manipulate the same object. In this system, the number of degrees of freedom of a single arm is n. i , where i = [l, r], representing the subscripts of the left and right arms; x = [x1, x r ] Tis the generalized coordinate system of the robot end effector; if there is no superscript, the coordinate system is expressed in the global base system ∑base by default;

[0108] In the dual-arm coordinated motion, the dual arms satisfy the motion coupling constraints at the position level and velocity level. The position level constraint is expressed as:

[0109]

[0110] Among them, T obj , T1, T r is the homogeneous transformation matrix of the object, left arm and right arm relative to the global basis system, is the homogeneous transformation matrix of the left arm and the right arm relative to the object coordinate system;

[0111] The grasping matrix of the coordinated motion of the two arms is expressed as:

[0112]

[0113] in, To capture the matrix, I3, is the three-dimensional unit matrix and the three-dimensional zero matrix, are antisymmetric factors, r1, r r is the position vector between the left and right arm end coordinate systems and the contact point of the object;

[0114] According to rigid body kinematics, the velocity level constraint of the coordinated motion of the two arms can be obtained as:

[0115]

[0116] in, is the Jacobian matrix, is the linear velocity and angular velocity matrix of the double-arm end coordinate system, is the joint angular velocity of both arms, is the linear velocity and angular velocity matrix of the object;

[0117] According to the position-level constraint expression and the velocity-level constraint expression of the coordinated motion of the two arms, when the motion trajectory of the object in the task space is known, the trajectory of the two arms is calculated through the velocity-level constraint, and then the inverse kinematics solution is performed based on the damped least squares method, so that the motion of the object is mapped to the joint space of the two arms, thereby forming an object-oriented centralized inverse kinematics solution model.

[0118] This embodiment uses a dual-arm centralized inverse solution (also known as a centralized inverse solution) to map the motion trajectory of the dual-arm ends to the joint space and sends it to the joint motion controller to drive the movement of the dual arms.

[0119] Implementation 8: This implementation is based on an embodiment of a dual-arm collaborative planning method based on a dynamic system as described above, and specifically includes:

[0120] This embodiment will implement dynamic collaborative planning for dual arms, proposing a dynamic, real-time solution to enable the dual arms to flexibly respond to dynamic targets and environmental interference. This is primarily targeted at dual arm application scenarios requiring: (1) dual arm handling or palletizing tasks, where dynamic planning is required to track potentially changing targets; and (2) dynamic scenarios such as human-machine collaboration, where the dual arms need to actively avoid dynamic obstacle interference.

[0121] Among existing planning methods, approaches based on dynamic systems demonstrate superior planning performance in terms of autonomy and dynamic response, offering an effective alternative to traditional approaches. Therefore, we propose an effective method for dynamic collaborative planning of dual arms based on this dynamic system planning approach. This method, while satisfying the closed-loop kinematic constraints of the dual arms, considers the geometric influence of the dual arm shapes and those of obstacles, enabling dynamic following of potentially changing target poses and enabling the dual arms to collaboratively respond to dynamic obstacle interference. Figure 1 The overall framework of the method is presented.

[0122] This framework embodies the data flow of the planning method: given the target pose of an object, the planning method will combine the current actual state of the two arms, obstacle information, and motion boundary constraints to generate a two-arm end motion trajectory that satisfies the target and copes with dynamic obstacles; then, through the centralized inverse solution of the two arms (also known as the centralized inverse solution), the end motion trajectory is mapped to the joint space to drive the movement of the two arms; finally, the robot perceives the surrounding environment and obstacle information in real time and feeds it back to the planner, thus forming a data flow loop.

[0123] S1. Online planning method based on dynamic system, including:

[0124] S1.1. Establish a dynamic system model:

[0125] Consider a state variable It defines a state (position and attitude, hereinafter referred to as "pose") of the robot arm actuator in the task space. Its time evolution can be controlled by an autonomous (time-invariant) dynamic system as shown below

[0126]

[0127] Among them, f(.) is continuous. Therefore, given the initial state x0 of the end effector, its state at time t can be iterated as

[0128] {x} t ={x} t-1 +f(x)·δt

[0129] Where δt is the integration time step.

[0130] In order to realize the autonomous motion of the robot arm, f(.) should be an autonomous dynamic system that continuously drives the robot arm to converge to the target state, satisfying lim t→∞ f(x) = 0. The most direct system to achieve this goal is a linear dynamic system, which is defined as follows

[0131] f(x)=-k(xx a )

[0132] Where k is the proportional coefficient; x a is the target state, serving as the attractor of the dynamic system.

[0133] Driven by the dynamic system, the motion of the robot arm iterates over time and eventually converges to a unique equilibrium state. In this embodiment, unless otherwise specified, the default dynamic system proportional coefficient k is 1.

[0134] S1.2. Dynamic Online Planning

[0135] In this embodiment, all obstacles are defined as convex bodies, and the distance function Γ(x) between the outside world and the obstacle satisfies C′ continuity. Assuming the robotic arm end effector is considered a "point," but as a non-point mass object, that is, a shape and volume, by extending the obstacle boundary, the end effector's motion can be considered as point motion. Based on the relationship between the point and the obstacle, the motion area is divided into the following three categories, as shown in 2a:

[0136] Free point:

[0137] Boundary points:

[0138] Interior points:

[0139] Where Γ(x) varies with the obstacle center or reference point x o，r The distance between the point and the obstacle increases monotonically. The reference direction r between the point and the obstacle is defined as

[0140] r(x)=(xx o，r ) / ||xx o，r ||

[0141] Where, ||r(x)||=1.

[0142] Define the normal vector at the intersection point (i.e., boundary point) of the reference direction and the extended boundary as n(x), as follows: Figure 2 As shown in a in , the reference direction vector r(x) and the normal vector n(x) satisfy the relationship

[0143] <r(x),n(x)> >0

[0144] Based on dynamic system drive, the online planning of point motion can draw on the Mill-Thomson circle theorem in advanced fluid mechanics and the principle of flow around a cylinder in fluid mechanics. By introducing a dynamic modulation matrix into the dynamic system f(x), it can be used for obstacle avoidance and flow around motion.

[0145] The dynamic system after introducing the modulation matrix is:

[0146]

[0147] Where M(x) is the modulation matrix, defined as

[0148] M(x)=E(x)D(x)E(x) -1

[0149] Where E(x) is the direction basis matrix, defined as

[0150] E(x)=[r(x) e1(x) … e d-1 (x)]

[0151] Where e(.) is the orthogonal vector forming the tangent plane, that is<n(x),e(x)> = 0. D(x) is the eigenvalue matrix, which defines the stretching length in each orthogonal basis direction.

[0152] D(x)=diag(λ r (x) λ e (x) … λ e (x))

[0153] The eigenvalues ​​are set to

[0154]

[0155] Where, is the obstacle rejection factor, which defaults to 1 in this embodiment.

[0156] The normal velocity component at any boundary point is 0, the motion trajectory does not interfere with the obstacle, and the motion trajectory has global convergence. Figure 2 Figure b shows a workspace with a static obstacle. Assuming a feasible solution exists in the workspace, according to the principle of flow around the obstacle, any trajectory starting from any starting point outside the obstacle will converge to the target state. The streamlines in the figure, with arrows indicating the direction of motion at different locations in the workspace, demonstrate that all paths meet the requirements for obstacle avoidance and convergence.

[0157] S2. Dual-arm collaborative planning based on dynamic system:

[0158] The movement of two arms is different from that of a single arm and cannot be simply regarded as point movement. In addition to considering the geometric properties and dynamic properties of the obstacle, the geometric interference between the two arms and the obstacle also needs to be considered. In addition, the coordinated movement of the two arms also adds the relative position / posture constraints between the left and right arms (kinematic coupling constraints), so it is also necessary to consider how to collaboratively avoid obstacles. Although it is feasible to simply expand the obstacle boundary, surround the two arms with a circle or a sphere, and regard the movement of the two arms as point movement, it consumes a lot of space and does not meet actual needs. However, if the shape of the two arms and the obstacle is directly substituted into the planning process, the calculation dimension will be greatly increased. Therefore, in traditional planning methods, whether single-arm or multi-arm, especially in dynamic scenes, most obstacles are simplified to point objects without considering geometric factors.

[0159] This section will first model the dual-arm motion and establish a dual-arm centralized inverse kinematics model; then, considering the motion coupling constraints between the two arms and the geometric properties of the two arms and obstacles, an equivalent simplified model for dual-arm planning will be established. Based on the dynamic system planning theory, a dual-arm dynamic collaborative planning method will be proposed.

[0160] S2.1. Centralized inverse kinematics solution model:

[0161] Consider Figure 3 In the scenario shown, two arms work together to manipulate the same object. In this system, the number of degrees of freedom of a single arm is defined as n. i , where i = [l, r], indicating the subscripts of the left and right arms. x = [x1, x r ] T is the generalized coordinate system of the robot end effector. If there is no superscript, the coordinate system is expressed in the global base system ∑base by default.

[0162] In dual-arm collaborative motion, such as palletizing and handling, the dual arms must satisfy motion coupling constraints at the position and velocity levels.

[0163] The position-level constraint can be directly expressed as

[0164]

[0165] Among them, T obj , T1, T r is the homogeneous transformation matrix of the object, left arm and right arm relative to the global basis system, is the homogeneous transformation matrix of the left arm and the right arm relative to the object coordinate system;

[0166] The grasping matrix of the coordinated motion of the two arms can be expressed as

[0167]

[0168] in, To capture the matrix, I3, is the three-dimensional unit matrix and the three-dimensional zero matrix, are antisymmetric factors, r1, r r is the position vector between the left and right arm end coordinate systems and the contact point of the object;

[0169] According to rigid body kinematics, the velocity level constraint of the coordinated motion of the two arms can be obtained as follows:

[0170]

[0171] in, is the Jacobian matrix, is the linear velocity and angular velocity matrix of the double-arm end coordinate system, is the joint angular velocity of both arms, is the linear velocity and angular velocity matrix of the object;

[0172] According to equations (2) and (3), when the motion trajectory of the object in the task space is known, the dual-arm trajectory can be calculated through velocity level constraints, and then the inverse kinematics solution is performed based on the damped least squares method, thereby mapping the object's motion to the dual-arm joint space, thereby forming an object-oriented centralized inverse kinematics solution model.

[0173] S2.2. Dual-arm dynamic collaborative planning method:

[0174] If the shape contours of the two arms and obstacles are directly substituted into the planning process, the calculation dimension will be greatly increased. Therefore, the planning model is first simplified to reduce the calculation dimension, and then a two-arm collaborative planning method is proposed based on the simplified model.

[0175] Figure 4 The actual kinematic relationship between the two arms and the obstacle, along with the corresponding equivalent simplified relationship, is demonstrated. Due to the coupling between the two arms, a rectangular bounding box is used to minimize the boundaries between the two arm tips and the manipulated object. This allows the kinematic chain between the two arm tips to be simplified to a single link. By extending the obstacle boundary, the link can be further simplified to a "line segment." The relationship between the two arms and the obstacle is now simplified to the relationship between the "line segment" and the extended obstacle boundary. In this simplified model, the geometric properties of the obstacle are fully preserved. The geometric properties of the two arms are characterized by their shape information, which is reflected in the extended obstacle boundary, while their position and posture are expressed through the motion of the line segment. Furthermore, the motion velocities of the two arms and the obstacle can be fully described. Therefore, this simplified model can serve as an equivalent simplified model that considers the geometric and dynamic properties of the two arms and the obstacle.

[0176] In order to achieve dual-arm following dynamic targets and collaborative obstacle avoidance, multiple control points xi and their corresponding weights ω are introduced on the simplified dual-arm model. iThe movement of each control point can be regarded as a point motion, which serves as a local guide and a response to obstacle interference. Then, through motion synthesis, dynamic obstacle avoidance and target convergence of the two arms can be achieved.

[0177] Theoretically, there is no limit to the number of control points, but computational efficiency decreases as the number increases. Control points are arranged using a symmetrical pattern based on the spacing between the arms. The minimum number of control points can be determined empirically or through testing, depending on the shape and motion of the obstacle. The goal is to ensure that the arms do not locally interfere with the obstacle during interaction.

[0178] S2.2.1. Motion synthesis of control points:

[0179] The motion synthesis strategy of this embodiment adopts an object-oriented approach. i The motion x of the object being manipulated obj Determine that the object's movement can be captured by the grab matrix Mapped to each control point of the arms Then The modulated motion of the dynamic system established by inputting the velocity as the control point Map the reverse direction to the object to synthesize the final movement of the object Figure 5 The motion streamline diagram of the three control points of the two arms in a single obstacle scenario is shown, which shows that it has the tendency of collaborative obstacle avoidance and convergence.

[0180] In particular, the control point x facing the object obj The dynamic system and the remaining control points x i The dynamic system is different. Taking the 2D scene as an example, the other control points x i For a dynamic system, we only need to consider the position, that is, x i =[p x , p y ,0], and the control point x obj The dynamic system also needs to consider the posture, that is, x obj =[p x , p y ,θ z ]. The reason is that the target pose is the control point x obj The target state of the control points is the target state of the target state, while the other control points only guide the local motion process. obj The relative position relationship between them is used to achieve motion synthesis, so there is no need to consider the posture.

[0181] The specific synthesis process is as follows: the motion of each control point is independently modulated using Equation (1). To ensure the coordinated constraints of the two arms, the velocity of each control point needs to be synthesized into the object, and then mapped to the two arms using the centralized inverse solution. Since it is not a point motion, the synthesized object motion will have changes in position and posture. Figure 5 The motion of the object is decomposed into linear velocity and angular velocity, showing the relationship between the linear velocity, angular velocity and the velocity of each control point. The motion of each control point combined into the object satisfies the following form

[0182]

[0183] Where, is the linear velocity and angular velocity of the object after synthesis, is the linear velocity of the object and each control point after modulation, is the angular velocity of the object after modulation (the other control points only have linear velocity). obj.pos 、x i.pos is the current position of the object and each control point, ω obj 、ω i is the weight of the object and each control point;

[0184] The attitude of an object is determined by its angular velocity However, due to the small distance between each control point and the object, the synthetic motion has little effect on the object's posture, which may cause the problem of not being flexible enough to deal with obstacles. Therefore, this embodiment introduces the angular velocity gain coefficient κ R，i , in order to enhance the flexibility of the object's posture response in the coordinated motion, the object's angular velocity synthesis will satisfy the following form

[0185]

[0186] In free space without obstacles, the angular velocity gain coefficient κ R，i It is usually set to 1. Since the motion between each control point of the two arms and the manipulated object can be mapped to each other through the grasping matrix, formula (4) can be simplified to

[0187]

[0188] Where, is the speed of each control point after modulation, is the object speed after synthesis, G i The grasping matrix that maps each control point to the center of the object.

[0189] During the movement, since the relative distance between each control point and the obstacle is different, each control point will be adjusted by its corresponding weight ω i (including ω obj, the same below, which means that each control point has Wi, including the middle obj point) to determine the size of the influence. The weight of each control point ω i The distance Γ(x i ) is related to satisfy the following formula

[0190]

[0191] ω i =ω i / ∑ω i

[0192] Where, Γ min is the minimum threshold of the control point distance from the obstacle boundary, satisfying 1≤Γ min <Γ cut .

[0193] In particular, if all control points satisfy Γ(x i )>Γ cut , which means that all control points are not within the influence range of obstacles, and the movement of the arms will be unrestricted. In this case, the weights ω of all control points i are all set to 1 to speed up convergence.

[0194] S2.2.2 Distance Function and Multi-Obstacle Scenario:

[0195] When the two arms are in coordinated motion mode, the motion coupling constraint will reduce the working space of the two arms. Therefore, in order to avoid obstacles while getting as close to them as possible and utilizing more working space, this embodiment uses an exponential form to calculate the distance between the control point and the boundary. The distance function formula is defined as follows:

[0196]

[0197] in, The control point is the boundary point of the obstacle o, satisfying Where b>0,κ dist is the distance magnification factor (κ dist ≥1);

[0198] Amplification factor κ dist The role of is to determine the range of influence of obstacles. When the magnification factor is small, the movement of the control point will be close to the obstacle boundary. However, once the control point approaches the obstacle boundary, the boundary distance function will increase exponentially, and vice versa.

[0199] In a multi-obstacle scenario, the weight of each control point still depends on the distance between it and the obstacle. i The shortest distance Γ(x i) min Calculate, satisfy the following

[0200] T(x i ) min =MinΓ o＝1:N (x i )

[0201]

[0202] Among them, Γ o＝1:N (x i ) is the set of distances between the control point and all obstacles.

[0203] Figure 6 The streamline diagram of the dual arms in a multi-obstacle scenario shows that, given a feasible path, the motion synthesized by the control points can achieve coordinated obstacle avoidance and converge toward the target.

[0204] S2.2.3, velocity symmetry phenomenon:

[0205] For the coordinated motion of two arms, when the velocities of the control points on both sides form a symmetrical distribution, there will be a special case, such as Figure 7 As shown, the synthesized object motion will still interfere with obstacles. Figure 7 The a in the figure shows the velocity symmetry phenomenon. At this time, the synthesized motion will drive the arms to approach the obstacle until interference occurs, such as Figure 7 This embodiment will analyze this special case and provide corresponding solutions.

[0206] This embodiment uses Figure 8 Let's take the scene shown in a as an example for analysis. is the expression of the left and right control points in the obstacle coordinate system, then the velocity symmetry phenomenon satisfies the following conditions:

[0207]

[0208] in, is the expression of each control point in the obstacle coordinate system, v i，x 、v i，y is the projection velocity of the control point i on the x and y axes, i∈{left, right}, Γ(x left ),Γ(x right ) are both in Γ min With Γ cut between, that is, within the influence range of the obstacle.

[0209] The reason for this phenomenon is that the motion streamlines of the control points on both sides are distributed on both sides of the obstacle, which leads to a symmetrical distribution of the speeds of the control points on both sides.obj There is still a component of motion close to the obstacle, e.g. Figure 7 To address this issue, this embodiment adopts a strategy of local guidance of virtual targets to unify the motion streamlines on both sides of the obstacle, as shown in Figure 8 shown.

[0210] The specific process is that when the motion of the control points on both sides satisfies the condition of formula (5), the virtual target will replace the global target for local guidance, such as Figure 8 As shown in a in Figure 1, as the target changes, the motion streamline trend of the entire workspace will be updated, and the motion directions on both sides of the obstacle will be unified. obj The arms will be driven to converge towards the virtual target, so as not to interfere with obstacles. When the relative distance between the object position and the virtual target is less than the set distance threshold, the virtual target will be cancelled and the local guidance will be updated to the global guidance, such as Figure 8 As shown in b.

[0211] Regarding the setting of the virtual target, the method of this embodiment is to set it on the side of the obstacle close to the current object, and the corresponding position is,

[0212]

[0213] Among them, x obs，x 、x obs，y is the coordinate of the obstacle center, l obs，x The length of the obstacle in the x direction. The default horizontal axis is x, the vertical axis is y, and r 1 / r，x is the length between the center of the object and the left or right end control point. It should be noted that this embodiment considers the simple symmetrical distribution under a single static obstacle.

Claims

1. A dual-arm collaborative planning method based on a dynamic system, characterized in that: The method comprises: Step 1: Establish a simplified model of the two arms, which is established based on the geometric attributes and dynamic properties of the two arms and obstacles; Step 2: Multiple control points and their corresponding weights are introduced into the simplified dual-arm model. The motion of each control point is considered as point motion, which is used for local guidance and obstacle avoidance. Dynamic obstacle avoidance and target convergence of the dual arms are achieved through motion synthesis. The motion synthesis specifically includes: obtaining the modulated motion of each control point; synthesizing the velocity of each control point into the object, solving it using centralized inverse kinematics and then mapping it to the two arms; The velocity of each control point is synthesized to satisfy the following equation: Where, is the linear velocity of the object before modulation, 、 are the linear velocity and angular velocity of the synthesized object respectively, 、 are the linear velocities of the modulated object and each control point, is the angular velocity of the object after modulation, 、 are the current positions of the object and each control point respectively, 、 are the weights of the object and each control point respectively; For the coordinated movement of both arms, when the speeds of the control points on both sides form a symmetrical distribution, let 、 are the expressions of the left and right control points in the obstacle coordinate system respectively. When the motion of the control points on both sides satisfies the following formula, the virtual target will replace the global target for local guidance. The following formula is: in, 、 are the projection velocities of the control point i on the x and y axes respectively, , the distance from the left and right control points to the obstacle boundary 、 All in and between, that is, within the influence range of the obstacle, where and are the control points on the left and right sides respectively. is the minimum threshold of the control point distance from the obstacle boundary, To determine whether the control point is close to the obstacle boundary distance threshold, satisfy ; The virtual target is set to the side of the obstacle close to the current object, corresponding to the position: in, 、 are the coordinates of the obstacle center, The length of the obstacle in the x direction. The default horizontal axis is x and the vertical axis is y. The distance between the center of the object and the left or right control point.

2. The dual-arm collaborative planning method based on a dynamic system according to claim 1, characterized in that: In step 1, the establishment of a simplified dual-arm model specifically includes: using a rectangular bounding box to minimize the encirclement of the dual-arm ends and the manipulated object, simplifying the kinematic chain between the dual-arm ends into a connecting rod, and then simplifying the connecting rod into a line segment by extending the obstacle boundary.

3. The dual-arm collaborative planning method based on a dynamic system according to claim 1, characterized in that: When the distance between each control point and the object is less than the preset value, the object's angular velocity synthesis will satisfy the following formula: in, is the angular velocity gain coefficient.

4. The dual-arm collaborative planning method based on a dynamic system according to claim 1, characterized in that: The weight of each control point The distance to the obstacle boundary Related, satisfying the following formula: If all control points satisfy , the weights of all control points Both are 1.

5. The dual-arm collaborative planning method based on a dynamic system according to claim 4, characterized in that: The distance function formula from each control point to the obstacle boundary is: in, is the control point position, For control points on obstacles Boundary point of the boundary , is the distance magnification factor, ; In a multi-obstacle scenario, the weight of each control point The shortest distance to all obstacles Calculate and satisfy the following formula: in, is the set of distances between the control point and all obstacles.

6. The dual-arm collaborative planning method based on a dynamic system according to claim 1, characterized in that: The centralized inverse kinematics solution specifically includes: The two arms work together to manipulate the same object. In this system, the number of degrees of freedom of a single arm is defined as ,in , indicating the subscripts of the left and right arms; Is the position of the left and right arm end coordinate system; if there is no superscript, the coordinate system defaults to the global base system Next expression; In the dual-arm coordinated motion, the dual arms satisfy the motion coupling constraints at the position level and velocity level. The position level constraint is expressed as: in, 、 、 are the homogeneous transformation matrices of the object and left and right arms relative to the global basis system, 、 are the homogeneous transformation matrices of the left and right arms relative to the object coordinate system; The grasping matrix of the coordinated motion of the two arms is expressed as: in, To grab the matrix, 、 are the three-dimensional unit matrix and the three-dimensional zero matrix respectively, is the antisymmetric factor, 、 are the position vectors between the coordinate systems of the left and right arm ends and the contact points of the object, respectively; According to rigid body kinematics, the velocity level constraint of the coordinated motion of the two arms can be obtained as: in, is the Jacobian matrix, is the linear velocity and angular velocity matrix of the left and right arm end coordinate systems, is the joint angular velocity of the left and right arms, is the linear velocity and angular velocity matrix of the object; According to the position-level constraint expression and the velocity-level constraint expression of the coordinated motion of the two arms, when the motion trajectory of the object in the task space is known, the trajectory of the two arms is calculated through the velocity-level constraint, and then the inverse kinematics solution is performed based on the damped least squares method, so that the motion of the object is mapped to the joint space of the two arms, thereby forming an object-oriented centralized inverse kinematics solution model.

7. A computer device comprising a memory and a processor, wherein a computer program is stored in the memory, wherein: When the processor runs the computer program stored in the memory, the steps of the method according to any one of claims 1 to 6 are performed.

8. A computer-readable storage medium, characterized in that The computer-readable storage medium stores a plurality of computer instructions, and the plurality of computer instructions are used to enable a computer to execute the method according to any one of claims 1 to 6.

9. An electronic device, characterized in that: include: at least one processor; as well as, a memory communicatively connected to the at least one processor; wherein, The memory stores instructions that can be executed by the at least one processor, and the instructions are executed by the at least one processor to enable the at least one processor to perform the method according to any one of claims 1 to 6.

Images