Under-actuated ship path tracking method and system based on active-disturbance-rejection first-direction control
Through self-immune control and nonlinear state error feedback control, combined with improved observers and controllers, the problem of reduced tracking effect and steady-state error caused by changes in control cycles in ship motion control is solved, and the accuracy of ship path tracking is improved. robustness.
Patent Information
- Application Number
- CN202411082955.6
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-08-07
- Publication Date
- 2024-11-15
AI Technical Summary
The existing ship motion control methods have failed to effectively solve the frequency selection problem of expanding state observer under different control cycles, resulting in a decrease in the tracking effect of the controller, and the problems of steady-state error and low first-way control accuracy.
The under-driven ship path tracking method based on self-immune disturbance control is adopted to estimate disturbance terms in real time by improving the drift angle observer and linear expansion state observer, and calculate the expected rudder angle by using the nonlinear state error feedback controller to realize the path tracking of the ship .
It effectively eliminates the steady-state error of the ship during path tracking, improves the controller's robustness and head-direction control accuracy, reduces position overshooting, and enhances the accuracy of observer estimation under different control cycles.
Smart Images

Figure CN118963365A_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the technical field of ship path tracking, and in particular to a method and system for underactuated ship path tracking based on anti-disturbance heading control. Background Art
[0002] With the vigorous development of the modern shipping industry and the continuous optimization of the transportation structure, ships are developing rapidly in the direction of large-scale, specialization and intelligence. This puts forward more stringent requirements on the maneuvering and control performance of ships. Due to the continuous increase in maritime transportation, the probability of ship accidents has also increased significantly. Many accidents are often caused by the negligence and faults of navigators. In order to effectively reduce the risk of accidents caused by human errors, it is particularly important to scientifically and accurately operate and control ships. In complex sea conditions, how to ensure that ships can reach their destination safely and on time has become a vital issue. Therefore, it is urgent to adopt advanced ship motion control methods to meet challenges and improve the safety and efficiency of the overall transportation system.
[0003] At present, the methods of ship motion control mainly include: proportional-integral-differential control (PID control), sliding mode control, fuzzy control and backstepping control. In actual navigation, ship motion control technology is mainly based on proportional-integral-differential control, combined with line-of-sight guidance law to achieve ship path tracking. However, the autopilot based on the proportional-integral-differential control method is difficult to achieve accurate heading control of the ship under environmental disturbances. In order to cope with the demand for precise control of the system state under external disturbances and internal model uncertainty, some people proposed an active disturbance rejection control algorithm based on proportional-integral-differential control, which has received widespread attention in the field of ship motion control. Compared with methods such as sliding mode control, fuzzy control and backstepping control, active disturbance rejection control has strong robustness to unmodeled dynamics and uncertainties, strong anti-noise performance, and low requirements for modeling accuracy. The active disturbance rejection control algorithm can design the frequency of the extended state observer according to the system bandwidth to ensure that the observer can effectively estimate unknown disturbances and remain stable when the system changes dynamically. The experimental simulation results show that the frequency selection of the extended state observer is not only related to the response characteristics of the control system, but also closely related to the length of the control cycle. It is worth noting that the current ship ADRC has not considered the frequency selection of the extended state observer under different control periods. Once the control period changes, the tracking effect of the controller will be significantly reduced. When the classic line-of-sight guidance method is used, there will always be a significant steady-state error between the ship and the target path due to the existence of the drift angle. When using the integral line-of-sight guidance method with fixed gain, excessive gain may lead to integral saturation, which in turn causes a large position overshoot in the process of tracking the path; conversely, too small a gain may make it difficult for the lateral tracking error to converge quickly.
[0004] In summary, there are still some shortcomings in the current methods of ship motion control, and more intelligent path planning methods that fully consider collision avoidance rules are needed to solve these problems. Summary of the invention
[0005] In order to solve the problems that the existing ship motion control method does not consider the extended state observer frequency under different control cycles, the tracking effect of the controller is significantly reduced, there is a steady-state error in the path tracking process, and the accuracy of the ship heading control is low, the present invention provides an underactuated ship path tracking method based on self-disturbance rejection heading control, based on the actual rudder angle of the underactuated ship, the center coordinates of the ship, the actual heading angle and the combined speed of the ship, and the improved drift angle observer and the improved linear extended state observer are used to estimate the disturbance term in real time, and then the nonlinear state error feedback controller is used to calculate the expected rudder angle based on the self-disturbance rejection heading control, thereby realizing the path tracking of the ship, and effectively solving the underactuation problem of the ship. The present invention also relates to an underactuated ship path tracking system based on self-disturbance rejection heading control.
[0006] The technical solution of the present invention is as follows:
[0007] A path tracking method for an underactuated ship based on active disturbance rejection heading control, characterized in that it comprises the following steps:
[0008] Coordinate system establishment and data acquisition steps: obtain the actual rudder angle of the under-actuated ship at the current moment through the steering wheel sensor, establish an inertial coordinate system with a fixed point on the ground as the origin, obtain the target path of the under-actuated ship, the center coordinates of the ship at the current moment and the actual heading angle in the inertial coordinate system, and obtain the coordinates of the initial path point on the target path; then establish a motion coordinate system with the center coordinates of the ship as the origin, and obtain the lateral speed and longitudinal speed of the under-actuated ship at the current moment in the motion coordinate system, and calculate the total speed of the ship at the current moment based on the lateral speed and longitudinal speed;
[0009] The following are the steps for calculating the tracking error and the expected heading angle: the coordinates of the path points on the target path corresponding to the actual position of the ship at the next moment after the initial moment are calculated according to the coordinates of the initial path points, and then the coordinates of the path points on the target path corresponding to the actual position of the ship at all moments are calculated, and then the path tangent angle is calculated according to the coordinates of a certain path point at the current moment among all moments, and the lateral tracking error and longitudinal tracking error between the ship and the target path at the current moment are calculated according to the path tangent angle, the coordinates of the path point and the coordinates of the center of the ship; the drift angle estimation value is calculated according to the lateral tracking error, the longitudinal tracking error and the combined speed of the ship by using the improved drift angle observer, and the expected heading angle of the ship at the current moment is calculated according to the drift angle estimation value, the path tangent angle and the lateral tracking error and the integrated line of sight guidance law;
[0010] Signal calculation step: based on the desired heading angle and using the tracking speed factor and the filter factor of the tracking differentiator, a transition signal of the desired heading angle and a differential signal of the transition signal are calculated;
[0011] The estimated value calculation step is as follows: the frequency of the improved linear extended state observer is calculated by using the preset frequency parameter and the preset control period of the improved linear extended state observer, and the characteristic polynomial is obtained according to the error matrix of the improved linear extended state observer, and the first adjustable gain value, the second adjustable gain value and the third adjustable gain value of the improved linear extended state observer are calculated according to the characteristic polynomial and the frequency; then the derivative of the current moment heading angle estimation value is calculated based on the actual heading angle and the first adjustable gain value, the derivative of the current moment heading angular velocity estimation value is calculated based on the actual heading angle, the actual rudder angle and the second adjustable gain value, the derivative of the current moment disturbance term estimation value is calculated based on the actual heading angle and the third adjustable gain value, and the derivative of the current moment heading angle estimation value, the derivative of the heading angular velocity estimation value and the derivative of the disturbance term estimation value are respectively integrated to obtain the current moment heading angle estimation value, the heading angular velocity estimation value and the disturbance term estimation value;
[0012] Path tracking steps: Based on the transition signal of the expected heading angle, the differential signal of the transition signal, the estimated value of the heading angle, the estimated value of the bow angular velocity and the estimated value of the disturbance term, a nonlinear state error feedback controller is used to calculate the expected rudder angle of the ship at the current moment based on the self-disturbance rejection heading control, thereby realizing the path tracking of the underactuated ship.
[0013] Preferably, in the tracking error and expected heading angle calculation step, the derivative of the ship's drift angle estimation value is calculated using an improved drift angle observer based on the lateral tracking error, the longitudinal tracking error and the combined speed of the ship, and the derivative of the drift angle estimation value is integrated to obtain the drift angle estimation value; the improved drift angle observer includes a gain value of the improved drift angle observer that is adaptively updated based on the lateral tracking error and the longitudinal tracking error, and when the derivative of the ship's drift angle estimation value is calculated using the improved drift angle observer, when the lateral tracking error and / or the longitudinal tracking error between the ship and the target path point increases, the gain value will decrease to prevent integral saturation, and when the lateral tracking error and / or the longitudinal tracking error decreases, the gain value will increase to accelerate the convergence speed of the error.
[0014] Preferably, in the tracking error and expected heading angle calculation step, the coordinates of the path points on the target path corresponding to the actual position of the ship at the next moment after the initial moment are calculated according to the coordinates of the initial path points according to the following steps:
[0015] S1: Take the initial path point on the target path as the starting point, and the time corresponding to the initial path point as the initial time, and calculate the path tangent angle according to the starting point coordinates;
[0016] S2: Calculate the lateral tracking error and longitudinal tracking error between the ship and the target path at the initial moment according to the path tangent angle, the starting point coordinates and the ship center coordinates; then calculate the target path point speed at the initial moment according to the lateral tracking error, the longitudinal tracking error and the combined speed of the ship at the initial moment, and calculate the path point coordinates on the target path corresponding to the actual position of the ship at the next moment after the initial moment according to the target path point speed;
[0017] S3: Take the calculated coordinates of the path point on the target path at the next moment of the initial moment as the new starting point coordinates, and repeat steps S1 and S2 based on the new starting point coordinates until the coordinates of the path point on the target path corresponding to the actual position of the ship at all moments are calculated.
[0018] Preferably, in the path tracking step, the use of a nonlinear state error feedback controller to calculate the desired rudder angle of the ship at the current moment based on the anti-disturbance heading control specifically includes: the nonlinear state error feedback controller calculates the heading angle deviation according to the transition signal of the desired heading angle and the estimated value of the heading angle, calculates the bow angular velocity deviation according to the differential signal of the transition signal and the estimated value of the bow angular velocity, and calculates the desired rudder angle of the ship at the current moment based on the anti-disturbance heading control according to the estimated value of the disturbance term, the estimated value of the bow angular velocity, the heading angle deviation and the bow angular velocity deviation.
[0019] Preferably, in the coordinate system establishment and data acquisition steps, the origin of the inertial coordinate system is a fixed point on the earth, with its x-axis pointing to due north, its y-axis pointing to due east, and its z-axis pointing to the center of the earth; the origin of the motion coordinate system is located at the center of the ship, with its x-axis pointing to the bow, its y-axis pointing to the starboard side of the hull, and its z-axis pointing to the center of the earth.
[0020] An underactuated ship path tracking system based on active disturbance rejection heading control is characterized by comprising an integral line of sight guidance law module and an active disturbance rejection heading controller, wherein the integral line of sight guidance law module comprises a coordinate system establishment and data acquisition submodule and a tracking error and desired heading angle calculation submodule, wherein the active disturbance rejection heading controller comprises a tracking differentiator module, an improved linear extended state observer module and a nonlinear state error feedback controller module, wherein the coordinate system establishment and data acquisition submodule, the tracking error and desired heading angle calculation submodule and the tracking differentiator module are connected in sequence, wherein the improved linear extended state observer module is connected to the coordinate system establishment and data acquisition submodule, wherein the nonlinear state error feedback controller module is connected to the tracking differentiator module and the improved linear extended state observer module respectively,
[0021] The coordinate system establishment and data acquisition submodule in the integrated line of sight guidance law module obtains the actual rudder angle of the underactuated ship at the current moment through the steering wheel sensor, establishes an inertial coordinate system with a fixed point on the ground as the origin, obtains the target path of the underactuated ship, the center coordinates of the ship at the current moment and the actual heading angle in the inertial coordinate system, and obtains the coordinates of the initial path point on the target path; then establishes a motion coordinate system with the center coordinates of the ship as the origin, and obtains the lateral speed and longitudinal speed of the underactuated ship at the current moment in the motion coordinate system, and calculates the combined speed of the ship at the current moment according to the lateral speed and the longitudinal speed;
[0022] The tracking error and expected heading angle calculation submodule in the integrated line of sight guidance law module calculates the coordinates of the path points on the target path corresponding to the actual position of the ship at the next moment after the initial moment according to the coordinates of the initial path points, and then calculates the coordinates of the path points on the target path corresponding to the actual position of the ship at all moments, and then calculates the path tangent angle according to the coordinates of a certain path point at the current moment among all moments, and calculates the lateral tracking error and the longitudinal tracking error between the ship and the target path at the current moment according to the path tangent angle, the coordinates of the path point and the center coordinates of the ship; according to the lateral tracking error, the longitudinal tracking error and the combined speed of the ship, an improved drift angle observer is used to calculate the drift angle estimation value, and according to the drift angle estimation value, the path tangent angle and the lateral tracking error and the integrated line of sight guidance law, the expected heading angle of the ship at the current moment is calculated;
[0023] The tracking differentiator module calculates a transition signal of the desired heading angle and a differential signal of the transition signal based on the desired heading angle and using a tracking speed factor and a filter factor of the tracking differentiator;
[0024] The improved linear extended state observer module calculates the frequency of the improved linear extended state observer through the preset frequency parameter and the preset control period of the improved linear extended state observer, obtains the characteristic polynomial according to the error matrix of the improved linear extended state observer, and calculates the first adjustable gain value, the second adjustable gain value and the third adjustable gain value of the improved linear extended state observer according to the characteristic polynomial and the frequency; then calculates the derivative of the current moment heading angle estimation value based on the actual heading angle and the first adjustable gain value, calculates the derivative of the current moment heading angular velocity estimation value based on the actual heading angle, the actual rudder angle and the second adjustable gain value, calculates the derivative of the current moment disturbance term estimation value based on the actual heading angle and the third adjustable gain value, and respectively performs integral operations on the derivative of the current moment heading angle estimation value, the derivative of the current moment heading angular velocity estimation value and the derivative of the disturbance term estimation value to obtain the current moment heading angle estimation value, the heading angular velocity estimation value and the disturbance term estimation value;
[0025] The nonlinear state error feedback controller module is based on the transition signal of the expected heading angle, the differential signal of the transition signal, the heading angle estimation value, the bow angular velocity estimation value and the disturbance term estimation value, and calculates the expected rudder angle of the ship at the current moment based on the anti-disturbance heading control through the nonlinear state error feedback controller, thereby realizing the path tracking of the underactuated ship.
[0026] Preferably, in the tracking error and expected heading angle calculation submodule, the derivative of the ship's drift angle estimation value is calculated using an improved drift angle observer based on the lateral tracking error, the longitudinal tracking error and the combined speed of the ship, and the derivative of the drift angle estimation value is integrated to obtain the drift angle estimation value; the improved drift angle observer includes a gain value of the improved drift angle observer that is adaptively updated based on the lateral tracking error and the longitudinal tracking error; when the derivative of the ship's drift angle estimation value is calculated using the improved drift angle observer, when the lateral tracking error and / or the longitudinal tracking error between the ship and the target path point increases, the gain value will decrease to prevent integral saturation, and when the lateral tracking error and / or the longitudinal tracking error decreases, the gain value will increase to accelerate the convergence speed of the error.
[0027] Preferably, in the tracking error and expected heading angle calculation submodule, when the coordinates of the path points on the target path corresponding to the actual position of the ship at the next moment after the initial moment are calculated based on the coordinates of the initial path points, the calculation is performed according to the following steps:
[0028] S1: Take the initial path point on the target path as the starting point, and the time corresponding to the initial path point as the initial time, and calculate the path tangent angle according to the starting point coordinates;
[0029] S2: Calculate the lateral tracking error and longitudinal tracking error between the ship and the target path at the initial moment according to the path tangent angle, the starting point coordinates and the ship center coordinates; then calculate the target path point speed at the initial moment according to the lateral tracking error, the longitudinal tracking error and the combined speed of the ship at the initial moment, and calculate the path point coordinates on the target path corresponding to the actual position of the ship at the next moment after the initial moment according to the target path point speed;
[0030] S3: Take the calculated coordinates of the path point on the target path at the next moment of the initial moment as the new starting point coordinates, and repeat steps S1 and S2 based on the new starting point coordinates until the coordinates of the path point on the target path corresponding to the actual position of the ship at all moments are calculated.
[0031] Preferably, in the nonlinear state error feedback controller module, the nonlinear state error feedback controller is used to calculate the expected rudder angle of the ship at the current moment based on the anti-disturbance heading control, specifically including: the nonlinear state error feedback controller calculates the heading angle deviation according to the transition signal of the expected heading angle and the estimated value of the heading angle, calculates the bow angular velocity deviation according to the differential signal of the transition signal and the estimated value of the bow angular velocity, and calculates the expected rudder angle of the ship at the current moment based on the anti-disturbance heading control according to the estimated value of the disturbance term, the estimated value of the bow angular velocity, the heading angle deviation and the bow angular velocity deviation.
[0032] Preferably, the origin of the inertial coordinate system is a fixed point on the earth, the x-axis points to the north, the y-axis points to the east, and the z-axis points to the center of the earth; the origin of the motion coordinate system is located at the center of the ship, the x-axis points to the bow, the y-axis points to the starboard side of the hull, and the z-axis points to the center of the earth.
[0033] The beneficial effects of the present invention are:
[0034] The present invention provides an underactuated ship path tracking method based on anti-disturbance heading control. Based on the actual rudder angle of the ship, the center coordinates of the ship, the actual heading angle and the combined speed of the ship, a specific calculation method is used to calculate the position error between the ship and the target path, and an improved drift angle observer is used to calculate the drift angle estimation value according to the lateral tracking error, the longitudinal tracking error and the combined speed of the ship. The drift angle is compensated in the form of integration, thereby eliminating the steady-state error existing in the ship during the path tracking process; and the derivative of the ship drift angle estimation value is calculated by the designed improved drift angle observer with a gain value that decreases with the increase of the lateral error and the longitudinal error. , which can reduce the adverse effects of integral saturation on control accuracy by gain parameters, and can reduce the position overshoot phenomenon that is easy to occur when the ship is tracking the target path; then, the expected heading angle of the ship is calculated based on the drift angle estimation value, the path tangential angle and the longitudinal tracking error and the integral line of sight guidance law, and the under-actuation problem of the ship is effectively solved by converting the position error between the ship and the target path into the expected heading angle; then, based on the expected heading angle and using a tracking differentiator to generate the transition signal of the expected heading angle and the differential signal of the transition signal, the process of ship heading control can be made smoother; then, the frequency parameters and control of the extended state observer are designed The improved linear extended state observer is constructed based on the corresponding relationship between the periods. The estimated value of the heading angle, the estimated value of the bow angular velocity and the estimated value of the disturbance term are calculated based on the actual heading angle and the actual rudder angle through the improved linear extended state observer, which effectively improves the robustness of the controller. The frequency of the extended state observer is adaptively adjusted with the control period. The environmental interference is estimated and compensated by the improved linear extended observer. Therefore, the accuracy of the interference estimation by the linear extended state observer will eventually affect the accuracy of the ship's heading control. However, the observation effect of the same linear extended state observer frequency for different control periods is quite different. The present invention designs The correspondence between the frequency and control period of the improved linear extended state observer is improved, which effectively enhances the estimation accuracy of the improved linear extended state observer under different control periods; finally, based on the transition signal of the expected heading angle, the differential signal of the transition signal, the heading angle estimate, the bow angular velocity estimate and the disturbance term estimate, the expected rudder angle of the ship at the current moment based on the self-anti-disturbance heading control is calculated through a nonlinear state error feedback controller, that is, a rudder angle output that is not affected by disturbances is obtained, thereby realizing the path tracking of the ship, solving the problem of accurate path tracking of under-actuated ships under environmental interference, and improving the overall safety and efficiency of transportation.
[0035] The present invention also relates to an underactuated ship path tracking system based on auto-disturbance rejection heading control, which corresponds to the above-mentioned underactuated ship path tracking method based on auto-disturbance rejection heading control, and can be understood as a system that implements the above-mentioned underactuated ship path tracking method based on auto-disturbance rejection heading control, and can also be called an underactuated ship path tracking system based on an auto-disturbance rejection heading controller, including an integral line of sight guidance law module and an auto-disturbance rejection heading controller, the integral line of sight guidance law module includes a coordinate system establishment and data acquisition submodule and a tracking error and expected heading angle calculation submodule, the auto-disturbance rejection heading controller includes a tracking differentiator module, an improved linear expansion state observer module and a nonlinear state error feedback controller module, each module works in coordination with each other, based on the actual rudder angle of the ship , ship center coordinates, actual heading angle and ship total speed, and adopt the integral line of sight guidance law in which the gain value of the drift angle observer adaptively changes with the tracking error and the improved linear extended state observer to estimate the disturbance term in real time, and then use the nonlinear state error feedback controller to calculate the desired rudder angle based on the self-disturbance rejection heading control, so as to realize the path tracking of the ship. In view of the position overshoot phenomenon that is easy to occur in the process of ship tracking path, the adaptive drift angle observer gain is used to reduce the adverse effect of integral saturation; the improved linear extended state observer module estimates the bow angular velocity and environmental interference according to the actual heading angle and rudder angle of the ship, so as to improve the robustness of the controller; the tracking differentiator module provides two signals: the transition signal of the desired heading angle, and the differential signal of the transition signal. The use of a tracking differentiator to generate two signals instead of the original desired heading angle signal can make the ship's heading control process smoother; the nonlinear state error feedback controller module obtains a rudder angle output that is not affected by disturbances based on the estimated values of the bow angular velocity and environmental interference, the transition signal of the desired heading angle, and the differential signal of the transition signal, eliminating the steady-state error existing in the ship's path tracking process, and can gain parameters to reduce the adverse effects of integral saturation on control accuracy, and can reduce the position overshoot phenomenon that is prone to occur in the process of tracking the target path, effectively solving the under-driven problem of the ship. BRIEF DESCRIPTION OF THE DRAWINGS
[0036] Figure 1 It is a flow chart of the underactuated ship path tracking method based on self-disturbance rejection heading control of the present invention.
[0037] Figure 2 It is a schematic diagram of the inertial coordinate system and the motion coordinate system of the present invention.
[0038] Figure 3 It is a schematic diagram of the target path of the present invention.
[0039] Figure 4 It is a structural block diagram of the underactuated ship path tracking system based on self-disturbance rejection heading control of the present invention. DETAILED DESCRIPTION
[0040] The present invention will be described below in conjunction with the accompanying drawings.
[0041] The present invention relates to a path tracking method for an underactuated ship based on anti-disturbance heading control, which is used to solve the problem of accurate path tracking of an underactuated ship under environmental interference. The flow chart of the method is as follows: Figure 1 As shown, the following steps are included in sequence:
[0042] 1. Integral line-of-sight guidance law step, which includes coordinate system establishment and data acquisition step and tracking error and expected heading angle calculation step. Among them, the coordinate system establishment and data acquisition step: obtain the actual rudder angle of the underactuated ship at the current moment through the steering wheel sensor, establish an inertial coordinate system with a fixed point on the ground as the origin, obtain the target path of the underactuated ship, the center coordinates of the ship at the current moment and the actual heading angle in the inertial coordinate system, and obtain the coordinates of the initial path point on the target path; then establish a motion coordinate system with the center coordinates of the ship as the origin, and obtain the lateral speed and longitudinal speed of the underactuated ship at the current moment in the motion coordinate system, and calculate the total speed of the ship at the current moment based on the lateral speed and longitudinal speed.
[0043] Specifically, firstly, the actual rudder angle δ of the underactuated ship at the current moment is obtained through the steering wheel sensor on the ship, and the following formula is constructed: Figure 2 The two right-handed coordinate systems shown: the inertial coordinate system o0-x0y0z0 and the kinematic coordinate system o b -x b y b z b Among them, the inertial coordinate system is also called the geodetic coordinate system, which is fixed on the surface of the earth. The inertial coordinate system o0-x0y0z0 coincides with the still water surface, the origin o0 is a fixed point on the earth, the x0 axis points to the due north, the y0 axis points to the due east, and the z0 axis is vertically downward (that is, pointing to the center of the earth), and then through Figure 2 The ship automatic identification system (AIS) in the ship motion system shown in the figure obtains the target path of the underactuated ship in the inertial coordinate system, the ship center coordinates (x, y) at the current moment, and the actual heading angle ψ. The motion coordinate system is also called the hull coordinate system, which is fixed in the ship. b -x b y b The plane coincides with the still water surface, and the origin is o b Located in the center of the ship, x b The y axis points to the bow direction. b The axis points to the starboard side of the ship, z bThe axis is vertically downward and positive (i.e. pointing to the center of the earth). The lateral velocity v and longitudinal velocity u of the under-actuated ship at the current moment are obtained through the ship automatic identification system (AIS) in the motion coordinate system. The combined velocity of the ship at the current moment is calculated based on the lateral velocity and longitudinal velocity. Figure 2 In the equation, β = arctan (v / u) is the drift angle, χ = ψ + β is the heading angle, and δ is the rudder angle.
[0044] 2. Calculation steps of tracking error and expected heading angle: Calculate the coordinates of the path points on the target path corresponding to the actual position of the ship at the next moment after the initial moment according to the coordinates of the initial path points, and then calculate the coordinates of the path points on the target path corresponding to the actual position of the ship at all moments, and then calculate the path tangent angle according to the coordinates of a path point at the current moment among all moments, and calculate the lateral tracking error and longitudinal tracking error between the ship and the target path at the current moment according to the path tangent angle, the coordinates of the path point and the coordinates of the center of the ship; Calculate the derivative of the ship's drift angle estimate value according to the lateral tracking error, the longitudinal tracking error and the combined speed of the ship using an improved drift angle observer, and integrate the derivative of the drift angle estimate value to obtain the drift angle estimate value, and calculate the expected heading angle of the ship at the current moment according to the drift angle estimate value, the path tangent angle and the lateral tracking error using the integrated line of sight guidance law.
[0045] Specifically, Figure 3 As shown, firstly, the parameterized target path is obtained through the map and path planning algorithm, which is expressed as: P(θ) = [x r (θ),y r (θ)], where θ ≥ 0, represents the path point variable (i.e., x r ,y r ); then according to the coordinate x of the initial path point on the target path r (θ),y r (θ) Calculate the path tangent angle according to the following formula:
[0046] χ r (θ) = atan2[y′ r (θ),x′ r (θ)] (1)
[0047] in, are all partial derivatives of the path point coordinates with respect to the path variable θ.
[0048] Then, the lateral tracking error y between the ship and the target path at the initial moment is calculated based on the path tangent angle, the coordinates of the initial path point and the center coordinates of the ship. e and the longitudinal tracking error x e, where the longitudinal tracking error x between the ship and the target path is e and the lateral tracking error y e It is expressed as follows:
[0049]
[0050] In the above formula, x, y are the coordinates of the center of the ship, x r (θ),y r (θ) is the coordinate of the path point.
[0051] According to the lateral tracking error y e , longitudinal tracking error x e And the ship's total speed U is used to calculate the speed U of the target path point at the initial moment p , expressed as follows:
[0052]
[0053] Where Δ is the foresight distance. As the foresight distance decreases, the ship converges to the target path faster. However, too large a foresight distance will cause an increase in overshoot and convergence time. Therefore, the foresight distance is generally set to 3 to 5 times the ship length. μ is a positive design parameter.
[0054] The guidance system can be expressed as follows:
[0055]
[0056] Where U p is the speed virtual input of the target path point. The path variable θ is updated according to the following formula:
[0057]
[0058] Where θ0 is the initial value of the path variable.
[0059] Since the actual position of the ship deviates from the target path, there is a path point on the target path corresponding to the actual position of the ship at any time, that is, the position where the ship should be on the target path at the same time. Therefore, based on the target path point speed at the initial moment, the path point coordinates on the target path corresponding to the actual position of the ship at the next moment after the initial moment can be calculated according to formula (5), and then the path point coordinates on the target path corresponding to the actual position of the ship at all times can be calculated.
[0060] Then, in order to reduce the influence of the drift angle on the control effect, according to the lateral tracking error, longitudinal tracking error and combined speed of the ship between the ship and the target path at the current moment, the following improved drift angle observer is designed to calculate the derivative of the estimated value of the ship's drift angle, and the derivative of the estimated value of the ship's drift angle is integrated to obtain the estimated value of the drift angle, which is calculated according to the following formula:
[0061]
[0062] In the formula, is the drift angle estimation value; γ is the gain value of the improved drift angle observer, is the initial value of the drift angle estimation value. It should be noted that in order to reduce the position overshoot phenomenon of the ship during the path tracking process, the gain value γ of the improved drift angle observer is adaptively updated according to the lateral tracking error and the longitudinal tracking error. When the lateral and longitudinal tracking errors between the ship and the target path point increase, the gain value γ will decrease to prevent integral saturation; when the lateral and longitudinal tracking errors decrease, the gain value γ will increase to accelerate the convergence speed of the error, where γ is expressed as follows:
[0063]
[0064] Where k1, k2 and k3 are positive parameters of the design.
[0065] Finally, according to the estimated drift angle, path tangent angle and lateral tracking error y e Calculate the expected heading angle of the ship at the current moment:
[0066]
[0067] If the ship tracks the desired heading angle calculated by equation (8), it will eventually converge to the target path.
[0068] 3. Signal calculation step, which may also be called tracking differentiator step: Based on the desired heading angle and through the tracking speed factor and filter factor of the tracking differentiator, the transition signal of the desired heading angle and the differential signal of the transition signal are calculated.
[0069] Specifically, the tracking differentiator can arrange the transition process in advance to solve the contradiction between overshoot and rapidity in the control process. The mathematical representation of the tracking differentiator is as follows:
[0070]
[0071] Where k represents any discrete moment; r0 is the tracking speed factor of the tracking differentiator; h0 is the filter factor of the tracking differentiator, ignoring signal noise; t sis the control period; fh is the output of the fhan function of the discrete system; x1 and x2 are the transition signal of the desired heading angle obtained by the tracking differentiator and the differential signal of the transition signal. The specific expression of the fastest control comprehensive function fhan(x1,x2,r,h) is as follows:
[0072]
[0073] The desired heading angle is taken as input, and the transition signal x1 of the desired heading angle and the differential signal x2 of the transition signal are output through the tracking differentiator.
[0074] 4. Estimated value calculation step, which can also be called improved linear extended state observer step. The frequency of the improved linear extended state observer is calculated by the preset frequency parameter and the preset control period of the improved linear extended state observer, and the characteristic polynomial is obtained according to the error matrix of the improved linear extended state observer, and the first adjustable gain value, the second adjustable gain value and the third adjustable gain value of the improved linear extended state observer are calculated according to the characteristic polynomial and the frequency; then the derivative of the current moment heading angle estimation value is calculated based on the actual heading angle and the first adjustable gain value, the derivative of the current moment turning angular velocity estimation value is calculated based on the actual heading angle, the actual rudder angle and the second adjustable gain value, the derivative of the current moment disturbance term estimation value is calculated based on the actual heading angle and the third adjustable gain value, and the derivative of the current moment heading angle estimation value, the derivative of the turning angular velocity estimation value and the derivative of the disturbance term estimation value are respectively integrated to obtain the current moment heading angle estimation value, the turning angular velocity estimation value and the disturbance term estimation value.
[0075] Specifically, the improved linear extended state observer is the core component of the ADRC algorithm. It can estimate the disturbance inside and outside the system in real time, and eliminate the influence of the disturbance by compensating in the feedback, and has a strong anti-interference ability. In ship motion control, the Nomoto model is usually used to describe the ship's turning motion. In order to improve the accuracy of ship heading control under environmental interference, a new state w is used to describe the uncertainty and external interference of the ship system:
[0076]
[0077] Where K and T represent the rotation index and stability index respectively.
[0078] The design of the improved linear extended state observer is as follows:
[0079]
[0080]
[0081] In the formula, and They represent the estimated value of the heading angle, the estimated value of the turning angular velocity and the estimated value of the disturbance term at time t = 0 (i.e. the initial time); and They represent the initial values of the heading angle, the heading angular velocity and the disturbance term estimate respectively; the input of the extended state observer is ψ, δ, and the output is β1, β2 and β3 are the first adjustable gain value, the second adjustable gain value and the third adjustable gain value of the improved linear extended state observer, respectively, and they are all positive values. According to the error matrix of the improved linear extended state observer, the characteristic polynomial f(s)=s 3 +β1s 2 +β2s+β3. If the error convergence of the observer is to be guaranteed, the eigenvalues s need to be all located in the left half plane, so f(s)=(s+ω 01 ) 3 , so these three adjustable gain values meet the following conditions:
[0082]
[0083] In the formula, ω 01 is the frequency of the improved linear expansion observer.
[0084] In the actual control process, ω 01 It is impossible to design it to be very large. 01 If ω is too large in a discrete system, the estimated value will oscillate, and the improved linear extended state observer will not be able to accurately estimate the state of the system. 01 When is small, there is a significant error between the actual value and the estimated value. At the same time, since the improved linear extended state observer adjusts the rate of change of the observed value according to the estimated error, in the discrete system, the control period t s The same will be true for ω 01 The choice of has a certain impact. The following relationship between the improved linear extended state observer and the preset control period is constructed:
[0085]
[0086] In the formula, ω b is the preset frequency parameter of the improved linear extended state observer; λ is a positive parameter.
[0087] According to the initial values of the heading angle, the heading angular velocity and the estimated value of the disturbance term, the estimated value of the heading angle, the estimated value of the heading angular velocity and the estimated value of the disturbance term at time t=0 (i.e., the initial time) can be calculated. Then, based on the estimated value of the heading angle, the estimated value of the heading angular velocity and the estimated value of the disturbance term at time t=0, and the actual heading angle and the actual steering angle at the next moment after the initial time, the estimated value of the heading angle, the estimated value of the heading angular velocity and the estimated value of the disturbance term at the next moment after the initial time can be calculated according to formula (12). By repeating the above process, the estimated value of the heading angle, the estimated value of the heading angular velocity and the estimated value of the disturbance term at all times can be calculated. Therefore, the derivative of the estimated value of the heading angle at the current moment is calculated based on the actual heading angle at the current moment and the first adjustable gain value β1, the derivative of the estimated value of the turning angular velocity at the current moment is calculated based on the actual heading angle at the current moment, the actual rudder angle and the second adjustable gain value β2, and the derivative of the estimated value of the disturbance term (or called interference term) at the current moment is calculated based on the actual heading angle at the current moment and the third adjustable gain value β3, and the derivative of the estimated value of the heading angle, the derivative of the estimated value of the turning angular velocity and the derivative of the estimated value of the disturbance term are respectively integrated to obtain the estimated value of the heading angle, the estimated value of the turning angular velocity and the estimated value of the disturbance term at the current moment.
[0088] 5. Path tracking step, which can also be called a nonlinear state error feedback controller step. Based on the transition signal of the desired heading angle, the differential signal of the transition signal, the estimated value of the heading angle, the estimated value of the turning angular velocity, and the estimated value of the disturbance term, the desired rudder angle of the ship at the current moment based on the self-resistance heading control is calculated using a nonlinear state error feedback controller, thereby realizing the path tracking of the underactuated ship. Preferably, the nonlinear state error feedback controller calculates the heading angle deviation according to the transition signal of the desired heading angle and the estimated value of the heading angle, calculates the turning angular velocity deviation according to the differential signal of the transition signal and the estimated value of the turning angular velocity, and calculates the desired rudder angle of the ship at the current moment based on the self-resistance heading control according to the estimated value of the disturbance term, the estimated value of the turning angular velocity, the heading angle deviation, and the turning angular velocity deviation, so as to obtain a rudder angle output that is not affected by disturbances.
[0089] Specifically, a nonlinear state error feedback controller is used to suppress and reduce the influence of disturbance on the control effect. Its mathematical structure is:
[0090]
[0091] Where r1 is the tracking speed factor of the nonlinear state error feedback controller; h1 is the precision factor of the nonlinear state error feedback controller; c is the damping coefficient of the nonlinear state error feedback controller; e1 is the heading angle deviation; e2 is the heading angular velocity deviation.
[0092] The present invention also relates to an underactuated ship path tracking system based on auto-disturbance rejection heading control, which corresponds to the underactuated ship path tracking method based on auto-disturbance rejection heading control, and can be understood as a system that implements the above method, and can also be called an underactuated ship path tracking system based on auto-disturbance rejection heading controller, such as Figure 4 The structure shown in FIG. 1 includes an integral line-of-sight guidance law module and an auto-disturbance rejection heading controller. The integral line-of-sight guidance law module includes a coordinate system establishment and data acquisition submodule and a tracking error and desired heading angle calculation submodule. The auto-disturbance rejection heading controller includes a tracking differentiator module, an improved linear extended state observer module and a nonlinear state error feedback controller module. The coordinate system establishment and data acquisition submodule, the tracking error and desired heading angle calculation submodule and the tracking differentiator module are connected in sequence. The improved linear extended state observer module is connected to the coordinate system establishment and data acquisition submodule. The nonlinear state error feedback controller module is connected to the tracking differentiator module and the improved linear extended state observer module, respectively. Specifically,
[0093] The coordinate system establishment and data acquisition submodule in the integrated line of sight guidance law module obtains the actual rudder angle of the underactuated ship at the current moment through the steering wheel sensor, establishes an inertial coordinate system with a fixed point on the ground as the origin, obtains the target path of the underactuated ship, the center coordinates of the ship at the current moment and the actual heading angle in the inertial coordinate system, and obtains the coordinates of the initial path point on the target path; then establishes a motion coordinate system with the center coordinates of the ship as the origin, and obtains the lateral speed and longitudinal speed of the underactuated ship at the current moment in the motion coordinate system, and calculates the combined speed of the ship at the current moment according to the lateral speed and the longitudinal speed;
[0094] The tracking error and expected heading angle calculation submodule in the integrated line of sight guidance law module calculates the coordinates of the path points on the target path corresponding to the actual position of the ship at the next moment after the initial moment according to the coordinates of the initial path points, and then calculates the coordinates of the path points on the target path corresponding to the actual position of the ship at all moments, and then calculates the path tangent angle according to the coordinates of a certain path point at the current moment among all moments, and calculates the lateral tracking error and the longitudinal tracking error between the ship and the target path at the current moment according to the path tangent angle, the coordinates of the path point and the center coordinates of the ship; according to the lateral tracking error, the longitudinal tracking error and the combined speed of the ship, an improved drift angle observer is used to calculate the drift angle estimation value, and according to the drift angle estimation value, the path tangent angle and the lateral tracking error and the integrated line of sight guidance law, the expected heading angle of the ship at the current moment is calculated;
[0095] The tracking differentiator module calculates a transition signal of the desired heading angle and a differential signal of the transition signal based on the desired heading angle and using a tracking speed factor and a filter factor of the tracking differentiator;
[0096] The improved linear extended state observer module calculates the frequency of the improved linear extended state observer through the preset frequency parameter and the preset control period of the improved linear extended state observer, obtains the characteristic polynomial according to the error matrix of the improved linear extended state observer, and calculates the first adjustable gain value, the second adjustable gain value and the third adjustable gain value of the improved linear extended state observer according to the characteristic polynomial and the frequency; then calculates the derivative of the current moment heading angle estimation value based on the actual heading angle and the first adjustable gain value, calculates the derivative of the current moment heading angular velocity estimation value based on the actual heading angle, the actual rudder angle and the second adjustable gain value, calculates the derivative of the current moment disturbance term estimation value based on the actual heading angle and the third adjustable gain value, and respectively performs integral operations on the derivative of the current moment heading angle estimation value, the derivative of the current moment heading angular velocity estimation value and the derivative of the disturbance term estimation value to obtain the current moment heading angle estimation value, the heading angular velocity estimation value and the disturbance term estimation value;
[0097] The nonlinear state error feedback controller module is based on the transition signal of the expected heading angle, the differential signal of the transition signal, the heading angle estimation value, the bow angular velocity estimation value and the disturbance term estimation value, and calculates the expected rudder angle of the ship at the current moment based on the anti-disturbance heading control through the nonlinear state error feedback controller, thereby realizing the path tracking of the underactuated ship.
[0098] The invention relates to an underactuated ship path tracking system based on self-anti-disturbance heading control. The integral line of sight guidance law module converts the position error between the ship and the target path into an expected heading angle to solve the underactuation problem of the ship. In view of the position overshoot phenomenon that is easy to occur in the process of tracking the path, an adaptive drift angle observer gain is used to reduce the adverse effects of integral saturation. The improved linear expansion state observer module estimates the turning angular velocity and environmental interference according to the actual heading angle and rudder angle of the ship, thereby improving the robustness of the controller. The tracking differentiator module is used to provide two signals: a transition signal of the expected heading angle, and a differential signal of the transition signal. The use of the tracking differentiator to generate two signals instead of the original expected heading angle signal can make the ship heading control process smoother. The nonlinear state error feedback controller module obtains a rudder angle output that is not affected by disturbance according to the estimated values of the turning angular velocity and environmental interference, the transition signal of the expected heading angle, and the differential signal of the transition signal.
[0099] Preferably, in the tracking error and expected heading angle calculation submodule, the derivative of the ship's drift angle estimation value is calculated using an improved drift angle observer based on the lateral tracking error, the longitudinal tracking error and the combined speed of the ship, and the derivative of the drift angle estimation value is integrated to obtain the drift angle estimation value; the improved drift angle observer includes a gain value of the improved drift angle observer that is adaptively updated based on the lateral tracking error and the longitudinal tracking error; when the derivative of the ship's drift angle estimation value is calculated using the improved drift angle observer, when the lateral tracking error and / or the longitudinal tracking error between the ship and the target path point increases, the gain value will decrease to prevent integral saturation, and when the lateral tracking error and / or the longitudinal tracking error decreases, the gain value will increase to accelerate the convergence speed of the error.
[0100] Preferably, in the tracking error and expected heading angle calculation submodule, when the coordinates of the path points on the target path corresponding to the actual position of the ship at the next moment after the initial moment are calculated based on the coordinates of the initial path points, the calculation is performed according to the following steps:
[0101] S1: Take the initial path point on the target path as the starting point, and the time corresponding to the initial path point as the initial time, and calculate the path tangent angle according to the starting point coordinates;
[0102] S2: Calculate the lateral tracking error and longitudinal tracking error between the ship and the target path at the initial moment according to the path tangent angle, the starting point coordinates and the ship center coordinates; then calculate the target path point speed at the initial moment according to the lateral tracking error, the longitudinal tracking error and the combined speed of the ship at the initial moment, and calculate the path point coordinates on the target path corresponding to the actual position of the ship at the next moment after the initial moment according to the target path point speed;
[0103] S3: Take the calculated coordinates of the path point on the target path at the next moment of the initial moment as the new starting point coordinates, and repeat steps S1 and S2 based on the new starting point coordinates until the coordinates of the path point on the target path corresponding to the actual position of the ship at all moments are calculated.
[0104] Preferably, in the nonlinear state error feedback controller module, the nonlinear state error feedback controller is used to calculate the expected rudder angle of the ship at the current moment based on the anti-disturbance heading control, specifically including: the nonlinear state error feedback controller calculates the heading angle deviation according to the transition signal of the expected heading angle and the estimated value of the heading angle, calculates the bow angular velocity deviation according to the differential signal of the transition signal and the estimated value of the bow angular velocity, and calculates the expected rudder angle of the ship at the current moment based on the anti-disturbance heading control according to the estimated value of the disturbance term, the estimated value of the bow angular velocity, the heading angle deviation and the bow angular velocity deviation.
[0105] Preferably, the origin of the inertial coordinate system is a fixed point on the earth, the x-axis points to the north, the y-axis points to the east, and the z-axis points to the center of the earth; the origin of the motion coordinate system is located at the center of the ship, the x-axis points to the bow, the y-axis points to the starboard side of the hull, and the z-axis points to the center of the earth.
[0106] Compared with the prior art, the present invention provides an objective and scientific underactuated ship path tracking method and system based on anti-disturbance heading control. Based on the actual rudder angle of the ship, the center coordinates of the ship, the actual heading angle and the combined speed of the ship, the drift angle observer, the tracking differentiator, the linear extended state observer, the nonlinear state error feedback controller and the specific calculation method are used to calculate the expected rudder angle, thereby realizing the path tracking of the ship, and effectively solving the underactuated problem of the ship. The integral line of sight guidance law in which the drift angle observer gain changes adaptively with the tracking error: the drift angle is compensated in the form of integral to eliminate the steady-state error existing in the ship during the path tracking process. In order to reduce the position overshoot that is easy to occur when the ship uses the integral line of sight guidance method to track the target path, the drift angle observer gain parameter that decreases with the increase of the lateral error and the longitudinal error is used to reduce the adverse effect of integral saturation on the control accuracy. The linear extended state observer frequency that is adaptively adjusted with the control period: the environmental interference is estimated and compensated by the linear extended state observer, so the accuracy of the interference estimation by the linear extended state observer will eventually affect the accuracy of the ship's heading control. However, the observation effect of the same extended state observer frequency is quite different for different control periods. Therefore, a corresponding relationship between the frequency of the extended state observer and the control period is designed to enhance the accuracy of the linear extended state observer estimation under different control periods.
[0107] It should be noted that the above-described specific implementations can enable those skilled in the art to more fully understand the invention, but do not limit the invention in any way. Therefore, although this specification has described the invention in detail with reference to the drawings and embodiments, those skilled in the art should understand that the invention can still be modified or replaced by equivalents. In short, all technical solutions and improvements that do not deviate from the spirit and scope of the invention should be included in the protection scope of the patent for the invention.
Claims
1. A path tracking method for underactuated ships based on anti-disturbance heading control. It is characterized in that The following steps are involved: Coordinate system establishment and data acquisition steps: obtain the actual rudder angle of the under-actuated ship at the current moment through the steering wheel sensor, establish an inertial coordinate system with a fixed point on the ground as the origin, obtain the target path of the under-actuated ship, the center coordinates of the ship at the current moment and the actual heading angle in the inertial coordinate system, and obtain the coordinates of the initial path point on the target path; then establish a motion coordinate system with the center coordinates of the ship as the origin, and obtain the lateral speed and longitudinal speed of the under-actuated ship at the current moment in the motion coordinate system, and calculate the total speed of the ship at the current moment based on the lateral speed and longitudinal speed; The following are the steps for calculating the tracking error and the expected heading angle: the coordinates of the path points on the target path corresponding to the actual position of the ship at the next moment after the initial moment are calculated according to the coordinates of the initial path points, and then the coordinates of the path points on the target path corresponding to the actual position of the ship at all moments are calculated, and then the path tangent angle is calculated according to the coordinates of a certain path point at the current moment among all moments, and the lateral tracking error and longitudinal tracking error between the ship and the target path at the current moment are calculated according to the path tangent angle, the coordinates of the path point and the coordinates of the center of the ship; the drift angle estimation value is calculated according to the lateral tracking error, the longitudinal tracking error and the combined speed of the ship by using the improved drift angle observer, and the expected heading angle of the ship at the current moment is calculated according to the drift angle estimation value, the path tangent angle and the lateral tracking error and the integrated line of sight guidance law; Signal calculation step: based on the desired heading angle and using the tracking speed factor and the filter factor of the tracking differentiator, a transition signal of the desired heading angle and a differential signal of the transition signal are calculated; The estimated value calculation step is as follows: the frequency of the improved linear extended state observer is calculated by using the preset frequency parameter and the preset control period of the improved linear extended state observer, and the characteristic polynomial is obtained according to the error matrix of the improved linear extended state observer, and the first adjustable gain value, the second adjustable gain value and the third adjustable gain value of the improved linear extended state observer are calculated according to the characteristic polynomial and the frequency; then the derivative of the current moment heading angle estimation value is calculated based on the actual heading angle and the first adjustable gain value, the derivative of the current moment heading angular velocity estimation value is calculated based on the actual heading angle, the actual rudder angle and the second adjustable gain value, the derivative of the current moment disturbance term estimation value is calculated based on the actual heading angle and the third adjustable gain value, and the derivative of the current moment heading angle estimation value, the derivative of the heading angular velocity estimation value and the derivative of the disturbance term estimation value are respectively integrated to obtain the current moment heading angle estimation value, the heading angular velocity estimation value and the disturbance term estimation value; Path tracking steps: Based on the transition signal of the expected heading angle, the differential signal of the transition signal, the estimated value of the heading angle, the estimated value of the bow angular velocity and the estimated value of the disturbance term, a nonlinear state error feedback controller is used to calculate the expected rudder angle of the ship at the current moment based on the self-disturbance rejection heading control, thereby realizing the path tracking of the underactuated ship.
2. The underactuated ship path tracking method based on active disturbance rejection heading control according to claim 1, It is characterized in that In the tracking error and expected heading angle calculation step, the derivative of the ship drift angle estimation value is calculated by using an improved drift angle observer according to the lateral tracking error, the longitudinal tracking error and the combined speed of the ship, and the derivative of the drift angle estimation value is integrated to obtain the drift angle estimation value; the improved drift angle observer includes a gain value of the improved drift angle observer that is adaptively updated according to the lateral tracking error and the longitudinal tracking error. When the derivative of the ship drift angle estimation value is calculated by using the improved drift angle observer, when the lateral tracking error and / or the longitudinal tracking error between the ship and the target path point increases, the gain value will decrease to prevent integral saturation, and when the lateral tracking error and / or the longitudinal tracking error decreases, the gain value will increase to accelerate the convergence speed of the error.
3. The underactuated ship path tracking method based on active disturbance rejection heading control according to claim 1 or 2, It is characterized in that In the tracking error and expected heading angle calculation step, the coordinates of the path points on the target path corresponding to the actual position of the ship at the next moment of the initial moment are calculated according to the coordinates of the initial path points, and the calculation is performed according to the following steps: S1: Take the initial path point on the target path as the starting point, and the time corresponding to the initial path point as the initial time, and calculate the path tangent angle according to the starting point coordinates; S2: Calculate the lateral tracking error and longitudinal tracking error between the ship and the target path at the initial moment according to the path tangent angle, the starting point coordinates and the ship center coordinates; then calculate the target path point speed at the initial moment according to the lateral tracking error, the longitudinal tracking error and the combined speed of the ship at the initial moment, and calculate the path point coordinates on the target path corresponding to the actual position of the ship at the next moment after the initial moment according to the target path point speed; S3: Take the calculated coordinates of the path point on the target path at the next moment of the initial moment as the new starting point coordinates, and repeat steps S1 and S2 based on the new starting point coordinates until the coordinates of the path point on the target path corresponding to the actual position of the ship at all moments are calculated.
4. The underactuated ship path tracking method based on active disturbance rejection heading control according to claim 1 or 2, It is characterized in that In the path tracking step, the nonlinear state error feedback controller is used to calculate the expected rudder angle of the ship at the current moment based on the anti-disturbance heading control, which specifically includes: the nonlinear state error feedback controller calculates the heading angle deviation according to the transition signal of the expected heading angle and the estimated value of the heading angle, calculates the bow angular velocity deviation according to the differential signal of the transition signal and the estimated value of the bow angular velocity, and calculates the expected rudder angle of the ship at the current moment based on the anti-disturbance heading control according to the estimated value of the disturbance term, the estimated value of the bow angular velocity, the heading angle deviation and the bow angular velocity deviation.
5. The underactuated ship path tracking method based on active disturbance rejection heading control according to claim 1, It is characterized in that In the coordinate system establishment and data acquisition steps, the origin of the inertial coordinate system is a fixed point on the earth, its x-axis points to the north, its y-axis points to the east, and its z-axis points to the center of the earth; the origin of the motion coordinate system is located at the center of the ship, its x-axis points to the bow, its y-axis points to the starboard side of the hull, and its z-axis points to the center of the earth.
6. An underactuated ship path tracking system based on anti-disturbance heading control, It is characterized in that The invention comprises an integral line-of-sight guidance law module and an auto-disturbance rejection heading controller, wherein the integral line-of-sight guidance law module comprises a coordinate system establishment and data acquisition submodule and a tracking error and desired heading angle calculation submodule, and the auto-disturbance rejection heading controller comprises a tracking differentiator module, an improved linear extended state observer module and a nonlinear state error feedback controller module, wherein the coordinate system establishment and data acquisition submodule, the tracking error and desired heading angle calculation submodule and the tracking differentiator module are connected in sequence, the improved linear extended state observer module is connected to the coordinate system establishment and data acquisition submodule, and the nonlinear state error feedback controller module is connected to the tracking differentiator module and the improved linear extended state observer module respectively. The coordinate system establishment and data acquisition submodule in the integrated line of sight guidance law module obtains the actual rudder angle of the underactuated ship at the current moment through the steering wheel sensor, establishes an inertial coordinate system with a fixed point on the ground as the origin, obtains the target path of the underactuated ship, the center coordinates of the ship at the current moment and the actual heading angle in the inertial coordinate system, and obtains the coordinates of the initial path point on the target path; then establishes a motion coordinate system with the center coordinates of the ship as the origin, and obtains the lateral speed and longitudinal speed of the underactuated ship at the current moment in the motion coordinate system, and calculates the combined speed of the ship at the current moment according to the lateral speed and the longitudinal speed; The tracking error and expected heading angle calculation submodule in the integrated line of sight guidance law module calculates the coordinates of the path points on the target path corresponding to the actual position of the ship at the next moment after the initial moment according to the coordinates of the initial path points, and then calculates the coordinates of the path points on the target path corresponding to the actual position of the ship at all moments, and then calculates the path tangent angle according to the coordinates of a certain path point at the current moment among all moments, and calculates the lateral tracking error and the longitudinal tracking error between the ship and the target path at the current moment according to the path tangent angle, the coordinates of the path point and the center coordinates of the ship; according to the lateral tracking error, the longitudinal tracking error and the combined speed of the ship, an improved drift angle observer is used to calculate the drift angle estimation value, and according to the drift angle estimation value, the path tangent angle and the lateral tracking error and the integrated line of sight guidance law, the expected heading angle of the ship at the current moment is calculated; The tracking differentiator module calculates a transition signal of the desired heading angle and a differential signal of the transition signal based on the desired heading angle and using a tracking speed factor and a filter factor of the tracking differentiator; The improved linear extended state observer module calculates the frequency of the improved linear extended state observer through the preset frequency parameter and the preset control period of the improved linear extended state observer, obtains the characteristic polynomial according to the error matrix of the improved linear extended state observer, and calculates the first adjustable gain value, the second adjustable gain value and the third adjustable gain value of the improved linear extended state observer according to the characteristic polynomial and the frequency; then calculates the derivative of the current moment heading angle estimation value based on the actual heading angle and the first adjustable gain value, calculates the derivative of the current moment heading angular velocity estimation value based on the actual heading angle, the actual rudder angle and the second adjustable gain value, calculates the derivative of the current moment disturbance term estimation value based on the actual heading angle and the third adjustable gain value, and respectively performs integral operations on the derivative of the current moment heading angle estimation value, the derivative of the current moment heading angular velocity estimation value and the derivative of the disturbance term estimation value to obtain the current moment heading angle estimation value, the heading angular velocity estimation value and the disturbance term estimation value; The nonlinear state error feedback controller module is based on the transition signal of the expected heading angle, the differential signal of the transition signal, the heading angle estimation value, the bow angular velocity estimation value and the disturbance term estimation value, and calculates the expected rudder angle of the ship at the current moment based on the anti-disturbance heading control through the nonlinear state error feedback controller, thereby realizing the path tracking of the underactuated ship.
7. The underactuated ship path tracking system based on active disturbance rejection heading control according to claim 6, It is characterized in that In the tracking error and expected heading angle calculation submodule, the derivative of the ship drift angle estimation value is calculated by using an improved drift angle observer according to the lateral tracking error, the longitudinal tracking error and the combined speed of the ship, and the derivative of the drift angle estimation value is integrated to obtain the drift angle estimation value; the improved drift angle observer includes a gain value of the improved drift angle observer that is adaptively updated according to the lateral tracking error and the longitudinal tracking error. When the derivative of the ship drift angle estimation value is calculated by using the improved drift angle observer, when the lateral tracking error and / or the longitudinal tracking error between the ship and the target path point increases, the gain value will decrease to prevent integral saturation, and when the lateral tracking error and / or the longitudinal tracking error decreases, the gain value will increase to accelerate the convergence speed of the error.
8. The underactuated ship path tracking system based on active disturbance rejection heading control according to claim 6 or 7, It is characterized in that In the tracking error and expected heading angle calculation submodule, when the coordinates of the path points on the target path corresponding to the actual position of the ship at the next moment of the initial moment are calculated based on the coordinates of the initial path points, the calculation is performed according to the following steps: S1: Take the initial path point on the target path as the starting point, and the time corresponding to the initial path point as the initial time, and calculate the path tangent angle according to the starting point coordinates; S2: Calculate the lateral tracking error and longitudinal tracking error between the ship and the target path at the initial moment according to the path tangent angle, the starting point coordinates and the ship center coordinates; then calculate the target path point speed at the initial moment according to the lateral tracking error, the longitudinal tracking error and the combined speed of the ship at the initial moment, and calculate the path point coordinates on the target path corresponding to the actual position of the ship at the next moment after the initial moment according to the target path point speed; S3: Take the calculated coordinates of the path point on the target path at the next moment of the initial moment as the new starting point coordinates, and repeat steps S1 and S2 based on the new starting point coordinates until the coordinates of the path point on the target path corresponding to the actual position of the ship at all moments are calculated.
9. The underactuated ship path tracking system based on active disturbance rejection heading control according to claim 6 or 7, It is characterized in that In the nonlinear state error feedback controller module, the nonlinear state error feedback controller is used to calculate the expected rudder angle of the ship at the current moment based on the anti-disturbance heading control, specifically including: the nonlinear state error feedback controller calculates the heading angle deviation according to the transition signal of the expected heading angle and the estimated value of the heading angle, calculates the bow angular velocity deviation according to the differential signal of the transition signal and the estimated value of the bow angular velocity, and calculates the expected rudder angle of the ship at the current moment based on the anti-disturbance heading control according to the estimated value of the disturbance term, the estimated value of the bow angular velocity, the heading angle deviation and the bow angular velocity deviation.
10. The underactuated ship path tracking system based on active disturbance rejection heading control according to claim 6, It is characterized in that The origin of the inertial coordinate system is a fixed point on the earth, the x-axis points to the north, the y-axis points to the east, and the z-axis points to the center of the earth; the origin of the motion coordinate system is located at the center of the ship, the x-axis points to the bow, the y-axis points to the starboard side of the hull, and the z-axis points to the center of the earth.
Citation Information
Cited By
Hovercraft path tracking method, device and equipment based on reinforcement learning and storage medium
CN119668271A
