CN114643581B - Double-mechanical-arm collision avoidance track planning method and system based on improved artificial potential field method - Google Patents
Double-mechanical-arm collision avoidance track planning method and system based on improved artificial potential field method Download PDFInfo
- Publication number
- CN114643581B CN114643581B CN202210415041.1A CN202210415041A CN114643581B CN 114643581 B CN114643581 B CN 114643581B CN 202210415041 A CN202210415041 A CN 202210415041A CN 114643581 B CN114643581 B CN 114643581B
- Authority
- CN
- China
- Prior art keywords
- mechanical arm
- mechanical
- potential field
- field
- arm
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 113
- 238000001514 detection method Methods 0.000 claims description 13
- 238000011217 control strategy Methods 0.000 claims description 10
- 239000011159 matrix material Substances 0.000 claims description 6
- 230000008569 process Effects 0.000 claims description 4
- 230000005484 gravity Effects 0.000 claims description 3
- 230000004048 modification Effects 0.000 abstract description 5
- 238000012986 modification Methods 0.000 abstract description 5
- 230000010355 oscillation Effects 0.000 abstract description 5
- 238000004422 calculation algorithm Methods 0.000 abstract description 2
- 230000009471 action Effects 0.000 description 12
- 239000012636 effector Substances 0.000 description 11
- 230000001133 acceleration Effects 0.000 description 6
- 239000000463 material Substances 0.000 description 4
- 230000008859 change Effects 0.000 description 3
- 238000004088 simulation Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 230000009977 dual effect Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 230000002265 prevention Effects 0.000 description 1
- 238000003466 welding Methods 0.000 description 1
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
- B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
- B25J9/1666—Avoiding collision or forbidden zones
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Manipulator (AREA)
Abstract
The invention discloses a double-mechanical-arm collision avoidance track planning method and system based on an improved artificial potential field method, wherein the invention improves the artificial potential field method: in the modified artificial potential field approach, the potential field force is directly related to the robot arm joint velocity. The modification not only reduces the calculated amount of the algorithm, but also solves the problem of oscillation of the mechanical arm at the target. A threshold distance is set in the gravitational field function in the artificial potential field method, and when the distance between the mechanical arm and the target point is larger than the threshold distance, the gravitational force is not increased any more and is equal to a constant. This modification avoids the robot arm being subjected to excessive attraction when it is far from the target point.
Description
Technical Field
The invention relates to the technical field of track planning, in particular to a double-mechanical-arm collision avoidance track planning method and system using an improved artificial potential field method.
Background
The double mechanical arm system has higher flexibility and higher working efficiency than a single mechanical arm. Currently, dual-robot systems have been used in a variety of industrial applications, such as handling, loading, assembly, welding, and the like. However, the two mechanical arms often share the same working space, so that coordinated control is required to be performed on the two mechanical arms so as to avoid collision between the mechanical arms. The coordination control strategy of the double mechanical arm system mainly comprises master/slave coordination control, hybrid position/force control, dynamic coordination control and the like. Wherein the master/slave coordination control is most suitable for being combined with the collision avoidance track planning of the double mechanical arm system. The principle is that two mechanical arms are respectively set as a master mechanical arm and a slave mechanical arm, and the slave mechanical arm avoids collision with the master mechanical arm through modes of time delay action, speed change, working range limitation and the like. The scholars also propose methods of using time scheduling, B-spline interpolation to locally change the mechanical arm track, using heuristic rules to generate the mechanical arm track, and the like for the double mechanical arms. However, many existing methods can affect the working efficiency of the mechanical arm, and some methods are complex in calculation and long in solving time, so that the coordinated movement and collision avoidance track planning of the double mechanical arms are not facilitated.
The principle of the traditional artificial potential field method (TAPF) is to establish a virtual artificial potential field in the motion space of a double mechanical arm, wherein the virtual artificial potential field consists of a gravitational field and a repulsive force field. The gravitational field is set at the target point and the repulsive force field is set at the obstacle. The mechanical arm avoids the obstacle under the action of the repulsive field and reaches the target point of the track under the action of the gravitational field. However, this method has some problems that the potential field force in the conventional artificial potential field method is related to the joint acceleration of the mechanical arm, which causes the mechanical arm to have an acceleration of 0 when reaching the target point, but still has a velocity, thereby causing the mechanical arm to oscillate near the target point. And the gravitation in the traditional artificial potential field method is proportional to the distance from the mechanical arm to the gravitational field, when the mechanical arm is far away from the target point, the potential field gravitation can be large, which may cause collision of the two mechanical arms.
Disclosure of Invention
Aiming at the defects of the prior art, the invention aims to provide the double-mechanical-arm collision avoidance track planning method based on the improved artificial potential field method, which is simple and convenient to calculate and high in solving speed, and the motion track of the double-mechanical-arm system is generated in advance through offline planning, so that the safety is higher.
In order to achieve the above object, the present invention is realized by the following technical scheme:
in a first aspect, the present invention provides a method for planning a collision avoidance trajectory of a double mechanical arm based on an improved artificial potential field method, which comprises the following steps:
step 1: setting a control strategy of the double mechanical arms and setting priorities of the two mechanical arms;
step 2: acquiring size data of the mechanical arms, and establishing a three-dimensional model and a kinematic model of the two mechanical arms according to the acquired size data;
step 3: establishing a collision detection model of the double mechanical arms according to the three-dimensional model and the kinematic model;
step 4: determining all the passing points of the working path of the double mechanical arms, wherein the passing points comprise a starting point, a first middle point, a second middle point and a target point, and completing interpolation fitting among all the passing points to obtain a complete initial track of the two mechanical arms;
step 5: when collision risk is detected between two mechanical arms, an improved artificial potential field method starts to act and modifies the initial track of the slave mechanical arm, the improved artificial potential field method generates a virtual gravitational field and a repulsive force field in a working space of a double mechanical arm system, the gravitational field is arranged at a target point of the slave mechanical arm, a threshold distance is arranged in a gravitational field function, and the repulsive force field is arranged at each joint of the master mechanical arm; the modified trajectory is obtained by improving the artificial potential field method.
As a further technical solution, the master/slave coordination control is used as a control strategy of the double mechanical arms.
As a further technical scheme, in step 2, measuring the size data of the mechanical arm, and establishing a three-dimensional model of the mechanical arm by using software according to the obtained size data; and calculating the size data to obtain a mechanical arm D-H parameter table, and establishing a kinematic model and a URDF file of the double mechanical arm system according to a standard D-H parameter method.
As a further technical solution, the collision detection model in step 3 may determine whether there is a collision risk between the two mechanical arms by calculating the distance from the mechanical arm to the main mechanical arm offline.
As a further technical scheme, in step 4, a linear interpolation function is adopted to fit the distance from the starting point to the first intermediate point and the distance from the second intermediate point to the end point, and a cubic polynomial interpolation function is adopted to fit the track from the first intermediate point to the second intermediate point.
In a second aspect, the present invention further provides a system for planning a collision avoidance trajectory of a dual mechanical arm based on an improved artificial potential field method, including:
module one: the control strategy of the two mechanical arms is formulated, and the priorities of the two mechanical arms are set;
and a second module: the method comprises the steps of acquiring size data of mechanical arms, and establishing a three-dimensional model and a kinematic model of two mechanical arms according to the acquired size data;
and a third module: the method comprises the steps of establishing a collision detection model of a double mechanical arm according to a three-dimensional model and a kinematic model;
and a fourth module: the method comprises the steps of determining all passing points of a double-mechanical-arm working path, including a starting point, a first intermediate point, a second intermediate point and a target point, and completing interpolation fitting among all the passing points to obtain an integral initial track of two mechanical arms;
and a fifth module: the method comprises the steps of using a collision model to detect an initial track offline, when collision risk is detected between two mechanical arms, improving an artificial potential field method to start to act and modifying the initial track of a slave mechanical arm, generating a virtual gravitational field and a repulsive force field in a working space of a double mechanical arm system by the improved artificial potential field method, wherein the gravitational field is arranged at a target point of the slave mechanical arm, a threshold distance is arranged in a gravitational field function, and the repulsive force field is arranged at each joint of a main mechanical arm; the modified trajectory is obtained by improving the artificial potential field method.
As a further technical scheme, the master/slave coordination control is used as a control strategy of the double mechanical arms in the first module.
As a further technical scheme, the second module is configured to measure the size data of the mechanical arm, and establish a three-dimensional model of the mechanical arm by using software according to the obtained size data; and calculating the size data to obtain a mechanical arm D-H parameter table, and establishing a kinematic model and a URDF file of the double mechanical arm system according to a standard D-H parameter method.
As a further technical solution, the collision detection model is configured to determine whether there is a collision risk between the two mechanical arms by calculating the distance from the mechanical arm to the main mechanical arm offline.
As a further technical scheme, the fourth module is configured to fit the distance from the start point to the first intermediate point and the distance from the second intermediate point to the end point by using a linear interpolation function, and fit the track from the first intermediate point to the second intermediate point by using a cubic polynomial interpolation function.
The beneficial effects of the embodiment of the invention are as follows:
the invention provides a double-mechanical-arm collision avoidance track planning method based on an improved artificial potential field method, which has the advantages of good collision avoidance effect, simple structure and easiness in implementation. Firstly, the method provides a collision detection model, the track of the mechanical arm is locally modified after collision is detected to realize collision avoidance, and compared with a collision avoidance method of globally modifying the track, the influence on the working efficiency of the double mechanical arms is reduced. Secondly, the method improves the artificial potential field method, solves the problems of overlarge attraction and terminal oscillation in the artificial potential field method, and reduces the calculated amount. Finally, simulation tests are carried out on the method, and the result shows that the method is fast in convergence and solving, and under the action of the method, the two mechanical arms always keep a safe distance in the moving process.
In the improved artificial potential field method, the potential field force is directly related to the joint speed of the mechanical arm. The modification not only reduces the calculated amount of the algorithm, but also solves the problem of oscillation of the mechanical arm at the target; a threshold distance is set in the gravitational field function in the artificial potential field method, and when the distance between the mechanical arm and the gravitational field is larger than the threshold distance, the gravitational force is not increased any more and is equal to a constant.
Drawings
The accompanying drawings, which are included to provide a further understanding of the invention and are incorporated in and constitute a part of this specification, illustrate embodiments of the invention and together with the description serve to explain the invention.
Fig. 1 is a schematic diagram of an implementation object-gantry dual-mechanical arm loading system of the present embodiment;
FIG. 2 is a schematic view of a three-dimensional model of a robot arm according to the present embodiment;
FIG. 3 is a D-H parameter table of the robot arm of the present embodiment;
fig. 4, 5 and 6 are positional relationships of the geometric bodies in the collision detection model of the present embodiment;
FIG. 7 is a view showing a collision avoidance path of the dual-arm system according to the present embodiment;
fig. 8 shows the distance between the end effectors of the mechanical arm R1 and the mechanical arm R2 in this embodiment.
In the figure: the mechanical arm comprises a mechanical arm 11, a mechanical arm 12, a mechanical arm 13, a conveyor belt 14, a vehicle 15 to be assembled, a portal frame 16, a mechanical arm base 21, a mechanical arm rotating shaft 22, a mechanical arm rotating shaft 23, a mechanical arm rotating shaft 24, a mechanical arm rotating shaft 25, a mechanical arm rotating shaft 26 and a mechanical arm rotating shaft 27.
Detailed Description
It should be noted that the following detailed description is illustrative and is intended to provide further explanation of the invention. Unless defined otherwise, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this invention belongs.
It is noted that the terminology used herein is for the purpose of describing particular embodiments only and is not intended to be limiting of exemplary embodiments according to the present invention. As used herein, the singular forms also are intended to include the plural forms unless the present invention clearly dictates otherwise, and furthermore, it should be understood that when the terms "comprise" and/or "include" are used in this specification, they specify the presence of stated features, steps, operations, devices, components, and/or combinations thereof;
as described in the background art, the present invention provides a method and a system for planning a collision avoidance trajectory of a double mechanical arm based on an improved artificial potential field method in order to solve the above technical problems.
Example 1
Referring to fig. 1, the gantry dual-mechanical-arm loading system shown in fig. 1 is selected as an implementation object of the method of the present invention, and the key devices of the gantry dual-mechanical-arm loading system include a master mechanical arm 11, a slave mechanical arm 12, a mechanical gripper 13, a conveyor belt 14, a vehicle 15 to be loaded, and a gantry 16; the invention provides a double-mechanical-arm collision avoidance track planning method based on an improved artificial potential field method, which comprises the following steps of:
the method comprises the following steps:
step 1: the two mechanical arms are respectively set as a master mechanical arm R1 and a slave mechanical arm R2 according to a master/slave coordination control principle, and the priority of the master mechanical arm is set to be higher than that of the slave mechanical arm. When collision risk between the two mechanical arms is detected, the initial track of the main mechanical arm with high priority is not changed, and the collision between the two mechanical arms is avoided by changing the track of the auxiliary mechanical arm with low priority.
Step 2: mechanical arm type used in this exampleThe number is KUKA KR 270R3100 ultra K. And measuring the dimension data of the mechanical arm, including the shape and the dimension of a large arm, a small arm, a rotating shaft and the like. Establishing a three-dimensional model of the mechanical arm in Solidworks software according to the measured size data, wherein the three-dimensional model of the mechanical arm is shown in FIG. 2; the D-H parameters of the robot arm are calculated according to the dimensions of the robot arm, and the D-H parameter table is shown in fig. 3. Alpha in FIG. 3 i Is the torsion angle of the connecting rod, p i Is the length of the connecting rod, d i Is the torque of the connecting rod, theta i Is the joint variable i=1, 2, …,6. And a kinematic model and a URDF file of the mechanical arm can be established according to the D-H parameter table.
Step 3: in order to conveniently analyze the collision problem, the double-mechanical-arm three-dimensional model is simplified into a geometric model. Consider the end effector of the master arm R2 as having a radius R a The joint of the main mechanical arm R1 is regarded as the spherical region with the radius R b The connecting rod of the main mechanical arm R1 is regarded as the sphere with the length of l b Is a cylinder with a radius r c . The distance between the end effector of the main mechanical arm R2 and the joint and connecting rod of the main mechanical arm R1 is d 1 ,…,d i ]And [ d ] 1 ,…,d j ]The distance between the end effector of the main mechanical arm R1 and the end effector of the main mechanical arm R2 is d k . Referring to fig. 4, 5 and 6 for the positional relationship between the geometric bodies in the collision detection model, R1J, R1L, R E respectively represents the joint region, the link region and the end effector region of the mechanical arm R1, and R2E represents the end effector region of the mechanical arm R2. And calculating the distance between the two mechanical arms at the same moment in the track according to the kinematic model, and considering that the two mechanical arms collide when one of the following conditions in the formula (1) is satisfied.
D in s1 ,d s2 ,d s3 The minimum safe distances from the end effector of the mechanical arm R2 to the joint, the connecting rod and the end effector of the mechanical arm R1 are respectively. D in this example s1 ,d s2 ,d s3 The values of (2) are 0.8m,1.28m, respectively.
Step 4: all the passing points of the working path of the double mechanical arm are determined, wherein the passing points comprise a starting point, a first middle point, a second middle point and a target point. The starting point is at the material grabbing position of the tail end of the conveyor belt, the first middle point is above the material grabbing position, the second middle point is above the material stacking position of the vehicle to be loaded, and the target point is at the material stacking position of the vehicle to be loaded. A linear interpolation function is used to fit the distance from the starting point to the first intermediate point and the distance from the second intermediate point to the end point. A third order polynomial interpolation function is used to fit the trajectory of the first intermediate point to the second intermediate point.
The interpolation function of the cubic polynomial is shown in formula (2).
In the formula, theta (t),respectively represent the joint angle, the joint angular velocity, the joint angular acceleration, a 0 ,a 1 ,a 2 ,a 3 Representing the coefficients of the interpolation function, respectively. And (5) completing interpolation fitting among all the passing points to obtain the complete initial track of the two mechanical arms.
Step 5: and (3) detecting the initial track offline by using a collision model, and when the collision risk between the two mechanical arms is detected, improving the artificial potential field method to start to act and modifying the initial track of the slave mechanical arm. A virtual force field is provided in the joint space of the dual-robot system, which acts only on the robot R2 to change its trajectory. The gravitational field of the improved artificial potential field method is arranged at the target point of the mechanical arm R2, and the gravitational field function is shown as a formula (3):
wherein K is att Is the coefficient of gravity, ρ (q, q g ) Is a vector whose value is the current position q of the mechanical arm to the target point q g Euler's of (A)Distance, the direction of which is from the mechanical arm to the target point position d q Is the distance between the mechanical arm and the target point, r q Is the critical acting distance of the gravitational field, U k Is a constant. The potential field force function of the gravitational field is shown in formula (4):
f in the formula k Is a constant. The repulsive field is arranged at 6 joints of the mechanical arm R1. According to the traditional artificial potential field method, the potential field force applied to the mechanical arm is K att ρ(q,q g ). When the collision risk is just detected, the mechanical arm R2 is far away from the target point, and therefore a large potential field force is applied, which requires that the mechanical arm R2 generate a large acceleration in the moment, and may damage the mechanical arm joint. However, the improved artificial potential field method sets a threshold distance r in the gravitational field function q When the distance between the mechanical arm R2 and the target point is greater than the threshold distance, the potential field force applied to the mechanical arm is constant F k The safe operation of the mechanical arm R2 is ensured.
The repulsive force field function of the improved artificial potential field method is shown in a formula (5):
k in the formula rep Is the repulsive force coefficient, ρ (q, q o ) Is a vector whose value is the current position q of the mechanical arm to the obstacle q o Is directed from the robot arm to the obstacle position, d i Is the distance between the joints of the mechanical arm R2 and the mechanical arm R1, R s Is the critical action distance of the repulsive field. The potential field force function of the repulsive field is shown in formula (6):
the global potential field is formed by all virtual gravitational fields and repulsive force occasions in the working space of the double mechanical arm system, and the potential field function is shown as a formula (7):
where U represents the global potential field and n is the number of links of the mechanical arm. The function of the global potential field force is shown in equation (8):
the global potential field force is related to the joint velocity of the mechanical arm, which can be obtained by the formula (9-11):
M＝J F T (θ 1 ,θ 2 ,…,θ 6 )F(9
wherein M is joint moment, J F Is a jacobian matrix that is a matrix of jacobian,is the joint speed of the mechanical arm, B J Is the rotational inertia of the mechanical arm joint. If the mechanical arm obtains joint acceleration from the global potential field force according to the traditional artificial potential field method>This results in a joint acceleration of 0 when the arm reaches the target point, but a joint velocity other than 0, and the arm continues to move due to such inertial velocity without stopping at the target point. The improved artificial potential field method directly relates potential field force with joint speed, when the mechanical arm moves to the target point, the joint speed is reduced to 0,the mechanical arm stops at the target point, and the oscillation phenomenon is solved.
Through the first five steps, the collision avoidance track of the double mechanical arms is obtained. Specifically, the robotic arm URDF file may be visualized with a Roboticssystem toolbox tool in Matlab. The track of the mechanical arm end effector is known, the track of the mechanical arm joint space is obtained by solving the inverse solution of the kinematics of the mechanical arm, and the movement of the mechanical arm is planned according to the track of the mechanical arm joint space. The collision prevention process of the mechanical arm is realized through simulation:
the distance between the two mechanical arms on the Y axis is 2.25m, and the central coordinates of the bases of the two mechanical arms are [0,0 respectively]And [0,2.25,0 ]]. The mechanical arm R1 is from the initial point [1.5, -1.2, -0.5 ]]To the target point [2,0.5, -1 ]]The mechanical arm R2 is from the initial point [1.5,3.45, -0.5]Move to the target point [2,1, -1 ]]. The collision risk is detected by the two mechanical arms when t=2.9s, the action of an artificial potential field method is improved, the mechanical arm R2 avoids R1 under the action of a virtual repulsive field, the mechanical arm R1 returns to an initial point after finishing the stacking task, and the mechanical arm R2 reaches a target point under the action of the virtual repulsive field. The collision avoidance trajectories of the mechanical arms R1 and R2 are shown in fig. 7. The distance between the end executions of the robot arms R1 and R2 is shown in fig. 8. As can be seen from fig. 8, the improved artificial potential field method converges before the conventional artificial potential field method, and the distance between the mechanical arm R2 and the end effector of the mechanical arm R1 is always greater than the minimum safety distance d during collision avoidance s3 . The mechanical arm R2 moves to the target point stably under the action of the improved artificial potential field method, and the phenomenon of oscillation at the target point does not occur.
Example 2
The embodiment provides a double-mechanical-arm collision avoidance track planning system based on an improved artificial potential field method, which comprises the following steps:
module one: the control strategy of the two mechanical arms is formulated, and the priorities of the two mechanical arms are set;
and a second module: the method comprises the steps of acquiring size data of mechanical arms, and establishing a three-dimensional model and a kinematic model of two mechanical arms according to the acquired size data;
and a third module: the method comprises the steps of establishing a collision detection model of a double mechanical arm according to a three-dimensional model and a kinematic model;
and a fourth module: the method comprises the steps of determining all passing points of a double-mechanical-arm working path, including a starting point, a first intermediate point, a second intermediate point and a target point, and completing interpolation fitting among all the passing points to obtain an integral initial track of two mechanical arms;
and a fifth module: the method comprises the steps of using a collision model to detect an initial track offline, when collision risk is detected between two mechanical arms, improving an artificial potential field method to start to act and modifying the initial track of a slave mechanical arm, generating a virtual gravitational field and a repulsive force field in a working space of a double mechanical arm system by the improved artificial potential field method, wherein the gravitational field is arranged at a target point of the slave mechanical arm, a threshold distance is arranged in a gravitational field function, and the repulsive force field is arranged at each joint of a main mechanical arm; the modified trajectory is obtained by improving the artificial potential field method.
The specific steps of each module described above may refer to embodiment 1, the first module corresponds to step 1 in embodiment 1, the second module corresponds to step 2 in embodiment 1, the third module corresponds to step 3 in embodiment 1, the fourth module corresponds to step 4 in embodiment 1, and the fifth module corresponds to step 5 in embodiment 1, which is not described herein.
Finally, it is pointed out that relational terms such as first and second are used solely to distinguish one entity or action from another entity or action without necessarily requiring or implying any actual such relationship or order between such entities or actions.
The above description is only of the preferred embodiments of the present invention and is not intended to limit the present invention, but various modifications and variations can be made to the present invention by those skilled in the art. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention should be included in the protection scope of the present invention.
Claims (10)
1. The double-mechanical-arm collision avoidance track planning method based on the improved artificial potential field method is characterized by comprising the following steps of:
step 1: setting a control strategy of the double mechanical arms and setting priorities of the two mechanical arms;
step 2: acquiring size data of the mechanical arms, and establishing a three-dimensional model and a kinematic model of the two mechanical arms according to the acquired size data;
step 3: establishing a collision detection model of the double mechanical arms according to the three-dimensional model and the kinematic model;
step 4: determining all the passing points of the working path of the double mechanical arms, wherein the passing points comprise a starting point, a first middle point, a second middle point and a target point, and completing interpolation fitting among all the passing points to obtain a complete initial track of the two mechanical arms;
step 5: when collision risk is detected between two mechanical arms, an improved artificial potential field method starts to act and modifies the initial track of the slave mechanical arm, the improved artificial potential field method generates a virtual gravitational field and a repulsive force field in a working space of a double mechanical arm system, the gravitational field is arranged at a target point of the slave mechanical arm, a threshold distance is arranged in a gravitational field function, and the repulsive force field is arranged at each joint of the master mechanical arm; obtaining a modified track by improving an artificial potential field method;
the gravitational field of the improved artificial potential field method is arranged at the target point of the slave mechanical arm, and the gravitational field function formula is as follows:
in the method, in the process of the invention,K att is the coefficient of gravity and is used for the dynamic balance of the device,ρ(q,q g ) Is a vector whose value is the current position of the mechanical armqTo the target pointq g Is directed from the robot arm to the target point position,d q is the distance between the robotic arm and the target point,r q is the critical acting distance of the gravitational field,U k is a constant;
the potential field force function formula of the gravitational field is as follows:
in the middle ofF k Is a constant;
the repulsive force field function formula of the improved artificial potential field method is as follows:
in the middle ofK rep Is the coefficient of the repulsive force,ρ(q,q o ) Is a vector whose value is the current position of the mechanical armqTo an obstacleq o Is directed from the robot arm to the obstacle location,d i is the distance from the robot arm to the joint of the main robot arm,r s is the critical acting distance of the repulsive field, and the potential field force function formula of the repulsive field is as follows:
the global potential field is formed by all virtual gravitational fields and repulsive force occasions in the working space of the double mechanical arm system, and the potential field function formula is as follows:
wherein the method comprises the steps ofURepresenting the field of the global potential and,nthe function formula of the overall potential field force is as follows:
the global potential field force is related to the joint velocity of the mechanical arm, which can be obtained by the following formula:
in the middle ofMIs the moment of the joints, and the moment of the joints,J F is a jacobian matrix that is a matrix of jacobian,is the joint speed of the mechanical arm,B J is the rotational inertia of the joint of the mechanical arm,θ i is a variable of the joints, and is a variable of the joints,i=1, 2, …, 6。
2. the method for planning collision avoidance trajectories of two mechanical arms based on the improved artificial potential field method of claim 1, wherein in step 1, master/slave coordination control is used as a control strategy of the two mechanical arms.
3. The method for planning the collision avoidance trajectory of the double mechanical arms based on the improved artificial potential field method according to claim 1, wherein in the step 2, the size data of the mechanical arms are measured, and a three-dimensional model of the mechanical arms is built by using software according to the obtained size data; and calculating the size data to obtain a mechanical arm D-H parameter table, and establishing a kinematic model and a URDF file of the double mechanical arm system according to a standard D-H parameter method.
4. The method for planning collision avoidance trajectories of two mechanical arms based on the improved artificial potential field method of claim 1, wherein the collision detection model in step 3 can determine whether there is a collision risk between the two mechanical arms by calculating the distance from the mechanical arm to the main mechanical arm offline.
5. The method for planning the collision avoidance trajectory of the double mechanical arms based on the improved artificial potential field method according to claim 1, wherein in the step 4, a linear interpolation function is adopted to fit the distance from the starting point to the first intermediate point and the distance from the second intermediate point to the target point, and a cubic polynomial interpolation function is adopted to fit the trajectory from the first intermediate point to the second intermediate point.
6. A double-mechanical-arm collision avoidance track planning system based on an improved artificial potential field method is characterized by comprising the following steps:
module one: the control strategy of the two mechanical arms is formulated, and the priorities of the two mechanical arms are set;
and a second module: the method comprises the steps of acquiring size data of mechanical arms, and establishing a three-dimensional model and a kinematic model of two mechanical arms according to the acquired size data;
and a third module: the method comprises the steps of establishing a collision detection model of a double mechanical arm according to a three-dimensional model and a kinematic model;
and a fourth module: the method comprises the steps of determining all passing points of a double-mechanical-arm working path, including a starting point, a first intermediate point, a second intermediate point and a target point, and completing interpolation fitting among all the passing points to obtain an integral initial track of two mechanical arms;
and a fifth module: the method comprises the steps of using a collision detection model to detect an initial track offline, when collision risk is detected between two mechanical arms, improving an artificial potential field method to start to act and modifying the initial track of a slave mechanical arm, generating a virtual gravitational field and a repulsive force field in a working space of a double mechanical arm system by the improved artificial potential field method, wherein the gravitational field is arranged at a target point of the slave mechanical arm, a threshold distance is arranged in a gravitational field function, and the repulsive force field is arranged at each joint of a main mechanical arm; obtaining a modified track by improving an artificial potential field method;
the gravitational field of the improved artificial potential field method is arranged at the target point of the slave mechanical arm, and the gravitational field function formula is as follows:
in the method, in the process of the invention,K att is the coefficient of gravity and is used for the dynamic balance of the device,ρ(q,q g ) Is a vector whose value is the current position of the mechanical armqTo the target pointq g Is directed from the robot arm to the target point position,d q is the distance between the robotic arm and the target point,r q is the critical acting distance of the gravitational field,U k is a constant;
the potential field force function formula of the gravitational field is as follows:
in the middle ofF k Is a constant;
the repulsive force field function formula of the improved artificial potential field method is as follows:
in the middle ofK rep Is the coefficient of the repulsive force,ρ(q,q o ) Is a vector whose value is the current position of the mechanical armqTo an obstacleq o Is directed from the robot arm to the obstacle location,d i is the distance from the robot arm to the joint of the main robot arm,r s is the critical acting distance of the repulsive field, and the potential field force function formula of the repulsive field is as follows:
the global potential field is formed by all virtual gravitational fields and repulsive force occasions in the working space of the double mechanical arm system, and the potential field function formula is as follows:
wherein the method comprises the steps ofURepresenting the field of the global potential and,nthe function formula of the overall potential field force is as follows:
the global potential field force is related to the joint velocity of the mechanical arm, which can be obtained by the following formula:
in the middle ofMIs the moment of the joints, and the moment of the joints,J F is a jacobian matrix that is a matrix of jacobian,is the joint speed of the mechanical arm,B J is the rotational inertia of the joint of the mechanical arm,θ i is a variable of the joints, and is a variable of the joints,i=1, 2, …, 6。
7. the method for planning collision avoidance trajectories of two robots based on an improved artificial potential field method of claim 6 wherein master/slave coordination control is used as a control strategy for the two robots in a first module.
8. The method for planning collision avoidance trajectories of two mechanical arms based on the improved artificial potential field method of claim 6, wherein the second module is configured to measure the size data of the mechanical arms, and to build a three-dimensional model of the mechanical arms using software according to the obtained size data; and calculating the size data to obtain a mechanical arm D-H parameter table, and establishing a kinematic model and a URDF file of the double mechanical arm system according to a standard D-H parameter method.
9. The method for planning collision avoidance trajectories of two mechanical arms based on the improved artificial potential field method of claim 6, wherein said collision detection model is configured to determine whether there is a risk of collision between the two mechanical arms by calculating the distance from the mechanical arm to the main mechanical arm off-line.
10. The method of claim 6, wherein the fourth module is configured to fit the trajectory from the first intermediate point to the second intermediate point using a linear interpolation function and the distance from the second intermediate point to the target point using a cubic polynomial interpolation function.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210415041.1A CN114643581B (en) | 2022-04-20 | 2022-04-20 | Double-mechanical-arm collision avoidance track planning method and system based on improved artificial potential field method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210415041.1A CN114643581B (en) | 2022-04-20 | 2022-04-20 | Double-mechanical-arm collision avoidance track planning method and system based on improved artificial potential field method |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114643581A CN114643581A (en) | 2022-06-21 |
| CN114643581B true CN114643581B (en) | 2024-01-19 |
Family
ID=81997203
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210415041.1A Active CN114643581B (en) | 2022-04-20 | 2022-04-20 | Double-mechanical-arm collision avoidance track planning method and system based on improved artificial potential field method |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114643581B (en) |
Citations (15)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US5483630A (en) * | 1990-07-12 | 1996-01-09 | Hitachi, Ltd. | Method and apparatus for representing motion of multiple-jointed object, computer graphic apparatus, and robot controller |
| JP2003241836A (en) * | 2002-02-19 | 2003-08-29 | Keio Gijuku | Control method and apparatus for free-running mobile unit |
| JP2012011498A (en) * | 2010-06-30 | 2012-01-19 | Toshiba Corp | System and method for operating robot arm |
| CN104029203A (en) * | 2014-06-18 | 2014-09-10 | 大连大学 | Path planning method for implementation of obstacle avoidance for space manipulators |
| CN108153298A (en) * | 2017-04-19 | 2018-06-12 | 中国北方车辆研究所 | A kind of legged type robot traction control method and system based on improvement Artificial Potential Field |
| CN110471420A (en) * | 2019-08-23 | 2019-11-19 | 苏州浪潮智能科技有限公司 | A kind of machine barrier-avoiding method and device based on Artificial Potential Field Method |
| EP3634695A1 (en) * | 2017-05-29 | 2020-04-15 | Franka Emika GmbH | Collision handling by a robot |
| CN111546343A (en) * | 2020-05-13 | 2020-08-18 | 浙江工业大学 | Method and system for planning route of defense mobile robot based on improved artificial potential field method |
| CN112327831A (en) * | 2020-10-20 | 2021-02-05 | 大连理工大学 | Factory AGV track planning method based on improved artificial potential field method |
| CN112596525A (en) * | 2020-12-16 | 2021-04-02 | 中国地质大学（武汉） | Robot path planning method based on genetic algorithm and improved artificial potential field method |
| CN113334379A (en) * | 2021-05-30 | 2021-09-03 | 南京理工大学 | Master-slave following and collision avoidance method based on virtual force |
| CN113858210A (en) * | 2021-11-01 | 2021-12-31 | 贵州大学 | Mechanical arm path planning method based on hybrid algorithm |
| CN113997291A (en) * | 2021-11-23 | 2022-02-01 | 南方电网电力科技股份有限公司 | Machine manpower-guided autonomous control method and related device |
| CN114028153A (en) * | 2021-09-18 | 2022-02-11 | 深圳华鹊景医疗科技有限公司 | Rehabilitation robot and control method thereof |
| CN114355937A (en) * | 2021-12-31 | 2022-04-15 | 江苏理工学院 | Unmanned vehicle local path planning method based on improved artificial potential field method |
-
2022
- 2022-04-20 CN CN202210415041.1A patent/CN114643581B/en active Active
Patent Citations (15)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US5483630A (en) * | 1990-07-12 | 1996-01-09 | Hitachi, Ltd. | Method and apparatus for representing motion of multiple-jointed object, computer graphic apparatus, and robot controller |
| JP2003241836A (en) * | 2002-02-19 | 2003-08-29 | Keio Gijuku | Control method and apparatus for free-running mobile unit |
| JP2012011498A (en) * | 2010-06-30 | 2012-01-19 | Toshiba Corp | System and method for operating robot arm |
| CN104029203A (en) * | 2014-06-18 | 2014-09-10 | 大连大学 | Path planning method for implementation of obstacle avoidance for space manipulators |
| CN108153298A (en) * | 2017-04-19 | 2018-06-12 | 中国北方车辆研究所 | A kind of legged type robot traction control method and system based on improvement Artificial Potential Field |
| EP3634695A1 (en) * | 2017-05-29 | 2020-04-15 | Franka Emika GmbH | Collision handling by a robot |
| CN110471420A (en) * | 2019-08-23 | 2019-11-19 | 苏州浪潮智能科技有限公司 | A kind of machine barrier-avoiding method and device based on Artificial Potential Field Method |
| CN111546343A (en) * | 2020-05-13 | 2020-08-18 | 浙江工业大学 | Method and system for planning route of defense mobile robot based on improved artificial potential field method |
| CN112327831A (en) * | 2020-10-20 | 2021-02-05 | 大连理工大学 | Factory AGV track planning method based on improved artificial potential field method |
| CN112596525A (en) * | 2020-12-16 | 2021-04-02 | 中国地质大学（武汉） | Robot path planning method based on genetic algorithm and improved artificial potential field method |
| CN113334379A (en) * | 2021-05-30 | 2021-09-03 | 南京理工大学 | Master-slave following and collision avoidance method based on virtual force |
| CN114028153A (en) * | 2021-09-18 | 2022-02-11 | 深圳华鹊景医疗科技有限公司 | Rehabilitation robot and control method thereof |
| CN113858210A (en) * | 2021-11-01 | 2021-12-31 | 贵州大学 | Mechanical arm path planning method based on hybrid algorithm |
| CN113997291A (en) * | 2021-11-23 | 2022-02-01 | 南方电网电力科技股份有限公司 | Machine manpower-guided autonomous control method and related device |
| CN114355937A (en) * | 2021-12-31 | 2022-04-15 | 江苏理工学院 | Unmanned vehicle local path planning method based on improved artificial potential field method |
Non-Patent Citations (2)
| Title |
|---|
| 基于改进人工势场法的双机械臂避障路径规划;薛阳等;机械传动;第44卷(第3期);39-44 * |
| 基于速度场的人工势场法机械臂动态避障研究;史亚飞;张力;刘子煊;吕一平;;机械传动(第04期);44-50 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114643581A (en) | 2022-06-21 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US8600554B2 (en) | System and method for robot trajectory generation with continuous accelerations | |
| US8560122B2 (en) | Teaching and playback method based on control of redundancy resolution for robot and computer-readable medium controlling the same | |
| EP2660013B1 (en) | Method of controlling seven-shaft multi-joint robot, control program, and robot control device | |
| WO2020010626A1 (en) | Robot motion control method, robot, and robot motion control system | |
| Korayem et al. | Dynamic load-carrying capacity of mobile-base flexible joint manipulators | |
| US20200189101A1 (en) | Trajectory generation system and trajectory generating method | |
| WO2023069292A1 (en) | Nonlinear trajectory optimization for robotic devices | |
| CN115476356A (en) | Self-adaptive hybrid impedance control method of space manipulator without force sensor | |
| CN114310914A (en) | Fuzzy self-adaptive iterative trajectory tracking control method and system for multi-degree-of-freedom mechanical arm | |
| CN114643581B (en) | Double-mechanical-arm collision avoidance track planning method and system based on improved artificial potential field method | |
| CN113263496A (en) | Method for optimizing path of six-degree-of-freedom mechanical arm and computer equipment | |
| Zhang et al. | Kuka youBot arm shortest path planning based on geodesics | |
| Muller-Karger et al. | Hyperbolic trajectories for pick-and-place operations to elude obstacles | |
| JP2020110884A (en) | Robot control device, robot control method, and robot control program | |
| CN114888812A (en) | Mobile mechanical arm station planning method based on rigidity performance optimization | |
| Bae et al. | A dynamic visual servoing of robot manipulator with eye-in-hand camera | |
| Kim | Design of a Novel 4-DOF High-Speed Parallel Robot | |
| Yang et al. | Collision avoidance trajectory planning for a dual-robot system: using a modified APF method | |
| Zhang et al. | Varying inertial parameters model based robust control for an aerial manipulator | |
| Chávez et al. | Kinematic and dynamic modeling of the phantomx ax metal hexapod mark iii robot using quaternions | |
| Ren et al. | Performance improvement of tracking control for a planar parallel robot using synchronized control | |
| Omrčen et al. | Autonomous motion of a mobile manipulator using a combined torque and velocity control | |
| Li et al. | Dynamic modeling and analysis for 6-DOF industrial robots | |
| Kuppan Chetty et al. | A heuristic approach towards path planning and obstacle avoidance control of planar manipulator | |
| KR102643316B1 (en) | Detecting device and method for calculating amount of impact of robot manipulator |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |