CN101825903B - Water surface control method for remotely controlling underwater robot - Google Patents

Abstract

The invention aims to provide a water surface control method for remotely controlling an underwater robot, which comprises the following steps: obtaining information of underwater objects to be tracked, underwater scene images and underwater obstacles through a CCD and a forward-looking sonar arranged on the underwater robot; obtaining actual heading and depth of the underwater robot by adopting an improved Sage-Husa adaptive kalman filter algorithm according to the obtained information; and automatically transmitting motion control commands to the underwater robot by a recursive Ethernet neural network DPRFNN algorithm according to the actual heading and depth of the underwater robot. The invention has the advantages of simpleness, flexibility, strong functions, strong self-adaption and the like.

Description

A kind of water surface control method for remotely controlling underwater robot

Technical field

What the present invention relates to is a kind of control method, is specifically applied to a kind of control method of the field robots such as data acquisition under water, deep ocean work and hull detection.

Background technology

The ocean of taking up an area ball surface area 75%, be one richly endowed and obtain far away the treasure-house of exploitation.The mankind will survive and multiply and development, make full use of the last territory leaved for development of only this piece of the earth, will be without avoidable selection.Underwater robot is because its action is flexible, power is sufficient, the transmission of information and data and exchange efficient and convenient, data volume is large, can in water, work long hours, now be widely used in the aspects such as Underwater Engineering, offshore petroleum resources exploitation, marine mineral resources investigation, living marine resources investigation, deep-sea salvaging, hull detection, so the development and application of underwater robot has important strategic importance.

Application number is 200810064256.3 Chinese patent document (open day: the control method that disclosed on April 8th, 2008) " the underwater hiding-machine space variable structure control method based on Recurrent Fuzzy Neural Network " provides underwater hiding-machine autopilot to control.Although the underwater hiding-machine space variable structure control method based on Recurrent Fuzzy Neural Network belongs to same technical field with this patent, but it is that yaw rudder control system, casing rudder control system, the tail based on Recurrent Fuzzy Neural Network (RFNN) rises rudder control system by design, and then forming underwater submersible space motion combined control system, the method realizing from the present invention is different.

And the present invention controls the motion of underwater robot by obtaining sensor and environment sensing information, and then realize it and keep away barrier, follow the tracks of the functions such as detection.

Summary of the invention

The object of the present invention is to provide for controlling a kind of water surface control method for remotely controlling underwater robot of the field robots such as data acquisition under water, deep ocean work and hull detection.

The object of the present invention is achieved like this:

The CCD that the present invention is equipped with by underwater robot and Forward-looking Sonar are obtained the information of tracked object, scene image and underwater obstacle under water, according to the information of obtaining adopt bow that improved Sage-Husa adaptive Kalman filter algorithm obtains underwater robot reality to and the degree of depth, according to the bow of the underwater robot reality obtaining to automatically underwater robot being sent to motion control instruction with the degree of depth, ether neural network DPRFNN algorithm by recurrence.

A kind of water surface control method for remotely controlling underwater robot of the present invention also comprises:

1, described improved Sage-Husa adaptive Kalman filter algorithm is the estimated value that increases system noise statistics on basic KALMAN filtering basis estimated value with measurement noise statistics adjustment:

System interference average

$\hat{q}\left(k\right)=(1-d_{k-1})\hat{q}(k-1)+d_{k-1}[\hat{X}(k/k)-\mathrm{\Φ}(k,k-1)\hat{X}(k-1/k-1)]$

System interference variance matrix

$\hat{Q}\left(k\right)=(1-d_{k-1})\hat{Q}(k-1)+d_{k-1}[K\left(k\right)\mathrm{\ϵ}\left(k\right){\mathrm{\ϵ}}^T\left(k\right)+P(k/k)$

$-\mathrm{\Φ}(k,k-1)P(k-1/k-1){\mathrm{\Φ}}^T(k,k-1)]$

Measurement noise average

$\hat{r}\left(k\right)=(1-d_{k-1})\hat{r}(k-1)+d_{k-1}[Z\left(k\right)-H\left(k\right)\hat{X}(k-1/k-1)]$

Measuring noise square difference matrix

$\hat{R}\left(k\right)=(1-d_{k-1})\hat{R}(k-1)+d_{k-1}[\mathrm{\ϵ}\left(k\right){\mathrm{\ϵ}}^T\left(k\right)-H\left(k\right)P(k/k-1)H^T\left(k\right)]$

Wherein the estimation of state X (k), Φ (k, k-1) be t (k-1) constantly to t (k) step transfer matrix constantly, H (k) is for measuring battle array, for the estimation of the variance battle array Q (k) of system noise sequence, for the estimation of measurement noise serial variance battle array R (k), for new breath matrix, new breath includes the error of one-step prediction, and it is done to suitable weighting processing will separate correction b is forgetting factor it plays vital effect to dispersing with precision of filtering, and by adjusting P _k+1|kcontrol filter gain battle array K _k+1prevent dispersing of wave filter, when be false, press revise P _k+1|k, wherein γ>=1 is to determine in advance adjustability coefficients, S _k+1be adaptation coefficient, system interference average, system interference variance matrix, measurement noise average, measuring noise square difference matrix and Kalman filtering combination have just been formed to improved Sage-Husa adaptive Kalman filter algorithm.

2, described ether neural network DPRFNN algorithm comprises input layer i, degree of membership layer j, petri ether layer, rules layer k layer and output layer o layer five-layer structure, recurrence feedback realizes by embedding feedback link at degree of membership layer, and the propagation of signal and the basic function of every one deck are expressed as follows:

(1) ground floor is input layer, and each node of input layer is directly input variable x _i(i=1,2,3,4) are directly delivered to lower one deck, input node be the degree of depth and angle with diving speed and angular velocity error, net _i ¹ground floor output:

(2) second layer is degree of membership layer, and each node in layer passes through membership function, and degree of membership is input as

$r_i^j\left(n\right)=x_i\left(n\right)+{\mathrm{\μ}}_i^j(n-1){\mathrm{\α}}_i^j$

N is frequency of training, α _i ^jthe weights that represent self feed back circulation, μ _i ^j(n-1) be the output signal of the last training second layer, it defines by Gaussian subordinate function:

${\mathrm{net}}_j\left(r_i^j\right)=-\frac{{(r_i^j-m_i^j)}^2}{{\left({\mathrm{\σ}}_i^j\right)}^2},$ ${\mathrm{\μ}}_i^j\left[{\mathrm{net}}_j\left(r_i^j\right)\right]=\exp \left({\mathrm{net}}_j\left(r_i^j\right)\right)$

M _i ^jand σ _i ^jbe respectively average and the standard deviation of Gaussian subordinate function of j fuzzy set of i input variable, they are all adjustable parameters, j=1, and 2 ..., n _j, n _jthe quantity of the semantic variant of each input, μ _i ^j[net _j(r _i ^j)] be second layer output;

(3) the 3rd layers is Petri layer, and it provides token, uses the following rules of competition

$t_i^j=\left\{\begin{array}{cc}1,& {\mathrm{\&mu;}}_i^j\left[{\mathrm{net}}_j\left(r_i^j\right)\right]\&GreaterEqual;d_{\mathrm{th}}\\ 0,& {\mathrm{\&mu;}}_i^j\left[{\mathrm{net}}_j\left(r_i^j\right)\right]<d_{\mathrm{th}}\end{array}\right.$

T wherein _i ^jconversion value, d _thbe dynamic change threshold value, along with system responses error and change;

(4) the 4th layers is rules layer, and each node k is represented by ∏, connects and takes advantage of input input Output rusults:

${\mathrm{\&phi;}}_k=\left\{\begin{array}{cc}\underset{i=1}{\overset{4}{\mathrm{\&Pi;}}}{\mathrm{\&omega;}}_{\mathrm{ji}}^k{\mathrm{\&mu;}}_i^j\left[{\mathrm{net}}_j\left(r_i^j\right)\right],& t_i^j=1\\ 0,& t_i^j=0\end{array}\right.$

ω in formula _ji ^kthe weights of Petri net and rules layer, for constant value, φ _k(k=1,2 ... n _y) output of k layer, n _yrule sum, φ _kthe 4th layer of output;

(5) layer 5 is output layer:

Wherein, connect weights ω _k ^othe output intensity of 0th output relevant with k rule, y _obe layer 5 output, output valve is the control magnitude of voltage of thruster;

The on-line learning algorithm that DPRFNN is used is supervision-ascent algorithm, and this algorithm is defined as for its energy function E first:

In formula h and θ be the real-time deep value of remote underwater robot in controlling and bow to value, h _dand θ _drespectively the expectation value of h and θ, e _h=h _d-h, e _θ=θ _d-θ be respectively the degree of depth and bow to error, dynamic change threshold value d _thby following formula, adjusted:

In formula, α and β are normal numbers;

At output layer, error back propagation value is:

Connect weights ω _k ^oupgrade according to the following formula:

η _ωconnect weights learning rate, next is ω constantly _k ^ofor:

The weights of rules layer are constant, and the error of this layer is:

In Petri layer, error is calculated as follows

${\mathrm{\&rho;}}_j\left(r_i^j\right)=\left\{\begin{array}{cc}\underset{k}{\mathrm{\&Sigma;}}{\mathrm{\&zeta;}}_k{\mathrm{\&phi;}}_k,& t_i^j=1\\ 0,& t_i^j=0\end{array}\right.$

The update rule of each parameter of obfuscation layer is:

$\mathrm{\&Delta;}{m}_i^j={\mathrm{\&eta;}}_m{\mathrm{\&rho;}}_j\frac{2{(r_i^j-m_i^j)}^2}{{\left({\mathrm{\&sigma;}}_i^j\right)}^2}{\mathrm{\&mu;}}_i^j(n-1),$ ${\mathrm{\&Delta;\&sigma;}}_i^j={\mathrm{\&eta;}}_s{\mathrm{\&rho;}}_j\frac{2{(r_i^j-m_i^j)}^2}{{\left({\mathrm{\&sigma;}}_i^j\right)}^3}$

$\mathrm{\&Delta;}{\mathrm{\&alpha;}}_i^j={-\mathrm{\&eta;}}_{\mathrm{\&alpha;}}{\mathrm{\&rho;}}_j\frac{2(r_i^j-m_i^j)}{{\left({\mathrm{\&sigma;}}_i^j\right)}^2}{\mathrm{\&mu;}}_i^j(n-1)$

Next each parameter of the layer of obfuscation is constantly:

$m_i^j(n+1)=m_i^j\left(n\right)+\mathrm{\&Delta;}{m}_i^j\left(n\right),$ ${\mathrm{\&sigma;}}_i^j(n+1)={\mathrm{\&sigma;}}_i^j\left(n\right)+\mathrm{\&Delta;}{\mathrm{\&sigma;}}_i^j\left(n\right),$ ${\mathrm{\&alpha;}}_i^j(n+1)={\mathrm{\&alpha;}}_i^j\left(n\right)+\mathrm{\&Delta;}{\mathrm{\&alpha;}}_i^j\left(n\right)$

${\mathrm{\&eta;}}_m=\frac{E\left(n\right)}{4[\underset{i=1}{\overset{n_i}{\mathrm{\&Sigma;}}}\underset{j=1}{\overset{n_j}{\mathrm{\&Sigma;}}}\underset{O=1}{\overset{n_O}{\mathrm{\&Sigma;}}}{\left(\frac{\&PartialD;E\left(n\right)}{{\&PartialD;x}_i}\frac{\&PartialD;x_i}{\&PartialD;y_O}\frac{\&PartialD;y_O}{\&PartialD;m_i^j}\right)}^2+\mathrm{\&epsiv;}]}$ ${\mathrm{\&eta;}}_w=\frac{E\left(n\right)}{4[\underset{O=1}{\overset{n_O}{\mathrm{\&Sigma;}}}\underset{k=1}{\overset{n_y}{\mathrm{\&Sigma;}}}{\left(\frac{\&PartialD;E\left(n\right)}{{\&PartialD;y}_O}\frac{\&PartialD;y_O}{\&PartialD;{\mathrm{\&omega;}}_k^o}\right)}^2+\mathrm{\&epsiv;}]}$

${\mathrm{\&eta;}}_{\mathrm{\&sigma;}}=\frac{E\left(n\right)}{4[\underset{i=1}{\overset{n_i}{\mathrm{\&Sigma;}}}\underset{j=1}{\overset{n_j}{\mathrm{\&Sigma;}}}\underset{O=1}{\overset{n_O}{\mathrm{\&Sigma;}}}{\left(\frac{\&PartialD;E\left(n\right)}{{\&PartialD;x}_i}\frac{\&PartialD;x_i}{\&PartialD;y_O}\frac{\&PartialD;y_O}{\&PartialD;{\mathrm{\&sigma;}}_i^j}\right)}^2+\mathrm{\&epsiv;}]}$ ${\mathrm{\&eta;}}_{\mathrm{\&alpha;}}=\frac{E\left(n\right)}{4[\underset{i=1}{\overset{n_i}{\mathrm{\&Sigma;}}}\underset{j=1}{\overset{n_j}{\mathrm{\&Sigma;}}}\underset{O=1}{\overset{n_O}{\mathrm{\&Sigma;}}}{\left(\frac{\&PartialD;E\left(n\right)}{{\&PartialD;x}_i}\frac{\&PartialD;x_i}{\&PartialD;y_O}\frac{\&PartialD;y_O}{\&PartialD;{\mathrm{\&alpha;}}_i^j}\right)}^2+\mathrm{\&epsiv;}]}$

η in formula _m, η _w, η _σ, η _αbe the Study rate parameter of Gaussian function, ε is normal number.

Advantage of the present invention is: simple, flexibly, powerful, strong adaptability etc.

Accompanying drawing explanation

Fig. 1 is that control method of the present invention is always schemed;

Fig. 2 is that wireless operating bar hand of the present invention is controlled method system block diagram processed;

Fig. 3 is improved Sage-Husa self-adaptation KALMAN filtering process flow diagram of the present invention;

Fig. 4 is that the remote underwater robot FNN degree of depth of the present invention and bow are to hierarchy of control block diagram;

Fig. 5 is five layers of DPRFNN structural drawing of automatic control remote underwater robot of the present invention;

Fig. 6 is remote underwater robot pursuit movement control flow chart of the present invention;

Fig. 7 is remote underwater robot collision prevention motion control process flow diagram of the present invention;

Fig. 8 be PC/104 computing machine and water surface main control computer contact SOCKET communication process figure.

Embodiment

Below in conjunction with accompanying drawing, for example the present invention is described in more detail:

In conjunction with Fig. 1～8, the main body of present embodiment is water surface remote control method processed.Wherein water surface remote control method hardware processed is connected with ROV by optical fiber, realizes the control of underwater robot.Underwater robot autocontrol method water surface controller adopts WindowsXP operating system, utilizes visual c++ figure to carry out Visual Programming, sets up primary control program.

Water surface control method in conjunction with Fig. 1 remote underwater robot is realized by water surface remote control platform and underwater human body two parts, the hardware device of water surface remote control platform is by a water surface main control computer, optical transmitter and receiver, liquid crystal display He Yige robot controlled in wireless control crank forms.The software of water surface remote control platform is controlled system by the operating rod hand of underwater robot, sound visual identity and obstacle avoidance algorithm, and view planning track algorithm, the SOCKET communication module of Sensor Filter Algorithm and under water PC/104 forms.The environment sensing equipment that water surface remote control platform is equipped with by underwater robot and motion perception equipment obtain operation (comprising video, acoustic image), attitude and depth information (processing after filtering), effector can grasp by operating rod hand, FNN orientation and Depth control, follow the tracks of and control of collision avoidance, distribution of machine people's motion control commands, this order passes to PC/104 under water by SOCKET communication module and optical transmitter and receiver, thus the ducted propeller execution of control.

In conjunction with Fig. 2 wireless operating bar hand behaviour control method, provide in real time the current degree of depth of underwater robot for operator; Bow is to angle, each thruster information of voltage; Movement locus and current location, the information such as the video collecting and Forward-looking Sonar (SONAR) signal, operator is according to these information, (comprise that two are promoted mainly to six ducted propeller thrusters of underwater robot, two thrusters, two vertical pushing away) send and control voltage, control advances along object pose and direction.Wherein, bow is obtained through unruly-value rejecting and improved Sage-Husa self-adaptation KALMAN filtering by compass and depthometer to angle and the degree of depth; Hand behaviour carries out fault detection and diagnosis to equipment simultaneously, with assurance equipment, normally moves.

In conjunction with Fig. 3 adopt bow that improved Sage-Husa adaptive Kalman filter obtains underwater robot reality to and the degree of depth.

Its implementation is exactly the adjustment that has increased system noise statistics q (k), Q (k) and measurement noise statistics r (k), R (k) on basic KALMAN filtering basis:

System interference average

$\hat{q}\left(k\right)=(1-d_{k-1})\hat{q}(k-1)+d_{k-1}[\hat{X}(k/k)-\mathrm{\&Phi;}(k,k-1)\hat{X}(k-1/k-1)]$

System interference variance matrix

$\hat{Q}\left(k\right)=(1-d_{k-1})\hat{Q}(k-1)+d_{k-1}[K\left(k\right)\mathrm{\&epsiv;}\left(k\right){\mathrm{\&epsiv;}}^T\left(k\right)+P(k/k)$

$-\mathrm{\&Phi;}(k,k-1)P(k-1/k-1){\mathrm{\&Phi;}}^T(k,k-1)]$

Measurement noise average

$\hat{r}\left(k\right)=(1-d_{k-1})\hat{r}(k-1)+d_{k-1}[Z\left(k\right)-H\left(k\right)\hat{X}(k-1/k-1)]$

Measuring noise square difference matrix

$\hat{R}\left(k\right)=(1-d_{k-1})\hat{R}(k-1)+d_{k-1}[\mathrm{\&epsiv;}\left(k\right){\mathrm{\&epsiv;}}^T\left(k\right)-H\left(k\right)P(k/k-1)H^T\left(k\right)]$

Wherein the estimation of state X (k), Φ (k, k-1) be t (k-1) constantly to t (k) step transfer matrix constantly, H (k) is for measuring battle array, for the estimation of the variance battle array Q (k) of system noise sequence, for the estimation of measurement noise serial variance battle array R (k), for new breath matrix, new breath includes the error of one-step prediction, and it is done to suitable weighting just processes can be by separate correction b is forgetting factor it plays vital effect to dispersing with precision of filtering.

And by adjusting P _k+1|kcontrol filter gain battle array K _k+1prevent dispersing of wave filter, when be false, press revise P _k+1|k, wherein γ>=1 is to determine in advance adjustability coefficients, S _k+1it is adaptation coefficient.

Formula above and Kalman filtering have just been formed to improved Sage-Husa adaptive Kalman filter algorithm in conjunction with replacing to calculate.

In conjunction with Fig. 4,5 underwater robot autocontrol methods: adopt the method for improved Sage-Husa self-adaptation KALMAN filtering to carry out optimal estimation to data in Fig. 4.Robot controller adopts fuzzy neural network controller, and controller carries out robot body to carry out when automatically controlling the control of the degree of depth or object pose according to the attitude information of receiving.In conjunction with hydrodynamic parameter, robot body and emulation are carried out simultaneously, with the real-time and accuracy that guarantees to control.Equipment is carried out to fault detection and diagnosis simultaneously, with assurance equipment, normally move.

Five layers of DPRFNN structure that automatic control remote underwater robot is used have been described in Fig. 5.Comprise input layer i, degree of membership layer j, petri ether layer, rules layer k layer, and output layer o layer, as shown in the figure.Recurrence feedback realizes by embedding feedback link at degree of membership layer.The propagation of signal and the basic function of every one deck are expressed as follows:

Ground floor is input layer.Each node of input layer is directly input variable x _i(i=1,2,3,4) are directly delivered to lower one deck, in this patent, input node be the degree of depth and angle with diving speed and angular velocity error, net _i ¹ground floor output.

The second layer is obfuscation (degree of membership) layer.Each node in layer passes through membership function, and degree of membership is input as

Here n is frequency of training, α _i ^jthe weights that represent self feed back circulation, μ _i ^j(n-1) be the output signal of the last training second layer, it defines by Gaussian subordinate function:

${\mathrm{net}}_j\left(r_i^j\right)=-\frac{{(r_i^j-m_i^j)}^2}{{\left({\mathrm{\&sigma;}}_i^j\right)}^2}---\left(7\right)$

${\mathrm{\&mu;}}_i^j\left[{\mathrm{net}}_j\left(r_i^j\right)\right]=\exp \left({\mathrm{net}}_j\left(r_i^j\right)\right)---\left(8\right)$

M _i ^jand σ _i ^jbe respectively average and the standard deviation of Gaussian subordinate function of j fuzzy set of i input variable, they are all adjustable parameters.J=1,2 ..., n _j, n _jthe quantity of the semantic variant of each input, μ _i ^j[net _j(r _i ^j)] be second layer output.

The 3rd layer is Petri layer.The object of this layer is for token is provided, to use the rules of competition below:

$t_i^j=\left\{\begin{array}{cc}1,& {\mathrm{\&mu;}}_i^j\left[{\mathrm{net}}_j\left(r_i^j\right)\right]\&GreaterEqual;d_{\mathrm{th}}\\ 0,& {\mathrm{\&mu;}}_i^j\left[{\mathrm{net}}_j\left(r_i^j\right)\right]<d_{\mathrm{th}}\end{array}\right.---\left(9\right)$

T wherein _i ^jconversion value, d _ththe threshold value of dynamic change, it by along with system responses error and change

The 4th layer is rules layer.Each node k is represented by ∏, connects and takes advantage of input input Output rusults.

${\mathrm{\&phi;}}_k=\left\{\begin{array}{cc}\underset{i=1}{\overset{4}{\mathrm{\&Pi;}}}{\mathrm{\&omega;}}_{\mathrm{ji}}^k{\mathrm{\&mu;}}_i^j\left[{\mathrm{net}}_j\left(r_i^j\right)\right],& t_i^j=1\\ 0,& t_i^j=0\end{array}\right.---\left(10\right)$

ω in formula _ji ^kbeing the weights of Petri net and rules layer, is constant value, φ _k(k=1,2 ... n _y) output of k layer, n _yrule sum, φ _kthe 4th layer of output.

Layer 5 is output layer.

Wherein, connect weights ω _k ^oo the output intensity exported relevant with k rule, y _olayer 5 output.In this patent, output valve is the control magnitude of voltage of thruster.

The on-line learning algorithm that DPRFNN is used is supervision-ascent algorithm, and first its energy function E is defined as $E=\frac{1}{2}(e_d^2+{\stackrel{\&CenterDot;}{e}}_d^2+e_{\mathrm{\&theta;}}^2+{\stackrel{\&CenterDot;}{e}}_{\mathrm{\&theta;}}^2)---\left(12\right)$

In formula h and θ be the real-time deep value of remote underwater robot in controlling and bow to value, h _dand θ _dit is respectively the expectation value of h and θ.E _h=h _d-h, e _θ=θ _d-θ be respectively the degree of depth and bow to error.The dynamic change threshold value of formula (9) is adjusted by following formula.

In formula, α and β are normal numbers.This means that the larger threshold value of error is less, the larger threshold value of error reduces, so that many control laws come into operation as far as possible.

At output layer, error back propagation value is

Connect weights ω _k ^oupgrade according to the following formula

η _ωconnect weights learning rate, next is ω constantly _k ^ofor:

Because the weights of rules layer are constant, the error of this layer is:

In Petri layer, error is calculated as follows

The update rule of each parameter of obfuscation layer is:

$\mathrm{\&Delta;}{m}_i^j={\mathrm{\&eta;}}_m{\mathrm{\&rho;}}_j\frac{2{(r_i^j-m_i^j)}^2}{{\left({\mathrm{\&sigma;}}_i^j\right)}^2}{\mathrm{\&mu;}}_i^j(n-1),$ ${\mathrm{\&Delta;\&sigma;}}_i^j={\mathrm{\&eta;}}_s{\mathrm{\&rho;}}_j\frac{2{(r_i^j-m_i^j)}^2}{{\left({\mathrm{\&sigma;}}_i^j\right)}^3}$

$\mathrm{\&Delta;}{\mathrm{\&alpha;}}_i^j={-\mathrm{\&eta;}}_{\mathrm{\&alpha;}}{\mathrm{\&rho;}}_j\frac{2(r_i^j-m_i^j)}{{\left({\mathrm{\&sigma;}}_i^j\right)}^2}{\mathrm{\&mu;}}_i^j(n-1)---\left(19\right)$

Next each parameter of the layer of obfuscation is constantly:

$m_i^j(n+1)=m_i^j\left(n\right)+\mathrm{\&Delta;}{m}_i^j\left(n\right),$ ${\mathrm{\&sigma;}}_i^j(n+1)={\mathrm{\&sigma;}}_i^j\left(n\right)+\mathrm{\&Delta;}{\mathrm{\&sigma;}}_i^j\left(n\right),$ ${\mathrm{\&alpha;}}_i^j(n+1)={\mathrm{\&alpha;}}_i^j\left(n\right)+\mathrm{\&Delta;}{\mathrm{\&alpha;}}_i^j\left(n\right)$

η wherein _m, η _w, η _σ, η _αit is the Study rate parameter of Gaussian function.

${\mathrm{\&eta;}}_m=\frac{E\left(n\right)}{4[\underset{i=1}{\overset{n_i}{\mathrm{\&Sigma;}}}\underset{j=1}{\overset{n_j}{\mathrm{\&Sigma;}}}\underset{O=1}{\overset{n_O}{\mathrm{\&Sigma;}}}{\left(\frac{\&PartialD;E\left(n\right)}{{\&PartialD;x}_i}\frac{\&PartialD;x_i}{\&PartialD;y_O}\frac{\&PartialD;y_O}{\&PartialD;m_i^j}\right)}^2+\mathrm{\&epsiv;}]},$ ${\mathrm{\&eta;}}_w=\frac{E\left(n\right)}{4[\underset{O=1}{\overset{n_O}{\mathrm{\&Sigma;}}}\underset{k=1}{\overset{n_y}{\mathrm{\&Sigma;}}}{\left(\frac{\&PartialD;E\left(n\right)}{{\&PartialD;y}_O}\frac{\&PartialD;y_O}{\&PartialD;{\mathrm{\&omega;}}_k^o}\right)}^2+\mathrm{\&epsiv;}]}---\left(20\right)$

${\mathrm{\&eta;}}_{\mathrm{\&sigma;}}=\frac{E\left(n\right)}{4[\underset{i=1}{\overset{n_i}{\mathrm{\&Sigma;}}}\underset{j=1}{\overset{n_j}{\mathrm{\&Sigma;}}}\underset{O=1}{\overset{n_O}{\mathrm{\&Sigma;}}}{\left(\frac{\&PartialD;E\left(n\right)}{{\&PartialD;x}_i}\frac{\&PartialD;x_i}{\&PartialD;y_O}\frac{\&PartialD;y_O}{\&PartialD;{\mathrm{\&sigma;}}_i^j}\right)}^2+\mathrm{\&epsiv;}]},$ ${\mathrm{\&eta;}}_m=\frac{E\left(n\right)}{4[\underset{i=1}{\overset{n_i}{\mathrm{\&Sigma;}}}\underset{j=1}{\overset{n_j}{\mathrm{\&Sigma;}}}\underset{O=1}{\overset{n_O}{\mathrm{\&Sigma;}}}{\left(\frac{\&PartialD;E\left(n\right)}{{\&PartialD;x}_i{\&PartialD;y}_O}\frac{\&PartialD;x_i}{\&PartialD;y_O}\frac{\&PartialD;y_O}{\&PartialD;m_i^j}\right)}^2+\mathrm{\&epsiv;}]}$

In formula, ε is normal number.Energy function like this

$E(n+1)=E\left(n\right)+\mathrm{\&Delta;E}\left(n\right)$

$=\mathrm{\&epsiv;}({\mathrm{\&eta;}}_w+{\mathrm{\&eta;}}_m+{\mathrm{\&eta;}}_{\mathrm{\&sigma;}}+{\mathrm{\&eta;}}_{\mathrm{\&alpha;}})<\frac{E\left(n\right)}{4}+\frac{E\left(n\right)}{4}+\frac{E\left(n\right)}{4}+\frac{E\left(n\right)}{4}=E\left(n\right)$

Tracking in conjunction with Fig. 6 remote underwater robot is controlled, the CCD being equipped with by underwater robot obtains tracked object and scene image under water, by gray scale, process and cut apart, use neural network classifier to carry out feature extraction, classification and identification in conjunction with knowledge base and logical reasoning mechanism, adopting potential field method to plan robotic tracking path.Adopt the method for Fig. 4, underwater robot is sent to motion control instruction, thereby the ducted propeller of control makes it complete the action of tracking.

Control of collision avoidance in conjunction with Fig. 7 remote underwater robot, the Forward-looking Sonar being equipped with by underwater robot is obtained underwater obstacle information, by form, expand to learn acoustic image is cut apart and processed, obtain safety zone, adopt potential field method to plan robot collision avoidance path, adopt the method for Fig. 4, underwater robot is sent to motion control instruction, thereby the ducted propeller of control makes it complete the action of tracking.

In conjunction with Fig. 8, the mode that PC/104 embedded program adopts host computer SOCKET to trigger robot flush bonding processor is carried out rhythm control.Robot flush bonding processor is set up SOCKET server end, and bundling port starts to monitor and waits for.The water surface controller request connection of shaking hands.If success, triggers SOCKET event, PC/104 sends steering order, and sensing data is returned to water surface controller by SOCKET.Later every 0.1s water surface controller is sent out a steering order to PC/104, and SOCKET triggers PC/104: output steering order is to actuator; Processes sensor information also returns to water surface controller by SOCKET, completes the closed loop of a control.If unsuccessful, output error message, carries out error handling processing.

In Fig. 8, embedded software is by PC/104 bus communication under water.Comprise SOCKET communication module, analog signal voltage capture program, digital-to-analog (D/A) conversion and voltage send program, digital signal acquiring program.Wherein, SOCKET communication module is for the network service of water surface controller; Analog signal voltage capture program is responsible for the magnitude of voltage that sampling depth meter feeds back; D/A conversion and voltage send the rotating speed of programmed control ducted propeller; Water-leakage alarm in digital signal acquiring sequential monitoring watertight compartment.

In PC/104, adopted real-time embedded operating system (VxWorks).Because the embedded OS of VxWorks provides the BSP of Pentium3, BSP is carried out to simple modification and can use.Main is exactly the support for Compact Flash Card card (CF).CF card can be used as to a hard disk processes.And for network interface card, employing be Intel 82559ER network interface card, this is the network interface card of VxWorks acquiescence, drives and all can directly use.PC/104 just can be by startup self-detection CF card start-up VxWorks like this.The VxWorks kernel of acquiescence is supported two serial ports "/tyCo/0 " and "/the tyCo/1 " that on CUP plate, carry.Owing to adopting serial ports plate to carry out the data acquisition of optical fiber compass, therefore must under VxWorks, drive serial ports plate.

By receiving pose (the depth D epth (being z value) of robot, attitude angle (Yaw, Pith, Roll) and the information of whether leaking), the information that the video that CCD photographs and Forward-looking Sonar obtain and process, robot joystick or controller are to water surface controller sending controling instruction, and water surface controller sends to ducted propeller by thruster instruction by PC/104 by SOCKET.

Claims (1)

1. a water surface control method for remotely controlling underwater robot, it is characterized in that: what the CCD being equipped with by underwater robot and Forward-looking Sonar were obtained tracked object, scene image and underwater obstacle under water comprises that bow is to the information with the degree of depth, according to the information of obtaining adopt bow that improved Sage-Husa adaptive Kalman filter algorithm obtains underwater robot reality to and the degree of depth, according to the bow of the underwater robot reality obtaining to automatically underwater robot being sent to motion control instruction with the degree of depth, ether neural network DPRFNN algorithm by recurrence;

Described improved Sage-Husa adaptive Kalman filter algorithm is to increase on basic KALMAN filtering basis adjustment, wherein for the estimation of the variance battle array Q (k) of system noise sequence, for the estimation of measurement noise serial variance battle array R (k), for the estimated value of system noise statistics, estimated value for measurement noise statistics:

$\hat{q}\left(k\right)=(1-d_{k-1})\hat{q}(k-1)+d_{k-1}[\hat{X}(k/k)-\mathrm{\&Phi;}(k,k-1)\hat{X}(k-1/k-1)]$

$\begin{array}{c}\hat{Q}\left(k\right)=(1-d_{k-1})\hat{Q}(k-1)+d_{k-1}[K\left(k\right)\mathrm{\&epsiv;}\left(k\right){\mathrm{\&epsiv;}}^T\left(k\right)+P(k/k)\\ -\mathrm{\&Phi;}(k,k-1)P(k-1/k-1){\mathrm{\&Phi;}}^T(k,k-1)]\end{array}$

$\hat{r}\left(k\right)=(1-d_{k-1})\hat{r}(k-1)+d_{k-1}[Z\left(k\right)-H\left(k\right)\hat{X}(k-1/k-1)]$

$\hat{R}\left(k\right)=(1-d_{k-1})\hat{R}(k-1)+d_{k-1}[\mathrm{\&epsiv;}\left(k\right){\mathrm{\&epsiv;}}^T\left(k\right)-H\left(k\right)P(k/k-1)H^T\left(k\right)]$

Wherein the estimation of state X (k), φ (k, k-1) be t (k-1) constantly to t (k) step transfer matrix constantly, H (k) is for measuring battle array, $\mathrm{\&epsiv;}\left(k\right)=Z\left(k\right)-\hat{r}(k-1)-H\left(k\right)X(k/k-1)$ For new breath matrix, new breath includes the error of one-step prediction, and it is done to suitable weighting processing will separate correction $d_k=\frac{1-b}{1-b^{k-1}},$ B is forgetting factor, $b^{k-1}=\frac{{\tilde{Z}}_k^T{\tilde{Z}}_k-\mathrm{tr}[H_k{Q}_{k-1}{H}_k^T+R_k]}{\mathrm{tr}\left[H_k{\mathrm{\&Phi;}}_{k,k-1}{P}_{k-1}{\mathrm{\&Phi;}}_{k,k-1}^T{H}_k^T\right]},$ It plays vital effect to dispersing with precision of filtering, and by adjusting P _k+1kcontrol filter gain battle array K _k+1prevent dispersing of wave filter, when be false, press revise P _l+1k, wherein γ>=1 is to determine in advance adjustability coefficients, S _k+1adaptation coefficient, by the estimation of the variance battle array Q (k) of system noise sequence the estimation of measurement noise serial variance battle array R (k) the estimated value of system noise statistics the estimated value of measurement noise statistics combination just forms improved Sage-Husa adaptive Kalman filter algorithm with Kalman filtering;

Described ether neural network DPRFNN algorithm comprises input layer i, degree of membership layer j, petri layer, rules layer k layer and output layer o layer five-layer structure, recurrence feedback realizes by embedding feedback link at degree of membership layer, and the propagation of signal and the basic function of every one deck are expressed as follows:

(1) ground floor is input layer, and each node of input layer is directly input variable x _i(i=1,2,3,4) are directly delivered to lower one deck, and input node is depth error, angular error, diving speed error and angular velocity error, ground floor output:

(2) second layer is degree of membership layer, and each node in layer passes through membership function, and degree of membership is input as

$r_i^j\left(n\right)=x_i\left(n\right)+{\mathrm{\&mu;}}_i^j(n-1){\mathrm{\&alpha;}}_i^j$

N is frequency of training, the weights that represent self feed back circulation, be the output signal of the last training second layer, it defines by Gaussian subordinate function:

${\mathrm{net}}_j\left(r_i^j\right)=-\frac{{(r_i^j-m_i^j)}^2}{{\left({\mathrm{\&sigma;}}_i^j\right)}^2},{\mathrm{\&mu;}}_i^j\left[{\mathrm{net}}_j\left(r_i^j\right)\right]=\exp \left({\mathrm{net}}_j\left(r_i^j\right)\right)$

with the average of Gaussian subordinate function and the border of j fuzzy set that is respectively i input variable is accurate poor, and they are all adjustable parameters, j=1, and 2 ..., n _j, n _jthe quantity of the semantic variant of each input, second layer output;

(3) the 3rd layers is Petri layer, and it provides token, uses the following rules of competition

$t_i^j=\left\{\begin{array}{cc}1,& {\mathrm{\&mu;}}_i^j\left[{\mathrm{net}}_j\left(r_i^j\right)\right]\&GreaterEqual;d_{\mathrm{th}}\\ 0,& {\mathrm{\&mu;}}_i^j\left[{\mathrm{net}}_j\left(r_i^j\right)\right]<d_{\mathrm{th}}\end{array}\right.$

Wherein conversion value, d _thbe dynamic change threshold value, along with the variation of system responses error, change;

(4) the 4th layers is rules layer, and each node k is represented by Π, connects and takes advantage of input Output rusults:

${\mathrm{\&phi;}}_k=\left\{\begin{array}{cc}\underset{i=1}{\overset{4}{\mathrm{\&Pi;}}}{\mathrm{\&omega;}}_{\mathrm{ji}}^k{\mathrm{\&mu;}}_i^j\left[{\mathrm{net}}_j\left(r_i^j\right)\right],& t_i^j=1\\ 0,& t_i^j=0\end{array}\right.$

In formula the weights of petri layer and rules layer, for constant value, φ _k, k=1,2 ... n _y, n _yrule sum, φ _kthe 4th layer of output;

(5) layer 5 is output layer

Wherein, connect weights o the output intensity exported relevant with k rule, y _obe layer 5 output, output valve is the control magnitude of voltage of thruster;

The on-line learning algorithm that DPRFNN is used is supervision-ascent algorithm, and this algorithm is defined as for its energy function E first $E=\frac{1}{2}(e_d^2+{\stackrel{\&CenterDot;}{e}}_d^2+e_{\mathrm{\&theta;}}^2+{\stackrel{\&CenterDot;}{e}}_{\mathrm{\&theta;}}^2)e_{\mathrm{\&theta;}}$

E _d=h _d-h, e _θ=θ _d-θ be respectively the degree of depth and bow to error;

In above formula, h and θ be the real-time deep value of remote underwater robot in controlling and bow to value, h _dand θ _drespectively the expectation value of h and θ, dynamic change threshold value d _thby following formula, adjusted:

In formula, α and β are normal numbers;

At output layer, error back propagation value is ${\mathrm{\&delta;}}_o=e_d\frac{\&PartialD;h}{{\&PartialD;y}_o}+e_{\mathrm{\&theta;}}\frac{\&PartialD;\mathrm{\&theta;}}{{\&PartialD;y}_o}+{\stackrel{\&CenterDot;}{e}}_d\frac{\&PartialD;\stackrel{\&CenterDot;}{h}}{{\&PartialD;y}_o}+{\stackrel{\&CenterDot;}{e}}_{\mathrm{\&theta;}}\frac{\&PartialD;\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}{{\&PartialD;y}_o}$

Connect weights upgrade according to the following formula:

η _ωconnect weights learning rate, next constantly for:

The weights of rules layer are constant, and the error of this layer is: ${\mathrm{\&xi;}}_k=\left\{\begin{array}{cc}{\mathrm{\&delta;}}_o{\mathrm{\&omega;}}_k^o,& {\mathrm{\&phi;}}_k\&NotEqual;0\\ 0,& {\mathrm{\&phi;}}_k=0\end{array}\right.$

In Petri layer, error is calculated as follows: ${\mathrm{\&rho;}}_j\left(r_i^j\right)=\left\{\begin{array}{cc}\underset{k}{\mathrm{\&Sigma;}}{\mathrm{\&zeta;}}_k{\mathrm{\&phi;}}_k,& t_i^j=1\\ 0,& t_i^j=0\end{array}\right.$

The update rule of each parameter of degree of membership layer is:

${\mathrm{\&Delta;m}}_i^j={\mathrm{\&eta;}}_m{\mathrm{\&rho;}}_j\frac{2{(r_i^j-m_i^j)}^2}{{\left({\mathrm{\&sigma;}}_i^j\right)}^2}{\mathrm{\&mu;}}_i^j(n-1),{\mathrm{\&Delta;\&sigma;}}_i^j={\mathrm{\&eta;}}_s{\mathrm{\&rho;}}_j\frac{2{(r_i^j-m_i^j)}^2}{{\left({\mathrm{\&sigma;}}_i^j\right)}^3}$

${\mathrm{\&Delta;\&alpha;}}_i^j=-{\mathrm{\&eta;}}_{\mathrm{\&alpha;}}{\mathrm{\&rho;}}_j\frac{2(r_i^j-m_i^j)}{{\left({\mathrm{\&sigma;}}_i^j\right)}^2}{\mathrm{\&mu;}}_i^j(n-1)$

Next moment each parameter of degree of membership layer is:

$\begin{array}{c}{m}_i^j(n+1)=m_i^j\left(n\right)+{\mathrm{\&Delta;m}}_i^j\left(n\right),{\mathrm{\&sigma;}}_i^j(n+1)={\mathrm{\&sigma;}}_i^j\left(n\right)+{\mathrm{\&Delta;\&sigma;}}_i^j\left(n\right),\\ {\mathrm{\&alpha;}}_i^j(n+1)={\mathrm{\&alpha;}}_i^j\left(n\right)+{\mathrm{\&Delta;\&alpha;}}_i^j\left(n\right)\end{array}$

${\mathrm{\&eta;}}_m=\frac{E\left(n\right)}{4[\underset{i=1}{\overset{n_i}{\mathrm{\&Sigma;}}}\underset{j=1}{\overset{n_j}{\mathrm{\&Sigma;}}}\underset{o=1}{\overset{n_o}{\mathrm{\&Sigma;}}}{\left(\frac{\&PartialD;E\left(n\right)}{{\&PartialD;x}_i}\frac{{\&PartialD;x}_i}{{\&PartialD;y}_o}\frac{{\&PartialD;y}_o}{{\&PartialD;m}_i^j}\right)}^2+\mathrm{\&epsiv;}]},{\mathrm{\&eta;}}_w=\frac{E\left(n\right)}{4[\underset{o=1}{\overset{n_o}{\mathrm{\&Sigma;}}}\underset{k=1}{\overset{n_y}{\mathrm{\&Sigma;}}}{\left(\frac{\&PartialD;E\left(n\right)}{{\&PartialD;y}_o}\frac{{\&PartialD;y}_o}{{\&PartialD;\mathrm{\&omega;}}_k^o}\right)}^2+\mathrm{\&epsiv;}]},$

${\mathrm{\&eta;}}_s=\frac{E\left(n\right)}{4[\underset{i=1}{\overset{n_i}{\mathrm{\&Sigma;}}}\underset{j=1}{\overset{n_j}{\mathrm{\&Sigma;}}}\underset{o=1}{\overset{n_o}{\mathrm{\&Sigma;}}}{\left(\frac{\&PartialD;E\left(n\right)}{{\&PartialD;x}_i}\frac{{\&PartialD;x}_i}{{\&PartialD;y}_O}\frac{{\&PartialD;y}_O}{{\&PartialD;\mathrm{\&sigma;}}_i^j}\right)}^2+\mathrm{\&epsiv;}]},$

${\mathrm{\&eta;}}_{\mathrm{\&alpha;}}=\frac{E\left(n\right)}{4[\underset{i=1}{\overset{n_i}{\mathrm{\&Sigma;}}}\underset{j=1}{\overset{n_j}{\mathrm{\&Sigma;}}}\underset{o=1}{\overset{n_o}{\mathrm{\&Sigma;}}}{\left(\frac{\&PartialD;E\left(n\right)}{{\&PartialD;x}_i}\frac{{\&PartialD;x}_i}{{\&PartialD;y}_o}\frac{{\&PartialD;y}_o}{{\&PartialD;\mathrm{\&alpha;}}_i^j}\right)}^2+\mathrm{\&epsiv;}]},$

η in formula _m, η _s, η _αbe the Study rate parameter of Gaussian function, ε is normal number.