A cooperative navigation motion planning method for underwater robots

Abstract

The present application relates to the field of underwater robot navigation control, especially to the navigation motion control of underwater robot equipped with large inertia mechanical arm. It includes the following steps: planning the required system speed of the system through the reference trajectory of the system; predicting the water resistance suffered by the underwater flying mechanical arm robot mechanical arm according to the system speed; constructing the dynamics model of the underwater flying mechanical arm robot carrier based on the water resistance; using the dynamics model to predict the motion planning of the system; taking the prediction result as the input of the system to control the underwater robot to perform collaborative navigation motion. Through the hydrodynamic model of the mechanical arm, the present application uses the model predictive control framework to plan the appropriate angle for the mechanical arm, so as to actively use the water resistance and torque of the mechanical arm for navigation. The present application actively uses the hydrodynamic navigation of the mechanical arm to increase a method of controlling the angular velocity for the system, improve the maneuverability of the system, and has important significance for underactuated carriers or carriers encountering thruster failure.

Description

Technical Field

[0001] This invention relates to the field of navigation control for underwater robots, and more particularly to the navigation motion control of underwater robots equipped with large-inertia robotic arms. Background Technology

[0002] The development and progress of human society are inseparable from the development and utilization of resources. The construction, maintenance, and upkeep of equipment and structures have created a large demand for underwater operations, leading to the widespread application of underwater robots. Underwater robots need to perform underwater inspection and intervention tasks simultaneously, making trajectory tracking one of their crucial capabilities for underwater missions. However, due to the complexity of the underwater environment, communication delays, and the dynamic coupling between robots, trajectory control of underwater robots is often a challenging task.

[0003] Underwater robots are motion-redundant systems, with their degrees of freedom often exceeding the mission requirements, resulting in an infinite number of motion solutions for the tracking problem. The traditional approach to solving redundancy problems is to assign a secondary task to coordinate the movement of the carrier and the manipulator, optimizing energy consumption or maximizing system stability while satisfying the primary task. During system operation, the damping effect of the manipulator increases, becoming a major factor affecting the system's tracking performance; however, this damping effect can be used to adjust the carrier's attitude. For underactuated carriers or underwater robots with faulty thrusters, actively utilizing the damping effect of the manipulator is a method to improve system maneuverability. Summary of the Invention

[0004] The purpose of this invention is to provide a motion planning method for underwater robots that navigates by actively utilizing system resistance. This method can provide underwater robots with an additional speed control mechanism and improve the system's maneuverability.

[0005] The technical solution adopted by the present invention to achieve the above objectives is as follows:

[0006] A method for cooperative navigation motion planning of an underwater robot includes the following steps:

[0007] 1) Plan the required system velocity using the system's reference trajectory;

[0008] 2) Predict the water resistance experienced by the underwater flying robotic arm based on the system speed;

[0009] 3) Construct a dynamic model of the underwater flying robotic arm carrier based on water resistance;

[0010] 4) Use dynamic models to predict the motion of the system;

[0011] 5) Use the prediction results as input to control the underwater robot to perform cooperative navigation.

[0012] Step 1) specifically refers to:

[0013] Define the system's reference trajectory as q vd The current position and attitude of the system is q. v The scalar function σ for trajectory tracking is:

[0014]

[0015] The time derivative of the scalar function for this task for:

[0016]

[0017] Among them, J x It is the relevant Jacobian matrix. It is the speed of the robot system carrier;

[0018] Plan the desired system speed based on the task scalar function.

[0019]

[0020] in, It is a six-degree-of-freedom vector, including the desired linear velocity ν1 and angular velocity ν2 of the robot system. It is the pseudo-inverse of the Jacobian matrix.

[0021] Step 2) specifically refers to:

[0022] Based on the empirical formula for water resistance of underwater robotic arms, the water resistance acting on a certain link i of robotic arm j is obtained. and water resistance torque They are respectively:

[0023]

[0024]

[0025] Where ρ is the density of water. It is the frontal area of ​​the robotic arm. It is the speed of the connecting rod, C D It is a surface-dependent damping coefficient. It is the length of the connecting rod, r i j It is the position vector from joint i-1 to joint i, j represents the robot arm code, and i represents the link code of the robot arm. For underwater flying robot arm, i = 1, 2, 3, 4, j = 1, 2;

[0026] By transforming the coordinates, the torques acting on all the robotic arms are transferred to the carrier coordinate system, thus obtaining the sum of the water resistance and resistance torques caused by the robotic arms acting on the carrier.

[0027]

[0028] in, It is the rotation matrix from the link coordinate system to the carrier coordinate system, r b,j It is the position vector pointing from the origin of the carrier coordinate system to the origin of the link coordinate system.

[0029] In step 3), the dynamic model of the underwater flying robotic arm carrier constructed by water resistance is as follows:

[0030]

[0031] Where, q v It is the location of the carrier. It is the acceleration of the carrier, M v It is the mass and inertia matrix. It is the Coriolis force and the centripetal matrix. It is the hydrodynamic and damping matrix, g v (q v τ is the vector of forces and moments generated by buoyancy and gravity. v It refers to the force and torque generated by the propulsion system that act on the carrier.

[0032] Step 4) specifically involves:

[0033] Select system state x1 = q v , Select system input u1 = q m , The spatial state differential equation of the system is obtained as follows:

[0034]

[0035] J e It is the relevant Jacobian matrix;

[0036] Select a set of reference trajectories x r With reference output u r The relationship between the two The expression is as follows:

[0037]

[0038] Will Linearize the path near the reference trajectory and discard higher-order terms to obtain:

[0039]

[0040] Define state error Input error The derivative of the state error with respect to time The linearized model is obtained:

[0041]

[0042] Where A and B are process variables,

[0043] By selecting a time interval scalar T, the linearized model is discretized to obtain...

[0044]

[0045] Where I is the identity matrix;

[0046] The discretized linear model is applied to the model predictive control framework to define the system's performance metrics. for:

[0047]

[0048] Where p is the rolling prediction interval, Q, R, and F are all related penalty matrices, and (k+i|k) is the variable at time K+i predicted at time K.

[0049] The present invention has the following beneficial effects and advantages:

[0050] 1. Compared with the traditional method of controlling the navigation trajectory of underwater robots using propellers and thrusters, the present invention provides a novel method for controlling the navigation trajectory, which increases the system's maneuverability.

[0051] 2. This invention is applicable to underwater robot systems carrying multiple robotic arms, with no requirement on the number of robotic arms, and can make full use of the water resistance encountered by the robotic arms.

[0052] 3. This invention utilizes a model predictive control framework to achieve rational planning of the joint positions of the robotic arm through rolling prediction, thereby achieving high-precision trajectory control. Attached Figure Description

[0053] Figure 1 Structural diagram of the underwater flying robotic arm involved in this invention;

[0054] Figure 2 A flowchart of the underwater flying robotic arm planning method of the present invention. Detailed Implementation

[0055] The present invention will now be described in further detail with reference to the accompanying drawings and embodiments.

[0056] The entire system consists of a flying robotic arm carrier, two five-degree-of-freedom robotic arms, and a navigation system. The navigation system is equipped with necessary navigation sensors, including an inertial measurement unit (IMU), an electronic compass, a Doppler velocimeter, and a depth gauge. The IMU measures the three-axis acceleration of the robotic carrier; the electronic compass measures the heading, pitch, and roll angles; the Doppler velocimeter measures the translational velocity; and the depth gauge measures the depth. The robotic arms are used to perform underwater operations, and each joint is equipped with an encoder. The encoder indicates the joint's actual position in real time and can also output information such as joint velocity and acceleration.

[0057] The structure of the flying robotic arm is as follows Figure 1 As shown.

[0058] like Figure 2 As shown, the entire system operates according to the following process:

[0059] The first step is to plan the linear and angular velocities required by the reference trajectory planning system.

[0060] Define the system's reference trajectory as q vd The current position and attitude of the system is q. v Then the scalar function for trajectory tracking can be defined as follows:

[0061]

[0062] The time derivative of the scalar function for this task for

[0063]

[0064] J x It is the relevant Jacobian matrix. This indicates the speed of the robot system's carrier.

[0065] Based on equation (2), the desired system speed can be planned according to the task scalar function.

[0066]

[0067] in It is a six-degree-of-freedom vector, including the desired linear velocity ν1 and angular velocity ν2 of the robot system. This represents the pseudo-inverse of the Jacobian matrix.

[0068] The second step is to predict the water resistance experienced by the underwater flying robotic arm.

[0069] Based on the empirical formula for water resistance of an underwater robotic arm, the water resistance acting on a certain link i of the robotic arm j can be obtained. and water resistance torque for

[0070]

[0071] Where ρ is the density of water, It is the frontal area of ​​the robotic arm. It is the speed of the connecting rod, C D It is a surface-dependent damping coefficient. It is the length of the connecting rod, r i j It is the position vector from joint i-1 to joint i, j represents the robot arm code, and i represents the link code of the robot arm. For underwater flying robot arm, i = 1, 2, 3, 4, j = 1, 2.

[0072] Coordinate transformation can transfer all the torques acting on the robotic arms to the carrier coordinate system. Therefore, the sum of the water resistance and drag torque acting on the carrier caused by the robotic arms can be obtained.

[0073]

[0074] in It is the rotation matrix from the link coordinate system to the carrier coordinate system, r b,j It is the position vector pointing from the origin of the carrier coordinate system to the origin of the link coordinate system. The resultant force term is the joint position q of the robotic arm. m With speed The function.

[0075] The third step is to obtain the dynamic model of the underwater flying robotic arm carrier.

[0076] The dynamic model of the underwater flying robotic arm can be expressed as follows:

[0077]

[0078] Where q v Represents the location of the carrier. M represents the acceleration of the carrier. v Represents the mass and inertia matrix, including the additional mass forces. Representing the Coriolis force and the centripetal matrix, including the additional mass force. Represents the hydrodynamic and damping matrix, g v (q v τ represents the force and torque vector generated by buoyancy and gravity. v τ represents the force and torque generated by the propeller acting on the carrier. d This represents the force and torque generated by the water resistance of the robotic arm.

[0079] The fourth step is to use model predictive control to perform motion planning through a dynamic model.

[0080] Select system state x1 = q v , Select system input u1 = q m , Then the spatial state differential equation of the system can be obtained as follows:

[0081]

[0082] J e It is the relevant Jacobian matrix.

[0083] Select a set of reference trajectories x r With reference output u r The reference trajectory was obtained experimentally and is a known quantity. The reference trajectory and the reference output satisfy equation (7), and their relationship can be expressed as follows:

[0084]

[0085] Linearizing equation (7) near the reference trajectory and discarding higher-order terms yields...

[0086]

[0087] Define the state error as Input error is The derivative of the state error with respect to time is: The linearized model can be obtained by subtracting (8) from (9).

[0088]

[0089] in

[0090] By selecting the time interval scalar T and discretizing equation (10), we can obtain the following equation.

[0091]

[0092] Where I is the identity matrix.

[0093] Equation (11) can then be applied to the model predictive control framework, defining the system's performance index as follows:

[0094]

[0095] Where p is the rolling prediction interval, Q, R, and F are the relevant penalty matrices, and the bracketed label (k+i|k) indicates the variable predicted at time K+i at time K.

[0096] The fifth step is to obtain the model's predicted output, which is the system input.

[0097] The output of the model predictive control framework can be obtained through step four. The actual input of the system can then be expressed as

[0098]

[0099] By definition, u contains the system's desired robotic arm position q. m With speed The information, combined with the motion planning results obtained in step one, constitutes the final result of the motion planning.

[0100] This invention mainly consists of three parts. First, it plans according to the desired trajectory to obtain the desired system linear velocity and angular velocity. Second, it uses model parameters to analyze the resistance experienced by the robotic arm link and the torque generated on the carrier, and derives the carrier model of the robot. Finally, it uses a model predictive control framework to perform motion planning and actively uses the resistance experienced by the robotic arm to control the state of the system.

Claims

1. A method for cooperative navigation motion planning of an underwater robot, characterized in that, Includes the following steps: 1) Plan the required system velocity using the system's reference trajectory; 2) Predict the water resistance experienced by the underwater flying robotic arm based on the system speed; 3) Construct a dynamic model of the underwater flying robotic arm carrier based on water resistance; 4) Use dynamic models to predict the motion of the system; 5) Use the prediction results as input to control the underwater robot to perform cooperative navigation. Step 4) specifically refers to: Select system status , Select system input , The spatial state differential equation of the system is obtained as follows: in It is the relevant Jacobian matrix; Select a set of reference trajectories With reference output The relationship between the two The expression is as follows: Will Linearize the path near the reference trajectory and discard higher-order terms to obtain: Define state error Input error Then the derivative of the state error with respect to time The linearized model is obtained as follows: in, A, B It is a process variable. , ; Selecting a time interval scalar Discretizing the linearized model yields in, It is the identity matrix; The discretized linear model is applied to the model predictive control framework to define the system's performance metrics. for: in, It is a rolling forecast interval. These are all related penalty matrices. Is The time-predicted The variable of time.

2. The method for cooperative navigation motion planning of an underwater robot according to claim 1, characterized in that, Step 1) specifically refers to: Define the system's reference trajectory as The current position and attitude of the system are Scalar function for trajectory tracking task for: The time derivative of the scalar function for this task for: in, It is the relevant Jacobian matrix. It is the speed of the robot system carrier; Plan the desired system speed based on the task scalar function. : in, It is a six-degree-of-freedom vector, including the desired linear velocity of the robot system. With angular velocity , It is the pseudo-inverse of the Jacobian matrix.

3. The method for cooperative navigation motion planning of an underwater robot according to claim 1, characterized in that, Step 2) specifically refers to: Based on the empirical formula for water resistance in underwater robotic arms, the force acting on the robotic arm is obtained. A certain link Water resistance and water resistance torque They are respectively: in, It is the density of water. It is the frontal area of ​​the robotic arm. It is the speed of the connecting rod. It is a surface-dependent damping coefficient. It is the length of the connecting rod. From the joint To the joint The position vector, Indicates the code name of the robotic arm. The link designation indicates the robotic arm's linkage; for underwater flying robotic arms, , ; By transforming the coordinates, the torques acting on all the robotic arms are transferred to the carrier coordinate system, thus obtaining the sum of the water resistance and resistance torques caused by the robotic arms acting on the carrier. in, It is the rotation matrix from the link coordinate system to the carrier coordinate system. It is the position vector pointing from the origin of the carrier coordinate system to the origin of the link coordinate system.

4. The method for cooperative navigation motion planning of an underwater robot according to claim 1, characterized in that, In step 3), the dynamic model of the underwater flying robotic arm carrier constructed by water resistance is as follows: in, It is the location of the carrier. It is the acceleration of the carrier. It is the mass and inertia matrix. It is the Coriolis force and the centripetal matrix. It is the hydrodynamic and damping matrix. It is a vector of forces and moments generated by buoyancy and gravity. It refers to the force and torque generated by the propulsion system that act on the carrier.

Images