CN112068576B - 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 PDF

Info

Publication number
CN112068576B
CN112068576B CN202010700570.7A CN202010700570A CN112068576B CN 112068576 B CN112068576 B CN 112068576B CN 202010700570 A CN202010700570 A CN 202010700570A CN 112068576 B CN112068576 B CN 112068576B
Authority
CN
China
Prior art keywords
task
underwater
arm
mechanical arms
tasks
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
Application number
CN202010700570.7A
Other languages
Chinese (zh)
Other versions
CN112068576A (en
Inventor
向先波
熊昕飏
张琴
杨少龙
董东磊
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Huazhong University of Science and Technology
Original Assignee
Huazhong University of Science and Technology
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Huazhong University of Science and Technology filed Critical Huazhong University of Science and Technology
Priority to CN202010700570.7A priority Critical patent/CN112068576B/en
Publication of CN112068576A publication Critical patent/CN112068576A/en
Application granted granted Critical
Publication of CN112068576B publication Critical patent/CN112068576B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • 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

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

Task-hierarchical timing optimization-based underwater unmanned ship-double mechanical arms cooperative control method
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 underwater robot has the advantages that 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 underwater robot with the two arms, 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-double mechanical arm cooperative work system is increased, the purpose of reducing dynamic disturbance of mechanical arm motion on an underwater robot mother ship is achieved, and a prerequisite condition is created for accurate operation of an underwater unmanned ship-double 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 two mechanical arms;
(9) minimizing underwater unmanned boat motion;
(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: taking epsilon1,ε2Is two sufficiently small positive numbers (epsilon)1To satisfy the coefficient of safe navigation height and safe limit of the system, epsilon2To 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: controlling the mechanical arm with the grabbing task to move, and simultaneously controlling the x of the other mechanical arm relative to the unmanned boat0oz0The plane and the task mechanical arm symmetrically move to reduce the transverse tilting moment generated by the movement of the mechanical arm on the unmanned boat 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 is also to reduce the machineThe arm movement generates a heeling moment on the unmanned boat 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
Figure BDA0002592891030000051
The following relations exist with linear velocity and angular velocity of the hull and angular velocity vector xi of the manipulator joint
Figure BDA0002592891030000052
Inverting the above formula to obtain:
Figure BDA0002592891030000053
in the formula
Figure BDA0002592891030000054
Is JxThe pseudo-inverse of (a) is,
Figure BDA0002592891030000055
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:
Figure BDA0002592891030000056
the weight W is introduced into the joint speeds of the boat body and the mechanical arm:
Figure BDA0002592891030000057
the weighted pseudo-inverse is therefore:
Figure BDA0002592891030000058
the general solution for the cost function is:
Figure BDA0002592891030000059
in the formula, 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 as a system secondary task, related to the gradient value of the objective function, can be determined by selecting ξaTo achieve a local minimum as follows:
Figure BDA00025928910300000510
where H (q) is the objective function, kHFor the gain factor, the system speed is determined, and thus can be expressed as:
Figure BDA0002592891030000061
in the formula
Figure BDA0002592891030000062
The system primary task is represented for the pseudo-inverse solution term,
Figure BDA0002592891030000063
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∈RmTo, forThe Jacobian matrix is Jp(q), m is the number of dimensions required by the task, and can be obtained as follows:
Figure BDA0002592891030000064
similarly, a secondary task η can be definedi∈RtThe Jacobian matrix of which is Ji(q), namely:
Figure BDA0002592891030000065
where i ∈ N+Representing the number of secondary tasks; a priority planning algorithm under multiple tasks:
Figure BDA0002592891030000066
in the formula
Figure BDA0002592891030000067
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 pictorial view of an underwater dual robot arm in an embodiment;
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)
Figure BDA0002592891030000071
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 base plate (147) is connected with an underwater unmanned underwater vehicle (carried object), the other end of the base 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), and 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 larger than the minimum height h0；
(2) Obstacle avoidance of unmanned ship and mechanical arm, and keeping distance between equipment and obstacle larger than safety 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 mechanical arm with the grabbing task to move, and simultaneously controlling the other mechanical arm to move relative 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 respectively control the 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 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 is also used for reducing the transverse tilting moment generated by the movement of the mechanical arm on the unmanned boat 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
Figure BDA0002592891030000101
Wherein
Figure BDA0002592891030000102
Figure BDA0002592891030000111
Representing the position coordinates of the unmanned boat in an absolute coordinate system,
Figure BDA0002592891030000112
representing the euler angular coordinates of the unmanned boat in an absolute coordinate system. Defining the velocity vector of the unmanned boat as
Figure BDA0002592891030000113
Wherein
Figure BDA0002592891030000114
Representing the linear velocity of the unmanned ship under an absolute coordinate system,
Figure BDA0002592891030000115
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 moving, when the single arm carries out 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 variation 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 mode, the unmanned boat hardly generates rolling and yawing, and the stability of the unmanned boat is greatly improved, which means that when the two mechanical arms move in the symmetrical mode, if the attitude of the unmanned boat is expected to be relatively stable, corresponding power compensation is only needed to be performed on the pitching direction of the unmanned boat, so that 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 (5)

1. An underwater unmanned ship-double mechanical arms cooperative control method based on task hierarchical time sequence optimization is characterized by comprising the following steps: 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 method comprises the following steps of subdividing the grabbing action of the underwater double-mechanical-arm robot, and performing optimized division on the execution sequence of the grabbing action, wherein 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 mechanical arm and the obstacle to be greater than the safety distance l0；
(3) The joint angle of the mechanical arm is limited, and the change theta of the joint angle in an angle limiting range is keptmin<θ<θ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) minimizing the motion of the double mechanical arms;
the specific optimization algorithm for tasks (9) - (10) is as follows:
velocity of end of manipulator
Figure FDA0003486584140000011
The following relations exist among linear velocity, angular velocity and angular velocity vector xi of manipulator joint of the submarine body
Figure FDA0003486584140000021
Inverting the above formula to obtain:
Figure FDA0003486584140000022
in the formula
Figure FDA0003486584140000023
Is JxThe pseudo-inverse of (a) is,
Figure FDA0003486584140000024
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:
Figure FDA0003486584140000025
the weight W is introduced into the speeds of each joint of the boat body and the mechanical arm:
Figure FDA0003486584140000026
the weighted pseudo-inverse is therefore:
Figure FDA0003486584140000027
the general solution to the cost function is:
Figure FDA0003486584140000028
in the formula, 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 of hydrogenaAs 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:
Figure FDA0003486584140000029
where H (q) is the objective function, kHFor the gain factor, the system speed is determined, and thus can be expressed as:
Figure FDA00034865841400000210
in the formula
Figure FDA00034865841400000211
The system primary task is represented for the pseudo-inverse solution term,
Figure FDA00034865841400000212
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:
Figure FDA0003486584140000031
similarly, a secondary task η can be definedi∈RtThe Jacobian matrix of which is Ji(q), namely:
Figure FDA0003486584140000032
wherein i ∈ N+Representing the number of secondary tasks; a priority planning algorithm under multiple tasks:
Figure FDA0003486584140000033
in the formula
Figure FDA0003486584140000034
A jacobian matrix corresponding to each secondary task.
2. The task-based hierarchical timing optimization-based cooperative control method for the unmanned underwater 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) to (7) correspond to core capture tasks, category 4: tasks (8) - (10) correspond to control optimization objectives.
3. The task-based hierarchical timing optimization-based cooperative control method for the unmanned underwater vehicle-double mechanical arms as claimed in claim 2, wherein: setting a task hierarchical time sequence optimization function Taskpriority for the motion planning system, carrying out priority planning on tasks of the category 1 to the category 3, wherein the value range of the Taskpriority is {1,2,3}, and when the value of the Taskpriority is i, executing the ith task.
4. The task-based hierarchical timing optimization-based cooperative control method for the unmanned underwater vehicle-double mechanical arms according to claim 3, is characterized in that: calculating the priorities of the tasks in the categories 1 to 3 respectively:
P1＝min{h-h0,l-l0,θ-θmin,θmax-θ}
P2＝k2·D
P3＝k3·(|pc-pg|+|φc-φg|+|θc-θg|+|ψc-ψg|)
wherein, PiIndicating that the priority i of the i-th task is 1,2,3, kjThe priority coefficient j representing the j-th task is 2,3, 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 double 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 of the end effector, pgRepresents 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 timing optimization-based cooperative control method for the unmanned underwater vehicle-double mechanical arms as claimed in claim 4, wherein: the Taskpriority value rule is as follows: taking epsilon1,ε2Two sufficiently small positive numbers, epsilon1To satisfy the coefficient of safe navigation height and safe limit of the system, epsilon2If P is the coefficient for the end effector of the robot arm to move to the vicinity of the target point1<ε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.
CN202010700570.7A 2020-07-21 2020-07-21 Task-hierarchical timing optimization-based underwater unmanned ship-double mechanical arms cooperative control method Active CN112068576B (en)

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 CN112068576A (en) 2020-12-11
CN112068576B true 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)

Families Citing this family (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112698574B (en) * 2020-12-29 2022-05-13 南京理工大学 Hybrid task priority based double-arm space robot coordination control method
CN113459089B (en) * 2021-06-09 2022-04-29 华中科技大学 Dynamics coupling effect evaluation method for underwater unmanned ship-double-mechanical-arm operation system
CN114083537B (en) * 2021-11-30 2023-07-07 深圳市优必选科技股份有限公司 Mechanical arm clamping control method, device, robot and readable storage medium
CN115407768B (en) * 2022-08-02 2023-12-12 哈尔滨工程大学 Underwater robot marine organism efficient catching path planning method
CN115446851B (en) * 2022-11-11 2023-04-07 北京炎凌嘉业机电设备有限公司 Double-arm robot control system and double-arm robot for automatic spraying

Family Cites Families (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US10331136B2 (en) * 2006-02-27 2019-06-25 Perrone Robotics, Inc. General purpose robotics operating system with unmanned and autonomous vehicle extensions
US10518372B2 (en) * 2016-09-12 2019-12-31 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
CN108563235B (en) * 2018-05-24 2022-02-08 南方科技大学 Multi-rotor unmanned aerial vehicle, method, device and equipment for grabbing target object
CN109032145B (en) * 2018-08-29 2021-09-10 广州安商智能科技有限公司 Method and device for scheduling paths of multiple robots
CN109822554B (en) * 2019-03-20 2020-09-18 华中科技大学 Underwater dual-arm cooperative grabbing, holding and collision avoidance integrated method and system
CN110231821B (en) * 2019-06-03 2022-05-31 哈尔滨工程大学 Improved self-adaptive zero-space behavior fusion method for multi-robot formation
CN110815235B (en) * 2019-09-23 2021-05-18 深圳市联众互动科技有限公司 Intelligent shopping service method and system based on data matching
CN111399509B (en) * 2020-03-24 2021-04-20 华中科技大学 Multi-mobile-robot cooperative transfer control method and system

Also Published As

Publication number Publication date
CN112068576A (en) 2020-12-11

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
Sverdrup-Thygeson et al. The underwater swimming manipulator—A bioinspired solution for subsea operations
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
CN108860527B (en) Underwater robot-underwater mechanical arm system
Paul et al. A multirotor platform employing a three-axis vertical articulated robotic arm for aerial manipulation tasks
CN111923049B (en) Visual servo and multitasking control method for flying mechanical arm based on spherical model
Santhakumar Investigation into the dynamics and control of an underwater vehicle-manipulator system
Sverdrup-Thygeson et al. The underwater swimming manipulator-a bio-inspired AUV
Sverdrup-Thygeson et al. A control framework for biologically inspired underwater swimming manipulators equipped with thrusters
Prats et al. Advances in the specification and execution of underwater autonomous manipulation tasks
Cataldi et al. Basic interaction operations for an underwater vehicle-manipulator system
Lv et al. Sliding mode control of cable-driven redundancy parallel robot with 6 DOF based on cable-length sensor feedback
Christensen et al. The hydrobatic dual-arm intervention AUV cuttlefish
Lai et al. An onboard-eye-to-hand visual servo and task coordination control for aerial manipulator based on a spherical model
Ivanovic et al. Exploiting null space in aerial manipulation through model-in-the-loop motion planning
Ishizu et al. Preliminary experiments of a human-portable underwater gripper robot for dexterous tasks
Abdulov et al. Extra steering for ROV control system by tracking the gamepad orientation
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
Sotiropoulos et al. Optimum docking of an unmanned underwater vehicle for high dexterity manipulation
CN212635747U (en) Visual servo structure of flying mechanical arm based on spherical model
Liang et al. Dynamics modeling and analysis for unmanned quadrotor transportation systems with double-pendulum swing effects
Ambar et al. Coordinated motion control of a 3-Link dual-arm underwater vehicle-manipulator system using resolved acceleration control 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