CN105785999A - Unmanned surface vehicle course motion control method - Google Patents

Abstract

The present invention provides an unmanned surface vehicle course motion control method. The method comprises the steps of receiving an unmanned surface vehicle real-time course angle acquired by a sensor module, wherein the sensor module comprises a gyroscope, an accelerometer and a magnetic field intensity sensor; comparing the real-time course angle and a set course angle to obtain an unmanned surface vehicle course deviation and a course deviation ratio; adopting a fuzzy PID control algorithm to determine an instruction rudder angle according to the course deviation and the course deviation ratio, wherein the instruction rudder angle comprises a steering direction and a steering speed; sending the instruction rudder angle to a motor driver; using the motor driver to control the unmanned surface vehicle motion according to the steering direction and the steering speed. The unmanned surface vehicle course motion control method of the present invention realizes the unmanned surface vehicle course control, and enables the steady-state performance, the dynamic performance and the control precision of the unmanned surface vehicle course motion to be improved and the course control adjustment time to be reduced.

Description

Unmanned boat course motion control method

Technical field

The present embodiments relate to unmanned boat movement control technology field, particularly relate to a kind of unmanned boat course motion control method.

Background technology

PID controller is a kind of conventional design, due in the design, the parameter of controlled system is thought of as constant, they can only be effective within the specific limits, their disadvantage is that closed-loop control system does not have robustness, and actual marine system often has uncertainty, non-linear, instability and complexity, is difficult to set up accurate model equation, even can not be made directly analysis and represent, thus intended control effect cannot be obtained.And human operators by them to the process experience of met situation and Intelligent Understanding and explanation, just can efficiently control ship's navigation.Therefore, people naturally enough begin look for being similar to manually-operated intelligent control method.Wherein FUZZY ALGORITHMS FOR CONTROL can carry out simple and effective control in the face of complicated and the unclear system of model, but simple fuzzy controller does not possess integral element, namely as input e and the e of fuzzy controller_cWhen being near zero or zero, its output is zero too, is therefore difficult to eliminate steady-state error in the system of fuzzy control, and when variable classification is insufficient, usually has little oscillatory occurences near equilibrium point.

Summary of the invention

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:

Receiving the real-time course angle of unmanned boat that sensor assembly gathers, described sensor assembly includes: gyroscope, accelerometer and magnetic field strength transducer；

Contrast described real-time course angle and obtain unmanned boat course deviation, course deviation rate with described set course angle；

Fuzzy PID is adopted to determine that ordered rudder angle, described ordered rudder angle include beating rudder direction and beating rudder speed according to described course deviation and described course deviation rate；

Described ordered rudder angle is sent to motor driver；

Described motor driver according to described beat rudder direction and described beat rudder speed controlling unmanned boat motion.

Further, the described real-time course angle of described contrast and described set course angle also include before obtaining unmanned boat course deviation, course deviation rate:

Receiving the angular velocity of gyroscope detection three axles of accelerometer, magnetic field strength transducer gathers the course angle of unmanned boat, and accelerometer gathers the roll angle of unmanned boat, pitch angle；

Adopt Kalman filtering will revise the roll angle of unmanned boat, pitch angle that described accelerometer gathers according to described angular velocity；

Described revised roll angle, pitch angle and described course angle are merged and determines Precision course direction angle.

Further, described according to described angular velocity adopt Kalman filtering will revise described accelerometer gather the roll angle of unmanned boat, pitch angle, including:

State equation according to the angular speed deviation of the gyroscope collection in attitude transducer module, angular speed structure system is

${\left[\begin{array}{c}\mathrm{\α}\\ \mathrm{\β}\end{array}\right]}_k=\left[\begin{array}{cc}1& -\mathrm{\Δ}t\\ 0& 1\end{array}\right]{\left[\begin{array}{c}\mathrm{\α}\\ \mathrm{\β}\end{array}\right]}_{k-1}+\left[\begin{array}{c}\mathrm{\Δ}t\\ 0\end{array}\right]{\mathrm{\ω}}_{k-1}+\left[\begin{array}{c}{w}_g\mathrm{\Δ}t\\ 0\end{array}\right]---\left(1\right)$

Measurement equation is

$Z\left(k\right)=\[\begin{array}{cc}1& 0\end{array}\]\left[\begin{array}{c}\mathrm{\α}\\ \mathrm{\β}\end{array}\right]+w_a---\left(2\right)$

Wherein, α represents attitude angle, and described attitude angle includes roll angle, pitch angle, and β represents the angular speed deviation that gyroscope exports, and Δ t represents sampling period, ω_k-1Expression k-1 (k=1,2 ..., the n) angular speed of moment gyroscope detection, w_gRepresent the white noise of gyroscope output, w_aWhite noise, the measured value that described Z (k) is accelerometer is exported for accelerometer；

According to state equation and measurement equation in conjunction with formula

X (k | k-1)=AX (k-1 | k-1)+BU (k-1) (3)

Obtain the optimum attitude angle in k-1 moment, the attitude angle of current time is predicted by the optimum attitude angle in k-1 moment, wherein, the optimum attitude angle that X (k-1 | k-1) is the k-1 moment, the attitude angle according to a preliminary estimate that X (k | k-1) is the k moment, the controlled quentity controlled variable that U (k) is current time, described A is sytem matrix, and described B is for controlling input matrix；

According to formula

P (k | k-1)=AP (k-1 | k-1) A^T+Q(4)

Calculating the covariance of forecast error, wherein, the covariance that P (k | k-1) is k moment forecast error, the covariance that P (k-1 | k-1) is k-1 moment optimal estimation value, Q is system noise covariance, A^TTransposed matrix for sytem matrix；

Covariance according to described forecast error, adopts formula

K_g(k)=P (k | k-1) H^T[HP(k|k-1)H^T+R]^-1(5)

Calculating Kalman gain, wherein, described Kg is Kalman filter gain, and H is observing matrix, and R is for measuring noise covariance matrix, described H^TTransposed matrix for observing matrix；

According to described Kalman gain, adopt formula

X (k | k)=X (k | k-1)+K_g(k)[Z(k)-HX(k|k-1)](6)

Revise the optimum attitude angle in k moment, wherein, the measured value that described Z (k) is accelerometer.

Further, described described revised roll angle, pitch angle and described course angle fusions determines Precision course direction angle, including:

Formula is adopted according to revised roll angle, pitch angle

$M^h=C_b^h{M}^b---\left(7\right)$

Described course angle is transformed into by sensor coordinate system the magnetic field intensity in horizontal coordinates, wherein, M^bFor the magnetic field intensity of respective sensor coordinate system, M^hFor the magnetic field intensity of corresponding horizontal coordinates,For transition matrix；

Component according to the magnetic field intensity in horizontal coordinates calculates the magnetic heading angle of unmanned boat hull plane；

Precision course direction angle is determined according to described magnetic heading angle and magnetic declination.

Further, described employing Fuzzy PID determines ordered rudder angle according to described course deviation, including:

Adopt triangular function

$f(x,a,b,c)=\left\{\begin{array}{cc}0,& x\≤a\\ \frac{x-a}{b-a},& a\≤x\≤b\\ \frac{c-x}{c-b},& b\≤x\≤c\\ 0,& c\≤x\end{array}\right.---\left(8\right)$

The exact value of course deviation, course deviation rate being carried out Fuzzy processing, obtains fuzzy output set, wherein, x is course deviation or course deviation rate, a, b, and c specifies the shape of triangular function, and requires a≤b≤c；

Determine fuzzy tuning table according to described fuzzy output set, and determine the proportionality coefficient of PID controller, integral coefficient and differential coefficient according to described fuzzy tuning table described fuzzy output set of adjusting；

Adopt weighted mean method that described proportionality coefficient, integral coefficient and differential coefficient are waken up with a start fuzzy judgment, it is thus achieved that the exact value of described proportionality coefficient, integral coefficient and differential coefficient；

The exact value of described proportionality coefficient, integral coefficient and differential coefficient is inputted described PID controller and obtains ordered rudder angle.

Further, described fuzzy tuning table is:

Wherein, e is course deviation, e_cFor course deviation rate, NB, NM, NS, Z, PS, PM, PB, PM are fuzzy subset, k_pFor proportionality coefficient；

Wherein, k_iFor integral coefficient；

Wherein, k_dFor differential coefficient.

The invention solves Marine Autopilot steady-state behaviour and bad dynamic performance in Heading control, control accuracy is low, the problem of regulating time length, poor robustness.

Accompanying drawing explanation

In order to be illustrated more clearly that the embodiment of the present invention or technical scheme of the prior art, the accompanying drawing used required in embodiment or description of the prior art will be briefly described below, apparently, accompanying drawing in the following describes is some embodiments of the present invention, for those of ordinary skill in the art, under the premise not paying creative work, it is also possible to obtain other accompanying drawing according to these accompanying drawings.

Fig. 1 is unmanned boat course of the present invention motion control method flow chart；

Fig. 2 is the attitude data schematic diagram of drawing instrument of the present invention storage；

Fig. 3 is related angle and the coordinate relation schematic diagram of magnetic field strength transducer of the present invention；

Fig. 4 is fuzzy domain and the 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.

Detailed description of the invention

For making the purpose of the embodiment of the present invention, technical scheme and advantage clearly, below in conjunction with the accompanying drawing in the embodiment of the present invention, technical scheme in the embodiment of the present invention is clearly and completely described, obviously, described embodiment is a part of embodiment of the present invention, rather than whole embodiments.Based on the embodiment in the present invention, the every other embodiment that those of ordinary skill in the art obtain under not making creative work premise, broadly fall into the scope of protection of the invention.

Fig. 1 is unmanned boat course of the present invention motion control method flow chart, as it is shown in figure 1, the present embodiment method, including:

The real-time course angle of unmanned boat that step 101, reception sensor assembly gather, described sensor assembly includes: gyroscope, accelerometer and magnetic field strength transducer；

Step 102, contrast described real-time course angle and described set course angle and obtain unmanned boat course deviation, course deviation rate；

According to described course deviation and described course deviation rate, step 103, employing Fuzzy PID determine that ordered rudder angle, described ordered rudder angle include beating rudder direction and beating rudder speed；

Step 104, described ordered rudder angle is sent to motor driver；

Step 105, described motor driver according to described beat rudder direction and described beat rudder speed controlling unmanned boat motion.

Further, the described real-time course angle of described contrast and described set course angle also include before obtaining unmanned boat course deviation, course deviation rate:

Receiving the angular velocity of gyroscope detection three axles of accelerometer, magnetic field strength transducer gathers the course angle of unmanned boat, and accelerometer gathers the roll angle of unmanned boat, pitch angle；

Adopt Kalman filtering will revise the roll angle of unmanned boat, pitch angle that described accelerometer gathers according to described angular velocity；

Described revised roll angle, pitch angle and described course angle are merged and determines Precision course direction angle.

Specifically, the attitude angle of unmanned boat: roll angle, pitch angle are all accurately and in time obtained in navigation control process by attitude transducer GY-86, are the keys that unmanned boat carries out accurately control.Three axis accelerometer in attitude transducer module and three-axis gyroscope can both carry out alone the measurement of attitude angle, but any one of both sensors is used alone all it cannot be guaranteed that the accuracy measured, and the feature that two kinds of sensors are had nothing in common with each other.First acceierometer sensor is analyzed by we, accelerometer obtains the principle summary of attitude angle: accelerometer is able to detect that acceleration of gravity is at each axial component, when attitude of carrier changes, three axial detected values of accelerometer also can change, and just can calculate according to each axial detected value and obtain attitude data.Secondly, the vibrations interference in unmanned boat navigation process can have a strong impact on the accuracy of accelerometer detection.Similarly for gyroscope, it is able to detect that the angular velocity of corresponding three axles, then it is multiplied with the sampling period and can be obtained by the angle that a sampling period turns over, but the attitude angle short time internal ratio obtained in this way is relatively accurate, will cause that measurement error is increasing due to the effect of drift for a long time.For effectively solving vibrations interference and the error problem brought that drifts about, we adopt Kalman (Kalman) to filter, and the collection signal from accelerometer and gyroscope is merged.Kalman filtering is a kind of high performance recursion filter, and maximum feature is the state that can estimate dynamical system in the measurement time series that not exclusively, even comprises noise.Kalman filtering, with Minimum Mean Square Error for the optimum criterion estimated, in conjunction with the algorithm of a set of recurrence estimation, and then realizes the prediction of state.Kalman filtering is less to the requirement of memory data output and operand, and therefore it is suitable for processing in real time and single-chip microcomputer computing.

Further, described according to described angular velocity adopt Kalman filtering will revise described accelerometer gather the roll angle of unmanned boat, pitch angle, including:

State equation according to the angular speed deviation of the gyroscope collection in attitude transducer module, angular speed structure system is

${\left[\begin{array}{c}\mathrm{\α}\\ \mathrm{\β}\end{array}\right]}_k=\left[\begin{array}{cc}1& -\mathrm{\Δ}t\\ 0& 1\end{array}\right]{\left[\begin{array}{c}\mathrm{\α}\\ \mathrm{\β}\end{array}\right]}_{k-1}+\left[\begin{array}{c}\mathrm{\Δ}t\\ 0\end{array}\right]{\mathrm{\ω}}_{k-1}+\left[\begin{array}{c}{w}_g\mathrm{\Δ}t\\ 0\end{array}\right]---\left(1\right)$

Measurement equation is

$Z\left(k\right)=\[\begin{array}{cc}1& 0\end{array}\]\left[\begin{array}{c}\mathrm{\α}\\ \mathrm{\β}\end{array}\right]+w_a---\left(2\right)$

Wherein, α represents attitude angle, and described attitude angle includes roll angle, pitch angle, and β represents the angular speed deviation that gyroscope exports, and Δ t represents sampling period, ω_k-1Expression k-1 (k=1,2 ..., the n) angular speed of moment gyroscope detection, w_gRepresent the white noise of gyroscope output, w_aWhite noise, the measured value that described Z (k) is accelerometer is exported for accelerometer；

According to state equation and measurement equation in conjunction with formula

X (k | k-1)=AX (k-1 | k-1)+BU (k-1) (3)

Obtain the optimum attitude angle in k-1 moment, the attitude angle of current time is predicted by the optimum attitude angle in k-1 moment, wherein, the optimum attitude angle that X (k-1 | k-1) is the k-1 moment, the attitude angle according to a preliminary estimate that X (k | k-1) is the k moment, the controlled quentity controlled variable that U (k) is current time, described A is sytem matrix, and described B is for controlling input matrix；

According to formula

P (k | k-1)=AP (k-1 | k-1) A^T+Q(4)

Calculating the covariance of forecast error, wherein, the covariance that P (k | k-1) is k moment forecast error, the covariance that P (k-1 | k-1) is k-1 moment optimal estimation value, Q is system noise covariance, A^TTransposed matrix for sytem matrix；

Covariance according to described forecast error, adopts formula

K_g(k)=P (k | k-1) H^T[HP(k|k-1)H^T+R]^-1(5)

Calculating Kalman gain, wherein, described Kg is Kalman filter gain, and H is observing matrix, and R is for measuring noise covariance matrix, described H^TTransposed matrix for observing matrix；

According to described Kalman gain, adopt formula

X (k | k)=X (k | k-1)+K_g(k)[Z(k)-HX(k|k-1)](6)

Revise the optimum attitude angle in k moment, wherein, the measured value that described Z (k) is accelerometer.

Specifically, the present embodiment predicts attitude by the measurement data of gyroscope, and the measured value then passing through accelerometer is modified.State equation and measurement equation are formula (1) and formula (2), state equation and measurement equation can obtain the process of the iteration of numerical computations in conjunction with Kalman's theory.First two time update equation formula (3) and the formula (4) of Kalman filtering can be obtained.Formula (3) is by the state value of the optimal estimation value prediction current time in k-1 moment, and formula (4) calculates, according to the covariance in k-1 moment, the covariance obtaining forecast error.Also realizing state renewal, three state renewal equations such as formula (5) of Kalman filtering and formula (6) after the deadline updates, its effect mainly obtains Kalman gain, and then asks for the optimal estimation value in k moment.According to

P (k | k)=[I-K_gH]P(k|k-1)(7)

Calculating the covariance obtaining current time optimal estimation value, the interative computation for the next moment is prepared.Wherein, I is the matrix of 1, measures for single model list, I=1.When system enters k+1 state, P (k | k) is exactly the P (k-1 | k-1) of formula 7 formula, this formula act as the optimal estimation covariance that the optimal estimation covariance in k-1 moment is updated to the k moment, and carrying out Kalman filtering, to calculate involved system variable initial value X (0), predicting covariance initial value P (0 | 0), system noise covariance Q, measurement noise covariance R value as follows:

$X\left(0\right)=\left[\begin{array}{c}0\\ 0\end{array}\right],P\left(0\right|0)=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right],Q=\left[\begin{array}{cc}0.001& 0\\ 0& 0.003\end{array}\right],R=0.69---\left(8\right)$

Kalman filtering is a kind of high performance recursion filter, and maximum feature is the state that can estimate dynamical system in the measurement time series that not exclusively, even comprises noise.Kalman filtering is relative to other filtering such as the advantage of wiener Wiener filtering, be need not before whole measured values corresponding to moment, Kalman filtering is to estimate the currency of signal according to previous estimated value and a nearest measured value, it is estimated with state equation and recurrence estimation algorithm, and therefore Kalman filtering does not require stationarity and the timeinvariance of signal.Kalman filtering, with Minimum Mean Square Error for the optimum criterion estimated, in conjunction with the algorithm of a set of recurrence estimation, and then realizes the prediction of state.Its basic ideas are: adopt the state-space model of signal and noise, by the estimated value of previous moment and the measured value of current time complete the renewal that state variable is estimated, draw the estimated value of current time.With the process model of system, predict the system of NextState.Assume that present system mode is k, the model according to system, it is possible to dope status praesens based on the laststate of system.

For verifying the effect of Kalman filtering, following test has been done: be fixed on unmanned boat by attitude transducer module for the vibrations interference of accelerometer and the drift phenomenon of gyroscope, open unmanned boat electromotor and produce vibrating effect, microcontroller STM32 is used by iic bus, accelerometer and gyroscope to be sampled with 20ms for the cycle, then pass through the Kalman filtering algorithm attitude data to two kinds of sensors to process, again the attitude information before process and after process is sent to host computer display preservation by serial ports, finally adopt Matlab drawing function by the attitude data of storage as shown in Figure 2, Kalman filtering can eliminate the interference of accelerometer and the drift impact of gyroscope effectively as seen from the figure, draw relatively accurate attitude information.Through the calculating of kalman filter method, roll angle, pitch angle can be revised preferably.

Further, described described revised roll angle, pitch angle and described course angle fusions determines Precision course direction angle, including:

Formula is adopted according to revised roll angle, pitch angle

$M^h=C_b^h{M}^b---\left(9\right)$

Described course angle is transformed into by sensor coordinate system the magnetic field intensity in horizontal coordinates, wherein, M^bFor the magnetic field intensity of respective sensor coordinate system, M^hFor the magnetic field intensity of corresponding horizontal coordinates,For transition matrix；

Component according to the magnetic field intensity in horizontal coordinates calculates the magnetic heading angle of unmanned boat hull plane；

Precision course direction angle is determined according to described magnetic heading angle and magnetic declination.

Specifically, course angle draws mainly by magnetic field strength transducer, under level, magnetic field strength transducer is through based on being obtained with relatively accurate course value after the compass deviation compensation of ellipse hypothesis, when magnetic field strength transducer is not at level, it is accomplished by the data according to above-mentioned required roll angle and pitch angle and magnetic field strength transducer collection to merge, asks for course angle.As it is shown on figure 3, H_x、H_y、H_zRepresent the magnetic field intensity of three axles, wherein H under magnetic field strength transducer body coordinate system respectively_xPoint to the direction of advance of carrier.H_X、H_Y、H_ZFor H under any attitude_x、H_y、H_zProjection under horizontal coordinates.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 axles must be transformed under horizontal coordinates and try to achieve by the parsing of course angle, and meets, magnetic heading angle angle+ magnetic declination β=geography course angle, so sensing data can first be calculated obtain magnetic heading angle, add and work as geomagnetic declination, thus obtaining geographical course angle.The algorithm of magnetic heading angle angle divides following two situation:

The first situation, when magnetic field strength transducer horizontal positioned, namely the coordinate system of sensor and horizontal coordinates angle are zero, now, H_ZThe magnetic vector of axle is zero, then:

$angle=\arctan \frac{H_Y}{H_X}---\left(10\right)$

The second situation, when magnetic field strength transducer and horizontal plane exist angle, illustrates that the situation of pitching, rolling occurs in carrier, then must record pitch angle by acceleration transducer and gyro sensorWith roll angle θ.Then the Magnetic Field that sensor coordinate system is measured is transformed in horizontal coordinates, solves the magnetic heading angle of carrier.The transition matrix being tied to horizontal coordinates by sensor coordinates is:

M^bFor the magnetic field intensity of respective sensor coordinate system, M^hMagnetic field intensity for corresponding horizontal coordinates.The conversion formula of coordinate system is:

The component form of formula (10) is:

Wherein, M^b _x、M^b _Y、M^b _ZFor magnetic field intensity component on sensor coordinate system three axle, M^h _X、M^h _Y、M^h _ZFor magnetic field intensity component on horizontal coordinates three axle.The variable of above formula is transformed into variable corresponding in coordinate system, the magnetic induction component at horizontal coordinates can be obtained:

Such that it is able to the magnetic heading angle angle calculating hull plane is:

Angle=arctan (H_Y/H_X)(15)

After trying to achieve magnetic heading angle angle, checking in as geomagnetic declination β, can obtain geographical course angle α is

α=angle+ β (16)

After course angle pretreatment, overcome the inaccuracy that the course angle brought due to hull pitching, rolling, change of magnetic field strength is measured, by data fusion, draw hull course angle accurately.

Further, described employing Fuzzy PID determines ordered rudder angle according to described course deviation, including:

Adopt triangular function

$f(x,a,b,c)=\left\{\begin{array}{cc}0,& x\≤a\\ \frac{x-a}{b-a},& a\≤x\≤b\\ \frac{c-x}{c-b},& b\≤x\≤c\\ 0,& c\≤x\end{array}\right.---\left(17\right)$

The exact value of course deviation, course deviation rate being carried out Fuzzy processing, obtains fuzzy output set, wherein, x is course deviation or course deviation rate, a, b, and c specifies the shape of triangular function, and requires a≤b≤c；

Determine fuzzy tuning table according to described fuzzy output set, and determine the proportionality coefficient of PID controller, integral coefficient and differential coefficient according to described fuzzy tuning table described fuzzy output set of adjusting；

Adopt weighted mean method that described proportionality coefficient, integral coefficient and differential coefficient are waken up with a start fuzzy judgment, it is thus achieved that the exact value of described proportionality coefficient, integral coefficient and differential coefficient；

The exact value of described proportionality coefficient, integral coefficient and differential coefficient is inputted described PID controller and obtains ordered rudder angle.

Specifically, for fuzzy technology and pid algorithm are combined, common are the following two kinds: one is utilize fuzzy controller, to PID controller Online Auto-tuning PID parameter, to form Fuzzy self-turning parameter PID controller；Another kind is to adopt different variance thresholds to build multimode segmentation controller, according to different condition and require that segmentation different modalities is controlled, different algorithms is made to give full play to respective advantage in the different control stages, for instance P-FUZZY-PI multi mode control device.The present embodiment adopts the first control mode, as it is shown in figure 5, on the basis of PID direction controller, with the deviation variation rate e in course deviation e and course_cAs input, adopt fuzzy control to pid parameter K_p, K_iAnd K_dCarry out online self-tuning, to meet the different requirements to controller parameter of the different control stages, so that controlled device has good dynamic and static properties.

Fuzzy PID Control System is the closed-loop control system being made up of fuzzy controller and conventional PID controller two parts.R represents given set amount, and what y represented is the output of system, and e represents systematic error, i.e. the difference of setting value and real output value, e_cRepresent the rate of change of systematic error, E and E_cRepresent e and e respectively_cFuzzy quantity obtained after Fuzzy processing, K_p、K_iAnd K_dThe matching convention PID representing fuzzy controller output controls the adjusted value of three parameters, and u refers to the output that conventional PID controller acts in controlled device.

As shown in Figure 6, the contrast first passing through default value and system output value draws systematic error e, error rate e to the control flow of fuzzy paste PID control system_cSubsequently into fuzzy controller, input variable being carried out Fuzzy processing, secondly, the input quantity after Fuzzy processing can obtain a fuzzy output set according to fuzzy tuning table, again this fuzzy output set is carried out defuzzification, obtain an accurate output valve.Its essence is in certain output area, find a most suitable output controlling value.The final output variable respectively Proportional coefficient K of fuzzy controller_p, integral coefficient K_iWith differential coefficient K_dAdjustment amount.Finally, utilizing the output of application fuzzy control that PID controller parameter is adjusted, its calculating formula adjusted is:

$\left\{\begin{array}{c}{K}_p=K_{p0}+{\mathrm{\ΔK}}_p\\ K_i=K_{i0}+{\mathrm{\ΔK}}_i\\ K_d=K_{d0}+{\mathrm{\ΔK}}_d\end{array}\right.---\left(18\right)$

Wherein, K_p0, K_i0And K_d0For the reference value of parameter adjustment, Δ K_p, Δ K_iWith Δ K_dIt it is the amount of adjusting having fuzzy algorithmic approach to obtain.Usual fuzzy controller is made up of following three parts: the obfuscation of input variable and the selection of degree of membership, fuzzy control are adjusted the ambiguity solution of table and output variable.

The present embodiment fuzzy controller has two input: course error e and course error rate of change e_c, owing in the controls, input quantity is all clear value after control structure is determined, in order to make these, clearly values can be suitable with the fuzzy tuning table of language expression, carrying out approximate resoning, it is necessary to they are transformed into fuzzy quantity, the present embodiment is that variable is quantified as 7 grades, namely { negative big, in negative, negative little, zero, just little, center, 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 correct structure membership function be to make good use of, the key of fuzzy set, but up to the present, but without the method that a maturation is effective and unified.In actual application, it is determined that membership function still relies on experience and solves, the feedback information then obtained again through experiment or computer simulation is modified.Existing frequently-used membership function includes Z function, S function, trapezoidal function, bell function, triangular function and Gauss type function etc., considering sensitivity and the arithmetic speed of control, in the present embodiment, the membership function of each variable all adopts triangular function formula (15), and the effect of this triangular function is that exact value is converted into fuzzy value.The fuzzy domain of all variablees and membership function be as shown in Figure 4:

Further, described fuzzy tuning table is:

Wherein, e is course deviation, e_cFor course deviation rate, NB, NM, NS, Z, PS, PM, PB, PM are fuzzy subset, k_pFor proportionality coefficient；

Wherein, k_iFor integral coefficient；

Wherein, k_dFor differential coefficient.

Specifically, obtaining result by above-mentioned fuzzy tuning table is a fuzzy set, but needs fuzzy control finally to export a value determined in actual applications.The process taking a monodrome that relatively can represent this fuzzy set in fuzzy set is referred to as fuzzy decision.Anti-fuzzy has multiple method, and the obtained result of diverse ways is also different.Adopt centroid method the most reasonable in theory, but the method calculates more complicated, so generally not adopting in this way in the system that requirement of real-time is higher.Simplest Anti-fuzzy method is maximum membership degree method, this method takes that maximum value of degree of membership in all fuzzy sets or membership function as output, but value less for degree of membership is not taken into account by this method, representative bad, so being only used for relatively simple system.Marginal also have various averaging methods, the weighted mean method that the present embodiment adopts.Its computing formula is:

$u^{*}=\frac{\underset{i}{\Σ}\mathrm{\μ}\left(u_i\right)\·u_i}{\underset{i}{\Σ}\mathrm{\μ}\left(u_i\right)}---\left(19\right)$

Wherein, μ (u_i) for ambiguity solution output exact value, u_iIt is the subset of the fuzzy domain of output, μ (u_i) for exporting the degree of membership corresponding to subset.

In the present embodiment the basic domain of course error e is set as [-45,45], course error rate of change e_cBasic domain be set as [-5 °/s, 5 °/s], by e, e_c、ΔK_p、ΔK_i、ΔK_dDomain in fuzzy set is set as that {-6 ,-4 ,-2,0,2,4,6}, so quantizing factor is respectively as follows: K_e=6/45=0.133, K_ec=6/5=1.2.All variablees are quantified as 7 grades, namely negative big, and in negative, negative little, zero, just little, center, honest.

The most important part of fuzzy control is exactly Proportional coefficient K_p, integral coefficient K_iWith differential coefficient K_dFuzzy tuning table.K_p、K_iAnd K_dFuzzy tuning table respectively table 1 to table 3.

After fuzzy tuning table establishes, according to K_p、K_iAnd K_dFuzzy tuning table content determine fuzzy relation, calculate the fuzzy set of reflection controlled quentity controlled variable change, then adopt weighted mean method to carry out fuzzy judgment, it is thus achieved that K_p、K_iAnd K_dThe exact value of adjustment amount.

The present invention improves the steady-state behaviour of unmanned boat course motor control, dynamic performance and control accuracy, decreases the regulating time of Heading control.

Last it is noted that various embodiments above is only in order to illustrate technical scheme, it is not intended to limit；Although the present invention being described in detail with reference to foregoing embodiments, it will be understood by those within the art that: the technical scheme described in foregoing embodiments still can be modified by it, or wherein some or all of technical characteristic is carried out equivalent replacement；And these amendments or replacement, do not make the essence of appropriate technical solution depart from the scope of various embodiments of the present invention technical scheme.

Claims (6)

1. a unmanned boat course motion control method, it is characterised in that including:

Receiving the real-time course angle of unmanned boat that sensor assembly gathers, described sensor assembly includes: gyroscope, accelerometer and magnetic field strength transducer；

Contrast described real-time course angle and obtain unmanned boat course deviation, course deviation rate with described set course angle；

Fuzzy PID is adopted to determine that ordered rudder angle, described ordered rudder angle include beating rudder direction and beating rudder speed according to described course deviation and described course deviation rate；

Described ordered rudder angle is sent to motor driver；

Described motor driver according to described beat rudder direction and described beat rudder speed controlling unmanned boat motion.

2. method according to claim 1, it is characterised in that the described real-time course angle of described contrast and described set course angle also include before obtaining unmanned boat course deviation, course deviation rate:

Receiving the angular velocity of gyroscope detection three axles of accelerometer, magnetic field strength transducer gathers the course angle of unmanned boat, and accelerometer gathers the roll angle of unmanned boat, pitch angle；

Adopt Kalman filtering will revise the roll angle of unmanned boat, pitch angle that described accelerometer gathers according to described angular velocity；

Described revised roll angle, pitch angle and described course angle are merged and determines Precision course direction angle.

3. method according to claim 2, it is characterised in that described according to described angular velocity adopt Kalman filtering will revise described accelerometer gather the roll angle of unmanned boat, pitch angle, including:

State equation according to the angular speed deviation of the gyroscope collection in attitude transducer module, angular speed structure system is

${\left[\begin{array}{c}\mathrm{\α}\\ \mathrm{\β}\end{array}\right]}_k=\left[\begin{array}{cc}1& -\mathrm{\Δ}t\\ 0& 1\end{array}\right]{\left[\begin{array}{c}\mathrm{\α}\\ \mathrm{\β}\end{array}\right]}_{k-1}+\left[\begin{array}{c}\mathrm{\Δ}t\\ 0\end{array}\right]{\mathrm{\ω}}_{k-1}+\left[\begin{array}{c}{w}_g\mathrm{\Δ}t\\ 0\end{array}\right]---\left(1\right)$

Measurement equation is

$Z\left(k\right)=\[\begin{array}{cc}1& 0\end{array}\]\left[\begin{array}{c}\mathrm{\α}\\ \mathrm{\β}\end{array}\right]+w_a---\left(2\right)$

Wherein, α represents attitude angle, and described attitude angle includes roll angle, pitch angle, and β represents the angular speed deviation that gyroscope exports, and Δ t represents sampling period, ω_k-1Expression k-1 (k=1,2 ..., the n) angular speed of moment gyroscope detection, w_gRepresent the white noise of gyroscope output, w_aWhite noise, the measured value that described Z (k) is accelerometer is exported for accelerometer；

According to state equation and measurement equation in conjunction with formula

X (k | k-1)=AX (k-1 | k-1)+BU (k-1) (3)

Obtain the optimum attitude angle in k-1 moment, the attitude angle of current time is predicted by the optimum attitude angle in k-1 moment, wherein, the optimum attitude angle that X (k-1 | k-1) is the k-1 moment, the attitude angle according to a preliminary estimate that X (k | k-1) is the k moment, the controlled quentity controlled variable that U (k) is current time, described A is sytem matrix, and described B is for controlling input matrix；

According to formula

P (k | k-1)=AP (k-1 | k-1) A^T+Q(4)

Calculating the covariance of forecast error, wherein, the covariance that P (k | k-1) is k moment forecast error, the covariance that P (k-1 | k-1) is k-1 moment optimal estimation value, Q is system noise covariance, A^TTransposed matrix for sytem matrix；

Covariance according to described forecast error, adopts formula

K_g(k)=P (k | k-1) H^T[HP(k|k-1)H^T+R]^-1(5)

Calculating Kalman gain, wherein, described Kg is Kalman filter gain, and H is observing matrix, and R is for measuring noise covariance matrix, described H^TTransposed matrix for observing matrix；

According to described Kalman gain, adopt formula

X (k | k)=X (k | k-1)+K_g(k)[Z(k)-HX(k|k-1)](6)

Revise the optimum attitude angle in k moment, wherein, the measured value that described Z (k) is accelerometer.

4. method according to claim 3, it is characterised in that described described revised roll angle, pitch angle and described course angle fusions determines Precision course direction angle, including:

Formula is adopted according to revised roll angle, pitch angle

$M^h=C_b^h{M}^b---\left(7\right)$

Described course angle is transformed into by sensor coordinate system the magnetic field intensity in horizontal coordinates, wherein, M^bFor the magnetic field intensity of respective sensor coordinate system, M^hFor the magnetic field intensity of corresponding horizontal coordinates,For transition matrix；

Component according to the magnetic field intensity in horizontal coordinates calculates the magnetic heading angle of unmanned boat hull plane；

Precision course direction angle is determined according to described magnetic heading angle and magnetic declination.

5. method according to claim 1, it is characterised in that described employing Fuzzy PID determines ordered rudder angle according to described course deviation, including:

Adopt triangular function

$f(x,a,b,c)=\left\{\begin{array}{c}0,x\≤a\\ \frac{x-a}{b-a},a\≤x\≤b\\ \frac{c-x}{c-b},b\≤x\≤c\\ 0,c\≤x\end{array}\right.---\left(8\right)$

The exact value of course deviation, course deviation rate being carried out Fuzzy processing, obtains fuzzy output set, wherein, x is course deviation or course deviation rate, a, b, and c specifies the shape of triangular function, and requires a≤b≤c；

Determine fuzzy tuning table according to described fuzzy output set, and determine the proportionality coefficient of PID controller, integral coefficient and differential coefficient according to described fuzzy tuning table described fuzzy output set of adjusting；

Adopt weighted mean method that described proportionality coefficient, integral coefficient and differential coefficient are waken up with a start fuzzy judgment, it is thus achieved that the exact value of described proportionality coefficient, integral coefficient and differential coefficient；

The exact value of described proportionality coefficient, integral coefficient and differential coefficient is inputted described PID controller and obtains ordered rudder angle.

6. method according to claim 5, it is characterised in that described fuzzy tuning table is:

Wherein, e is course deviation, e_cFor course deviation rate, NB, NM, NS, Z, PS, PM, PB, PM are fuzzy subset, k_pFor proportionality coefficient；

Wherein, k_iFor integral coefficient；

Wherein, k_dFor differential coefficient.