CN104571128A - Obstacle avoidance method used for underwater robot and based on distance and parallax information - Google Patents

Abstract

An obstacle avoidance method used for an underwater robot and based on distance and parallax information comprises steps as follows: a control device performs overall path planning on the underwater robot according to an operation task and nautical chart database information; a detection sonar and a camera are taken as overall and local obstacle measuring instruments respectively so as to obtain relative distance and parallax information of the underwater robot and an obstacle; then the control device judges whether obstacle avoidance is required and adopts predictive control to adjust an obstacle avoidance strategy; if the underwater robot arrives to a target point, the path planning is finished. The obstacle avoidance method has the advantages of capacity of dynamically adjusting the obstacle avoidance strategy, better path, good avoidance effect, high reliability and the like, can handle complex obstacles, and improves the reliability of the underwater robot for executing the tasks.

Description

Based on the underwater robot barrier-avoiding method of distance with parallax information

Technical field

The present invention relates to underwater robot field of intelligent control technology, the underwater robot barrier-avoiding method of especially a kind of fusion distance and parallax information.

Background technology

Underwater robot is searched and rescued in the perils of the sea, anti-intrusion technology, species are followed the tracks of the fields such as detection and are with a wide range of applications under water.Under water in environment, sediment and the reefs etc. such as shipwreck make underwater environment very complicated, if underwater robot can not initiatively, hide these obstacles effectively, the failure of executing the task likely causing underwater robot even causes underwater robot to damage.

Retrieve discovery in the prior art, Chinese Patent Application No. is 201310488141.8, name is called: based on the underwater robot collision prevention method of Forward-looking Sonar, and the method Forward-looking Sonar view data is introduced in robot obstacle-avoiding strategy, can reduce robot collision avoidance blind area.But said method is when designing collision-free Trajectory Planning algorithm, do not consider the control inputs constraint of underwater robot.In actual applications, underwater robot is all generally driven by motor, and motor driver has Power Limitation, when input signal exceedes this restriction, saturated generation, and then the dynamic property of system is reduced (such as, overshoot, regulating time and concussion number of times etc.), even cause closed-loop system unstable.

Further, Chinese Patent Application No. is 201210539473.X, name is called: a kind of automatic obstacle avoiding method of Intelligent Underwater Robot, and underwater robot moving target, barrier and underwater robot control performance carry out unifying to consider to realize keeping away barrier task by the method.Said method designs based on range information, but only consider that range information is inadequate, because effective avoid-obstacle behavior not only needs range information in real time, also need Viewing-angle information, such as people, in the process of obstacle avoidance, are effectively kept away barrier route and are jointly established by distance and parallax adjustment.Under water in three-dimensional environment, directivity seems even more important.Therefore, how under water under complex barrier substance environment, utilize Distance geometry parallax information to underwater robot design collision-free trajectory optimisation strategy, to improve the validity of underwater robot trajectory planning, seem particularly important.

Summary of the invention

The object of the invention be to provide a kind of dynamic adjust obstacle strategy, path more excellent, hide effective, reliability high based on the underwater robot barrier-avoiding method of distance with parallax information.

For achieving the above object, have employed following technical scheme:

The present invention mainly comprises underwater robot, detection sonar, forward direction infrared camera, side direction infrared camera and afterbody infrared camera, under water detection sonar is installed by robot, the front end symmetry of robot installs two forward direction infrared cameras under water, robot top two side position difference installation side is to infrared camera under water, robot afterbody symmetry installs afterbody infrared camera under water, and described barrier-avoiding method comprises the following steps:

(1) according to the position of underwater robot and impact point, and chart data library information segregation reasons goes out the overall reference locus of underwater robot, is set as follows cost function and makes underwater robot can move to impact point in accessible situation

$E\left(t\right)=\mathrm{\ω}{\left|\right|q\left(t\right)-q_0\left(t\right)\left|\right|}^2+\mathrm{\υ}{\left|\right|\stackrel{\·}{q}\left(t\right)-{\stackrel{\·}{q}}_0\left(t\right)\left|\right|}^2,$

In formula, ω >0 and υ >0 is scale factor; for reference locus, q ₀(t) ∈ R ³with represent the Position And Velocity vector of reference locus respectively;

It is the particle of m that described underwater robot is considered as a quality, and particle postition is underwater robot central point, and particle has three degree of freedom in space coordinates, i.e. q (t)=[x (t), y (t), z (t)] ^t; By feedback linearization, the kinematical equation of underwater robot can use second-order integrator model representation, and its medium velocity and control inputs vector are expressed as with u (t)=[u _x(t), u _y(t), u _z(t)] ^t; Control inputs constraint representation is-u _xmin≤ u _x(t)≤u _xmax,-u _ymin≤ u _y(t)≤u _ymaxwith-u _zmin≤ u _z(t)≤u _zmax, wherein u _xmin>0, u _xmax>0, u _ymin>0, u _ymax>0, u _zmin>0 and u _zmax>0 is input saturation constant;

(2) detection sonar is as Global obstacle surveying instrument, and monitoring underwater robot periphery long distance environment information, obtains the relative distance information of underwater robot and barrier;

Infrared camera is as local disturbance's surveying instrument, and monitoring underwater robot periphery closely environmental information, obtains the Viewing-angle information of closely barrier;

(3) judge whether that barrier kept away by needs, if need to keep away barrier, according to relative distance information, utilize PREDICTIVE CONTROL to adjust Robot dodge strategy, construct the optimized algorithm based on PREDICTIVE CONTROL; If do not need to keep away barrier, according to former route running;

(4) if underwater robot arrives impact point, then trajectory planning terminates, and finishes the work; If underwater robot does not arrive impact point, then re-execute step (2).

Further, the Robot dodge strategy in step (3) is as follows:

First, barrier set is defined; The spatial dimension of setting barrier represents by the common factor/union of multiple ball; Utilize the superposition of ball, the final barrier region described out is irregularly shaped or regular shape; Therefore, m (m=1 ...) individual barrier is included in a two-dimensional circle, uses symbol B _m(O _m, ρ _m) represent, wherein O _m(O _m∈ R ³) and ρ _m(ρ _m>0) represent center and the radius of this ball respectively, and then the collision prevention of underwater robot and barrier m is constrained to || q-O _m|| > ρ _m+ d _s;

Secondly, the region residing for barrier is divided; Setting d is barrier and underwater robot surface minimum distance, d _safefor keeping away barrier warning distance, d _lremote for keeping away barrier, d _sfor keeping away barrier closely, d _safe>d _l>d _s>0;

Work as d>d _safetime, barrier is in outside warning line, does not need to hide obstacle;

Work as d _safe>=d>d _ltime, barrier is in remote obstacle-avoidance area, and the range information that underwater robot utilizes detection sonar to feed back carries out keeping away barrier, constructs the potential-energy function based on range information

$E_{\mathrm{obs}}^d=\frac{{\left(\right||q-O_m||-d_l-{\mathrm{\ρ}}_m)}^2}{\left|\right|q-O_m\left|\right|-{\mathrm{\ρ}}_m},$

Wherein time, underwater robot can move along barrier influence domain radius under the prerequisite that do not collide with barrier;

Work as d _l>=d>d _stime, barrier is in closely obstacle-avoidance area, and the parallax information that underwater robot utilizes infrared camera to feed back carries out keeping away barrier, structure parallax function as the terminal state controller of predictive control algorithm, the subtense angle of forward direction infrared camera square shaped barrier is θ ₁, the subtense angle of side direction infrared camera to irregular shape barrier is θ ₂, underwater robot movement direction angle is α, considers the direction of motion of underwater robot, corrects, the parallax that must make new advances to parallax with

Structure is similar to parallax function using as terminal state controller, wherein k _obs>0 is weighted value; Minimize visual angle function, finally desirable angle is underwater obstacle finally departs from the central point of infrared camera, realizes the active dodge to barrier;

Fusion distance and parallax information construct keeps away barrier function, as the terminal state controller of predictive control algorithm; Wherein d _safe>=d>d _s; In obstacle-avoidance area, along with barrier convergence underwater robot, the barrier of keeping away based on parallax occupies leading role, and on the contrary, along with barrier is away from underwater robot, the barrier of keeping away based on distance occupies leading role.

Further, the optimized algorithm of described PREDICTIVE CONTROL, at any sampling instant t _k, solve the finite time-domain open loop optimization problem relating to state constraint and control constraints and obtain one group of control sequence u ^*, show that the supposition in prediction time domain exports, and current control inputs acted on controlled device; At next sampling instant t _k+1, repeated the optimizing process in a upper moment based on new original state, continuous rolling optimization, thus form closed-loop control, complete collision-free trajectory and optimize task.

Compared with prior art, tool of the present invention has the following advantages:

1, underwater robot can according to the relative distance and Viewing-angle information with barrier, and dynamic conditioning Robot dodge strategy can reflect the information of underwater obstacle better, and path is more excellent, hide obstacle better effects if;

2, in control algolithm, have employed PREDICTIVE CONTROL, can easily control inputs constraint and state constraint be attached in the design of controller, underwater robot effectively can be avoided to be absorbed in input state of saturation, to achieve the seamless fusion of the constraint of underwater robot control inputs and Robot dodge strategy.

Accompanying drawing explanation

Fig. 1 be the inventive method keep away barrier process flow diagram.

Fig. 2 is that the obstacle-avoidance area of underwater robot in the inventive method divides design sketch.

Fig. 3 is that in the inventive method, underwater robot constructs vertical view to the visual angle of underwater obstacle.

Drawing reference numeral: 1-underwater robot, 2-detection sonar, 3-forward direction infrared camera, 4-side direction infrared camera, 5-afterbody infrared camera, 6-square obstacle, 7-irregular slalom thing.

Embodiment

Below in conjunction with accompanying drawing, the present invention will be further described:

The present invention mainly comprises underwater robot 1, detection sonar 2, forward direction infrared camera 3, side direction infrared camera 4, and afterbody infrared camera 5, under water detection sonar is installed by robot, the front end symmetry of robot installs two forward direction infrared cameras under water, robot top two side position difference installation side is to infrared camera under water, afterbody symmetry installs two afterbody infrared cameras, and described barrier-avoiding method comprises the following steps:

Keeping away of the inventive method as shown in Figure 1 hinders in process flow diagram;

(1) according to the position of underwater robot and impact point, and chart data library information segregation reasons goes out the overall reference locus of underwater robot, is easy to draw, optimal reference locus is the line segment between underwater robot to impact point.But consider the complicacy of underwater environment, such as fix the stops such as reef, need to consider chart data library information, and then design one from underwater robot starting point to the nearest circuit of impact point as with reference to track, and be set as follows cost function and make underwater robot can move to impact point in accessible situation

$E\left(t\right)=\mathrm{\ω}{\left|\right|q\left(t\right)-q_0\left(t\right)\left|\right|}^2+\mathrm{\υ}{\left|\right|\stackrel{\·}{q}\left(t\right)-{\stackrel{\·}{q}}_0\left(t\right)\left|\right|}^2,$

In formula, ω >0 and υ >0 is scale factor; for reference locus, q ₀(t) ∈ R ³with represent the Position And Velocity vector of reference locus respectively;

It is the particle of m that described underwater robot is considered as a quality, and particle postition is underwater robot central point, and particle has three degree of freedom in space coordinates, i.e. q (t)=[x (t), y (t), z (t)] ^t; By feedback linearization, the kinematical equation of underwater robot can use second-order integrator model representation, and its medium velocity and control inputs (i.e. acceleration) vector are expressed as with u (t)=[u _x(t), u _y(t), u _z(t)] ^t; Control inputs constraint representation is-u _{x min}≤ u _x(t)≤u _{x max},-u _{y min}≤ u _y(t)≤u _{y max}with-u _{z min}≤ u _z(t)≤u _{z max}, wherein u _{x min}>0, u _{x max}>0, u _{y min}>0, u _{y max}>0, u _{z min}>0 and u _{z max}>0 is input saturation constant;

(2) above-mentioned overall trajectory planning off-line on known chart zoom Information base draws, but in the underwater environment of complicated dynamic change, nautical chart information likely has a very large change, such as shipwreck deposition and reef move, and the path causing global path planning method to calculate probably is blocked.Therefore, underwater robot needs the change tread adjustment track according to underwater environment on overall trajectory planning basis.And then detection sonar is as Global obstacle surveying instrument, and monitoring underwater robot periphery long distance environment information, obtains the relative distance information of underwater robot and barrier; Infrared camera is as local disturbance's surveying instrument, and monitoring underwater robot periphery closely environmental information, obtains the Viewing-angle information of closely barrier;

(3) judge whether that barrier kept away by needs, if need to keep away barrier, according to relative distance information, utilize PREDICTIVE CONTROL to adjust Robot dodge strategy, construct the optimized algorithm based on PREDICTIVE CONTROL; If do not need to keep away barrier, according to former route running;

First, barrier set is defined; The spatial dimension of setting barrier represents by the common factor/union of multiple ball; Utilize the superposition of ball, the final barrier region described out is irregularly shaped or regular shape; Therefore, m (m=1 ...) individual barrier is included in a two-dimensional circle, uses symbol B _m(O _m, ρ _m) represent, wherein O _m(O _m∈ R ³) and ρ _m(ρ _m>0) represent center and the radius of this ball respectively, and then the collision prevention of underwater robot and barrier m is constrained to || q-O _m|| > ρ _m+ d _s;

Secondly, when underwater robot perceive barrier exist time, need the region residing for disturbance in judgement thing; Setting d is barrier and underwater robot surface minimum distance, d _safefor keeping away barrier warning distance, d _lremote for keeping away barrier, d _sfor keeping away barrier closely, d _safe>d _l>d _s>0;

As shown in Figure 2, d>d is worked as _safetime, barrier is in outside warning line, does not need to hide obstacle;

Work as d _safe>=d>d _ltime, barrier is in remote obstacle-avoidance area, and the range information that underwater robot utilizes detection sonar to feed back carries out keeping away barrier, constructs the potential-energy function based on range information

$E_{\mathrm{obs}}^d=\frac{{\left(\right||q-O_m||-d_l-{\mathrm{\ρ}}_m)}^2}{\left|\right|q-O_m\left|\right|-{\mathrm{\ρ}}_m},$

Wherein time, underwater robot can move along barrier influence domain radius under the prerequisite that do not collide with barrier;

Work as d _l>=d>d _stime, barrier is in closely obstacle-avoidance area, and the parallax information that underwater robot utilizes infrared camera to feed back carries out keeping away barrier, structure parallax function as the terminal state controller of predictive control algorithm, the subtense angle of forward direction infrared camera (being arranged on underwater robot anterior position) square shaped barrier 6 is θ ₁, side direction infrared camera (being arranged on underwater robot top two side position) is θ to the subtense angle of irregular shape barrier 7 ₂, underwater robot movement direction angle is α, considers the direction of motion of underwater robot, corrects, the parallax that must make new advances to parallax with

Structure is similar to parallax function using as terminal state controller, wherein k _obs>0 is weighted value; Can find out, if minimize visual angle function, finally desirable angle is underwater obstacle finally departs from the central point of infrared camera, and underwater robot can realize the active dodge to barrier accordingly;

Fusion distance and parallax information construct keeps away barrier function, as the terminal state controller of predictive control algorithm; Wherein d _safe>=d>d _s; In obstacle-avoidance area, along with barrier convergence underwater robot, the barrier of keeping away based on parallax occupies leading role, and on the contrary, along with barrier is away from underwater robot, the barrier of keeping away based on distance occupies leading role.

Work as d _sduring>=d>0, be the region that is in extreme danger, now need, under above-mentioned Robot dodge strategy orders about, to avoid barrier to enter region.

Finally construct the optimized algorithm based on PREDICTIVE CONTROL, at any sampling instant t _k, solve the finite time-domain open loop optimization problem relating to state constraint (avoidance obstacle constraint) and control constraints (input saturation constraints) and obtain one group of control sequence u ^*, show that the supposition in prediction time domain exports, and current control inputs acted on controlled device; At next sampling instant t _k+1, repeated the optimizing process in a upper moment based on new original state, continuous rolling optimization, thus form closed-loop control, complete collision-free trajectory and optimize task.

Embodiment one: the sampling period is δ >0, and prediction step is prediction time domain is T=N _rδ, the prediction moment is t _k=t ₀+ k δ, wherein pREDICTIVE CONTROL problem builds as follows:

At updated time t _k, for underwater robot: given current state z (t _k) with prediction time domain in control inputs wherein τ ∈ [t _k, t _k+ T], solve similar following optimal control problem

$J^{*}(t_k,z^{*}\left(t_k\right),u^{*}(\·))=\underset{u(\·)}{\min }J(z\left(t_k\right),u(\·))$

S.t. system model, collision prevention constraint and input saturation constraints

And then, the optimum control input of underwater robot can be drawn, and then draw optimum output state.

(4) if underwater robot arrives impact point, then trajectory planning terminates, and finishes the work; If underwater robot does not arrive impact point, then re-execute step (2), continue dynamic optimization track.

The present invention is not limited to above-mentioned preferred implementation, and anyone should learn the structure change made under enlightenment of the present invention, and every have identical or akin technical scheme with the present invention, all belongs to protection scope of the present invention.

Claims (3)

1. one kind based on the underwater robot barrier-avoiding method of distance with parallax information, comprise underwater robot, detection sonar, forward direction infrared camera, side direction infrared camera and afterbody infrared camera, under water detection sonar is installed by robot, the front end symmetry of robot installs two forward direction infrared cameras under water, robot top two side position difference installation side is to infrared camera under water, robot afterbody symmetry installs two afterbody infrared cameras under water, it is characterized in that, described barrier-avoiding method comprises the following steps:

(1) according to the position of underwater robot and impact point, and chart data library information segregation reasons goes out the overall reference locus of underwater robot, is set as follows cost function and makes underwater robot can move to impact point in accessible situation

$E\left(t\right)=\mathrm{\ω}{\left|\right|q\left(t\right)-q_0\left(t\right)\left|\right|}^2+\mathrm{\υ}{\left|\right|\stackrel{.}{q}\left(t\right)-{\stackrel{.}{q}}_0\left(t\right)\left|\right|}^2,$

In formula, ω >0 and υ >0 is scale factor; for reference locus, q ₀(t) ∈ R ³with represent the Position And Velocity vector of reference locus respectively;

It is the particle of m that described underwater robot is considered as a quality, and particle postition is underwater robot central point, and particle has three degree of freedom in space coordinates, i.e. q (t)=[x (t), y (t), z (t)] ^t; By feedback linearization, the kinematical equation of underwater robot can use second-order integrator model representation, and its medium velocity and control inputs vector are expressed as with u (t)=[u _x(t), u _y(t), u _z(t)] ^t; Control inputs constraint representation is-u _xmin≤ u _x(t)≤u _xmax,-u _ymin≤ u _y(t)≤u _ymaxwith-u _zmin≤ u _z(t)≤u _zmax, wherein u _xmin>0, u _xmax>0, u _ymin>0, u _ymax>0, u _zmin>0 and u _zmax>0 is input saturation constant;

(2) detection sonar is as Global obstacle surveying instrument, and monitoring underwater robot periphery long distance environment information, obtains the relative distance information of underwater robot and barrier;

Infrared camera is as local disturbance's surveying instrument, and monitoring underwater robot periphery closely environmental information, obtains the Viewing-angle information of closely barrier;

(3) judge whether that barrier kept away by needs, if need to keep away barrier, according to relative distance information, utilize PREDICTIVE CONTROL to adjust Robot dodge strategy, construct the optimized algorithm based on PREDICTIVE CONTROL; If do not need to keep away barrier, according to former route running;

(4) if underwater robot arrives impact point, then trajectory planning terminates, and finishes the work; If underwater robot does not arrive impact point, then re-execute step (2).

2. according to claim 1ly it is characterized in that based on the underwater robot barrier-avoiding method of distance with parallax information, the Robot dodge strategy in described step (3) is as follows:

First, barrier set is defined; The spatial dimension of setting barrier represents by the common factor/union of multiple ball; Utilize the superposition of ball, the final barrier region described out is irregularly shaped or regular shape; Therefore, m (m=1 ...) individual barrier is included in a two-dimensional circle, uses symbol B _m(O _m, ρ _m) represent, wherein O _m(O _m∈ R ³) and ρ _m(ρ _m>0) represent center and the radius of this ball respectively, and then the collision prevention of underwater robot and barrier m is constrained to || q-O _m|| > ρ _m+ d _s;

Secondly, the region residing for barrier is divided; Setting d is barrier and underwater robot surface minimum distance, d _safefor keeping away barrier warning distance, d _lremote for keeping away barrier, d _sfor keeping away barrier closely, d _safe>d _l>d _s>0;

Work as d>d _safetime, barrier is in outside warning line, does not need to hide obstacle;

Work as d _safe>=d>d _ltime, barrier is in remote obstacle-avoidance area, and the range information that underwater robot utilizes detection sonar to feed back carries out keeping away barrier, constructs the potential-energy function based on range information

$E_{\mathrm{obs}}^d=\frac{{\left(\right||q-O_m||-d_l-{\mathrm{\ρ}}_m)}^2}{\left|\right|q-O_m\left|\right|-{\mathrm{\ρ}}_m},$

Wherein time, underwater robot can move along barrier influence domain radius under the prerequisite that do not collide with barrier;

Work as d _l>=d>d _stime, barrier is in closely obstacle-avoidance area, and the parallax information that underwater robot utilizes infrared camera to feed back carries out keeping away barrier, structure parallax function as the terminal state controller of predictive control algorithm, the subtense angle of forward direction infrared camera square shaped barrier is θ ₁, the subtense angle of side direction infrared camera to irregular shape barrier is θ ₂, underwater robot movement direction angle is α, considers the direction of motion of underwater robot, corrects, the parallax that must make new advances to parallax with

Structure is similar to parallax function using as terminal state controller, wherein k _obs>0 is weighted value;

Fusion distance and parallax information construct keeps away barrier function, as the terminal state controller of predictive control algorithm; Wherein d _safe>=d>d _s.

3. according to claim 1ly to it is characterized in that: the optimized algorithm of described PREDICTIVE CONTROL based on the underwater robot barrier-avoiding method of distance with parallax information, at any sampling instant t _k, solve the finite time-domain open loop optimization problem relating to state constraint and control constraints and obtain one group of control sequence u ^*, show that the supposition in prediction time domain exports, and current control inputs acted on controlled device; At next sampling instant t _k+1, repeated the optimizing process in a upper moment based on new original state, continuous rolling optimization, thus form closed-loop control, complete collision-free trajectory and optimize task.