Guiding path tracking control method of laser sounding unmanned ship

Abstract

The invention relates to the technical field of laser detection, and particularly discloses a guide path tracking control method for a laser depth sounding unmanned ship, which comprises the following steps of: calculating a guide rate based on an expected path in combination with the position of the unmanned ship, obtaining an expected course, obtaining course control parameters of the unmanned ship in combination with the current course, and controlling the navigation of the unmanned ship. According to the guiding path tracking control method for the laser sounding unmanned ship, the problems that when the unmanned ship turns, due to the fact that the convergence rate is low and overshoot is large, the advancing path of the unmanned ship and the planned path have large errors, laser sounding strips do not coincide, and air strip adjustment cannot be carried out are solved.

Description

Technical Field

[0001] The present invention relates to the technical field of laser detection, and particularly relates to a guidance path tracking control method for an unmanned boat for laser bathymetry. Background Art

[0002] The guidance of an unmanned boat is to calculate the guidance rate based on the desired path and the position of the unmanned boat to obtain the desired heading, and combine it with the current heading to obtain the heading control parameter of the unmanned boat to control the navigation of the unmanned boat. Currently, common guidance rate (LOS (Line of Sight) guidance rate, PP (pure pursuit) guidance rate, CB (Constant Bearing) guidance rate) calculation methods often have a large error between the traveling path of the unmanned boat and the planned path due to slow convergence rate and large overshoot when the unmanned boat turns, resulting in non - coincidence of the laser bathymetry strips and inability to perform strip adjustment.

[0003] For example, in 2010, Oh published a paper titled "Path following of underactuated marine surface vessels using line - of - sight based model predictive control" in the journal "Ocean Engineering", which studied the process of an interceptor achieving interception by moving along the line of sight between a reference point and a target through restrictions. Based on the three - degree - of - freedom dynamic model of the surface vessel of the controller, it formulated the use of quadratic programming (QP) to solve the linear parameters with continuous optimization of the line - of - sight (LOS) model to achieve model predictive control (MPC) and improve path - following performance. Computer simulations proved the effectiveness of the developed control program. However, this program cannot solve the problems of slow convergence rate and large overshoot.

[0004] Another example is that in 2022, Liu Yifan published a paper titled "Fuzzy path tracking control method based on pure tracking model" in the journal "Machine Design and Research". To improve the accuracy of the AGV autonomous navigation system, based on the pure tracking model and using a fuzzy controller to adaptively determine the preview distance in the pure tracking model, a fuzzy control method for AGV path tracking based on the pure tracking model was proposed, which improved the adaptability of the pure tracking control to the path and the accuracy of path tracking. However, this model still cannot solve the problems of slow convergence rate and large overshoot.

[0005] For another example, in 2007, Berivik published a paper titled "Applying Missile Guidance Concepts To Motion Control Of Marine Craft" in IFAC Proceedings Volumes (Volume 40, Issue 17, Pages 349 - 354). During the research on intercept missiles, the intercept missile should align the relative linear velocity between the intercept missile and the target along the line of sight between the intercept missile and the target, and reduce the LOS rotation rate to zero. Then the intercept missile can sense the target at a constant azimuth, approach directly, collide with, and destroy the target during the approaching process. However, the problems of slow convergence rate and large overshoot still cannot be solved. Summary of the Invention

[0006] The present invention provides a guidance path tracking control method for a laser sounding unmanned ship to solve the problem that due to the slow convergence rate and large overshoot during the turning of the laser sounding unmanned ship, there is a large error between the traveling path of the unmanned ship and the planned path, resulting in non - coincidence of the laser sounding strips and inability to perform strip adjustment.

[0007] To achieve the above object, the technical solution adopted by the present invention is: a guidance path tracking control method for a laser sounding unmanned ship, comprising the following steps:

[0008] S1. Construct a geodetic coordinate system and a ship - body coordinate system. The positive x - direction of the geodetic coordinate system is the due - north direction, the positive y - direction is the due - east direction, and the positive z - direction is perpendicular to the xoy plane and downward. The geodetic coordinate system uses the roll angle φ to represent the rotation angle of the unmanned ship around the x - axis, the pitch angle θ to represent the rotation angle around the y - axis, and the yaw angle ψ to represent the rotation angle around the z - axis. The positive x - n direction of the ship - body coordinate system is the forward direction of the ship, the positive y - n direction is on the right side of the forward direction, and the positive z - n points to the ground. The velocity in the x - n direction is represented by u, the velocity in the y - n direction is represented by v, the velocity in the z - n direction is represented by w. The angular velocity around the x - n axis is represented by p, the angular velocity around the y - n axis is represented by q, and the angular velocity around the z - n axis is represented by r;

[0009] S2. Construct the kinematic equation of the unmanned ship, and respectively represent the velocities u, v, w and angular velocities p, q, r in the ship - body coordinate system by the position derivatives and attitude - angle derivatives in the geodetic coordinate system to obtain the three - degree - of - freedom kinematic equation of the unmanned ship:

[0010]

[0011] Among them, p(x, y) is the position coordinate value of the unmanned ship;

[0012] S3. Establish a three-degree-of-freedom model of the unmanned ship:

[0013]

[0014] Among them, m is the mass of the ship, m 11 represents the added mass of the x n axis, m 22 represents the added mass of the y n axis, m 66 represents the added inertia moment of the z n axis, d 11 represents the hydrodynamic damping coefficient of the x n axis, d 22 represents the hydrodynamic damping coefficient of the y n axis, d 33 represents the hydrodynamic damping coefficient of the z n axis,

[0015] m 11 、m 22 and m 66 The calculation formulas are:

[0016]

[0017] In the formula, L represents the length of the ship, W represents the width of the ship, D is the draft of the ship, ρ is the density of water, d represents the distance between the propellers,

[0018] d 11 、d 22 and d 33 The calculation formulas are:

[0019]

[0020] In the formula, x s represents the state vector, u s represents the input vector, B s (u s ) represents the force or rotational moment generated by the input command, the vector φ(x s ) represents the term parameterized by the matrix A s in the system dynamics. Since the dynamics of the unmanned ship is stable, when the input vector is set to a constant value (u ss ), the unmanned ship starts to move; after the movement of the unmanned ship reaches a steady state, then Get

[0021] 0 = A s φ(x ss ) + B s (u ss ),

[0022] where x ss is the steady-state value of the state vector. Based on this, a mapping can be obtained that relates the input vector to the steady-state value of the state variable, and this mapping can be used to identify the damping coefficient of the unmanned ship;

[0023] S4. Calculate the path following guidance rate of the unmanned ship. Let P k-1 (x k-1 , y k-1 , P k (x k , y k ) and P k+1 (x k+1 , y k+1 ) be three waypoints of the unmanned ship, where the straight line P k-1 P k is the current desired path, ψ k is the tangential angle of the horizontal path, θ los is the LOS error angle, and the desired heading of the unmanned ship is Ψ d , and its calculation formula is:

[0024] Ψ d = Ψ k - θ los ,

[0025] where the tangential angle ψ k of the horizontal path has the following calculation formula:

[0026] Ψ k = atan2(y k - y k-1 , x k - x k-1 ),

[0027] In the formula, the function atan2 can determine the quadrant where the angle is located, and the range of ψ k is [-π, π],

[0028] The heading error angle θ los has the following calculation formula:

[0029]

[0030] In the formula, Δ is the look-ahead distance, and d e is the lateral deviation of the unmanned ship from the desired path, and its calculation formula is:

[0031] d e = -(x - xk -1)sinΨ k +(-y k-1 )cosΨ k ; S5: Set the tracking circle of the unmanned ship. Under the control of the guidance rate, make the unmanned ship converge to the desired path P k-1 P k and move along P k-1 P k Set a circle with the desired waypoint P k as the center. This area is called the "switching circle". When the unmanned ship sails into the "switching circle" area, the unmanned ship converts the desired waypoint and path according to the formula: Convert the desired waypoint and path, where P(x, y) is the current position of the unmanned ship and R is the radius of the "switching circle";

[0032] S6. Optimize the control parameters through LQR-PD, specifically including:

[0033] S61. Establish a control model:

[0034]

[0035] where m′ 33 = I z + m 66 , N p is the steering torque output by the unmanned ship;

[0036] S62. Install the two thrusters of the unmanned ship behind the two pontoons respectively, and decouple the control of the forward movement and turning of the unmanned ship to obtain the final control model:

[0037]

[0038] where Δn is the control command variable;

[0039] S63. Optimize the control parameters. Let the state vector x v = [r, Ψ e T , matrix matrix Then the above final control model can be changed to:

[0040]

[0041] According to the linear quadratic regulator principle, set the cost function J as:

[0042]

[0043] where Q and R are weight matrices,

[0044] ​To solve the function J, the state feedback of the system is set as:

[0045] Δn = -Kx v ,

[0046] where K = [k 1 , k 2 is the state feedback vector. By solving for K, we get:

[0047] Δn = -Kx v = -k 1 r - k 2 Ψ e ,

[0048] Adding the differential term of the course error to the above equation, we get:

[0049]

[0050] where K D is the differential term gain.

[0051] Preferably, the unmanned ship control system adopted by the control method includes a computer and ground station software, a remote controller, a data transmission module, an unmanned ship, a battery, an electronic speed controller, a positioning module, a control board, a data transmission module, a remote control receiver, and an attitude detection module.

[0052] The beneficial effects are as follows: Compared with the prior art, the beneficial effects of a guiding path tracking control method for a laser sounding unmanned ship of the present invention are as follows:

[0053] (1) The conversion from the unmanned ship coordinate system to the geodetic coordinate system ensures that the coordinate system of the unmanned ship is consistent with the coordinate systems of the positioning module and the attitude detection module, reducing the computational amount brought by subsequent coordinate conversion and also reducing the error caused by coordinate conversion;

[0054] (2) Describing the path of the unmanned ship during the transportation state through the kinematic equation of the unmanned ship and comparing it with the waypoint data to depict the deviation between the movement trajectory of the unmanned ship and the waypoint;

[0055] (3) The dynamic equation of the unmanned ship can convert the parameters of the throttle or speed sent from the remote controller or the ground station into the speed of the unmanned ship advancing in different directions;

[0056] (4) The unmanned ship tracking path guidance rate controls the angle between the bow direction of the unmanned ship in the geodetic coordinate system and the waypoint trajectory direction to be 0, ensuring that the unmanned ship sails according to the predetermined waypoint;

[0057] (5) Setting an unmanned ship tracking circle ensures that the unmanned ship sails according to the waypoint trajectory when turning;

[0058] (6) The LQR-PD optimization control parameters are used to calculate the optimal navigation parameters of the unmanned ship according to the above model by the LQR-PD optimization method, so as to realize the unmanned ship sailing along the waypoint trajectory. BRIEF DESCRIPTION OF THE DRAWINGS

[0059] The following further details the specific embodiments of the present invention in conjunction with the accompanying drawings, where:

[0060] Figure 1 It is a schematic diagram of the path tracking control of the laser sounding unmanned ship of the present invention;

[0061] Figure 2 It is a schematic diagram of the coordinate system transformation of the laser sounding unmanned ship of the present invention;

[0062] Figure 3 It is a schematic diagram of the path tracking guidance rate calculation of the laser sounding unmanned ship of the present invention;

[0063] Figure 4 It is a schematic diagram of the path tracking switching circle of the laser sounding unmanned ship of the present invention;

[0064] Figure 5 It is a schematic diagram of the composition of the laser sounding unmanned ship system and the experimental location of the present invention;

[0065] Figure 6 It is a schematic diagram of the three test paths P1, P2 and P3 of the present invention;

[0066] Figure 7 It is a schematic diagram of the comparison results of the test path P1 of the present invention with the desired path and PID;

[0067] Figure 8 It is a schematic diagram of the comparison results of the test path P2 of the present invention with the desired path and PID;

[0068] Figure 9 It is a schematic diagram of the comparison results of the test path P3 of the present invention with the desired path and PID. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0069] Next, the technical solutions in the embodiments of the present invention will be clearly and completely described in conjunction with the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative efforts shall fall within the protection scope of the present invention.

[0070] It should be noted that when a component is referred to as "fixed to" another component, it can be directly on the other component or there may be an intermediate component. When a component is considered to be "connected to" another component, it can be directly connected to the other component or there may be an intermediate component at the same time. When a component is considered to be "disposed on" another component, it can be directly disposed on the other component or there may be an intermediate component at the same time. When a part is referred to as "disposed in the middle", it is not only disposed at the exact middle position, as long as it is not disposed at the two ends, it belongs to the range defined by the middle. The terms "vertical", "horizontal", "left", "right" and similar expressions used in this article are only for the purpose of illustration.

[0071] Unless otherwise defined, all technical and scientific terms used in this article have the same meaning as those commonly understood by those skilled in the technical field to which this invention belongs. The terms used in the description of the present invention in this article are only for the purpose of describing specific embodiments, and are not intended to limit the present invention. The term "and / or" used in this article includes any and all combinations of one or more of the related listed items.

[0072] The implementation principle of the laser sounding unmanned ship is as follows: First, a corresponding ground station needs to be supported. The operator needs to plan the expected path in advance in the corresponding software of the ground station and send the expected path to the controller of the unmanned ship; then the unmanned ship continuously obtains information such as positioning, attitude angle, heading, and speed through sensors, and automatically adjusts the rudder angle or the propeller speed according to the path tracking control method to make the ship move along the expected path. The key problem to be solved in this invention is the path tracking control method of the unmanned ship.

[0073] Therefore, this application discloses a guiding path tracking control method for a laser sounding unmanned ship, including the following steps:

[0074] S1: Construct a geodetic coordinate system and a hull coordinate system

[0075] Use the geodetic coordinate system to describe the position and attitude of the unmanned ship on the earth; the positive x direction of the geodetic coordinate system is the due north direction, the positive y direction is the due east direction, and according to the right-hand rule of the Cartesian coordinate system, the positive z direction is perpendicular to the xoy plane and downward. In addition, the geodetic coordinate system uses the roll angle φ to represent the rotation angle of the unmanned ship around the x-axis, the pitch angle θ to represent the rotation angle around the y-axis, and the yaw angle Ψ to represent the rotation angle around the z-axis. These three angles are also called Euler angles.

[0076] The hull coordinate system is a coordinate system fixed to the unmanned ship, and the origin is generally set at the center of gravity of the ship. It can be seen from this that the positive x n of the hull coordinate system is the forward direction of the ship, the positive y n is on the right side of the forward direction, and the positive z nThen it points to the ground (the hull coordinate system also needs to conform to the right-hand rule of the Cartesian coordinate system). In the hull coordinate system, the velocity in the x n direction is called the forward velocity, denoted by u; the velocity in the y n direction is called the lateral velocity, denoted by v; the velocity in the z n direction is called the heave velocity, denoted by w; the angular velocity of rotation about the x n axis is called the roll angular velocity, denoted by p; the angular velocity of rotation about the y n axis is called the pitch angular velocity, denoted by q; the angular velocity of rotation about the z n axis is called the yaw angular velocity, denoted by r. All symbols are shown in Table 1:

[0077] Table 1 Symbols related to the coordinate system

[0078]

[0079] S2: Construct the kinematic equation of the unmanned ship

[0080] For the motion of the unmanned ship, the velocities u, v, w and angular velocities p, q, r in the hull coordinate system need to be described by the position derivatives in the earth coordinate system and the derivatives of the attitude angles :

[0081]

[0082] Since the unmanned ship of the present invention has characteristics such as small size and low center of gravity and is usually used in relatively calm waters, the effects of roll, pitch and heave motions on the unmanned ship are ignored; at the same time, the unmanned ship only performs path tracking on the water surface and detects the water depth. Therefore, the position of the ship and the yaw angle Ψ (heading angle) are controlled. So let w = 0, p = 0, q = 0, φ = 0, θ = 0, and substitute them into formulas (1) and (2) to obtain the three-degree-of-freedom kinematic equation of the unmanned ship as:

[0083]

[0084] where p(x, y) is the position coordinate value of the unmanned ship.

[0085] S3: Establish the dynamic equation of the unmanned ship

[0086] The dynamic equation is described as:

[0087]

[0088] where m is the mass of the ship; (x g , y g , z g ) is the position of the center of gravity of the unmanned ship; ∑X, ∑Y and ∑Z respectively represent the unmanned ship in the hull coordinate system xn , y n , z n The external forces acting in the axial directions of x, y, and z; ∑K, ∑M, and ∑N respectively represent the moments acting on the unmanned ship about the x, n , y n , z n axes of the hull coordinate system; I x , I y and I z respectively represent the moments of inertia of the unmanned ship about the x, n , y n , z n axes of the hull coordinate system; I xy , I xz and I yz respectively represent the products of inertia of each axis of the hull coordinate system. The moments of inertia and products of inertia are all constants, and their calculation formulas are as follows:

[0089]

[0090] Since the unmanned ship adopts a catamaran hull type symmetric design, it is considered that the mass of the unmanned ship is symmetric about the x n z n plane; and the origin of the hull coordinate system is set at the center of gravity of the unmanned ship, so let I xy = I yz = 0, (x g , y g , z g ) = (0, 0, 0). In addition, the unmanned ship is an underactuated system, and its control inputs are only the bow control force and the yaw control moment, and it cannot control other dimensions. Therefore, the roll, pitch, and heave motions of the unmanned ship are not considered for the time being. Based on the above, Equation (4) can be simplified to a three-degree-of-freedom model:

[0091]

[0092] Let X R = Y R = N R = 0; in addition, the dynamic forces (X H , Y H , N H ) acting on the hull are obtained according to the principle of inertial-like dynamic action to get Equation (8):

[0093]

[0094] In the formula, the label I represents the inertial-like force, and P represents the propeller force.

[0095] The calculation formulas for the inertial-like dynamic forces and moments of the ship are:

[0096]

[0097] In the formula, m 11 represents the added mass of the x n axis; m 22 represents the added mass of the y n axis; m 66 represents the added inertia moment of the z n axis; d 11 represents the hydrodynamic damping coefficient of the x n axis; d 22 represents the hydrodynamic damping coefficient of the y n axis; d 33 represents the hydrodynamic damping coefficient of the z n axis.

[0098] Finally, substituting equations (8) and (9) into equation (7), the three-degree-of-freedom model of the unmanned ship can be obtained as:

[0099]

[0100] The calculation formulas for the added mass (m 11 , m 22 ) and the inertia moment (m 66 ) are:

[0101]

[0102] In the formula, L represents the length of the ship, W represents the width of the ship, D is the draft of the ship, ρ is the density of water, and d represents the distance between the propellers.

[0103] The calculation formula for the drag coefficient (d 11 , d 22 , d 33 ) is expressed as:

[0104]

[0105] In the formula, x s represents the state vector; u s represents the input vector; B s (u s ) represents the force or torque generated by the input command; the vector φ(x s ) represents the (linear and non-linear) terms parameterized by the matrix A s in the system dynamics. Since the dynamics of the unmanned ship is stable, when the input vector is set to a constant value (u ss ), the unmanned ship starts to move; after the movement of the unmanned ship reaches a steady state, there is That is, simplifying equation (12) gives the formula:

[0106] 0 = A s φ(xss ) + B s (u ss ) (13)

[0107] Where x ss is the steady-state value of the state vector.

[0108] Based on formula (13), a mapping that relates the input vector to the steady-state value of the state variable can be obtained, and this mapping can be used to identify the damping coefficient of the unmanned ship.

[0109] S4: Calculate the path following guidance rate of the unmanned ship

[0110] Let P k-1 (x k-1 , y k-1 ), P k (x k , y k ), and P k+1 (x k+1 , y k+1 ) be three waypoints of the unmanned ship. Connecting all the waypoints in sequence is the entire path that the unmanned ship needs to follow. Among them, the straight line P k-1 P k is the current desired path, and Ψ k is the tangential angle of the horizontal path (i.e., the angle between the desired path P k-1 P k and the positive direction of the x-axis of the earth coordinate system), and θ los is the LOS error angle.

[0111] The control objective of the unmanned ship is to approach the waypoint P k , and converge to the desired path P k-1 P k . When the ship deviates from the desired path P k-1 P k , from the perspective of a helmsman, it is not desired that the ship continues to move in the direction of PP k , but rather hopes to move in the direction of PP los , because this can make the ship gradually approach the desired path. So at this time, let the desired heading of the ship be Ψ d , and its calculation formula is:

[0112] Ψ d = Ψ k - θ los (14)

[0113] Where the calculation formula for the tangential angle Ψ k of the horizontal path is:

[0114] Ψ k = atan2(y k-y k-1 ,x k -x k-1 ) (15)

[0115] In the formula, the function atan2 can determine the quadrant where the angle is located, and Ψ k ranges from [-π, π].

[0116] In Equation (14), the course error angle θ los is calculated as follows:

[0117]

[0118] In the formula, Δ is the forward-looking distance; d e is the lateral deviation between the unmanned ship and the desired path, and its calculation formula is:

[0119]

[0120] In the formula, x e is the longitudinal deviation, and R T is the rotation matrix for converting the earth coordinate system to the hull coordinate system, and its calculation formula is:

[0121]

[0122] Substituting Equation (17) into Equation (18) and expanding, the calculation formula for the lateral deviation can be obtained:

[0123] d e = -(x - x k-1 )sinΨ k + (y - y k-1 )cosΨ k (19)

[0124] Thus, according to Equation (14), the desired course Ψ d can be calculated.

[0125] S5: Set the tracking circle of the unmanned ship

[0126] Under the control of the guidance ratio, the unmanned ship converges to the desired path P k-1 P k , and moves along P k-1 P k . When the unmanned ship approaches the desired waypoint P k , the desired waypoint should be converted from P k to P k+1 , that is, the desired path is converted from P k-1 P k to P k P k+1 . Therefore, set a circle with the desired waypoint P kA circle centered at the origin, and this area is named the "switching circle". When the unmanned boat sails into the "switching circle" area, the unmanned boat will convert the expected waypoint and path according to the formula: where P(x, y) is the current position of the unmanned boat, and R is the radius of the "switching circle". The value of R depends on the positioning accuracy of the positioning module in the control system and the control accuracy of the control algorithm.

[0127] S6: Optimize the control parameters through LQR-PD. LQR-PD is a linear quadratic regulator-differential proportional control, which specifically includes the following steps:

[0128] S6-1: Establish a control model

[0129] After the LOS guidance rate gives the expected heading, an algorithm is required to complete the heading control. When the heading of the unmanned boat is the same as the expected heading, no control action is required at this time; while when the heading of the unmanned boat deviates from the expected heading, a heading error is generated. The calculation formula for the heading error is:

[0130] ψ e = Ψ d - Ψ(21)

[0131] where Ψ e is the heading error; Ψ is the current heading angle; ψ d is the expected heading, which can be regarded as a constant during the heading control. Taking the derivative of equation (21) gives:

[0132]

[0133] where r is the yaw angular velocity. In order to control r, a control model of r is required. Therefore, the control model of the yaw angular velocity is designed as:

[0134]

[0135] Since the unmanned boat is low-speed and an underactuated unmanned boat (the thrust of the thruster in the y n direction is zero and cannot control the y n direction), the velocity in the y n direction is much lower than the velocity in the x n direction. Therefore, the velocity in the y n direction is ignored, that is, let v = 0, and rewrite formula (23) as:

[0136]

[0137] Combining equation (22) and equation (24), we can get:

[0138]

[0139] Rearranging Equation (25) into a state - space expression gives:

[0140]

[0141] Where m′ 33 =I z +m 66 , N p is the turning moment output by the unmanned ship.

[0142] S6 - 2: Output control parameters

[0143] The calculation formula for the turning moment output by the unmanned ship is:

[0144] N R =-(x R +a H x H )F N cosδ (27)

[0145] Where N R is the rudder moment; F N is the normal force of the rudder; δ is the rudder angle; a H is the correction factor; x H is the interference coefficient; x R is the vertical distance from the rudder to the center of gravity of the ship.

[0146] Since the dynamic range of the rudder angle is small (generally within ±30°), the turning speed of the unmanned ship with a rudder control method is slow and the turning radius is large. Therefore, the laser bathymetric unmanned ship adopts a dual - propulsion power and uses differential steering. The two thrusters of the unmanned ship are respectively installed behind the two side pontoons. The thrusters on both sides should not only output the forward power but also output the turning moment. In order to decouple the control of forward movement and turning, the control output method of differential steering is designed as follows:

[0147] Because the thrust provided by the two thrusters acts in the direction of the x n axis, the forward thrust X P generated by the thrusters can be expressed as:

[0148] X P =F l +F r (28)

[0149] Where F l and F r are the thrusts of the left and right thrusters respectively.

[0150] Since the thrusters do not generate thrust on the y n axis, the thrust Y P= 0. And the unmanned boat can generate torque by differential of thrusters, that is, the steering torque generated on the z n axis is expressed as:

[0151]

[0152] In the formula, d is the distance between the two thrusters.

[0153] To establish the connection between the forward thrust and the steering torque and the controller commands, assume that the thruster thrust is approximately proportional to the control command, and its expression formula is:

[0154]

[0155] In the formula, a is the proportionality coefficient between the control command and the thrust; n l , n r are the control commands of the left and right thrusters respectively. At the same time, in order to decouple the control of forward and turning, the control commands n l , n r are set to two parts, and its formula is:

[0156]

[0157] In the formula, n 0 is the control command constant, and Δn is the control command variable. Substitute formula (29) into formula (30) to get formula (31):

[0158] F l = a(n 0 + Δn)

[0159] F r = a(n 0 - Δn) (32)

[0160] Substitute formula (32) into formula (28) to get:

[0161] X P = 2an 0 (33)

[0162] It can be seen from formula (33) that since a is a fixed value, the thrust X n on the x P axis only depends on the setting of the control command constant (n 0 ), that is, n 0 determines the forward speed of the unmanned boat. Therefore, adjusting the value of n 0 in the control of the unmanned boat can control the speed of the boat.

[0163] Substitute formula (32) into formula (29) to get:

[0164] N P= 2adΔn (34)

[0165] As can be seen from Equation (33), since a and d are fixed values, the magnitude change of the turning moment (N P ) is only related to the control command variable (Δn); when the control command variable is greater than zero, the thrust of the left thruster is greater than that of the right thruster, and the unmanned ship turns to the right; when the control command variable is less than zero, the thrust of the left thruster is less than that of the right thruster, and the unmanned ship turns to the left.

[0166] Through Equation (33) and Equation (34), the forward and turning controls are successfully decoupled. Finally, substituting Equation (34) into Equation (26) gives the final control model, and its expression formula is:

[0167]

[0168] S6-3: Optimize control parameters

[0169] Let: State vector x v = [r, Ψ e T , matrix Matrix Then Equation (34) can be rewritten as:

[0170]

[0171] To find the optimal control law of Δn, according to the linear quadratic regulator (LQR) principle, set the cost function J as:

[0172]

[0173] In the formula, Q and R are weight matrices (also real symmetric matrices). The cost function contains state variables and inputs, and different weight coefficients can be set according to the different importance degrees of the state variables and inputs.

[0174] To facilitate the solution of the function J, set the state feedback of the system as:

[0175] Δn = -Kx v (38)

[0176] In the formula, K = [k 1 , k 2 is the state feedback vector. Substituting Equation (38) into Equation (36) gives:

[0177]

[0178] As can be seen from Equation (39), through state feedback, the state equation is transformed into a form without input.

[0179] ​Meanwhile, substituting Equation (38) into Equation (37) gives:

[0180]

[0181] As can be seen from Equation (40), through state feedback, the cost function is also transformed into a form without input. To solve Equation (40), it is necessary to find the primitive function of, and assume that there exists a real symmetric matrix P satisfying:

[0182]

[0183] Expanding the left side of Equation (41) gives:

[0184]

[0185] Substituting Equation (36) into Equation (42) gives:

[0186]

[0187] Comparing Equation (43) and Equation (41) gives:

[0188] -Q-K T RK=(A - BK) T P + P(A - BK) (42)

[0189] The process of solving K in the above equation is relatively complex. To simplify the solution of the optimal value of the function J, let:

[0190] K = R -1 B T P (43)

[0191] Substituting Equation (43) into Equation (11) gives:

[0192] PA + A T P - PBR -1 B T P + Q = 0 (44)

[0193] The matrix P can be solved through Equation (44), and then K can be solved through Equation (43) to obtain the state feedback solution:

[0194] Δn=-Kx v =-k 1 r - k 2 Ψ e (45)

[0195] Meanwhile, to improve the convergence speed and reduce the overshoot, the derivative term of the heading error is added to Equation (45), and then we can get:

[0196]

[0197] In the formula, K D is the differential term gain.

[0198] The following will be described in conjunction with specific embodiments. As Figure 1 shown, the path tracking control principle of the laser bathymetric unmanned vessel of the present invention is as follows: Based on the desired path and the position of the unmanned vessel, the guidance rate is calculated to obtain the desired heading. Combining with the current heading, the heading control parameter of the unmanned vessel is obtained to control the navigation of the unmanned vessel.

[0199] As Figure 2 shown, according to step S1, the conversion of the unmanned vessel to the geodetic coordinate system is realized.

[0200] According to step S2, the kinematic equation of the unmanned vessel is constructed.

[0201] According to step S3, the dynamic equation of the unmanned vessel is constructed.

[0202] As Figure 3 shown, according to step S4, the tracking path guidance rate of the unmanned vessel is calculated.

[0203] As Figure 4 shown, according to step S5, the tracking circle of the unmanned vessel is set.

[0204] According to step S6, the LQR-PD optimization control parameter is realized.

[0205] As Figure 5 shown, the laser bathymetric unmanned vessel control system tested in the present invention includes a computer and ground station software, a remote controller, a data transmission module, an unmanned vessel, a battery, an electronic speed controller, a positioning module, a control board, a data transmission module, a remote control receiver, and an attitude detection module.

[0206] The test location of the present invention is an artificial lake at No. 319, Yanshan Street, Yanshan District, Guilin City, Guangxi (Yanshan Campus of Guilin University of Technology), with an area of 5000 square meters.

[0207] As Figure 6These are the three test paths P1, P2, and P3 of the present invention. To fully verify the reliability of the algorithm, multiple-path tracking experiments were conducted manually. The paths are P1 (P11 → P12 → P13 → P14 → P11), P2 (P21 → P22 → P23 → P21), and P3 (P31 → P32 → P33 → P34 → P35). The first test path P1 of the present invention is a parallelogram with acute and obtuse angles, having four vertices P11, P12, P13, and P14. The second test path P2 of the present invention is a triangle with an acute angle, having three vertices P21, P22, and P23. The third test path P3 of the present invention is a serpentine path with three turns, having five vertices P31, P32, P33, P34, and P35. The latitude and longitude of each waypoint are shown in Table 1:

[0208] Table 1 Latitude and Longitude of Waypoints in the Artificial Lake

[0209]

[0210] The specific experimental process is as follows:

[0211] (1) After installing each module, turn on the power switch of the unmanned boat and place the unmanned boat in the lake; turn on the power of the computer and open the ground station software. This software has the function of communicating with the unmanned boat control board through the data transmission module, and can set the switch of the unmanned boat, as well as basic parameters such as waypoints, speed, and attitude angle required for navigation.

[0212] (2) Connect the data transmission module to the computer. Open the serial port in the ground station software to establish communication with the data transmission module.

[0213] (3) Use the ground station software to plan a path (P1 or P2 or P3) and send it to the unmanned boat.

[0214] (4) Use the ground station software to set the LQR-PD algorithm and send it to the unmanned boat.

[0215] (5) Use the remote control to turn on the manual control mode and manually drive the unmanned boat to a position near the starting point.

[0216] (6) Use the remote control to turn off the manual control mode and at the same time turn on the automatic control mode. At this time, the unmanned boat starts to move. The ground station software records the movement trajectory and lateral deviation of the unmanned boat in real time.

[0217] (7) After the unmanned boat completes the tracking, use the ground station software to save the unmanned boat trajectory and lateral deviation data.

[0218] (8) Use the ground station software to set the PID algorithm and repeat steps (5)-(7) until the tracking paths of the two comparison algorithms are completed.

[0219] (9) Use the ground station software to plan another path (P1 or P2 or P3), and repeat steps (4)-(8) until the path tracking control comparison experiment for the three paths is completed.

[0220] The comparison results of the trajectory and lateral error between the test path P1, the desired path, and PID in the present invention are as Figure 7 shown. The average error data of the four straight-line segments of P1 are shown in Table 2:

[0221] Table 2 Path P 1 Experimental data

[0222]

[0223]

[0224] Observation Figure 7 and Table 2 reveal that:

[0225] (1) In the path P11P12, both algorithms achieved good tracking effects, but the trajectory of the LQR-PD algorithm was significantly closer to the desired path, with an average lateral deviation of only 0.82 m, which was 0.18 m less than that of the PID algorithm.

[0226] (2) In the path P12P13, the convergence speed of the PID algorithm at the P12 turn was slower than that of the LQR-PD algorithm. Therefore, the average lateral deviation of the LQR-PD algorithm was only 1.35 m, which was 0.1 m less than that of the PID algorithm.

[0227] (3) In the path P13P14, the PID algorithm had a large lateral deviation at the P13 turn, with an average lateral deviation as high as 1.79 m. This was because the turning angle at P13 was large and the distance of P13P14 was short (only 8.63 m). When the unmanned boat entered P13P14 not long ago, it had to turn to P14P11, and there would be a violent steering change at this time. Therefore, a higher requirement was imposed on the heading control ability. The LQR-PD algorithm demonstrated better heading control ability. It quickly stabilized at the turn P13, with an average lateral deviation of only 0.80 m, which was 0.99 m less than the error of the traditional PID.

[0228] (4) In the path P14P11, due to the influence of the previous path (violent heading change), both algorithms showed oscillations to varying degrees, but the oscillation amplitude of the LQR-PD algorithm was smaller, with an average lateral deviation of only 0.24 m, which was 0.02 m less than that of the PID algorithm.

[0229] Through the above analysis, the following conclusions can be drawn: (1) Compared with PID, the LQR-PD algorithm has a faster convergence speed, a smaller average deviation, and can make the unmanned ship closer to the desired path. Among the four segments of path P1, the average lateral deviation of the LQR-PD algorithm can be reduced by up to 0.99 m (P13 - P14) compared with the PID algorithm. (2) Both algorithms can basically complete path tracking, but when facing large heading changes, the LQR-PD algorithm has stronger adjustment ability and is not prone to large lateral deviations and oscillations. (3) Throughout the path of P1, the average lateral deviation of the LQR-PD algorithm is reduced by approximately 0.57 m compared with the traditional PID algorithm.

[0230] The comparison results of the trajectory and lateral error between the test path P2, the desired path, and PID of the present invention are as Figure 8 shown. The average error data of the three straight-line segments of P2 are shown in Table 3:

[0231] Table 3 Path P 2 Experimental data

[0232]

[0233] Observation Figure 8 and Table 3 reveal that:

[0234] (1) In the path P21 - P22, both algorithms have good tracking effects, but overall, the LQR-PD algorithm has a better effect. This is because after reaching point P21, the LQR-PD algorithm better adjusts the unmanned ship to make it closer to the trajectory, with an average deviation of only 1.18 m, which is 0.16 m less than the PID algorithm.

[0235] (2) In the path P22 - P23, both algorithms can smoothly track the path, but the average lateral deviation of the LQR-PD algorithm is only 1.36 m, which is 0.2 m less than the PID algorithm. This is because the LQR-PD algorithm can stabilize more quickly at the turn of P22.

[0236] (3) In the path P23 - P21, due to the large turning angle at P23, the PID algorithm has a slow convergence speed and oscillation phenomenon, while the LQR-PD algorithm stabilizes more quickly, and its average lateral deviation is 0.02 m less than the PID algorithm.

[0237] Through the above analysis, the following conclusions can be drawn: (1) Compared with PID, the LQR-PD algorithm has a faster convergence speed when dealing with large-angle turns; in the three sections of path P2, the average lateral deviation of the LQR-PD algorithm can be reduced by up to 0.2 m compared with the PID algorithm (P22P23). (2) The LQR-PD algorithm has stronger adjustment ability and less oscillation when turning. (3) In the entire path of P2, the average lateral deviation of the LQR-PD algorithm is reduced by about 0.13 m compared with the traditional PID algorithm.

[0238] The comparison results of the trajectory and lateral error of the present invention on the test path P3 with the desired path and PID are as Figure 9 shown. The average error data of the four straight-line sections of P3 are shown in Table 4:

[0239] Table 4 Path P 3 Experimental data

[0240]

[0241] Observation Figure 9 and Table 4 show that:

[0242] (1) In the path P31P32, the trajectory of the LQR-PD algorithm is closer to the desired path. The average lateral deviation of the LQR-PD algorithm is only 1.41 m, which is reduced by 0.43 m compared with the PID algorithm.

[0243] (2) In the path P32P33, the trajectory of the LQR-PD algorithm is significantly closer to the desired path. Especially at the turning point of P32, the LQR-PD algorithm has stronger adjustment ability, and its average lateral deviation is only 1.17 m, which is reduced by 0.19 m compared with the PID algorithm.

[0244] (3) In the path P33P34, the trajectory of the PID algorithm shows a large amplitude oscillation, which is an unstable state. This is because of the poor heading control ability of the PID algorithm, which causes a large lateral deviation at the beginning of the turn; then it is necessary to get closer to the desired path urgently, resulting in a large change in the desired heading angle and unable to adjust the heading in time. On the contrary, the LQR-PD algorithm can stabilize quickly.

[0245] (4) In the path P34P35, the trajectory of the PID algorithm has a large lateral deviation at the beginning and does not stabilize finally; the LQR-PD algorithm quickly stabilizes on a straight line, and the average lateral deviation is only 1.34 m, which is reduced by 0.36 m compared with the PID algorithm.

[0246] Through the above analysis, the following conclusions can be drawn: (1) Compared with PID, the LQR-PD algorithm exhibits better heading control performance and a faster convergence speed when the desired heading angle changes significantly; in the four segments of path P3, its maximum average lateral deviation can be reduced by up to 0.43 m (P31P32) compared with the PID algorithm. (2) Compared with PID, the LQR-PD algorithm has higher stability and does not exhibit large oscillations. (3) Throughout the entire path of P3, the average lateral deviation of the LQR-PD algorithm is reduced by approximately 0.21 m compared with the PID algorithm.

[0247] The above embodiments are only used to illustrate the technical solutions of the present invention and are not intended to limit them. Any modification or equivalent replacement that does not depart from the spirit and scope of the present invention shall be covered by the technical solutions of the present invention.

Claims

1. A guidance path tracking control method for a laser depth measurement unmanned ship, characterized in that: The following steps are involved: S1. Construct the geodetic coordinate system and the hull coordinate system. The positive x direction of the geodetic coordinate system is due north, the positive y direction is due east, and the positive z direction is perpendicular to the xoy plane and downward. The geodetic coordinate system uses the roll angle φ to represent the rotation angle of the unmanned ship around the x-axis, the pitch angle θ to represent the rotation angle around the y-axis, and the bow roll angle Ψ to represent the rotation angle around the z-axis. The positive x direction of the hull coordinate system is n Direction is the ship's forward direction, positive y n The direction is to the right of the forward direction, and the positive z n points to the ground, x n The speed in the direction is represented by u, and y n The speed in the direction is represented by v, and z n The speed in the direction is represented by w, and the speed around x n The angular velocity of rotation is represented by p, about y n The angular velocity of rotation is represented by q, about z n The angular velocity of the direction of rotation is represented by r; S2. Construct the kinematic equation of the unmanned ship, and transform the velocity u, v, w and angular velocity p, q, r of the ship coordinate system into position derivatives in the earth coordinate system. and attitude angle derivative Description, the three-degree-of-freedom kinematic equation of the unmanned ship is obtained: Among them, p(x, y) is the position coordinate value of the unmanned ship; S3. Establish a three-degree-of-freedom model of the unmanned ship: Where m is the mass of the ship, m 11 Represents x n Additional mass of the shaft, m 22 Represents y n Additional mass of the shaft, m 66 Indicates z n Additional moment of inertia of the axis, d 11 Represents x n Hydrodynamic damping coefficient of the shaft, d 22 Represents y n Hydrodynamic damping coefficient of the shaft, d 33 Indicates z n The hydrodynamic damping coefficient of the shaft, m 11 、m 22 and m 66 The calculation formula is: Where L is the length of the ship, W is the width of the ship, D is the draft of the ship, ρ is the density of water, and d is the distance between the propellers. d 11 d 22 and d 33 The calculation formula is: In the formula, x s represents the state vector, u s represents the input vector, B s (u s ) represents the force or torque generated by the input command, and the vector φ(x s ) represents the system dynamics represented by the matrix A s Parameterized terms, since the dynamics of the unmanned ship is stable, when the input vector is set to a constant value (u ss ), the unmanned ship starts to move; after the unmanned ship reaches a stable state, there is get 0=A s φ(x ss )+B s (u ss ), In the formula, x ss is the steady-state value of the state vector. Based on this, a mapping that links the input vector to the steady-state value of the state variable can be obtained. This mapping can be used to identify the damping coefficient of the unmanned ship. S4. Calculate the unmanned ship tracking path guidance rate, let P k-1 (x k-1 ,y k-1 ), P k (x k ,y k ), P k+1 (x k+1 ,y k+1 ) are the three waypoints of the unmanned ship, among which the straight line P k-1 P k is the current expected path, Ψ k is the horizontal path tangent angle, θ los is the LOS error angle, and the desired heading of the unmanned ship is Ψ d , and its calculation formula is: P d =Ψ k -θ los , Among them, the horizontal path tangent angle Ψ k The calculation formula is: Ψ k =atan2(y k -y k-1 ,x k -x k+1 ), In the formula, the function atan2 can determine the quadrant in which the angle is located, Ψ k The range is [-π, π], Heading error angle θ los The calculation formula is: Where Δ is the foresight distance, d e is the lateral deviation between the unmanned ship and the expected path, and its calculation formula is: d e =-(x-x k-1 )sinΨ k +(y-y k-1 )cosΨ k ; S5: Set the unmanned ship tracking circle, and under the control of the guidance rate, make the unmanned ship converge to the desired path P k-1 P k , and along P k-1 P k Movement, set a desired waypoint P k The circle with is the center, this area is called "switching circle". When the unmanned boat travels to the "switching circle" area, the unmanned boat follows the formula: Convert the desired waypoint and path, where P(x, y) is the current position of the unmanned ship and R is the radius of the "switching circle"; S6. Optimize control parameters through LQR-PD, including: S61. Establish control model: In the formula, m′ 33 =I z +m 66 , N p The steering torque output for the unmanned ship; S62. Install the two propellers of the unmanned boat behind the buoys on both sides, and decouple the control of the unmanned boat's forward movement and steering to obtain the final control model: In the formula, Δn is the control command variable; S63, optimize the control parameters, let the state vector x v =[r,Ψ e ] T ,matrix matrix Then the final control model can be transformed into: According to the principle of linear quadratic regulator, the cost function is set as: Where Q and R are weight matrices, To solve the function J, set the state feedback of the system to: Δn=-Kx v , Where K = [k1, k2] is the state feedback vector. Solving for K, we get: Δn=-Kx v =-k1r-k2Ψ e , Adding the differential term of the heading error to the above equation, we get: In the formula, K D is the differential gain.

2. The guiding path tracking control method of a laser depth measuring unmanned ship according to claim 1 is characterized in that: The unmanned boat control system used in the control method includes a computer and ground station software, a remote controller, a data transmission module, an unmanned boat, a battery, an electronic speed regulator, a positioning module, a control panel, a data transmission module, a remote control receiver, and a posture detection module.

Images