CN111522351A - Three-dimensional formation and obstacle avoidance method for underwater robot - Google Patents

Three-dimensional formation and obstacle avoidance method for underwater robot Download PDF

Info

Publication number
CN111522351A
CN111522351A CN202010412640.9A CN202010412640A CN111522351A CN 111522351 A CN111522351 A CN 111522351A CN 202010412640 A CN202010412640 A CN 202010412640A CN 111522351 A CN111522351 A CN 111522351A
Authority
CN
China
Prior art keywords
underwater robot
formation
underwater
obstacle
robot
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202010412640.9A
Other languages
Chinese (zh)
Other versions
CN111522351B (en
Inventor
姚鹏
魏欣
邱立艳
刘玉会
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Ocean University of China
Original Assignee
Ocean University of China
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Ocean University of China filed Critical Ocean University of China
Priority to CN202010412640.9A priority Critical patent/CN111522351B/en
Publication of CN111522351A publication Critical patent/CN111522351A/en
Application granted granted Critical
Publication of CN111522351B publication Critical patent/CN111522351B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/04—Control of altitude or depth
    • G05D1/06—Rate of change of altitude or depth
    • G05D1/0692—Rate of change of altitude or depth specially adapted for under-water vehicles

Landscapes

  • Engineering & Computer Science (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

The invention relates to a three-dimensional formation and obstacle avoidance method for underwater robots, which comprises the following steps: defining a geometric formation by adopting a virtual structure method, and constructing a three-dimensional formation system mathematical model of the underwater robot according to a reference track of the formation center; under the barrier-free environment, calculating the reference state vector of each underwater robot based on the Lyapunov stability principle; calculating the influence of a single obstacle on the movement of the underwater robots so as to correct the reference movement speed of each underwater robot; according to a zero-space strategy, considering the influence of all obstacles on the motion of the underwater robot, and obtaining reference state vectors of all underwater robots in a multi-obstacle environment; and solving the formation control problem by utilizing the nonlinear model predictive controller according to the respective reference state vector of each underwater robot to obtain the optimal control input. The method considers the influence of all obstacles on the motion of the underwater robot based on the corrected zero-space model prediction, simultaneously ensures the stable formation navigation and safe obstacle avoidance of the robot, and is suitable for complex environments.

Description

Three-dimensional formation and obstacle avoidance method for underwater robot
Technical Field
The invention belongs to the technical field of navigation guidance and control of underwater robots, and particularly relates to a three-dimensional formation and obstacle avoidance method of an underwater robot based on correction zero-space model predictive control.
Background
In recent years, as underwater robots are gradually applied to the fields of marine environment monitoring, submarine resource detection, search and rescue and the like, scholars at home and abroad make a great deal of research on how to improve the autonomous navigation operation capability and the environmental adaptability of the underwater robots. The formation control technology is a basic key technology for cooperatively executing specific operation tasks by underwater robots, and requires that a plurality of robots are controlled to form and maintain a certain geometric formation. Scholars at home and abroad propose a series of control structures aiming at the formation problem, mainly comprising a Changji-Liquan law, a behavior-based method and a virtual structure law, which are suitable for different environments and have advantages and disadvantages: the principle of the Changji-bureaucratic law is simple, but the problems of accumulative formation error, poor robustness and the like exist; the behavior-based method has strong environment adaptation capability, but has the defects of difficult system design and the like; the formation precision of the virtual structure method is high, but the flexibility is poor.
When the underwater robots are formed into a formation and sailing under a three-dimensional complex environment, the underwater robots often meet various sudden obstacles or threats, and the underwater robots are required to be capable of safely avoiding the obstacles and avoiding collision among the robots. The constraint greatly increases the complexity of the formation problem, and the model predictive control method can flexibly process various constraints and has high robustness, so that the combination of the model predictive control and the virtual structure method is an effective means. Since the model predictive controller can stably track the reference state vector, how to define a reasonable reference state vector according to the formation and obstacle avoidance requirements is the key point of the problem. The existing method is often only suitable for avoiding simple obstacles and cannot be applied to formation and obstacle avoidance tasks in a three-dimensional complex environment.
Therefore, there is a need to provide a method for three-dimensional formation and obstacle avoidance of underwater robots based on modified zero-space model predictive control, so as to ensure stable formation navigation and safe obstacle avoidance of the robots at the same time, and adapt to complex environments.
Disclosure of Invention
The invention provides a three-dimensional formation and obstacle avoidance method for underwater robots on the basis of the defects of a traditional underwater robot formation control method, which is based on correction zero-space model prediction control to simultaneously ensure stable formation navigation and safe obstacle avoidance of the robots and is suitable for complex environments.
In order to achieve the purpose, the invention provides a three-dimensional formation and obstacle avoidance method for underwater robots, which comprises the following steps:
(S1) constructing a three-dimensional formation system mathematical model of the underwater robot: defining a geometric formation by adopting a virtual structure method, and constructing a three-dimensional formation system mathematical model of the underwater robot according to a reference track of the formation center;
(S2) under the barrier-free environment, calculating the reference state vector of each underwater robot based on the Lyapunov stability principle;
(S3) calculating an influence of the single obstacle on the movement of the underwater robots to correct the reference movement speed of each underwater robot;
(S4) according to a zero-space strategy, considering the influence of all obstacles on the movement of the underwater robot, and obtaining reference state vectors of the underwater robots in a multi-obstacle environment;
(S5) each underwater robot utilizes the nonlinear model predictive controller to solve the formation control problem according to the respective reference state vector, and the optimal control input is obtained.
Preferably, in the step (S1), a virtual structure method is used to define the geometric formation, and a method for constructing a mathematical model of a three-dimensional formation system of the underwater robot according to a reference trajectory of the center of the formation is as follows:
not considering the influence of ocean flow field, and enabling the ith underwater robot RiThe particle motion model in the inertial coordinate system O-xyz is defined as:
Figure BDA0002493846650000031
wherein s isi＝[xi,yi,zi,vi,ψi,γi]TAnd ui＝[uxi,uyi,uzi]TRespectively representing the state and the control input vector of the ith underwater robot under an inertial coordinate system O-xyz, (x)i,yi,zi) As position coordinates of the robot, vi、ψi、γiRespectively robot velocity, heading angle, pitch angle uxi、uyi、uziRespectively representing the overload components of the underwater robot in the advancing direction, the heading direction and the trim direction; and satisfy ux,min≤uxi≤ux,max、uy,min≤uyi≤uy,max、uz,min≤uzi≤uz,max、zmin≤zi≤zmax、vmin≤vi≤vmax、γmin≤γi≤γmax；
All underwater robots are regarded as a virtual rigid body, a virtual structure legal geometric formation is adopted, and the geometric central point is OrThe expected motion trail of the robot formation is the formation center OrThe reference motion trajectory is defined as:
Figure BDA0002493846650000041
wherein, the ith underwater robot R under an inertial coordinate system O-xyziAnd a center point OrRespectively, are Pi＝[xi,yi,zi]TAnd Pr＝[xr,yr,zr]TVirtual center point OrThe desired velocity, azimuth angle and pitch angle are respectively vr、ψr、γr，ωrIs the turn angular rate;
with a virtual center point OrThe projection of the velocity vector of (a) in the horizontal plane is xrAxis, in a vertically upward direction, zrAxial direction, establishing a three-dimensional formation coordinate system Or-xryrzrThen the formation coordinate system Or-xryrzrLower ith underwater robot RiPosition P ofir＝[xir,yir,zir]TExpressed as:
Figure BDA0002493846650000042
derivation of formula (3) yields:
Figure BDA0002493846650000043
determining the underwater robot R according to the expected geometric formationiIn formation coordinate system Or-xryrzrDesired position P ofdir＝[xdir,ydir,zdir]TThen the queuing error is expressed as:
Pie＝[xie,yie,zie]T＝Pir-Pdir＝[xir-xdir,yir-ydir,zir-zdir]T(5)
constructing an underwater robot R according to formulas (1) to (5)iThe three-dimensional formation system mathematical model is as follows:
Figure BDA0002493846650000051
wherein, the ith underwater robot RiIn formation coordinate system Or-xryrzrThe state vector ofi＝[xie,yie,zie,vi,ψi,γi]T。
Preferably, in the step (S2), under the barrier-free environment, the method for calculating the reference state vector of each underwater robot based on the Lyapunov stability principle includes:
defining a Lyapunov distance function:
Figure BDA0002493846650000052
derivation of equation (7) yields:
Figure BDA0002493846650000053
wherein,
Figure BDA0002493846650000054
β1、β2、β3a constant coefficient greater than 0; v. ofi、ψi、γiRespectively as underwater robots RiDesired velocity v ofirHeading angle psiirLongitudinal inclination angle gammairThe expected formation error is 0, and the ith underwater robot R in the free environment is obtainediIn formation coordinate system Or-xryrzrThe reference state vector at is sir＝[0,0,0,vir,ψir,γir]T。
Preferably, the step (S3) of calculating the influence of the single obstacle on the motions of the underwater robots to correct the reference motion speeds of the respective underwater robots includes:
according to the formula:
Figure BDA0002493846650000061
calculating the k-th barrier pair underwater robot RiInfluence of movement Mk；
Wherein,
Figure BDA0002493846650000062
surface equation representing the kth obstacle, (x)k0,yk0,zk0) Denotes the center of the obstacle, dk、ek、fkAnd ak、bk、ckThe shape coefficient and the size coefficient of the obstacle are respectively, K ∈ { 1., K } represents the number of the obstacles in the navigation space of the underwater robot, I is a third-order unit matrix,
Figure BDA0002493846650000063
radial normal vector, p, representing the k-th obstaclekIs the barrier reaction coefficient and pk＞0，nk TVirIs less than or equal to 0 and represents the underwater robot RiApproaching the k-th obstacle, nk TVir> 0 denotes the underwater robot RiThe kth obstacle has been avoided;
calculating the weight value omega of each obstacle according to the distance between the underwater robot and the obstaclekAnd weight value omega is weightedkNormalization processing is carried out to obtain a normalized weight value
Figure BDA0002493846650000064
Figure BDA0002493846650000065
According to the underwater robot R in the free environmentiReference state vector sirCalculating the reference motion velocity V of the underwater robotirAnd correcting the reference movement speed of the underwater robot under the influence of the kth obstacle to obtain a reference movement speed correction value
Figure BDA0002493846650000071
Namely:
Figure BDA0002493846650000072
wherein the motion speed of the k-th obstacle is Uobs,k，
Figure BDA00024938466500000713
Is an exponential decay term.
Preferably, the step (S4) of obtaining the reference state vector of each underwater robot in the multi-obstacle environment by considering the influence of all obstacles on the movement of the underwater robot according to the null-space strategy includes:
consider all obstacles to underwater robot RiInfluence of movement, correcting all reference movement speed values
Figure BDA0002493846650000073
Sorting according to size, and determining priority sequence numbers { 1.., K };
according to the formula:
Figure BDA0002493846650000074
completely reserving reference movement speed correction value with highest priority based on null space strategy
Figure BDA0002493846650000075
Correcting the reference movement speed with the highest priority
Figure BDA0002493846650000076
Only the component of the reference motion velocity is kept in the null space, and the corrected value of each reference motion velocity is fused
Figure BDA0002493846650000077
And compensating and determining the reference velocity vector of the underwater robot
Figure BDA0002493846650000078
According to underwater robot RiReference velocity vector of
Figure BDA0002493846650000079
Calculating respective reference states
Figure BDA00024938466500000710
Determining underwater robot R in multi-obstacle environmentiThe reference state vector of
Figure BDA00024938466500000711
Preferably, the formation position error is determined according to each underwater robot
Figure BDA00024938466500000712
Determining the priority order of the underwater robots according to the size;
according to the priority coordination strategy, the influence of other underwater robots is not required to be considered for the underwater robot with the highest priority; for excellenceThe underwater robot with lower priority takes the underwater robot with higher priority as a virtual obstacle which can be encountered by the underwater robot, namely the underwater robot with higher priority has a radius dminAt a speed of movement of
Figure BDA0002493846650000081
In which dminRepresenting a safe distance between the underwater robots.
Preferably, in the step (S5), each underwater robot uses the nonlinear model predictive controller to solve the formation control problem according to the respective reference state vector, and the method for obtaining the optimal control input includes:
each underwater robot constructs an objective function according to the state vectors, the expected formation, the reference track and the environmental information of the underwater robot and other underwater robots at the current time t:
Figure BDA0002493846650000082
wherein the current moment is t, the time domain length is N, and the ith underwater robot RiThe optimal control input sequence, the predicted state vector sequence and the reference state vector sequence are respectively ui(t:t+N-1)＝{ui(t),…,ui(t+N-1)}、si(t+1:t+N)＝{si(t+1),…,si(t+N)}、sir(t+1:t+N)＝{sir(t+1),…,sir(t + N) }; A. b, C, respectively representing state cost, terminal state cost, and weighting matrix of control input cost;
optimally calculating an optimal control input sequence:
Figure BDA0002493846650000083
solving for optimal control input sequences
Figure BDA0002493846650000084
According to the optimal control input sequence at the current time t
Figure BDA0002493846650000091
And updating the actual state of the underwater robot.
Compared with the prior art, the invention has the advantages and positive effects that:
the invention provides a three-dimensional formation and obstacle avoidance method for underwater robots, which is based on correction zero space model prediction, combines a virtual structure method, calculates reference state vectors of all robots by constructing a mathematical model of a three-dimensional formation system of the underwater robots and adopting a Lyapunov stability theory, and realizes three-dimensional formation and track tracking; meanwhile, the influence of a single obstacle on the motion of the underwater robot is quantitatively calculated, and the reference motion speed of the robot is corrected, so that the safe obstacle avoidance can be realized without influencing the stability of a formation system; the method has the advantages that the influence of all barriers on the movement of the robot is comprehensively considered by adopting a zero-space strategy so as to meet the obstacle avoidance requirement, compared with the traditional weighted summation mode, the strategy is more reasonable and efficient, the stable formation navigation and safe obstacle avoidance of the robot can still be ensured simultaneously, and the method is suitable for the complex environment.
Drawings
FIG. 1 is a flow chart of a three-dimensional formation and obstacle avoidance method for underwater robots of the present invention;
FIG. 2 is a schematic diagram of three-dimensional formation of underwater robots based on a virtual structure method;
FIG. 3 is a schematic diagram of a null-space strategy in a two-dimensional space;
FIG. 4 shows the multi-robot three-dimensional formation and obstacle avoidance results;
wherein: fig. 4a shows a three-dimensional formation view of the underwater robot, fig. 4b shows a top view of the underwater robot, and fig. 4c shows a formation error curve.
Detailed Description
Hereinafter, embodiments of the present invention will be further described with reference to the accompanying drawings.
The underwater robots are often subjected to various sudden obstacles or threats when being formed and navigated in a three-dimensional complex environment, and the underwater robots are required to be capable of safely avoiding the obstacles and avoiding collision among the robots. The above constraints greatly increase the complexity of the formation problem, and the model predictive control method can flexibly process various constraints and has high robustness, so the invention considers the combination of the model predictive control and the virtual structure method. Since the model predictive controller can stably track the reference state vector, how to define a reasonable reference state vector according to the formation and obstacle avoidance requirements is the key point of the problem. Aiming at the formation requirement, the invention calculates the reference state vector based on the stability principle, the spatial position or the speed difference value and the like; on the basis, in order to meet the obstacle avoidance requirement, a zero-space strategy is adopted to comprehensively consider the influence of all obstacles on the movement of the robot.
Based on the analysis, the invention provides an underwater robot three-dimensional formation and obstacle avoidance method based on correction zero-space model predictive control. By constructing a three-dimensional formation model of the underwater robot based on a virtual structure and further determining a reference state vector of a model prediction controller by adopting a Lyapunov stability principle and a correction null-space strategy, the underwater robot can be guided to form a three-dimensional formation and accurately track an expected track, and simultaneously, obstacles are safely avoided. The method flow is shown in fig. 1, and specifically comprises the following steps:
(1) constructing a three-dimensional formation system mathematical model of the underwater robot: and defining a geometric formation by adopting a virtual structure method, and constructing a three-dimensional formation system mathematical model of the underwater robot according to a reference track of the center of the formation. The method specifically comprises the following steps:
① No consideration of ocean flow field effects, the ith underwater robot RiThe particle motion model in the inertial coordinate system O-xyz can be defined as:
Figure BDA0002493846650000111
wherein s isi＝[xi,yi,zi,vi,ψi,γi]TAnd ui＝[uxi,uyi,uzi]TRespectively representing the underwater robot state and control inputs. (x)i,yi,zi) Is the position of the robot, vi、ψi、γiRespectively the speed, heading angle and trim angle u of the robotxi、uyi、uziRespectively representing the overload components of the underwater robot in the advancing direction, the heading direction and the trim direction; in addition, the state quantity and the control quantity of the underwater robot need to meet the following motion constraint condition ux,min≤uxi≤ux,max，uy,min≤uyi≤uy,max，uz,min≤uzi≤uz,max，zmin≤zi≤zmax，vmin≤vi≤vmax，γmin≤γi≤γmax。
② As shown in FIG. 2, the virtual structure method is used to define the geometric formation, i.e. all underwater robots are treated as a virtual rigid body, and the geometric center point is OrTherefore, the expected motion track of the robot formation is the formation center OrThe reference motion trajectory of (2). No. i underwater robot R under inertial coordinate system O-xyziAnd a center point OrRespectively, are Pi＝[xi,yi,zi]TAnd Pr＝[xr,yr,zr]TVirtual center point OrThe desired velocity, azimuth angle and pitch angle are respectively vr、ψr、γrThen, the reference motion trajectory may be defined as:
Figure BDA0002493846650000112
wherein, ω isrIs the turn angular rate.
③ for more clear description of the model, with a virtual center point OrThe projection of the velocity vector of (a) in the horizontal plane is xrAxis, in a vertically upward direction, zrAxial direction, establishing a three-dimensional formation coordinate system Or-xryrzrTo obtain a formation coordinate system Or-xryrzrLower robot RiPosition P ofir＝[xir,yir,zir]TComprises the following steps:
Figure BDA0002493846650000121
derivation of the above equation (3) yields:
Figure BDA0002493846650000122
determining the robot R according to the desired geometric formationiIn a three-dimensional formation coordinate system Or-xryrzrDesired position P ofdir＝[xdir,ydir,zdir]T；
Then find the alignment error Pie＝[xie,yie,zie]T＝Pir-Pdir＝[xir-xdir,yir-ydir,zir-zdir]T；
By formation error PieEquation (4) may be redefined as:
Figure BDA0002493846650000123
then the ith underwater robot R is constructediThe three-dimensional formation system mathematical model is as follows:
Figure BDA0002493846650000124
wherein, the ith underwater robot RiIn formation coordinate system Or-xryrzrThe state vector ofi＝[xie,yie,zie,vi,ψi,γi]T。
(2) And under the barrier-free environment, calculating the reference state vector of each underwater robot based on the Lyapunov stability principle. The method specifically comprises the following steps:
in order to make the robots form a formation and navigate along a desired trajectorySolving the gradual trend of the formation error of each robot to 0, namely xie→0,yie→0,zie→ 0, so the Lyapunov distance function can be defined as:
Figure BDA0002493846650000131
derived from formula (7)
Figure BDA0002493846650000132
To realize
Figure BDA0002493846650000133
Can define
Figure BDA0002493846650000134
Wherein, β1、β2、β3A constant coefficient greater than 0; v. ofi、ψi、γiRespectively as the ith underwater robot RiDesired velocity v ofirHeading angle psiirLongitudinal inclination angle gammairAnd obtaining the ith underwater robot R in the free environment because the expected formation errors of the underwater robots are all 0iIn formation coordinate system Or-xryrzrThe reference state vector at is sir＝[0,0,0,vir,ψir,γir]T。
(3) And calculating the influence of the single obstacle on the movement of the underwater robots so as to correct the reference movement speed of each underwater robot. The method specifically comprises the following steps:
① assume that there are K obstacles in the navigation space of the robot, and the K-th obstacle is taken as an example, K ∈ { 1., K }, and the surface equation is
Figure BDA0002493846650000135
Wherein (x)k0,yk0,zk0) Denotes the center of the obstacle, dk,ek,fkAnd ak,bk,ckBarrier shape factor and size factor, respectively.
Kth obstacle to underwater robot RiThe effects of the movement are:
Figure BDA0002493846650000141
wherein d isk、ek、fkAnd ak、bk、ckThe shape coefficient and the size coefficient of the kth barrier are respectively, K ∈ { 1., K }, wherein K represents the number of barriers in the navigation space of the underwater robot, I is a third-order unit matrix,
Figure BDA0002493846650000142
radial normal vector, p, representing the k-th obstaclekIs the barrier reaction coefficient and pk＞0，nk TVirIs less than or equal to 0 and represents the underwater robot RiApproaching the k-th obstacle, nk TVir> 0 denotes the underwater robot RiThe k-th obstacle has been avoided.
② calculating weight value omega of each obstacle according to the distance between the robot and the obstaclek：
Figure BDA0002493846650000143
Due to the fact that
Figure BDA0002493846650000144
Therefore, the weight value needs to be normalized to obtain a normalized weight value
Figure BDA0002493846650000145
③ reference state vector s according to robot in free environmentirCalculable underwater robot RiReference movement velocity V ofir＝[vircosγircosψir,vircosγirsinψir,virsinγir]TAnd correcting the reference movement speed under the influence of the k-th obstacle to obtain a reference movement speed correction value
Figure BDA0002493846650000146
Namely:
Figure BDA0002493846650000147
wherein the motion speed of the k-th obstacle is Uobs,kExponential decay term
Figure BDA0002493846650000148
To appropriately reduce the influence of a distant obstacle.
(4) And according to a zero-space strategy, considering the influence of all obstacles on the motion of the underwater robot, and obtaining the reference state vector of each underwater robot in the multi-obstacle environment. The method specifically comprises the following steps:
because the shape, the size, the distance and the like of each obstacle are different from each other, the obstacles have different influences on the motion of the underwater robot, and the corrected reference motion speed
Figure BDA0002493846650000151
Are different from each other and may even contradict each other. For example, to avoid an obstacle, the robot needs to move to the left, and to avoid another obstacle, the robot may need to move to the right. The traditional speed summing method can not cope with the problems, so the invention adopts a zero-space strategy to fuse each corrected reference motion speed: the priority of each reference motion velocity is determined first, then the higher priority reference motion velocity is retained, while for the lower priority reference motion velocity only its component in the null space (i.e. perpendicular to the higher priority velocity vector) is retained, while the other components are ignored. The method specifically comprises the following steps:
① correcting all reference movement speed
Figure BDA0002493846650000152
According to the size from large to smallLine sorting, determining their priority sequence number { 1., K };
fusing all reference motion speed correction values according to a null space strategy:
Figure BDA0002493846650000153
according to the above formula, the reference movement speed correction value with the highest priority is completely reserved, and only the component of the reference movement speed correction value with the second highest priority in the null space is reserved for the reference movement speed correction value with the second highest priority, and so on, so that all the reference movement speed correction values can be fused. FIG. 3 shows a schematic diagram of a null-space strategy in two-dimensional space, where
Figure BDA0002493846650000161
Has a higher priority than
Figure BDA0002493846650000162
Is shown and
Figure BDA0002493846650000163
vertical
Figure BDA0002493846650000164
And (4) components. In addition, since the partial reference motion velocity correction value component is ignored, the rate of fusion
Figure BDA0002493846650000165
Yet further compensation is required, i.e.
Figure BDA0002493846650000166
③ based on the corrected reference velocity vector
Figure BDA0002493846650000167
Can calculate each reference state such as
Figure BDA0002493846650000168
Therefore, the underwater robot R under the multi-obstacle environmentiThe reference state vector of
Figure BDA0002493846650000169
And each underwater robot calculates the respective reference state vector according to the steps, but other underwater robots are also required to be used as virtual obstacles so as to avoid collision among the robots. Specifically, first, the formation position error of each robot is determined
Figure BDA00024938466500001610
Determining a priority order, wherein the smaller the error is, the higher the priority of the robot is; then, according to the priority coordination strategy, the robot with the highest priority does not need to consider the influence of other robots, and for the robot with lower priority, the robot with higher priority needs to be used as a virtual obstacle which the robot with higher priority may encounter, namely, the robot with the radius dminAt a speed of movement of
Figure BDA00024938466500001611
In which dminRepresenting a safe distance between the underwater robots.
(5) And solving the formation control problem by utilizing the nonlinear model predictive controller according to the respective reference state vector of each underwater robot to obtain the optimal control input. The method specifically comprises the following steps:
① Each Underwater robot tracks the reference state using a nonlinear model predictive controller to obtain the optimal control input assuming that the current time is t, the time domain length is N, the robot RiThe optimal control input sequence, the predicted state vector sequence and the reference state vector sequence are respectively ui(t:t+N-1)＝{ui(t),…,ui(t+N-1)}、si(t+1:t+N)＝{si(t+1),…,si(t+N)}、sir(t+1:t+N)＝{sir(t+1),…,sir(t+N)}。
Then the underwater robot RiConstructing an objective function according to state vectors, expected formation, reference tracks, environmental information and the like of the robot and other robots:
Figure BDA0002493846650000171
a, B, C represents weighting matrix of state cost, terminal state cost and control input cost, which are set in advance.
② optimal control input sequence is optimized and calculated by Grey wolf optimization method
Figure BDA0002493846650000172
Figure BDA0002493846650000173
Wherein
Figure BDA0002493846650000174
③ input the optimal control sequence at the current time t
Figure BDA0002493846650000175
The method is used for navigation of the robot and updating the actual state of the underwater robot. And at the moment of t +1, recalculating the optimal control input sequence and updating the state value, and so on to realize the rolling optimization.
Fig. 4 shows three-dimensional formation and obstacle avoidance results of the underwater robot in a complex environment, fig. 4a shows a three-dimensional formation view of the underwater robot, fig. 4b shows a top view of the underwater robot, and fig. 4c shows a formation error curve. The 5 underwater robots R1-R5 can form a triangular formation rapidly, track the expected track FGT and avoid various static or dynamic obstacles and other robots safely. Although each robot changes the expected formation shape due to obstacle avoidance and increases the formation error, the robot quickly returns to the expected position after avoiding the obstacle and the formation error approaches to 0 again.
Therefore, in summary, the underwater robot three-dimensional formation and obstacle avoidance method provided by the invention is based on the prediction of a modified zero-space model, based on a virtual model structure, through constructing a mathematical model of an underwater robot three-dimensional formation system, and calculating the reference state vector of each robot by adopting a Lyapunov stability theory, so that three-dimensional formation and track tracking are realized; meanwhile, the influence of a single obstacle on the motion of the underwater robot is quantitatively calculated, and the reference motion speed of the robot is corrected, so that the safe obstacle avoidance can be realized without influencing the stability of a formation system; the method has the advantages that the influence of all barriers on the movement of the robot is comprehensively considered by adopting a zero-space strategy so as to meet the obstacle avoidance requirement, compared with the traditional weighted summation mode, the strategy is more reasonable and efficient, the stable formation navigation and safe obstacle avoidance of the robot can still be ensured simultaneously, and the method is suitable for the complex environment.
The above description is only a preferred embodiment of the present invention, and not intended to limit the present invention in other forms, and any person skilled in the art may apply the above modifications or changes to the equivalent embodiments with equivalent changes, without departing from the technical spirit of the present invention, and any simple modification, equivalent change and change made to the above embodiments according to the technical spirit of the present invention still belong to the protection scope of the technical spirit of the present invention.

Claims (7)

1. A three-dimensional formation and obstacle avoidance method for underwater robots is characterized by comprising the following steps:
(S1) constructing a three-dimensional formation system mathematical model of the underwater robot: defining a geometric formation by adopting a virtual structure method, and constructing a three-dimensional formation system mathematical model of the underwater robot according to a reference track of the formation center;
(S2) under the barrier-free environment, calculating the reference state vector of each underwater robot based on the Lyapunov stability principle;
(S3) calculating an influence of the single obstacle on the movement of the underwater robots to correct the reference movement speed of each underwater robot;
(S4) according to a zero-space strategy, considering the influence of all obstacles on the movement of the underwater robot, and obtaining reference state vectors of the underwater robots in a multi-obstacle environment;
(S5) each underwater robot utilizes the nonlinear model predictive controller to solve the formation control problem according to the respective reference state vector, and the optimal control input is obtained.
2. The underwater robot three-dimensional formation and obstacle avoidance method according to claim 1, wherein the step (S1) of defining a geometric formation by using a virtual structure method, and constructing a mathematical model of a three-dimensional formation system of the underwater robot based on a reference trajectory of a center of the formation comprises:
not considering the influence of ocean flow field, and enabling the ith underwater robot RiThe particle motion model in the inertial coordinate system O-xyz is defined as:
Figure FDA0002493846640000021
wherein s isi＝[xi,yi,zi,vi,ψi,γi]TAnd ui＝[uxi,uyi,uzi]TRespectively representing the state and the control input vector of the ith underwater robot under an inertial coordinate system O-xyz, (x)i,yi,zi) As position coordinates of the robot, vi、ψi、γiRespectively robot velocity, heading angle, pitch angle uxi、uyi、uziRespectively representing the overload components of the underwater robot in the advancing direction, the heading direction and the trim direction; and satisfy ux,min≤uxi≤ux,max、uy,min≤uyi≤uy,max、uz,min≤uzi≤uz,max、zmin≤zi≤zmax、vmin≤vi≤vmax、γmin≤γi≤γmax；
All underwater robots are regarded as a virtual rigid body, a virtual structure legal geometric formation is adopted, and the geometric central point is OrThe expected motion trail of the robot formation is the formation center OrThe reference motion trajectory is defined as:
Figure FDA0002493846640000022
wherein, the ith underwater robot R under an inertial coordinate system O-xyziAnd a center point OrRespectively, are Pi＝[xi,yi,zi]TAnd Pr＝[xr,yr,zr]TVirtual center point OrThe desired velocity, azimuth angle and pitch angle are respectively vr、ψr、γr，ωrIs the turn angular rate;
with a virtual center point OrThe projection of the velocity vector of (a) in the horizontal plane is xrAxis, in a vertically upward direction, zrAxial direction, establishing a three-dimensional formation coordinate system Or-xryrzrThen the formation coordinate system Or-xryrzrLower ith underwater robot RiPosition P ofir＝[xir,yir,zir]TExpressed as:
Figure FDA0002493846640000031
derivation of formula (3) yields:
Figure FDA0002493846640000032
determining the underwater robot R according to the expected geometric formationiIn formation coordinate system Or-xryrzrDesired position P ofdir＝[xdir,ydir,zdir]TThen the queuing error is expressed as:
Pie＝[xie,yie,zie]T＝Pir-Pdir＝[xir-xdir,yir-ydir,zir-zdir]T(5)
constructing an underwater robot R according to formulas (1) to (5)iThe three-dimensional formation system mathematical model is as follows:
Figure FDA0002493846640000033
wherein, the ith underwater robot RiIn formation coordinate system Or-xryrzrThe state vector ofi＝[xie,yie,zie,vi,ψi,γi]T。
3. The underwater robot three-dimensional formation and obstacle avoidance method according to claim 2, wherein in the step (S2), in an obstacle-free environment, the method for calculating the reference state vector of each underwater robot based on the Lyapunov stability principle comprises:
defining a Lyapunov distance function:
Figure FDA0002493846640000041
derivation of equation (7) yields:
Figure FDA0002493846640000042
wherein,
Figure FDA0002493846640000043
β1、β2、β3a constant coefficient greater than 0; v. ofi、ψi、γiRespectively as underwater robots RiDesired velocity v ofirHeading angle psiirLongitudinal inclination angle gammairThe expected formation error is 0, and the ith underwater robot R in the free environment is obtainediIn formation coordinate system Or-xryrzrThe reference state vector at is sir＝[0,0,0,vir,ψir,γir]T。
4. The underwater robot three-dimensional formation and obstacle avoidance method according to claim 3, wherein the step (S3) of calculating the influence of a single obstacle on the movement of the underwater robots to correct the reference movement speed of each underwater robot comprises:
according to the formula:
Figure FDA0002493846640000044
calculating the k-th barrier pair underwater robot RiInfluence of movement Mk；
Wherein,
Figure FDA0002493846640000045
surface equation representing the kth obstacle, (x)k0,yk0,zk0) Denotes the center of the obstacle, dk、ek、fkAnd ak、bk、ckThe shape coefficient and the size coefficient of the obstacle are respectively, K ∈ { 1., K } represents the number of the obstacles in the navigation space of the underwater robot, I is a third-order unit matrix,
Figure FDA0002493846640000051
radial normal vector, p, representing the k-th obstaclekIs the barrier reaction coefficient and pk＞0，nk TVirIs less than or equal to 0 and represents the underwater robot RiApproaching the k-th obstacle, nk TVir> 0 denotes the underwater robot RiThe kth obstacle has been avoided;
calculating the weight value omega of each obstacle according to the distance between the underwater robot and the obstaclekAnd weight value omega is weightedkNormalization processing is carried out to obtain a normalized weight value
Figure FDA0002493846640000052
Figure FDA0002493846640000053
According to the underwater robot R in the free environmentiReference state vector sirCalculating the reference motion velocity V of the underwater robotirAnd correcting the reference movement speed of the underwater robot under the influence of the kth obstacle to obtain a reference movement speed correction value
Figure FDA0002493846640000054
Namely:
Figure FDA0002493846640000055
wherein the motion speed of the k-th obstacle is Uobs,k，
Figure FDA0002493846640000056
Is an exponential decay term.
5. The underwater robot three-dimensional formation and obstacle avoidance method according to claim 4, wherein the step (S4) of obtaining the reference state vector of each underwater robot in the multi-obstacle environment in consideration of the influence of all obstacles on the movement of the underwater robot according to a null-space strategy is as follows:
consider all obstacles to underwater robot RiInfluence of movement, correcting all reference movement speed values
Figure FDA0002493846640000061
Sorting according to size, and determining priority sequence numbers { 1.., K };
according to the formula:
Figure FDA0002493846640000062
based on the null space strategy, the reference operation with the highest priority is completely reservedCorrection value of dynamic velocity
Figure FDA0002493846640000063
Correcting the reference movement speed with the highest priority
Figure FDA0002493846640000064
Only the component of the reference motion velocity is kept in the null space, and the corrected value of each reference motion velocity is fused
Figure FDA0002493846640000065
And compensating and determining the reference velocity vector of the underwater robot
Figure FDA0002493846640000066
Figure FDA0002493846640000067
According to underwater robot RiReference velocity vector of
Figure FDA0002493846640000068
Calculating respective reference states
Figure FDA0002493846640000069
Determining underwater robot R in multi-obstacle environmentiThe reference state vector of
Figure FDA00024938466400000610
6. The underwater robot three-dimensional formation and obstacle avoidance method according to claim 5, wherein the formation position error of each underwater robot is determined according to
Figure FDA00024938466400000611
Determining the priority order of the underwater robots according to the size;
according to the priority coordination strategy, for the underwater vehicle with the highest priorityThe robot does not need to consider the influence of other underwater robots; for the underwater robot with lower priority, the underwater robot with higher priority is taken as a virtual obstacle which can be encountered by the underwater robot, namely the underwater robot with higher priority has a radius dminAt a speed of movement of
Figure FDA00024938466400000612
In which dminRepresenting a safe distance between the underwater robots.
7. The underwater robot three-dimensional formation and obstacle avoidance method according to claim 5 or 6, wherein in the step (S5), each underwater robot performs formation control problem solving by using a nonlinear model predictive controller according to a respective reference state vector, and the method for obtaining the optimal control input comprises:
each underwater robot constructs an objective function according to the state vectors, the expected formation, the reference track and the environmental information of the underwater robot and other underwater robots at the current time t:
Figure FDA0002493846640000071
wherein the current moment is t, the time domain length is N, and the ith underwater robot RiThe optimal control input sequence, the predicted state vector sequence and the reference state vector sequence are respectively ui(t:t+N-1)＝{ui(t),…,ui(t+N-1)}、si(t+1:t+N)＝{si(t+1),…,si(t+N)}、sir(t+1:t+N)＝{sir(t+1),…,sir(t + N) }; A. b, C, respectively representing state cost, terminal state cost, and weighting matrix of control input cost;
optimally calculating an optimal control input sequence:
Figure FDA0002493846640000072
solving for optimal control input sequences
Figure FDA0002493846640000073
Figure FDA0002493846640000074
According to the optimal control input sequence at the current time t
Figure FDA0002493846640000075
And updating the actual state of the underwater robot.
CN202010412640.9A 2020-05-15 2020-05-15 Three-dimensional formation and obstacle avoidance method for underwater robot Active CN111522351B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010412640.9A CN111522351B (en) 2020-05-15 2020-05-15 Three-dimensional formation and obstacle avoidance method for underwater robot

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010412640.9A CN111522351B (en) 2020-05-15 2020-05-15 Three-dimensional formation and obstacle avoidance method for underwater robot

Publications (2)

Publication Number Publication Date
CN111522351A true CN111522351A (en) 2020-08-11
CN111522351B CN111522351B (en) 2021-05-18

Family

ID=71907374

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010412640.9A Active CN111522351B (en) 2020-05-15 2020-05-15 Three-dimensional formation and obstacle avoidance method for underwater robot

Country Status (1)

Country Link
CN (1) CN111522351B (en)

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112558483A (en) * 2020-12-29 2021-03-26 北京电子工程总体研究所 Formation control method and system based on model predictive control
CN113009924A (en) * 2021-03-08 2021-06-22 哈尔滨工程大学 Obstacle avoidance method of underwater unmanned aerial vehicle based on non-quadratic inverse optimal control
CN113190037A (en) * 2021-04-08 2021-07-30 上海吞山智能科技有限公司 Unmanned aerial vehicle optimal path searching method based on improved fluid disturbance and sparrow algorithm
CN113448338A (en) * 2021-05-25 2021-09-28 鹏城实验室 Robot control method, robot, computer program product, and storage medium
CN115629613A (en) * 2022-12-19 2023-01-20 北京卓翼智能科技有限公司 Formation fault-tolerant control method for multiple intelligent agents

Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102042835A (en) * 2010-11-05 2011-05-04 中国海洋大学 Autonomous underwater vehicle combined navigation system
CN102722177A (en) * 2012-06-27 2012-10-10 哈尔滨工程大学 Autonomous underwater vehicle (AUV) three-dimensional straight path tracking control method with PID (Piping and Instruments Diagram) feedback gain
CN103576693A (en) * 2013-11-11 2014-02-12 哈尔滨工程大学 Underwater robot three-dimensional path tracking control method based on second-order filter
CN103781602A (en) * 2011-09-15 2014-05-07 株式会社安川电机 Robotic system and robot controller
CN104918263A (en) * 2015-06-08 2015-09-16 浙江理工大学 Mobile auxiliary networking device based on underwater acoustic sensor network, and networking method thereof
CN108829113A (en) * 2018-09-01 2018-11-16 哈尔滨工程大学 A kind of adaptive kernel action amalgamation method of multi-robot formation
CN109062229A (en) * 2018-08-03 2018-12-21 北京理工大学 The navigator of underwater robot system based on binocular vision follows formation method
CN109375646A (en) * 2018-11-14 2019-02-22 江苏科技大学 AUV docking recycling autonomous navigation method based on FMSRUPF algorithm
CN110231821A (en) * 2019-06-03 2019-09-13 哈尔滨工程大学 The adaptive kernel action amalgamation method of the improvement of multi-robot formation
CN110333369A (en) * 2019-07-10 2019-10-15 哈尔滨工程大学 The DVL velocity-measuring system and self-adaptive solution method of UUV based on water surface GPS correction

Patent Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102042835A (en) * 2010-11-05 2011-05-04 中国海洋大学 Autonomous underwater vehicle combined navigation system
CN103781602A (en) * 2011-09-15 2014-05-07 株式会社安川电机 Robotic system and robot controller
CN102722177A (en) * 2012-06-27 2012-10-10 哈尔滨工程大学 Autonomous underwater vehicle (AUV) three-dimensional straight path tracking control method with PID (Piping and Instruments Diagram) feedback gain
CN103576693A (en) * 2013-11-11 2014-02-12 哈尔滨工程大学 Underwater robot three-dimensional path tracking control method based on second-order filter
CN104918263A (en) * 2015-06-08 2015-09-16 浙江理工大学 Mobile auxiliary networking device based on underwater acoustic sensor network, and networking method thereof
CN109062229A (en) * 2018-08-03 2018-12-21 北京理工大学 The navigator of underwater robot system based on binocular vision follows formation method
CN108829113A (en) * 2018-09-01 2018-11-16 哈尔滨工程大学 A kind of adaptive kernel action amalgamation method of multi-robot formation
CN109375646A (en) * 2018-11-14 2019-02-22 江苏科技大学 AUV docking recycling autonomous navigation method based on FMSRUPF algorithm
CN110231821A (en) * 2019-06-03 2019-09-13 哈尔滨工程大学 The adaptive kernel action amalgamation method of the improvement of multi-robot formation
CN110333369A (en) * 2019-07-10 2019-10-15 哈尔滨工程大学 The DVL velocity-measuring system and self-adaptive solution method of UUV based on water surface GPS correction

Non-Patent Citations (5)

* Cited by examiner, † Cited by third party
Title
FILIPPO ARRICHIELLO等: "Formation Control of Underactuated Surface Vessels using the Null-Space-Based Behavioral Control", 《INTERNATIONAL CONFERENCE ON INTELLIGENT ROBOTS AND SYSTEMS》 *
PENG YAO等: "Three-Dimensional Path Planning for AUV Based on Interfered Fluid Dynamical System Under Ocean Current", 《IEEE ACCESS》 *
朱大奇等: "基于生物启发模型的AUV三维自主路径规划与安全避障算法", 《控制与决策》 *
潘无为等: "人工势场和虚拟结构相结合的多水下机器人编队控制", 《兵工学报》 *
牟春晖: "面向空间曲线路径跟踪的多UUV协调编队控制", 《 中国博士学位论文电子期刊网 工程科技Ⅱ辑》 *

Cited By (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112558483A (en) * 2020-12-29 2021-03-26 北京电子工程总体研究所 Formation control method and system based on model predictive control
CN112558483B (en) * 2020-12-29 2023-05-16 北京电子工程总体研究所 Formation control method and system based on model predictive control
CN113009924A (en) * 2021-03-08 2021-06-22 哈尔滨工程大学 Obstacle avoidance method of underwater unmanned aerial vehicle based on non-quadratic inverse optimal control
CN113190037A (en) * 2021-04-08 2021-07-30 上海吞山智能科技有限公司 Unmanned aerial vehicle optimal path searching method based on improved fluid disturbance and sparrow algorithm
CN113448338A (en) * 2021-05-25 2021-09-28 鹏城实验室 Robot control method, robot, computer program product, and storage medium
CN113448338B (en) * 2021-05-25 2023-11-28 鹏城实验室 Robot control method, robot, computer program product, and storage medium
CN115629613A (en) * 2022-12-19 2023-01-20 北京卓翼智能科技有限公司 Formation fault-tolerant control method for multiple intelligent agents

Also Published As

Publication number Publication date
CN111522351B (en) 2021-05-18

Similar Documents

Publication Publication Date Title
CN111522351B (en) Three-dimensional formation and obstacle avoidance method for underwater robot
CN111413966B (en) Progressive model prediction unmanned planning tracking cooperative control method
Zheng et al. Planning and tracking in image space for image-based visual servoing of a quadrotor
CN110262492B (en) Real-time collision avoidance and target tracking method for unmanned ship
CN112198901B (en) Unmanned aerial vehicle autonomous collision avoidance decision method based on three-dimensional dynamic collision area
CN113093804B (en) Unmanned ship formation control method and control system based on inversion sliding mode control
CN110561420B (en) Arm profile constraint flexible robot track planning method and device
Shentu et al. Motion control and trajectory planning for obstacle avoidance of the mobile parallel robot driven by three tracked vehicles
CN112327872A (en) Double unmanned ship cooperative track tracking method for oil spill containment
CN111309002A (en) Wheel type mobile robot obstacle avoidance method and system based on vector
CN112327892A (en) Anti-interference control method with AUV (autonomous Underwater vehicle) error limited
CN113721639B (en) Path planning method and control method for docking of small underwater robot
Ollero et al. Integrated mechanical design and modelling of a new mobile robot
Ding et al. Chat-PM: A class of composite hybrid aerial/terrestrial precise manipulator
CN114940164B (en) Unmanned vehicle driving track optimization method and system for parking scene
CN116714780A (en) Rotor flying mechanical arm and planning and control method for rapid aerial grabbing
CN114291117B (en) Four-wheel steering unmanned vehicle track tracking control method
Kanjanawanishkul Coordinated path following for mobile robots using a virtual structure strategy with model predictive control
McNinch et al. Application of a coordinated trajectory planning and real-time obstacle avoidance algorithm
Hernandez-Martinez et al. Marching control of unicycles based on the leader-followers scheme
Thanh An effective trajectory determination for collision avoidance of unmanned vehicles
Bae et al. A fuzzy compensated PID controller for formation control of mobile robots
Sánchez-Orta et al. Aerial following of a non-holonomic mobile robot subject to velocity fields: A case study for autonomous vehicles surveillance
Tziortziotis et al. Motion planning with energy reduction for a floating robotic platform under disturbances and measurement noise using reinforcement learning
Sebastian et al. Gaussian Kernel Controller for Path Tracking in Mobile Robots

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant