CN111522351B - 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
CN111522351B
CN111522351B CN202010412640.9A CN202010412640A CN111522351B CN 111522351 B CN111522351 B CN 111522351B CN 202010412640 A CN202010412640 A CN 202010412640A CN 111522351 B CN111522351 B CN 111522351B
Authority
CN
China
Prior art keywords
underwater robot
formation
obstacle
underwater
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.)
Active
Application number
CN202010412640.9A
Other languages
Chinese (zh)
Other versions
CN111522351A (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 the content of the first and second substances,
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 the content of the first and second substances,
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 is belonged to { 1., K }, and K represents the number of the obstacles in the navigation space of the underwater robot; i is a third-order identity 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 is repaired under the influence of the k-th obstacleObtaining a reference movement speed correction value by the reference movement speed of the underwater robot
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 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 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:
regardless of ocean flow field influence, 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 figure 2, a virtual structure method is adopted to define geometric formation, namely all underwater machinesThe robot team is regarded as a virtual rigid body, and the geometric center point of the robot team 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.
Thirdly, in order to describe the model more clearly, a virtual center point O is usedrThe 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 form and navigate along an expected track, the formation error of each robot is required to gradually approach 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, beta1、β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:
firstly, suppose that K barriers exist in a navigation space of the robot, taking the K-th barrier as an example, K belongs to { 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、ckRespectively the k-th obstacleK belongs to { 1., K }, and K represents the number of obstacles in the navigation space of the underwater robot; i is a third-order identity 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.
Secondly, calculating the 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
Thirdly, according to the robot reference state vector s in the 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 motion speed values
Figure BDA0002493846650000152
Sorting the priority sequences from large to small, and determining priority sequence numbers { 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 retained, andand only retaining the component of the reference motion speed correction value with the highest priority in the null space, and the like, namely fusing all the reference motion speed correction values. 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
Thirdly, according to 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:
firstly, each underwater robot tracks a reference state by using a nonlinear model predictive controller so as to obtain optimal control input. Assuming that the current time is t, the time domain length is N, and 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.
Secondly, the optimal control input sequence is optimized and calculated by adopting a gray wolf optimization method
Figure BDA0002493846650000172
Figure BDA0002493846650000173
Wherein
Figure BDA0002493846650000174
Inputting the optimal control at the current time t into the sequence
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 (3)

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 underwater robot motion to correct the reference motion velocity of each underwater robot:
according to the formula:
Figure FDA0002969010570000011
calculating the k-th barrier pair underwater robot RiInfluence of movement Mk；
Wherein the content of the first and second substances,
Figure FDA0002969010570000012
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 is belonged to { 1., K }, and K represents the number of the obstacles in the navigation space of the underwater robot; i is a third-order identity matrix,
Figure FDA0002969010570000013
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 FDA0002969010570000021
Figure FDA0002969010570000022
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 FDA0002969010570000023
Namely:
Figure FDA0002969010570000024
wherein the motion speed of the k-th obstacle is Uobs,k，
Figure FDA0002969010570000025
Is an exponential decay term;
(S4) according to the null-space strategy, considering the influence of all obstacles on the movement of the underwater robot, and obtaining the reference state vector of each underwater robot under the multi-obstacle environment:
consider all obstacles to underwater robot RiInfluence of movement, correcting all reference movement speed values
Figure FDA0002969010570000026
Sorting according to size, and determining priority sequence numbers { 1.., K };
according to the formula:
Figure FDA0002969010570000031
completely reserving reference movement speed correction value with highest priority based on null space strategy
Figure FDA0002969010570000032
Correcting the reference movement speed with the highest priority
Figure FDA0002969010570000033
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 FDA0002969010570000034
And compensating and determining the reference velocity vector of the underwater robot
Figure FDA0002969010570000035
Figure FDA0002969010570000036
According to underwater robot RiReference velocity vector of
Figure FDA0002969010570000037
Calculating respective reference states
Figure FDA0002969010570000038
Determining underwater robot R in multi-obstacle environmentiThe reference state vector of
Figure FDA0002969010570000039
(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:
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 FDA00029690105700000310
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 FDA0002969010570000041
solving for optimal control input sequences
Figure FDA0002969010570000042
Figure FDA0002969010570000043
According to the optimal control input sequence at the current time t
Figure FDA0002969010570000044
And updating the actual state of the underwater robot.
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 FDA0002969010570000045
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 FDA0002969010570000051
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 FDA0002969010570000052
derivation of the above formula yields:
Figure FDA0002969010570000053
determining water according to the desired geometric formationLower robot RiIn 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
constructing an underwater robot RiThe three-dimensional formation system mathematical model is as follows:
Figure FDA0002969010570000061
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 FDA0002969010570000062
derivation of the above equation yields:
Figure FDA0002969010570000063
wherein the content of the first and second substances,
Figure FDA0002969010570000064
β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。
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 CN111522351A (en) 2020-08-11
CN111522351B true 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)

Families Citing this family (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
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
CN113448338B (en) * 2021-05-25 2023-11-28 鹏城实验室 Robot control method, robot, computer program product, and storage medium
CN115629613B (en) * 2022-12-19 2023-04-04 北京卓翼智能科技有限公司 Formation fault-tolerant control method for multiple intelligent agents

Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109062229A (en) * 2018-08-03 2018-12-21 北京理工大学 The navigator of underwater robot system based on binocular vision follows formation method

Family Cites Families (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102042835B (en) * 2010-11-05 2012-10-24 中国海洋大学 Autonomous underwater vehicle combined navigation system
WO2013038544A1 (en) * 2011-09-15 2013-03-21 株式会社安川電機 Robotic system and robot controller
CN102722177B (en) * 2012-06-27 2014-06-25 哈尔滨工程大学 Autonomous underwater vehicle (AUV) three-dimensional straight path tracking control method with PID (Piping and Instruments Diagram) feedback gain
CN103576693B (en) * 2013-11-11 2016-06-29 哈尔滨工程大学 Underwater robot three-dimensional path tracking and controlling method based on second order filter
CN104918263B (en) * 2015-06-08 2018-08-21 浙江理工大学 A kind of mobile auxiliary network device and its network-building method based on underwater sound sensor net
CN108829113B (en) * 2018-09-01 2021-05-28 哈尔滨工程大学 Self-adaptive zero-space behavior fusion method for multi-robot formation
CN109375646A (en) * 2018-11-14 2019-02-22 江苏科技大学 AUV docking recycling autonomous navigation method based on FMSRUPF algorithm
CN110231821B (en) * 2019-06-03 2022-05-31 哈尔滨工程大学 Improved self-adaptive zero-space behavior fusion method for multi-robot formation
CN110333369B (en) * 2019-07-10 2021-05-11 哈尔滨工程大学 UUV DVL speed measurement system based on water surface GPS correction and self-adaptive denoising method

Patent Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109062229A (en) * 2018-08-03 2018-12-21 北京理工大学 The navigator of underwater robot system based on binocular vision follows formation method

Also Published As

Publication number Publication date
CN111522351A (en) 2020-08-11

Similar Documents

Publication Publication Date Title
CN111522351B (en) Three-dimensional formation and obstacle avoidance method for underwater robot
CN105807789B (en) UUV control methods based on the compensation of T-S Fuzzy Observers
CN112198901B (en) Unmanned aerial vehicle autonomous collision avoidance decision method based on three-dimensional dynamic collision area
CN112068440B (en) AUV recovery butt joint power positioning control method based on model prediction 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
CN108829134A (en) A kind of real-time automatic obstacle avoiding method of deepwater robot
CN113093804B (en) Unmanned ship formation control method and control system based on inversion sliding mode control
CN105929849A (en) Wheeled mobile robot target tracking control method based on point stabilization
CN111309002A (en) Wheel type mobile robot obstacle avoidance method and system based on vector
CN114940164B (en) Unmanned vehicle driving track optimization method and system for parking scene
CN112327892A (en) Anti-interference control method with AUV (autonomous Underwater vehicle) error limited
CN112327872A (en) Double unmanned ship cooperative track tracking method for oil spill containment
Zhao et al. Nonlinear model predictive control-based guidance algorithm for quadrotor trajectory tracking with obstacle avoidance
CN114047744A (en) Self-adaptive backstepping sliding mode multi-unmanned ship formation control method based on sampling communication
CN117389312A (en) Model-based three-dimensional tracking control method for counter roll of underwater vehicle
CN117055556A (en) Multi-robot formation path planning method and device, electronic equipment and storage medium
CN116714780A (en) Rotor flying mechanical arm and planning and control method for rapid aerial grabbing
CN114610047B (en) QMM-MPC underwater robot vision docking control method for online depth estimation
Hernández-Martínez et al. Trajectory tracking for groups of unicycles with convergence of the orientation angles
CN115902868A (en) Unmanned aerial vehicle high-precision confrontation target tracking method and system based on constrained particle filtering
Ding et al. Chat-PM: A class of composite hybrid aerial/terrestrial precise manipulator
Kanjanawanishkul Coordinated path following for mobile robots using a virtual structure strategy with model predictive control
Hernandez-Martinez et al. Marching control of unicycles based on the leader-followers scheme
Bae et al. A fuzzy compensated PID controller for formation control of 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