A dual-arm cooperative motion planning method and system for closed-chain singularity avoidance

Abstract

The application discloses a double-arm cooperative motion planning method and system for closed-chain singularity avoidance, and comprises the following steps: acquiring a starting point and an ending point of a carried object; determining joint paths and Jacobians of a closed-chain system according to the starting point and the ending point of the carried object; determining a singular region part of the joint paths of the closed-chain system according to the Jacobians of the closed-chain system; determining a master arm and a slave arm of the closed-chain system for the singular region part, determining joint speeds of the master arm based on a damped least square pseudo-inverse method of Jacobians; determining joint speeds of the slave arm according to the joint speeds of the master arm; integrating the joint speeds of each arm to obtain joint angle change values of each arm; and replacing the singular region part in the joint paths by the joint angle change values of each arm to obtain a planning path of the closed-chain system without singularity. The singularity of the closed-chain system is avoided, and the certainty of the joint solution of the closed-chain system at the singularity and the solution of the carried object in the task space are ensured.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of dual-arm cooperative motion planning, and particularly relates to a dual-arm cooperative motion planning method and system for closed-chain singularity avoidance. BACKGROUND

[0002] The statements in this section merely provide background information related to the present application and do not necessarily constitute the prior art.

[0003] Compared with single-arm robots, dual-arm robots have better load capacity. Due to the increase in degrees of freedom and the number of end effectors, dual-arm robots can complete more tasks that single-arm robots cannot complete, such as dual-arm cooperative object carrying work. Therefore, dual-arm robots have more extensive applications in the manufacturing industry and other fields.

[0004] When a dual-arm robot performs a carrying task, motion planning for the robot according to environmental constraints is an extremely important step. Motion planning needs to obtain a collision-free path for the carrying task to ensure the safety of the carried object and the robot body. For dual-arm cooperative carrying motion planning, existing work mainly falls into two categories: planning methods in joint space and planning methods in task space. However, when the environmental constraints are complex, direct planning in joint space cannot guarantee that the running path of the carried object meets the obstacle constraints in the environment, and motion planning methods in task space are often used. Motion planning methods in task space mainly set a master arm and a slave arm of the dual-arm robot, first plan a path for the master arm end, and then calculate a continuous path for the slave arm end and the carried object according to the kinematic constraints of dual-arm cooperation to complete the motion planning task.

[0005] During the process of carrying objects by dual-arm robots, there are strict kinematic constraints at the end of the dual arms, and the dual arms and the carried object form a closed chain. Similar to a single robot arm, the closed chain also has singular points. Within the workspace range of the closed chain, singular points are singular points of the end effector of the closed chain system, which are caused by special joint configurations and result in the loss of a certain direction freedom of the end coordinate system in the Cartesian coordinate system. Zhou et al. proposed fixing the object by one robot arm at the singular point, changing the grasping configuration of the other robot arm, so that the closed chain system in the new configuration is non-singular. This kind of re-planning method effectively avoids the situation of no solution at the singular point, but reduces the planning efficiency. The occurrence of singular points is related to the joint configuration and the position of the end coordinate system of the closed chain. Park et al. changed the position of the end coordinate system to increase the redundancy of the system, so as to change the Jacobian structure of the closed chain system and realize the traversal of the singular point of the closed chain. When the working environment is relatively simple, the singular point traversal method can effectively avoid re-planning or re-grasping at the singular point, and improve the planning efficiency. However, in the singular point traversal method of the closed chain, the position of the end coordinate system is changed, so the path of the carried object in the task space cannot be determined near the singular point of the closed chain, which may cause the error between the planned path and the expected path in this area to be too large, and cannot meet the obstacle constraints in the environment. SUMMARY

[0006] In order to solve the above problems, the present application provides a dual-arm cooperative motion planning method for avoiding singular points of a closed chain and a system thereof, which realizes the avoidance of singular points of a closed chain system and ensures the certainty of joint solutions of the closed chain system and solutions of a carried object in a task space at singular points.

[0007] To achieve the above object, the present application adopts the following technical solutions:

[0008] In a first aspect, a dual-arm cooperative motion planning method for avoiding singular points of a closed chain is provided, comprising:

[0009] obtaining a starting point and a terminal point of a carried object;

[0010] determining a joint path and a Jacobian of the closed chain system according to the starting point and the terminal point of the carried object;

[0011] determining a singular region part of the joint path of the closed chain system according to the Jacobian of the closed chain system;

[0012] for the singular region part, determining a master arm and a slave arm of the closed chain system, determining a joint speed of the master arm based on a damped least squares pseudo-inverse method, determining a joint speed of the slave arm according to the joint speed of the master arm, integrating the joint speeds of the arms to obtain joint angle change values of the arms, and replacing the singular region part in the joint path by the joint angle change values of the arms to obtain a planned path of the closed chain system without singular points.

[0013] When not in the singular region, the joint speed of the dual arms is determined by the inverse mapping of the Jacobian.

[0014] In a second aspect, a dual-arm cooperative motion planning system for closed-chain singular point avoidance is provided, comprising:

[0015] A start and end point acquisition module is configured to acquire a start point and an end point of the object to be carried.

[0016] A Jacobian determination module is configured to determine a joint path of the closed-chain system and a Jacobian according to the start point and the end point of the object to be carried.

[0017] A singular region determination module is configured to determine a singular region part of the joint path of the closed-chain system according to the Jacobian of the closed-chain system.

[0018] A closed-chain system singular point-free planning path determination module is configured to, for the singular region part, determine a master arm and a slave arm of the closed-chain system, determine a joint speed of the master arm based on a damped least square pseudo-inverse method of the Jacobian, determine a joint speed of the slave arm according to the joint speed of the master arm, integrate the joint speeds of the arms to obtain a joint angle change value of each arm, and replace the singular region part in the joint path by the joint angle change value of each arm to obtain a closed-chain system singular point-free planning path.

[0019] In a third aspect, an electronic device is provided, comprising a memory and a processor, and computer instructions stored in the memory and running on the processor, when the computer instructions are run by the processor, the steps of a dual-arm cooperative motion planning method for closed-chain singular point avoidance are completed.

[0020] In a fourth aspect, a computer readable storage medium is provided for storing computer instructions, when the computer instructions are executed by a processor, the steps of a dual-arm cooperative motion planning method for closed-chain singular point avoidance are completed.

[0021] Compared with the prior art, the beneficial effects of the present application are:

[0022] 1. The method disclosed in the present application, after obtaining the joint path of the closed chain system, the singular region in the joint path is determined, and for the part in the singular region, the master arm and the slave arm of the closed chain system are determined, the joint speed of the master arm is determined based on the damped least square Jacobian pseudo-inverse method, the joint speed of the slave arm is determined according to the joint speed of the master arm, the joint angle change value of each arm is obtained by integrating the joint speed of each arm, the singular point of the closed chain is avoided by replacing the singular region part in the joint path, the certainty of the joint solution of the closed chain system at the singular point and the object to be carried in the task space is ensured, and the problem of reduced planning efficiency caused by re-planning is avoided.

[0023] 2. When the path planning is performed according to the damped least square Jacobian pseudo-inverse method, not only the singular point of the closed chain system is avoided, but also the error between the actual running path of the object to be carried and the expected path is minimized.

[0024] The advantages of the additional aspects of the present application will be partially given in the following description, partially become obvious from the following description, or be known by the practice of the present application. BRIEF DESCRIPTION OF DRAWINGS

[0025] The drawings accompanying the specification of this application form a part thereof, serve to provide further understanding of the application, and together with the description of the exemplary embodiments of the application and explanations thereof serve to explain the application, and do not constitute improper limitations on the application.

[0026] Figure 1 The overall flowchart of the method disclosed in embodiment 1 is disclosed;

[0027] Figure 2 The basic coordinate system schematic diagram of the dual-arm closed chain system disclosed in embodiment 1 is disclosed;

[0028] Figure 3 The path planning flowchart in the task space disclosed in embodiment 1 is disclosed;

[0029] Figure 4 The closed chain singular point avoidance algorithm flowchart based on the damped least square Jacobian pseudo-inverse method disclosed in embodiment 1 is disclosed. DETAILED DESCRIPTION

[0030] The present application will be further described below in combination with the drawings and embodiments.

[0031] It should be pointed out that the following detailed description is exemplary and is intended to provide further explanation of the present application. Unless otherwise specified, all technical and scientific terms used herein have the same meaning as generally understood by those skilled in the art to which the present application belongs.

[0032] Embodiment 1

[0033] In this embodiment, a dual-arm cooperative motion planning method for closed-chain singularity avoidance is disclosed, as shown in the formula (1), comprising: Figures 1-4

[0034] S1: obtaining the start point S s and the end point S e of the carried object.

[0035] S2: determining the joint path and Jacobian of the closed-chain system according to the start point and the end point of the carried object.

[0036] The process of determining the joint path of the closed-chain system is as follows:

[0037] determining the path of the dual-arm closed-chain system in the task space according to the start point and the end point of the carried object;

[0038] determining the joint path of the closed-chain system according to the path of the dual-arm closed-chain system in the task space and the closed-chain kinematics model.

[0039] determining the Jacobian of the closed-chain system according to the joint path of the closed-chain system.

[0040] As shown in the formula (1), the process of determining the path of the dual-arm closed-chain system in the task space comprises: Figure 3

[0041] S21: decomposing the start point and the end point into position vectors and attitude vectors respectively.

[0042] The start point S s and the end point S e obtained in this embodiment are both six-dimensional pose vectors of the carried object in the task space, and the six-dimensional pose vectors of the carried object in the task space are decomposed into two three-dimensional vectors of position and attitude.

[0043] S=P⊕R (1)

[0044] In the formula, S is a six-dimensional pose vector, P is a three-dimensional position vector, and R is a three-dimensional attitude vector.

[0045] The start point S s and the end point S e are decomposed into position vectors and attitude vectors respectively according to the formula (1).

[0046] P s =[P sx ,P sy ,P sz ] (2)

[0047] R s =[R sx ,R sy ,R sz ] (3)​​

[0048] P e = [P ex , P ey , P ez ] (4)

[0049] R e = [R ex , R ey , R ez ] (5)

[0050] P s is the position vector of the starting point, R s is the pose vector of the starting point, P e is the position vector of the ending point, R e is the pose vector of the ending point.

[0051] S22: Obtain the position path according to the position vector of the start and end points and the position vector of the end point.

[0052] This embodiment takes P s as the starting point and P e as the ending point, and plans the three-dimensional position path in the task space through the RRT algorithm, to obtain the three-dimensional position path R P only about position.

[0053] S23: Determine the pose path according to the pose vector of the start and end points and the pose vector of the end point.

[0054] This embodiment takes the path R P as the basis, and plans the three-dimensional pose vector through the RRT algorithm in the process of traversing the path points according to the path point sequence number, taking R s as the starting point and R e as the ending point, to obtain the feasible pose path R R in the three-dimensional pose space.

[0055] S24: Superimpose the position path and the pose path to obtain the path R of the dual-arm closed-loop system in the task space.

[0056] This embodiment superimposes the three-dimensional position and the three-dimensional pose at each path point to obtain the six-dimensional complete path R of the dual-arm closed-loop system in the task space.

[0057] R = R P ⊕ R R (6)

[0058] Wherein, the closed-loop kinematics model includes the mechanical arm kinematics equation respectively constructed for each single arm in the dual arm through the D-H method and the dual-arm cooperative carrying kinematics constraint.

[0059] The construction process of the closed-chain kinematics model is: determining the basic coordinate system of the dual-arm closed-chain system, and establishing the kinematics model of the single mechanical arm through the D-H method, taking the left arm base coordinate system as the world coordinate system, and determining the dual-arm cooperative carrying kinematics constraint:

[0060] W T LE LE T A ≡ W T R R T RE RE T A (7)

[0061] wherein, W T LE , LE T A , W T R , R T RE , RE T A are respectively the rotation transformation matrixes from the world coordinate system to the left arm end coordinate system, from the left arm end coordinate system to the object coordinate system, from the world coordinate system to the right arm base coordinate system, from the right arm base coordinate system to the right arm end coordinate system, and from the right arm end coordinate system to the object coordinate system.

[0062] The dual-arm closed-chain system coordinate system is shown in Figure 2 , the left arm base coordinate system is L, the right arm base coordinate system is R, the left arm end coordinate system is LE, the right arm end coordinate system is RE, the reference coordinate system W coincides with L, and the carried object coordinate system is A. The following two transformations can be obtained through the kinematics of the left arm and the right arm respectively from the reference coordinate system to the carried object coordinate system:

[0063] W T A = W T LE LE T A (8)

[0064] W T A = W T R R T RE RE T A (9)

[0065] In the formula, W T A is the conversion matrix from the reference coordinate system to the carried object coordinate system.

[0066] The two-arm cooperative carrying kinematics constraint can be obtained by combining formula (8) and formula (9):

[0067] W T LE LE T A ≡ W T R R T RE RE T A

[0068] The constraint is always present when the two arms form a closed chain with the carried object, ensuring that the relative pose of the two-arm end remains unchanged.

[0069] According to the path of the two-arm closed chain system in the task space and the two-arm cooperative carrying kinematics constraint, the pose matrix of the two-arm end at each path point on the path in the task space is determined; the rotation transformation matrix of the two arms from the respective base coordinate system to the end coordinate system is obtained according to the two-arm end pose matrix, and then the joint angle of the closed chain system at each path point in the task space is obtained by solving the kinematics equation of the mechanical arm through the analytical method, and each joint angle constitutes the joint path of the closed chain system.

[0070] According to the joint path of the closed chain system, the Jacobian of each single arm is determined, and then the Jacobian of the closed chain system is determined according to the Jacobian of the two single arms.

[0071] The Jacobian of the two-arm closed chain system is constructed through the velocity vector superposition principle, specifically:

[0072] When the two mechanical arm ends contact to form a closed chain, the origins of the two mechanical arm end coordinate systems coincide, at this time the velocities of the two mechanical arm ends in the task space are W v A ∈R 6×1 . Let the velocities of the left and right arm ends be W v LE ∈R 6×1 and W v RE ∈R 6×1 , then the relationship between them and W v A is:

[0073]

[0074] Considering the actual situation, the object carried by the two arms must have a volume, and there is a certain velocity mapping relationship between the velocities of the two arm ends and W v A , then the actual mapping relationship between the carried object and the velocity in the joint space of the closed chain system is:

[0075]

[0076] wherein X L ∈R 6×6 and X R ∈R 6×6 are the mapping matrices of the left and right arm end-effector velocities and the center of mass velocity of A, respectively, J L ∈R 6×6 and J R ∈R 6×6 are the Jacobians of the left and right arms, respectively, is the joint velocity of the closed-chain system. According to (11), the Jacobian J of the closed-chain system is obtained as:

[0077]

[0078] S3: Determine the singular region part of the joint path of the closed-chain system according to the Jacobian of the closed-chain system.

[0079] According to the Jacobian of the closed-chain system and the Jacobian of each arm in the dual-arm, the singular region part of the joint path of the closed-chain system is determined, and the discrimination condition of whether the dual-arm closed-chain system is in the singular region is:

[0080]

[0081] As shown in Figure 4 , according to the Jacobian corresponding to each joint angle in the joint path, it is judged whether the joint angle is in the singular region, when the Jacobian corresponding to the joint angle satisfies formula (13), it is determined that the joint angle is in the singular point, otherwise, it is not in the singular point.

[0082] S4: For the singular region part, determine the master arm and the slave arm of the closed-chain system, determine the joint velocity of the master arm based on the Jacobian pseudo-inverse method of the least square damping, determine the joint velocity of the slave arm according to the joint velocity of the master arm, integrate the joint velocities of the arms to obtain the joint angle change values of the arms, and replace the singular region part in the joint path by the joint angle change values of the arms to obtain the planning path of the closed-chain system without singular points.

[0083] When in the singular region, the master arm and the slave arm of the closed-chain system are determined according to the Jacobian of each arm and the rank of the Jacobian of each arm. The joint velocity of the master arm is determined based on the Jacobian pseudo-inverse method of the least square damping, wherein the norm of the error between the actual velocity and the expected velocity of the end-effector of the robot arm is minimized as the target, and the norm of the change of the joint velocity of the robot arm is less than or equal to the set threshold as the constraint, and the method of sequential quadratic programming is used to determine the damping factor in the Jacobian pseudo-inverse method of the least square damping.

[0084] The mapping relationship between joint velocity and end velocity in the damped least square based pseudo-inverse Jacobian method is used to determine the joint velocity of the master arm, and the mapping relationship between joint velocity and end velocity in the damped least square based pseudo-inverse Jacobian method is specifically as follows:

[0085]

[0086] Wherein, Δx and Δq are first-order differentials of end pose and joint position respectively, I n is an n-dimensional unit matrix, and φ is an arbitrary n-dimensional column vector, is a damped least square pseudo-inverse Jacobian:

[0087]

[0088] Wherein, λ is a damping factor. For optimization based on the damped least square pseudo-inverse Jacobian method, an adaptive damping factor is used, and the change mode is as follows:

[0089]

[0090] σ d is a damping factor, and a sequential quadratic programming method is used to optimize the optimal damping factor to determine the final damping factor. The damping factor makes the norm of the error between the actual velocity of the end of the mechanical arm and the expected velocity minimum, and the norm of the change of the joint velocity of the mechanical arm is less than or equal to a set threshold.

[0091] The optimization of the damping factor is expressed as a quadratic programming problem:

[0092]

[0093] Wherein, f(σ d ) is a nonlinear objective function, representing the norm of the error between the actual velocity of the end of the mechanical arm and the expected velocity; g(σ d ) is a nonlinear constraint function, representing the norm of the change of the joint velocity of the mechanical arm, and the value should be less than a given threshold Δ. The expressions of the objective function and the constraint function are as follows:

[0094]

[0095]

[0096] Wherein, is a six-dimensional calculation velocity of the end of the mechanical arm, is a six-dimensional expected velocity in the task space, is a joint velocity.

[0097] According to the joint velocity of the master arm and the slave arm joint velocity update equation, the joint velocity of the slave arm is determined, and the slave arm joint velocity update equation is as follows:

[0098]

[0099] wherein D1 and D2 are singular region judgment coefficients, when the system is located in a singular region, D1 = 1, D2 = 0; λ1 and λ2 are master-slave arm judgment coefficients, when the left arm is the master arm, λ1 = 1, λ2 = 0, and vice versa, λ1 = 0, λ2 = 1; and are joint velocities of the left and right arms respectively, and are Jacobian pseudo-inverses of the left and right arms based on the damped least square method respectively, and are slave arm Jacobian pseudo-inverses obtained according to the approximate solution of the master arm.

[0100] For example, when the left arm is determined as the master arm and the right arm as the slave arm, the joint velocity of the left arm is first determined, and then the joint velocity of the right arm is determined according to the joint velocity updating equation of the slave arm based on the joint velocity of the left arm, at this time, the slave arm joint velocity updating equation is:

[0101]

[0102] Through the above algorithm, the joint velocity at the closed-chain singular point is calculated, when the time interval between adjacent path points is determined, the change value of the joint angle is obtained by integrating the joint velocity, and the singular region part in the joint path is replaced through the change value of the joint angle, to obtain the planned path of the closed-chain system without singular points, to realize the avoidance of the closed-chain singular point.

[0103] In addition, for the path points not in the singular region, the joint velocities of the dual arms are determined through the inverse Jacobian mapping.

[0104] The Jacobian of each single arm is solved through the inverse Jacobian mapping, and the formula for determining the joint velocity of each single arm is:

[0105]

[0106] The method disclosed in the embodiment is centered on the object to be carried, and the RRT algorithm is used to plan a collision-free path for the object in the task space, the kinematic constraints of the closed-chain system are analyzed, the Jacobian of the closed-chain system is constructed, the closed-chain singular point avoidance algorithm based on the damped least square Jacobian pseudo-inverse method is designed, and the algorithm is applied to the closed-chain motion planning, thereby avoiding re-planning or re-grasping, and ensuring that there is no large error between the actual running path of the object to be carried and the expected path, effectively improving the efficiency of the dual-arm cooperative motion planning, and having practical application value.

[0107] Embodiment 2

[0108] In this embodiment, a dual-arm cooperative motion planning system for closed-chain singularity avoidance is disclosed, comprising:

[0109] A start and end point acquisition module is configured to acquire a start point and an end point of the object to be carried.

[0110] A Jacobian determination module is configured to determine a joint path and a Jacobian of the closed-chain system according to the start point and the end point of the object to be carried.

[0111] A singular region determination module is configured to determine a singular region part of the joint path of the closed-chain system according to the Jacobian of the closed-chain system.

[0112] A singularity-free planned path determination module is configured to, for the singular region part, determine joint velocities of the master arm and the slave arm of the closed-chain system based on a damped least square pseudo-inverse method of the Jacobian of the master arm, determine joint velocities of the slave arm according to the joint velocities of the master arm, integrate the joint velocities of the arms to obtain joint angle change values of the arms, and replace the singular region part in the joint path by the joint angle change values of the arms to obtain a singularity-free planned path of the closed-chain system.

[0113] Embodiment 3

[0114] In this embodiment, an electronic device is disclosed, comprising a memory and a processor, and computer instructions stored in the memory and running on the processor, when the computer instructions are run by the processor, the steps of the dual-arm cooperative motion planning method for closed-chain singularity avoidance disclosed in Embodiment 1 are completed.

[0115] Embodiment 4

[0116] In this embodiment, a computer readable storage medium is disclosed, configured to store computer instructions, when the computer instructions are executed by a processor, the steps of the dual-arm cooperative motion planning method for closed-chain singularity avoidance disclosed in Embodiment 1 are completed.

[0117] Finally, it should be noted that: the above embodiments are only used to illustrate the technical solutions of the present application rather than limit them, although the present application has been described in detail with reference to the above embodiments, those skilled in the art should understand that: the specific embodiments of the present application can be modified or replaced by equivalents without departing from the spirit and scope of the present application, any modification or equivalent replacement thereof should be covered in the protection scope of the claims of the present application.

Claims

1. A dual-arm cooperative motion planning method for closed-chain singularity avoidance, characterized in that, The method comprises the following steps: Obtaining the start point and the end point of the object to be carried; According to the start point and the end point of the object to be carried, the joint path and the Jacobian of the closed chain system are determined; According to the Jacobian of the closed chain system, the singular region part of the joint path of the closed chain system is determined; For the singular region part, the master arm and the slave arm of the closed chain system are determined, the joint speed of the master arm is determined based on the damped least square Jacobian pseudo-inverse method, the joint speed of the slave arm is determined according to the joint speed of the master arm, the joint angle change value of each arm is obtained by integrating the joint speed of each arm, the singular region part in the joint path is replaced by the joint angle change value of each arm, and the planning path of the closed chain system without singular points is obtained; According to the joint speed of the master arm and the slave arm joint speed update equation, the joint speed of the slave arm is determined, wherein the slave arm joint speed update equation is: wherein, and is a singular region determination coefficient, when the system is located in a singular region, ; and is a master-slave arm determination coefficient, when the left arm is a master arm, , on the contrary, ; and are joint velocities of the left and right arms respectively, and are pseudo-inverses of Jacobians of the left and right arms based on a damped least square method respectively, and are pseudo-inverses of Jacobians of the slave arm obtained according to an approximate solution of the master arm respectively, is a unit matrix of dimension n, is an arbitrary column vector of dimension n, and are Jacobians of the left and right single arms respectively.

2. The dual-arm cooperative motion planning method for closed-chain singularity avoidance of claim 1, wherein, According to the start point and the end point of the object to be carried, the path of the dual-arm closed chain system in the task space is determined; According to the path of the dual-arm closed chain system in the task space and the closed chain kinematics model, the joint path of the closed chain system is determined.

3. The dual-arm cooperative motion planning method for closed-chain singularity avoidance of claim 2, wherein, The start point and the end point are respectively decomposed into position vectors and attitude vectors; According to the position vectors of the start point and the end point, the position path is obtained; According to the attitude vectors of the start point and the end point, the attitude path is determined; The position path and the attitude path are superimposed to obtain the path of the dual-arm closed chain system in the task space.

4. The dual-arm cooperative motion planning method for closed-chain singularity avoidance of claim 2, wherein, The closed chain kinematics model comprises the mechanical arm kinematics equation of each single arm constructed by the D-H method and the dual-arm cooperative carrying kinematics constraint.

5. The dual-arm cooperative motion planning method for closed-chain singularity avoidance of claim 1, wherein, Further comprising: For the path points not in the singular region, the joint speed of the dual arms is determined through the inverse Jacobian mapping.

6. The dual-arm cooperative motion planning method for closed-chain singularity avoidance of claim 1, wherein, Taking the norm of the error between the actual speed and the expected speed of the mechanical arm end as the target and taking the norm of the change of the joint speed of the mechanical arm less than or equal to the set threshold as the constraint, the damping factor in the damped least square Jacobian pseudo-inverse method is determined by using the sequential quadratic programming method.

7. A dual-arm collaborative motion planning system for closed-chain singularity avoidance, characterized by, The method comprises the following steps: A start point and an end point acquisition module is configured to obtain the start point and the end point of the object to be carried; A Jacobian determination module is configured to determine the joint path and the Jacobian of the closed chain system according to the start point and the end point of the object to be carried; A planning path singular region determination module is configured to determine the singular region part of the joint path of the closed chain system according to the Jacobian of the closed chain system; A closed chain system planning path without singular points determination module is configured to, for the singular region part, determine the master arm and the slave arm of the closed chain system, determine the joint speed of the master arm based on the damped least square Jacobian pseudo-inverse method, determine the joint speed of the slave arm according to the joint speed of the master arm, integrate the joint speed of each arm to obtain the joint angle change value of each arm, replace the singular region part in the joint path by the joint angle change value of each arm, and obtain the planning path of the closed chain system without singular points; and determine the joint speed of the slave arm according to the joint speed of the master arm and the slave arm joint speed update equation, wherein the slave arm joint speed update equation is: wherein, and are singular region determination coefficients, when the system is located in a singular region, ; and are master-slave arm determination coefficients, when the left arm is a master arm, , on the contrary, ; and are joint velocities of the left and right arms respectively, and are pseudo-inverses of Jacobians of the left and right arms based on a damped least square method respectively, and are pseudo-inverses of Jacobians of the slave arm obtained according to an approximate solution of the master arm respectively, is a unit matrix of dimension n, is an arbitrary column vector of dimension n, and are Jacobians of the left and right single arms respectively.

8. An electronic device, comprising: A computer program product, comprising a memory and a processor, and computer instructions stored on the memory and run on the processor, which, when run by the processor, complete the steps of the dual-arm cooperative motion planning method for closed-chain singularity avoidance according to any one of claims 1-6.

9. A computer-readable storage medium, characterized in that, A computer program product for storing computer instructions, which, when executed by a processor, complete the steps of the dual-arm cooperative motion planning method for closed-chain singularity avoidance according to any one of claims 1-6.

Images