CN117687430B - Motion planning method for multi-machine collaborative handling of unmanned arm-carrying submarine - Google Patents
Motion planning method for multi-machine collaborative handling of unmanned arm-carrying submarine Download PDFInfo
- Publication number
- CN117687430B CN117687430B CN202311566702.1A CN202311566702A CN117687430B CN 117687430 B CN117687430 B CN 117687430B CN 202311566702 A CN202311566702 A CN 202311566702A CN 117687430 B CN117687430 B CN 117687430B
- Authority
- CN
- China
- Prior art keywords
- unmanned
- arm
- planning
- submarine
- carrying
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 73
- 230000008569 process Effects 0.000 claims abstract description 26
- 238000005457 optimization Methods 0.000 claims abstract description 22
- 238000004590 computer program Methods 0.000 claims description 14
- 230000001133 acceleration Effects 0.000 claims description 13
- 230000009471 action Effects 0.000 claims description 12
- 238000004891 communication Methods 0.000 claims description 12
- 239000012636 effector Substances 0.000 claims description 9
- 239000011159 matrix material Substances 0.000 claims description 9
- 238000005265 energy consumption Methods 0.000 claims description 6
- 208000012661 Dyskinesia Diseases 0.000 claims description 3
- 238000011160 research Methods 0.000 description 5
- 230000008859 change Effects 0.000 description 4
- 238000012545 processing Methods 0.000 description 4
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 4
- 238000010586 diagram Methods 0.000 description 3
- 230000006870 function Effects 0.000 description 3
- 238000013461 design Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 230000002411 adverse Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000015572 biosynthetic process Effects 0.000 description 1
- 239000012141 concentrate Substances 0.000 description 1
- 230000008878 coupling Effects 0.000 description 1
- 238000010168 coupling process Methods 0.000 description 1
- 238000005859 coupling reaction Methods 0.000 description 1
- 230000003247 decreasing effect Effects 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 230000006855 networking Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000001360 synchronised effect Effects 0.000 description 1
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention belongs to the technical field of unmanned underwater vehicles, and discloses a motion planning method for multi-machine collaborative transport of a carrier-arm unmanned underwater vehicle. The method comprises the steps of establishing a closed chain kinematic model of a plurality of heterogeneous unmanned aerial vehicles with arms in the carrying process based on pose constraint and speed constraint, a track planner of the unmanned aerial vehicles with arms based on high-order polynomial interpolation and an optimal motion planning algorithm based on collaborative carrying of an improved non-dominant sequential evolution algorithm. The method comprises the following steps: establishing a closed-chain kinematic model of the multi-arm unmanned submarine based on motion constraint analysis; completing track planning of collaborative handling of the multi-carrier unmanned underwater vehicle based on high-order polynomial interpolation; the motion trail of the multi-arm unmanned underwater vehicle is optimized in a multi-objective mode based on the improved non-dominant order evolution algorithm, coordination consistency among the unmanned underwater vehicles in the carrying process is achieved, and time optimization, energy optimization, contact force optimization and stability optimization of the unmanned underwater vehicle in the carrying process are achieved.
Description
Technical Field
The invention belongs to the technical field of unmanned underwater vehicles, and particularly relates to a motion planning method for multi-machine collaborative transport of a carrier-arm unmanned underwater vehicle.
Background
The unmanned submersible vehicle with the carrying arm is a multipurpose special submersible vehicle formed by carrying a plurality of isomorphic or heterogeneous multi-degree-of-freedom underwater mechanical arms on the conventional unmanned submersible vehicle, the high-precision execution control of the hull and the mechanical arms is realized by means of multi-sensor information fusion such as sonar, vision and the like, the diversified tasks under the complex underwater environment can be independently completed, the capability of cruising, reconnaissance, detection and the like of the conventional unmanned submersible vehicle is provided, and the underwater operation tasks such as carrying, laying, assembly, breaking and disassembly and the like which cannot be completed by the conventional submersible vehicle can be completed. The unmanned submersible vehicles with the multiple carrying arms form a cluster to cooperatively operate, so that the limitation of operation of the unmanned submersible vehicle with the single carrying arm in a complex environment can be effectively solved.
However, at present, the research on the collaborative handling method of the multi-arm unmanned submersible vehicle in China is just started, and most of the research on the domestic well-known universities and scientific research institutions concentrate on the grabbing and handling of the single unmanned submersible vehicle, and the research on the formation navigation control, the underwater acoustic communication networking protocol design and the collaborative navigation method of the multi-arm unmanned submersible vehicle, and the research on the execution of the collaborative handling task of the multi-arm unmanned submersible vehicle is insufficient. In addition, many problems of the existing multi-arm unmanned submarine cooperative transportation technology continue to be studied and solved. Firstly, environmental perception sensors in an underwater environment are limited in variety, the underwater acoustic communication delay is large, and high requirements are provided for the control instantaneity of the unmanned submersible vehicle with the carrying arm; second, in complex water flow environments. The unmanned submersible vehicle with the carrying arm is different from a land mobile robot, and particularly is influenced by adverse factors such as water flow, water viscous resistance, buoyancy and the like in dynamics; finally, when the multi-arm unmanned submarine is used for carrying the same object, a tight cooperative system is formed, and the movement process has larger coupling problems, disturbance problems caused by severe environments, uncertain load problems and the like. These all increase the difficulty of control.
Disclosure of Invention
The invention provides a motion planning method for multi-machine collaborative handling of a plurality of unmanned boom submarines, which optimizes collaborative handling of a plurality of different unmanned boom submarines so as to solve the technical problem of synchronous coordination and handling of the actual intelligent path planning of the unmanned boom submarines and the unmanned boom submarines in an actual water area environment.
The invention is realized by the following technical scheme:
the motion planning method for the multi-machine collaborative handling of the unmanned submersible vehicle with the carrying arm comprises the following steps:
step 1: a closed chain kinematic model of the unmanned carrier arm submarines of the heterogeneous carrier arm submarines in the carrying process is established jointly through single carrier arm unmanned submarines kinematics and pose constraint and speed constraint;
step 2: based on the closed-chain kinematic model in the step 1, a high-order polynomial interpolation mode is adopted to realize the track planning of the multi-arm unmanned underwater vehicle;
Step 3: and (3) based on the track planning of the multi-arm unmanned submarine, the improved non-dominant sequential evolution algorithm is used for realizing the optimal motion planning of the cooperative transportation.
Further, the pose constraint in the step 1 is specifically that,
Wherein q l is the boat pose and the manipulator joint variable of the main arm unmanned submersible vehicle, and q f is the boat pose and the manipulator joint variable of the auxiliary arm unmanned submersible vehicle; The method is a position vector of a centroid of a carrying object in a main manipulator base coordinate system; /(I) The position vector of the end effector of the main carrier arm unmanned underwater vehicle in the base coordinate system of the main carrier arm unmanned underwater vehicle; /(I)Cosine of the rigid body centroid to a direction in the main manipulator end effector coordinate system; /(I)Is the cosine of the direction of the main manipulator end effector coordinate system relative to the base coordinate system.
Further, the speed constraint of the step 1 is specifically that,
In the method, in the process of the invention,An angular velocity vector that is the centroid of the object; /(I)Is the velocity vector of the unmanned submarine body of the main carrying arm and the joint of the manipulator; j ll(ql)∈R3×n is a pose Jacobian matrix of the main submarine; j la(ql)∈R3×n is the attitude jacobian matrix of the main submersible; j +(qf) is the pseudo-inverse of the jacobian matrix from the submarine.
Further, the track planning result based on the polynomial curve interpolation in the step 2 is shown in fig. 3, and the track can satisfy the following three constraint conditions:
(1) Third order derivative of position-time curve
(2) Second order derivative of speed-time curve
(3) First order conductance of acceleration-time curve
When interpolation is carried out when n points exist, interpolation points need to be carried out between every two interpolation points, n-1 sections of quintic curves can appear, 6n-6 parameters are undetermined, and corresponding constraints are as follows: the interpolation equation can be solved because of 6n-6 constraints when five curve fits are performed on adjacent points in the n points.
Further, the step 2 higher order polynomial interpolation is specifically,
q(t)＝b+a1(t-t0)+a2(t-t0)2+a3(t-t0)3+a4(t-t0)4+a5(t-t0)5,
B, a 1,a2,a3,a4,a5 is undetermined parameters of different order items of the planned track, and the complete planned track can be obtained by resolving the 6 undetermined parameters; t is the current running time, and t 0 is the planning starting time;
in the case of two points, the position of the start point, the position of the end point, the speed and the acceleration are known, i.e
q(t0)＝q0,q(t1)＝q1，
Wherein q (t 0) is an initial state,For initial speed,/>For initial acceleration, q (t 1) is the end point state,/>To reach the end point speed,/>To reach the acceleration at the end point, q 0 is the initial state, q 1 is the end point state, v 0 is the initial speed, and v 1 is the speed at the end point.
Further, let t=t 1-t0,h＝q1-q0, then T represents the time step and h represents the amount of change in the state of the submarine;
Then it is possible to obtain:
Furthermore, the improved non-dominant rank evolution algorithm in the step 3 is specifically that a cooperative algorithm and an NSGA-II algorithm are combined, namely, the track planning of each unmanned arm-carrying underwater vehicle is regarded as a plurality of sub-populations, information communication is carried out among the sub-populations by optimal individuals, and iterative optimization is independently carried out in the sub-populations by using the NSGA-II algorithm.
Further, the optimal motion planning for implementing cooperative transportation in the step 3 is specifically that the adopted multi-optimization targets are as follows
Wherein n is the number of joints; m is the number of the length of the rear arm of the ith joint; t 0 is the planned start time; t f is the planned termination time; g 1 is an action amplitude index in the conveying process, alpha i is an action amplitude coefficient of each joint, and the action amplitude index is the sum of the lengths d of all rods behind the ith joint, and the weight corresponding to the boat body is larger when the action amplitude index is closer to the boat body; g 2 is an energy consumption index, the larger the index is, the larger the energy consumption is; g 3 is a joint impact index, which relates to the jerkiness of the joint angle, and the smaller the index is, the smaller the joint impact is, and the more stable the submarine operation is; delta is the Euclidean distance from the solving point p to the target point q by an algorithm, and delta is an index of the positioning precision of the mechanical arm; θ i is the position,Is angular velocity,/>Is angular acceleration.
An electronic device comprises a processor, a communication interface, a memory and a communication bus, wherein the processor, the communication interface and the memory are communicated with each other through the communication bus;
A memory for storing a computer program;
and the processor is used for realizing the method when executing the program stored in the memory.
A computer readable storage medium having stored therein a computer program which when executed by a processor implements the method described above.
The beneficial effects of the invention are as follows:
The invention realizes the coordination consistency among the unmanned submersible vehicles with the heterogeneous carrier arms in the carrying process, and realizes the optimal time, optimal energy, optimal contact force and optimal stability of the unmanned submersible vehicles with the carrier arms in the carrying process.
According to the invention, coordinated motion planning is carried out on the multi-arm unmanned underwater vehicle, so that the accuracy and stability of the multi-arm unmanned underwater vehicle in the carrying process are realized, and the robustness of the system to external interference is greatly improved.
Drawings
Fig. 1 is a schematic structural view of the present invention.
FIG. 2 is a diagram of a closed-chain motion analysis of the multi-arm unmanned submersible vehicle of the present invention.
FIG. 3 is a schematic diagram of the interpolation result of the quintic curve track of the multi-arm unmanned submarine cooperative transportation process by applying the invention.
FIG. 4 is a block diagram of the multi-objective trajectory optimization flow based on the modified NSGA-II of the present invention.
Detailed Description
In the following description, for purposes of explanation and not limitation, specific details are set forth such as the particular system architecture, techniques, etc., in order to provide a thorough understanding of the embodiments of the present application. It will be apparent, however, to one skilled in the art that the present application may be practiced in other embodiments that depart from these specific details. In other instances, detailed descriptions of well-known systems, devices, circuits, and methods are omitted so as not to obscure the description of the present application with unnecessary detail.
It should be understood that the terms "comprises" and/or "comprising," when used in this specification and the appended claims, specify the presence of stated features, integers, steps, operations, elements, and/or components, but do not preclude the presence or addition of one or more other features, integers, steps, operations, elements, components, and/or groups thereof.
It is also to be understood that the terminology used in the description of the application is for the purpose of describing particular embodiments only and is not intended to be limiting of the application. As used in this specification and the appended claims, the singular forms "a," "an," and "the" are intended to include the plural forms as well, unless the context clearly indicates otherwise.
The following description of the embodiments of the present application will be made more fully with reference to the accompanying drawings, in which 1-4 are shown, it being apparent that the embodiments described are only some, but not all embodiments of the application. All other embodiments, which can be made by those skilled in the art based on the embodiments of the application without making any inventive effort, are intended to be within the scope of the application.
In the following description, numerous specific details are set forth in order to provide a thorough understanding of the present application, but the present application may be practiced in other ways other than those described herein, and persons skilled in the art will readily appreciate that the present application is not limited to the specific embodiments disclosed below.
Example 1
The motion planning method for the multi-machine collaborative handling of the unmanned submersible vehicle with the carrying arm comprises the following steps:
step 1: a closed chain kinematic model of the unmanned carrier arm submarines of the heterogeneous carrier arm submarines in the carrying process is established jointly through single carrier arm unmanned submarines kinematics and pose constraint and speed constraint;
the method comprises the steps of establishing a closed-chain kinematic model of the unmanned carrier arm underwater vehicle of a plurality of heterogeneous unmanned carrier arm underwater vehicle systems in the carrying process based on pose constraint and speed constraint obtained by analysis and the unmanned carrier arm underwater vehicle kinematics;
step 2: based on the closed-chain kinematic model in the step 1, a high-order polynomial interpolation mode is adopted to realize the track planning of the multi-arm unmanned underwater vehicle;
Step 3: and (3) based on the track planning of the multi-arm unmanned submarine, the improved non-dominant sequential evolution algorithm is used for realizing the optimal motion planning of the cooperative transportation.
A motion planning method for multi-machine collaborative handling of a manned unmanned submersible vehicle with a carrying arm, wherein the pose constraint of the step 1 is specifically that,
Wherein q l is the boat pose and the manipulator joint variable of the main arm unmanned submersible vehicle, and q f is the boat pose and the manipulator joint variable of the auxiliary arm unmanned submersible vehicle; The method is a position vector of a centroid of a carrying object in a main manipulator base coordinate system; /(I) The position vector of the end effector of the main carrier arm unmanned underwater vehicle in the base coordinate system of the main carrier arm unmanned underwater vehicle; /(I)Cosine of the rigid body centroid to a direction in the main manipulator end effector coordinate system; /(I)Is the cosine of the direction of the main manipulator end effector coordinate system relative to the base coordinate system.
A motion planning method for multi-machine collaborative handling of an unmanned submersible vehicle with a carrier arm, wherein the speed constraint of the step 1 is specifically that,
In the method, in the process of the invention,An angular velocity vector that is the centroid of the object; /(I)Is the velocity vector of the unmanned submarine body of the main carrying arm and the joint of the manipulator; j ll(ql)∈R3×n is a pose Jacobian matrix of the main submarine; j la(ql)∈R3×n is the attitude jacobian matrix of the main submersible; j +(qf) is the pseudo-inverse of the jacobian matrix from the submarine.
A motion planning method for multi-machine collaborative handling of an unmanned submersible vehicle with a carrying arm comprises the following steps that a track planning result based on five-time polynomial curve interpolation in step 2 is shown in a figure 3, and the track can meet the following three constraint conditions:
(4) Third order derivative of position-time curve
(5) Second order derivative of speed-time curve
(6) First order conductance of acceleration-time curve
When interpolation is carried out when n points exist, interpolation points need to be carried out between every two interpolation points, n-1 sections of quintic curves can appear, 6n-6 parameters are undetermined, and corresponding constraints are as follows: the interpolation equation can be solved because of 6n-6 constraints when five curve fits are performed on adjacent points in the n points.
The motion planning method for multi-machine collaborative handling of the unmanned carrier arm unmanned submersible vehicle is characterized in that the designed high-order polynomial interpolation method for track planning of the multi-carrier arm unmanned submersible vehicle can meet the continuous aim of the motion position, speed and acceleration (output force/moment) of the unmanned carrier arm submersible vehicle in the collaborative handling process, so that smooth control can be better realized, the step 2 high-order polynomial interpolation is specifically that,
q(t)＝b+a1(t-t0)+a2(t-t0)2+a3(t-t0)3+a4(t-t0)4+a5(t-t0)5,
B, a 1,a2,a3,a4,a5 is undetermined parameters of different order items of the planned track, and the complete planned track can be obtained by resolving the 6 undetermined parameters; t is the current running time, and t 0 is the planning starting time;
in the case of two points, the position of the start point, the position of the end point, the speed and the acceleration are known, i.e
q(t0)＝q0,q(t1)＝q1，
Wherein q (t 0) is an initial state,For initial speed,/>For initial acceleration, q (t 1) is the end point state,/>To reach the end point speed,/>To reach the acceleration at the end point, q 0 is the initial state, q 1 is the end point state, v 0 is the initial speed, and v 1 is the speed at the end point.
Further, let t=t 1-t0,h＝q1-q0, then T represents the time step and h represents the amount of change in the state of the submarine;
Then it is possible to obtain:
a motion planning method for multi-machine collaborative handling of unmanned aerial vehicles with carrier arms comprises the following steps that an improved non-dominant sequential evolution algorithm in the step 3 is specifically combined with an NSGA-II algorithm, namely, the track planning of each unmanned aerial vehicle with carrier arms is regarded as a plurality of sub-populations, information exchange is carried out among the sub-populations by optimal individuals, and iterative optimization is independently carried out in the sub-populations by using the NSGA-II algorithm; considering the synergy between the unmanned submarines of the plurality of carrying arms, the time-space synergy coefficient is to be utilized to replace the crowded distance in the traditional algorithm.
Specifically, a cooperative collaborative algorithm is combined with an NSGA-II algorithm, a change dominance relation is defined, a flexibility function is obtained, each optimization target is balanced after the change dominance relation is changed, the flexibility function is used for tracking the target, and the balance optimization targets are combined with easy tracking of the target, so that multi-target track optimization is achieved.
A motion planning method for carrying arm unmanned submarine multimachine cooperative transportation, wherein the optimal motion planning for realizing cooperative transportation in the step 3 is specifically that the adopted multi-optimization targets are as follows
Wherein n is the number of joints; m is the number of the length of the rear arm of the ith joint; t 0 is the planned start time; t f is the planned termination time; g 1 is an action amplitude index in the conveying process, alpha i is an action amplitude coefficient of each joint, and the action amplitude index is the sum of the lengths d of all rods behind the ith joint, and the weight corresponding to the boat body is larger when the action amplitude index is closer to the boat body; g 2 is an energy consumption index, the larger the index is, the larger the energy consumption is; g 3 is a joint impact index, which relates to the jerkiness of the joint angle, and the smaller the index is, the smaller the joint impact is, and the more stable the submarine operation is; delta is the Euclidean distance from the solving point p to the target point q by an algorithm, and delta is an index of the positioning precision of the mechanical arm; θ i is the position,Is angular velocity,/>Is angular acceleration.
Specifically, the closed-chain kinematic model of the unmanned submersible vehicle with the multiple heterogeneous arm is built together with the kinematics of the unmanned submersible vehicle with the single arm based on the pose constraint and the velocity constraint obtained by analysis, and the coordination consistency among the unmanned submersible vehicles with the multiple heterogeneous arm in the carrying process is realized by using the model, and meanwhile, the optimal time, the optimal energy, the optimal contact force and the optimal stability of the unmanned submersible vehicle with the arm in the carrying process are realized.
Example two
The embodiment of the invention provides a motion planning system for collaborative handling of a multi-arm unmanned submarine, which comprises a modeling unit, a track planning unit and an optimal motion planning unit
The modeling unit is used for establishing a closed chain kinematic model of the unmanned carrier arm submarines of the heterogeneous carrier arm submarines in the carrying process through single carrier arm unmanned submarines kinematics and pose constraint and speed constraint;
The track planning unit is used for realizing track planning of the multi-arm unmanned underwater vehicle by adopting a high-order polynomial interpolation mode based on a closed-chain kinematic model;
the optimal motion planning unit is based on the track planning of the multi-arm unmanned submarine, and an improved non-dominant sequential evolution algorithm is used for realizing optimal motion planning of collaborative handling.
From the above, the embodiment of the invention realizes the accuracy and stability of the unmanned arm-carrying submersible vehicle in the carrying process by carrying out coordinated motion planning on the unmanned arm-carrying submersible vehicle, and greatly improves the robustness of the system to external interference. Experimental results show that the coordinated motion planning of the method optimizes a plurality of performance indexes including time optimization, energy optimization, contact force optimization, stability optimization and the like while planning the actual underwater carrying purpose.
Example III
The embodiment of the invention provides an electronic device, which comprises a memory, a processor and a computer program stored in the memory and capable of running on the processor, wherein the memory is used for storing the software program and a module, and the processor executes various functional applications and data processing by running the software program and the module stored in the memory. The memory and the processor are connected by a bus. In particular, the processor implements any of the steps of the above-described embodiment by running the above-described computer program stored in the memory.
It should be appreciated that in embodiments of the present invention, the Processor may be a central processing unit (Central Processing Unit, CPU), which may also be other general purpose Processor, digital signal Processor (DIGITAL SIGNAL Processor, DSP), application SPECIFIC INTEGRATED Circuit (ASIC), off-the-shelf Programmable gate array (Field-Programmable GATEARRAY, FPGA) or other Programmable logic device, discrete gate or transistor logic device, discrete hardware components, or the like. A general purpose processor may be a microprocessor or the processor may be any conventional processor or the like.
The memory may include read-only memory, flash memory, and random access memory, and provides instructions and data to the processor. Some or all of the memory may also include non-volatile random access memory.
From the above, the electronic device provided by the embodiment of the invention can implement the coordinated motion planning method for the multi-arm unmanned underwater vehicle according to the first embodiment by running the computer program, thereby realizing the accuracy and stability of the arm unmanned underwater vehicle in the carrying process and greatly improving the robustness of the system to external interference. Experimental results show that the coordinated motion planning of the method optimizes a plurality of performance indexes including time optimization, energy optimization, contact force optimization, stability optimization and the like while planning the actual underwater carrying purpose.
It should be appreciated that the above-described integrated modules/units, if implemented in the form of software functional units and sold or used as stand-alone products, may be stored in a computer-readable storage medium. Based on such understanding, the present invention may implement all or part of the flow of the method of the above embodiment, or may be implemented by instructing related hardware by a computer program, where the computer program may be stored in a computer readable storage medium, and the computer program may implement the steps of each of the method embodiments described above when executed by a processor. The computer program comprises computer program code, and the computer program code can be in a source code form, an object code form, an executable file or some intermediate form and the like. The computer readable medium may include: any entity or device capable of carrying the computer program code described above, a recording medium, a U disk, a removable hard disk, a magnetic disk, an optical disk, a computer Memory, a Read-Only Memory (ROM), a random access Memory (Random Access Memory, RAM), an electrical carrier signal, a telecommunications signal, a software distribution medium, and so forth. The content of the computer readable storage medium can be appropriately increased or decreased according to the requirements of the legislation and the patent practice in the jurisdiction.
The previous description of the disclosed embodiments is provided to enable any person skilled in the art to make or use the present invention. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the generic principles defined herein may be applied to other embodiments without departing from the spirit or scope of the invention. Thus, the present invention is not intended to be limited to the embodiments shown herein but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.
It will be apparent to those skilled in the art that, for convenience and brevity of description, only the above-described division of the functional units and modules is illustrated, and in practical application, the above-described functional distribution may be performed by different functional units and modules according to needs, i.e. the internal structure of the apparatus is divided into different functional units or modules to perform all or part of the above-described functions. The functional units and modules in the embodiment may be integrated in one processing unit, or each unit may exist alone physically, or two or more units may be integrated in one unit, where the integrated units may be implemented in a form of hardware or a form of a software functional unit. In addition, the specific names of the functional units and modules are only for distinguishing from each other, and are not used for limiting the protection scope of the present invention. The specific working process of the units and modules in the above system may refer to the corresponding process in the foregoing method embodiment, which is not described herein again.
It should be noted that, the method and the details thereof provided in the foregoing embodiments may be combined into the apparatus and the device provided in the embodiments, and are referred to each other and are not described in detail.
Those of ordinary skill in the art will appreciate that the elements and algorithm steps of the examples described in connection with the embodiments disclosed herein may be implemented as electronic hardware, or combinations of computer software and electronic hardware. Whether such functionality is implemented as hardware or software depends upon the particular application and design constraints imposed on the solution. Skilled artisans may implement the described functionality in varying ways for each particular application, but such implementation decisions should not be interpreted as causing a departure from the scope of the present invention.
In the embodiments provided in the present invention, it should be understood that the disclosed apparatus/terminal device and method may be implemented in other manners. For example, the apparatus/device embodiments described above are merely illustrative, e.g., the division of modules or elements described above is merely a logical functional division, and may be implemented in other ways, e.g., multiple elements or components may be combined or integrated into another system, or some features may be omitted, or not performed.
The above embodiments are only for illustrating the technical solution of the present invention, and are not limiting; although the invention has been described in detail with reference to the foregoing embodiments, it will be understood by those of ordinary skill in the art that: the technical scheme described in the foregoing embodiments can be modified or some technical features thereof can be replaced by equivalents; such modifications and substitutions do not depart from the spirit and scope of the technical solutions of the embodiments of the present invention, and are intended to be included in the scope of the present invention.
Claims (10)
1. The motion planning method for the multi-machine collaborative handling of the unmanned arm-carrying submarine is characterized by comprising the following steps of:
step 1: a closed chain kinematic model of the unmanned carrier arm submarines of the heterogeneous carrier arm submarines in the carrying process is established jointly through single carrier arm unmanned submarines kinematics and pose constraint and speed constraint;
step 2: based on the closed-chain kinematic model in the step 1, a high-order polynomial interpolation mode is adopted to realize the track planning of the multi-arm unmanned underwater vehicle;
Step 3: based on the multi-arm unmanned submarine trajectory planning in the step 2, an improved non-dominant sequential evolution algorithm is used for realizing the optimal motion planning of cooperative transportation, and the improved non-dominant sequential evolution algorithm is specifically that a crowding distance in a traditional algorithm is replaced by a time-space cooperative coefficient.
2. The motion planning method for multi-machine collaborative handling of the unmanned submersible vehicle with the carrier arm according to claim 1, wherein the pose constraint of the step 1 is specifically that,
Wherein q l is the boat pose and the manipulator joint variable of the main arm unmanned submersible vehicle, and q f is the boat pose and the manipulator joint variable of the auxiliary arm unmanned submersible vehicle; The method is a position vector of a centroid of a carrying object in a main manipulator base coordinate system; /(I) The position vector of the end effector of the main carrier arm unmanned underwater vehicle in the base coordinate system of the main carrier arm unmanned underwater vehicle; /(I)Cosine of the rigid body centroid to a direction in the main manipulator end effector coordinate system; /(I)Is the cosine of the direction of the main manipulator end effector coordinate system relative to the base coordinate system.
3. The motion planning method for multi-machine collaborative handling of a boom-mounted unmanned submersible vehicle according to claim 1, wherein the speed constraint of step 1 is specifically that,
In the method, in the process of the invention,An angular velocity vector that is the centroid of the object; /(I)Is the velocity vector of the unmanned submarine body of the main carrying arm and the joint of the manipulator; j ll(ql)∈R3×n is a pose Jacobian matrix of the main submarine; j la(ql)∈R3×n is the attitude jacobian matrix of the main submersible; j +(qf) is the pseudo-inverse of the jacobian matrix from the submarine.
4. A motion planning method for multi-machine collaborative handling of a boom-mounted unmanned submarine according to claim 2 or 3, wherein the trajectory planning result based on the five-degree polynomial curve interpolation in step2 is shown in fig. 3, and the trajectory can satisfy the following three constraint conditions:
(1) Third order derivative of position-time curve
(2) Second order derivative of speed-time curve
(3) First order conductance of acceleration-time curve
When interpolation is carried out when n points exist, interpolation points need to be carried out between every two interpolation points, n-1 sections of quintic curves can appear, 6n-6 parameters are undetermined, and corresponding constraints are as follows: the interpolation equation can be solved because of 6n-6 constraints when five curve fits are performed on adjacent points in the n points.
5. The method for planning the movement of the unmanned arm-carrying submersible vehicle multi-machine collaborative handling according to claim 4, wherein the step 2 higher order polynomial interpolation is specifically,
q(t)＝b+a1(t-t0)+a2(t-t0)2+a3(t-t0)3+a4(t-t0)4+a5(t-t0)5,
B, a 1,a2,a3,a4,a5 is undetermined parameters of different order items of the planned track, and the complete planned track can be obtained by resolving the 6 undetermined parameters; t is the current running time, and t 0 is the planning starting time;
in the case of two points, the position of the start point, the position of the end point, the speed and the acceleration are known, i.e
q(t0)＝q0,q(t1)＝q1，
Wherein q (t 0) is an initial state,For initial speed,/>For initial acceleration, q (t 1) is the end point state,To reach the end point speed,/>To reach the acceleration at the end point, q 0 is the initial state, q 1 is the end point state, v 0 is the initial speed, and v 1 is the speed at the end point.
6. The motion planning method for multi-machine collaborative handling of a boom-mounted unmanned submersible vehicle according to claim 5, wherein let T = T 1-t0,h＝q1-q0, then T represents a time step and h represents a variable quantity of the submersible vehicle state;
Then it is possible to obtain:
7. The method for planning the movement of the multi-machine collaborative handling of the unmanned submersible vehicle with the carrier arm according to claim 1, wherein the improved non-dominant rank-ordered evolution algorithm in the step 3 is specifically that the cooperative collaborative algorithm is combined with an NSGA-II algorithm, namely, the trajectory planning of the unmanned submersible vehicle with each carrier arm is regarded as a plurality of sub-populations, information communication is carried out among the sub-populations by optimal individuals, and iterative optimization is independently carried out in the sub-populations by using the NSGA-II algorithm.
8. The motion planning method for multi-machine cooperative transportation of unmanned submersible vehicle with carrier arm according to claim 7, wherein the optimal motion planning for realizing cooperative transportation in the step 3 is specifically that the adopted multi-optimization targets are as follows
Wherein n is the number of joints; m is the number of the length of the rear arm of the ith joint; t 0 is the planned start time; t f is the planned termination time; g 1 is an action amplitude index in the conveying process, alpha i is an action amplitude coefficient of each joint, and the action amplitude index is the sum of the lengths d of all rods behind the ith joint, and the weight corresponding to the boat body is larger when the action amplitude index is closer to the boat body; g 2 is an energy consumption index, the larger the index is, the larger the energy consumption is; g 3 is a joint impact index, which relates to the jerkiness of the joint angle, and the smaller the index is, the smaller the joint impact is, and the more stable the submarine operation is; delta is the Euclidean distance from the solving point p to the target point q by an algorithm, and delta is an index of the positioning precision of the mechanical arm; θ i is the position,Is angular velocity,/>Is angular acceleration.
9. The electronic equipment is characterized by comprising a processor, a communication interface, a memory and a communication bus, wherein the processor, the communication interface and the memory are communicated with each other through the communication bus;
A memory for storing a computer program;
a processor for implementing the method of any of claims 1-8 when executing a program stored on a memory.
10. A computer readable storage medium, characterized in that the computer readable storage medium has stored therein a computer program which, when executed by a processor, implements the method of any of claims 1-8.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311566702.1A CN117687430B (en) | 2023-11-23 | 2023-11-23 | Motion planning method for multi-machine collaborative handling of unmanned arm-carrying submarine |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311566702.1A CN117687430B (en) | 2023-11-23 | 2023-11-23 | Motion planning method for multi-machine collaborative handling of unmanned arm-carrying submarine |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN117687430A CN117687430A (en) | 2024-03-12 |
| CN117687430B true CN117687430B (en) | 2024-06-25 |
Family
ID=90138109
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311566702.1A Active CN117687430B (en) | 2023-11-23 | 2023-11-23 | Motion planning method for multi-machine collaborative handling of unmanned arm-carrying submarine |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117687430B (en) |
Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106557844A (en) * | 2016-11-23 | 2017-04-05 | 华东理工大学 | A kind of welding robot paths planning method |
| CN113296407A (en) * | 2021-05-25 | 2021-08-24 | 南京航空航天大学 | Multi-machine cooperative track optimization method based on 5-time non-uniform rational B-spline |
Family Cites Families (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103464344B (en) * | 2013-09-23 | 2016-01-20 | 电子科技大学中山学院 | Economical spraying robot spray gun track planning method |
| CN110598863A (en) * | 2019-09-05 | 2019-12-20 | 南宁师范大学 | Multi-target differential evolution method for co-evolution |
| CN114489040A (en) * | 2021-12-13 | 2022-05-13 | 中煤科工集团信息技术有限公司 | Hybrid path planning method based on improved A-star algorithm and artificial potential field algorithm |
| CN116698032A (en) * | 2023-05-16 | 2023-09-05 | 哈尔滨工程大学 | Unmanned aerial vehicle track planning method based on multi-target self-organizing squirrel foraging mechanism |
-
2023
- 2023-11-23 CN CN202311566702.1A patent/CN117687430B/en active Active
Patent Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106557844A (en) * | 2016-11-23 | 2017-04-05 | 华东理工大学 | A kind of welding robot paths planning method |
| CN113296407A (en) * | 2021-05-25 | 2021-08-24 | 南京航空航天大学 | Multi-machine cooperative track optimization method based on 5-time non-uniform rational B-spline |
Also Published As
| Publication number | Publication date |
|---|---|
| CN117687430A (en) | 2024-03-12 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111538949B (en) | Redundant robot inverse kinematics solving method and device and redundant robot | |
| US20190009408A1 (en) | Apparatus and methods for programming and training of robotic devices | |
| CN106708069B (en) | Coordinated planning and control method of underwater movement robot | |
| CN109050835B (en) | Full-drive autonomous underwater robot structure and recovery three-dimensional path tracking method | |
| CN109669479A (en) | A kind of mobile robot trace tracking and controlling method based on event triggering | |
| CN110609556A (en) | Multi-unmanned-boat cooperative control method based on LOS navigation method | |
| CN113296507B (en) | Multi-power positioning ship cooperative formation control method based on space-time decoupling | |
| CN114779645B (en) | Pectoral fin flapping robot fish formation control method under directional fixed communication topology | |
| CN112083727B (en) | Multi-autonomous system distributed collision avoidance formation control method based on speed obstacle | |
| CN114115262B (en) | Multi-AUV actuator saturation cooperative formation control system and method based on azimuth information | |
| CN111766783A (en) | Cluster system-oriented formation enclosure tracking method capable of converging in limited time | |
| CN112684781B (en) | Multi-agent distributed model prediction control method and system | |
| CN112363527A (en) | Multi-aircraft cooperative trapping method and system based on optimal control theory | |
| Tan et al. | A local path planning method based on Q-learning | |
| CN116501056A (en) | Vehicle track tracking control method and device, unmanned vehicle and storage medium | |
| CN113961001B (en) | RVO strategy and optimal control algorithm-based hybrid multi-agent cooperative path planning method | |
| CN117687430B (en) | Motion planning method for multi-machine collaborative handling of unmanned arm-carrying submarine | |
| CN111531532A (en) | Robot climbing movement speed modeling method based on momentum theory | |
| CN112034869A (en) | Design method and application of variable parameter neurodynamics controller of unmanned aerial vehicle | |
| Song et al. | A TC-RRT-based path planning algorithm for the nonholonomic mobile robots | |
| CN114545971A (en) | Multi-agent distributed flyable path planning method, system, computer equipment and medium under communication constraint | |
| Wang et al. | Cooperative hunting of spherical multi-robots based on improved artificial potential field method | |
| CN114967441A (en) | Networked incomplete constraint multi-robot grouping consistent tracking control method, micro-control unit and control system | |
| CN114721273A (en) | Multi-agent formation control method of fixed time convergence zero-ization neural network | |
| CN113619815A (en) | Spacecraft cluster dynamic path planning 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 |