CN101825903A - 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, specifically is applied to a kind of control method of field robots such as data acquisition under water, deep ocean work and hull detection.

Background technology

The ocean of taking up an area of ball surface area 75%, be one richly endowed and obtain the treasure-house developed far away.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 no avoidable selections.Underwater robot is because its action is flexible, the power abundance, the transmission of information and data and exchange efficient and convenient, data volume is big, can in water, work long hours, now be widely used in 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 of underwater robot and application has the important strategic meaning.

Application number is 200810064256.3 Chinese patent file (open day: the control method that disclosed on April 8th, 2008) " based on the underwater hiding-machine space variable structure control method of recurrence fuzzy neural network " provides the underwater hiding-machine autopilot to control.Though the underwater hiding-machine space variable structure control method based on the recurrence fuzzy neural network belongs to same technical field with this patent, but it is to rise the rudder control system by yaw rudder control system, casing rudder control system, the tail that designs based on recurrence fuzzy neural network (RFNN), and then constituting the underwater submersible space motion combined control system, the method that realizes with the present invention is different.

And the present invention controls the motion of underwater robot by obtaining sensor and environment sensing information, and then realizes that it keeps away barrier, follows the tracks of functions such as detection.

Summary of the invention

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

The object of the present invention is achieved like this:

The information that CCD that the present invention is equipped with by underwater robot and Forward-looking Sonar are obtained 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 that obtains to automatically underwater robot being sent 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 the system noise statistics on basic KALMAN filtering basis

Estimated value with the measurement noise statistics

Adjustment:

The 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)]$

The 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)]$

The 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)]$

The 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

Be 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,

Be the estimation of the variance battle array Q (k) of system noise sequence,

Be the estimation of measurement noise serial variance battle array R (k),

Be new breath matrix, new breath includes the error of one-step prediction, and it is made suitable weighted will

Separate correction

B is a forgetting factor

It plays crucial effects 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, then press

Revise P _K+1|k, wherein γ 〉=1 is to determine adjustability coefficients, S in advance _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 constituted 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, the recurrence feedback realizes that by embed the feedback connection at the degree of membership layer propagation of signal and the basic function of each layer are expressed as follows:

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

(2) second layer is the degree of membership layer, and each node in the layer all 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 a frequency of training, α _i ^jBe to represent self feed back round-robin weights, μ _i ^j(n-1) be the output signal of the last training second layer, it defines by the 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 the average and the standard deviation of Gaussian subordinate function of j fuzzy set of i input variable, they all are adjustable parameters, j=1, and 2 ..., n _j, n _jBe the quantity of the semantic variant of each input, μ _i ^j[net _j(r _i ^j)] be second layer output;

(3) the 3rd layers is the 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;{\mathrm{<figure-callout\; id="0"\; label="d\; th"\; filenames="HSA00000099087200021.png"\; state="\{\{state\}\}">d</figure-callout>}}_{\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 ^jBe conversion value, d _ThBe dynamic change threshold value, along with the system responses error and change;

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

${\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 the formula _Ji ^kBe the weights of Petri net and rules layer, for constant value, φ _k(k=1,2 ... n _y) output of k layer, n _yBe the rule sum, φ _kBe the 4th layer of output;

(5) layer 5 is an output layer:

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

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

In the formula h and θ be in the control real-time deep value of remote underwater robot and bow to value, h _dAnd θ _dBe respectively 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 _ThAdjust by following formula:

α and β are positive constants in the formula;

At output layer, the error back propagation value is:

Connect weights ω _k ^oUpgrade according to following formula:

η _ωConnect the weights learning rate, next is ω constantly _k ^oFor:

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

The following calculating of error in the Petri layer

${\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 constantly is:

$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 the formula _m, η _w, η _σ, η _αBe the learning rate parameter of Gaussian function, ε is positive constant.

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

Description of drawings

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 system method system block diagram;

Fig. 3 is an 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 a remote underwater robot pursuit movement control flow chart of the present invention;

Fig. 7 is a 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 get in touch SOCKET communication process figure.

Embodiment

For example the present invention is done description in more detail below in conjunction with accompanying drawing:

In conjunction with Fig. 1～8, the main body of present embodiment is a water surface remote control system method.Wherein water surface remote control system method hardware links to each other with ROV by optical fiber, realizes the control of underwater robot.Underwater robot autocontrol method water surface controller adopts WindowsXP operating system, utilizes the 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 robot body two parts, the hardware device of water surface remote control platform is by a water surface main control computer, fiber optic, LCD and a robot controlled in wireless control crank are formed.The software of water surface remote control platform is controlled system by the operating rod hand of underwater robot, sound visual identity and keep away the barrier algorithm, vision planning track algorithm, sensor filtering algorithm and the SOCKET communication module composition of PC/104 under water.The water surface remote control platform obtains operation (comprising video, acoustic image), attitude and depth information (through Filtering Processing) by environment sensing equipment and the motion awareness apparatus that underwater robot is equipped with, the effector can be by operating rod hand behaviour, directed and the depthkeeping control of FNN, follow the tracks of and collision prevention control, distribution of machine people's motion control commands, this order passes to PC/104 under water by SOCKET communication module and fiber optic, thus the ducted propeller execution of control robot.

Control the system method provides underwater robot in real time for the operator the current degree of depth in conjunction with Fig. 2 wireless operating bar hand; Bow is to the angle, each thruster information of voltage; Movement locus and current location, information such as video that is collected and Forward-looking Sonar (SONAR) signal, the operator is according to these information, giving under water, six ducted propeller thrusters of robot (comprise that two are promoted mainly, two thrusters, two vertical pushing away) send control voltage, control robot is advanced 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 the angle and the degree of depth; Hand behaviour carries out fault detection and diagnosis to equipment simultaneously, normally moves with assurance equipment.

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:

The 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)]$

The 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)]$

The 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)]$

The 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

Be 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,

Be the estimation of the variance battle array Q (k) of system noise sequence,

Be the estimation of measurement noise serial variance battle array R (k),

Be new breath matrix, new breath includes the error of one-step prediction, to its do suitable weighted just can with

Separate correction

B is a forgetting factor

It plays crucial effects 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, then press

Revise P _K+1|k, wherein γ 〉=1 is to determine adjustability coefficients, S in advance _K+1It is adaptation coefficient.

Top formula and Kalman filtering have just been constituted improved Sage-Husa adaptive Kalman filter algorithm in conjunction with alternately calculating.

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

Five layers of DPRFNN structure that automatic control remote underwater robot uses have been described among 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.The recurrence feedback realizes by embed the feedback connection at the degree of membership layer.The propagation of signal and the basic function of each layer are expressed as follows:

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

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

Here n is a frequency of training, α _i ^jBe to represent self feed back round-robin weights, μ _i ^j(n-1) be the output signal of the last training second layer, it defines by the 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 the average and the standard deviation of Gaussian subordinate function of j fuzzy set of i input variable, they all are adjustable parameters.J=1,2 ..., n _j, n _jBe the quantity of the semantic variant of each input, μ _i ^j[net _j(r _i ^j)] be second layer output.

The 3rd layer is the Petri layer.The purpose of this layer is in order to provide token, to use 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;{\mathrm{<figure-callout\; id="0"\; label="d\; th"\; filenames="HSA00000099087200021.png"\; state="\{\{state\}\}">d</figure-callout>}}_{\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 ^jBe conversion value, d _ThBe the threshold value of dynamic change, it will along with the system responses error and change

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

${\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 the 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 _yBe the rule sum, φ _kBe the 4th layer of output.

Layer 5 is an output layer.

Wherein, connect weights ω _k ^oBe O the output intensity exported relevant with k bar rule, y _oBe layer 5 output.Output valve is the control magnitude of voltage of thruster in this patent.

The on-line learning algorithm that DPRFNN uses is a supervision-ascent algorithm, and at 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 the formula h and θ be in the control real-time deep value of remote underwater robot 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.

α and β are positive constants in the formula.This means that the big more threshold value of error is more little, promptly the big more threshold value of error reduces, so that control law as much as possible comes into operation.

At output layer, the error back propagation value is

Connect weights ω _k ^oUpgrade according to following formula

η _ωConnect the weights learning rate, next is ω constantly _k ^oFor:

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

The following calculating of error in the Petri layer

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 constantly is:

$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 learning 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;}]}$

ε is positive constant in the formula.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 Control in conjunction with Fig. 6 remote underwater robot, the CCD that is equipped with by underwater robot obtains tracked object and scene image under water, handle and cut apart, use neural network classifier to carry out feature extraction, classification and identification in conjunction with knowledge base and logical reasoning mechanism by gray scale, the employing potential field method is planned the robotic tracking path.Adopt the method for Fig. 4, underwater robot is sent motion control instruction, thereby the ducted propeller of control robot makes it finish the action of tracking.

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

In conjunction with Fig. 8, the mode that the PC/104 embedded program adopts host computer SOCKET to trigger the robot flush bonding processor is carried out beat control.The robot flush bonding processor is set up the SOCKET server end, and bundling port begins to monitor and waits for.The water surface controller request connection of shaking hands.If success triggers the SOCKET incident, 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 and given PC/104, and SOCKET triggers PC/104: the output steering order is given actuator; Processes sensor information also returns to water surface controller by SOCKET, finishes the closed loop of a control.If unsuccessful, output error message carries out error handling processing.

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

Adopted real-time embedded operating system (VxWorks) among the PC/104.Because the embedded OS of VxWorks provides the BSP of Pentium3, and BSP is simply revised and can use.It mainly is exactly support for Compact Flash Card card (CF).Can be used as a hard disk to the CF card handles.And for network interface card, employing be Intel 82559ER network interface card, this is the network interface card of VxWorks acquiescence, drives all can directly use.PC/104 just can be by startup self-detection CF card start-up VxWorks like this.Two serial ports "/tyCo/0 " and "/the tyCo/1 " that carry on the VxWorks kernel support CUP plate of acquiescence.Owing to adopt the serial ports plate to carry out the data acquisition of optical fiber compass, therefore must under VxWorks, drive the serial ports plate.

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

Claims (3)

1. water surface control method for remotely controlling underwater robot, it is characterized in that: the information that CCD that is equipped with by underwater robot and Forward-looking Sonar are obtained 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 that obtains to automatically underwater robot being sent motion control instruction with the degree of depth, ether neural network DPRFNN algorithm by recurrence.

2. a kind of water surface control method for remotely controlling underwater robot according to claim 1 is characterized in that: described improved Sage-Husa adaptive Kalman filter algorithm is the estimated value that increases the system noise statistics on basic KALMAN filtering basis

Estimated value with the measurement noise statistics

Adjustment:

The 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)]$

The 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)]$

The 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)]$

The 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

Be 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,

Be the estimation of the variance battle array Q (k) of system noise sequence,

Be the estimation of measurement noise serial variance battle array R (k),

Be new breath matrix, new breath includes the error of one-step prediction, and it is made suitable weighted will

Separate correction

B is a forgetting factor

It plays crucial effects 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, then press

Revise P _K+1|k, wherein γ 〉=1 is to determine adjustability coefficients, S in advance _K+1Be adaptation coefficient, system interference average, system interference variance matrix, measurement noise average, measuring noise square difference matrix and Kalman filtering combination are just constituted improved Sage-Husa adaptive Kalman filter algorithm.

3. a kind of water surface control method for remotely controlling underwater robot according to claim 1 and 2, it is characterized in that: 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, the recurrence feedback realizes that by embed the feedback connection at the degree of membership layer propagation of signal and the basic function of each layer are expressed as follows:

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

${\mathrm{net}}_i^1=x_i^1;$

(2) second layer is the degree of membership layer, and each node in the layer all 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 a frequency of training, α _i ^jBe to represent self feed back round-robin weights, μ _i ^j(n-1) be the output signal of the last training second layer, it defines by the 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)$

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

(3) the 3rd layers is the 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 ^jBe conversion value, d _ThBe dynamic change threshold value, along with the system responses error and change;

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

${\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 the formula _Ji ^kBe the weights of Petri net and rules layer, for constant value, φ _k(k=1,2 ... n _y) output of k layer, n _yBe the rule sum, φ _kBe the 4th layer of output;

(5) layer 5 is an output layer

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

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

In the formula h and θ be in the control real-time deep value of remote underwater robot and bow to value, h _dAnd θ _dBe respectively 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 _ThAdjust by following formula:

α and β are positive constants in the formula;

At output layer, the error back propagation value is

Connect weights ω _k ^oUpgrade according to following formula:

η _ωConnect the weights learning rate, next is ω constantly _k ^oFor:

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

The following calculating of error in the Petri layer:

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 constantly is:

$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 the formula _m, η _w, η _σ, η _αBe the learning rate parameter of Gaussian function, ε is positive constant.

Images