A trajectory planning method for an autonomous underwater vehicle-rope driven manipulator

Abstract

This invention discloses a trajectory planning method for an autonomous underwater vehicle—a rope-driven manipulator. The invention relates to the field of manipulator trajectory planning technology and includes the following steps: Based on the manipulator's geometry, a rigid-body dynamics framework is constructed using the Lagrange method, and generalized fluid forces are calculated using fluid dynamics simulation to form a rigid-fluid coupling model; a flexible motion model of the rope is established using Cosserat rod theory and coupled with the rigid-fluid coupling model to construct a rigid-flexible-fluid coupling dynamic model; based on the model, the maximum workspace of the manipulator is determined, and a virtual workspace is defined as the constraint boundary for trajectory planning; a smooth basic trajectory is generated through quintic spline interpolation, the force conditions at different path points are analyzed, and a basic desired trajectory that meets the conditions is selected, and characteristic curve parameters are extracted; finally, the optimal trajectory is selected using an optimization function to ensure smooth and shock-free system motion.

Description

Technical Field

[0001] This invention relates to the field of robotic manipulator trajectory planning technology, specifically to a method for planning the trajectory of an autonomous underwater vehicle-rope driven robotic manipulator. Background Technology

[0002] Underwater operations face extreme environments such as high pressure, turbid visibility, and ocean current disturbances, which limit the operational capabilities of divers. Underwater vehicle-manipulator systems (UVMS) have become key tools for marine engineering, equipment maintenance, and resource extraction, and are crucial to my country's marine economic development and marine engineering equipment construction. However, UVMS still lags behind international advanced levels in technical indicators such as payload-to-weight ratio and endurance. Its development is limited by core theories and methods such as multibody coupled dynamics, high-redundancy degree-of-freedom trajectory planning, and control strategies under complex disturbances.

[0003] To address the challenges posed by multi-layered technical issues in robotic arm systems, a rope-driven robotic arm is now being used to replace the traditional direct-drive robotic arm. Rope-driven technology offers advantages such as low moment of inertia, high load-to-weight ratio, and good human-machine interaction safety, making it more suitable for the lightweight and low-disturbance requirements of AUV platforms.

[0004] In existing technologies, the dynamics of rope-driven systems exhibit complex fluid-structure interaction characteristics. Current research on the fluid dynamics and fluid-structure interaction mechanisms of rope-driven manipulators is insufficient. Traditional manipulator trajectory planning methods are often based on rigid body dynamics models, failing to effectively consider the impact of the underwater fluid environment on the manipulator. The motion of underwater fluids and the forces they exert on the manipulator are complex and variable, resulting in the inability to fully reflect the dynamic characteristics of the actual working environment when planning the trajectory. This can easily lead to insufficient accuracy in trajectory planning, thereby affecting the working efficiency and operational safety of the manipulator.

[0005] Therefore, how to effectively couple fluid dynamics with the dynamic model of the manipulator to obtain more accurate force conditions and ensure continuous and smooth trajectory, thereby ensuring stable and shock-free system motion, has become a technical problem that urgently needs to be solved.

[0006] The information disclosed in the background section is only intended to enhance the understanding of the background of this disclosure, and therefore may include information that does not constitute prior art known to those skilled in the art. Summary of the Invention

[0007] The purpose of this invention is to provide a trajectory planning method for an autonomous underwater vehicle-rope driven manipulator to solve the problems mentioned in the background art.

[0008] To achieve the above objectives, the present invention provides the following technical solution:

[0009] A trajectory planning method for an autonomous underwater vehicle-rope-driven manipulator, comprising the following steps:

[0010] Based on the geometric structure of the cable-driven manipulator, the Lagrange method is applied to construct the rigid body dynamics framework of the system. Fluid dynamics simulation technology is introduced to calculate the generalized fluid force of seawater acting on each link joint of the cable-driven manipulator, and integrate it as an external load into the rigid body dynamics framework to form a rigid-fluid coupling model.

[0011] The Cosserat rod theory is used to describe the motion characteristics of the rope of the rope-driven manipulator underwater. A flexible rope model is constructed and coupled with the rigid-fluid coupling model to establish a rigid-flexible-fluid coupling dynamic model.

[0012] Based on the rigid-flexible-fluid coupling dynamics model, several joint configurations are determined by random sampling to determine the maximum workspace of the rope-driven manipulator. The virtual workspace of the rope-driven manipulator is defined based on the maximum workspace as the constraint boundary for trajectory planning.

[0013] Based on the action task, task path points are set in the virtual workspace. A quintic spline interpolation algorithm is used to generate several smooth basic trajectories. The force situation of the rope-driven manipulator at different task path points is analyzed through a rigid-flexible-fluid coupling dynamic model. Force screening conditions are set to select several basic expected trajectories. The time-series change curves of position, velocity and acceleration during the operation of each basic expected trajectory are plotted and recorded as characteristic curves.

[0014] Extract the shape feature parameters of each basic expected trajectory feature curve, set an optimization function based on the shape feature parameters, and determine the basic expected trajectory with the largest optimization function value as the optimal rope-driven manipulator motion trajectory.

[0015] Furthermore, the geometry of the rope-driven manipulator refers to the relative position and shape characteristics of each link joint of the rope-driven manipulator, and a coordinate system is established to determine the generalized coordinates of each link joint;

[0016] Based on the generalized coordinates of each link joint, the rigid body dynamics framework of the system is constructed using the Lagrange method. The rigid body dynamics framework is specifically represented as follows:

[0017] ;

[0018] In the formula, Let be the time variable during the movement of the cable-driven robotic arm. Let be the generalized velocity of the u-th link joint. Let be the generalized coordinates of the u-th link joint. This is a Lagrangian quantity, specifically the difference between the sum of the kinetic energies of all links and the sum of their potential energies. Let be the generalized force vector of the u-th link joint, specifically the vector sum of the mechanical driving force, generalized fluid force, and generalized rope tension acting on each link, where u is the index of the link joint;

[0019] The sum of the kinetic energies of each link is specifically solved by combining the mass matrix of each link with the generalized velocity. The sum of the potential energies of each link is specifically the sum of the gravitational potential energy of each link and the elastic potential energy of the rope driven manipulator.

[0020] Furthermore, the logic for calculating the generalized fluid force of seawater acting on each link is as follows: based on the seawater flow velocity at different positions of the cable-driven manipulator, the distributed fluid force at different positions of the cable-driven manipulator is solved by the Navier-Stokes equations, and the solved distributed fluid force is mapped onto the generalized coordinates of each link joint to obtain the generalized fluid force of seawater acting on each link joint.

[0021] The specific formula used to solve the distributed fluid dynamics through the Navier-Stokes equations is as follows:

[0022] ;

[0023] In the formula, To distribute fluid forces, The density of seawater, For the seawater velocity field, For gradient operators, The dynamic viscosity of seawater, The pressure field of seawater;

[0024] The specific method for equivalently mapping the distributed fluid force to the generalized coordinates of each link joint is as follows: using the principle of virtual work, the distributed fluid force... The contribution is equivalent to that of generalized fluid forces. The specific formula used to calculate generalized fluid forces is as follows:

[0025] ;

[0026] In the formula, Let u be the generalized fluid force of the u-th link joint. Let be the distributed fluid force matrix of the infinitesimal element on the upper surface of the u-th link joint surface. Let be the geometric Jacobian matrix of the u-th link joint. The u-th link joint surface represents the first... A surface micro-element, Let be the index of the surface element on the u-th link joint surface.

[0027] Furthermore, the logic underlying the construction of the flexible rope model is as follows: the rope is discretized into multiple rope units, and the state of each rope unit is described by a position vector. The specific formula underlying the flexible rope model is:

[0028] ;

[0029] In the formula, Let be the internal force vector of the s-th rope element. The externally distributed force is specifically a combined force vector comprising fluid pressure and surface friction. The length of the rope unit. For rope density, Let the cross-sectional area of ​​the rope be . Let be the position vector of the s-th rope unit, where s is the index of the rope unit;

[0030] The finite difference method is used to numerically solve the equations of the flexible rope model. After convergence, based on the position vectors of each rope element, the internal force vectors of the rope elements falling into the generalized coordinate range of each link joint at different times during the rope-driven manipulator's motion are extracted. Based on the internal force vectors of the rope elements, the rope force vectors acting on each link joint are calculated. The specific formula used is as follows:

[0031] ;

[0032] In the formula, Let the generalized coordinates of the u-th link joint at time t be the coordinates of the u-th link joint. The force vector of each rope element. Let the generalized coordinates of the u-th link joint at time t be the coordinates of the u-th link joint. The rotation matrix of the local coordinates of a rope element section relative to the generalized coordinates. Let the generalized coordinates of the u-th link joint at time t be the coordinates of the u-th link joint. The internal force vector of each rope element. This is the index of the rope element within the generalized coordinate range of the u-th link joint;

[0033] Based on the generalized coordinate range of the u-th link joint at time t, the... The rope force vector of each rope element The generalized cable tension at the u-th link joint is calculated using the following formula:

[0034] ;

[0035] In the formula, Let be the generalized generalized rope tension at time t, Let t be the total number of rope elements within the generalized coordinate range of the u-th link joint at time t;

[0036] The specific expression for establishing the rigid-flexible-fluid coupled dynamic model is as follows:

[0037] ;

[0038] In the formula, Let be the mechanical driving force at the u-th link joint.

[0039] Furthermore, based on the motion range constraints of each link joint of the manipulator, a uniform sampling distribution is established. Specifically, the number of link joints in the rope-driven manipulator is defined as U, and the angular range of motion of the u-th link joint is... The angular range of motion of each link joint is divided into several sub-intervals. For any link joint, an active angle sampling point is randomly selected in each sub-interval of its angular range. The active angle sampling points of each link joint are arranged and combined to determine multiple active angle combinations. The workspace of the rope-driven manipulator under each active angle combination is calculated. The virtual workspace of the rope-driven manipulator is determined based on the maximum workspace.

[0040] For any rope-driven manipulator action task, the starting point of the rope-driven manipulator action task is taken as the starting task path point, and the ending point of the action task is taken as the ending task path point. Several task path points are randomly selected multiple times in the virtual workspace between the starting task path point and the ending task path point. Based on the starting space path point, the ending space path point, and the task path points determined by one random selection, a path path point combination is formed. Based on the task path points randomly selected in different rounds, several path path point combinations are constructed. For any path path point combination, a smooth basic trajectory is generated by a fifth-order spline interpolation algorithm. The smooth basic trajectory is then filtered by force screening conditions to obtain the basic expected trajectory.

[0041] Furthermore, the force situation at the linkage joint is analyzed using a rigid-flexible-fluid coupled dynamic model, with the model serving as the motion background field. A simulation analysis time interval is set, within which the motion process of the cable-driven manipulator evolves. The position, velocity, and acceleration data of the cable-driven manipulator at different times within the simulation analysis time interval are analyzed and extracted. The specific force screening conditions are as follows:

[0042] ;

[0043] In the formula, Specifically, it refers to the per-unit force value of the u-th link joint;

[0044] For each combination of path points, several basic trajectories are generated using a quintic spline interpolation algorithm. The specific formula used is as follows:

[0045] ;

[0046] In the formula, , , , , and For undetermined coefficients, The basic trajectory is determined by using the continuity of position, velocity, and acceleration at different times within the simulation analysis time interval as the solution condition for the undetermined coefficients, where r is the time variable within the simulation analysis time interval.

[0047] Furthermore, the shape characteristic parameters of the basic expected trajectory characteristic curve specifically include the curve length of the position time-series change curve, and the curvature values ​​of the velocity and acceleration time-series change curves, wherein the curvature values ​​of the velocity and acceleration time-series change curves are specifically characterized by the root mean square curvature of the curves.

[0048] The optimization function is set based on shape feature parameters. Specifically, the optimization objective is to minimize the absolute values ​​of the curve length, velocity, and acceleration curvature values ​​of the position time-series variation curves. The optimization function is specifically set as follows:

[0049] ;

[0050] In the formula, To optimize the function value, Based on the curvature of the velocity curve of the expected trajectory, The curvature of the acceleration curve of the expected trajectory, The curve length of the time-series change curve of the expected trajectory position. and These are the weighting coefficients, where and and All are greater than 0;

[0051] The optimization function value of each basic expected trajectory is determined based on the optimization function expression, and the basic expected trajectory with the largest optimization function value is taken as the optimal rope-driven manipulator motion trajectory.

[0052] Compared with the prior art, the beneficial effects of the present invention are:

[0053] First, by constructing a rigid body dynamics framework based on the geometric structure of the rope-driven manipulator using the Lagrange method and introducing fluid dynamics simulation technology, the generalized fluid force of seawater on each link of the manipulator can be accurately calculated. This allows the manipulator to fully consider external fluid loads during trajectory planning, thereby significantly improving the accuracy and safety of trajectory planning.

[0054] Secondly, the Cosserat rod theory is used to describe the motion characteristics of the rope, a flexible rope model is constructed, and it is coupled with the rigid-fluid coupling model to form a rigid-flexible-fluid coupling dynamic model. This enables the rope-driven manipulator to more realistically reflect its dynamic characteristics in the underwater environment and to comprehensively analyze the force situation of the rope-driven manipulator under different working conditions.

[0055] Finally, based on the extraction of shape feature parameters and the setting of optimization functions for the characteristic curve of the basic expected trajectory, the changes in position velocity and acceleration ensure the continuity and smoothness of the expected trajectory, thus ensuring that the system moves smoothly and without impact. Attached Figure Description

[0056] Figure 1 This is a schematic diagram of the overall method flow of the present invention;

[0057] Figure 2 Fit a curve to the position curve length minus the optimized function value;

[0058] Figure 3 To optimize the function value-acceleration curve curvature color mapping. Detailed Implementation

[0059] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to specific embodiments.

[0060] It should be noted that, unless otherwise defined, the technical or scientific terms used in this invention should have the ordinary meaning understood by one of ordinary skill in the art to which this invention pertains. The terms "first," "second," and similar terms used in this invention do not indicate any order, quantity, or importance, but are merely used to distinguish different components. Terms such as "comprising" or "including" mean that the element or object preceding the word encompasses the elements or objects listed following the word and their equivalents, without excluding other elements or objects. Terms such as "connected" or "linked" are not limited to physical or mechanical connections, but can include electrical connections, whether direct or indirect. Terms such as "upper," "lower," "left," and "right" are used only to indicate relative positional relationships; when the absolute position of the described object changes, the relative positional relationship may also change accordingly.

[0061] Example:

[0062] Please see Figures 1-3 The present invention provides a technical solution:

[0063] A trajectory planning method for an autonomous underwater vehicle-rope-driven manipulator, comprising the following steps:

[0064] Step 1: Based on the geometry of the cable-driven manipulator, the rigid body dynamics framework of the system is constructed using the Lagrange method. Fluid dynamics simulation technology is introduced to calculate the generalized fluid force of seawater acting on each link joint of the cable-driven manipulator, and this force is integrated into the rigid body dynamics framework as an external load to form a rigid-fluid coupling model.

[0065] The geometry of the rope-driven manipulator refers to the relative position and shape characteristics of each link joint of the rope-driven manipulator, and a coordinate system is established to determine the generalized coordinates of each link joint.

[0066] The specific method for obtaining the geometry of the rope-driven manipulator is as follows: by measuring the size and shape characteristics of each component, including the length of the link, the position of the joint, and the rope fixing point, through the physical prototype or CAD model of the manipulator, or by scanning the manipulator with a 3D scanner to obtain its geometry and size data.

[0067] The specific method for establishing a coordinate system using the DH method is as follows: A global reference coordinate system is established at the base of the robot arm, typically starting from the fixed base. The relative positions of each link and joint are determined, and a local coordinate system is established for each link and joint. Depending on the type of robot arm joint, such as rotary or translating joints, the DH parameter method defines parameters for the relationship between each joint and link: Rotary joints: The joint rotation angle is the main variable; Translating joints: The linear translation amount of the joint is the main variable; Establishing local coordinate systems: Coordinate systems are defined for the starting and ending ends of each link, typically using the following rules: Axis: Defined along the rotational or translational axis of the joint; X-axis: ... The axis is perpendicular and points in the direction of connection between adjacent links. Axis: Determined using the right-hand coordinate system rule; For two adjacent links, the following four DH parameters are defined, including link length, link torsion angle, joint offset, and joint rotation angle. The homogeneous transformation matrix of the geometric relationship between each joint and its adjacent links is defined using the four DH parameters. The homogeneous transformation matrix of each joint is recursively derived sequentially from the base to the end effector to calculate the overall position and orientation of the manipulator; Through the above matrix recursion, the position and orientation of the end effector can be determined, linking the geometry of the manipulator with the joint variables, thereby determining the generalized coordinates of each link joint.

[0068] Based on the generalized coordinates of each link joint, the rigid body dynamics framework of the system is constructed using the Lagrange method. The rigid body dynamics framework is specifically represented as follows:

[0069] ;

[0070] In the formula, Let be the time variable during the movement of the cable-driven robotic arm. Let be the generalized velocity of the u-th link joint. Let be the generalized coordinates of the u-th link joint. This is a Lagrangian quantity, specifically the difference between the sum of the kinetic energies of all links and the sum of their potential energies. Let be the generalized force vector of the u-th link joint, specifically the vector sum of the mechanical driving force, generalized fluid force, and generalized rope tension acting on each link, where u is the index of the link joint;

[0071] The formula is based on the fundamental principles of Lagrange mechanics. Lagrange dynamics is based on the principle of least action, which assumes that the motion of a system follows a specific path, such that the integral of the Lagrange quantity reaches an extreme value. For multi-degree-of-freedom systems, the Lagrange equations can be used to describe the motion of the system.

[0072] To effectively describe the motion of complex systems, such as the motion of a cable-driven manipulator, generalized coordinates are used for modeling. Each degree of freedom of motion in the system can be represented by a coordinate, thereby simplifying the dynamic analysis.

[0073] By using Lagrange Taking partial derivatives, we obtain the dynamic behavior of the system with respect to generalized velocity and coordinates. The first term... The second term represents the conservation of momentum. This represents the external forces or constraints acting on the system, while the right side represents... This represents the actual external force applied to the system; this equation represents the balance between the system's motion state and the external force at a certain moment; it is used to analyze the dynamic response of the robot under different working conditions and optimize its performance.

[0074] The sum of the kinetic energies of each link is specifically solved by combining the mass matrix of each link with the generalized velocity. The sum of the potential energies of each link is specifically the sum of the gravitational potential energy of each link and the elastic potential energy of the rope driving the manipulator. The specific method includes: determining the mass of each link: first, the mass of each link needs to be known, which can be obtained through design specifications or experimental measurement. Then, the velocity of each link is determined. This velocity is calculated based on the joint movement and is usually related to the angle change or linear movement of the joint. The velocity of each link can be regarded as its generalized velocity, which is caused by the joint movement. Finally, the kinetic energy is calculated. The calculation of kinetic energy is based on the mass and velocity of the link. The kinetic energy of each link is proportional to its mass and the square of its velocity. Therefore, the kinetic energy of each link can be calculated based on its mass and velocity. The kinetic energies of all links are added together to obtain the total kinetic energy of the entire manipulator linkage. This total kinetic energy reflects the energy state of the manipulator during the movement process.

[0075] The logic behind calculating the generalized fluid force of seawater acting on each link is as follows: based on the seawater flow velocity at different positions of the cable-driven manipulator, the distributed fluid force at different positions of the cable-driven manipulator is solved using the Navier-Stokes equations, and the solved distributed fluid force is mapped onto the generalized coordinates of each link joint to obtain the generalized fluid force of seawater acting on each link joint.

[0076] The specific formula used to solve the distributed fluid dynamics through the Navier-Stokes equations is as follows:

[0077] ;

[0078] In the formula, To distribute fluid forces, The density of seawater, For the seawater velocity field, For gradient operators, The dynamic viscosity of seawater, The pressure field of seawater;

[0079] It should be noted that the Navier-Stokes equations are fundamental equations in fluid mechanics, used to describe the motion and flow characteristics of fluids. These equations show that the acceleration of a fluid is affected by pressure, viscosity, and external forces. By solving these equations, the velocity distribution of the fluid at different positions in a rope-driven manipulator can be obtained, thereby deriving the distributed fluid forces acting on each link and joint.

[0080] The method for obtaining the parameters in the Navier-Stokes equations is as follows: Under standard conditions, the density of seawater is typically 1.025 kg / m³, but the actual value varies depending on temperature, salinity, and depth. Density values ​​under specific conditions can be obtained by consulting marine physics or chemistry literature, or through actual measurements.

[0081] Seawater velocity field can be measured using devices such as current meters and acoustic Doppler current meters (ADCPs). Measurements can be taken at different locations and depths to obtain the spatial distribution of seawater velocity.

[0082] Seawater pressure can be calculated from the height and density of the water column. In addition, pressure sensors can also be used to measure actual pressure.

[0083] The dynamic viscosity of seawater is generally between 0.001 and 0.0015. The specific values ​​depend on temperature and salinity, and can be obtained by consulting relevant literature or by measuring in a laboratory.

[0084] The specific method for equivalently mapping the distributed fluid force to the generalized coordinates of each link joint is as follows: using the principle of virtual work, the distributed fluid force... The contribution is equivalent to that of generalized fluid forces. The specific formula used to calculate generalized fluid forces is as follows:

[0085] ;

[0086] In the formula, Let u be the generalized fluid force of the u-th link joint. Let be the distributed fluid force matrix of the infinitesimal element on the upper surface of the u-th link joint surface. Let be the geometric Jacobian matrix of the u-th link joint. The u-th link joint surface represents the first... A surface micro-element, Let be the index of the surface element on the u-th link joint surface.

[0087] It should be noted that by solving the fluid force, the distributed fluid force can be mapped to the joints of each link. The specific mapping method is based on the principle of virtual work. The principle of virtual work specifically means that the total work or energy of the system can be obtained by summing the local work of each part. In this embodiment, the distributed fluid force acts on each surface micro-element, and the virtual work done can be equivalent to the contribution of the generalized fluid force.

[0088] The geometric Jacobian matrix of the u-th link joint Used to describe the changes of a joint in space, it can convert the forces acting on the actual surface into forces in the generalized coordinates of the joint. Since the forces acting on a fluid are distributed, while the motion of a joint is discrete, an efficient conversion can be made using the geometric Jacobian matrix.

[0089] Obtain the geometric Jacobian matrix of the u-th link joint. The process includes the following steps: Based on the generalized coordinates of the link joints, the type of joint is defined, including rotary joints or translational joints. For each link joint, its influence on other links is calculated, forming a Jacobian matrix. Common calculation steps include: If the first... The first joint is a rotational joint, and the column vectors of the Jacobian matrix are typically the directions of the joint axes. A joint is a moving joint, and the column vectors of the Jacobian matrix are typically vectors that are in the same direction as the linear movement of that joint.

[0090] Rope-driven manipulators operating underwater are subject to the influence of fluid forces. Understanding the distribution and magnitude of these forces is key to achieving stable control. By calculating generalized fluid forces, a more accurate dynamic model of the manipulator can be established.

[0091] Step 2: The Cosserat rod theory is used to describe the motion characteristics of the rope driving the manipulator underwater. A flexible rope model is constructed and coupled with the rigid-fluid coupling model to establish a rigid-flexible-fluid coupling dynamic model.

[0092] The logic behind constructing the flexible rope model is as follows: the rope is discretized into multiple rope units, and the state of each rope unit is described by a position vector. The specific formula underlying the flexible rope model is:

[0093] ;

[0094] In the formula, Let be the internal force vector of the s-th rope element. The externally distributed force is specifically a combined force vector comprising fluid pressure and surface friction. The length of the rope unit. For rope density, Let the cross-sectional area of ​​the rope be . Let be the position vector of the s-th rope element, used to describe the position of the element in space, where s is the index of the rope element;

[0095] It should be noted that this model is based on fundamental principles of continuum mechanics and elasticity. The core of Cosserat's rod theory lies in establishing equations describing the deformation and internal force equilibrium of the rod. This model describes the balance relationship between internal and external forces in the rod through equilibrium equations. The terms on the left side... This describes the variation of internal forces within a rope element with length, reflecting the changes in internal force distribution caused by external loads and the external distributed forces. These are key factors affecting rope motion. In an underwater environment, the pressure exerted by the fluid on the rope influences its shape and movement. The friction between the rope and the water flow also affects rope motion, especially when there is relative motion. (The item on the right...) The inertial force of the rope element reflects the acceleration of the element after being subjected to force. It is the second derivative of the position vector with respect to time, representing the acceleration of the rope element.

[0096] The specific method for discretizing a rope into multiple rope units is as follows: First, the rope's geometry and material properties need to be defined. Assuming the rope is a continuous elastic body, its geometry can be a straight line, a curve, or a complex shape. The overall shape of the rope is defined, and based on the actual application, the initial shape of the rope in three-dimensional space is determined. The rope length is then measured or calculated. Next, a discretization process is performed, dividing the continuous rope into multiple discrete units to facilitate analysis and calculation. The length of each rope unit is determined, typically choosing a value smaller than the overall rope length. This length should be appropriate for the rope's stiffness and hydrodynamic properties. The required number of units is calculated based on the total length and unit lengths. If the calculated number of units is not an integer, the unit lengths need to be adjusted. Finally, an index is assigned to each unit.

[0097] The position of each rope unit can be represented by a position vector. To describe, representing the spatial position of the unit, the position of each unit is calculated step by step based on the geometry of the rope and the discretization scheme. Specifically, for a straight rope, it can be simply calculated proportionally, and the specific formula used is as follows:

[0098] ;

[0099] In the formula, Let be the position vector of the rope's initial position. The unit direction vector of the rope;

[0100] The externally distributed force is a combined force vector that includes fluid pressure and surface friction. In practical applications, the fluid may be dynamic, so it is necessary to consider the kinetic and potential energy of the fluid in order to calculate the total pressure acting on the rope more accurately. Specifically, in a flowing underwater environment, the fluid pressure can also be estimated using Bernoulli's equation.

[0101] The specific value of the coexisting friction force is the force between the rope and the fluid. The specific friction coefficient between the rope material and the fluid can be obtained through experiments. Based on the friction coefficient between the rope material and the fluid, combined with the force exerted by the fluid on the rope surface area, the coexisting friction force is determined through the friction calculation formula.

[0102] The finite difference method is used to numerically solve the equations of the flexible rope model. After convergence, based on the position vectors of each rope element, the internal force vectors of the rope elements falling within the generalized coordinate range of each link joint at different times during the movement of the rope-driven manipulator are extracted. The specific steps include: discretizing the equations, using the finite difference method to process the spatial and temporal derivatives, and performing iterative solutions. The specific steps of the iterative solutions include: setting initial positions and internal forces for all rope elements, which can usually be set to zero; setting boundary conditions, such as fixed ends, free ends, or conditions for applying external forces, according to the actual problem; and iterating through a loop structure to gradually update the internal force and position vectors at each time step until the convergence condition is met. The convergence condition can be set to the internal force change being less than a given change threshold, which can be set based on expert experience.

[0103] The rope force vectors acting on each link joint are calculated based on the internal force vectors of the rope element. The specific formula used is as follows:

[0104] ;

[0105] In the formula, Let the generalized coordinates of the u-th link joint at time t be the coordinates of the u-th link joint. The force vector of each rope element. Let the generalized coordinates of the u-th link joint at time t be the coordinates of the u-th link joint. The rotation matrix of the local coordinates of a rope element section relative to the generalized coordinates describes the orientation and orientation changes of the rope element in the joint coordinate system. Let the generalized coordinates of the u-th link joint at time t be the coordinates of the u-th link joint. The internal force vector of each rope element. This is the index of the rope element within the generalized coordinate range of the u-th link joint;

[0106] It should be noted that in the formula This describes the force exerted by the rope element on the link joint at a specific moment. This force is the result of the rope's internal forces being transformed by a rotation matrix, reflecting the force exerted by the rope on the joint in a given posture; the rotation matrix... Transforming the internal force vector from the local coordinate system of the rope element to the generalized coordinate system of the joint is a good practice, since the internal force itself is defined in the local coordinate system of the rope element, while the dynamic analysis of the joint is usually performed in the global or generalized coordinate system. This transformation can accurately transfer the influence of the local internal force of the rope to the joint.

[0107] Internal force vector This represents the stress or tension distribution in the rope element. By combining it with the rotation matrix, the effective force of the rope element at the joint can be obtained.

[0108] Rotation matrix The construction of the system includes the following steps: The rotation matrix transforms the local coordinate system of a rope element into the generalized coordinate system of the link joint, thereby accurately describing the forces and effects of the rope element on the joint. A rope is a flexible structure, and different elements may have different orientations in space. Therefore, the local coordinate system of the rope element needs to be defined through the following steps. The direction of the rope is its primary direction of tension, typically along the central axis of the rope element. This direction can be determined by analyzing the geometric positions of the rope at two points in space. For example, the connection direction between two nodes can be observed, which is the main direction of the rope. In the local coordinate system of the rope element, the direction of the cross section also needs to be identified. To ensure that the local coordinate system is a three-dimensional coordinate system in space, one direction can be chosen that is perpendicular to the main direction, and a third direction can be chosen that is perpendicular to the two directions mentioned above, forming a right-handed coordinate system. The third direction can be accurately determined by the main direction and the cross section direction. The main tension direction of the rope is denoted as the tangent vector of the rope element. According to the geometric characteristics of the rope, a unit vector perpendicular to the tangent vector of the rope element is selected and denoted as the rope element normal vector. The subnormal vector is constructed based on the rope element normal vector and the tangent vector, specifically obtained by the cross product of the rope element normal vector and the tangent vector. The above rope element normal vector, tangent vector and subnormal vector are combined to form a rotation matrix.

[0109] Based on the generalized coordinate range of the u-th link joint at time t, the... The rope force vector of each rope element The generalized cable tension at the u-th link joint is calculated using the following formula:

[0110] ;

[0111] In the formula, Let be the generalized generalized rope tension at time t, Let t be the total number of rope elements within the generalized coordinate range of the u-th link joint at time t;

[0112] It should be noted that, This represents the first time at a specific moment. The generalized rope tension at each link joint integrates the first... The forces exerted by all rope units at each link joint on the link joint reflect the overall influence of the rope at the joint.

[0113] The specific expression for establishing the rigid-flexible-fluid coupled dynamic model is as follows:

[0114] ;

[0115] In the formula, Let be the mechanical driving force at the u-th link joint.

[0116] Step 3: Based on the rigid-flexible-fluid coupling dynamic model, determine several joint configurations through random sampling, determine the maximum workspace of the rope-driven manipulator, and define the virtual workspace of the rope-driven manipulator based on the maximum workspace as the constraint boundary for trajectory planning.

[0117] Based on the motion range constraints of each link joint of the manipulator, a uniform sampling distribution is established. Specifically, the number of link joints in the rope-driven manipulator is defined as U, and the angular range of motion of the u-th link joint is... The angular range of motion of each link joint is divided into several sub-intervals. For any link joint, an active angle sampling point is randomly selected in each sub-interval of its angular range. The active angle sampling points of each link joint are arranged and combined to determine multiple active angle combinations. The workspace of the rope-driven manipulator under each active angle combination is calculated. The virtual workspace of the rope-driven manipulator is determined based on the maximum workspace.

[0118] Randomly selecting a sampling point at an active angle allows for a thorough exploration of the joint configuration space, avoiding insufficient or incomplete sample coverage that might result from over-reliance on uniform distribution. Multiple random samplings provide a more comprehensive coverage of the entire joint's range of motion, ensuring no possible configurations are missed. This method is suitable for manipulators with complex ranges of motion, especially cable-driven manipulators. It can flexibly handle the constraint conditions of different joints, exhibiting strong adaptability. Multiple independent random samplings can be accelerated using a parallel computing framework. This significantly improves the efficiency of workspace estimation in practical applications.

[0119] The specific method for determining the angular range of motion of each link joint is as follows: Each joint of the robot is determined by the actual mechanical structure. Therefore, the angular range of motion of the joint is first limited by the mechanical design parameters. For rope-driven robots, the stretching or contraction of the rope is limited by the material properties and the actuator. Therefore, the angular range of motion of each link joint is determined by the specific mechanical design constraints.

[0120] Step 4: Based on the action task, set task path points in the virtual workspace, use the quintic spline interpolation algorithm to generate several smooth basic trajectories, analyze the force situation of the rope-driven manipulator at different task path points through the rigid-flexible-fluid coupling dynamic model, set force screening conditions to select several basic expected trajectories, and draw the time-series change curves of position, velocity and acceleration of each basic expected trajectory during the operation process, which are recorded as characteristic curves.

[0121] For any rope-driven manipulator action task, the starting point of the rope-driven manipulator action task is taken as the starting task path point, and the ending point of the action task is taken as the ending task path point. Several task path points are randomly selected multiple times in the virtual workspace between the starting task path point and the ending task path point. Based on the starting space path point, the ending space path point, and the task path points determined by one random selection, a path path point combination is formed. Based on the task path points randomly selected in different rounds, several path path point combinations are constructed. For any path path point combination, a smooth basic trajectory is generated by a fifth-order spline interpolation algorithm. The smooth basic trajectory is then filtered by force screening conditions to obtain the basic expected trajectory.

[0122] The force situation at the linkage joint is analyzed using a rigid-flexible-fluid coupled dynamic model, with the model serving as the motion background field. A simulation analysis time interval is set, within which the motion process of the rope-driven manipulator evolves. The position, velocity, and acceleration data of the rope-driven manipulator at different moments within the simulation analysis time interval are analyzed and extracted. Specific force screening conditions are set as follows:

[0123] ;

[0124] In the formula, Specifically, it refers to the per-unit force value of the u-th link joint; this is obtained through mechanical settings.

[0125] For each combination of path points, several basic trajectories are generated using a quintic spline interpolation algorithm. The specific formula used is as follows:

[0126] ;

[0127] In the formula, , , , , and For undetermined coefficients, The basic trajectory is used, where the position, velocity, and acceleration are continuous at different times within the simulation analysis time interval as the solution condition for the undetermined coefficients, to determine each undetermined coefficient. r is the time variable within the simulation analysis time interval. The specific steps for determining each undetermined coefficient include: determining the boundary conditions, which will be used to calculate the undetermined coefficients in the polynomial. The boundary conditions include:

[0128] Starting position: The initial position of the robotic arm at the start time.

[0129] End position: The target position of the robotic arm at the end time point;

[0130] Initial velocity: The initial velocity of the robotic arm at the beginning of the time.

[0131] End velocity: The target motion velocity of the robotic arm at the end time point;

[0132] Initial acceleration: The initial acceleration of the robotic arm at the beginning of the time period;

[0133] Terminal acceleration: The target acceleration of the robotic arm at the end of the time period;

[0134] Using these boundary conditions, the derivatives of the polynomial are constructed. Specifically, the first derivative of this polynomial, representing the change in velocity, and the second derivative, representing the change in acceleration, need to be calculated. These derivatives are used to represent the relationship between position, velocity, and acceleration at different time points. Substituting the boundary conditions one by one into the constructed polynomial and its derivatives, each condition corresponds to an equation describing the value of the polynomial, velocity, or acceleration at a specific time point. This will generate six equations, corresponding to the six coefficients to be determined: the position equation, representing the initial and final positions; the velocity equation, representing the initial and final velocities; and the acceleration equation, representing the initial and final accelerations.

[0135] Organize all the equations together to form a system of six linear equations. Solve this system using appropriate linear algebra methods to determine the six unknown coefficients. Common methods include Gaussian elimination or using computer software such as NumPy to perform the solution process.

[0136] Step 5: Extract the shape feature parameters of each basic expected trajectory feature curve, set the optimization function based on the shape feature parameters, and determine the basic expected trajectory with the largest optimization function value as the optimal rope-driven manipulator motion trajectory.

[0137] The shape characteristic parameters of the basic expected trajectory characteristic curve specifically include the curve length of the position time-series change curve, and the curvature values ​​of the velocity and acceleration time-series change curves. The curvature values ​​of the velocity and acceleration time-series change curves are specifically characterized by the root mean square curvature of the curves.

[0138] The optimization function is set based on shape feature parameters. Specifically, the optimization objective is to minimize the absolute values ​​of the curve length, velocity, and acceleration curvature values ​​of the position time-series variation curves. The optimization function is specifically set as follows:

[0139] ;

[0140] In the formula, To optimize the function value, Based on the curvature of the velocity curve of the expected trajectory, The curvature of the acceleration curve of the expected trajectory, The curve length of the time-series change curve of the expected trajectory position. and These are the weighting coefficients, where and and All are greater than 0;

[0141] It should be noted that optimizing function values Used to evaluate the overall quality of a base desired trajectory, it considers the smoothness of velocity and acceleration as well as the path length, reflecting the smoothness and efficiency of the mechanical system during motion. A higher value indicates better performance in terms of velocity and acceleration smoothness, and also means a shorter path and more efficient motion; conversely, a lower value indicates better performance. A smaller value indicates that the changes in velocity and acceleration are more drastic, the path may be too long or not smooth enough, such a trajectory may lead to increased wear of mechanical system, unstable motion, and may even affect safety;

[0142] Where the velocity curvature An increase means a larger change in speed, which leads to an increase in the value in the denominator, thus making... The smaller the velocity curvature, the smoother the motion, thus optimizing the function value. The larger the value, the better the trajectory.

[0143] Acceleration curve curvature Similar to velocity curvature, an increase in acceleration curvature also leads to an increase in the denominator, making... Reduce. Smaller changes in acceleration mean smoother motion, optimizing the function value. The larger the value, the better the trajectory optimization effect;

[0144] trajectory length When the denominator increases, the value of the fraction also increases, thus making Reduce. Shorter trajectories generally represent more efficient motion, therefore, smaller trajectory lengths will encourage optimization functions to be optimized. The value increases.

[0145] The optimization function value of each basic expected trajectory is determined based on the optimization function expression, and the basic expected trajectory with the largest optimization function value is taken as the optimal rope-driven manipulator motion trajectory.

[0146] In the motion of a mechanical system, changes in velocity and acceleration directly affect the system's smoothness and safety. Large changes in velocity or acceleration can lead to wear, impact, and vibration of mechanical components. Therefore, optimizing for smooth velocity and acceleration is crucial, and this is achieved by setting... The weight is greater than or equal to This can ensure velocity curvature and acceleration curvature It plays a more important role in the optimization process, which means that during optimization, the curvature changes of velocity and acceleration should be minimized to achieve smoother motion.

[0147] The above formulas are all dimensionless calculations. The formulas are derived from software simulations based on a large amount of collected data to obtain the most recent real-world results. The preset parameters in the formulas are set by those skilled in the art according to the actual situation.

[0148] The above embodiments can be implemented, in whole or in part, by software, hardware, firmware, or any other combination thereof. When implemented in software, the above embodiments can be implemented, in whole or in part, as a computer program product. Those skilled in the art will recognize that the units and algorithm steps of the various examples described in conjunction with the embodiments disclosed herein can be implemented by electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are implemented in hardware or software depends on the specific application and design constraints of the technical solution.

[0149] The units described as separate components may or may not be physically separate. The components shown as units may or may not be physical units; they may be located in one place or distributed across multiple network units. Some or all of the units can be selected to achieve the purpose of this embodiment, depending on actual needs.

[0150] The above description is merely a specific embodiment of this application, but the scope of protection of this application is not limited thereto. Any changes or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in this application should be included within the scope of protection of this application.

Claims

1. A trajectory planning method for an autonomous underwater vehicle-rope-driven manipulator, characterized in that, The specific steps include: Based on the geometric structure of the cable-driven manipulator, the Lagrange method is applied to construct the rigid body dynamics framework of the system. Fluid dynamics simulation technology is introduced to calculate the generalized fluid force of seawater acting on each link joint of the cable-driven manipulator, and integrate it as an external load into the rigid body dynamics framework to form a rigid-fluid coupling model. The Cosserat rod theory is used to describe the motion characteristics of the rope of the rope-driven manipulator underwater. A flexible rope model is constructed and coupled with the rigid-fluid coupling model to establish a rigid-flexible-fluid coupling dynamic model. Based on the rigid-flexible-fluid coupling dynamics model, several joint configurations are determined by random sampling to determine the maximum workspace of the rope-driven manipulator. The virtual workspace of the rope-driven manipulator is defined based on the maximum workspace as the constraint boundary for trajectory planning. Based on the action task, task path points are set in the virtual workspace. A quintic spline interpolation algorithm is used to generate several smooth basic trajectories. The force situation of the rope-driven manipulator at different task path points is analyzed through a rigid-flexible-fluid coupling dynamic model. Force screening conditions are set to select several basic expected trajectories. The time-series change curves of position, velocity and acceleration during the operation of each basic expected trajectory are plotted and recorded as characteristic curves. Extract the shape feature parameters of each basic expected trajectory feature curve, set an optimization function based on the shape feature parameters, and determine the basic expected trajectory with the largest optimization function value as the optimal rope-driven manipulator motion trajectory.

2. The trajectory planning method for an autonomous underwater vehicle-rope-driven manipulator according to claim 1, characterized in that: The geometry of the rope-driven manipulator refers to the relative position and shape characteristics of each link joint of the rope-driven manipulator, and a coordinate system is established to determine the generalized coordinates of each link joint. Based on the generalized coordinates of each link joint, the rigid body dynamics framework of the system is constructed using the Lagrange method. The rigid body dynamics framework is specifically represented as follows: ； In the formula, Let be the time variable during the movement of the cable-driven robotic arm. Let be the generalized velocity of the u-th link joint. Let be the generalized coordinates of the u-th link joint. It is a Lagrangian quantity, specifically the difference between the sum of the kinetic energies of all links and the sum of their potential energies. Let be the generalized force vector of the u-th link joint, specifically the vector sum of the mechanical driving force, generalized fluid force, and generalized rope tension acting on each link, where u is the index of the link joint; The sum of the kinetic energies of each link is specifically solved by combining the mass matrix of each link with the generalized velocity. The sum of the potential energies of each link is specifically the sum of the gravitational potential energy of each link and the elastic potential energy of the rope driven manipulator.

3. The trajectory planning method for an autonomous underwater vehicle-rope-driven manipulator according to claim 2, characterized in that: The logic behind calculating the generalized fluid force of seawater acting on each link is as follows: based on the seawater flow velocity at different positions of the cable-driven manipulator, the distributed fluid force at different positions of the cable-driven manipulator is solved using the Navier-Stokes equations, and the solved distributed fluid force is mapped onto the generalized coordinates of each link joint to obtain the generalized fluid force of seawater acting on each link joint. The specific formula used to solve the distributed fluid dynamics through the Navier-Stokes equations is as follows: ； In the formula, To distribute fluid forces, The density of seawater, For the seawater velocity field, For gradient operators, The dynamic viscosity of seawater, The pressure field of seawater; The specific method for equivalently mapping the distributed fluid force to the generalized coordinates of each link joint is as follows: using the principle of virtual work, the distributed fluid force... The contribution is equivalent to that of generalized fluid forces. The specific formula used to calculate generalized fluid forces is as follows: ； In the formula, Let the generalized fluid force be the force at the u-th link joint. Let be the distributed fluid force matrix of the infinitesimal element on the upper surface of the u-th link joint surface. Let be the geometric Jacobian matrix of the u-th link joint. The u-th link joint surface represents the first... A surface micro-element, Let be the index of the surface element on the u-th link joint surface.

4. The trajectory planning method for an autonomous underwater vehicle-rope-driven manipulator according to claim 3, characterized in that: The logic behind constructing the flexible rope model is as follows: the rope is discretized into multiple rope units, and the state of each rope unit is described by a position vector. The specific formula underlying the flexible rope model is: ； In the formula, Let be the internal force vector of the s-th rope element. The externally distributed force is specifically a combined force vector comprising fluid pressure and surface friction. The length of the rope unit. For rope density, Let the cross-sectional area of ​​the rope be . Let be the position vector of the s-th rope unit, where s is the index of the rope unit; The finite difference method is used to numerically solve the equations of the flexible rope model. After convergence, based on the position vectors of each rope element, the internal force vectors of the rope elements falling into the generalized coordinate range of each link joint at different times during the rope-driven manipulator's motion are extracted. Based on the internal force vectors of the rope elements, the rope force vectors acting on each link joint are calculated. The specific formula used is as follows: ； In the formula, Let the generalized coordinates of the u-th link joint at time t be the coordinates of the u-th link joint. The force vector of each rope element. Let the generalized coordinates of the u-th link joint at time t be the coordinates of the u-th link joint. The rotation matrix of the local coordinates of a rope element section relative to the generalized coordinates. Let the generalized coordinates of the u-th link joint at time t be the coordinates of the u-th link joint. The internal force vector of each rope element. This is the index of the rope element within the generalized coordinate range of the u-th link joint; Based on the generalized coordinate range of the u-th link joint at time t, the... The force vector of the rope in each rope element The generalized cable tension at the u-th link joint is calculated using the following formula: ； In the formula, Let be the generalized generalized rope tension at time t, Let t be the total number of rope elements within the generalized coordinate range of the u-th link joint at time t; The specific expression for the rigid-flexible-fluid coupled dynamic model is as follows: ； In the formula, Let be the mechanical driving force at the u-th link joint.

5. The trajectory planning method for an autonomous underwater vehicle-rope-driven manipulator according to claim 4, characterized in that: Based on the motion range constraints of each link joint of the manipulator, a uniform sampling distribution is established. Specifically, the number of link joints in the rope-driven manipulator is defined as U, and the angular range of motion of the u-th link joint is... The angular range of motion of each link joint is divided into several sub-intervals. For any link joint, an active angle sampling point is randomly selected in each sub-interval of its angular range. The active angle sampling points of each link joint are arranged and combined to determine multiple active angle combinations. The workspace of the rope-driven manipulator under each active angle combination is calculated. The virtual workspace of the rope-driven manipulator is determined based on the maximum workspace. For any rope-driven manipulator action task, the starting point of the rope-driven manipulator action task is taken as the starting task path point, and the ending point of the action task is taken as the ending task path point. Several task path points are randomly selected multiple times in the virtual workspace between the starting task path point and the ending task path point. Based on the starting space path point, the ending space path point, and the task path points determined by one random selection, a path path point combination is formed. Based on the task path points randomly selected in different rounds, several path path point combinations are constructed. For any path path point combination, a smooth basic trajectory is generated by a fifth-order spline interpolation algorithm. The smooth basic trajectory is then filtered by force screening conditions to obtain the basic expected trajectory.

6. The trajectory planning method for an autonomous underwater vehicle-rope-driven manipulator according to claim 5, characterized in that: The force situation at the linkage joint is analyzed using a rigid-flexible-fluid coupled dynamic model, with the model serving as the motion background field. A simulation analysis time interval is set, within which the motion process of the rope-driven manipulator evolves. The position, velocity, and acceleration data of the rope-driven manipulator at different moments within the simulation analysis time interval are analyzed and extracted. Specific force screening conditions are set as follows: ； In the formula, Specifically, it refers to the per-unit force value of the u-th link joint; For each combination of path points, several basic trajectories are generated using a quintic spline interpolation algorithm. The specific formula used is as follows: ； In the formula, , , , , and For undetermined coefficients, The basic trajectory is determined by using the continuity of position, velocity, and acceleration at different times within the simulation analysis time interval as the solution condition for the undetermined coefficients, where r is the time variable within the simulation analysis time interval.

7. The trajectory planning method for an autonomous underwater vehicle-rope-driven manipulator according to claim 6, characterized in that: The shape characteristic parameters of the basic expected trajectory characteristic curve specifically include the curve length of the position time-series change curve, and the curvature values ​​of the velocity and acceleration time-series change curves. The curvature values ​​of the velocity and acceleration time-series change curves are specifically characterized by the root mean square curvature of the curves. The optimization function is set based on shape feature parameters. Specifically, the optimization objective is to minimize the absolute values ​​of the curve length, velocity, and acceleration curvature values ​​of the position time-series variation curves. The optimization function is specifically set as follows: ； In the formula, To optimize the function value, Based on the curvature of the velocity curve of the expected trajectory, The curvature of the acceleration curve of the expected trajectory, The curve length of the time-series change curve of the expected trajectory position. and These are the weighting coefficients, where and and All are greater than 0; The optimization function value of each basic expected trajectory is determined based on the optimization function expression, and the basic expected trajectory with the largest optimization function value is taken as the optimal rope-driven manipulator motion trajectory.

Images