Trajectory optimization method for unmanned underwater vehicle with boom based on speed maneuverability measurement

Abstract

The present invention belongs to the technical field of unmanned submersibles, and specifically relates to a trajectory optimization method for an arm-carrying unmanned submersible based on speed operability measurement. A kinematic model of an arm-carrying unmanned submersible is established based on the principle of rigid body kinematics; the capture trajectory of the arm-carrying unmanned submersible is planned in the joint space based on the skill learning method; the speed operability ellipsoid is constrained in the Riemann space based on the zero space cost function to optimize the capture trajectory of the arm-carrying unmanned submersible, realize the dexterous and precise control operation of the arm-carrying unmanned submersible in a complex underwater environment, enhance the robustness of the arm-carrying unmanned submersible to external interference, and improve the operating efficiency of the arm-carrying unmanned submersible. The present invention solves the problem of the accuracy of capturing dynamic targets during the operation of underwater manipulators and the problem of the dexterity of capturing moving underwater manipulators.

Description

Technical Field

[0001] The present invention belongs to the technical field of unmanned underwater vehicles, and in particular relates to a trajectory optimization method of an arm-carrying unmanned underwater vehicle based on speed operability measurement. Background Art

[0002] The arm-carrying unmanned submersible is flexible and has strong autonomous operation capabilities. It can perform various complex underwater operations through its mechanical arm, such as grasping, transporting, repairing and other control tasks. Therefore, the arm-carrying unmanned submersible has a wide range of application prospects in underwater operations and can be used in the fields of marine resource development, seabed facility maintenance, and sunken ship fishing.

[0003] Most industrial-scale underwater manipulators still use master-slave control systems, and the accuracy and efficiency of underwater operations are low. How to enable underwater robots to autonomously plan a safe, stable and precise motion trajectory so that underwater manipulators can still complete their operating tasks well under the influence of factors such as water flow is the current research focus.

[0004] Although the prior art considers the singularity points in the process of underwater manipulators and can avoid the manipulators from losing control, it lacks autonomy through remote control, and it is difficult for the underwater robot to autonomously adjust the motion trajectory of the underwater manipulators in time when the operating environment changes. The focus of the present invention is to design a trajectory optimization method for an arm-carrying unmanned submersible, plan the motion trajectory of the underwater manipulator through a skill learning method, and optimize the motion trajectory considering the manipulator's operating ability, so as to achieve the dexterity and fine control operation of the arm-carrying unmanned submersible in a complex underwater environment, enhance the robustness of the arm-carrying unmanned submersible to external interference, and improve the operating efficiency of the arm-carrying unmanned submersible. Summary of the invention

[0005] The present invention provides a trajectory optimization method for an arm-carrying unmanned underwater vehicle based on speed operability measurement, which is used to solve the accuracy problem of capturing dynamic targets during the operation of an underwater mechanical arm and the dexterity problem of capturing a moving underwater mechanical arm.

[0006] The present invention is achieved through the following technical solutions:

[0007] A trajectory optimization method for an arm-carrying unmanned underwater vehicle based on speed operability measurement, the trajectory optimization method comprising the following steps:

[0008] Step 1: Establish the kinematic model of the arm-carrying unmanned underwater vehicle;

[0009] Step 2: Based on the kinematic model of step 1, design the dynamic motion primitives of the capture in the joint space;

[0010] Step 3: Based on the dynamic motion primitives designed in the joint space in step 2, the training weight vector of the teaching trajectory is given, and the underwater manipulator capture trajectory is planned according to the position of the target point;

[0011] Step 4: Based on the underwater manipulator capture trajectory planned in step 3, considering the manipulator operation capability, the capture trajectory is optimized by constraining the velocity maneuverability ellipsoid in Riemann space in combination with the zero space cost function.

[0012] Furthermore, the step 1 is specifically to establish a kinematic model of the arm-carrying unmanned underwater vehicle based on the rigid body kinematics principle, describe the mapping relationship between the motion of the end effector of the manipulator arm and the motion of each joint of the arm and each degree of freedom of the hull, and the positive kinematic equation of the end effector of the arm-carrying unmanned underwater vehicle is shown as follows:

[0013]

[0014]

[0015] in, is the generalized velocity vector of the underwater manipulator end effector, ; is the velocity Jacobian matrix of the underwater manipulator end effector; when hour, , , is the position vector of the arm-carrying unmanned underwater vehicle, is the underwater manipulator joint angle vector, are the position components of the arm-carrying unmanned underwater vehicle in the longitudinal, lateral and vertical directions respectively, They are the attitude components of the boom-carrying unmanned submersible in the three directions of roll, pitch and bow, They are the two joint angle components of the underwater manipulator; when hour, , , is the velocity vector of the UUV, is the joint angular velocity vector of the underwater manipulator, They are the linear velocity components corresponding to the three position components of the arm-carrying unmanned underwater vehicle, They are the angular velocity components corresponding to the three attitude components of the arm-carrying unmanned underwater vehicle, They are the two joint angular velocity components of the underwater manipulator; Represents the cross product matrix of a vector; is the translational Jacobian of the underwater manipulator, ; is the rotation Jacobian of the underwater manipulator, , is a 3×3 identity matrix, is a 3×3 zero matrix.

[0016] Furthermore, the step 2 is specifically that the conversion system used is as shown in the following formula:

[0017]

[0018] in, is the total duration of the expected motion trajectory, is the angular velocity of the underwater manipulator's scaling joint, , is a constant coefficient that ensures the system is a critical damping system, is the expected position of each joint of the underwater manipulator, , is the current joint position of the underwater manipulator, , is the phase variable, is a nonlinear forcing term, as shown below:

[0019]

[0020] in, is the initial position of each joint of the underwater manipulator, is the weight vector associated with the joint rotation, is the radial basis function, is the number of radial basis functions, is the i-th radial basis function;

[0021] The designed canonical system of dynamic motion primitives for capturing in joint space is shown as follows:

[0022]

[0023] in, is a constant coefficient that ensures that the angular velocity of the scaled joint converges to zero. is a phase variable.

[0024] Furthermore, the step 3 comprises the following steps:

[0025] Step 3.1: Input the teaching joint trajectory , determine the model parameters and the number of radial basis functions , is the joint angle taught at time t, is the angular velocity of the teaching joint at time t, is the teaching joint angular acceleration at time t, P is the total step length of the teaching joint trajectory;

[0026] Step 3.2: Train the weight vector using local weighted regression ;

[0027] Step 3.3: Reproduce and generalize the motion trajectory according to the desired configuration of the underwater manipulator .

[0028] Furthermore, the step 4 specifically includes the following steps:

[0029] Step 4.1: Based on step 3, the trajectory of the underwater manipulator is planned, and the motion trajectory of the underwater manipulator end effector is obtained through forward kinematics; and considering the operation capability of the underwater manipulator, a set of desired speed maneuverability ellipsoids is determined. , combined with the zero space cost function to optimize the motion trajectory of the underwater manipulator;

[0030] Step 4.2: Determine the number of iterations and the scaling factor , iterate at each path point to obtain the expected joint velocity after optimization , and then get the optimized underwater manipulator joint trajectory ;

[0031] Step 4.3: Using Riemann distance Determine whether the speed operability ellipsoid profile error meets the accuracy requirements. If so, output the optimized underwater manipulator joint trajectory , the algorithm ends, otherwise iterate again from step 4.2.

[0032] Furthermore, the step 4.1 is specifically based on the zero space cost function The specific expression of the control speed maneuverability ellipsoid and the zero space cost function is as follows:

[0033]

[0034] in, is the desired velocity maneuverability ellipsoid at the current moment, It is the actual velocity maneuverability ellipsoid at the current moment.

[0035] Furthermore, the step 4.2 is specifically to design the optimized expected joint velocity vector based on the kinematic equation The specific expression is as follows:

[0036]

[0037] in, is the weighted pseudo-inverse Jacobian matrix of the arm-carrying unmanned underwater vehicle, is the proportionality coefficient between the gradient vector magnitude and the system redundant solution, is the identity matrix, is the zero space cost function, is the end effector pose, is the Jacobian matrix of the end effector.

[0038] Furthermore, the step 4.3 is specifically to use the Riemann distance in the Riemann space Measures the profile error between the actual velocity maneuverability ellipsoid and the desired velocity maneuverability ellipsoid.

[0039] Furthermore, the Riemann distance in the Riemann space The specific expression is shown as follows:

[0040]

[0041] in, is a Riemann space point to The vector of It is the Frobenius norm used to measure the length of a vector.

[0042] Furthermore, the optimized expected joint velocity vector , update the underwater manipulator joint trajectory , further calculations are performed to obtain the optimized velocity maneuverability ellipsoid , using the Riemann distance The error contour between it and the desired velocity maneuverability ellipsoid is measured.

[0043] The beneficial effects of the present invention are:

[0044] The present invention reproduces and generalizes the motion trajectory of the underwater manipulator after learning the teaching trajectory through a skill learning method. The manipulator's operating ability is considered in Riemann space, and the motion trajectory of the underwater manipulator is optimized based on the zero space cost function. The trajectory optimization method enables the underwater robot to have better autonomy, and can realize the dexterous and fine control operation of the arm-carrying unmanned submersible in a complex underwater environment, enhance the robustness of the arm-carrying unmanned submersible to external interference, and improve the operating efficiency of the arm-carrying unmanned submersible. BRIEF DESCRIPTION OF THE DRAWINGS

[0045] Figure 1 Flow chart of the method of the present invention.

[0046] Figure 2a to Figure 2f This is the simulation verification result of the present invention using the skill learning method to capture dynamic primitives in joint space and generalize the motion trajectory of the arm-carrying unmanned underwater vehicle. Figure 2a Reproduce joint trajectories for underwater robotic arms, Figure 2b Generalize joint trajectories for underwater manipulators, Figure 2c Reproduce the joint angular velocity curve for the underwater manipulator, Figure 2dGeneralize the joint angular velocity curve for the underwater manipulator, Figure 2e Reproduce the joint angular acceleration curve for the underwater manipulator, Figure 2f Generalize the joint angular acceleration curves for underwater manipulators.

[0047] Figures 3a to 3f The following are the visual diagrams of the cooperative capture motion state of the unmanned underwater vehicle under different working conditions of the present invention, where Figure 3a For working condition 1 t = The visual image of the coordinated capture motion state of the unmanned underwater vehicle at 1 second, Figure 3b For working condition 2 t = The visual image of the coordinated capture motion state of the unmanned underwater vehicle at 1 second, Figure 3c For working condition 1 t = The visual image of the coordinated capture motion state of the unmanned underwater vehicle at 3 seconds, Figure 3d For working condition 2 t = The visual image of the coordinated capture motion state of the unmanned underwater vehicle at 3 seconds, Figure 3e For working condition 1 t = The visual image of the coordinated capture motion state of the unmanned underwater vehicle at 5 seconds, Figure 3f For working condition 2 t = Visual diagram of the coordinated capture motion state of the unmanned underwater vehicle at 5 seconds.

[0048] Figure 4a to Figure 4b Schematic diagram of simulation verification results of optimizing the motion trajectory of the arm-carrying unmanned underwater vehicle based on the zero space cost function and considering the manipulator operation capability in Riemann space in the present invention, wherein Figure 4a Schematic diagram of simulation verification results of the expected terminal trajectory. Figure 4b Schematic diagram of simulation verification results of expected operability ellipsoid and actual operability ellipsoid. DETAILED DESCRIPTION

[0049] In the following description, specific details such as specific system structures, technologies, etc. are provided for the purpose of illustration rather than limitation, so as to provide a thorough understanding of the embodiments of the present application. However, it should be clear to those skilled in the art that the present application may also be implemented in other embodiments without these specific details. In other cases, detailed descriptions of well-known systems, devices, circuits, and methods are omitted to prevent unnecessary details from obstructing the description of the present application.

[0050] It should be understood that when used in this specification and the appended claims, the term "comprising" indicates the presence of described features, integers, steps, operations, elements and / or components, but does not exclude the presence or addition of one or more other features, integers, steps, operations, elements, components and / or groups thereof.

[0051] It should also be understood that the terms used in this application specification are only for the purpose of describing specific embodiments and are not intended to limit the application. As used in this application specification and the appended claims, the singular forms "a", "an" and "the" are intended to include plural forms unless the context clearly indicates otherwise.

[0052] The following is a clear and complete description of the technical solutions in the embodiments of the present application in conjunction with the drawings of the present application specification. Obviously, the described embodiments are only part of the embodiments of the present application, not all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of this application.

[0053] In the following description, many specific details are set forth to facilitate a full understanding of the present application, but the present application may also be implemented in other ways different from those described herein, and those skilled in the art may make similar generalizations without violating the connotation of the present application. Therefore, the present application is not limited to the specific embodiments disclosed below.

[0054] Implementation Method 1

[0055] This embodiment provides a trajectory optimization method for an unmanned submersible vehicle with a boom based on speed operability measurement, such as Figure 1 As shown, the arm-carrying unmanned underwater vehicle includes a hull and an underwater mechanical arm arranged on the hull, the underwater mechanical arm has two joint degrees of freedom, and the trajectory optimization method includes the following steps:

[0056] Step 1: Establish the kinematic model of the arm-carrying unmanned underwater vehicle;

[0057] Step 2: Based on the kinematic model of step 1, design the dynamic motion primitives of the capture in the joint space;

[0058] Step 3: Based on the dynamic motion primitives designed in the joint space in step 2, the training weight vector of the teaching trajectory is given, and the underwater manipulator capture trajectory is planned according to the position of the target point;

[0059] Step 4: Based on the underwater manipulator capture trajectory planned in step 3, considering the manipulator operation capability, the capture trajectory is optimized by constraining the velocity maneuverability ellipsoid in Riemann space in combination with the zero space cost function.

[0060] Furthermore, the step 1 of establishing the kinematic model of the arm-carrying unmanned submersible is specifically to establish the kinematic model of the arm-carrying unmanned submersible based on the rigid body kinematics principle, describe the mapping relationship between the motion of the end effector of the manipulator and the motion of each joint of the arm and each degree of freedom of the hull, and the positive kinematic equation of the end effector of the arm-carrying unmanned submersible is shown as follows:

[0061]

[0062]

[0063] in, is the generalized velocity vector of the underwater manipulator end effector, ; is the velocity Jacobian matrix of the underwater manipulator end effector; when hour, , , is the position vector of the arm-carrying unmanned underwater vehicle, is the underwater manipulator joint angle vector, are the position components of the arm-carrying unmanned underwater vehicle in the longitudinal, lateral and vertical directions respectively, They are the attitude components of the boom-carrying unmanned submersible in the three directions of roll, pitch and bow, They are the two joint angle components of the underwater manipulator; when hour, , , is the velocity vector of the UUV, is the joint angular velocity vector of the underwater manipulator, They are the linear velocity components corresponding to the three position components of the arm-carrying unmanned underwater vehicle, They are the angular velocity components corresponding to the three attitude components of the arm-carrying unmanned underwater vehicle, They are the two joint angular velocity components of the underwater manipulator; Represents the cross product matrix of a vector; is the translational Jacobian of the underwater manipulator, ; is the rotation Jacobian of the underwater manipulator, , is a 3×3 identity matrix, is a 3×3 zero matrix.

[0064] Furthermore, the step 2 of designing the dynamic motion primitives for capturing in the joint space is specifically that the designed dynamic motion primitive trajectory planning method in the joint space can reproduce and generalize the teaching trajectory, and even adjust the initial and final configurations of the manipulator when the target point changes, so as to ensure the operation accuracy of the manipulator in the complex underwater environment. The conversion system used is shown in the following formula:

[0065]

[0066] in, is the total duration of the expected motion trajectory, is the angular velocity of the underwater manipulator's scaling joint, , is a constant coefficient that ensures the system is a critical damping system, is the expected position of each joint of the underwater manipulator, , is the current joint position of the underwater manipulator, , is the phase variable, is a nonlinear forcing term, as shown below:

[0067]

[0068] in, is the initial position of each joint of the underwater manipulator, is the weight vector associated with the joint rotation, is the radial basis function, is the number of radial basis functions, is the i-th radial basis function;

[0069] Since each joint has an independent transformation system, the canonical system for capturing dynamic motion primitives in a common joint space adopts the following formula to ensure the consistency of motion time of different joints, which is specifically expressed as:

[0070]

[0071] in, is a constant coefficient that ensures that the angular velocity of the scaled joint converges to zero. is a phase variable.

[0072] Furthermore, the step 3 comprises the following steps:

[0073] Step 3.1: Input the teaching joint trajectory , determine the model parameters and the number of radial basis functions , is the joint angle taught at time t, is the angular velocity of the teaching joint at time t, is the teaching joint angular acceleration at time t, P is the total step length of the teaching joint trajectory;

[0074] Step 3.2: Train the weight vector using local weighted regression ;

[0075] Step 3.3: Reproduce and generalize the motion trajectory according to the desired configuration of the underwater manipulator .

[0076] Furthermore, the step 4 specifically includes the following steps:

[0077] Step 4.1: Based on step 3, the trajectory of the underwater manipulator is planned, and the motion trajectory of the underwater manipulator end effector is obtained through forward kinematics; and considering the operation capability of the underwater manipulator, a set of desired speed maneuverability ellipsoids is determined. , combined with the zero space cost function to optimize the motion trajectory of the underwater manipulator;

[0078] Step 4.2: Determine the number of iterations and the scaling factor , iterate at each path point to obtain the expected joint velocity after optimization , and then get the optimized underwater manipulator joint trajectory ;

[0079] Step 4.3: Using Riemann distance Determine whether the speed operability ellipsoid profile error meets the accuracy requirements. If so, output the optimized underwater manipulator joint trajectory , the algorithm ends, otherwise iterate again from step 4.2.

[0080] Furthermore, the step 4.1 is specifically based on the zero space cost function The specific expression of the control speed maneuverability ellipsoid and the zero space cost function is as follows:

[0081]

[0082] in, is the desired velocity maneuverability ellipsoid at the current moment, It is the actual velocity maneuverability ellipsoid at the current moment.

[0083] Furthermore, the step 4.2 is specifically to design the optimized expected joint velocity vector based on the kinematic equation The specific expression is as follows:

[0084]

[0085] in, is the weighted pseudo-inverse Jacobian matrix of the arm-carrying unmanned underwater vehicle, is the proportionality coefficient between the gradient vector magnitude and the system redundant solution, is the identity matrix, is the zero space cost function, is the end effector pose, is the Jacobian matrix of the end effector.

[0086] Furthermore, the step 4.3 is specifically to use the Riemann distance in the Riemann space Measures the profile error between the actual velocity maneuverability ellipsoid and the desired velocity maneuverability ellipsoid.

[0087] Furthermore, the Riemann distance in the Riemann space The specific expression is shown as follows:

[0088]

[0089] in, is a Riemann space point to The vector of It is the Frobenius norm used to measure the length of a vector.

[0090] Furthermore, the optimized expected joint velocity vector , update the underwater manipulator joint trajectory , further calculations are performed to obtain the optimized velocity maneuverability ellipsoid , using the Riemann distance The error contour between it and the desired velocity maneuverability ellipsoid is measured.

[0091] like Figure 2a to Figure 2f As shown, Figures 3a to 3f As shown Figure 4a to Figure 4b shown.

[0092] Figure 3a , Figure 3c , Figure 3e They are the motion scenes at t=1s, t=3s, and t=5s under the first working condition. Under the first working condition, after the unmanned submersible approaches the capture object, the capture object remains stationary in the water flow, and the two unmanned submersibles perform the capture according to the planned trajectory movement.

[0093] Figure 3b , Figure 3d , Figure 3f They are the motion scenes at t=1s, t=3s, and t=5s under the second working condition. Under the second working condition, after the unmanned submersible approaches the capture object, the capture object undergoes a slight offset and rotation under the influence of the water flow. The two unmanned submersibles generalize the trajectory according to the change of the capture point to execute the capture.

[0094] This implementation method realizes the dexterous and precise control operation of the arm-carrying unmanned submersible in a complex underwater environment, enhances the robustness of the arm-carrying unmanned submersible to external interference, and improves the operating efficiency of the arm-carrying unmanned submersible.

[0095] Implementation Method 2

[0096] This embodiment provides a trajectory optimization system for an arm-carrying unmanned submersible based on speed operability measurement, the system uses the trajectory optimization method for an arm-carrying unmanned submersible as described in embodiment 1, and the system includes:

[0097] Motion model building module: building the kinematic model of the arm-carrying unmanned underwater vehicle;

[0098] Motion primitive design module: Based on the kinematic model established by the motion model building module, it designs the dynamic motion primitives for capturing in the joint space;

[0099] Underwater manipulator capture trajectory planning module: Based on the capture dynamic motion primitives in the joint space designed by the motion primitive design module, the underwater manipulator capture trajectory is planned according to the position of the target point given the teaching trajectory training weight vector;

[0100] Collaborative capture trajectory optimization module: Based on the underwater manipulator capture trajectory planned by the underwater manipulator capture trajectory planning module, the capture trajectory is optimized by considering the manipulator operation capability and combining the zero space cost function to constrain the speed maneuverability ellipsoid in Riemann space.

[0101] The working principle of the motion model establishment module is specifically to establish a kinematic model of the arm-carrying unmanned submersible based on the rigid body kinematics principle, to describe the mapping relationship between the motion of the end effector of the manipulator arm and the motion of each joint of the arm and each degree of freedom of the hull. The positive kinematic equation of the end effector of the arm-carrying unmanned submersible is shown as follows:

[0102]

[0103]

[0104] in, is the generalized velocity vector of the end effector of the underwater manipulator; is the velocity Jacobian matrix of the underwater manipulator end effector; when hour, , , is the position vector of the arm-carrying unmanned underwater vehicle, is the underwater manipulator joint angle vector, are the position components of the arm-carrying unmanned underwater vehicle in the longitudinal, lateral and vertical directions respectively, They are the attitude components of the boom-carrying unmanned submersible in the three directions of roll, pitch and bow, They are the two joint angle components of the underwater manipulator; when hour, , , is the velocity vector of the UUV, is the joint angular velocity vector of the underwater manipulator, They are the linear velocity components corresponding to the three position components of the arm-carrying unmanned underwater vehicle, They are the angular velocity components corresponding to the three attitude components of the arm-carrying unmanned underwater vehicle, They are the two joint angular velocity components of the underwater manipulator; Represents the cross product matrix of a vector; is the translational Jacobian of the underwater manipulator, ; is the rotation Jacobian of the underwater manipulator, , is a 3×3 identity matrix, is a 3×3 zero matrix.

[0105] The working principle of the motion primitive design module is specifically as follows:

[0106]

[0107] in, is the total duration of the expected motion trajectory, is the angular velocity of the underwater manipulator's scaling joint, , is a constant coefficient that ensures the system is a critical damping system, is the expected position of each joint of the underwater manipulator, , is the current joint position of the underwater manipulator, , is the phase variable, is a nonlinear forcing term, as shown below:

[0108]

[0109] in, is the initial position of each joint of the underwater manipulator, is the weight vector associated with the joint rotation, is the radial basis function, is the number of radial basis functions, is the i-th radial basis function;

[0110] Since each joint has an independent transformation system, the canonical system for capturing dynamic motion primitives in a common joint space adopts the following formula to ensure the consistency of motion time of different joints, which is specifically expressed as:

[0111]

[0112] in, is a constant coefficient that ensures that the angular velocity of the scaling joint converges to zero;

[0113] The working principle of the underwater manipulator capture trajectory planning module is as follows:

[0114] Input the teaching joint trajectory to the underwater manipulator capture trajectory planning module , determine the model parameters and the number of radial basis functions , is the joint angle taught at time t, is the angular velocity of the teaching joint at time t, is the teaching joint angular acceleration at time t, P is the total step length of the teaching joint trajectory;

[0115] Using local weighted regression to train the weight vector in the underwater manipulator capture trajectory planning module ;

[0116] In the underwater manipulator capture trajectory planning module, the motion trajectory is reproduced and generalized according to the desired configuration of the underwater manipulator .

[0117] The working principle of the collaborative capture trajectory optimization module is as follows:

[0118] The underwater manipulator capture trajectory is planned based on the underwater manipulator capture trajectory planning module. The motion trajectory of the underwater manipulator end effector is obtained through forward kinematics in the collaborative capture trajectory optimization module. Considering the underwater manipulator's operational capability, a set of desired speed maneuverability ellipsoids is determined. , combined with the zero space cost function to optimize the motion trajectory of the underwater manipulator;

[0119] Determine the number of iterations and the proportional coefficient in the collaborative capture trajectory optimization module , iterate at each path point to obtain the expected joint velocity after optimization , and then get the optimized underwater manipulator joint trajectory ;

[0120] Using Riemann distance in collaborative capture trajectory optimization module Determine whether the speed operability ellipsoid profile error meets the accuracy requirements. If so, output the optimized underwater manipulator joint trajectory , the algorithm ends, otherwise the number of iterations and the proportional coefficient are determined , iterate at each path point to obtain the expected joint velocity after optimization , and then get the optimized underwater manipulator joint trajectory Re-iterate.

[0121] Based on the zero space cost function The specific expression of the control speed maneuverability ellipsoid and the zero space cost function is as follows:

[0122]

[0123] in, is the desired velocity maneuverability ellipsoid at the current moment, It is the actual velocity maneuverability ellipsoid at the current moment.

[0124] Designed optimized expected joint velocity vector based on kinematic equations The specific expression is as follows:

[0125]

[0126] in, is the weighted pseudo-inverse Jacobian matrix of the arm-carrying unmanned underwater vehicle, is the proportionality coefficient between the gradient vector magnitude and the system redundant solution, is the identity matrix, is the zero space cost function, is the end effector pose, is the Jacobian matrix of the end effector.

[0127] Using the Riemann distance in Riemann space Measures the profile error between the actual velocity maneuverability ellipsoid and the desired velocity maneuverability ellipsoid.

[0128] The Riemann distance in the Riemann space The specific expression is shown as follows:

[0129]

[0130] in, is a Riemann space point to The vector of It is the Frobenius norm used to measure the length of a vector.

[0131] The expected joint velocity vector after optimization , update the underwater manipulator joint trajectory , further calculations are performed to obtain the optimized velocity maneuverability ellipsoid , using the Riemann distance The error contour between it and the desired velocity maneuverability ellipsoid is measured.

[0132] As can be seen from the above, the embodiment of the present invention plans the motion trajectory of the underwater manipulator through the skill learning method, and optimizes the motion trajectory considering the manipulator's operating ability, so as to realize the dexterous and fine control operation of the arm-carrying unmanned submersible in a complex underwater environment, enhance the robustness of the arm-carrying unmanned submersible to external interference, and improve the operating efficiency of the arm-carrying unmanned submersible. The experimental results show that the arm-carrying unmanned submersible trajectory optimization method of the present method makes the operating efficiency of the arm-carrying unmanned submersible better than the existing method, which proves the effectiveness and generalization of the method.

[0133] Implementation Method 3

[0134] An embodiment of the present invention provides an electronic device, which includes a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the memory is used to store software programs and modules, and the processor executes various functional applications and data processing by running the software programs and modules stored in the memory. The memory and the processor are connected via a bus. Specifically, the processor implements any step in the first embodiment above by running the computer program stored in the memory.

[0135] It should be understood that in the embodiments of the present invention, the processor may be a central processing unit (CPU), and the processor may also be other general-purpose processors, digital signal processors (DSP), application-specific integrated circuits (ASIC), field-programmable gate arrays (FPGA) or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. A general-purpose processor may be a microprocessor or the processor may also be any conventional processor, etc.

[0136] The memory may include a read-only memory, a flash memory, and a random access memory, and provides instructions and data to the processor. A part or all of the memory may also include a nonvolatile random access memory.

[0137] It can be seen from the above that the electronic device provided in the embodiment of the present invention can implement the trajectory optimization method of the arm-carrying unmanned submersible as described in the first embodiment by running a computer program, thereby realizing the dexterous and precise control operation of the arm-carrying unmanned submersible in a complex underwater environment, enhancing the robustness of the arm-carrying unmanned submersible to external interference, and improving the operating efficiency of the arm-carrying unmanned submersible.

[0138] It should be understood that if the above-mentioned integrated module / unit is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the present invention implements all or part of the processes in the above-mentioned implementation method, and can also be completed by instructing the relevant hardware through a computer program. The above-mentioned computer program can be stored in a computer-readable storage medium. When the computer program is executed by the processor, the steps of the above-mentioned method implementation can be implemented. Among them, the above-mentioned computer program includes computer program code, and the above-mentioned computer program code can be in source code form, object code form, executable file or some intermediate form. The above-mentioned computer-readable medium may include: any entity or device capable of carrying the above-mentioned computer program code, recording medium, U disk, mobile hard disk, disk, optical disk, computer memory, read-only memory (ROM), random access memory (RAM), electric carrier signal, telecommunication signal and software distribution medium. It should be noted that the content contained in the above-mentioned computer-readable storage medium can be appropriately increased or decreased according to the requirements of legislation and patent practice in the jurisdiction.

[0139] The above description of the disclosed embodiments enables one skilled in the art to implement or use the present invention. Various modifications to these embodiments will be apparent to one skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the present invention. Therefore, the present invention will not be limited to the embodiments shown herein, but rather to the widest range consistent with the principles and novel features disclosed herein.

[0140] Those skilled in the art can clearly understand that for the convenience and simplicity of description, only the division of the above-mentioned functional units and modules is used as an example for illustration. In actual applications, the above-mentioned function allocation can be completed by different functional units and modules as needed, that is, the internal structure of the above-mentioned device can be divided into different functional units or modules to complete all or part of the functions described above. The functional units and modules in the implementation method can be integrated into a processing unit, or each unit can exist physically separately, or two or more units can be integrated into one unit. The above-mentioned integrated unit can be implemented in the form of hardware or in the form of software functional units. In addition, the specific names of the functional units and modules are only for the convenience of distinguishing each other, and are not used to limit the scope of protection of the present invention. The specific working process of the units and modules in the above-mentioned system can refer to the corresponding process in the aforementioned method implementation method, which will not be repeated here.

[0141] It should be noted that the methods and detailed examples provided in the above embodiments can be combined with the devices and equipment provided in the embodiments, and references can be made to each other, and no further details will be given.

[0142] Those skilled in the art will appreciate that the units and algorithm steps of each example described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are performed in hardware or software depends on the specific application and design constraints of the technical solution. Professional and technical personnel can use different methods to implement the described functions for each specific application, but such implementation should not be considered to be beyond the scope of the present invention.

[0143] In the embodiments provided by the present invention, it should be understood that the disclosed device / terminal equipment and method can be implemented in other ways. For example, the device / equipment implementation described above is only illustrative, for example, the division of the above modules or units is only a logical function division, and in actual implementation, there may be other division methods, such as multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed.

[0144] The above-mentioned embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit them. Although the present invention has been described in detail with reference to the above-mentioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the above-mentioned embodiments, or replace some of the technical features therein by equivalents. These modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the various embodiments of the present invention, and should all be included in the protection scope of the present invention.

Claims

1. A trajectory optimization method for an arm-carrying unmanned underwater vehicle based on speed operability measurement, characterized in that: The trajectory optimization method comprises the following steps: Step 1: Establish the kinematic model of the arm-carrying unmanned underwater vehicle; Step 2: Based on the kinematic model of step 1, design the dynamic motion primitives of capture in joint space; Step 3: Based on the capture dynamic motion primitives in the joint space designed in step 2, a training weight vector of the teaching trajectory is given, and the capture trajectory of the underwater manipulator is planned according to the position of the target point; Step 4: Based on the underwater manipulator capture trajectory planned in step 3, considering the manipulator's maneuverability, the zero space cost function is combined to constrain the velocity maneuverability ellipsoid in Riemann space to optimize the capture trajectory; The step 4 specifically comprises the following steps: Step 4.1: Based on step 3, the trajectory of the underwater manipulator is planned, and the motion trajectory of the underwater manipulator end effector is obtained through forward kinematics; and considering the operation capability of the underwater manipulator, a set of desired speed maneuverability ellipsoids is determined. Combined with the zero space cost function to optimize the motion trajectory of the underwater manipulator; Step 4.2: Determine the number of iterations and the proportional coefficient k, and iterate at each path point to obtain the expected joint velocity after optimization Then the optimized underwater manipulator joint trajectory is obtained Where P is the total step length of the teaching joint trajectory; Step 4.3: Using the Riemann distance d Υ Determine whether the speed operability ellipsoid profile error meets the accuracy requirements. If so, output the optimized underwater manipulator joint trajectory. The algorithm ends, otherwise iterate again from step 4.2; The step 4.1 is specifically as follows: based on the zero space cost function g t (θ) controls the velocity maneuverability ellipsoid, and the specific expression of the zero space cost function is shown as follows: in, is the desired velocity maneuverability ellipsoid at the current moment, Υ t (θ) is the actual velocity maneuverability ellipsoid at the current moment.

2. The trajectory optimization method of an arm-carrying unmanned underwater vehicle based on speed operability measurement according to claim 1 is characterized in that: Specifically, step 1 is to establish a kinematic model of the arm-carrying unmanned underwater vehicle based on the principle of rigid body kinematics, describe the mapping relationship between the motion of the end effector of the manipulator arm and the motion of each joint of the arm and each degree of freedom of the hull, and the positive kinematic equation of the end effector of the arm-carrying unmanned underwater vehicle is as follows: in, is the generalized velocity vector of the underwater manipulator end effector, J E (ξ) is the velocity Jacobian matrix of the underwater manipulator end effector; when ξ=[η,q] T When, η = [x, y, z, φ, θ, ψ] T , q=[q1,q2] T , η is the position vector of the arm-carrying unmanned submersible, q is the joint angle vector of the underwater manipulator, x, y, z are the position components of the arm-carrying unmanned submersible in the longitudinal, transverse and vertical directions respectively, φ, θ, ψ are the attitude components of the arm-carrying unmanned submersible in the rolling, pitching and bowing directions respectively, q1, q2 are the two joint angle components of the underwater manipulator; when When v = [u, v, w, p, q, r] T , v is the velocity vector of the UUV, is the joint angular velocity vector of the underwater manipulator, u, v, w are the linear velocity components corresponding to the three position components of the arm-carrying unmanned submersible, p, q, r are the angular velocity components corresponding to the three attitude components of the arm-carrying unmanned submersible, They are the two joint angular velocity components of the underwater manipulator; represents the cross product matrix of the vector; J1 is the translation Jacobian of the underwater manipulator, J2 is the rotation Jacobian of the underwater manipulator, I3 is the 3×3 identity matrix, O 3×3 is a 3×3 zero matrix.

3. The trajectory optimization method of an arm-carrying unmanned underwater vehicle based on speed operability measurement according to claim 1 is characterized in that: The step 2 of designing the capture dynamic motion primitive in the joint space is specifically that the designed dynamic motion primitive trajectory planning method in the joint space can reproduce and generalize the teaching trajectory, and even adjust the initial and final configurations of the manipulator when the target point changes, so as to ensure the operation accuracy of the manipulator in the complex underwater environment. The capture dynamic motion primitive conversion system in the joint space is shown as follows: Among them, τ is the total duration of the expected motion trajectory, ω is the angular velocity of the underwater manipulator's scaling joint, and α z , β z is a constant coefficient to ensure that the system is a critical damping system, θ g is the expected position of each joint of the underwater manipulator, θ is the current joint position of the underwater manipulator, x is the phase variable and f(x) is the nonlinear forcing term, as shown below: Among them, θ0 is the initial position of each joint of the underwater manipulator, ω θ，i is the weight vector associated with the joint rotation, ψ i (x) is the radial basis function, N is the number of radial basis functions, and i is the i-th radial basis function; The designed canonical system of dynamic motion primitives for capturing in joint space is shown as follows: τx=-a x x Among them, α x is a constant coefficient that ensures that the scaling joint angular velocity converges to zero, and x is the phase variable.

4. The trajectory optimization method of an arm-carrying unmanned underwater vehicle based on speed operability measurement according to claim 1 is characterized in that: The step 3 comprises the following steps: Step 3.1: Input the teaching joint trajectory Determine the model parameter α z , β z , α x and the number of radial basis functions N, is the joint angle taught at time t, is the angular velocity of the teaching joint at time t, is the angular acceleration of the teaching joint at time t, and P is the total step length of the teaching joint trajectory; Step 3.2: Train the weight vector ω using local weighted regression θ，i ; Step 3.3: Reproduce and generalize the motion trajectory according to the desired configuration of the underwater manipulator 5. The trajectory optimization method of an arm-carrying unmanned underwater vehicle based on speed operability measurement according to claim 1 is characterized in that: The step 4.2 is specifically to design the optimized expected joint velocity vector based on the kinematic equation The specific expression is as follows: in, is the weighted pseudo-inverse Jacobian matrix of the arm-carrying unmanned underwater vehicle, k is the proportional coefficient between the gradient vector magnitude and the system redundant solution, I is the unit matrix, gt(θ) is the zero space cost function, is the end effector pose, J E (θ) is the Jacobian matrix of the end effector.

6. The trajectory optimization method of an arm-carrying unmanned underwater vehicle based on speed operability measurement according to claim 5 is characterized in that: Specifically, step 4.3 uses the Riemann distance d in the Riemann space. Υ Measures the profile error between the actual velocity maneuverability ellipsoid and the desired velocity maneuverability ellipsoid.

7. The trajectory optimization method of an arm-carrying unmanned underwater vehicle based on speed operability measurement according to claim 6 is characterized in that: The Riemann distance d in the Riemann space Υ The specific expression is shown as follows: in, is a Riemann space composed of t (θ) Direction A vector of , F is the Frobenius norm used to measure the length of the vector.

8. The trajectory optimization method of an arm-carrying unmanned underwater vehicle based on speed operability measurement according to claim 6 is characterized in that: The expected joint velocity vector after optimization Update underwater robot arm joint trajectory Further calculations to obtain the optimized velocity maneuverability ellipsoid Using the Riemann distance d Υ The error contour between it and the desired velocity maneuverability ellipsoid is measured.

Images