CN115014355B - A method and device for controlling fixed-point return of a catamaran unmanned ship - Google Patents
A method and device for controlling fixed-point return of a catamaran unmanned ship Download PDFInfo
- Publication number
- CN115014355B CN115014355B CN202210662859.3A CN202210662859A CN115014355B CN 115014355 B CN115014355 B CN 115014355B CN 202210662859 A CN202210662859 A CN 202210662859A CN 115014355 B CN115014355 B CN 115014355B
- Authority
- CN
- China
- Prior art keywords
- unmanned ship
- return
- value
- fixed point
- parameters
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
- G01C21/203—Specially adapted for sailing ships
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T70/00—Maritime or waterways transport
- Y02T70/10—Measures concerning design or construction of watercraft hulls
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明公开了一种双体无人船的定点返航调控方法及装置，所述方法包括：当确定无人船偏离定点位置时，分别计算无人船的返航控制参数，以及应对环境扰动的补偿控制参数；基于所述返航控制参数和所述补偿控制参数控制无人船返回至定点位置。本发明可以在确定无人船出现偏移时，实时计算返航所需的控制参数以及可应对环境扰动的返航补偿参数，基于两个参数实时控制无人船移动，使无人船能快速返回其定位点，以减少环境扰动影响对返航的影响，使得无人船可以平稳地返航至定点位置，也可以避免返航过程中出现多条调整航线的问题，以缩短调控耗时，提高调控的效率。
The present invention discloses a method and device for controlling the fixed-point return of a catamaran unmanned ship, the method comprising: when it is determined that the unmanned ship deviates from the fixed-point position, respectively calculating the return control parameters of the unmanned ship and the compensation control parameters for dealing with environmental disturbances; and controlling the unmanned ship to return to the fixed-point position based on the return control parameters and the compensation control parameters. The present invention can calculate the control parameters required for return and the return compensation parameters that can deal with environmental disturbances in real time when it is determined that the unmanned ship is deviating, 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 positioning point, so as to reduce the impact of environmental disturbances on the return, so that the unmanned ship can smoothly return to the fixed-point 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.
Description
Technical Field
The invention relates to the technical field of unmanned ship control, in particular to a fixed-point return regulation and control method and device for a double-body unmanned ship.
Background
The unmanned ship is a water surface aircraft with autonomous navigation or remote control functions to realize normal navigation, operation and operation, can execute specified tasks by carrying various task loads, and is widely applied to the field of water area automation operation due to the advantages of safe operation, high efficiency, energy saving, low cost and the like.
The unmanned ship is easily influenced by various unstable factors (such as disturbance of wind, waves and currents) in water, so that the unmanned ship is easily deviated when the unmanned ship is parked at a fixed point, and the unmanned ship is gradually far away from a target position point or a current positioning point of the unmanned ship. In order to fix an unmanned ship to a specific location, the method commonly used at present is: when the unmanned ship is found to deviate, the current position point of the unmanned ship is determined, and then the driving route is planned again based on the current position point and the target position point (or the target positioning point) for the unmanned ship to sail to the target positioning point.
However, the current common methods have the following technical problems: although the course is corrected again to enable the unmanned ship to run, the unmanned ship is still possibly influenced by various unstable factors in the course of returning to the course, so that the unmanned ship deviates from the course again and cannot sail to the fixed point position, and further, a new running course is required to be planned for the unmanned ship repeatedly for a plurality of times, so that not only is the correction workload and data processing capacity increased, but also the efficiency of planning and adjusting the course is low, and the time consumption of sailing is increased.
Disclosure of Invention
The invention provides a fixed-point return regulation and control method and device for a double-body unmanned ship, wherein when the unmanned ship is determined to deviate, control parameters required by return and return compensation parameters capable of coping with environmental disturbance are calculated in real time, and the unmanned ship is controlled to move in real time based on the two parameters, so that the unmanned ship can quickly return to a locating point of the unmanned ship, the influence of the environmental disturbance on return is reduced, the regulation time consumption is shortened, and the regulation efficiency is improved.
The first aspect of the embodiment of the invention provides a fixed-point return regulation and control method of a double-body unmanned ship, which comprises the following steps:
When the unmanned ship deviates from the fixed point position, respectively calculating a return control parameter of the unmanned ship and a compensation control parameter for coping with environmental disturbance;
And controlling the unmanned ship to return to the fixed point position based on the return control parameter and the compensation control parameter.
In a possible implementation manner of the first aspect, the return control parameter includes a return speed value and a return angle value;
the calculation operation of the return control parameters specifically comprises the following steps:
respectively acquiring the current position and the deviation angle value of the unmanned ship;
calculating a position distance value between the current position and the fixed point position of the unmanned ship;
Calculating a return navigational speed value based on the difference between the position distance value and the fixed point holding tolerance radius value;
and calculating a heading angle value based on the deviation angle value.
In a possible implementation manner of the first aspect, the calculating operation of the deviation angle value is specifically:
acquiring longitude and latitude of the current position and longitude and latitude of the fixed point position respectively;
A departure angle value is calculated based on the longitude and latitude of the current location and the longitude and latitude of the fixed point location.
In a possible implementation manner of the first aspect, the compensation control parameter includes a compensation speed value and a compensation angle value;
the calculation reference of the compensation control parameter is specifically as follows:
Respectively acquiring real-time state parameters and historical state parameters;
Constructing a state equation by using the historical state parameters and preset first disturbance parameters, and constructing an observation equation by using the real-time state parameters and preset second disturbance parameters;
invoking a preset Kalman filter to estimate the state equation and the observation equation to obtain a compensation speed value and a compensation heading angle value respectively;
The preset Kalman filter is a Kalman filter model built by taking historical state parameters and first disturbance parameters of the unmanned ship as state variables and taking real-time measurement state parameters and second disturbance parameters of the unmanned ship as measurement variables.
In a possible implementation manner of the first aspect, the invoking a preset kalman filter to estimate the state equation and the observation equation to obtain a compensation velocity value and a compensation heading angle value respectively includes:
substituting the state equation and the observation equation into a preset Kalman filter, and calling a noise statistics time-varying estimator by the preset Kalman filter to evaluate according to the statistical noise to obtain an evaluation parameter;
Constructing a kinetic equation by using the evaluation parameters;
and carrying out inverse dynamics solving on the dynamics equation to respectively obtain a compensation speed value and a compensation heading angle value.
In a possible implementation manner of the first aspect, the constructing a kinetic equation using the evaluation parameter includes:
adding the evaluation parameters into a preset dynamics model, and solving a dynamics equation;
The preset dynamics model is constructed based on a position coordinate system of the double-body unmanned ship and a first-order linear response model;
The kinetic model is shown in the following formula:
In the above formula, delta is an input rudder angle, r is a yaw speed value of the unmanned ship, and K r and T r are ship steering indexes respectively; a and b are speed equation parameters of the unmanned ship on the water surface, u is the sailing speed of the unmanned ship, and epsilon is an accelerator input value.
In a possible implementation manner of the first aspect, the controlling the unmanned ship to return to the fixed point position based on the return control parameter and the compensation control parameter includes:
Respectively calculating an expected course value and an expected speed value by adopting the return control parameter and the compensation control parameter;
Calculating a thrust parameter of the unmanned ship propeller based on the expected heading value and the expected speed value;
and controlling the unmanned ship to return to the fixed point position according to the thrust parameter.
In a possible implementation manner of the first aspect, the thrust parameter includes: left and right throttle amounts;
the calculation of the left throttle amount is shown as follows:
the calculation of the right throttle amount is shown as follows:
In the above formula, T left is the left throttle amount, T right is the right throttle amount, k 1 and k 2 are power distribution scaling factors, thrust is the virtual thrust equivalent to the expected speed value, rudder is the virtual rudder amount equivalent to the expected heading value, wherein,
R is a fixed point holding tolerance radius value, d is a position distance value,Is the offset angle value.
In a possible implementation manner of the first aspect, the determining that the unmanned ship deviates from the fixed point position is specifically:
calculating a position distance value between the current position and the fixed point position of the unmanned ship;
If the position distance value is larger than the fixed point keeping tolerance radius value, determining that the unmanned ship deviates from the fixed point position;
and if the position distance value is smaller than the fixed point keeping tolerance radius value, determining that the unmanned ship is not deviated from the fixed point position.
A second aspect of an embodiment of the present invention provides a fixed-point return regulation device for a twin-hull unmanned ship, the device including:
The calculation module is used for respectively calculating the return control parameters of the unmanned ship and the compensation control parameters for coping with the environmental disturbance when the unmanned ship is determined to deviate from the fixed point position;
and the regulation and control module is used for controlling the unmanned ship to return to the fixed point position based on the return control parameter and the compensation control parameter.
Compared with the prior art, the fixed-point return regulation and control method and device for the double-body unmanned ship provided by the embodiment of the invention have the beneficial effects that: according to the invention, when the unmanned ship is determined to deviate, the control parameters required by the return and the return compensation parameters capable of coping with the environmental disturbance can be calculated in real time, and the unmanned ship is controlled to move in real time based on the two parameters, so that the unmanned ship can quickly return to the target position point of the unmanned ship, the influence of the environmental disturbance on the return is reduced, the unmanned ship can stably return to the fixed position, the problem of multiple adjustment routes in the return process can be avoided, the time consumption of regulation is shortened, and the regulation efficiency is improved.
Drawings
FIG. 1 is a schematic flow chart of a fixed-point return regulation method for a twin-hull unmanned ship according to an embodiment of the present invention;
FIG. 2 is a schematic view of the departure angle of an unmanned ship according to an embodiment of the present invention;
FIG. 3 is a schematic view of a coordinate system of an unmanned ship according to an embodiment of the present invention;
FIG. 4 is a flowchart illustrating operations for calculating compensation control parameters according to one embodiment of the present invention;
FIG. 5 is a flowchart illustrating operations for calculating thrust parameters according to one embodiment of the present invention;
FIG. 6 is an operational flow diagram of a method for controlling fixed-point return of a twin-hull unmanned ship according to an embodiment of the present invention;
fig. 7 is a schematic structural diagram of a fixed-point return regulation device of a twin-hull unmanned ship according to an embodiment of the present invention.
Detailed Description
The following description of the embodiments of the present invention will be made clearly and completely with reference to the accompanying drawings, in which it is apparent that the embodiments described are only some embodiments of the present invention, but not all embodiments. All other embodiments, which can be made by those skilled in the art based on the embodiments of the invention without making any inventive effort, are intended to be within the scope of the invention.
The current common return regulation and control method has the following technical problems: although the course is corrected again to enable the unmanned ship to run, the unmanned ship is still possibly influenced by various environmental disturbances in the course of returning to the course, so that the unmanned ship deviates from the course again and cannot sail to the fixed point position, and further, a new running course is required to be planned for the unmanned ship repeatedly for a plurality of times, so that not only is the correction workload and data processing capacity increased, but also the efficiency of planning and adjusting the course is low, and the time consumption of sailing is increased.
In order to solve the above problems, the following detailed description and explanation will be given for a fixed-point return regulation method of a double-body unmanned ship according to the embodiments of the present application.
Referring to fig. 1, a flow diagram of a method for controlling fixed-point return of a twin-hull unmanned ship according to an embodiment of the present invention is shown.
The fixed-point return regulation and control method of the double-body unmanned ship can comprise the following steps:
And S11, when the unmanned ship is determined to deviate from the fixed point position, respectively calculating the return control parameters of the unmanned ship and the compensation control parameters for coping with the environmental disturbance.
In an embodiment, the current position of the unmanned ship can be detected, whether the unmanned ship deviates from the fixed point position or not is determined, and if the unmanned ship is determined to deviate, the return control parameter and the compensation control parameter of the unmanned ship can be calculated immediately.
In one embodiment, the unmanned ship rolls in the water, possibly due to waves or currents, so that the unmanned ship swings back and forth at the fixed point location, but it does not actually leave the fixed point location. Thus, in order to be able to accurately determine that the unmanned ship deviates from its setpoint position, step S11 may, as an example, comprise the following sub-steps:
S111, calculating a position distance value between the current position and the fixed point position of the unmanned ship.
And S112, if the position distance value is larger than the fixed point holding tolerance radius value, determining that the unmanned ship deviates from the fixed point position.
And S113, if the position distance value is smaller than the fixed point holding tolerance radius value, determining that the unmanned ship does not deviate from the fixed point position.
Specifically, a coordinate point of the current position and a coordinate point of the fixed point position of the unmanned ship may be acquired, respectively, and then a distance value between the two positions may be calculated based on the two coordinate points. And then judging whether the position distance value is larger than the fixed point keeping tolerance radius value, if the position distance value is larger than the fixed point keeping tolerance radius value, determining that the unmanned ship deviates from the fixed point position, otherwise, determining that the unmanned ship does not deviate from the fixed point position.
The fixed point keeping tolerance radius value refers to an area radius which takes the fixed point position as the center and can be used for the swinging movement of the unmanned ship, and the radius distance value can be adjusted according to actual needs.
Specifically, the calculation of the position distance value is as follows:
In the above formula, d is a position distance value, R is an earth radius, B w is a latitude of a fixed point position, B j is a longitude of the fixed point position, a w is a latitude of a current position, and a j is a longitude of the current position.
By setting the radius distance value, the unmanned ship can regulate and control the navigational speed in the circle, so that the navigational speed is reduced according to a certain rule, and the fixed point keeping effect is more elastic.
In order to enable the unmanned ship to return to the fixed point position accurately, the return control parameters comprise a return speed value and a return angle value in one embodiment, because the unmanned ship can move in different directions in water.
The calculation operation of the return control parameter specifically includes, as an example, the following steps:
S21, respectively obtaining the current position and the deviation angle value of the unmanned ship.
Referring to fig. 2, a schematic view of the departure angle of the unmanned ship according to an embodiment of the present invention is shown.
The deviation angle value is the deviation angle of the connecting line of the current direction of the unmanned ship and the current position and the fixed point position of the unmanned ship.
Wherein step S21 may comprise the sub-steps of:
S211, acquiring the longitude and the latitude of the current position and the longitude and the latitude of the fixed point position respectively.
S212, calculating a deviation angle value based on the longitude and latitude of the current position and the longitude and latitude of the fixed point position.
Specifically, the calculation of the deviation angle value is shown as follows:
wherein the value range of the off angle is
Wherein, For the offset angle value, ψ is the unmanned boat heading angle, B w is the latitude of the fixed point location, B j is the longitude of the fixed point location, a w is the latitude of the current location, and a j is the longitude of the current location.
S22, calculating a position distance value between the current position and the fixed point position of the unmanned ship.
Specifically, the calculation of the position distance value is shown in the above formula, and the calculation can be specifically referred to the above formula.
S23, calculating a return speed value based on the difference value between the position distance value and the fixed point holding tolerance radius value.
Specifically, the calculation of the return rate value may be as follows:
v＝k*(d-r)；
where d is the position distance value, r is the setpoint hold tolerance radius value, and k is the speed scaling factor (in m/s).
S24, calculating a heading angle value based on the deviation angle value.
In one embodiment, to enable the unmanned ship to flexibly return to the fixed point position, the heading angle value may be calculated according to the magnitude of the deviation angle value.
Specifically, when the deviation angle value is smaller than 90 degrees, the deviation angle value may be taken as a heading angle value; when the deviation angle value is larger than 90 degrees, the deviation angle value can be subtracted by 180 degrees to obtain the heading angle value.
For example, if the deviation angle value is 60 degrees, the heading angle value is 60; if the deviation angle value is greater than 120 degrees, the heading angle value is 180-120=60 degrees.
Because the deviation angle value is smaller than ninety degrees, the pointing position point can be selected, the unmanned ship can be directly controlled to advance to the position of the holding point, and when the deviation angle is larger than ninety degrees, the unmanned ship is directly controlled to execute running to the fixed point position, and the unmanned ship needs to wind a large circle, so that the unmanned ship can directly drive back to the fixed point position, the form distance of the unmanned ship is shortened, and the probability of being influenced by wind waves in the running process is reduced.
Through the control strategy, the convergence of the deviation angle is more flexible to reach the fixed point position, and the regulation and control efficiency is improved.
Since the heading of the unmanned ship requires a return speed value and a return angle value, correspondingly, in order to compensate the return speed value and the return angle value, in an embodiment, the compensation control parameters include a compensation speed value and a compensation angle value;
the compensation speed value and the compensation angle value are compensated as compensation values of thrust and rudder quantity, and the thrust and rudder quantity can be equivalent to the thrust values of two propulsive forces according to a power distribution strategy.
The calculation references of the compensation control parameters specifically include, for example:
S31, respectively acquiring real-time state parameters and historical state parameters.
In this embodiment, the real-time status parameters may be the current forward speed and the current yaw rate of the unmanned ship. The historical state parameters can be the forward speed and the yaw rate of the unmanned ship which is subjected to the previous return voyage, or the forward speed and the yaw rate of a certain previous time node, and the specific time node can be adjusted according to actual needs.
S32, constructing a state equation by using the historical state parameters and preset first disturbance parameters, and constructing an observation equation by using the real-time state parameters and preset second disturbance parameters.
Specifically, the equation of state is shown as follows:
x(k)＝x(k-1)+w(k-1)；
Wherein, the k moment is marked, k is the current moment, and k-1 is the last moment.
The observation equation is shown as follows:
y(k)＝x(k)+v(k)；
In the above formula, x (k) and y (k) are real-time state variables and observation state variable matrixes respectively, x (k-1) is a history state parameter, w (k-1) and v (k) are a first disturbance parameter and a second disturbance parameter respectively, and the values of the disturbance parameters can be adjusted according to actual conditions and can be preset by a user. Specifically, w (k) and v (k) are input noise and measurement noise respectively, and the state variable matrix consists of heading angle yaw, navigational speed and acceleration of x and y axes in an attached coordinate system.
And dynamically correcting the state vector according to the IMU (inertial measurement unit) measurement value and the Kalman filtering gain lifting matrix, and continuously improving the prediction accuracy. And the predicted state value and the mathematical model of the unmanned ship are used for reliably forecasting the gesture in the limited time in the future, so that a better fixed point can keep the control effect.
By adding disturbance parameters, the influence of wind, wave and current of the external environment on the unmanned ship can be simulated, and further required compensation parameters can be calculated.
S33, a preset Kalman filter is called to estimate the state equation and the observation equation, and a compensation speed value and a compensation angle value are obtained respectively.
In an embodiment, a preset kalman filter may be invoked to perform evaluation calculation by using two equations, so as to obtain a compensation speed value and a compensation heading angle value through calculation respectively.
The Kalman filter is an effective algorithm for carrying out the most effective filtering on the signal with random noise, and consists of a series of recursion processes, so that the Kalman filter has better dynamic and disturbance rejection performance and can realize on-line state estimation.
Optionally, the noise estimator is designed by using a kalman filter, wherein the kalman filter is a kalman filter model built by taking historical state parameters and first disturbance parameters of the unmanned ship as state variables and taking real-time measurement state parameters and second disturbance parameters of the unmanned ship as measurement variables.
Specifically, the historical voyage state parameter is a historical state parameter x (k-1) at a previous time, and the first disturbance parameter is an input noise w (k-1). The real-time state variable matrix is x (k), and the second disturbance parameter is measurement noise v (k).
In an embodiment, step S33 may comprise the sub-steps of:
S331, substituting the state equation and the observation equation into a preset Kalman filter, and carrying out online estimation by the preset Kalman filter to obtain an estimated value of the state. .
S332, constructing a kinetic equation by using the evaluation parameters.
In an embodiment, the substep S332 may include the following substeps:
S3321, adding the obtained state estimation value into a preset dynamics model, and solving a dynamics equation;
the preset dynamics model is constructed based on a position coordinate system of the double-body unmanned ship and a first-order linear response model.
The kinetic model is shown in the following formula:
In the above formula, delta is an input rudder angle, r is a yaw speed value of the unmanned ship, and K r and T r are ship steering indexes respectively; a and b are speed equation parameters of the unmanned ship on the water surface, u is the sailing speed of the unmanned ship, and epsilon is an accelerator input value.
Referring to fig. 3, a schematic diagram of a coordinate system of an unmanned ship according to an embodiment of the present invention is shown.
In this embodiment, the water movement process of the unmanned ship is studied by adopting two coordinate systems, i.e., an inertial coordinate system and an appendage coordinate system, as shown in fig. 3, O EXEYE is an inertial coordinate system fixed on the surface of the earth, and O bXbYb is an appendage coordinate system with an origin point on the front-back, left-right symmetry point of the water surface in the unmanned ship. According to actual research needs, only the motion condition of the horizontal plane of the unmanned ship is considered, heave, roll and pitch are ignored, six-degree-of-freedom motion of the ship is simplified into three-degree-of-freedom motion of the horizontal plane, and the forward speed u, the transverse moving speed v and the yaw speed value r of the unmanned ship are considered.
Because the state of Kalman filtering estimation is closer to the state in a system process equation, the influence of sensor noise on the noise of a controller can be effectively reduced, and after disturbance is estimated on line in the control process, the new heading angle and the new navigational speed which are obtained according to the dynamics equation of the dynamics model are required to meet the following dynamics equation:
S333, carrying out inverse dynamics solving on the dynamics equation to respectively obtain a compensation speed value and a compensation angle value.
And solving the dynamics equation in an inverse dynamics way to obtain a compensation speed value and a compensation angle value respectively, wherein the compensation speed value and the compensation angle value are shown in the following specific formulas:
Compensation rate value:
Compensating angle value:
In this embodiment, a preset kalman filter is provided with a noise statistics time-varying estimator, and the preset kalman filter is a kalman filter model built by taking a historical state parameter and a first disturbance parameter of an unmanned ship as state variables and taking a real-time state parameter and a second disturbance parameter of the unmanned ship as measurement variables.
The method is characterized in that the Kalman filtering is adopted to reliably forecast the gesture in the limited time in the future, the established active enhancement disturbance compensation controller measures the heading angle yaw, the navigational speed and the acceleration of the x and y axes under the attached coordinate system of the ship motion through the shipborne integrated navigation, the unmanned ship model processes the measured data by combining the Kalman filtering algorithm, the compensation control is carried out on the heading and the navigational speed of the unmanned ship, and the fixed point keeping effect with higher sight precision is achieved. Meanwhile, the Kalman filtering wave band has certain autonomous correction capability, and the Kalman filtering gain can be continuously corrected by continuously comparing actual measurement data with a state estimation value so as to improve the prediction accuracy.
Referring to FIG. 4, a flowchart of operations for calculating compensation control parameters is shown, provided in one embodiment of the present invention.
In actual operation, a nominal controller may be used, through which the regulation is performed.
Specifically, the return control parameters are input to a nominal controller, the nominal controller is triggered to call a Kalman filtering model to carry out compensation evaluation, a compensation speed value and a compensation angle value are finally obtained, and corresponding compensation controllers are built by utilizing the two compensation values, so that the compensation speed value and the compensation angle value are respectively obtained:
the rate disturbance compensation controller is as follows:
The angle disturbance compensation controller is as follows:
Because of the disturbance such as environmental wind, wave and current during the control, the disturbance is estimated on line by adopting a Kalman filter, a compensation controller which can be actively enhanced is designed, and finally the compensation controller is compensated to a nominal controller in a feedforward way, and the nominal controller (for example, a PID controller) is used for controlling the twin-hull unmanned ship so as to reduce the influence of various environmental disturbances and disturbance in water on the ship.
The method comprises the steps of using a Kalman filtering model to estimate disturbance of wind, wave and flow on line, establishing an active enhancement controller, establishing a course and navigational speed compensation controller, compensating influence of uncertainty brought in a modeling process on a control system, improving performance of the model, combining a nominal controller according to the proposed on-line active enhancement fixed point maintenance controller, generating a feedforward compensation controller to the nominal controller by on-line disturbance estimation, carrying out fixed point maintenance control on the twin-hull unmanned ship, adding the influence of disturbance in the control, and realizing fixed point maintenance control more accurately.
And S12, controlling the unmanned ship to return to the fixed point position based on the return control parameter and the compensation control parameter.
After the return control parameters and the compensation control parameters are determined, the two control parameters can be used for controlling the unmanned ship to push the motor, so that the unmanned ship can return to the fixed point according to the specific direction and speed.
In an alternative embodiment, step S12 may comprise the sub-steps of:
S121, calculating an expected heading value and an expected speed value respectively by adopting the return control parameter and the compensation control parameter.
Specifically, the return speed value and the compensation speed value are added to obtain a desired speed value, and the return angle value and the compensation angle value are added to obtain a desired heading value.
S122, calculating a thrust parameter of the unmanned ship propeller based on the expected heading value and the expected speed value.
In one embodiment, the unmanned ship is a double-body unmanned ship, and the left side and the right side are respectively provided with a pushing motor for pushing the unmanned ship to advance from the left side and the right side.
Wherein, as an example, the thrust parameters include: left and right throttle amounts;
the calculation of the left throttle amount is shown as the following formula:
the calculation of the right throttle amount is shown as follows:
In the above formula, T left is the left throttle amount, T right is the right throttle amount, k 1 and k 2 are power distribution scaling factors, thrust is the virtual thrust equivalent to the expected speed value, rudder is the virtual rudder amount equivalent to the expected heading value, wherein,
R is a fixed point holding tolerance radius value, d is a position distance value,Is the offset angle value.
Referring to FIG. 5, a flowchart of operations for calculating thrust parameters is shown, provided in accordance with one embodiment of the present invention.
Specifically, the expected heading value can be equivalent to a virtual rudder amount, the expected speed value can be equivalent to a virtual thrust, and then the virtual rudder amount and the virtual thrust are substituted into the above formula, so that two throttle amounts can be calculated and respectively used for controlling the throttles of the left pushing motor and the right pushing motor, and the two pushing motors work.
And S123, controlling the unmanned ship to return to the fixed point position according to the thrust parameter.
And finally, respectively controlling the left pushing motor and the right pushing motor of the unmanned ship to work according to the two accelerator amounts.
According to the invention, the control of the direction and the speed is separated, the expected navigational speed and the expected heading angle are respectively controlled by the feedback controller, and finally the throttle amounts of the left propeller and the right propeller are distributed and controlled by the mixed control distribution strategy, so that the fixed-point maintenance of the double-body unmanned ship is realized.
In actual operation, rotation may be required to be a little larger, for example, the unmanned ship may need to be stressed in a change of heading angle and a change of navigational speed in an actual operation process, and for more accurate regulation and control, power distribution can be performed based on the proportionality coefficients K1 and K2 so as to provide for the unmanned ship to return to the voyage.
Referring to fig. 6, an operation flow chart of a fixed-point return regulation method of a double-body unmanned ship according to an embodiment of the present invention is shown.
Specifically, the position coordinates of the unmanned ship are acquired firstly, whether the unmanned ship is deviated or not is determined based on the position coordinates, when the unmanned ship is deviated, return control parameters and compensation control parameters are calculated respectively, and then a nominal controller is called to use the two parameters to perform corresponding throttle control so as to control the unmanned ship to return to the fixed point position.
In this embodiment, the embodiment of the invention provides a fixed-point return regulation method for a double-body unmanned ship, which has the following beneficial effects: according to the invention, when the unmanned ship is determined to deviate, the control parameters required by the return and the return compensation parameters capable of coping with the environmental disturbance can be calculated in real time, and the unmanned ship is controlled to move in real time based on the two parameters, so that the unmanned ship can quickly return to the target position point of the unmanned ship, the influence of the environmental disturbance on the return is reduced, the unmanned ship can stably return to the fixed position, the problem of multiple adjustment routes in the return process can be avoided, the time consumption of regulation is shortened, and the regulation efficiency is improved.
The embodiment of the invention also provides a fixed-point return regulation device of the double-body unmanned ship, and referring to fig. 7, a structural schematic diagram of the fixed-point return regulation device of the double-body unmanned ship is shown.
Wherein, as an example, the fixed point back voyage regulation and control device of the double-body unmanned ship can comprise:
a calculating module 701, configured to calculate a return control parameter of the unmanned ship and a compensation control parameter for coping with the environmental disturbance when it is determined that the unmanned ship deviates from the fixed point position;
And the regulation and control module 702 is used for controlling the unmanned ship to return to the fixed-point position based on the return control parameter and the compensation control parameter.
Optionally, the return control parameter includes a return speed value and a return angle value;
the calculation operation of the return control parameters specifically comprises the following steps:
respectively acquiring the current position and the deviation angle value of the unmanned ship;
calculating a position distance value between the current position and the fixed point position of the unmanned ship;
Calculating a return navigational speed value based on the difference between the position distance value and the fixed point holding tolerance radius value;
and calculating a heading angle value based on the deviation angle value.
Optionally, the calculating operation of the deviation angle value specifically includes:
acquiring longitude and latitude of the current position and longitude and latitude of the fixed point position respectively;
A departure angle value is calculated based on the longitude and latitude of the current location and the longitude and latitude of the fixed point location.
Optionally, the compensation control parameter includes a compensation speed value and a compensation heading angle value;
the calculation reference of the compensation control parameter is specifically as follows:
Respectively acquiring real-time state parameters and historical state parameters;
Constructing a state equation by using the historical state parameters and preset first disturbance parameters, and constructing an observation equation by using the real-time state parameters and preset second disturbance parameters;
invoking a preset Kalman filter to estimate the state equation and the observation equation to obtain a compensation speed value and a compensation heading angle value respectively;
The preset Kalman filter is a Kalman filter model built by taking historical state parameters and first disturbance parameters of the unmanned ship as state variables and taking real-time measurement state parameters and second disturbance parameters of the unmanned ship as measurement variables.
Optionally, the invoking a preset kalman filter to estimate the state equation and the observation equation to obtain a compensation speed value and a compensation heading angle value respectively, which includes:
substituting the state equation and the observation equation into a preset Kalman filter, and calling a noise statistics time-varying estimator by the preset Kalman filter to evaluate according to the statistical noise to obtain an evaluation parameter;
Constructing a kinetic equation by using the evaluation parameters;
and carrying out inverse dynamics solving on the dynamics equation to respectively obtain a compensation speed value and a compensation heading angle value.
Optionally, the constructing a kinetic equation using the evaluation parameter includes:
substituting the evaluation parameters into a preset dynamics model to obtain a dynamics equation;
The preset dynamics model is constructed based on a position coordinate system of the double-body unmanned ship and a first-order linear response model;
The kinetic model is shown in the following formula:
In the above formula, delta is an input rudder angle, r is a yaw speed value of the unmanned ship, and K r and T r are ship steering indexes respectively; a and b are speed equation parameters of the unmanned ship on the water surface, u is the sailing speed of the unmanned ship, and epsilon is an accelerator input value.
Optionally, the regulation module is further configured to:
Respectively calculating an expected course value and an expected speed value by adopting the return control parameter and the compensation control parameter;
Calculating a thrust parameter of the unmanned ship propeller based on the expected heading value and the expected speed value;
and controlling the unmanned ship to return to the fixed point position according to the thrust parameter.
Optionally, the thrust parameter includes: left and right throttle amounts;
the calculation of the left throttle amount is shown as follows:
the calculation of the right throttle amount is shown as follows:
In the above formula, T left is the left throttle amount, T right is the right throttle amount, k 1 and k 2 are power distribution scaling factors, thrust is the virtual thrust equivalent to the expected speed value, rudder is the virtual rudder amount equivalent to the expected heading value, wherein,
R is a fixed point holding tolerance radius value, d is a position distance value,Is the offset angle value.
Optionally, the determining that the unmanned ship deviates from the fixed point position specifically includes:
calculating a position distance value between the current position and the fixed point position of the unmanned ship;
If the position distance value is larger than the fixed point keeping tolerance radius value, determining that the unmanned ship deviates from the fixed point position;
and if the position distance value is smaller than the fixed point keeping tolerance radius value, determining that the unmanned ship does not deviate from the fixed point position.
It will be clearly understood by those skilled in the art that, for convenience and brevity, the specific working process of the apparatus described above may refer to the corresponding process in the foregoing method embodiment, which is not described herein again.
Further, an embodiment of the present application further provides an electronic device, including: the system comprises a memory, a processor and a computer program stored in the memory and capable of running on the processor, wherein the processor realizes the fixed-point return regulation method of the double-body unmanned ship according to the embodiment when executing the program.
Further, the embodiment of the application also provides a computer readable storage medium, which stores computer executable instructions for causing a computer to execute the fixed-point return regulation method of the double-body unmanned ship.
While the foregoing is directed to the preferred embodiments of the present invention, it will be appreciated by those skilled in the art that changes and modifications may be made without departing from the principles of the invention, such changes and modifications are also intended to be within the scope of the invention.
Claims (8)
1. The fixed-point return regulation and control method for the double-body unmanned ship is characterized by comprising the following steps of:
When the unmanned ship deviates from the fixed point position, respectively calculating a return control parameter of the unmanned ship and a compensation control parameter for coping with environmental disturbance;
controlling the unmanned ship to return to the fixed point position based on the return control parameter and the compensation control parameter;
the controlling the unmanned ship to return to the fixed point position based on the return control parameter and the compensation control parameter comprises the following steps:
Respectively calculating an expected course value and an expected speed value by adopting the return control parameter and the compensation control parameter;
Calculating a thrust parameter of the unmanned ship propeller based on the expected heading value and the expected speed value;
Controlling the unmanned ship to return to the fixed point position according to the thrust parameter;
The thrust parameters include: left and right throttle amounts;
the calculation of the left throttle amount is shown as follows:
the calculation of the right throttle amount is shown as follows:
In the above formula, T left is the left throttle amount, T right is the right throttle amount, k 1 and k 2 are power distribution scaling factors, thrust is the virtual thrust equivalent to the expected speed value, rudder is the virtual rudder amount equivalent to the expected heading value, wherein,
R is a fixed point holding tolerance radius value, d is a position distance value,Is the offset angle value.
2. The fixed point return voyage control method of a double-body unmanned ship according to claim 1, wherein the return voyage control parameters comprise a return voyage speed value and a return voyage angle value;
the calculation operation of the return control parameters specifically comprises the following steps:
respectively acquiring the current position and the deviation angle value of the unmanned ship;
calculating a position distance value between the current position and the fixed point position of the unmanned ship;
Calculating a return navigational speed value based on the difference between the position distance value and the fixed point holding tolerance radius value;
and calculating a heading angle value based on the deviation angle value.
3. The fixed-point return regulation and control method of the double-body unmanned ship according to claim 2, wherein the calculation operation of the deviation angle value is specifically as follows:
acquiring longitude and latitude of the current position and longitude and latitude of the fixed point position respectively;
A departure angle value is calculated based on the longitude and latitude of the current location and the longitude and latitude of the fixed point location.
4. The fixed point return voyage control method of a double-body unmanned ship according to claim 1, wherein the compensation control parameters comprise a compensation speed value and a compensation angle value;
the calculation reference of the compensation control parameter is specifically as follows:
Respectively acquiring real-time state parameters and historical state parameters;
Constructing a state equation by using the historical state parameters and preset first disturbance parameters, and constructing an observation equation by using the real-time state parameters and preset second disturbance parameters;
invoking a preset Kalman filter to estimate the state equation and the observation equation to obtain a compensation speed value and a compensation heading angle value respectively;
The preset Kalman filter is a Kalman filter model built by taking historical state parameters and first disturbance parameters of the unmanned ship as state variables and taking real-time measurement state parameters and second disturbance parameters of the unmanned ship as measurement variables.
5. The method for controlling the fixed-point return voyage of the twin-hull unmanned ship according to claim 4, wherein the step of calling a preset kalman filter to estimate the state equation and the observation equation to obtain a compensation speed value and a compensation heading angle value respectively comprises the steps of:
substituting the state equation and the observation equation into a preset Kalman filter, and calling a noise statistics time-varying estimator by the preset Kalman filter to evaluate according to the statistical noise to obtain an evaluation parameter;
Constructing a kinetic equation by using the evaluation parameters;
and carrying out inverse dynamics solving on the dynamics equation to respectively obtain a compensation speed value and a compensation heading angle value.
6. The method for controlling the fixed point return voyage of a double-body unmanned ship according to claim 5, wherein the constructing a kinetic equation by using the evaluation parameters comprises:
adding the evaluation parameters into a preset dynamics model, and solving a dynamics equation;
The preset dynamics model is constructed based on a position coordinate system of the double-body unmanned ship and a first-order linear response model;
The kinetic model is shown in the following formula:
In the above formula, delta is an input rudder angle, r is a yaw speed value of the unmanned ship, and K r and T r are ship steering indexes respectively; a and b are speed equation parameters of the unmanned ship on the water surface, u is the sailing speed of the unmanned ship, and epsilon is an accelerator input value.
7. The fixed point return voyage control method of a double-body unmanned ship according to any one of claims 1 to 6, wherein the determining of the deviation of the unmanned ship from the fixed point position is specifically:
calculating a position distance value between the current position and the fixed point position of the unmanned ship;
If the position distance value is larger than the fixed point keeping tolerance radius value, determining that the unmanned ship deviates from the fixed point position;
and if the position distance value is smaller than the fixed point keeping tolerance radius value, determining that the unmanned ship is not deviated from the fixed point position.
8. A fixed point return regulation and control device of a twin-hull unmanned ship, the device comprising:
The calculation module is used for respectively calculating the return control parameters of the unmanned ship and the compensation control parameters for coping with the environmental disturbance when the unmanned ship is determined to deviate from the fixed point position;
The regulation and control module is used for controlling the unmanned ship to return to the fixed point position based on the return control parameter and the compensation control parameter;
the controlling the unmanned ship to return to the fixed point position based on the return control parameter and the compensation control parameter comprises the following steps:
Respectively calculating an expected course value and an expected speed value by adopting the return control parameter and the compensation control parameter;
Calculating a thrust parameter of the unmanned ship propeller based on the expected heading value and the expected speed value;
Controlling the unmanned ship to return to the fixed point position according to the thrust parameter;
The thrust parameters include: left and right throttle amounts;
the calculation of the left throttle amount is shown as follows:
the calculation of the right throttle amount is shown as follows:
In the above formula, T left is the left throttle amount, T right is the right throttle amount, k 1 and k 2 are power distribution scaling factors, thrust is the virtual thrust equivalent to the expected speed value, rudder is the virtual rudder amount equivalent to the expected heading value, wherein,
R is a fixed point holding tolerance radius value, d is a position distance value,Is the offset angle value.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210662859.3A CN115014355B (en) | 2022-06-13 | 2022-06-13 | A method and device for controlling fixed-point return of a catamaran unmanned ship |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210662859.3A CN115014355B (en) | 2022-06-13 | 2022-06-13 | A method and device for controlling fixed-point return of a catamaran unmanned ship |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN115014355A CN115014355A (en) | 2022-09-06 |
| CN115014355B true CN115014355B (en) | 2024-11-26 |
Family
ID=83075012
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210662859.3A Active CN115014355B (en) | 2022-06-13 | 2022-06-13 | A method and device for controlling fixed-point return of a catamaran unmanned ship |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115014355B (en) |
Families Citing this family (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114217625B (en) * | 2021-12-15 | 2024-05-28 | 浙江九段智能科技有限公司 | Control method for specified point floating mode of autonomous underwater vehicle |
| CN117742352B (en) * | 2024-02-21 | 2024-05-10 | 陕西欧卡电子智能科技有限公司 | Vector propulsion unmanned ship-based track control method, device, equipment and medium |
Family Cites Families (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109814547B (en) * | 2018-12-24 | 2022-08-05 | 上海大学 | Device and method for maintaining the course of unmanned boat under the action of wind and wave disturbance |
| CN111930119B (en) * | 2020-07-31 | 2022-04-08 | 河海大学 | Velocity-adaptive autonomous path planning and accurate motion tracking method for unmanned ships |
-
2022
- 2022-06-13 CN CN202210662859.3A patent/CN115014355B/en active Active
Non-Patent Citations (3)
| Title |
|---|
| 基于扰动补偿的无人艇跟踪控制算法研究;周利;《中国优秀硕士学位论文全文数据库基础科学辑》;20190115(第1期);A002-1392-第12-17、36-37页 * |
| 基于航向约束的无人艇位姿保持制导策略;李朋博等;《上海交通大学学报》;20200928;第54卷(第9期);第988-992页 * |
| 水面机器人主动增强LPV航向保持控制;熊俊峰等;《信息与控制》;20180615;第47卷(第3期);第268-272页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN115014355A (en) | 2022-09-06 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP4339016B2 (en) | Thrust distribution method and thrust distribution apparatus | |
| Tannuri et al. | Dynamic positioning systems: An experimental analysis of sliding mode control | |
| CN111487966B (en) | Self-adaptive path tracking control method for unmanned surface vehicle based on waypoints | |
| CN115014355B (en) | A method and device for controlling fixed-point return of a catamaran unmanned ship | |
| CN116360260B (en) | ASV preset performance control method based on trigger guidance and self-updating threshold | |
| CN115951581A (en) | High-speed unmanned ship path tracking control method based on improved EMPC | |
| CN114967702A (en) | Unmanned ship control system and path tracking method | |
| Liu | Pre-filtered backstepping control for underactuated ship path following | |
| Wang et al. | Adaptive nonlinear model predictive control for autonomous surface vessels with largely varying payload | |
| Han et al. | A Ship Path Tracking Control Method Using a Fuzzy Control Integrated Line-of-Sight Guidance Law | |
| CN114035567B (en) | Unmanned surface vehicle navigation control system | |
| Huo | Research on unmanned ship control system based on fuzzy PID | |
| Li et al. | Asymptotic trajectory tracking of underactuated non-minimum phase marine vessels | |
| Bui et al. | Optimal control design for automatic ship berthing by using bow and stern thrusters | |
| Bhattacharyya et al. | Target Path Iteration method for trajectory control of ships | |
| HE et al. | Monocular visual servo-based stabilization control of underactuated unmanned surface vehicle | |
| You et al. | Experimental research of the PID tune method for ship path following control | |
| Cabecinhas et al. | Trajectory tracking control of a nonlinear autonomous surface vessel | |
| JP4260612B2 (en) | Tidal current estimation method and apparatus, and navigation vehicle control method and apparatus using the same | |
| Belleter et al. | Observer based path following for underactuated marine vessels in the presence of ocean currents: A global approach-with proofs | |
| CN117608198B (en) | Method, system and device for distributing weighted pseudo-inverse thrust of propeller | |
| CN119024855B (en) | Unmanned ship path tracking control method | |
| YU et al. | Adaptive control of large transport ship based on grasshopper optimization algorithm | |
| CN117762152B (en) | Unmanned ship vector warehouse-in control method and system, unmanned ship and readable storage medium | |
| Frafjord et al. | A Comparative Study of Control Laws for a Maritime Surface Vessel Tracking an Underwater Vehicle |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |