A path tracking control method for unmanned boat

Abstract

The invention relates to the technical field of path tracking control, in particular to an unmanned boat path tracking control method, comprising: step one, tracking path identification; step two, motion data capture; step three, motion parameter analysis; step four, control parameter generation; and step five, motion parameter execution and path optimization. The invention establishes a PID control feedback mechanism, comprehensively considers the track deviation that has occurred and makes a prediction on the expansion or reduction of the future deviation value from three perspectives of current deviation, deviation value prediction and historical deviation value, and has forward-looking and compensation characteristics in data processing. The invention has better real-time and accuracy when the unmanned boat generates a signal transmission and processing delay. The invention realizes heading control through motion data acquisition and real-time analysis, reduces the number of parameters, improves the data response speed, and improves the real-time performance of the unmanned boat path tracking control.

Description

Technical Field

[0001] The invention relates to the technical field of path tracking control, and in particular to a path tracking control method for an unmanned boat. Background Art

[0002] Path tracking control technology is a technology widely used in many fields. Its core purpose is to enable the controlled object to accurately track and stabilize on a predetermined reference path. This technology is particularly important in the fields of self-driving cars, ships, drones, etc. Path tracking control technology refers to calculating and controlling the desired input of the controlled object through motion planning output and real-time feedback of the driving state of the controlled object, so that it can drive stably along the desired path. Its basic principle is to reduce the spatial and temporal errors between the controlled object and the reference trajectory by controlling the motion parameters of the controlled object such as position, speed, acceleration, etc.

[0003] Surface unmanned boats have the advantages of small size, strong maneuverability and low cost. They are currently widely used in civilian fields such as environmental monitoring, search and rescue, hydrological surveys, as well as in military tasks with higher risks such as moving target identification and tracking.

[0004] However, the existing path tracking control technology has significant deficiencies and limitations in many aspects. First, there is a delay in signal transmission and processing during the movement of the unmanned boat, which may cause a lag in the control command and affect the real-time and accuracy of path tracking. Secondly, in order to achieve high-precision path tracking control, a large amount of calculation is required, such as the establishment and training of convolutional neural networks to achieve multi-parameter response analysis, which requires a lot of computing power. However, the control system of the unmanned boat is usually limited by computing resources and it is difficult to meet all real-time requirements.

[0005] Therefore, it is necessary to propose an unmanned boat path tracking control method to solve the above problems. Summary of the invention

[0006] The purpose of the present invention is to solve the problems existing in the background technology and to propose a path tracking control method for an unmanned boat.

[0007] The purpose of the present invention can be achieved by the following technical solution: A path tracking control method for an unmanned boat comprises the following steps:

[0008] Step 1: Tracking path identification;

[0009] Define a spatial coordinate system, including the origin of the spatial coordinate system O (0,0,0) and three mutually perpendicular spatial coordinate axes X-axis, Y-axis and Z-axis.

[0010] As a preferred embodiment of the present invention, the total length S of the patrol target path line of the unmanned boat is obtained and discretized, and interpolated in an n-equal manner to generate n+1 discrete path points. The coordinates of each path point in the spatial coordinate system are obtained Pi (xi, yi, zi) = P1 (x1, y1, z1), P2 (x2, y2, z2), ..., Pn (xn+1, yn+1, zn+1). Among them, n+1 is a preset discrete control accuracy parameter, that is, the total number of path points obtained after discretization, and xi, yi and zi are the coordinate components of each path point Pi on the X-axis, Y-axis and Z-axis respectively.

[0011] Step 2: Motion data capture;

[0012] The motion data of the unmanned boat is obtained at every preset time interval t, including the current position coordinates, speed, acceleration and steering angle, specifically:

[0013] The current position coordinates of the unmanned boat (X, Y, Z), the velocity components of the unmanned boat in the X, Y, and Z directions in the spatial coordinate system (Vx, Vy, Vz), and the deflection angles of the unmanned boat in the X, Y, and Z directions in the spatial coordinate system (θx, θy, θz).

[0014] Step 3: motion parameter analysis;

[0015] Every preset time interval t, the coordinates of each path point in the spatial coordinate system are combined with the motion data of the unmanned boat for motion analysis, and the motion data error is extracted from three aspects: position, speed direction and deflection angle. Specifically:

[0016] Position data error extraction: every preset time through the formula Calculate the distance d between the current position coordinates of the unmanned boat and the coordinates of each path point, and extract the minimum value MINd, which is recorded as the position deviation parameter E1. Where i = 1, 2, 3, ..., n. The path point that produces the minimum distance MINd from the current position coordinates of the unmanned boat is recorded as the target path point P' (x', y', z').

[0017] As a preferred embodiment of the present invention, the current time t and the position deviation parameter E1 generated by the current time operation are recorded at preset time intervals to generate a position deviation parameter point (t, E1).

[0018] Speed ​​direction data error extraction: Every preset time interval t, the formula Calculate the angle COSα between the speed direction and the direction from the current position of the unmanned boat to the target path point, and record it as the direction deviation parameter E2.

[0019] As a preferred embodiment of the present invention, the current time t and the direction deviation parameter E2 generated by the current time operation are recorded at preset time intervals to generate a direction deviation parameter point (t, E2).

[0020] Deflection angle data error extraction: Extract the number i of the target path point P' (x', y', z') at every preset time interval t, obtain the coordinates of the path point numbered i-1, recorded as point P' (x', y', z'), and use the formula

[0021] Calculate the angle μ between the deflection angle of the unmanned boat in the XOY plane and the patrol target path line, which is recorded as the angle deviation parameter E3.

[0022] As a preferred embodiment of the present invention, the current time t and the turning angle deviation parameter E3 generated by the current time calculation are recorded at preset time intervals to generate a turning angle deviation parameter point (t, E3).

[0023] Step 4: Control parameter generation:

[0024] The error control is performed by combining the coordinates of each path point in the spatial coordinate system with the motion data of the unmanned boat, specifically:

[0025] As a preferred embodiment of the present invention, a PID controller is established, which specifically includes: a data image generating unit, an image feature extracting unit, a data fusion unit and a control parameter generating unit.

[0026] The data image generation unit records all input data points (t, r) of the PID controller, and then generates a data image with time as the horizontal axis and r as the vertical axis, i.e., the rt image, where r is a parameter in the input data point, i.e., the position deviation parameter E1, the direction deviation parameter E2 or the angle deviation parameter E3.

[0027] The image feature extraction unit extracts the preset threshold value rmax corresponding to the parameter r, and in the rt image, the formula Calculate the deviation ratio r1, deviation integral r2 and deviation differential r3 of parameter r.

[0028] The data fusion unit calculates the execution parameter δ by the formula δr=k1×r1+k2×r2+k3×r3, where k1, k2 and k3 are preset weight factors.

[0029] The position deviation parameter data point (t, E1), direction deviation parameter point (t, E2) and angle deviation parameter point (t, E3) are respectively input into the PID controller to obtain the corresponding execution parameters δE1, δE2 and δE3.

[0030] Step 5: motion parameter execution and path optimization;

[0031] According to the execution parameters obtained in step 4, the position deviation control and the steering angle deviation control are executed, specifically:

[0032] If the value of the execution parameter δE1 is greater than the preset threshold δE1Max, it is determined that a heading deviation has occurred, and a speed control command is generated. The value of δE3 is further determined. If the value of δE3 is greater than 0, it is determined that the heading is deviated to the left, and a steering command 1 is generated; otherwise, a steering command 2 is generated.

[0033] The speed control instruction is specifically: obtain the maximum adjustment speed nMAX and minimum adjustment speed nMin of the left and right propellers of the unmanned boat, set the speed base , where c1 and c2 are preset over-limit multiples.

[0034] The steering instruction 1 is: set the rotation speed of the left propeller to N0+nMin; set the rotation speed of the right propeller to N0-nMin;

[0035] The second steering instruction is to set the rotation speed of the left propeller to N0-nMin; and the rotation speed of the right propeller to N0+nMin.

[0036] Compared with the prior art, the present invention has the following beneficial effects:

[0037] (1) The present invention constructs a feedback mechanism based on the PID control strategy. The design of this mechanism deeply integrates the instant perception of the current deviation, the accurate prediction of the deviation value change trend, and the intelligent learning and analysis of the historical deviation data, thereby realizing the all-round and multi-dimensional management of the track deviation during the navigation of the unmanned boat. The deviation between the current position of the unmanned boat and the predetermined track is captured in real time by a high-precision sensor, and the control signal is quickly adjusted by the proportional term of the PID controller to reduce the current deviation in the most direct way, ensuring that the unmanned boat can quickly return to the predetermined route, thereby enhancing the instant response capability of the system. The synergistic effect of the integral term and the differential term are introduced in the PID control, which not only considers the cumulative effect of the current deviation for long-term correction, but also predicts the trend of the future deviation value through the prediction model. This forward-looking deviation value prediction capability enables the system to take measures in advance to effectively avoid further expansion of the deviation. The historical deviation data is used for statistical analysis to predict the possible deviation mode in the future, and the control parameters are dynamically adjusted and optimized. The present invention realizes the effective management and compensation of the track deviation in a complex navigation environment, and provides a strong technical support for the autonomous navigation and safe navigation of the unmanned boat.

[0038] (2) The present invention provides a fast-response unmanned boat path tracking control solution, which realizes heading control through motion data acquisition and real-time analysis. By streamlining the algorithm logic and optimizing the data processing flow, the present invention successfully reduces the number of parameters that need to be adjusted, thereby reducing the computational burden and improving the data response speed. This improvement enables the unmanned boat to make adjustments more quickly according to the current navigation status and environmental changes, thereby improving the real-time performance of path tracking control.

[0039] (3) The present invention comprehensively considers various execution parameters and dynamically adjusts the rotation speed of the left and right propellers, ensuring that the unmanned boat obtains sufficient correction power when the deviation distance is large, and allocates the specific rotation speeds of the left and right propellers according to the turning angle deviation and speed direction deviation of the unmanned boat, ensuring that the unmanned boat obtains sufficient rotation distance when the turning angle deviation is large; the path of the unmanned boat is corrected based on the speed, heading and turning angle, ensuring that the unmanned boat continuously corrects the heading on the preset path. BRIEF DESCRIPTION OF THE DRAWINGS

[0040] In order to facilitate understanding by those skilled in the art, the present invention is further described below with reference to the accompanying drawings:

[0041] Figure 1 is a flow chart of the method of the present invention;

[0042] Figure 2 It is a schematic diagram of the discretization of the path line of the present invention;

[0043] Figure 3 It is a schematic diagram of the PID controller of the present invention;

[0044] Figure 4 It is a flow chart of execution parameter response of the present invention. DETAILED DESCRIPTION

[0045] The technical solution of the present invention will be clearly and completely described below in conjunction with the embodiments. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0046] See also Figure 1 As shown, a path tracking control method for an unmanned boat comprises the following steps:

[0047] Step 1: Tracking path identification;

[0048] Define a spatial coordinate system, including the origin of the spatial coordinate system O (0,0,0) and three mutually perpendicular spatial coordinate axes X-axis, Y-axis and Z-axis.

[0049] See also Figure 2As shown in the figure, taking maritime patrol as an example, it is assumed that the unmanned boat is preset with a fixed patrol target path line. The total length S of the patrol target path line of the unmanned boat is obtained and discretized, and interpolated in n equal parts to generate n+1 discrete path points, namely: Pi=P1, P2,..., Pn+1. The coordinates of each path point in the spatial coordinate system are obtained Pi(xi, yi, zi)=P1(x1, y1, z1), P2(x2, y2, z2),..., Pn(xn+1, yn+1, zn+1). Among them, n+1 is the preset discrete control accuracy parameter, that is, the total number of path points obtained after discretization, and xi, yi and zi are the coordinate components of each path point Pi on the X-axis, Y-axis and Z-axis respectively.

[0050] Step 2: Motion data capture;

[0051] The motion data of the unmanned boat is obtained at every preset time interval t, including the current position coordinates, speed, acceleration and steering angle, specifically:

[0052] The current position coordinates of the unmanned boat (X, Y, Z), the velocity components of the unmanned boat in the X, Y, and Z directions in the spatial coordinate system (Vx, Vy, Vz), and the deflection angles of the unmanned boat around the X, Y, and Z coordinate axes in the spatial coordinate system (θx, θy, θz).

[0053] Step 3: motion parameter analysis;

[0054] Every preset time interval t, the coordinates of each path point in the spatial coordinate system are combined with the motion data of the unmanned boat for motion analysis, and the motion data error is extracted from three aspects: position, speed direction and deflection angle. Specifically:

[0055] Position data error extraction: every preset time through the formula Calculate the distance d between the current position coordinates of the unmanned boat and the coordinates of each path point, and extract the minimum value MINd, which is recorded as the position deviation parameter E1. Where i = 1, 2, 3, ..., n. The path point that produces the minimum distance MINd from the current position coordinates of the unmanned boat is recorded as the target path point P' (x', y', z').

[0056] Furthermore, the current time t and the position deviation parameter E1 generated by the current time operation are recorded at every preset time interval to generate a position deviation parameter point (t, E1).

[0057] It should be noted that, under ideal conditions, if the unmanned boat always moves along the patrol target path line, the value of the position deviation parameter E1 always fluctuates between 0 and S / n. If the unmanned boat deviates from the route, the value of the position deviation parameter E1 will continue to be greater than S / n for a short period of time. Where S is the total length of the patrol target path line, and n is the patrol target path line equal fraction.

[0058] Speed ​​direction data error extraction: Every preset time interval t, the formula Calculate the angle COSα between the speed direction and the direction from the current position of the unmanned boat to the target path point, and record it as the direction deviation parameter E2.

[0059] It should be noted that, under ideal conditions, if the unmanned boat always moves along the patrol target path line, the speed direction of the unmanned boat is always consistent with the direction from the current position of the unmanned boat to the target path point, that is, the value of the direction deviation parameter E2 is 1. If the unmanned boat deviates from the route, the value of the position deviation parameter E2 will continue to be greater than 0 for a short period of time.

[0060] Furthermore, the current time t and the direction deviation parameter E2 generated by the current time operation are recorded at every preset time interval to generate a direction deviation parameter point (t, E2).

[0061] Deflection angle data error extraction: Extract the number i of the target path point P' (x', y', z') at every preset time interval t, obtain the coordinates of the path point numbered i-1, recorded as point P' (x', y', z'), and use the formula

[0062] Calculate the angle μ between the deflection angle of the unmanned boat in the XOY plane and the patrol target path line, which is recorded as the angle deviation parameter E3.

[0063] It should be noted that, under ideal conditions, if the unmanned boat always moves along the patrol target path line, the deflection angle of the unmanned boat in the XOY plane is consistent with the direction of the patrol target path line, that is, μ = 0. If the unmanned boat deviates from the patrol target path line and deviates to the left, the value of μ will be greater than 0; if the unmanned boat deviates from the patrol target path line and deviates to the right, the value of μ will be less than 0.

[0064] Furthermore, the current time t and the turning angle deviation parameter E3 generated by the current time calculation are recorded at every preset time interval to generate a turning angle deviation parameter point (t, E3).

[0065] Step 4: Control parameter generation:

[0066] The error control is performed by combining the coordinates of each path point in the spatial coordinate system with the motion data of the unmanned boat, specifically:

[0067] See also Figure 3As shown, a PID controller is established, which specifically includes: a data image generating unit, an image feature extracting unit, a data fusion unit and a control parameter generating unit.

[0068] The data image generation unit records all input data points (t, r) of the PID controller, and then generates a data image with time as the horizontal axis and r as the vertical axis, i.e., the rt image, where r is a parameter in the input data point, i.e., the position deviation parameter E1, the direction deviation parameter E2 or the angle deviation parameter E3.

[0069] The image feature extraction unit extracts the preset threshold value rmax corresponding to the parameter r, and in the rt image, the formula Calculate the deviation ratio r1, deviation integral r2 and deviation differential r3 of parameter r.

[0070] The data fusion unit calculates the execution parameter δ by the formula δr=k1×r1+k2×r2+k3×r3, where k1, k2 and k3 are preset weight factors.

[0071] The position deviation parameter data point (t, E1), direction deviation parameter point (t, E2) and angle deviation parameter point (t, E3) are respectively input into the PID controller to obtain the corresponding execution parameters δE1, δE2 and δE3.

[0072] It should be noted that the execution parameter δE1 comprehensively reflects the deviation ratio, historical deviation accumulation and deviation value change rate between the unmanned boat and the patrol target path line in distance; the execution parameter δE2 comprehensively reflects the deviation ratio, historical deviation accumulation and deviation value change rate between the unmanned boat and the patrol target path line in direction; the execution parameter δE3 comprehensively reflects the deviation ratio, historical deviation accumulation and deviation value change rate of the unmanned boat in steering.

[0073] Step 5: motion parameter execution and path optimization;

[0074] According to the execution parameters obtained in step 4, the position deviation control and the steering angle deviation control are executed, specifically:

[0075] See also Figure 4 If the value of the execution parameter δE1 is greater than the preset threshold δE1Max, it is determined that a heading deviation has occurred, and a speed control instruction is generated. The value of δE3 is further determined. If the value of δE3 is greater than 0, the heading is determined to be left-biased, and a steering instruction 1 is generated; otherwise, a steering instruction 2 is generated.

[0076] The speed control instruction is specifically: obtain the maximum adjustment speed nMAX and minimum adjustment speed nMin of the left and right propellers of the unmanned boat, set the speed base , where c1 and c2 are preset over-limit multiples.

[0077] The steering instruction 1 is: set the rotation speed of the left propeller to N0+nMin; set the rotation speed of the right propeller to N0-nMin;

[0078] The second steering instruction is to set the rotation speed of the left propeller to N0-nMin; and the rotation speed of the right propeller to N0+nMin.

[0079] It should be understood that the terms "include" and "comprising" used in the specification and claims of the present disclosure indicate the presence of described features, integers, steps, operations, elements and / or components, but do not exclude the presence or addition of one or more other features, integers, steps, operations, elements, components and / or collections thereof.

[0080] It should also be understood that the terms used in this disclosure are only for the purpose of describing specific embodiments and are not intended to limit the disclosure. As used in this disclosure and claims, the singular forms "a", "an", and "the" are intended to include plural forms unless the context clearly indicates otherwise. It should also be further understood that the term "and / or" used in this disclosure and claims refers to any combination of one or more of the associated listed items and all possible combinations, and includes these combinations;

[0081] The preferred embodiments of the present invention disclosed above are only used to help explain the present invention. The preferred embodiments do not describe all the details in detail, nor do they limit the invention to only specific implementation methods. Obviously, many modifications and changes can be made according to the content of this specification. This specification selects and specifically describes these embodiments in order to better explain the principles and practical applications of the present invention, so that those skilled in the art can understand and use the present invention well. The present invention is limited only by the claims and their full scope and equivalents.

Claims

1. A path tracking control method for an unmanned boat, characterized in that: The following steps are involved: Step 1: Tracking path identification; Define a spatial coordinate system, including the origin of the spatial coordinate system and three mutually perpendicular spatial coordinate axes, the X-axis, the Y-axis and the Z-axis; obtain the total length of the patrol target path line of the unmanned boat and perform discretization processing; Step 2: Motion data capture; Obtaining the motion data of the unmanned boat at a preset time interval t; Step 3: motion parameter analysis; At every preset time interval, the coordinates of each path point in the space coordinate system are combined with the motion data of the unmanned boat for analysis, including position analysis, speed direction analysis and deflection angle analysis, to obtain data points of each motion data changing over time; Position data error extraction: every preset time through the formula Calculate the distance d between the current position coordinates of the unmanned boat and the coordinates of each path point, and extract the minimum value MINd, which is recorded as the position deviation parameter E1; where i = 1, 2, 3, ..., n; record the path point that produces the minimum distance MINd from the current position coordinates of the unmanned boat as the target path point P' (x', y', z'); Record the current time t and the position deviation parameter E1 generated by the current time operation at every preset time interval to generate a position deviation parameter point (t, E1); Speed ​​direction data error extraction: Every preset time interval t, the formula Calculate the angle COSα between the speed direction and the direction from the current position of the unmanned boat to the target path point, and record it as the direction deviation parameter E2; Record the current time t and the direction deviation parameter E2 generated by the current time operation at every preset time interval to generate a direction deviation parameter point (t, E2); Deflection angle data error extraction: Extract the number i of the target path point P' (x', y', z') at every preset time interval t, obtain the coordinates of the path point numbered i-1, recorded as point P' (x', y', z'), and use the formula Calculate the angle μ between the deflection angle of the unmanned boat in the XOY plane and the patrol target path line, recorded as the angle deviation parameter E3; Record the current time t and the turning angle deviation parameter E3 generated by the current time calculation at every preset time interval to generate a turning angle deviation parameter point (t, E3); Step 4: Control parameter generation: The coordinates of each path point in the space coordinate system are combined with the motion data of the unmanned boat to perform error control, and a PID controller is established. The data points of each motion data that change over time are input into the PID controller to obtain the corresponding execution parameters; The data image generating unit records all input data points (t, r) of the PID controller, and then generates a data image, i.e., an rt image, with time as the horizontal axis and r as the vertical axis, wherein r is a parameter in the input data point, i.e., a position deviation parameter E1, a direction deviation parameter E2, or a rotation angle deviation parameter E3; The image feature extraction unit extracts the preset threshold value rmax corresponding to the parameter r, and in the rt image, the formula Calculate the deviation ratio r1, deviation integral r2 and deviation differential r3 of parameter r; The data fusion unit calculates the execution parameter δ by the formula δr=k1×r1+k2×r2+k3×r3, where k1, k2 and k3 are preset weight factors; The position deviation parameter data point (t, E1), the direction deviation parameter point (t, E2) and the angle deviation parameter point (t, E3) are respectively input into the PID controller to obtain the corresponding execution parameters δE1, δE2 and δE3; Step 5: motion parameter execution and path optimization; Execute position deviation control and steering angle deviation control according to the execution parameters obtained in step three.

2. The unmanned boat path tracking control method according to claim 1, characterized in that: The specific process of obtaining the total length of the patrol target path line of the unmanned boat and discretizing it is as follows: Interpolation is performed in an n-equal-division manner to generate n+1 discrete path points; the coordinates of each path point in the spatial coordinate system are obtained Pi(xi, yi, zi) = P1(x1, y1, z1), P2(x2, y2, z2), ..., Pn(xn+1, yn+1, zn+1); where n+1 is the preset discrete control accuracy parameter, that is, the total number of path points obtained after discretization, and xi, yi and zi are the coordinate components of each path point Pi on the X-axis, Y-axis and Z-axis respectively.

3. The unmanned boat path tracking control method according to claim 1, characterized in that: The motion data of the unmanned boat includes: The current position coordinates of the unmanned boat (X, Y, Z), the velocity components of the unmanned boat in the X, Y, and Z directions in the spatial coordinate system (Vx, Vy, Vz), and the deflection angles of the unmanned boat in the X, Y, and Z directions in the spatial coordinate system (θx, θy, θz).

4. The unmanned boat path tracking control method according to claim 1, characterized in that: The specific process of executing the position deviation control and steering angle deviation control according to the execution parameters obtained in step 4 is as follows: If the value of the execution parameter δE1 is greater than the preset threshold δE1Max, it is determined that a heading deviation has occurred and a speed control instruction is generated; the value of δE3 is further determined. If the value of δE3 is greater than 0, the heading is determined to be left-deviated and a steering instruction 1 is generated; otherwise, a steering instruction 2 is generated.

5. The unmanned boat path tracking control method according to claim 4, characterized in that: The speed control instruction, steering instruction 1 and steering instruction 2 are specifically: Speed ​​control command: Get the maximum speed nMAX and minimum speed nMin of the left and right propellers of the unmanned boat, and set the speed base , where c1 and c2 are preset over-limit multiples; Steering instruction 1: Set the speed of the left propeller to N0+nMin; the speed of the right propeller to N0-nMin; Steering instruction 2: Set the speed of the left propeller to N0-nMin; the speed of the right propeller to N0+nMin.

Images