A method and device for controlling fixed-point return of a catamaran unmanned ship
By calculating and applying the return control parameters and environmental compensation parameters in real time, the compensation value is estimated using the Kalman filter to achieve rapid fixed-point return of the unmanned ship, solving the problem of deviation of the unmanned ship under wind and wave disturbances, and improving the efficiency of fixed-point mooring.
Patent Information
- Application Number
- CN202210662859.3
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2022-06-13
- Publication Date
- 2024-11-26
- Estimated Expiration
- 2042-06-13
AI Technical Summary
Unmanned ships are prone to deviations from wind, waves and flow when parked at fixed points. The existing technology requires planning new driving routes many times, which increases workload and time-consuming and has low efficiency.
The fixed-point return control method of the catamaran unmanned ship is adopted to calculate the return control parameters and environmental disturbance compensation parameters in real time, and the compensation rate and heading angle values are estimated through the Kalman filter. Based on these parameters, the unmanned ship returns to fixed-points in real time to reduce disturbances. Influence, shorten the regulation time.
The unmanned ship can return to fixed points quickly and smoothly, avoid multiple adjustments to the route, improve regulation efficiency, and reduce the impact of environmental disturbances on return.
Smart Images

Figure CN115014355B_ABST
Abstract
Description
Background Art
[0002] An unmanned ship is a surface vehicle that has the ability to autonomously navigate or remotely control the ship to achieve normal navigation, manipulation and operation. It can carry various mission payloads to perform designated tasks. Due to its advantages such as safe operation, high efficiency and energy saving, and low cost, it is widely used in the field of automated operations in waters.
[0003] Since unmanned boats are easily affected by various unstable factors in the water (such as wind, waves, and current disturbances), they are prone to deviation when moored at a fixed point, causing the unmanned boat to gradually move away from its target position or its current positioning point. In order to allow the unmanned boat to be fixed at a specific location, the commonly used method is: when the unmanned boat is found to be offset, the current position of the unmanned boat is determined, and then the driving route is replanned based on the current position and the target position (or target positioning point) so that the unmanned boat can sail to the target positioning point.
[0004] However, the commonly used methods currently have the following technical problems: although the route is corrected to allow the unmanned ship to sail, the unmanned ship may still be affected by various unstable factors during its return journey, causing the unmanned ship to deviate from its route again and be unable to sail to the fixed location, and then it is necessary to repeatedly plan a new route for the unmanned ship, which not only increases the workload of correction and data processing, but also the efficiency of route planning and adjustment is low, which increases the time spent on navigation. Summary of the invention
[0005] The present invention proposes a method and device for controlling the fixed-point return of a catamaran unmanned ship. The method can calculate in real time the control parameters required for the return and the return compensation parameters that can cope with environmental disturbances when it is determined that the unmanned ship is offset. The movement of the unmanned ship is controlled in real time based on the two parameters, so that the unmanned ship can quickly return to its positioning point, thereby reducing the impact of environmental disturbances on the return, shortening the control time, and improving the efficiency of the control.
[0006] A first aspect of an embodiment of the present invention provides a method for controlling a fixed-point return of a catamaran unmanned ship, the method comprising:
[0007] When it is determined that the unmanned ship deviates from the fixed position, the return control parameters of the unmanned ship and the compensation control parameters for coping with environmental disturbances are calculated respectively;
[0008] The unmanned ship is controlled to return to a fixed position based on the return control parameter and the compensation control parameter.
[0009] In a possible implementation manner of the first aspect, the return control parameter includes a return rate value and a return angle value;
[0010] The calculation operation of the return control parameter is specifically as follows:
[0011] Get the current position and deviation angle of the unmanned ship respectively;
[0012] Calculate the distance between the current position of the unmanned ship and the fixed point position;
[0013] Calculate the return speed value based on the difference between the position distance value and the fixed point holding tolerance radius value;
[0014] A heading angle value is calculated based on the deviation angle value.
[0015] In a possible implementation manner of the first aspect, the calculation operation of the deviation angle value is specifically:
[0016] Get the longitude and latitude of the current location, as well as the longitude and latitude of the fixed location respectively;
[0017] The deviation angle value is calculated based on the longitude and latitude of the current position and the longitude and latitude of the fixed-point position.
[0018] In a possible implementation manner of the first aspect, the compensation control parameter includes a compensation rate value and a compensation angle value;
[0019] The calculation of the compensation control parameter is specifically as follows:
[0020] Get real-time status parameters and historical status parameters respectively;
[0021] A state equation is constructed using the historical state parameter and a preset first disturbance parameter, and an observation equation is constructed using the real-time state parameter and a preset second disturbance parameter;
[0022] Calling a preset Kalman filter to estimate the state equation and the observation equation to obtain a compensation rate value and a compensation heading angle value respectively;
[0023] Among them, the preset Kalman filter is provided with a noise statistical time-varying estimator. The preset Kalman filter is a Kalman filter model constructed with the historical state parameters of the unmanned ship and the first disturbance parameter as state variables, and the real-time measurement state parameters of the unmanned ship and the second disturbance parameter as measurement variables.
[0024] In a possible implementation manner of the first aspect, calling a preset Kalman filter to estimate the state equation and the observation equation to obtain a compensation rate value and a compensation heading angle value, respectively, includes:
[0025] Substituting the state equation and the observation equation into a preset Kalman filter, the preset Kalman filter calls a noise statistical time-varying estimator to perform evaluation based on the statistical noise to obtain evaluation parameters;
[0026] constructing a kinetic equation using the evaluation parameters;
[0027] The inverse dynamics solution is performed on the dynamic equation to obtain the compensation rate value and the compensation heading angle value respectively.
[0028] In a possible implementation manner of the first aspect, constructing a kinetic equation using the evaluation parameters includes:
[0029] Adding the evaluation parameters to a preset kinetic model and solving the kinetic equation;
[0030] The preset dynamic model is constructed based on the position coordinate system of the catamaran unmanned ship and the first-order linear response model;
[0031] The kinetic model is shown below:
[0032]
[0033]
[0034] In the above formula, δ is the input rudder angle, r is the yaw speed of the unmanned ship, and K r and T r are the ship maneuvering indexes respectively; a and b are the speed equation parameters of the surface unmanned ship, u is the navigation speed of the unmanned ship, and ε is the throttle input value.
[0035] In a possible implementation manner of the first aspect, controlling the unmanned ship to return to a fixed position based on the return control parameter and the compensation control parameter includes:
[0036] Calculating the expected heading value and the expected speed value respectively by using the return control parameter and the compensation control parameter;
[0037] Calculate the thrust parameter of the unmanned ship propeller based on the expected heading value and the expected speed value;
[0038] The unmanned boat is controlled to return to the fixed position according to the thrust parameters.
[0039] In a possible implementation of the first aspect, the thrust parameter includes: a left throttle amount and a right throttle amount;
[0040] The left throttle amount is calculated as follows:
[0041]
[0042] The calculation of the right throttle amount is shown in the following formula:
[0043]
[0044] In the above formula, Tleft is the left throttle amount, T right is the right throttle amount, k1 and k2 are power distribution proportional factors, thrust is the virtual thrust equivalent to the desired speed value, and rudder is the virtual rudder amount equivalent to the desired heading value, where:
[0045]
[0046] r is the fixed point holding tolerance radius value, d is the position distance value, is the deviation angle value.
[0047] In a possible implementation manner of the first aspect, determining that the unmanned ship deviates from the fixed point position is specifically:
[0048] Calculate the distance between the current position of the unmanned ship and the fixed point position;
[0049] If the position distance value is greater than the fixed point maintenance tolerance radius value, it is determined that the unmanned ship deviates from the fixed point position;
[0050] If the position distance value is less than the fixed point maintenance tolerance radius value, it is determined that the unmanned ship has not deviated from the fixed point position.
[0051] A second aspect of an embodiment of the present invention provides a fixed-point return control device for a catamaran unmanned ship, the device comprising:
[0052] A calculation module is used to calculate the return control parameters of the unmanned ship and the compensation control parameters for coping with environmental disturbances when it is determined that the unmanned ship deviates from the fixed point position;
[0053] The control module is used to control the unmanned ship to return to a fixed position based on the return control parameter and the compensation control parameter.
[0054] Compared with the prior art, the embodiment of the present invention provides a method and device for controlling the fixed-point return of a catamaran unmanned ship, which has the beneficial effect that: when it is determined that the unmanned ship is offset, the present invention can calculate in real time the control parameters required for return and the return compensation parameters that can cope with environmental disturbances, and control the movement of the unmanned ship in real time based on the two parameters, so that the unmanned ship can quickly return to its target position, thereby reducing the impact of environmental disturbances on the return, so that the unmanned ship can return smoothly to the fixed position, and can also avoid the problem of multiple adjustment routes during the return process, so as to shorten the control time and improve the efficiency of the control. BRIEF DESCRIPTION OF THE DRAWINGS
[0055] Figure 1 It is a flow chart of a method for controlling a fixed-point return of a catamaran unmanned ship provided by an embodiment of the present invention;
[0056] Figure 2 is a schematic diagram of a deviation angle of an unmanned ship provided by an embodiment of the present invention;
[0057] Figure 3 is a schematic diagram of a coordinate system of an unmanned ship provided by an embodiment of the present invention;
[0058] Figure 4 is an operation flow chart of calculating compensation control parameters provided by an embodiment of the present invention;
[0059] Figure 5 is an operation flow chart of calculating thrust parameters provided by an embodiment of the present invention;
[0060] Figure 6 This is an operation flow chart of a fixed-point return control method for a catamaran unmanned ship provided by an embodiment of the present invention;
[0061] Figure 7 The present invention is a schematic structural diagram of a fixed-point return control device for a catamaran unmanned ship provided in one embodiment of the present invention. DETAILED DESCRIPTION
[0062] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. 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.
[0063] The currently commonly used return control method has the following technical problems: although the route is corrected to allow the unmanned ship to sail, the unmanned ship may still be affected by various environmental disturbances during the return process, causing the unmanned ship to deviate from its route again and be unable to sail to the fixed position, and then it is necessary to repeatedly plan a new route for the unmanned ship, which not only increases the workload of correction and data processing, but also the efficiency of route planning and adjustment is low, which increases the time spent on navigation.
[0064] In order to solve the above problems, a method for controlling the fixed-point return of a catamaran unmanned ship provided in an embodiment of the present application will be introduced and explained in detail through the following specific embodiments.
[0065] Reference Figure 1 , showing a flow chart of a method for controlling a fixed-point return of a catamaran unmanned ship provided by an embodiment of the present invention.
[0066] Wherein, as an example, the fixed-point return control method of the catamaran unmanned ship may include:
[0067] S11. When it is determined that the unmanned ship deviates from the fixed position, the return control parameters of the unmanned ship and the compensation control parameters for coping with environmental disturbances are calculated respectively.
[0068] In one embodiment, the current position of the unmanned ship can be detected to determine whether the unmanned ship deviates from the fixed position. If it is determined that the unmanned ship has deviated, the return control parameters and compensation control parameters of the unmanned ship can be calculated immediately.
[0069] In one embodiment, the unmanned boat may sway back and forth at a fixed position due to the rolling of waves or currents in the water, but it does not actually leave the fixed position. Therefore, in order to accurately determine whether the unmanned boat deviates from its fixed position, as an example, step S11 may include the following sub-steps:
[0070] S111. Calculate the position distance value between the current position of the unmanned ship and the fixed point position.
[0071] S112: If the position distance value is greater than the fixed point maintenance tolerance radius value, it is determined that the unmanned ship deviates from the fixed point position.
[0072] S113: If the position distance value is less than the fixed point maintenance tolerance radius value, it is determined that the unmanned ship has not deviated from the fixed point position.
[0073] Specifically, the coordinate point of the current position of the unmanned ship and the coordinate point of the fixed point position can be obtained respectively, and then the distance value between the two positions is calculated based on the two coordinate points. Then, it is determined whether the position distance value is greater than the fixed point maintenance tolerance radius value. If the position distance value is greater than the fixed point maintenance tolerance radius value, it is determined that the unmanned ship deviates from the fixed point position, otherwise, it is determined that the unmanned ship does not deviate from the fixed point position.
[0074] Among them, the fixed-point holding tolerance radius value refers to the radius of an area centered on the fixed-point position where the unmanned boat can shake and move, and the radius distance value can be adjusted according to actual needs.
[0075] Specifically, the calculation of the position distance value is as follows:
[0076]
[0077] In the above formula, d is the distance value, R is the radius of the earth, and B w is the latitude of the fixed point, B j is the longitude of the fixed point, A w is the latitude of the current location, A j The longitude of the current location.
[0078] By setting the radius distance value, the unmanned ship can adjust the speed within the circle and reduce it according to a certain rule, making the fixed-point holding effect more flexible.
[0079] Since the unmanned boat may move in different directions in the water, in order to enable the unmanned boat to accurately return to a fixed position, in one embodiment, the return control parameters include a return rate value and a return angle value.
[0080] As an example, the calculation operation of the return control parameter specifically includes:
[0081] S21. Obtain the current position and deviation angle value of the unmanned ship respectively.
[0082] Reference Figure 2 , showing a schematic diagram of the deviation angle of an unmanned ship provided by an embodiment of the present invention.
[0083] The deviation angle value is the deviation angle between the current direction of the unmanned ship and the line connecting the current position of the unmanned ship and the fixed point position.
[0084] Wherein, step S21 may include the following sub-steps:
[0085] S211. Obtain the longitude and latitude of the current position and the longitude and latitude of the fixed point position respectively.
[0086] S212: Calculate a deviation angle value based on the longitude and latitude of the current position and the longitude and latitude of the fixed-point position.
[0087] Specifically, the calculation of the deviation angle value is as follows:
[0088] The deviation angle range is
[0089] in, is the deviation angle value, ψ is the heading angle of the unmanned boat, B w is the latitude of the fixed point, B j is the longitude of the fixed point, A w is the latitude of the current location, A j The longitude of the current location.
[0090] S22. Calculate the distance between the current position of the unmanned ship and the fixed point position.
[0091] Specifically, the calculation of the position distance value is as shown in the above formula, and the specific calculation can be referred to the above formula.
[0092] S23. Calculate a return speed value based on the difference between the position distance value and the fixed point holding tolerance radius value.
[0093] Specifically, the return rate value can be calculated as follows:
[0094] v = k*(dr);
[0095] Where d is the position distance value, r is the fixed point holding tolerance radius value, and k is the speed proportional coefficient (in m / s).
[0096] S24. Calculate a heading angle value based on the deviation angle value.
[0097] In one embodiment, in order to allow the unmanned ship to flexibly return to a fixed position, the heading angle value may be calculated according to the deviation angle value.
[0098] Specifically, when the deviation angle value is less than 90 degrees, the deviation angle value may be used as the heading angle value; when the deviation angle value is greater than 90 degrees, the heading angle value may be obtained by subtracting the deviation angle value from 180 degrees.
[0099] For example, if the deviation angle value is 60 degrees, the heading angle value is 60 degrees; if the deviation angle value is greater than 120 degrees, the heading angle value is 180-120=60 degrees.
[0100] Since when the deviation angle value is less than ninety degrees, you can choose to point to the position point, and you can directly control the unmanned boat to move forward to the holding point. When the deviation angle is greater than ninety degrees, the unmanned boat is directly controlled to drive to the fixed position. The unmanned boat needs to make a big circle, so that the unmanned boat can directly drive back to the fixed position to shorten the unmanned boat's form distance and reduce the probability of being affected by wind and waves during driving.
[0101] Through the above control strategy, it can converge more flexibly to reach the fixed position according to its deviation angle, thereby improving the efficiency of regulation.
[0102] Since the heading of the unmanned ship requires a return rate value and a return angle value, correspondingly, in order to compensate the return rate value and the return angle value, in one embodiment, the compensation control parameter includes a compensation rate value and a compensation angle value;
[0103] The compensation rate value and the compensation angle value are compensated into the compensation values of the thrust and the steering amount. The thrust and the steering amount can be equivalent to the thrust values of the two propulsion forces according to the power distribution strategy.
[0104] As an example, the calculation of the compensation control parameter specifically includes:
[0105] S31. Obtain real-time status parameters and historical status parameters respectively.
[0106] In this embodiment, the real-time state parameters may be the current forward speed and the current yaw angular velocity of the unmanned ship. The historical state parameters may be the forward speed and the yaw angular velocity of the unmanned ship during a previous return trip, or the forward speed and the yaw angular velocity at a previous time node, and the specific time node may be adjusted according to actual needs.
[0107] S32: construct a state equation using the historical state parameter and a preset first disturbance parameter, and construct an observation equation using the real-time state parameter and a preset second disturbance parameter.
[0108] Specifically, the state equation is shown as follows:
[0109] x(k)=x(k-1)+w(k-1);
[0110] Among them, k is the moment mark, k is the current moment, and k-1 is the previous moment.
[0111] The observation equation is shown as follows:
[0112] y(k)=x(k)+v(k);
[0113] In the above formula, x(k) and y(k) are the real-time state variables and the observed state variable matrix, respectively, x(k-1) is the historical state parameter, w(k-1) and v(k) are the first disturbance parameter and the second disturbance parameter, respectively, and the numerical values of the disturbance parameters can be adjusted according to the actual situation and can be the values pre-set by the user. Specifically, w(k) and v(k) are the input noise and the measurement noise, respectively, and the state variable matrix consists of the heading angle yaw, the speed, and the acceleration of the x and y axes in the appendage coordinate system.
[0114] The state vector is dynamically corrected according to the IMU (Inertial Measurement Unit) measurement value and Kalman filter gain matrix to continuously improve the prediction accuracy. The predicted state value and the mathematical model of the unmanned boat can reliably predict the attitude in the future within a limited time, so as to achieve better fixed-point control effect.
[0115] By adding disturbance parameters, the impact of wind, waves and currents in the external environment on the unmanned ship can be simulated, and the required compensation parameters can be calculated.
[0116] S33, calling a preset Kalman filter to estimate the state equation and the observation equation, and obtaining a compensation rate value and a compensation angle value respectively.
[0117] In one embodiment, a preset Kalman filter may be called to use two equations to perform evaluation calculations to respectively calculate a compensation rate value and a compensation heading angle value.
[0118] Since the Kalman filter is an effective algorithm for filtering signals with random noise, it consists of a series of recursive processes, has good dynamic and anti-disturbance performance, and can realize online state estimation.
[0119] Optionally, a noise estimator is designed using a Kalman filter, wherein the Kalman filter is a Kalman filter model constructed with the historical state parameters of the unmanned ship and the first disturbance parameter as state variables, and the real-time measured state parameters of the unmanned ship and the second disturbance parameter as measurement variables.
[0120] Specifically, the historical navigation state parameter is the historical state parameter x(k-1) at the previous moment, the first disturbance parameter is the input noise w(k-1), the real-time state variable matrix is x(k), and the second disturbance parameter is the measurement noise v(k).
[0121] In one embodiment, step S33 may include the following sub-steps:
[0122] S331, substituting the state equation and the observation equation into a preset Kalman filter, and performing online estimation by the preset Kalman filter to obtain an estimated value of the state. .
[0123] S332, constructing a kinetic equation using the evaluation parameters.
[0124] In one embodiment, sub-step S332 may include the following sub-steps:
[0125] S3321, adding the obtained state estimation value to a preset dynamics model, and solving the dynamics equation;
[0126] The preset dynamic model is constructed based on the position coordinate system of the catamaran unmanned ship and the first-order linear response model.
[0127] The kinetic model is shown below:
[0128]
[0129]
[0130] In the above formula, δ is the input rudder angle, r is the yaw speed of the unmanned ship, and K r and T r are the ship maneuvering indexes respectively; a and b are the speed equation parameters of the surface unmanned ship, u is the navigation speed of the unmanned ship, and ε is the throttle input value.
[0131] Reference Figure 3 , showing a schematic diagram of a coordinate system of an unmanned ship provided by an embodiment of the present invention.
[0132] In this embodiment, the motion process of the unmanned boat on water is studied, and generally two coordinate systems, the inertial coordinate system and the attached coordinate system, are used. Figure 3 As shown, O E X E Y Eis an inertial coordinate system fixed on the surface of the earth, O b X b Y b It is the attached coordinate system whose origin is located at the front, back, left and right symmetrical points on the water surface of the unmanned boat. And according to the actual research needs, only the motion of the unmanned boat in the horizontal plane is considered, and the heave, roll and pitch are ignored. The six-degree-of-freedom motion of the ship is simplified to the three-degree-of-freedom motion of the horizontal plane, namely, the sway, pitch and bow, and the forward speed u, the lateral speed v and the bow speed value r of the unmanned boat are considered.
[0133] Since the state estimated by the Kalman filter is closer to the state in the system process equation, it can effectively reduce the impact of sensor noise on controller noise. After the disturbance is estimated online during the control process, according to the dynamic equation of the previous dynamic model, the new heading angle and speed should satisfy the following dynamic equation:
[0134]
[0135]
[0136] S333, performing inverse dynamics solution on the dynamic equation to obtain a compensation rate value and a compensation angle value respectively.
[0137] By performing inverse dynamics solution on the above dynamic equation, the compensation rate value and compensation angle value can be obtained respectively, as shown in the following formula:
[0138] Compensation rate value:
[0139] Compensation angle value:
[0140] In this embodiment, the preset Kalman filter is provided with a noise statistical time-varying estimator. The preset Kalman filter is a Kalman filter model constructed with the historical state parameters of the unmanned ship and the first disturbance parameters as state variables, and the real-time state parameters of the unmanned ship and the second disturbance parameters as measurement variables.
[0141] The Kalman filter is used to reliably predict the attitude in the future within a limited time. The active enhanced disturbance compensation controller established measures the ship's heading angle yaw, speed, and acceleration of the x and y axes in the body coordinate system through shipborne integrated navigation. The unmanned boat model combines the Kalman filter algorithm to process the measured data, and compensates and controls the heading and speed of the unmanned boat, achieving a fixed-point holding effect with high line of sight accuracy. At the same time, the Kalman filter has a certain autonomous correction capability, and can continuously use the actual measurement data to compare with the state estimation value, and continuously correct the Kalman filter gain to improve the prediction accuracy.
[0142] Reference Figure 4, shows an operational flow chart of calculating compensation control parameters provided by an embodiment of the present invention.
[0143] In actual operation, a nominal controller can be used to perform regulation.
[0144] Specifically, the return control parameters are input into the nominal controller, which triggers the nominal controller to call the Kalman filter model for compensation evaluation. Finally, the compensation rate value and compensation angle value are obtained. The corresponding compensation controller is built using these two supplementary values, and the following are obtained:
[0145] The rate disturbance compensation controller is:
[0146] The angle disturbance compensation controller is:
[0147] Since there are disturbances such as wind, waves, and currents in the environment during control, the Kalman filter is used to estimate the disturbance online, and then an actively enhanced compensation controller is designed. Finally, the compensation controller feeds forward compensation to the nominal controller, and the catamaran is controlled by the nominal controller (for example, PID controller) to reduce the impact of various environmental disturbances and disturbances in the water on the ship.
[0148] The Kalman filter model is used to estimate the disturbances of wind, waves and currents online, and an active enhancement controller is established, as well as a heading and speed compensation controller, to compensate for the impact of uncertainty brought about by the modeling process on the control system and improve the performance of the model. In addition, based on the proposed online active enhanced fixed-point holding controller, combined with the nominal controller, the disturbance is estimated online to generate a feedforward compensation controller into the nominal controller, and fixed-point holding control is performed on the catamaran unmanned boat. The influence of disturbances is added to the control, which can achieve fixed-point holding control more accurately.
[0149] S12: Control the unmanned boat to return to a fixed position based on the return control parameter and the compensation control parameter.
[0150] After determining the return control parameters and compensation control parameters, the two control parameters can be used to control the unmanned boat propulsion motor so that the unmanned boat can return to the fixed position in a specific direction and speed.
[0151] In an optional embodiment, step S12 may include the following sub-steps:
[0152] S121. Calculate the expected heading value and the expected speed value respectively using the return control parameter and the compensation control parameter.
[0153] Specifically, the return rate value is added to the compensation rate value to obtain the expected speed value, and the return angle value is added to the compensation angle value to obtain the expected heading value.
[0154] S122. Calculate the thrust parameters of the unmanned ship propeller based on the expected heading value and the expected speed value.
[0155] In one embodiment, the unmanned boat is a catamaran unmanned boat, with a propulsion motor on each side to propel the unmanned boat forward from the left and right sides.
[0156] Wherein, as an example, the thrust parameters include: left throttle amount and right throttle amount;
[0157] The calculation of the left throttle amount is shown in the following formula:
[0158]
[0159] The calculation of the right throttle amount is shown in the following formula:
[0160]
[0161] In the above formula, T left is the left throttle amount, T right is the right throttle amount, k1 and k2 are power distribution proportional factors, thrust is the virtual thrust equivalent to the desired speed value, and rudder is the virtual rudder amount equivalent to the desired heading value, where:
[0162]
[0163] r is the fixed point holding tolerance radius value, d is the position distance value, is the deviation angle value.
[0164] Reference Figure 5 , shows an operational flow chart for calculating thrust parameters provided by an embodiment of the present invention.
[0165] Specifically, the desired heading value can be equivalent to the virtual steering amount, and the desired speed value can be equivalent to the virtual thrust. Then, the virtual steering amount and the virtual thrust are substituted into the above formula to calculate two throttle amounts, which are respectively used to control the throttles of the left and right propulsion motors to make the two propulsion motors work.
[0166] S123. Control the unmanned boat to return to the fixed position according to the thrust parameters.
[0167] Finally, according to the two throttle values, the left and right propulsion motors of the unmanned boat are controlled to work respectively.
[0168] The present invention separates the control of direction and speed, and the desired speed and the desired heading angle are controlled by feedback controllers respectively. Finally, the throttle amounts of the left and right thrusters are distributed and controlled by a mixed control distribution strategy to achieve fixed-point holding of the catamaran unmanned boat.
[0169] In actual operation, a larger rotation or a higher speed may be required. For example, during the actual operation of the unmanned boat, it may be necessary to focus on the change of the bow angle or the change of the speed. In order to control it more accurately, power distribution can be carried out based on the above-mentioned proportional coefficients K1 and K2 for the unmanned boat to return.
[0170] Reference Figure 6 , shows an operation flow chart of a fixed-point return control method for a catamaran unmanned ship provided by an embodiment of the present invention.
[0171] Specifically, the position coordinates of the unmanned ship are first obtained, and based on the position coordinates, it is determined whether the unmanned ship is offset. When the unmanned ship is offset, the return control parameters and compensation control parameters are calculated respectively, and then the nominal controller is called to use the two parameters to perform corresponding throttle control to control the unmanned ship to return to the fixed position.
[0172] In this embodiment, an embodiment of the present invention provides a method for controlling the fixed-point return of a catamaran unmanned ship, and its beneficial effects are: when it is determined that the unmanned ship is offset, the present invention can calculate in real time the control parameters required for return and the return compensation parameters that can cope with environmental disturbances, and control the movement of the unmanned ship in real time based on the two parameters, so that the unmanned ship can quickly return to its target position point to reduce the impact of environmental disturbances on the return, so that the unmanned ship can return smoothly to the fixed position, and can also avoid the problem of multiple adjustment routes during the return process, so as to shorten the control time and improve the efficiency of the control.
[0173] The embodiment of the present invention also provides a fixed-point return control device for a catamaran unmanned ship, see Figure 7 , showing a schematic structural diagram of a fixed-point return control device for a catamaran unmanned ship provided by an embodiment of the present invention.
[0174] Wherein, as an example, the fixed-point return control device of the catamaran unmanned ship may include:
[0175] The calculation module 701 is used to calculate the return control parameters of the unmanned ship and the compensation control parameters for coping with environmental disturbances when it is determined that the unmanned ship deviates from the fixed point position;
[0176] The control module 702 is used to control the unmanned ship to return to a fixed position based on the return control parameter and the compensation control parameter.
[0177] Optionally, the return control parameters include a return rate value and a return angle value;
[0178] The calculation operation of the return control parameter is specifically as follows:
[0179] Get the current position and deviation angle of the unmanned ship respectively;
[0180] Calculate the distance between the current position of the unmanned ship and the fixed point position;
[0181] Calculate the return speed value based on the difference between the position distance value and the fixed point holding tolerance radius value;
[0182] A heading angle value is calculated based on the deviation angle value.
[0183] Optionally, the calculation operation of the deviation angle value is specifically:
[0184] Get the longitude and latitude of the current location, as well as the longitude and latitude of the fixed location respectively;
[0185] The deviation angle value is calculated based on the longitude and latitude of the current position and the longitude and latitude of the fixed-point position.
[0186] Optionally, the compensation control parameters include a compensation rate value and a compensation heading angle value;
[0187] The calculation of the compensation control parameter is specifically as follows:
[0188] Get real-time status parameters and historical status parameters respectively;
[0189] A state equation is constructed using the historical state parameter and a preset first disturbance parameter, and an observation equation is constructed using the real-time state parameter and a preset second disturbance parameter;
[0190] Calling a preset Kalman filter to estimate the state equation and the observation equation to obtain a compensation rate value and a compensation heading angle value respectively;
[0191] Among them, the preset Kalman filter is provided with a noise statistical time-varying estimator. The preset Kalman filter is a Kalman filter model constructed with the historical state parameters of the unmanned ship and the first disturbance parameter as state variables, and the real-time measurement state parameters of the unmanned ship and the second disturbance parameter as measurement variables.
[0192] Optionally, calling a preset Kalman filter to estimate the state equation and the observation equation to obtain a compensation rate value and a compensation heading angle value, respectively, includes:
[0193] Substituting the state equation and the observation equation into a preset Kalman filter, the preset Kalman filter calls a noise statistical time-varying estimator to perform evaluation based on the statistical noise to obtain evaluation parameters;
[0194] constructing a kinetic equation using the evaluation parameters;
[0195] The inverse dynamics solution is performed on the dynamic equation to obtain the compensation rate value and the compensation heading angle value respectively.
[0196] Optionally, constructing a kinetic equation using the evaluation parameters comprises:
[0197] Substituting the evaluation parameters into a preset kinetic model to obtain a kinetic equation;
[0198] The preset dynamic model is constructed based on the position coordinate system of the catamaran unmanned ship and the first-order linear response model;
[0199] The kinetic model is shown below:
[0200]
[0201]
[0202] In the above formula, δ is the input rudder angle, r is the yaw speed of the unmanned ship, and K r and T r are the ship maneuvering indexes respectively; a and b are the speed equation parameters of the surface unmanned ship, u is the navigation speed of the unmanned ship, and ε is the throttle input value.
[0203] Optionally, the control module is further used to:
[0204] Calculating the expected heading value and the expected speed value respectively by using the return control parameter and the compensation control parameter;
[0205] Calculate the thrust parameter of the unmanned ship propeller based on the expected heading value and the expected speed value;
[0206] The unmanned boat is controlled to return to the fixed position according to the thrust parameters.
[0207] Optionally, the thrust parameters include: left throttle amount and right throttle amount;
[0208] The left throttle amount is calculated as follows:
[0209]
[0210] The calculation of the right throttle amount is shown in the following formula:
[0211]
[0212] In the above formula, T left is the left throttle amount, T right is the right throttle amount, k1 and k2 are power distribution proportional factors, thrust is the virtual thrust equivalent to the desired speed value, and rudder is the virtual rudder amount equivalent to the desired heading value, where:
[0213]
[0214] r is the fixed point holding tolerance radius value, d is the position distance value, is the deviation angle value.
[0215] Optionally, the determining that the unmanned ship deviates from the fixed point position is specifically:
[0216] Calculate the distance between the current position of the unmanned ship and the fixed point position;
[0217] If the position distance value is greater than the fixed point maintenance tolerance radius value, it is determined that the unmanned ship deviates from the fixed point position;
[0218] If the position distance value is less than the fixed point maintenance tolerance radius value, it is determined that the unmanned ship has not deviated from the fixed point position.
[0219] Those skilled in the art can clearly understand that, for the sake of convenience and brevity of description, the specific working process of the device described above can refer to the corresponding process in the aforementioned method embodiment, and will not be repeated here.
[0220] Furthermore, an embodiment of the present application also provides an electronic device, comprising: a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein when the processor executes the program, the method for controlling the fixed-point return of the catamaran unmanned ship as described in the above embodiment is implemented.
[0221] Furthermore, an embodiment of the present application also provides a computer-readable storage medium, which stores computer-executable instructions, and the computer-executable instructions are used to enable a computer to execute the fixed-point return control method for the catamaran unmanned ship as described in the above embodiment.
[0222] The above is a preferred embodiment of the present invention. It should be pointed out that a person skilled in the art can make several improvements and modifications without departing from the principle of the present invention. These improvements and modifications are also considered to be within the scope of protection of the present invention.
Claims
1. A method for controlling the fixed-point return of a catamaran unmanned ship, characterized in that: The method comprises: When it is determined that the unmanned ship deviates from the fixed position, the return control parameters of the unmanned ship and the compensation control parameters for coping with environmental disturbances are calculated respectively; Controlling the unmanned ship to return to a fixed position based on the return control parameter and the compensation control parameter; The controlling the unmanned ship to return to a fixed position based on the return control parameter and the compensation control parameter includes: Calculating the expected heading value and the expected speed value respectively by using the return control parameter and the compensation control parameter; Calculate the thrust parameter of the unmanned ship propeller based on the expected heading value and the expected speed value; Control the unmanned ship to return to the fixed position according to the thrust parameters; The thrust parameters include: left throttle amount and right throttle amount; The left throttle amount is calculated as follows: The calculation of the right throttle amount is shown in the following formula: In the above formula, T left is the left throttle amount, T right is the right throttle amount, k1 and k2 are power distribution proportional factors, thrust is the virtual thrust equivalent to the desired speed value, and rudder is the virtual rudder amount equivalent to the desired heading value, where: r is the fixed point holding tolerance radius value, d is the position distance value, is the deviation angle value.
2. The fixed-point return control method of the unmanned catamaran ship according to claim 1 is characterized in that: The return control parameters include a return rate value and a return angle value; The calculation operation of the return control parameter is specifically as follows: Get the current position and deviation angle of the unmanned ship respectively; Calculate the distance between the current position of the unmanned ship and the fixed point position; Calculate the return speed value based on the difference between the position distance value and the fixed point holding tolerance radius value; A heading angle value is calculated based on the deviation angle value.
3. The fixed-point return control method of the unmanned catamaran ship according to claim 2 is characterized in that: The calculation operation of the deviation angle value is specifically as follows: Get the longitude and latitude of the current location, as well as the longitude and latitude of the fixed location respectively; The deviation angle value is calculated based on the longitude and latitude of the current position and the longitude and latitude of the fixed-point position.
4. The fixed-point return control method of the unmanned catamaran ship according to claim 1 is characterized in that: The compensation control parameters include a compensation rate value and a compensation angle value; The calculation of the compensation control parameter is specifically as follows: Get real-time status parameters and historical status parameters respectively; A state equation is constructed using the historical state parameter and a preset first disturbance parameter, and an observation equation is constructed using the real-time state parameter and a preset second disturbance parameter; Calling a preset Kalman filter to estimate the state equation and the observation equation to obtain a compensation rate value and a compensation heading angle value respectively; Among them, the preset Kalman filter is provided with a noise statistical time-varying estimator. The preset Kalman filter is a Kalman filter model constructed with the historical state parameters of the unmanned ship and the first disturbance parameter as state variables, and the real-time measurement state parameters of the unmanned ship and the second disturbance parameter as measurement variables.
5. The fixed-point return control method of the unmanned catamaran ship according to claim 4 is characterized in that: The calling of a preset Kalman filter to estimate the state equation and the observation equation to obtain a compensation rate value and a compensation heading angle value respectively includes: Substituting the state equation and the observation equation into a preset Kalman filter, the preset Kalman filter calls a noise statistical time-varying estimator to perform evaluation based on the statistical noise to obtain evaluation parameters; constructing a kinetic equation using the evaluation parameters; The inverse dynamics solution is performed on the dynamic equation to obtain the compensation rate value and the compensation heading angle value respectively.
6. The fixed-point return control method of the unmanned catamaran ship according to claim 5 is characterized in that: The step of constructing a kinetic equation using the evaluation parameters comprises: Adding the evaluation parameters to a preset kinetic model and solving the kinetic equation; The preset dynamic model is constructed based on the position coordinate system of the catamaran unmanned ship and the first-order linear response model; The kinetic model is shown below: In the above formula, δ is the input rudder angle, r is the yaw speed of the unmanned ship, and K r and T r are the ship maneuvering indexes respectively; a and b are the speed equation parameters of the surface unmanned ship, u is the navigation speed of the unmanned ship, and ε is the throttle input value.
7. The fixed-point return control method of the unmanned catamaran ship according to any one of claims 1 to 6, characterized in that: The determination of the unmanned ship deviating from the fixed point position is specifically as follows: Calculate the distance between the current position of the unmanned ship and the fixed point position; If the position distance value is greater than the fixed point maintenance tolerance radius value, it is determined that the unmanned ship deviates from the fixed point position; If the position distance value is less than the fixed point maintenance tolerance radius value, it is determined that the unmanned ship has not deviated from the fixed point position.
8. A fixed-point return control device for a catamaran unmanned ship, characterized in that: The device comprises: A calculation module is used to calculate the return control parameters of the unmanned ship and the compensation control parameters for coping with environmental disturbances when it is determined that the unmanned ship deviates from the fixed point position; A control module, used for controlling the unmanned ship to return to a fixed position based on the return control parameter and the compensation control parameter; The controlling the unmanned ship to return to a fixed position based on the return control parameter and the compensation control parameter includes: Calculating the expected heading value and the expected speed value respectively by using the return control parameter and the compensation control parameter; Calculate the thrust parameter of the unmanned ship propeller based on the expected heading value and the expected speed value; Control the unmanned ship to return to the fixed position according to the thrust parameters; The thrust parameters include: left throttle amount and right throttle amount; The left throttle amount is calculated as follows: The calculation of the right throttle amount is shown in the following formula: In the above formula, T left is the left throttle amount, T right is the right throttle amount, k1 and k2 are power distribution proportional factors, thrust is the virtual thrust equivalent to the desired speed value, and rudder is the virtual rudder amount equivalent to the desired heading value, where: r is the fixed point holding tolerance radius value, d is the position distance value, is the deviation angle value.
