CN109822554B - Underwater dual-arm cooperative grabbing, holding and collision avoidance integrated method and system - Google Patents
Underwater dual-arm cooperative grabbing, holding and collision avoidance integrated method and system Download PDFInfo
- Publication number
- CN109822554B CN109822554B CN201910214006.1A CN201910214006A CN109822554B CN 109822554 B CN109822554 B CN 109822554B CN 201910214006 A CN201910214006 A CN 201910214006A CN 109822554 B CN109822554 B CN 109822554B
- Authority
- CN
- China
- Prior art keywords
- arm
- path
- motion
- track
- mechanical
- 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
Images
Abstract
The invention discloses an underwater double-arm cooperative grabbing, holding and collision avoidance integrated method and system, and belongs to the field of automatic control of underwater operation. The invention provides three areas with different collision probabilities based on the original task track planning of double-arm cooperative work, and preferentially performs track planning in the area with low collision probability on the basis of the working space of double mechanical arms, divides the track into a plurality of small sections of paths by an interpolation method when the track has to pass through the area with high probability, performs collision judgment and collision avoidance planning before executing the motion of each small section, and updates the motion of the small sections of paths according to the collision avoidance planning, thereby performing preventive control on collision risks on the premise of not interfering the normal execution of the original task. The invention can carry out preventive movement collision avoidance of the mechanical arm while planning the operation track, is an integrated method for unifying the cooperative operation of the two underwater mechanical arms and the movement collision avoidance, and has certain practical value and development potential.
Description
Technical Field
The invention belongs to the field of automatic control of underwater operation, and relates to an integrated method and system for underwater double-arm cooperative grabbing, holding and collision prevention, in particular to an integrated method and system for underwater operation double-mechanical-arm cooperative grabbing, cooperative holding and collision prevention.
Background
At present, the ocean technology is highly valued and unprecedented developed by ocean countries in the world, the ocean operation depth is increasingly large, and the operation environment is also increasingly severe. An underwater manipulator is a tool which can well replace people to complete dangerous tasks. The increase of human marine exploration activities and the maturity of automation technology, underwater manipulators are gradually released into the market to assist humans in carrying out marine engineering operations. Because a large amount of manpower and material resources can be saved, the underwater manipulator completely exposes the corner in the aspect of underwater operation.
With the development of manned submersible vehicle technology and the increase of submergence depth, in recent years, the technology of underwater manipulators is rapidly developed, for example, a flood dragon manned submersible vehicle submerges to reach 7062 m depth in a Marina ditch, and breaks through the submergence maximum depth of similar operation type aircrafts in the world. However, the working depth of the manipulator is correspondingly increased, and how to avoid the manipulator in the deep sea severe environment is one of the most troublesome problems at present.
At present, most of mechanical arms applied to underwater are single-arm, and the trajectory planning is single. The underwater single mechanical arm only needs to consider the collision avoidance of the environment in the working process, belongs to static collision avoidance, and mostly carries out feedback through a sensor without considering the problem of self 'putting up'.
However, as the depth of diving increases, the underwater environment becomes more complex and the suspension operation mode becomes more difficult and has extremely low precision. Suspension operation difficulty under water, the carrier is rocking all at all times, and the unable accurate butt joint of hand claw and target object to, the hand claw snatchs and receives hand claw aperture size restriction, can't snatch to the object that the size surpassed the hand claw aperture, and the scope of embracing the action and grabbing the object is wider. On the basis, in order to improve the operation precision, a method of holding by two arms in cooperation with an anthropomorphic person can be adopted. The fault-tolerant rate of the holding action is high, and the adaptability to objects with different sizes is good. If the grabbing cannot achieve the purpose under the complex sea condition, a method of double-arm cooperative anthropomorphic holding is adopted to improve the operation efficiency, and the double-arm holding has better size adaptability to different grabbed objects.
Taking the AUV carrying two mechanical arms to carry out holding operation in the deep sea environment as an example, an object is held on the surface of the AUV by the cooperation of the two mechanical arms, the planned tail end tracks of the two mechanical arms are likely to be crossed under the condition, and the connecting rods of the two mechanical arms are likely to interfere in the motion process, so that a collision prevention scheme needs to be designed.
Just because the two arms are held in the underwater operation mode, the two arms are crossed and cooperated, the operation process is more complicated than that of a single mechanical arm, not only is the collision avoidance with the environment considered, but also more importantly, the collision between the two arms is prevented, so that the operation error rate can be effectively reduced, and the safety of the mechanical arm is ensured.
However, the existing collision avoidance mode mainly uses sensor feedback and is characterized in that the working mode is real-time detection, which causes the control effect to have higher dependence on the accuracy and stability of the sensor; the operation of mechanical arms on land also has the technology of prejudging through visual sensing, but because the deep sea environment is abominable, the illumination is not enough, and the installation and the formation of image of visual sensing equipment all have great difficulty, are not suitable for the collision avoidance detection of underwater operation.
Therefore, a collision prevention method suitable for the double mechanical arms in underwater operation is urgently needed.
Disclosure of Invention
Aiming at the defects or the improvement requirements of the prior art, the invention provides an underwater double-arm cooperative grabbing, embracing and collision avoidance integrated method and system, and aims to divide the working space of double arms, decompose the path by combining an interpolation method, further perform track intersection and mechanical arm interference judgment, and re-plan the track according to the judgment result and the divided working space, so that on the basis of the original task track planning of a mechanical arm, collision is pre-analyzed and the track is updated in advance to avoid, thereby realizing the integrated control of task execution and collision avoidance of the mechanical arm and reducing the dependence on the feedback technology of the traditional sensor.
In order to achieve the above object, according to an aspect of the present invention, an integrated method for underwater dual-arm cooperative grabbing, embracing, and collision avoidance is provided, in which a single-arm movement feasible region, a single-arm movement warning region, and a dual-arm cooperative work region are divided based on dual-arm work spaces of two mechanical arms, and it is determined whether a dual-arm cooperative work region exists or not:
if the double-arm cooperative working area does not exist, the double-arm action is directly executed according to the planned tail end track;
if the double-arm cooperative working area exists, interpolation is carried out on the tail end tracks of the double mechanical arms at the same time, the tail end track of each single arm is decomposed into a plurality of small sections of paths according to the interpolation point, and the following collision avoidance steps are carried out by taking the small sections of paths as units: before each path segment, performing track crossing and mechanical arm connecting rod interference judgment on the tail end of the mechanical arm, and if the track crossing and the mechanical arm connecting rod interference do not exist, executing the action of the current path segment by the mechanical arm; otherwise, replanning the action of the small path and performing intersection and interference judgment again;
circulating the collision avoidance step before each small path is executed until the tail ends of the two mechanical arms move to the track end point;
wherein the content of the first and second substances,
the two-arm cooperative working area refers to a crossed area of motion spaces of two single arms; the single-arm movement feasible region is a region of the movement space of each single arm after the two-arm cooperative working region is removed; the single-arm movement warning area is a single-arm movement warning area which divides a working space into two half areas by taking the central axis of the working space as a boundary, wherein the half area where one mechanical arm is located is the other mechanical arm;
on the premise of meeting the requirements of work tasks, each mechanical arm preferentially performs terminal track and motion planning in the single-arm motion feasible region, when the single-arm motion feasible region cannot complete the planning, the double-arm cooperation working region is considered, and finally the single-arm motion warning region is considered.
Further, before each path segment executes the action, judging whether the path segment of each mechanical arm is positioned in the corresponding single-arm movement feasible region, if so, directly executing the planning action of the path segment; otherwise, carrying out a collision avoidance step.
And further setting one mechanical arm as a driving arm and the other mechanical arm as a driven arm, simultaneously interpolating on the tail end tracks of the driving arm and the driven arm, keeping the tail end track of the driving arm planned unchanged if intersection or interference exists in the collision avoidance step, and re-planning the small segment of path of the driven arm so as to enable the driven arm to avoid the driving arm.
Further, the method comprises the following steps:
s1, judging whether a double-arm cooperative work area exists, if yes, turning to the step S2; otherwise, executing directly according to the originally planned tail end track;
s2, dividing a single-arm movement feasible area and a single-arm movement warning area, and entering the step S3;
s3, interpolating the tail end tracks of the two mechanical arms simultaneously, dividing the tail end tracks into a plurality of small paths according to interpolation points, and entering the step S4;
s4, when the double-arm action is executed to a small path, firstly, the track of the small path is judged:
the small section of path of each mechanical arm is in the corresponding single-arm movement feasible area, and the action of the small section of path is directly executed;
if the short path of at least one mechanical arm is not in the corresponding one-arm movement feasible region, the step S5 is performed;
s5, judging whether the motion trail of the two arm ends is crossed under the small path:
if so, taking one mechanical arm as a driving arm and the other mechanical arm as a driven arm, keeping the track of the small path of the driving arm unchanged, adjusting the joint angle of the driven arm to enable the driven arm and the driving arm to be staggered by corresponding angles, and taking the tail end position of the obtained driven arm as an interpolation point of a new track, returning to the step S3 and updating the tail end track and the interpolation point of the driven arm;
otherwise, go to step S6;
s6, judging whether the connecting rods of the two mechanical arms interfere under the small path:
if not, directly executing the action of the small path;
if the interference occurs, keeping the motion planning of the small section of path of the driving arm unchanged, selecting a group of solutions which meet the condition that the mechanical arm connecting rod has no interference and the weighted mechanical arm joint motion distance is optimal from the inverse kinematics solution of the small section of path of the driven arm, using the solutions as control parameters of the driven arm when the current small section of path moves, performing the motion planning on the driven arm again, and then executing the action of the small section of path.
Further, in step S5, the minimum size collision avoidance method is used to calculate the offset angle of the driven arm, and the method is as follows:
assuming that the distance from a connecting rod connected with the tail end of the driven arm to the shoulder joint is L, the diameter of the thickest section of the connecting rod is D, the shoulder joint only needs to move reversely by an angle theta towards the current movement direction to ensure that the tail ends of the two arms do not collide, and the collision-preventing movement angle of the shoulder joint is as follows:
substituting theta into positive kinematic equation of the mechanical arm to calculate space coordinate (x) of the tail end of the driven arm2，y2，z2) And coordinates (x) of active hand end1，y1，z1) Making a comparison if x1≠x2Or y1≠y2Or z1≠z2Then, it means that no collision occurs at the position of the two-arm end, and the coordinate point (x) of the driven hand end at this time2，y2，z2) And (4) interpolating points for the new track.
Further, the method for obtaining the solution satisfying the condition that the mechanical arm link has no interference and the weighted mechanical arm joint movement distance is optimal in step S6 is as follows:
setting the slave arm to have N joints, firstly defining a weight omega for each joint of the slave arm according to the priority motion sequence on the motion planiThe weight of the joint which preferentially moves is higher; assume that the initial angle of the ith joint at the current segment is θ0iObtaining solutions of multiple groups of joint angles corresponding to the target interpolation point of the current small section through inverse solution, and after removing the solution which can cause the interference of the connecting rod, remaining M groups of solutions, wherein the weighted mechanical arm joint movement distance a corresponding to the solution of the k group of joint angleskComprises the following steps:
calculated akAnd a group of joint angles corresponding to the minimum value in the motion vector calculation is a solution which meets the condition that the mechanical arm connecting rod has no interference and the weighted mechanical arm joint motion distance is optimal, and the group of joint angles are used as motion control parameters of the driven arm and executed.
Calculated akAnd a group of joint angles corresponding to the minimum value in the motion vector calculation is a solution which meets the condition that the mechanical arm connecting rod has no interference and the weighted mechanical arm joint motion distance is optimal, and the group of joint angles are used as motion control parameters of the driven arm and executed.
In order to achieve the above object, according to another aspect of the present invention, an underwater dual-arm cooperative grabbing, embracing, and collision avoidance integrated system is provided, which includes a controller and a collision avoidance program module, where the collision avoidance program module, when called by the controller, implements the dual-arm cooperative grabbing, cooperative embracing, and collision avoidance integrated method as described above. In general, compared with the prior art, the above technical solution contemplated by the present invention can obtain the following beneficial effects:
1. according to the method, the collision which possibly occurs can be predicted in advance while the planning of the double-arm cooperative work task is executed, and the original planning track is updated, so that the double-arm manipulator is effectively prevented from being erected in the operation process under the condition of not interfering the normal execution of the task, the safety of the underwater double-arm manipulator is ensured, and the reliability of underwater operation is improved.
2. The collision avoidance method provided by the invention is to directly perform judgment and planning before the mechanical arm executes the action, so as to realize preventive control. The method is more intelligent, and can realize accurate action. The method is further improved into a motion plan with autonomous analysis capability on the basis of the existing manipulator motion control, and has certain practical value and development potential.
3. Through the division of the single-arm movement feasible region, the single-arm movement warning region and the double-arm cooperative working region, whether collision possibility exists or not can be judged in advance, and the track part without collision possibility directly and normally executes the original planning action, so that the operation burden can be greatly reduced, and the working efficiency is improved; and when collision avoidance is carried out, the track planning is carried out according to the priority of the subareas, so that the collision probability can be further reduced, and the motion collision avoidance can be realized.
4. The invention can carry out motion collision avoidance of the mechanical arm while planning the operation track, is an integrated method for unifying the cooperative operation of the two underwater mechanical arms and the motion collision avoidance, and has good compatibility and wide application range.
5. The invention provides that under the condition of general brief tasks such as double-arm cooperative grabbing work, the movement of each arm moves in a feasible area divided by each arm as much as possible; if complex tasks such as double-arm cooperative embracing and cross cooperative grabbing exist, under the condition that self collision possibly exists between the double arms, a double-arm end effector track target point interference judgment method based on motion track interpolation is adopted, feasible tracks of the mechanical arms are decomposed into a plurality of small sections, and double-arm interference judgment is carried out on each small section, wherein the double-arm interference judgment comprises track cross judgment and connecting rod interference judgment. If the collision avoidance condition is not met, the minimum size collision avoidance method is provided to calculate the angles of all joints of the mechanical arm for solving the collision problem, all connecting rod joints of the mechanical arm are adjusted, a group of kinematic inverse solution values without arm collision are selected by adopting a weighted mechanical arm joint motion distance optimization method, and all joint actions are executed after the double mechanical arms are determined to be free of collision again through a double-arm interference judgment method and mechanical arm positive kinematic verification. The integrated method effectively solves the problem of self collision possibly existing when the two mechanical arms cooperatively grab or cooperatively embrace and take.
Drawings
Fig. 1 is a schematic diagram of division of a working space of two robots: the dotted line semicircle is the right arm working space, and the solid line semicircle is the left arm working space; the cross shadow area of the two working spaces is a double-arm cooperative working area, the part of the left arm working space after the double-arm cooperative working area is removed is a left arm movement feasible area, the part of the right arm working space after the double-arm cooperative working area is removed is a right arm movement feasible area, the area in a dotted line box is a left arm warning area, and the area in a solid line box is a right arm warning area.
Fig. 2 is a schematic diagram of trajectory planning performed by two arms cooperatively grasping and removing left and right arms in respective feasible areas: and (4) interpolating on the track, and calculating whether the track between the interpolation point and the next interpolation point has collision possibility when the tail end of the mechanical arm moves to the interpolation point.
FIG. 3 is a schematic illustration of the movement of the left and right arms into the dual-arm cooperative work zone: judging the collision possibility in the process of moving to the next interpolation point, before moving to the next interpolation point, in the planning of the next interpolation point, according to the motion track of the active hand (left arm) and the size of the arm, the driven hand is staggered by a corresponding distance, the angle of the joint of the driven hand, which needs to be adjusted, is calculated, and the angle is used for determining a new track interpolation point through positive kinematics calculation and verification.
Fig. 4 is a schematic view of the robot arm tip collision avoidance: according to the planning of the previous step, the slave arm (right arm) moves to a new interpolation point, and the master arm (left arm) is avoided.
FIG. 5 is a schematic diagram of dual arm tip movement to trajectory emphasis: after the driven arm bypasses the driving arm, the double arms normally move according to the originally planned path, the expected function is finally realized, and the whole process effectively realizes the collision prevention of the double mechanical arms.
Fig. 6 is a partial detail view of the shoulder joint misalignment during collision avoidance in fig. 4: through the rotation of shoulder joint motor, make the connecting rod that initiative arm and driven arm probably take place the interference originally not intersect on the projection plane of looking sideways at, consequently make two arms stagger in the space, avoid interfering.
FIG. 7 is a mathematical model of end collision avoidance when two arms are extracted in coordination;
fig. 8 is a schematic flow chart of a preferred embodiment of the present invention.
Detailed Description
In order to make the objects, technical solutions and advantages of the present invention more apparent, the present invention is described in further detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative of the invention and are not intended to limit the invention. In addition, the technical features involved in the embodiments of the present invention described below may be combined with each other as long as they do not conflict with each other.
The double-arm robot of this embodiment uses AUV (Autonomous Underwater Vehicle) as a carrier, and divides a double-arm working space into a single-arm movement possible area, a single-arm movement warning area, and a double-arm cooperation working area based on the double-arm working space according to the structure of the double-arm robot, as shown in fig. 1. The single-arm movement feasible region is a region where the left arm (or the right arm) cannot contact the right arm (or the left arm) working space when moving at any angle within the range of the reachable angle of each joint, in other words, the left arm and the right arm cannot collide with each other when moving in the single-arm movement feasible region.
As shown in fig. 1, the portion of the dotted semicircular area, from which the shaded area is removed, is a right arm movement feasible region. The single-arm movement warning area is a left arm as an example, and a right half part of the working space with a central axis as a boundary, namely an area in a dotted line box in fig. 1 is the single-arm movement warning area of the left arm. The overlapping area of the two-arm working space is divided into two-arm cooperative working areas, namely a shaded area in which a dotted line semicircle and a solid line semicircle are overlapped in fig. 1.
In this embodiment, for the right arm, there are a right arm movement feasible region and a right arm alert region; for the left arm, a left arm movement feasible area and a left arm warning area exist; while for both arms there is a dual arm cooperative work zone. In other embodiments, if there is no two-arm cooperative work area, it also means that each single arm does not move into the warning area, i.e. no collision occurs with the other arm anyway, in this case, the single arm may move directly according to the planned trajectory of the mission, or may move step by step in an interpolation manner, so as to slow down the movement speed.
When planning the mechanical arm track, one of the two arms is set as a master hand, and the other arm is set as a slave hand. The two arms simultaneously interpolate on the trajectory, which is decomposed into segments according to the interpolation points, as shown in fig. 2. And when the mechanical arm is at the initial position, firstly, the track intersection judgment is carried out on the first small segment of track, if the track does not intersect, the judgment is continuously carried out whether the connecting rod interferes, if the connecting rod does not interfere, the action end effector moves to a first interpolation point from the initial position, and then the steps are repeated. When the two mechanical arms carry out double-arm holding and taking actions, the tail end track of the mechanical arm obviously needs to cross the warning area to reach the double-arm cooperation working area. When the two mechanical arms determine that the intersection exists, as shown in the figure 3, the position of the mechanical arm is the same as the position of the mechanical arm, the motion plan of the active hand is unchanged, and the rotation angle of the motor of the joint of the shoulder of the driven hand is calculated by adopting a minimum size collision avoidance method. Through the extracted model in fig. 7, assuming that the arm length is L, the diameter of the thickest section of the arm is D, and the distance is the minimum arm offset distance, the shoulder joint can ensure that the two arms do not collide by offsetting the angle θ. From this model:
thus, it is possible to provide
Therefore, the collision-prevention movement angle of the shoulder joint is as follows:
the arm angle obtained at this time is substituted into the positive kinematic equation of the arm, and the spatial coordinate (x) of the driven hand end effector is calculated2，y2，z2) And coordinates (x) of active hand end1，y1，z1) A comparison is made.
If x1≠x2Or y1≠y2Or z1≠z2It means that the two-arm end effectors do not coincide in this position. And the coordinate point of the slave hand is a new track interpolation point.
The driven hand shoulder joint motor rotates by an angle theta in the opposite direction, as shown in fig. 4 and 7, the driven arm and the driving arm can be seen to be staggered from a side projection plane, and after the mechanical arm is staggered in space, next-step trajectory planning can be carried out according to tasks, so that the motion of the mechanical arm is completed.
And if the possibility of collision is judged by the interference of the connecting rods, the motion of the mechanical arm is planned again. The inverse kinematics operation of the mechanical arm has a plurality of groups of solutions, the solution which is free of interference and optimal in weighted mechanical arm joint movement distance is screened out from the other solutions, and the solution is input into the controller to execute the action of the mechanical arm.
In the following, the method of the present invention is further described by taking the cooperative clasping operation of the four-joint robot shown in fig. 1 to 7 as an example, and actually the method of the present invention is applicable to a common multi-joint robot, which is different only in the value of N, M, and is not limited to four joints.
The four-joint robot arm of the present embodiment includes: shoulder joint, big arm rotary joint, elbow joint, wrist joint, shoulder connecting rod, big arm connecting rod, forearm connecting rod and end effector. One end of the shoulder connecting rod is connected with a shoulder joint, the other end of the shoulder connecting rod is connected with the upper end of the big arm connecting rod through a big arm rotary joint, the lower end of the big arm connecting rod is connected with the upper end of the small arm connecting rod through an elbow joint, and the lower end of the small arm connecting rod is connected with the end effector through a wrist joint. The motion sequence of the four-joint mechanical arm is as follows: shoulder joint → forearm rotation joint → elbow joint → wrist joint.
As shown in fig. 8, the cooperative embracing and collision avoidance integrated operation method is as follows:
s1, as shown in fig. 1, judging that a dual-arm cooperative work area exists in the dual-arm work space, and then go to step S2;
s2, dividing a single-arm movement feasible area and a single-arm movement warning area, and entering the step S3;
s3, as shown in FIG. 2, interpolating the tail end tracks of the two mechanical arms simultaneously, dividing the tail end tracks into a plurality of small paths according to the interpolation points, and entering the step S4;
s4, when the double-arm action is executed to a small path, firstly, the track of the small path is judged:
as shown in fig. 2, the small segment of path of each mechanical arm is in the corresponding single-arm movement feasible region, which indicates that the current small segment of path does not collide, and the action of the small segment of path is directly executed;
as shown in fig. 3, when moving to the position of fig. 3, determining that the small segment of the path to be executed by the left arm and the right arm is not in the corresponding single-arm movement feasible region, and enters the two-arm cooperative working region, which indicates that there is a possibility of collision, the process proceeds to step S5;
s5, judging whether the motion trail of the two arm ends is crossed under the small path:
after judgment, the motion tracks of the tail ends of the double arms are crossed under the small-section path to be executed, and collision can occur at the crossed point. At this time, the left arm is used as the driving arm, the right arm is used as the driven arm, the track of the small path of the driving arm is kept unchanged, the joint angle of the driven arm is adjusted, the driven arm and the driving arm are staggered by corresponding angles, the obtained tail end position of the driven arm is an interpolation point of a new track, the step S3 is returned, and the tail end track and the interpolation point of the driven arm are updated.
Specifically, as shown in fig. 4 and 7, the offset angle of the driven arm is calculated by adopting a minimum size collision avoidance method
Substituting theta into positive kinematic equation of the mechanical arm to calculate space coordinate (x) of the tail end of the driven arm2，y2，z2) And coordinates (x) of active hand end1，y1，z1) Making a comparison when x1≠x2Indicating that the dual-arm end effector does not collide in this position, the coordinate point (x) of the driven hand end at this time2，y2，z2) Interpolating points for the new trajectory;
according to the updated interpolation point (x)2，y2，z2) After the trajectory of the small path is re-planned, judging that the trajectories of the left arm and the right arm on the small path are not crossed, namely the two-arm end effector does not collide, and entering step S6;
s6, judging whether the connecting rods of the two mechanical arms interfere under the small path:
after judgment, although the end effectors of the two mechanical arms do not collide after the trajectory of the small segment of path is re-planned according to the new interpolation point, the currently solved control parameters, namely the motion angles of the four joints, can cause the small arm connecting rods of the left arm and the right arm to interfere.
At this time, the motion planning of the small path of the driving arm (namely, the left arm) is kept unchanged, a group of solutions which meet the condition that the mechanical arm connecting rod has no interference and the weighted mechanical arm joint motion distance is optimal are selected from the inverse kinematics solutions of the small path of the driven arm (namely, the right arm) and are used as control parameters of the driven arm when the current small path moves, the motion planning is carried out on the driven arm again, and then the action of the small path is executed.
Specifically, since the number N of joints of the right arm is 4, the weight for setting each joint movement angle to four joint movement angles in turn is set to the shoulder joint ω in turn according to the priority order of the joint movements1Big arm rotary joint omega2Elbow joint omega3Wrist joint omega4. At this time (x)2，y2，z2) The kinematics inverse solution of (1) has a plurality of groups, wherein one part of the solution can lead to the interference of the mechanical arm connecting rod, and the other part of the solution can lead to the right arm entering the right arm warning area, after the two parts of the solution are eliminated, the rest 4 groups of solutions can not lead to the interference of the mechanical arm connecting rod, and can not lead to the entering of the right arm warning area, namely, the mechanical arm connecting rod is free of interference and the solution which can not enter the right arm warning area has M4 groups.
Let the initial angle of each joint be theta01、θ02、θ03、θ04(i.e., i is 1,2,3,4), which is the angle at which the right arm end effector moves to (x) in the original trajectory plan1，y1，z1) The angle of time. Then, a solution with the optimal weighted mechanical arm joint movement distance needs to be selected from the 4 groups of non-interference solutions, so that the overall running distance is shortest, and the working efficiency is improved. In particular, the amount of the solvent to be used,
when the target point changes, i.e. the value point is composed of (x)1，y1，z1) Is updated to (x)2，y2，z2) Time of flightAnd 4 groups of angles theta obtained by inverse solution of kinematic equation of the mechanical armk1、θk2、θk3、θk4Wherein k is 1,2,3, 4.
Calculating the weighted motion distance of the kth solution according to a weighted mechanical arm joint motion distance optimization method to obtain:
calculate four akAfter the right is summed, the selection is performed again, namely the mina is takenkThe sum of the motion distance weights which is the optimal solution, and the four corresponding joint angles are the solution which satisfies the condition that the mechanical arm connecting rod has no interference and the motion distance of the mechanical arm joint is weighted to be optimal.
In short, except the solution of the motion angle of the driven arm with the link interference, the minimum weight and the corresponding group of joint angles are selected from the solutions of the motion angles without the link interference, and the selected solution is the final joint angle. Inputting the group of joint angles into a controller of the mechanical arm, and updating and executing the motion of the mechanical arm. The method can solve the problem of interference of the mechanical arm connecting rod.
In general, as long as the joint angle does not cause interference of the mechanical arm connecting rod and does not cause the solution entering the right arm warning area, the optimal solution is screened from the solutions. However, in other embodiments, it is possible to only have the solution that causes the mechanical arm link to interfere and the solution that does not interfere but causes the right arm alert zone to be entered, in which case the optimal solution can only be selected by performing the weighted estimation of the movement distance from the solutions that do not interfere but cause the right arm alert zone to be entered. The warning area is set, but the solution passing through the warning area is preferentially excluded, so that the mechanical arm is prevented from passing through the warning area as much as possible, and the subsequent planning of the residual track and the probability of collision in the motion can be reduced to the greatest extent.
After collision avoidance of a small section of path is completed according to the steps, the collision avoidance process is repeated on the next small section of path until the mechanical arm moves to the target point, and the operation task is completed, so that integrated control of collision avoidance is realized while planning of the task track of the mechanical arm.
It will be understood by those skilled in the art that the foregoing is only a preferred embodiment of the present invention, and is not intended to limit the invention, and that any modification, equivalent replacement, or improvement made within the spirit and principle of the present invention should be included in the scope of the present invention.
Claims (7)
1. The utility model provides a towards underwater both arms collaborative snatch, embrace and get and avoid bumping integrated method for operation task planning and avoid bumping integrated control during operation in the complicated environment under water, its characterized in that, tentatively confirm the operation task planning to divide the both arms workspace of two arms into single armed motion feasible region, single armed motion warning area and both arms cooperation workspace, judge earlier whether there is both arms cooperation workspace:
if the double-arm cooperative working area does not exist, the double-arm action is directly executed according to the planned tail end track;
if the double-arm cooperative working area exists, interpolation is carried out on the tail end tracks of the double mechanical arms at the same time, the tail end track of each single arm is decomposed into a plurality of small sections of paths according to the interpolation point, and the following collision avoidance steps are carried out by taking the small sections of paths as units: before each path segment, performing track crossing and mechanical arm connecting rod interference judgment on the tail end of the mechanical arm, and if the track crossing and the mechanical arm connecting rod interference do not exist, executing the action of the current path segment by the mechanical arm; otherwise, replanning the action of the small path and performing intersection and interference judgment again;
circulating the collision avoidance step before each small path is executed until the tail ends of the two mechanical arms move to the track end point;
wherein the content of the first and second substances,
the two-arm cooperative working area refers to a crossed area of motion spaces of two single arms; the single-arm movement feasible region is a region of the movement space of each single arm after the two-arm cooperative working region is removed; the single-arm movement warning area is used for dividing the working space into two half areas by taking the central axis of the double-arm working space as a boundary, wherein the half area where one mechanical arm is located is the single-arm movement warning area of the other mechanical arm;
on the premise of meeting the requirements of work tasks, each mechanical arm preferentially performs terminal track and motion planning in the single-arm motion feasible region, when the single-arm motion feasible region cannot complete the planning, the double-arm cooperation working region is considered, and finally the single-arm motion warning region is considered.
2. The integrated underwater double-arm cooperative grabbing, embracing and collision avoidance method according to claim 1, wherein before each small section of path performs an action, whether the small section of path of each mechanical arm is located in a corresponding single-arm movement feasible region is judged, and if yes, a planning action of the small section of path is directly performed; otherwise, carrying out a collision avoidance step.
3. The integrated underwater double-arm cooperative grabbing, embracing and collision avoiding method as claimed in claim 1 or 2, wherein one mechanical arm is set as a driving arm, the other mechanical arm is set as a driven arm, interpolation is simultaneously performed on the end tracks of the driving arm and the driven arm, and in the collision avoiding step, if intersection or interference exists, the end track of the driving arm is kept planned unchanged, and the small section of path of the driven arm is re-planned, so that the driven arm avoids the driving arm.
4. The underwater double-arm cooperative grabbing, embracing and collision avoidance integrated method as claimed in claim 1, comprising the following steps:
s1, judging whether a double-arm cooperative work area exists, if yes, turning to the step S2; otherwise, executing directly according to the originally planned tail end track;
s2, dividing a single-arm movement feasible area and a single-arm movement warning area, and entering the step S3;
s3, interpolating the tail end tracks of the two mechanical arms simultaneously, dividing the tail end tracks into a plurality of small paths according to interpolation points, and entering the step S4;
s4, when the double-arm action is executed to a small path, firstly, the track of the small path is judged:
the small section of path of each mechanical arm is in the corresponding single-arm movement feasible area, and the action of the small section of path is directly executed;
if the short path of at least one mechanical arm is not in the corresponding one-arm movement feasible region, the step S5 is performed;
s5, judging whether the motion trail of the two arm ends is crossed under the small path:
if so, taking one mechanical arm as a driving arm and the other mechanical arm as a driven arm, keeping the track of the small path of the driving arm unchanged, adjusting the joint angle of the driven arm to enable the driven arm and the driving arm to be staggered by corresponding angles, and taking the tail end position of the obtained driven arm as an interpolation point of a new track, returning to the step S3 and updating the tail end track and the interpolation point of the driven arm;
otherwise, go to step S6;
s6, judging whether the connecting rods of the two mechanical arms interfere under the small path:
if not, directly executing the action of the small path;
if the interference occurs, keeping the motion planning of the small section of path of the driving arm unchanged, selecting a group of solutions which meet the condition that the mechanical arm connecting rod has no interference and the weighted mechanical arm joint motion distance is optimal from the inverse kinematics solution of the small section of path of the driven arm, using the solutions as control parameters of the driven arm when the current small section of path moves, performing the motion planning on the driven arm again, and then executing the action of the small section of path.
5. The underwater double-arm cooperative grabbing, embracing and collision preventing integrated method for the ship as claimed in claim 4, wherein the minimum size collision preventing method is adopted in step S5 to calculate the staggered angle of the driven arm, and the method comprises the following steps:
assuming that the distance from a connecting rod connected with the tail end of the driven arm to the shoulder joint is L, the diameter of the thickest section of the connecting rod is D, the shoulder joint only needs to move reversely by an angle theta towards the current movement direction to ensure that the tail ends of the two arms do not collide, and the collision-preventing movement angle of the shoulder joint is as follows:
substituting theta into positive kinematic equation of the mechanical arm to calculate space coordinate (x) of the tail end of the driven arm2，y2，z2) And coordinates (x) of the active arm end1，y1，z1) Making a comparison if x1≠x2Or y1≠y2Or z1≠z2Then, it means that no collision occurs at the position of the two-arm end, and the coordinate point (x) of the slave arm end is at this time2，y2，z2) And (4) interpolating points for the new track.
6. The underwater dual-arm cooperative grabbing, embracing and collision avoidance integrated method according to claim 4 or 5, wherein the method for obtaining the optimal solution satisfying the condition that the mechanical arm connecting rod has no interference and the weighted mechanical arm joint movement distance in the step S6 is as follows:
setting the slave arm to have N joints, firstly defining a weight omega for each joint of the slave arm according to the priority motion sequence on the motion planiThe weight of the joint which preferentially moves is higher; assume that the initial angle of the ith joint at the current segment is θ0iObtaining solutions of multiple groups of joint angles corresponding to the target interpolation point of the current small section through inverse solution, and after removing the solution which can cause the interference of the connecting rod, remaining M groups of solutions, wherein the weighted mechanical arm joint movement distance a corresponding to the solution of the k group of joint angleskComprises the following steps:
calculated akAnd a group of joint angles corresponding to the minimum value in the motion vector calculation is a solution which meets the condition that the mechanical arm connecting rod has no interference and the weighted mechanical arm joint motion distance is optimal, and the group of joint angles are used as motion control parameters of the driven arm and executed.
7. An underwater double-arm cooperative grabbing, embracing and collision preventing integrated system is characterized by comprising a controller and a collision preventing program module, wherein the collision preventing program module is used for realizing the double-arm cooperative grabbing, cooperative embracing and collision preventing integrated method as claimed in any one of claims 1 to 6 when being called by the controller.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910214006.1A CN109822554B (en) | 2019-03-20 | 2019-03-20 | Underwater dual-arm cooperative grabbing, holding and collision avoidance integrated method and system |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910214006.1A CN109822554B (en) | 2019-03-20 | 2019-03-20 | Underwater dual-arm cooperative grabbing, holding and collision avoidance integrated method and system |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN109822554A CN109822554A (en) | 2019-05-31 |
| CN109822554B true CN109822554B (en) | 2020-09-18 |
Family
ID=66870558
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910214006.1A Active CN109822554B (en) | 2019-03-20 | 2019-03-20 | Underwater dual-arm cooperative grabbing, holding and collision avoidance integrated method and system |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN109822554B (en) |
Families Citing this family (15)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112642741B (en) * | 2019-10-12 | 2023-02-28 | 顺丰科技有限公司 | Article sorting method and device, industrial control equipment and storage medium |
| CN110802578B (en) * | 2019-11-13 | 2022-08-30 | 吉林大学 | Maximum safe operation range detection method for upper limb of wearable robot |
| CN112264989B (en) * | 2020-06-17 | 2023-06-06 | 华中科技大学 | Double-mechanical-arm cooperative obstacle avoidance method based on neighborhood traversal |
| CN111633665B (en) * | 2020-07-03 | 2023-10-03 | 桂林电子科技大学 | Device for grabbing electromagnetic transducer to assist nondestructive testing and track planning method thereof |
| CN112068576B (en) * | 2020-07-21 | 2022-07-12 | 华中科技大学 | Task-hierarchical timing optimization-based underwater unmanned ship-double mechanical arms cooperative control method |
| US20220129851A1 (en) * | 2020-09-21 | 2022-04-28 | Vivek Mishra | System and method for making strategies and generating customized reports based on users in real-time |
| CN112372644B (en) * | 2020-11-06 | 2022-06-17 | 中国科学院合肥物质科学研究院 | Efficient sorting method of robot |
| CN113459089B (en) * | 2021-06-09 | 2022-04-29 | 华中科技大学 | Dynamics coupling effect evaluation method for underwater unmanned ship-double-mechanical-arm operation system |
| CN113664874A (en) * | 2021-08-30 | 2021-11-19 | 湖南工业大学 | Interference detection method for multiple arms of mobile robot |
| CN113715027A (en) * | 2021-09-13 | 2021-11-30 | 哈工大机器人（合肥）国际创新研究院 | 7-degree-of-freedom double-arm cooperative robot self-collision prevention trajectory planning method and system |
| CN113843794B (en) * | 2021-09-24 | 2022-12-09 | 清华大学 | Method and device for planning coordinated movement of two mechanical arms, electronic equipment and storage medium |
| CN113878587B (en) * | 2021-11-10 | 2023-03-17 | 天津大学 | Energy optimization control method and device for rotary shell of underwater swimming mechanical arm |
| CN113799143B (en) * | 2021-11-18 | 2022-04-19 | 广东隆崎机器人有限公司 | Safe cooperation method and device of multiple robots in working area |
| CN114454178B (en) * | 2022-03-15 | 2023-11-24 | 南京航空航天大学 | Heterogeneous integrated manufacturing-oriented double-robot collaborative path planning method and system |
| CN114872938A (en) * | 2022-05-12 | 2022-08-09 | 上海交通大学 | Self-growing flexible variable stiffness mechanical arm space cross-size target automatic capture control method |
Family Cites Families (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP4528312B2 (en) * | 2007-02-02 | 2010-08-18 | 川田工業株式会社 | Dual-arm robot shoulder width limiting device and dual-arm robot equipped with the device |
| CN107584474B (en) * | 2017-07-19 | 2020-12-15 | 合肥工业大学 | Spherical joint double-arm robot coordinated movement method based on geometric projection |
| CN107571246B (en) * | 2017-10-13 | 2020-07-31 | 上海神添实业有限公司 | Part assembling system and method based on double-arm robot |
| CN107891425B (en) * | 2017-11-21 | 2020-05-12 | 合肥工业大学 | Control method of intelligent double-arm safety cooperation man-machine co-fusion robot system |
| CN108838991B (en) * | 2018-03-06 | 2022-03-25 | 南昌工程学院 | Autonomous humanoid double-arm robot and tracking operation system thereof for moving target |
| CN109249391A (en) * | 2018-08-30 | 2019-01-22 | 厦门理工学院 | A kind of robot interactive cooperation method and system |
-
2019
- 2019-03-20 CN CN201910214006.1A patent/CN109822554B/en active Active
Also Published As
| Publication number | Publication date |
|---|---|
| CN109822554A (en) | 2019-05-31 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109822554B (en) | Underwater dual-arm cooperative grabbing, holding and collision avoidance integrated method and system | |
| JP6854549B2 (en) | AUV action planning and motion control methods based on reinforcement learning | |
| CN110262478B (en) | Man-machine safety obstacle avoidance path planning method based on improved artificial potential field method | |
| CN106094606A (en) | A kind of unmanned surface vehicle navigation and control remote-controlled operation platform | |
| WO2022095278A1 (en) | Qmix reinforcement learning algorithm-based ship welding spots collaborative welding method using multiple manipulators | |
| CN112428274B (en) | Space motion planning method of multi-degree-of-freedom robot | |
| CN112327872B (en) | Double unmanned ship cooperative track tracking method for oil spill containment | |
| CN110162053A (en) | The adaptive behavior fusion method that the more unmanned boats of isomery are formed into columns | |
| CN111515954B (en) | Method for generating high-quality motion path of mechanical arm | |
| CN115533920A (en) | Collaborative planning method and system for solving inverse kinematics of rope-driven mechanical arm and computer storage medium | |
| CN112698574A (en) | Hybrid task priority based double-arm space robot coordination control method | |
| Nazarzehi et al. | Decentralized three dimensional formation building algorithms for a team of nonholonomic mobile agents | |
| Filaretov et al. | Synthesis method of reference control signals for manipulators installed on autonomous underwater vehicles | |
| Kramar et al. | Self-collision avoidance control of dual-arm multi-link robot using neural network approach | |
| Kang et al. | Fuzzy logic based behavior fusion for multi-AUV formation keeping in uncertain ocean environment | |
| Wang et al. | Research on intelligent obstacle avoidance control method for mobile robot in multi-barrier environment | |
| CN115185262A (en) | Dynamic obstacle avoidance path rapid planning method based on minimum safe meeting distance | |
| Lin et al. | Research on multi-usv cooperative search method | |
| Silva et al. | Path planning optimization for a mobile manipulator | |
| Afaghani et al. | Advanced-collision-map-based on-line collision and deadlock avoidance between two robot manipulators with PTP commands | |
| Pi et al. | Multi-representation multi-heuristic A* motion planning for a dual-arm underwater vehicle manipulation system | |
| Chen et al. | An improved belief function method for path planning of AUV | |
| CN113084797B (en) | Dynamic cooperative control method for double-arm redundant mechanical arm based on task decomposition | |
| CN109807913A (en) | A kind of tow-armed robot removal of mines cooperative control system and method for removing mines | |
| CN115248599B (en) | Priority-variable multi-robot zero-space behavior fusion formation method |
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 |