CN105785999B - Unmanned boat course motion control method - Google Patents
Unmanned boat course motion control method Download PDFInfo
- Publication number
- CN105785999B CN105785999B CN201610269401.6A CN201610269401A CN105785999B CN 105785999 B CN105785999 B CN 105785999B CN 201610269401 A CN201610269401 A CN 201610269401A CN 105785999 B CN105785999 B CN 105785999B
- Authority
- CN
- China
- Prior art keywords
- angle
- course
- unmanned boat
- fuzzy
- rudder
- 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.)
- Expired - Fee Related
Links
- 238000000034 method Methods 0.000 title claims abstract description 34
- 238000010009 beating Methods 0.000 claims abstract description 12
- 239000011159 matrix material Substances 0.000 claims description 26
- 238000001914 filtration Methods 0.000 claims description 20
- 238000005259 measurement Methods 0.000 claims description 16
- 238000012545 processing Methods 0.000 claims description 9
- 238000001514 detection method Methods 0.000 claims description 7
- 238000005070 sampling Methods 0.000 claims description 5
- 230000007704 transition Effects 0.000 claims description 4
- 230000004927 fusion Effects 0.000 claims description 3
- 230000001105 regulatory effect Effects 0.000 abstract description 2
- 230000006870 function Effects 0.000 description 21
- 230000000694 effects Effects 0.000 description 7
- 238000004422 calculation algorithm Methods 0.000 description 6
- 238000005516 engineering process Methods 0.000 description 6
- 230000008859 change Effects 0.000 description 5
- 238000010586 diagram Methods 0.000 description 4
- 230000008569 process Effects 0.000 description 4
- 230000001133 acceleration Effects 0.000 description 3
- 230000008901 benefit Effects 0.000 description 3
- 230000011218 segmentation Effects 0.000 description 3
- 230000009897 systematic effect Effects 0.000 description 3
- 238000013461 design Methods 0.000 description 2
- 238000005183 dynamical system Methods 0.000 description 2
- 238000002474 experimental method Methods 0.000 description 2
- YBMRDBCBODYGJE-UHFFFAOYSA-N germanium dioxide Chemical compound O=[Ge]=O YBMRDBCBODYGJE-UHFFFAOYSA-N 0.000 description 2
- 230000005484 gravity Effects 0.000 description 2
- 238000005096 rolling process Methods 0.000 description 2
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical compound C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 description 1
- 238000012935 Averaging Methods 0.000 description 1
- 238000004458 analytical method Methods 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 238000005094 computer simulation Methods 0.000 description 1
- 230000001276 controlling effect Effects 0.000 description 1
- 238000011156 evaluation Methods 0.000 description 1
- 230000006698 induction Effects 0.000 description 1
- 230000035800 maturation Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000003534 oscillatory effect Effects 0.000 description 1
- 230000035945 sensitivity Effects 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/0206—Control of position or course in two dimensions specially adapted to water vehicles
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B13/00—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion
- G05B13/02—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric
- G05B13/04—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- General Physics & Mathematics (AREA)
- Remote Sensing (AREA)
- Radar, Positioning & Navigation (AREA)
- Aviation & Aerospace Engineering (AREA)
- Health & Medical Sciences (AREA)
- Artificial Intelligence (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Evolutionary Computation (AREA)
- Medical Informatics (AREA)
- Software Systems (AREA)
- Feedback Control In General (AREA)
Abstract
The present invention provides a kind of unmanned boat course motion control method, including：The real-time course angle of unmanned boat of receiving sensor module acquisition, the sensor assembly include：Gyroscope, accelerometer and magnetic field strength transducer；It compares the real-time course angle and obtains unmanned boat course deviation, course deviation rate with the setting course angle；Using Fuzzy PID according to the course deviation and the course deviation rate determine instruction rudder angle, described instruction rudder angle includes beating rudder direction and beating rudder speed；Described instruction rudder angle is sent to motor driver；The motor driver beats rudder direction according to and the rudder speed control unmanned boat of beating moves.The present invention realizes the Heading control of unmanned boat, improves the steady-state behaviour, dynamic performance and control accuracy of the motion control of unmanned boat course, reduces the regulating time of Heading control.
Description
Technical field
The present embodiments relate to unmanned boat movement control technology field more particularly to a kind of motion controls of unmanned boat course
Method.
Background technology
PID controller is a kind of conventional design, due in the design, the parameter of controlled system being thought of as constant, they
Can only in a certain range effectively, their disadvantage is that closed-loop control system does not have robustness, and practical marine system is normal
With uncertain, non-linear, instability and complexity, it is difficult to establish accurate model equation, or even cannot directly carry out
Analysis and expression, thus expected control effect cannot be obtained.And processing of the human operators by them to met situation
Experience and intelligent Understanding and explanation can efficiently control ship's navigation.Therefore, people naturally enough begin look for being similar to people
The intelligent control method of work operation.Wherein FUZZY ALGORITHMS FOR CONTROL can simply be had in face of system complicated and that model is not known
The control of effect, but simple fuzzy controller does not have integral element, i.e., as the input e and e of fuzzy controllercIn zero or
When near person zero, output is similarly zero, therefore is difficult to completely eliminate steady-state error, Er Qie in the system of fuzzy control
In the case that variable classification is insufficient, small oscillatory occurences is usually had near equalization point.
Invention content
The embodiment of the present invention provides a kind of unmanned boat course motion control method, to overcome above-mentioned technical problem.
Unmanned boat course of the present invention motion control method, including：
The real-time course angle of unmanned boat of receiving sensor module acquisition, the sensor assembly include：Gyroscope, acceleration
Meter and magnetic field strength transducer；
It compares the real-time course angle and obtains unmanned boat course deviation, course deviation rate with the setting course angle；
It is described using Fuzzy PID according to the course deviation and the course deviation rate determine instruction rudder angle
Ordered rudder angle includes beating rudder direction and beating rudder speed；
Described instruction rudder angle is sent to motor driver；
The motor driver beats rudder direction according to and the rudder speed control unmanned boat of beating moves.
Further, the comparison real-time course angle obtains unmanned boat course deviation, boat with the setting course angle
To before deviation ratio, further include：
The angular speed of gyroscope detection three axis of accelerometer is received, magnetic field strength transducer acquires the course of unmanned boat
Angle, accelerometer acquire the roll angle of unmanned boat, pitch angle；
Roll angle, the pitching of the unmanned boat of the accelerometer acquisition are corrected using Kalman filtering according to the angular speed
Angle；
The revised roll angle, pitch angle are merged with the course angle determines Precision course direction angle.
Further, the unmanned boat for correcting the accelerometer acquisition using Kalman filtering according to the angular speed
Roll angle, pitch angle, including：
The state equation of system is constructed according to the angular speed deviation of attitude transducer mould gyroscope acquisition in the block, angular speed
For
Measurement equation is
Wherein, α indicates that attitude angle, the attitude angle include roll angle, pitch angle, and β indicates the angular speed of gyroscope output
Deviation, Δ t indicate sampling period, ωk-1Indicate k-1 (k=1,2, n) moment gyroscope detection angular speed, wgTable
Show the white noise of gyroscope output, waWhite noise is exported for accelerometer, the Z (k) is the measured value of accelerometer；
According to state equation and measurement equation combination formula
X (k | k-1)=AX (k-1 | k-1)+BU (k-1) (3)
The optimum attitude angle at k-1 moment is obtained, the attitude angle at current time is predicted by the optimum attitude angle at k-1 moment,
In, X (k-1 | k-1) is the optimum attitude angle at k-1 moment, and X (k | k-1) is the attitude angle according to a preliminary estimate at k moment, and U (k) is current
The controlled quentity controlled variable at moment, the A are sytem matrix, B input matrixes in order to control；
According to formula
P (k | k-1)=AP (k-1 | k-1) AT+Q (4)
Calculating the covariance of prediction error, wherein P (k | k-1) is the covariance for predicting error at the k moment, P (k-1 | k-1)
For the covariance of k-1 moment optimal estimation values, Q is system noise covariance, ATFor the transposed matrix of sytem matrix；
According to the covariance of the prediction error, using formula
Kg(k)=P (k | k-1) HT[HP(k|k-1)HT+R]-1 (5)
Calculate kalman gain, wherein the Kg is Kalman filter gain, and H is observing matrix, and R is measurement noise
Covariance matrix, the HTFor the transposed matrix of observing matrix；
According to the kalman gain, using formula
X (k | k)=X (k | k-1)+Kg(k)[Z(k)-HX(k|k-1)] (6)
Correct the optimum attitude angle at k moment, wherein the Z (k) is the measured value of accelerometer.
Further, the revised roll angle, pitch angle are merged with the course angle determines Precision course direction angle,
Including：
Formula is used according to revised roll angle, pitch angle
The magnetic field intensity course angle being transformed by sensor coordinate system in horizontal coordinates, wherein MbFor correspondence
The magnetic field intensity of sensor coordinate system, MhTo correspond to the magnetic field intensity of horizontal coordinates,For transition matrix；
The magnetic heading angle of unmanned boat hull plane is calculated according to the component of the magnetic field intensity in horizontal coordinates；
Precision course direction angle is determined according to the magnetic heading angle and magnetic declination.
Further, described to use Fuzzy PID according to the course deviation determine instruction rudder angle, including：
Using triangular function
The exact value of course deviation, course deviation rate is subjected to Fuzzy processing, obtains fuzzy output set, wherein x is
Course deviation or course deviation rate, a, b, c specify the shape of triangular function, and require a≤b≤c；
Fuzzy tuning table is determined according to the fuzzy output set, and is adjusted according to the fuzzy tuning table described fuzzy defeated
Go out proportionality coefficient, integral coefficient and differential coefficient that set determines PID controller；
Fuzzy judgment is carried out to the proportionality coefficient, integral coefficient and differential coefficient using weighted mean method, obtains institute
State the exact value of proportionality coefficient, integral coefficient and differential coefficient；
The exact value of the proportionality coefficient, integral coefficient and differential coefficient is inputted into the PID controller acquisition instruction
Rudder angle.
Further, the fuzzy tuning table is：
Wherein, e is course deviation, ecFor course deviation rate, NB, NM, NS, Z, PS, PM, PB, PM are fuzzy subset, kpFor
Proportionality coefficient；
Wherein, kiFor integral coefficient；
Wherein, kdFor differential coefficient.
The present invention solves Marine Autopilot steady-state behaviour and bad dynamic performance in Heading control, and control accuracy is low, adjusts
The problem of saving long time, poor robustness.
Description of the drawings
In order to more clearly explain the embodiment of the invention or the technical proposal in the existing technology, to embodiment or will show below
There is attached drawing needed in technology description to be briefly described, it should be apparent that, the accompanying drawings in the following description is this hair
Some bright embodiments for those of ordinary skill in the art without having to pay creative labor, can be with
Obtain other attached drawings according to these attached drawings.
Fig. 1 is unmanned boat course motion control method flow chart of the present invention；
Fig. 2 is the attitude data schematic diagram of drawing tool of the present invention storage；
Fig. 3 is the related angle and coordinate relation schematic diagram of magnetic field strength transducer of the present invention；
Fig. 4 is the fuzzy domain and membership function schematic diagram of variable of the present invention；
Fig. 5 is the control flow chart of fuzzy of the present invention；
Fig. 6 is Fuzzy PID Control System schematic diagram of the present invention.
Specific implementation mode
In order to make the object, technical scheme and advantages of the embodiment of the invention clearer, below in conjunction with the embodiment of the present invention
In attached drawing, technical scheme in the embodiment of the invention is clearly and completely described, it is clear that described embodiment is
A part of the embodiment of the present invention, instead of all the embodiments.Based on the embodiments of the present invention, those of ordinary skill in the art
The every other embodiment obtained without creative efforts, shall fall within the protection scope of the present invention.
Fig. 1 is unmanned boat course motion control method flow chart of the present invention, as shown in Figure 1, the present embodiment method, including：
Step 101, the real-time course angle of unmanned boat of receiving sensor module acquisition, the sensor assembly include：Gyro
Instrument, accelerometer and magnetic field strength transducer；
Step 102, the comparison real-time course angle obtain unmanned boat course deviation, course deviation with the setting course angle
Rate；
Step 103, using Fuzzy PID according to the course deviation and the course deviation rate determine instruction rudder
Angle, described instruction rudder angle include beating rudder direction and beating rudder speed；
Described instruction rudder angle is sent to motor driver by step 104；
Step 105, the motor driver beat rudder direction according to and the rudder speed control unmanned boat of beating moves.
Further, the comparison real-time course angle obtains unmanned boat course deviation, boat with the setting course angle
To before deviation ratio, further include：
The angular speed of gyroscope detection three axis of accelerometer is received, magnetic field strength transducer acquires the course of unmanned boat
Angle, accelerometer acquire the roll angle of unmanned boat, pitch angle；
Roll angle, the pitching of the unmanned boat of the accelerometer acquisition are corrected using Kalman filtering according to the angular speed
Angle；
The revised roll angle, pitch angle are merged with the course angle determines Precision course direction angle.
Specifically, the attitude angle of unmanned boat：Roll angle, pitch angle are controlled in navigation by attitude transducer GY-86
System is accurate in the process and obtains in time, is accurately controlled to unmanned boat.Attitude transducer mould three axis in the block
Accelerometer and three-axis gyroscope can carry out alone the measurement of attitude angle, but any one list of both sensors
The characteristics of only to use the accuracy that all cannot be guaranteed to measure, and two kinds of sensors are had nothing in common with each other.We are to accelerometer first
Sensor is analyzed, and the principle summary that accelerometer obtains attitude angle is：Accelerometer is able to detect that acceleration of gravity exists
Each axial component, when attitude of carrier changes, the detected value of three axial directions of accelerometer can also change, according to
Attitude data can be calculated in each axial detected value.Secondly, the vibrations interference during unmanned boat navigation can be serious
Influence the accuracy of accelerometer detection.For gyroscope, it is able to detect that the angular speed of corresponding three axis, so
It is multiplied afterwards with the sampling period and can be obtained by the angle that a sampling period turned over, but the posture obtained in this way
Angle short time internal ratio is more accurate, measurement error will be caused increasing due to the effect of drift for a long time.Effectively to solve to shake
The error problem that dynamic interference and drift are brought, we are filtered using Kalman (Kalman), to coming from accelerometer and gyroscope
Acquisition signal merged.Kalman filtering is a kind of high performance recursion filter, and maximum feature is can be at one
Not exclusively, the state of dynamical system is even estimated in the time of measuring sequence comprising noise.Kalman filtering is with lowest mean square
Difference is the optimum criterion of estimation, in conjunction with the algorithm of a set of recurrence estimation, and then realizes the prediction of state.Kalman filtering logarithm
Requirement according to amount of storage and operand is smaller, therefore it is suitble to processing and microcontroller operation in real time.
Further, the unmanned boat for correcting the accelerometer acquisition using Kalman filtering according to the angular speed
Roll angle, pitch angle, including：
The state equation of system is constructed according to the angular speed deviation of attitude transducer mould gyroscope acquisition in the block, angular speed
For
Measurement equation is
Wherein, α indicates that attitude angle, the attitude angle include roll angle, pitch angle, and β indicates the angular speed of gyroscope output
Deviation, Δ t indicate sampling period, ωk-1Indicate k-1 (k=1,2, n) moment gyroscope detection angular speed, wgTable
Show the white noise of gyroscope output, waWhite noise is exported for accelerometer, the Z (k) is the measured value of accelerometer；
According to state equation and measurement equation combination formula
X (k | k-1)=AX (k-1 | k-1)+BU (k-1) (3)
The optimum attitude angle at k-1 moment is obtained, the attitude angle at current time is predicted by the optimum attitude angle at k-1 moment,
In, X (k-1 | k-1) is the optimum attitude angle at k-1 moment, and X (k | k-1) is the attitude angle according to a preliminary estimate at k moment, and U (k) is current
The controlled quentity controlled variable at moment, the A are sytem matrix, B input matrixes in order to control；
According to formula
P (k | k-1)=AP (k-1 | k-1) AT+Q (4)
Calculating the covariance of prediction error, wherein P (k | k-1) is the covariance for predicting error at the k moment, P (k-1 | k-1)
For the covariance of k-1 moment optimal estimation values, Q is system noise covariance, ATFor the transposed matrix of sytem matrix；
According to the covariance of the prediction error, using formula
Kg(k)=P (k | k-1) HT[HP(k|k-1)HT+R]-1 (5)
Calculate kalman gain, wherein the Kg is Kalman filter gain, and H is observing matrix, and R is measurement noise
Covariance matrix, the HTFor the transposed matrix of observing matrix；
According to the kalman gain, using formula
X (k | k)=X (k | k-1)+Kg(k)[Z(k)-HX(k|k-1)] (6)
Correct the optimum attitude angle at k moment, wherein the Z (k) is the measured value of accelerometer.
Specifically, the present embodiment predicts posture by the measurement data of gyroscope, then passes through the survey of accelerometer
Magnitude is modified.State equation and measurement equation are formula (1) and formula (2), and card is combined by state equation and measurement equation
Germania theory can obtain the process of the iteration of numerical computations.Two time update equations of Kalman filtering can be obtained first
Formula (3) and formula (4).Formula (3) is predicted the state value at current time, formula (4) basis by the optimal estimation value at k-1 moment
The covariance of prediction error is calculated in the covariance at k-1 moment.Also to realize that state updates after deadline update, card
Three state renewal equations of Kalman Filtering such as formula (5) and formula (6), effect mainly obtain kalman gain, in turn
Seek the optimal estimation value at k moment.According to
P (k | k)=[I-KgH]P(k|k-1) (7)
The covariance of current time optimal estimation value is calculated, prepares for the interative computation at next moment.Wherein,
The matrix that I is 1 measures single model list, I=1.When system enters k+1 states, P (k | k) is exactly the P (k-1 of 7 formula of formula
| k-1), which act as the optimal estimation covariance at k-1 moment being updated to the optimal estimation covariance at k moment, carries out karr
Graceful filtering calculates involved system variable initial value X (0), predicting covariance initial value P (0 | 0), system noise association
Variance Q, measurement noise covariance R values are as follows：
Kalman filtering is a kind of high performance recursion filter, and maximum feature is can be at one not exclusively, even
Including estimating the state of dynamical system in the time of measuring sequence of noise.Kalman filtering is relative to other filtering such as wiener
The advantages of Wiener is filtered, is moment corresponding whole measured value before not needing, Kalman filtering is estimated according to previous
Evaluation and a nearest measured value estimate the current value of signal, it is estimated with state equation and recurrence estimation algorithm
, therefore Kalman filtering does not require the stationarity and timeinvariance of signal.Kalman filtering is estimation with Minimum Mean Square Error
Optimum criterion in conjunction with the algorithm of a set of recurrence estimation, and then realizes the prediction of state.Its basic ideas are：Using signal
With the state-space model of noise, state variable is estimated by the measured value completion of estimated value and current time to previous moment
The update of meter obtains the estimated value at current time.With the process model of system, come the system for predicting NextState.Assuming that now
System mode be k, according to the model of system, present status can be predicted based on the laststate of system.
For verify Kalman filtering effect, for accelerometer vibrations interference and gyroscope drift phenomenon done as
Lower experiment：Attitude transducer module is fixed on unmanned boat, unmanned boat engine is opened and generates vibrating effect, use microcontroller
Device STM32 samples accelerometer and gyroscope by iic bus using 20ms as the period, is then calculated by Kalman filtering
The attitude data of two kinds of sensors of method pair is handled, then by posture information is transmitted to by serial ports with treated before processing
Position machine display preserves, and finally uses Matlab drawing functions by the attitude data of storage as shown in Fig. 2, Kalman filters as seen from the figure
The enough interference for effectively eliminating accelerometer of wave energy and the drift of gyroscope influence, and obtain relatively accurate posture information.By
The calculating of kalman filter method can preferably correct roll angle, pitch angle.
Further, described merge the revised roll angle, pitch angle with the course angle determines Precision course direction
Angle, including：
Formula is used according to revised roll angle, pitch angle
The magnetic field intensity course angle being transformed by sensor coordinate system in horizontal coordinates, wherein MbFor correspondence
The magnetic field intensity of sensor coordinate system, MhTo correspond to the magnetic field intensity of horizontal coordinates,For transition matrix；
The magnetic heading angle of unmanned boat hull plane is calculated according to the component of the magnetic field intensity in horizontal coordinates；
Precision course direction angle is determined according to the magnetic heading angle and magnetic declination.
Specifically, course angle is mainly obtained by magnetic field strength transducer, under horizontality, magnetic field intensity sensing
Device by being obtained with relatively accurate course value after the compass deviation compensation based on ellipse hypothesis, when magnetic field strength transducer not
When in horizontality, it is necessary to according to above-mentioned required roll angle and pitch angle and magnetic field strength transducer acquisition data into
Row fusion, seeks course angle.As shown in figure 3, Hx、Hy、HzThe magnetic of three axis under magnetic field strength transducer body coordinate system is indicated respectively
Field intensity, wherein HxIt is directed toward the direction of advance of carrier.HX、HY、HZFor H under any attitudex、Hy、HzThrowing under horizontal coordinates
Shadow.For pitch angle, θ is roll angle, and NS is the axis of geographical south poles, and N'S' is the axis of the magnetic south arctic.
As shown in Figure 3, it is known that the magnetic field intensity of three axis must be transformed under horizontal coordinates by the parsing of course angle to be acquired,
And meet, magnetic heading angle angle+ magnetic declination β=geography course angle, so can first calculate sensing data obtains magnetic heading angle,
In addition working as geomagnetic declination, to obtain geographical course angle.The following two situations of the algorithm of magnetic heading angle angle point：
The first situation, when magnetic field strength transducer is horizontally arranged, i.e., the coordinate system of sensor is pressed from both sides with horizontal coordinates
Angle is zero, at this point, HZThe magnetic vector of axis is zero, then：
The second situation, when there are when angle, illustrate that pitching, rolling occurs in carrier for magnetic field strength transducer and horizontal plane
Situation then must measure pitch angle by acceleration transducer and gyro sensorWith roll angle θ.Then by sensor coordinates
The Magnetic Field that system measures is transformed into horizontal coordinates, solves the magnetic heading angle of carrier.It is sat to level by sensor coordinate system
Marking the transition matrix for being is：
MbFor the magnetic field intensity of respective sensor coordinate system, MhFor the magnetic field intensity of corresponding horizontal coordinates.Coordinate system turns
Changing formula is：
The component form of formula (10) is：
Wherein, Mb x、Mb Y、Mb ZFor component of the magnetic field intensity on three axis of sensor coordinate system, Mh X、Mh Y、Mh ZIt is strong for magnetic field
Spend the component on three axis of horizontal coordinates.The variable of above formula is transformed into variable corresponding in coordinate system, you can obtain magnetic
Component of the induction in horizontal coordinates：
Magnetic heading angle angle so as to calculate hull plane is：
Angle=arctan (HY/HX) (15)
It after acquiring magnetic heading angle angle, checks in as geomagnetic declination β, can obtain geographical course angle α is
α=angle+ β (16)
After course angle pretreatment, overcome due on the course angular measurement that hull pitching, rolling, change of magnetic field strength are brought
Inaccuracy the accurate course angle of hull is obtained by data fusion.
Further, described to use Fuzzy PID according to the course deviation determine instruction rudder angle, including：
Using triangular function
The exact value of course deviation, course deviation rate is subjected to Fuzzy processing, obtains fuzzy output set, wherein x is
Course deviation or course deviation rate, a, b, c specify the shape of triangular function, and require a≤b≤c；
Fuzzy tuning table is determined according to the fuzzy output set, and is adjusted according to the fuzzy tuning table described fuzzy defeated
Go out proportionality coefficient, integral coefficient and differential coefficient that set determines PID controller；
Fuzzy judgment is carried out to the proportionality coefficient, integral coefficient and differential coefficient using weighted mean method, obtains institute
State the exact value of proportionality coefficient, integral coefficient and differential coefficient；
The exact value of the proportionality coefficient, integral coefficient and differential coefficient is inputted into the PID controller acquisition instruction
Rudder angle.
Specifically, for fuzzy technology and pid algorithm are combined, it common are the following two kinds：First, utilizing mould
Fuzzy controllers give PID controller Online Auto-tuning PID parameter, form Fuzzy self- turning parameter PID controller；Another kind is to adopt
Multimode segmentation controller is built with different variance thresholds, according to different conditions and segmentation is required to be controlled with different modalities
System makes different algorithms give full play to respective advantage, such as the multi-modal segmentation controls of P-FUZZY-PI in the different control stages
Device processed.The present embodiment uses the first control mode, as shown in figure 5, on the basis of PID direction controllers, with course deviation e
With the deviation variation rate e in coursecAs input, using fuzzy control to pid parameter Kp, KiAnd KdOnline self-tuning is carried out, with full
The foot different control stage requires the difference of controller parameter, to make controlled device have good dynamic and nature static
Energy.
Fuzzy PID Control System is the closed-loop control system being made of fuzzy controller and conventional PID controller two parts.
R indicates given set amount, and what y was indicated is the output quantity of system, and e indicates systematic error, the i.e. difference of setting value and real output value
Value, ecIndicate the change rate of systematic error, E and EcE and e is indicated respectivelycThe obtained fuzzy quantity after Fuzzy processing,
Kp、KiAnd KdThe adjusted value of three parameters of matching convention PID control of fuzzy controller output is represented, u refers to Traditional PID control
Device processed acts on the output quantity in controlled device.
The control flow of fuzzy paste PID control system as shown in fig. 6, pass through default value and system output value first
Comparison obtains systematic error e, error rate ec, input variable is subjected to Fuzzy processing subsequently into fuzzy controller,
Secondary, the input quantity after Fuzzy processing can obtain a fuzzy output set according to fuzzy tuning table, then be obscured to this defeated
Go out set and carry out defuzzification, obtains an accurate output valve.Its essence is one is found in certain output area most
Suitable output controlling value.The final output variable of fuzzy controller is respectively Proportional coefficient Kp, integral coefficient KiAnd differential coefficient
KdAdjustment amount.Finally, PID controller parameter is adjusted using the output quantity of application fuzzy control, the calculating adjusted
Formula is：
Wherein, Kp0, Ki0And Kd0For a reference value of parameter adjustment, Δ Kp, Δ KiWith Δ KdIt is have fuzzy algorithmic approach to obtain whole
It is quantitative.Usual fuzzy controller is made of following three parts：The blurring of input variable and the selection of degree of membership, fuzzy control are whole
Determine table and the ambiguity solution of output variable.
There are two inputs in the present embodiment fuzzy controller：Course error e and course error change rate ec, work as control
Since in the controls, input quantity is all clearly to be worth after structure determination processed, in order to make these, clearly value can be with language table
The fuzzy tuning table stated is adapted, and carries out approximate resoning, it is necessary to they are transformed into fuzzy quantity, the present embodiment is to quantify variable
For 7 grades, i.e., { negative big, in bearing, to bear small, zero, just small, center is honest }, it is abbreviated as { NB, NM, NS, Z, PS, PM, PB }.
Membership function is the basis that fuzzy set is applied to practical problem, and can the correct membership function that constructs be to make good use of, mould
The key of set is pasted, however up to the present, there are no the effective and unified methods of a maturation.In actual application, really
Determine membership function and still rely on experience to solve, the feedback information then obtained again by experiment or computer simulation is repaiied
Just.Existing frequently-used membership function includes Z functions, S function, trapezoidal function, bell function, triangular function and Gaussian
Function etc., considers sensitivity and the arithmetic speed of control, and the membership function of each variable is all made of triangle in the present embodiment
Shape function formula (15), the effect of the triangular function are to convert exact value to fuzzy value.The fuzzy domain of all variables and
Membership function is as shown in Figure 4：
Further, the fuzzy tuning table is：
Wherein, e is course deviation, ecFor course deviation rate, NB, NM, NS, Z, PS, PM, PB, PM are fuzzy subset, kpFor
Proportionality coefficient；
Wherein, kiFor integral coefficient；
Wherein, kdFor differential coefficient.
Specifically, it is obtained by above-mentioned fuzzy tuning table the result is that a fuzzy set, but needed in practical applications
One determining value of fuzzy control final output.An opposite monodrome that can represent this fuzzy set is taken in fuzzy set
Process be referred to as fuzzy decision.There are many method, the obtained results of different methods to be also different for Anti-fuzzy.Theoretically
It is the most reasonable using gravity model appoach, but this method calculating is more complicated, so in the higher system of requirement of real-time generally not
In this way.Simplest reverse formulating method is maximum membership degree method, and this method takes all fuzzy sets or is subordinate to
That maximum value of degree of membership is as output in function, but this method does not take into account the smaller value of degree of membership, generation
Table is bad, so being only used for relatively simple system.What is fallen between also has various averaging methods, the present embodiment to adopt
Weighted mean method.Its calculation formula is：
Wherein, μ (ui) it is the exact value that ambiguity solution exports, uiIt is the subset that output obscures domain, μ (ui) it is output subset
Corresponding degree of membership.
The basic domain of course error e is set as [- 45,45] in the present embodiment, course error change rate ecBase
This domain is set as [- 5 °/s, 5 °/s], by e, ec、ΔKp、ΔKi、ΔKdDomain in fuzzy set be set as -6, -4, -2,
0,2,4,6 }, so quantizing factor is respectively:Ke=6/45=0.133, Kec=6/5=1.2.All variables are quantified as 7 etc.
Grade, i.e., it is negative big, and it is negative small in bearing, zero, just small, center is honest }.
The most important part of fuzzy control is exactly Proportional coefficient Kp, integral coefficient KiWith differential coefficient KdFuzzy tuning table.
Kp、KiAnd KdFuzzy tuning table distinguish table 1 to table 3.
After fuzzy tuning table establishes, according to Kp、KiAnd KdFuzzy tuning table content determine fuzzy relation, calculate anti-
The fuzzy set of controlled quentity controlled variable variation is reflected, then weighted mean method is used to carry out fuzzy judgment, obtains Kp、KiAnd KdThe essence of adjustment amount
Really value.
The present invention improves the steady-state behaviour of unmanned boat course motion control, dynamic performance and control accuracy, reduces
The regulating time of Heading control.
Finally it should be noted that：The above embodiments are only used to illustrate the technical solution of the present invention., rather than its limitations；To the greatest extent
Present invention has been described in detail with reference to the aforementioned embodiments for pipe, it will be understood by those of ordinary skill in the art that：Its according to
So can with technical scheme described in the above embodiments is modified, either to which part or all technical features into
Row equivalent replacement；And these modifications or replacements, various embodiments of the present invention technology that it does not separate the essence of the corresponding technical solution
The range of scheme.
Claims (4)
1. a kind of unmanned boat course motion control method, which is characterized in that including：
The real-time course angle of unmanned boat of receiving sensor module acquisition, the sensor assembly include：Gyroscope, accelerometer with
And magnetic field strength transducer；
It compares the real-time course angle and obtains unmanned boat course deviation, course deviation rate with setting course angle；
Using Fuzzy PID according to the course deviation and the course deviation rate determine instruction rudder angle, described instruction
Rudder angle includes beating rudder direction and beating rudder speed；
Described instruction rudder angle is sent to motor driver；
The motor driver beats rudder direction according to and the rudder speed control unmanned boat of beating moves；
Before the comparison real-time course angle obtains unmanned boat course deviation, course deviation rate with the setting course angle,
Further include：
The angular speed of gyroscope detection three axis of accelerometer is received, magnetic field strength transducer acquires the course angle of unmanned boat, adds
Speedometer acquires the roll angle of unmanned boat, pitch angle；
Roll angle, the pitch angle of the unmanned boat of the accelerometer acquisition are corrected using Kalman filtering according to the angular speed；
The revised roll angle, pitch angle are merged with the course angle determines Precision course direction angle.
The roll angle of the unmanned boat for correcting the accelerometer acquisition using Kalman filtering according to the angular speed, pitching
Angle, including：
The state equation that system is constructed according to the angular speed deviation of sensor die gyroscope in the block acquisition, angular speed is
Measurement equation is
Wherein, α indicates that attitude angle, the attitude angle include roll angle, pitch angle, and β indicates the angular speed deviation of gyroscope output,
Δ t indicates sampling period, ωk-1Indicate k-1, k=1,2 ..., the angular speed of n moment gyroscopes detection, wgIndicate that gyroscope is defeated
The white noise gone out, waWhite noise is exported for accelerometer, the Z (k) is the measured value of accelerometer；
According to state equation and measurement equation combination formula
X (k | k-1)=AX (k-1 | k-1)+BU (k-1) (3)
The optimum attitude angle at k-1 moment is obtained, the attitude angle at current time is predicted by the optimum attitude angle at k-1 moment, wherein X
(k-1 | k-1) is the optimum attitude angle at k-1 moment, and X (k | k-1) is the attitude angle according to a preliminary estimate at k moment, and U (k) is current time
Controlled quentity controlled variable, the A is sytem matrix, B input matrixes in order to control；
According to formula
P (k | k-1)=AP (k-1 | k-1) AT+Q (4)
Calculate the covariance of prediction error, wherein P (k | k-1) is the covariance for predict error at the k moment, and P (k-1 | k-1) is k-1
The covariance of moment optimal estimation value, Q are system noise covariance, ATFor the transposed matrix of sytem matrix；
According to the covariance of the prediction error, using formula
Kg(k)=P (k | k-1) HT[HP(k|k-1)HT+R]-1 (5)
Calculate kalman gain, wherein the Kg(k) it is Kalman filter gain, H is observing matrix, and R assists for measurement noise
Variance matrix, the HTFor the transposed matrix of observing matrix；
According to the kalman gain, using formula
X (k | k)=X (k | k-1)+Kg(k)[Z(k)-HX(k|k-1)] (6)
Correct the optimum attitude angle at k moment, wherein the Z (k) is the measured value of accelerometer.
2. according to the method described in claim 1, it is characterized in that, the revised roll angle, pitch angle and the course
Angle fusion determines Precision course direction angle, including：
Formula is used according to revised roll angle, pitch angle
The magnetic field intensity course angle being transformed by sensor coordinate system in horizontal coordinates, wherein MbFor respective sensor
The magnetic field intensity of coordinate system, MhTo correspond to the magnetic field intensity of horizontal coordinates,For transition matrix；
The magnetic heading angle of unmanned boat hull plane is calculated according to the component of the magnetic field intensity in horizontal coordinates；
Precision course direction angle is determined according to the magnetic heading angle and magnetic declination.
3. according to the method described in claim 1, it is characterized in that, described use Fuzzy PID according to the course
Deviation determine instruction rudder angle, including：
Using triangular function
The exact value of course deviation, course deviation rate is subjected to Fuzzy processing, obtains fuzzy output set, wherein x is course
Deviation or course deviation rate, a, b, c specify the shape of triangular function, and require a≤b≤c；
Fuzzy tuning table is determined according to the fuzzy output set, and the fuzzy output collection is adjusted according to the fuzzy tuning table
Close proportionality coefficient, integral coefficient and the differential coefficient for determining PID controller；
Fuzzy judgment is carried out to the proportionality coefficient, integral coefficient and differential coefficient using weighted mean method, obtains the ratio
The exact value of example coefficient, integral coefficient and differential coefficient；
The exact value of the proportionality coefficient, integral coefficient and differential coefficient is inputted into the PID controller acquisition instruction rudder angle.
4. according to the method described in claim 1, it is characterized in that, the fuzzy tuning table is：
Wherein, e is course deviation, ecFor course deviation rate, NB, NM, NS, Z, PS, PM, PB, PM are fuzzy subset, kpFor ratio
Coefficient；
Wherein, kiFor integral coefficient；
Wherein, kdFor differential coefficient.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201610269401.6A CN105785999B (en) | 2016-04-27 | 2016-04-27 | Unmanned boat course motion control method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201610269401.6A CN105785999B (en) | 2016-04-27 | 2016-04-27 | Unmanned boat course motion control method |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN105785999A CN105785999A (en) | 2016-07-20 |
| CN105785999B true CN105785999B (en) | 2018-11-02 |
Family
ID=56398772
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201610269401.6A Expired - Fee Related CN105785999B (en) | 2016-04-27 | 2016-04-27 | Unmanned boat course motion control method |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN105785999B (en) |
Families Citing this family (22)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107315348B (en) * | 2017-07-04 | 2020-04-07 | 哈尔滨工程大学 | Position and posture control method of unmanned underwater vehicle based on punishment type wavelet network |
| CN108153311A (en) * | 2017-12-22 | 2018-06-12 | 武汉理工大学 | The intelligence control system and control method formed into columns towards unmanned boat |
| CN109084885A (en) * | 2018-08-14 | 2018-12-25 | 中国科学院上海高等研究院 | A kind of mechanical equipment vibration detection system, method, readable storage medium storing program for executing and terminal |
| CN109460041A (en) * | 2018-12-28 | 2019-03-12 | 珠海市汉图达科技有限公司 | A kind of unmanned boat auto-pilot control system and method |
| CN110426958A (en) * | 2019-08-06 | 2019-11-08 | 大连海事大学 | Unmanned ships and light boats navigation control method, system, storage medium and computer equipment |
| CN110703791B (en) * | 2019-10-22 | 2022-10-14 | 中国人民解放军海军工程大学 | Submarine X-rudder intelligent fuzzy control method under complex conditions |
| CN111061262B (en) * | 2019-11-19 | 2023-06-06 | 中国船舶重工集团公司第七0七研究所九江分部 | Course control method for reducing invalid steering |
| CN111399384B (en) * | 2020-04-27 | 2022-09-27 | 哈尔滨工程大学 | Remote control unmanned ship course fault-tolerant control method based on dynamic quantizer |
| CN111813107B (en) * | 2020-06-17 | 2021-11-02 | 珠海云洲智能科技股份有限公司 | Unmanned ship control method, unmanned ship control device and unmanned ship |
| CN112034717B (en) * | 2020-10-14 | 2022-07-08 | 德州海天机电科技有限公司 | Concrete intelligent scheduling control method |
| CN113093527B (en) * | 2021-04-07 | 2021-11-23 | 广东工业大学 | Unmanned ship anti-interference system with improved EKF and fuzzy PID double closed-loop control and use method |
| CN113325858A (en) * | 2021-06-08 | 2021-08-31 | 西北工业大学 | Simulated bat aircraft course control method based on flapping wing amplitude |
| CN113342011A (en) * | 2021-06-08 | 2021-09-03 | 西北工业大学 | Gliding course control method of simulated bat aircraft based on rolling mechanism |
| CN113342013B (en) * | 2021-06-08 | 2022-08-05 | 西北工业大学 | Course control method combining roll mechanism and flapping wing asymmetric phase difference |
| CN113900372A (en) * | 2021-09-03 | 2022-01-07 | 广东工业大学 | Unmanned ship course keeping method based on neural network active disturbance rejection control |
| CN113741292B (en) * | 2021-09-09 | 2022-10-21 | 中国船舶重工集团公司第七0七研究所九江分部 | Single-handle vehicle-rudder combined control device, system and method for sending instruction |
| CN113985878B (en) * | 2021-10-28 | 2023-12-19 | 西安应用光学研究所 | Navigation control method suitable for water surface detection unmanned ship fixed-point autonomous cruise mode |
| CN114371701B (en) * | 2021-12-17 | 2024-03-12 | 武汉理工大学 | Unmanned ship course control method, controller, autopilot and unmanned ship |
| CN114894187B (en) * | 2022-05-27 | 2024-04-19 | 哈尔滨工程大学 | Unmanned ship navigation method |
| CN114880874B (en) * | 2022-06-07 | 2024-03-12 | 东南大学 | Self-adaptive robust estimation method and system for parameters of unmanned ship on water surface |
| CN115469553B (en) * | 2022-11-02 | 2023-03-24 | 中国船舶集团有限公司第七〇七研究所 | Ship motion state reconstruction method, device, equipment and storage medium |
| CN116360260B (en) * | 2023-03-22 | 2023-10-20 | 大连海事大学 | ASV preset performance control method based on trigger guidance and self-updating threshold |
Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101881970A (en) * | 2010-06-04 | 2010-11-10 | 哈尔滨工程大学 | Twin-rudder synchronization control method of ship |
| CN103558854A (en) * | 2013-11-05 | 2014-02-05 | 武汉理工大学 | Course control method and system for sail navigation aid ship |
| CN103777522A (en) * | 2014-01-21 | 2014-05-07 | 上海海事大学 | Unmanned surface vessel linear tracking method based on fuzzy PID |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US9323236B2 (en) * | 2012-12-05 | 2016-04-26 | Aai Corporation | Fuzzy controls of towed objects |
-
2016
- 2016-04-27 CN CN201610269401.6A patent/CN105785999B/en not_active Expired - Fee Related
Patent Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101881970A (en) * | 2010-06-04 | 2010-11-10 | 哈尔滨工程大学 | Twin-rudder synchronization control method of ship |
| CN103558854A (en) * | 2013-11-05 | 2014-02-05 | 武汉理工大学 | Course control method and system for sail navigation aid ship |
| CN103777522A (en) * | 2014-01-21 | 2014-05-07 | 上海海事大学 | Unmanned surface vessel linear tracking method based on fuzzy PID |
Non-Patent Citations (1)
| Title |
|---|
| 无人艇运动模糊控制技术研究;董早鹏;《中国优秀硕士学位论文全文数据库》;20140415(第4期);第C036-196页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN105785999A (en) | 2016-07-20 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN105785999B (en) | Unmanned boat course motion control method | |
| Madyastha et al. | Extended Kalman filter vs. error state Kalman filter for aircraft attitude estimation | |
| CN111024064B (en) | SINS/DVL combined navigation method for improving Sage-Husa adaptive filtering | |
| CN101033973B (en) | Attitude determination method of mini-aircraft inertial integrated navigation system | |
| CN106979780B (en) | A kind of unmanned vehicle real-time attitude measurement method | |
| Davari et al. | An asynchronous adaptive direct Kalman filter algorithm to improve underwater navigation system performance | |
| CN105136145A (en) | Kalman filtering based quadrotor unmanned aerial vehicle attitude data fusion method | |
| CN103033186A (en) | High-precision integrated navigation positioning method for underwater glider | |
| CN111897225B (en) | Fuzzy self-adaptive output feedback control method and system for intelligent ship autopilot system | |
| CN110346821B (en) | SINS/GPS combined attitude-determining and positioning method and system for solving long-time GPS unlocking problem | |
| Qin et al. | Accuracy improvement of GPS/MEMS-INS integrated navigation system during GPS signal outage for land vehicle navigation | |
| CN111750865B (en) | Self-adaptive filtering navigation method for difunctional deep sea unmanned submersible vehicle navigation system | |
| CN112432644B (en) | Unmanned ship integrated navigation method based on robust adaptive unscented Kalman filtering | |
| CN110954102A (en) | Magnetometer-assisted inertial navigation system and method for robot positioning | |
| CN110941285A (en) | Unmanned aerial vehicle flight control system based on two IP cores | |
| RU2647205C2 (en) | Adaptive strap down inertial attitude-and-heading reference system | |
| CN108413986A (en) | A kind of gyroscope filtering method based on Sage-Husa Kalman filterings | |
| Nabavi et al. | Automatic landing control of a multi-rotor UAV using a monocular camera | |
| Keighobadi | Fuzzy calibration of a magnetic compass for vehicular applications | |
| CN104819717A (en) | Multi-rotor aircraft attitude detection method based on MEMS inertial sensor group | |
| CN112835369A (en) | Unmanned ship speed change curve path tracking control method based on ESO (electronic stability evaluation) drift angle estimation | |
| CN107340026A (en) | Unstable state level gauging value filtering method | |
| Chen et al. | A novel hybrid observation prediction methodology for bridging GNSS outages in INS/GNSS systems | |
| CN114200966B (en) | Unmanned aircraft target orientation equidistant tracking method based on perception information | |
| CN114935345A (en) | Vehicle-mounted inertial navigation mounting angle error compensation method based on pattern recognition |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| C06 | Publication | ||
| PB01 | Publication | ||
| C10 | Entry into substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant | ||
| CF01 | Termination of patent right due to non-payment of annual fee | ||
| CF01 | Termination of patent right due to non-payment of annual fee |
Granted publication date: 20181102 Termination date: 20200427 |