CN112068576A - Task-hierarchical timing optimization-based underwater unmanned ship-double mechanical arms cooperative control method - Google Patents
Task-hierarchical timing optimization-based underwater unmanned ship-double mechanical arms cooperative control method Download PDFInfo
- Publication number
- CN112068576A CN112068576A CN202010700570.7A CN202010700570A CN112068576A CN 112068576 A CN112068576 A CN 112068576A CN 202010700570 A CN202010700570 A CN 202010700570A CN 112068576 A CN112068576 A CN 112068576A
- Authority
- CN
- China
- Prior art keywords
- task
- underwater
- arm
- tasks
- mechanical arms
- 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.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 35
- 238000005457 optimization Methods 0.000 title claims abstract description 24
- 230000033001 locomotion Effects 0.000 claims abstract description 46
- 230000009471 action Effects 0.000 claims abstract description 7
- 239000012636 effector Substances 0.000 claims description 14
- 230000036544 posture Effects 0.000 claims description 11
- 239000013598 vector Substances 0.000 claims description 11
- 239000011159 matrix material Substances 0.000 claims description 8
- 230000008859 change Effects 0.000 claims description 7
- 230000000007 visual effect Effects 0.000 claims description 7
- 230000007613 environmental effect Effects 0.000 claims description 5
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 claims description 5
- 230000009977 dual effect Effects 0.000 claims description 3
- 230000008569 process Effects 0.000 claims description 2
- 230000005540 biological transmission Effects 0.000 description 10
- 210000000078 claw Anatomy 0.000 description 10
- 238000010586 diagram Methods 0.000 description 8
- 238000004088 simulation Methods 0.000 description 8
- 125000006850 spacer group Chemical group 0.000 description 6
- 230000006872 improvement Effects 0.000 description 5
- 230000004888 barrier function Effects 0.000 description 4
- 238000007796 conventional method Methods 0.000 description 4
- 238000005096 rolling process Methods 0.000 description 4
- 210000003857 wrist joint Anatomy 0.000 description 4
- 230000002411 adverse Effects 0.000 description 3
- 230000005484 gravity Effects 0.000 description 3
- 210000000323 shoulder joint Anatomy 0.000 description 3
- 230000009286 beneficial effect Effects 0.000 description 2
- 230000007547 defect Effects 0.000 description 2
- 238000011161 development Methods 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 210000000245 forearm Anatomy 0.000 description 2
- 238000012423 maintenance Methods 0.000 description 2
- 210000000707 wrist Anatomy 0.000 description 2
- 241000702489 Maize streak virus Species 0.000 description 1
- 238000004873 anchoring Methods 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 210000004247 hand Anatomy 0.000 description 1
- 238000011835 investigation Methods 0.000 description 1
- 230000001788 irregular Effects 0.000 description 1
- 210000001503 joint Anatomy 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000005070 sampling Methods 0.000 description 1
- 239000013049 sediment Substances 0.000 description 1
- 230000000087 stabilizing effect Effects 0.000 description 1
- 239000000725 suspension Substances 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/04—Control of altitude or depth
- G05D1/06—Rate of change of altitude or depth
- G05D1/0692—Rate of change of altitude or depth specially adapted for under-water vehicles
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention discloses an underwater operation-oriented collaborative motion planning method for an underwater unmanned ship and two mechanical arms and application thereof. The invention combines the control of the underwater double mechanical arms with a planning method based on task grading time sequence optimization, subdivides the actions to be executed by the underwater robot into a plurality of tasks, and reasonably optimizes and divides the execution sequence of the tasks, so as to reduce the dynamic disturbance of the mechanical arm movement to the underwater unmanned ship and enable the underwater unmanned ship to run more stably. The control method sets the tasks of keeping the hovering height of the unmanned ship, avoiding obstacles and the like as high priority, and ensures the safety of equipment as much as possible. The invention solves the problem of difficult control of the underwater unmanned ship-double-arm manipulator, realizes the purpose of autonomous safe operation of the underwater double-arm manipulator and stable operation of the pose of the integral operation platform formed by the underwater unmanned ship-double-arm manipulator, and creates safe and stable platform technical support for improving the underwater operation capability.
Description
Technical Field
The invention belongs to the field of mechanical arm motion planning, and particularly relates to an underwater robot-double mechanical arms cooperative control method based on task grading time sequence optimization.
Background
With the increase of the attention on ocean development, various novel ocean exploration technologies are developed, wherein the application of the underwater manipulator in underwater operations such as deep sea scientific investigation, submarine sediment sampling, underwater maintenance and the like is increasingly wide. Manned vehicles (MSVs) and Remotely Operated Vehicles (ROVs) are often equipped with underwater manipulators that are operated directly by personnel in the vehicle or remotely via cables. With the development of underwater technologies such as underwater scientific research, underwater maintenance, submarine cable detection and the like, the importance of the double mechanical arms to the underwater robot is gradually improved. The underwater autonomous operation system formed by the underwater unmanned ship and the double-arm manipulator has the following advantages: firstly, the two mechanical arms have higher bearing capacity and can grab and transport a target object with higher mass; secondly, the double mechanical arms can complete tasks which cannot be completed by a single mechanical arm, for example, one mechanical arm fixes a robot base by grabbing and anchoring, and the other hand completes operation, or one hand grabs or fixes a target, and the other hand maintains and samples the grabbed target, so that the operation efficiency and the operation capacity are improved; and finally, the double mechanical arms have stronger adaptability to the shape of the grabbed target object, and can complete the grabbing task of a sphere or other irregular objects in a manner of encircling by two hands.
However, the problem of poor stability of the underwater unmanned boat-mechanical arm system has been present. The base of the land robot can be fixed or relatively fixed, so that the system can stably run as long as the bearing capacity of the mechanical arm system is large enough, and the influence of the weight of a target object and the number of mechanical arms is small. Different from a land robot, the stable operation of the unmanned ship-double mechanical arms in an underwater environment needs to keep the relative stability of the gravity center position, namely, the gravity and the buoyancy vector of the system act on the same line, otherwise, the moment formed by the action of the gravity and the buoyancy can cause the inclination of the underwater unmanned ship-double mechanical arm system. Due to the fact that the deep sea environment is extremely complex, if the unmanned boat cannot keep relative stability of pose, disorder of a robot coordinate system is easily caused, the underwater operation robot cannot complete specified tasks, even collides with environmental obstacles, and great economic loss is generated. Under the suspension state of the underwater robot, the position and the posture of the underwater robot are easily disturbed by the motion of the carried mechanical arm, and if the underwater robot is expected to keep relatively stable posture, complex dynamics compensation is necessary, so that the control difficulty and the control cost are increased. Therefore, if the subtasks can be reasonably divided for a specific operation task and executed according to the optimized time sequence, the disturbance of the mechanical arm to the pose of the underwater robot can be greatly reduced, the control difficulty of the whole underwater unmanned ship-double-arm mechanical arm system is greatly reduced, and the simplification of the control is beneficial to the whole underwater unmanned ship-double-mechanical arm system. If appropriate human intervention is carried out on the joint configuration, the unfolding mode and the unfolding sequence of the underwater double mechanical arms, the pose stability of the underwater unmanned ship can be improved, and the unmanned ship can be helped to well keep stable operation.
Disclosure of Invention
The invention aims to provide a task-based hierarchical time sequence optimization-based underwater unmanned ship-double-mechanical-arm cooperative control method, so as to improve the safety of operation of an underwater robot in a complex and dangerous underwater environment, reduce the adverse disturbance of the movement of a mechanical arm to the posture of a mother ship of the underwater robot, enable the unmanned ship to keep the posture relatively stable more easily, and improve the operation efficiency and the operation capacity.
In order to achieve the above object, the present invention adopts the following technical solutions.
According to the invention, a task priority grading time sequence optimization method is adopted, so that the underwater operation control difficulty of an underwater unmanned ship-double mechanical arm system is reduced, and the safe execution of tasks is ensured; the symmetry advantages of the two mechanical arms are fully utilized, reasonable task priority planning and dual motion planning are carried out on the grabbing actions of the two-arm underwater robot, the two mechanical arms can be controlled to complete underwater operation tasks, joint configurations and unfolding processes of the two mechanical arms can be intelligently interfered, the coordination of an underwater unmanned ship-two mechanical arm cooperative work system is increased, the purpose of reducing dynamic disturbance of mechanical arm motions to an underwater robot mother ship is achieved, and preconditions are created for accurate operation of an underwater unmanned ship-two mechanical arm platform.
The following tasks are subdivided for the grabbing action of the underwater double-mechanical-arm robot, the execution sequence of the underwater double-mechanical-arm robot is optimized and divided, and the sequence number represents the task execution sequence:
(1) keeping the distance between the underwater robot and the water bottom larger than the minimum height h0；
(2) The unmanned boat and the mechanical arm avoid the barrier, and the distance between the equipment and the barrier is kept to be larger than the safe distance l0；
(3) Limiting the joint angle of the mechanical arm, and keeping the joint angle to change theta within the angle limiting rangemin<θ<θmax；
(4) Keeping the target object in the center of the visual field range of the camera;
(5) under the condition that environmental conditions allow, based on single-arm task priority and double-arm dual-motion method, two mechanical arms relate to x of unmanned boat0oz0Plane symmetry movement;
(6) controlling the positions of the end effectors of the two mechanical arms;
(7) controlling the postures of the end effectors of the two mechanical arms;
(8) optimizing the configuration of the double mechanical arms;
(9) motion minimization of the underwater unmanned vehicle;
(10) the motion of the two mechanical arms is minimized.
Tasks (1) - (3) correspond to physical constraints and safety control targets, task (4) is a prerequisite condition that the mechanical arm can grab the target, tasks (5) - (7) correspond to core grabbing tasks, and tasks (8) - (10) correspond to control optimization targets.
The invention mainly aims at the first to third tasks to carry out priority planning, the value range of the Taskpriority is {1,2,3}, and when the Taskpriority value is i, the ith task is executed.
Calculating the priority of the first to third tasks respectively:
P1＝min{h-h0,l-l0,θ-θmin,θmax-θ}
P2＝k2·D
P3＝k3·(|pc-pg|+|φc-φg|+|θc-θg|+|ψc-ψg|)
wherein, PiIndicates the priority (i is 1,2,3) of the ith task, kjA priority coefficient (j is 2,3) representing the j-th task, h represents the current distance between the underwater robot and the water bottom, l represents the current minimum distance between equipment and an obstacle, theta represents the current joint angle of all joints of the two mechanical arms, D represents the deviation distance between the center of the target object and the center of the visual field range of the camera, and p represents the current distance between the center of the target object and the center of the visual field range of the cameracRepresenting the current coordinates, p, of the end effector (i.e., the gripper portion)gRepresents the coordinates of the object, [ phi ]c,θc,ψc]Represents the current attitude of the end effector, [ phi ]g,θg,ψg]Representing an end effector target pose.
The Taskpriority value rule is as follows: get1,2Two sufficiently small positive numbers (1In order to meet the coefficients of safe sailing height and safe limit of the system,2to satisfy the factor of the robot arm end effector moving to the vicinity of the target point), if P) is present1<1The Taskpriority value is 1; if P1＞1，P3＞2Then compare P2And P3If P is the size of2＞P3The Taskpriority value is 2, if P2<P3The Taskpriority value is 3; if P1＞1，P3<2And considering that the underwater unmanned ship-two mechanical arms complete the grabbing task.
The specific explanation of task (5) is as follows:
(1) only one robot arm is required to grasp a single object. The conventional method is to control only the movement of the mechanical arm, and the other mechanical arm keeps the folding state. When the mechanical arm is unfolded towards the outer side of the unmanned boat body, the unmanned boat can be acted by torque which enables the unmanned boat to roll, the posture of the unmanned boat is changed, and the relative stability of the boat needs to be kept through complex dynamics compensation. The improvement mode is as follows: mechanical arm for controlling grabbing taskControlling x of another mechanical arm relative to the unmanned boat while moving0oz0The plane and the task mechanical arm symmetrically move to reduce the transverse tilting moment generated by the mechanical arm movement on the unmanned ship so as to achieve the purpose of reducing disturbance;
(2) when both arms have their own gripping task. The conventional method is to control two mechanical arms to reach respective target points in a path optimal planning mode. The defect of the mode is that the complex motion of the two mechanical arms can generate complex and variable tilting moment on the system, and the disturbance on the unmanned boat is large. The improvement mode is as follows: firstly analyzing the motion paths of the two mechanical arms, selecting the shorter one, controlling the mechanical arms to move to a target point to grab based on the priority task, and simultaneously controlling the other mechanical arm to be related to the x of the unmanned boat0oz0The plane and the task mechanical arm carry out symmetrical movement. After the first mechanical arm finishes grabbing, controlling the second mechanical arm to move to a corresponding target point, and meanwhile, enabling the first mechanical arm to be related to x0oz0The plane and the task mechanical arm carry out symmetrical movement. The control mode also reduces the transverse tilting moment generated by the motion of the mechanical arm to the unmanned ship so as to achieve the purpose of reducing disturbance.
The specific optimization algorithm for tasks (9) - (10) is as follows:
velocity of end of manipulator
The following relations exist among linear velocity, angular velocity and angular velocity vector xi of manipulator joint of the submarine body
Inverting the above formula to obtain:
in the formula
Is JxThe pseudo-inverse of (a) is,
is the desired speed, ξ, of the end of the manipulatorrA theoretical velocity vector calculated for inverse kinematics;
this solution corresponds to the minimization of the velocity least squares function:
the weight W is introduced into the joint speeds of the boat body and the mechanical arm:
the weighted pseudo-inverse is therefore:
the general solution for the cost function is:
wherein N is 6+ N which is the sum of six degrees of freedom of the boat body and the degree of freedom of the manipulator; xiaThe speed vectors of the boat body and the manipulator are obtained; n is a radical ofaAs an operator, is a Jacobian matrix JaA joint velocity vector in null space of (a); xi in the formulaaDescribed are system secondary tasks, which are related to the gradient values of the objective function by selecting xiaTo achieve a local minimum as follows:
where H (q) is the objective function, kHIn order to be the gain factor,the system speed is determined, and therefore can be expressed as:
in the formula
The system primary task is represented for the pseudo-inverse solution term,
the null-space solution item represents a system secondary task, the primary task and the secondary task can be planned through specific operation conditions, the primary task is expected to be completed preferentially under general conditions, if the primary task and the secondary task conflict, the primary task is high in priority, the primary task is guaranteed to be completed, and the secondary task is completed as far as possible;
the method is generalized to a general form under a multitask mode, and the main task eta of the system is defined firstp∈RmThe corresponding Jacobian matrix is Jp(q), m is the number of dimensions required by the task, and can be obtained as follows:
similarly, a secondary task η can be definedi∈RtThe Jacobian matrix of which is Ji(q), namely:
where i ∈ N+Representing the number of secondary tasks; a priority planning algorithm under multiple tasks:
in the formula
A jacobian matrix corresponding to each secondary task.
The invention has the beneficial effects that:
the invention provides a control method based on task grading time sequence optimization aiming at the problems of complex and dangerous working environment of an underwater unmanned ship-double mechanical arm system, the method preferentially ensures the safety performance of the underwater unmanned ship-double mechanical arm system, and ensures that the underwater unmanned ship-double mechanical arms do not touch the bottom and collide with environmental obstacles as much as possible in the complex underwater environment, thereby avoiding equipment damage.
Aiming at the problem that the posture of the mother boat of the underwater robot is easily disturbed by the motion of the mechanical arm, the invention provides a control method based on task grading optimization and the control of the underwater unmanned boat-double mechanical arms, so that the motion of the underwater unmanned boat-double mechanical arms is more stable, and the complex dynamic compensation required for keeping the posture of the mother boat relatively stable is reduced.
When the two mechanical arms are required to cooperatively operate, the adverse disturbance to the pose of the underwater unmanned ship during operation of the two arms is reduced based on the double-mechanical-arm cooperative motion planning strategy with the single-arm task priority, and the underwater operation complexity and the control difficulty of the underwater unmanned ship-double-arm mechanical-arm operation platform are reduced.
When the underwater combined operation platform only needs a single mechanical arm to operate, the configuration characteristics of double arms are fully utilized, and when the underwater combined operation platform only needs the single mechanical arm to operate, the unfavorable disturbance to the pose of the underwater robot during the single-arm operation is compensated and reduced by introducing double-arm dual motion.
Drawings
FIG. 1 is a system framework diagram of the method of the present invention;
FIG. 2 is a flow chart of a task-level timing optimization planning method;
FIG. 3 is a diagram of an embodiment of an underwater robot;
FIG. 4 is a schematic structural diagram of an underwater dual-robot in an embodiment;
FIG. 5 is a simplified coordinate system diagram of two robots in an embodiment;
fig. 6 is a schematic diagram of motion simulation of an underwater unmanned vehicle-double mechanical arm system.
Detailed Description
For a better understanding of the present invention, the technical solutions of the present invention will be described in detail below with reference to the accompanying drawings and examples.
FIG. 1 is a schematic system diagram of the method of the present invention.
Fig. 2 is a flowchart of a task hierarchical timing optimization planning method.
Fig. 3 is a physical diagram of the underwater double mechanical arms in the embodiment. The specific parameters of the underwater double mechanical arm are shown in the following table.
TABLE 1 double arm parameters (double arm spacing 600mm)
As shown in fig. 4, the two robots may be divided into five parts, namely, a shoulder joint (shoulder part), a forearm joint (forearm part), a wrist joint (wrist part), and a claw joint (claw part).
The shoulder portion includes: the shoulder joint motor (115), a base (135), a backing plate (147), a bearing cover (145), a transmission shaft 1(146), a motor cover (144) and a spacer; one end of a backing plate (147) is connected with an underwater unmanned underwater vehicle (carrying object), the other end of the backing plate is connected with a base (135), a bearing cover (145) and a motor cover (144) are installed on two sides of the base (135), a shoulder joint motor (115) is installed on one side of the base (135) and is connected with a transmission shaft 1(146) through a motor shaft to transmit the rotation of the motor.
The large arm portion includes: the large arm joint motor (119), the short arm (134), the bearing cover (145), the transmission shaft (2), (149), the motor cover (144), the large arm (84) and the spacer; the short arm (134) is connected with the transmission shaft 1(146), the large arm joint motor (119) and the transmission shaft 2(149) are arranged at the junction circle of the short arm (134) and the large arm (84) and are connected through the motor shaft, the bearing cover (145) and the motor cover (144) are arranged on two sides of the short arm (134), and the spacer is arranged between the bearing and the large arm (84).
The small arm portion includes: the device comprises a small arm joint motor (126), a small arm (83), a bearing cover (145), a transmission shaft (3) (86), a motor cover (144) and a spacer; the small arm joint motor (126) and the transmission shaft (3) (86) are arranged at the junction circle of the small arm (83) and the large arm (84) and are connected through the motor shaft, the bearing cover (145) and the motor cover (144) are arranged at two sides of the small arm (83), and the spacer is arranged between the bearing and the small arm (83).
The wrist section includes: a wrist joint motor (79), a shaft sleeve, a flange (12), a bearing seat (38), a sliding sleeve (12), a transmission cover (34), a small cover (37) and a box body (16); the wrist joint motor (79) is arranged inside the small arm (83) and is connected with the flange; the transmission cover (34) is connected with a wrist joint motor (79) through a shaft and used for transmitting torque; the transmission shaft is connected with the bearing seat (38), and the bearing seat (38) is connected with the box body (16); so that the motor can drive the box body (16) and the claw part (1) to rotate together.
The claw portion includes: a claw joint motor (17), a sliding sleeve (12), a T-shaped screw rod (33), a flange, a shifting block (11), a small shifting block (9), a push rod (8), a guide sleeve, a bracket (6), a spacer bush, a pin shaft (3), a connecting rod (5) and a claw part (1); the T-shaped screw rod (33) is connected with a motor shaft and transmits torque; the claw joint motor (17) drives the T-shaped screw rod (33) to rotate and drives the shifting block (11) to move up and down along the thread; the small shifting block (9) is fixed at a notch at one side of the shifting block (11), and a protruding part at the other side is clamped at the notch of the push rod (8), so that the shifting block (11) can drive the push rod (8) to move along the axial direction; a sliding sleeve (12) is additionally arranged to limit the push rod (8) so that the push rod can only move in the bracket (6) along the axial direction; the pin shaft (3) is arranged at the other side of the push rod (8) and moves up and down along the opening of the bracket (6) to drive the upper connecting rod (5) to rotate; one end of the connecting rod (5) is connected with the claw part (1), the other end of the connecting rod is connected with one corner of the claw part (1) on the pin shaft (3), and when the connecting rod (5) moves, the claw part (1) is naturally driven to open and close.
The robot arm can be simplified into a robot arm coordinate system schematic diagram as shown in fig. 5 by real objects and corresponding parameters, wherein a z axis is a rotating axis of each joint.
Aiming at a task-based hierarchical time sequence optimization method, the following task subdivision and hierarchical optimization are carried out on the grabbing action of an underwater unmanned ship-double mechanical arm system, and the sequence number represents the task execution sequence:
(1) keeping the distance between the underwater robot and the water bottom to be larger than the lowest heightDegree h0；
(2) The unmanned boat and the mechanical arm avoid the barrier, and the distance between the equipment and the barrier is kept to be larger than the safe distance l0；
(3) Limiting the joint angle of the mechanical arm, and keeping the joint angle to change theta within the angle limiting rangemin<θ<θmax；
(4) Keeping the target object in the center of the visual field range of the camera;
(5) under the condition that environmental conditions allow, based on a single-arm task priority and dual-arm dual-couple movement method, the two mechanical arms move symmetrically about an xoz plane;
(6) controlling the positions of the end effectors of the two mechanical arms;
(7) controlling the postures of the end effectors of the two mechanical arms;
(8) optimizing the configuration of the double mechanical arms;
(9) motion minimization of the underwater unmanned vehicle;
(10) the motion of the two mechanical arms is minimized.
Tasks (1) - (3) correspond to physical constraints and safety control targets, task (4) is a prerequisite condition that the mechanical arm can grab the target, tasks (5) - (7) correspond to core grabbing tasks, and tasks (8) - (10) correspond to control optimization targets.
For task (5), the following is specifically explained:
(1) only one robot arm is required to grasp a single object. The conventional method is to control only the movement of the mechanical arm, and the other mechanical arm keeps the folding state. When the mechanical arm is unfolded towards the outer side of the unmanned boat body, the unmanned boat can be acted by torque which enables the unmanned boat to roll, the posture of the unmanned boat is changed, and the relative stability of the boat needs to be kept through complex dynamics compensation. The improvement mode is as follows: controlling the movement of a robot arm with a gripping task and at the same time controlling another robot arm with respect to x0oz0The plane and the task mechanical arm symmetrically move to reduce the transverse tilting moment generated by the mechanical arm movement on the unmanned ship so as to achieve the purpose of reducing disturbance;
(2) when both arms have their own gripping task. The conventional method is to control two mechanical arms to take paths respectivelyThe optimal planning mode reaches respective target points. The defect of the mode is that the complex motion of the two mechanical arms can generate complex and variable tilting moment on the system, and the disturbance on the unmanned boat is large. The improvement mode is as follows: firstly analyzing the motion paths of the two mechanical arms, selecting the shorter one, controlling the mechanical arms to move to a target point to grab based on the priority task, and simultaneously controlling the other mechanical arm to move to the target point relative to x0oz0The plane and the task mechanical arm carry out symmetrical movement. After the first mechanical arm finishes grabbing, controlling the second mechanical arm to move to a corresponding target point, and meanwhile, enabling the first mechanical arm to be related to x0oz0The plane and the task mechanical arm carry out symmetrical movement. The control mode also reduces the transverse tilting moment generated by the motion of the mechanical arm to the unmanned ship so as to achieve the purpose of reducing disturbance.
In order to verify the contribution of the subdivision task (5) to reducing system disturbance and stabilizing the attitude of the underwater unmanned ship-double mechanical arm system, digital simulation work is carried out based on the proposed method. The system is first defined as follows: defining the pose vector of the unmanned ship as
Wherein
Representing the position coordinates of the unmanned boat in an absolute coordinate system,
representing the euler angular coordinates of the unmanned boat in an absolute coordinate system. Defining the velocity vector of the unmanned ship as
Wherein
Representing the linear velocity of the unmanned ship under an absolute coordinate system,
representing the angular velocity of the unmanned boat in an absolute coordinate system. The specific simulation analysis is as follows:
firstly, the pose change of the underwater robot with only one mechanical arm moving is analyzed. The whole system has no power compensation, so that the influence of the swing of the mechanical arm on the pose of the unmanned ship can be clearly seen. Applying a cubic polynomial interpolation track planning method to the mechanical arm, and setting the initial joint configuration of the mechanical arm as qs＝[0° 0° 0°]The target joint configuration is qf＝[90° 60°-30°]The average angular velocity of the joint motion was 15 deg./s and the total simulation time was 15 s.
And the dual-arm dual-motion replaces the single-arm motion. The pose change of the underwater robot with the double arms moving in a symmetrical manner about the xoz plane is analyzed. The cubic polynomial interpolation trajectory planning method is also applied to the double mechanical arms, and the starting joint configuration of the double mechanical arms is set to be qs1＝[0° 0° 0°]，qs2＝[0° 0° 0°]The target joint configuration is qf1＝[90°60°-30°]，qf2＝[90°-60°30°]Namely, one mechanical arm is moved to the target joint configuration of the single mechanical arm in the simulation (r), and the movement of the other mechanical arm is symmetrical about the xoz plane. The average angular velocity of the joint movement was 15 deg./s and the total simulation time was 15 s. The simulation results are shown in fig. 6.
According to the Euler angle change curve of the unmanned ship with only one mechanical arm for movement, when the single arm performs the movement, the unmanned ship can generate large shaking, specifically, the unmanned ship can generate back and forth rolling, pitching and yawing, the maximum value of the rolling is about-1.6 degrees, the maximum value of the pitching is about-3.5 degrees and the maximum value of the yawing is 2.0 degrees in 15s simulation time. From the above analysis, if the attitude of the unmanned ship is expected to be kept relatively stable, corresponding power compensation needs to be performed in the three directions of rolling, pitching and yawing of the unmanned ship so as to offset the influence of the motion of the mechanical arm on the attitude of the unmanned ship, and the control is relatively adverse. As can be seen from the euler angle change curve of the unmanned ship in which the double arms move symmetrically about the xoz plane, when the double robot arms perform the above-described movement, the unmanned ship is caused to pitch, which is equivalent to the pitch of the unmanned ship when the single robot arm moves. However, when the two mechanical arms move in the symmetrical manner, the unmanned ship hardly generates rolling and yawing, and the stability of the unmanned ship is greatly improved, which means that when the two mechanical arms move in the symmetrical manner, if the attitude of the unmanned ship is expected to be relatively stable, corresponding power compensation is only needed to be performed on the pitching direction of the unmanned ship, and the attitude control of the UVMS system is greatly simplified.
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 (6)
1. An underwater unmanned ship-double mechanical arms collaborative planning and control method based on task hierarchical time sequence optimization is characterized in that: by adopting a task priority grading time sequence optimization method, the underwater operation control difficulty of an underwater unmanned ship-double mechanical arm system is reduced, and the safe execution of tasks is guaranteed; the method fully utilizes the symmetry advantages of the two mechanical arms, reasonably performs task priority planning and dual motion planning on the grabbing action of the two-arm underwater robot, can control the two mechanical arms to complete underwater operation tasks, and can intelligently intervene the joint configuration and the unfolding process of the two mechanical arms to increase the harmony of an underwater unmanned ship-two mechanical arm cooperative work system, thereby achieving the purpose of reducing the dynamic disturbance of the mechanical arm motion on an underwater robot mother ship, and creating a prerequisite condition for the accurate operation of an underwater unmanned ship-two mechanical arm platform, and the specific implementation method comprises the following steps: the following tasks are subdivided for the grabbing action of the underwater double-mechanical-arm robot, the execution sequence of the underwater double-mechanical-arm robot is optimized and divided, and the sequence number represents the task execution sequence:
(1) keeping the distance between the underwater robot and the water bottom larger than the minimum height h0；
(2) The obstacle avoidance of the unmanned boat body and the mechanical arm keeps the distance between the unmanned boat and the obstacle to be greater than ampereTotal distance l0；
(3) Limiting the joint angle of the mechanical arm, and keeping the joint angle to change theta within the angle limiting rangemin＜θ＜θmax；
(4) Keeping the captured target object in the center of the visual field range of the camera;
(5) under the condition that environmental conditions allow, based on single-arm task priority and double-arm dual-motion method, two mechanical arms relate to x of unmanned boat0oz0Plane symmetry movement;
(6) controlling the positions of the end effectors of the two mechanical arms;
(7) controlling the postures of the end effectors of the two mechanical arms;
(8) optimizing the configuration of the double mechanical arms;
(9) motion minimization of the underwater unmanned vehicle;
(10) the motion of the two mechanical arms is minimized.
2. The task-based hierarchical optimization control method for the underwater unmanned vehicle-double mechanical arms according to claim 1, is characterized in that: the task sequence is divided into the following categories: class 1: tasks (1) to (3) correspond to physical constraints and safety control objectives, category 2: task (4) is a prerequisite for the robot arm to be able to grasp an object, category 3: tasks (5) - (7) correspond to core capture tasks, category 4: tasks (8) - (10) correspond to control optimization objectives.
3. The task-based hierarchical optimization control method for the underwater unmanned vehicle-double mechanical arms according to claim 2, is characterized in that: the invention mainly aims at the tasks of the 1 st to the 3 rd types to carry out priority planning, the value range of the task priority is {1,2,3}, and when the value of the task priority is i, the task priority is executed.
4. The task-based hierarchical optimization control method for the underwater unmanned vehicle-double mechanical arms according to claim 3, is characterized in that: calculating the priority of the first to third tasks respectively:
P1＝min{h-h0，l-l0，θ-θmin，θmax-θ}
P2＝k2·D
P3＝k3·(|pc-pg|+|φc-φg|+|θc-θg|+|ψc-ψg|)
wherein, PiIndicates the priority (i is 1,2,3) of the ith task, kjA priority coefficient (j is 2,3) representing the j-th task, h represents the current distance between the underwater robot and the water bottom, l represents the current minimum distance between equipment and an obstacle, theta represents the current joint angle of all joints of the two mechanical arms, D represents the deviation distance between the center of the target object and the center of the visual field range of the camera, and p represents the current distance between the center of the target object and the center of the visual field range of the cameracRepresenting the current coordinates, p, of the end effectorgRepresents the coordinates of the object, [ phi ]c，θc，ψc]Represents the current attitude of the end effector, [ phi ]g，θg，ψg]Representing an end effector target pose.
5. The task-based hierarchical optimization underwater unmanned vehicle-double mechanical arm control method according to claim 4, characterized in that: the Taskpriority value rule is as follows: get1，2Two sufficiently small positive numbers (1In order to meet the coefficients of safe sailing height and safe limit of the system,2to satisfy the factor of the robot arm end effector moving to the vicinity of the target point), if P) is present1＜1The Taskpriority value is 1; if P1＞1，P3＞2Then compare P2And P3If P is the size of2＞P3The Taskpriority value is 2, if P2＜P3The Taskpriority value is 3; if P1＞1，P3＜2And considering that the underwater unmanned ship-two mechanical arms complete the grabbing task.
6. The task-based hierarchical optimization control method for the underwater unmanned vehicle-double mechanical arms according to claim 1, is characterized in that: the specific optimization algorithm for tasks (9) - (10) is as follows:
velocity of end of manipulator
The following relations exist among linear velocity, angular velocity and angular velocity vector xi of manipulator joint of the submarine body
Inverting the above formula to obtain:
in the formula
Is JxThe pseudo-inverse of (a) is,
is the desired speed, ξ, of the end of the manipulatorrA theoretical velocity vector calculated for inverse kinematics;
this solution corresponds to the minimization of the velocity least squares function:
the weight W is introduced into the joint speeds of the boat body and the mechanical arm:
the weighted pseudo-inverse is therefore:
the general solution for the cost function is:
wherein N is 6+ N which is the sum of six degrees of freedom of the boat body and the degree of freedom of the manipulator; xiaThe speed vectors of the boat body and the manipulator are obtained; n is a radical ofaAs an operator, is a Jacobian matrix JaA joint velocity vector in null space of (a); xi in the formulaaDescribed are system secondary tasks, which are related to the gradient values of the objective function by selecting xiaTo achieve a local minimum as follows:
where H (q) is the objective function, kHFor the gain factor, the system speed is determined, and thus can be expressed as:
in the formula
The system primary task is represented for the pseudo-inverse solution term,
the null-space solution item represents a system secondary task, the primary task and the secondary task can be planned through specific operation conditions, the primary task is expected to be completed preferentially under general conditions, if the primary task and the secondary task conflict, the primary task is high in priority, the primary task is guaranteed to be completed, and the secondary task is completed as far as possible;
the method is generalized to a general form under a multitask mode, and the main task eta of the system is defined firstp∈RmThe corresponding Jacobian matrix is Jp(q), m is the number of dimensions required by the task, and can be obtained as follows:
similarly, a secondary task η can be definedi∈RtThe Jacobian matrix of which is Ji(q), namely:
where i ∈ N+Representing the number of secondary tasks; a priority planning algorithm under multiple tasks:
in the formula
A jacobian matrix corresponding to each secondary task.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010700570.7A CN112068576B (en) | 2020-07-21 | 2020-07-21 | Task-hierarchical timing optimization-based underwater unmanned ship-double mechanical arms cooperative control method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010700570.7A CN112068576B (en) | 2020-07-21 | 2020-07-21 | Task-hierarchical timing optimization-based underwater unmanned ship-double mechanical arms cooperative control method |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112068576A true CN112068576A (en) | 2020-12-11 |
| CN112068576B CN112068576B (en) | 2022-07-12 |
Family
ID=73657741
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202010700570.7A Active CN112068576B (en) | 2020-07-21 | 2020-07-21 | Task-hierarchical timing optimization-based underwater unmanned ship-double mechanical arms cooperative control method |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112068576B (en) |
Cited By (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112698574A (en) * | 2020-12-29 | 2021-04-23 | 南京理工大学 | Hybrid task priority based double-arm space robot coordination control method |
| CN113459089A (en) * | 2021-06-09 | 2021-10-01 | 华中科技大学 | Dynamics coupling effect evaluation method for underwater unmanned ship-double-mechanical-arm operation system |
| CN114083537A (en) * | 2021-11-30 | 2022-02-25 | 深圳市优必选科技股份有限公司 | Mechanical arm clamping control method and device, robot and readable storage medium |
| CN115407768A (en) * | 2022-08-02 | 2022-11-29 | 哈尔滨工程大学 | Underwater robot marine organism efficient fishing path planning method |
| CN115401697A (en) * | 2022-10-11 | 2022-11-29 | 深圳市智鼎自动化技术有限公司 | Task-graded double-mechanical-arm collaborative planning and control method and related device |
| CN115446851A (en) * | 2022-11-11 | 2022-12-09 | 北京炎凌嘉业机电设备有限公司 | Double-arm robot control system and double-arm robot for automatic spraying |
| CN116880197A (en) * | 2023-07-21 | 2023-10-13 | 哈尔滨工程大学 | Underwater robot operation track planning optimization method and optimization system based on multi-target multi-population backbone particle swarm optimization algorithm |
| CN117806162A (en) * | 2023-11-30 | 2024-04-02 | 同济大学 | Unmanned arm-carrying submarine-arm coupling coordination control method and system |
| CN118210318A (en) * | 2024-05-22 | 2024-06-18 | 陕西欧卡电子智能科技有限公司 | Unmanned ship planning method and device, computer equipment and unmanned ship |
Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107226186A (en) * | 2017-07-31 | 2017-10-03 | 武汉理工大学 | AUV multifunctional comprehensives service platform and formation progress control method |
| US20180071874A1 (en) * | 2016-09-12 | 2018-03-15 | Kindred Systems Inc. | Compound prismatic platforms for use in robotic systems |
| CN108563235A (en) * | 2018-05-24 | 2018-09-21 | 南方科技大学 | Multi-rotor unmanned aerial vehicle, method, device and equipment for grabbing target object |
| CN109032145A (en) * | 2018-08-29 | 2018-12-18 | 广州市君望机器人自动化有限公司 | To the dispatching method and device in multirobot path |
| CN109822554A (en) * | 2019-03-20 | 2019-05-31 | 华中科技大学 | Towards underwater both arms collaboration crawl, embraces and take and collision prevention integral method and system |
| CN110231821A (en) * | 2019-06-03 | 2019-09-13 | 哈尔滨工程大学 | The adaptive kernel action amalgamation method of the improvement of multi-robot formation |
| US20200026285A1 (en) * | 2006-02-27 | 2020-01-23 | Perrone Robotics, Inc. | General purpose robotics operating system with unmanned and autonomous vehicle extensions |
| CN110815235A (en) * | 2019-09-23 | 2020-02-21 | 苏州商信宝信息科技有限公司 | Intelligent shopping service method and system based on data matching |
| CN111399509A (en) * | 2020-03-24 | 2020-07-10 | 华中科技大学 | Multi-mobile-robot cooperative transfer control method and system |
-
2020
- 2020-07-21 CN CN202010700570.7A patent/CN112068576B/en active Active
Patent Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20200026285A1 (en) * | 2006-02-27 | 2020-01-23 | Perrone Robotics, Inc. | General purpose robotics operating system with unmanned and autonomous vehicle extensions |
| US20180071874A1 (en) * | 2016-09-12 | 2018-03-15 | Kindred Systems Inc. | Compound prismatic platforms for use in robotic systems |
| CN107226186A (en) * | 2017-07-31 | 2017-10-03 | 武汉理工大学 | AUV multifunctional comprehensives service platform and formation progress control method |
| CN108563235A (en) * | 2018-05-24 | 2018-09-21 | 南方科技大学 | Multi-rotor unmanned aerial vehicle, method, device and equipment for grabbing target object |
| CN109032145A (en) * | 2018-08-29 | 2018-12-18 | 广州市君望机器人自动化有限公司 | To the dispatching method and device in multirobot path |
| CN109822554A (en) * | 2019-03-20 | 2019-05-31 | 华中科技大学 | Towards underwater both arms collaboration crawl, embraces and take and collision prevention integral method and system |
| CN110231821A (en) * | 2019-06-03 | 2019-09-13 | 哈尔滨工程大学 | The adaptive kernel action amalgamation method of the improvement of multi-robot formation |
| CN110815235A (en) * | 2019-09-23 | 2020-02-21 | 苏州商信宝信息科技有限公司 | Intelligent shopping service method and system based on data matching |
| CN111399509A (en) * | 2020-03-24 | 2020-07-10 | 华中科技大学 | Multi-mobile-robot cooperative transfer control method and system |
Non-Patent Citations (5)
| Title |
|---|
| 吴仁杰等: "《机械臂多任务协同建模与分配方法》", 《机械科学与技术》 * |
| 李宁: "《煤矸分拣机器人控制系统研究》", 《万方学位论文》 * |
| 杜晔: "《双机械臂协同运动规划方法研究》", 《万方学位论文》 * |
| 陈恩怜等: "《基于ROS平台下的多机械臂协同抓取研究》", 《电气应用》 * |
| 陈文皞等: "《基于模型预测控制的协作焊接双机械臂轨迹跟踪算法》", 《轻工机械》 * |
Cited By (13)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112698574A (en) * | 2020-12-29 | 2021-04-23 | 南京理工大学 | Hybrid task priority based double-arm space robot coordination control method |
| CN112698574B (en) * | 2020-12-29 | 2022-05-13 | 南京理工大学 | Hybrid task priority based double-arm space robot coordination control method |
| CN113459089A (en) * | 2021-06-09 | 2021-10-01 | 华中科技大学 | Dynamics coupling effect evaluation method for underwater unmanned ship-double-mechanical-arm operation system |
| CN113459089B (en) * | 2021-06-09 | 2022-04-29 | 华中科技大学 | Dynamics coupling effect evaluation method for underwater unmanned ship-double-mechanical-arm operation system |
| CN114083537A (en) * | 2021-11-30 | 2022-02-25 | 深圳市优必选科技股份有限公司 | Mechanical arm clamping control method and device, robot and readable storage medium |
| CN115407768B (en) * | 2022-08-02 | 2023-12-12 | 哈尔滨工程大学 | Underwater robot marine organism efficient catching path planning method |
| CN115407768A (en) * | 2022-08-02 | 2022-11-29 | 哈尔滨工程大学 | Underwater robot marine organism efficient fishing path planning method |
| CN115401697A (en) * | 2022-10-11 | 2022-11-29 | 深圳市智鼎自动化技术有限公司 | Task-graded double-mechanical-arm collaborative planning and control method and related device |
| CN115446851A (en) * | 2022-11-11 | 2022-12-09 | 北京炎凌嘉业机电设备有限公司 | Double-arm robot control system and double-arm robot for automatic spraying |
| CN116880197A (en) * | 2023-07-21 | 2023-10-13 | 哈尔滨工程大学 | Underwater robot operation track planning optimization method and optimization system based on multi-target multi-population backbone particle swarm optimization algorithm |
| CN116880197B (en) * | 2023-07-21 | 2024-07-16 | 哈尔滨工程大学 | Underwater robot operation track planning optimization method and optimization system based on multi-target multi-population backbone particle swarm optimization algorithm |
| CN117806162A (en) * | 2023-11-30 | 2024-04-02 | 同济大学 | Unmanned arm-carrying submarine-arm coupling coordination control method and system |
| CN118210318A (en) * | 2024-05-22 | 2024-06-18 | 陕西欧卡电子智能科技有限公司 | Unmanned ship planning method and device, computer equipment and unmanned ship |
Also Published As
| Publication number | Publication date |
|---|---|
| CN112068576B (en) | 2022-07-12 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112068576B (en) | Task-hierarchical timing optimization-based underwater unmanned ship-double mechanical arms cooperative control method | |
| Fernández et al. | Grasping for the seabed: Developing a new underwater robot arm for shallow-water intervention | |
| Lippiello et al. | Exploiting redundancy in Cartesian impedance control of UAVs equipped with a robotic arm | |
| CN106708069B (en) | Coordinated planning and control method of underwater movement robot | |
| Paul et al. | A multirotor platform employing a three-axis vertical articulated robotic arm for aerial manipulation tasks | |
| CN108860527B (en) | Underwater robot-underwater mechanical arm system | |
| Sverdrup-Thygeson et al. | The underwater swimming manipulator-a bio-inspired AUV | |
| Anzai et al. | Aerial grasping based on shape adaptive transformation by halo: Horizontal plane transformable aerial robot with closed-loop multilinks structure | |
| CN111923049B (en) | Visual servo and multitasking control method for flying mechanical arm based on spherical model | |
| JP2006334774A (en) | Method for controlling track of effector | |
| Sverdrup-Thygeson et al. | A control framework for biologically inspired underwater swimming manipulators equipped with thrusters | |
| Cataldi et al. | Basic interaction operations for an underwater vehicle-manipulator system | |
| Prats et al. | Advances in the specification and execution of underwater autonomous manipulation tasks | |
| Giglio et al. | Selective compliance control for an unmanned aerial vehicle with a robotic arm | |
| Ouyang et al. | Control of an aerial manipulator using a quadrotor with a replaceable robotic arm | |
| CN118034060A (en) | Method for tracking and controlling optimal track of under-actuated autonomous underwater vehicle | |
| Ivanovic et al. | Exploiting null space in aerial manipulation through model-in-the-loop motion planning | |
| Lai et al. | An onboard-eye-to-hand visual servo and task coordination control for aerial manipulator based on a spherical model | |
| Ishizu et al. | Preliminary experiments of a human-portable underwater gripper robot for dexterous tasks | |
| Mišković et al. | Unmanned marsupial sea-air system for object recovery | |
| Juan et al. | Manipulation in the seabed: A new underwater manipulation system for shallow water intervention | |
| CN212635747U (en) | Visual servo structure of flying mechanical arm based on spherical model | |
| Sotiropoulos et al. | Optimum docking of an unmanned underwater vehicle for high dexterity manipulation | |
| Liang et al. | Dynamics modeling and analysis for unmanned quadrotor transportation systems with double-pendulum swing effects | |
| Liu et al. | Lyapunov-based Model Predictive Visual Servo Control of an Underwater Vehicle-Manipulator System |
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 |