CN111522351B - Three-dimensional formation and obstacle avoidance method of underwater robot - Google Patents

Abstract

Translated fromChinese

本发明涉及一种水下机器人三维编队及避障方法，包括：采用虚拟结构法定义几何队形，并根据队形中心参考轨迹，构建水下机器人的三维编队系统数学模型；无障碍自由环境下，基于Lyapunov稳定性原理计算各水下机器人的参考状态向量；计算单个障碍对水下机器人运动的影响，以修正各水下机器人的参考运动速度；根据零空间策略，考虑所有障碍物对水下机器人运动的影响，获得多障碍环境下的各水下机器人参考状态向量；各水下机器人根据各自的参考状态向量，利用非线性模型预测控制器进行编队控制问题求解，获得最优控制输入。本发明基于修正零空间模型预测考虑所有障碍物对水下机器人运动的影响，同时保证机器人稳定编队航行与安全避障，适应于复杂环境。

The invention relates to a three-dimensional formation and obstacle avoidance method of an underwater robot. , calculate the reference state vector of each underwater robot based on the Lyapunov stability principle; calculate the influence of a single obstacle on the movement of the underwater robot to correct the reference movement speed of each underwater robot; according to the zero space strategy, consider the impact of all obstacles on the underwater robot According to the influence of robot motion, the reference state vector of each underwater robot in the multi-obstacle environment is obtained; each underwater robot uses the nonlinear model predictive controller to solve the formation control problem according to its own reference state vector, and obtains the optimal control input. The invention considers the influence of all obstacles on the motion of the underwater robot based on the correction of the zero-space model prediction, and at the same time ensures the stable formation navigation and safe obstacle avoidance of the robot, and is suitable for complex environments.

Description

Translated fromChinese

水下机器人三维编队及避障方法Three-dimensional formation and obstacle avoidance method of underwater robot

技术领域technical field

本发明属于水下机器人导航制导与控制技术领域，尤其涉及一种基于修正零空间模型预测控制的水下机器人三维编队及避障方法。The invention belongs to the technical field of navigation, guidance and control of underwater robots, and in particular relates to a three-dimensional formation and obstacle avoidance method of an underwater robot based on a modified zero-space model predictive control.

背景技术Background technique

近些年来，随着水下机器人逐渐在海洋环境监控、海底资源探测、搜索救援等领域展开应用，国内外学者针对如何提升水下机器人的自主航行作业能力与环境适应能力进行了大量研究。其中，编队控制技术是水下机器人协同执行具体作业任务的基础关键技术，它要求控制多艘机器人组成并保持一定的几何队形。国内外学者针对编队问题提出了一系列控制结构，主要包括长机-僚机法、基于行为的方法、以及虚拟结构法，它们适用于不同环境且各有优缺点：长机-僚机法原理简单，但存在编队误差累计、鲁棒性差等问题；基于行为的方法适应环境的能力强，但存在系统设计困难等缺陷；虚拟结构法的编队精度高，但灵活性差。In recent years, with the gradual application of underwater robots in the fields of marine environment monitoring, seabed resource detection, search and rescue, etc., domestic and foreign scholars have conducted a lot of research on how to improve the autonomous navigation and environmental adaptability of underwater robots. Among them, formation control technology is the basic key technology for underwater robots to cooperate to perform specific tasks. It requires controlling multiple robots to form and maintain a certain geometric formation. Scholars at home and abroad have proposed a series of control structures for the formation problem, mainly including the lead-wingman method, the behavior-based method, and the virtual structure method, which are suitable for different environments and have their own advantages and disadvantages: the lead-wingman method is simple in principle, However, there are problems such as accumulation of formation errors and poor robustness; the behavior-based method has a strong ability to adapt to the environment, but has defects such as difficulty in system design; the virtual structure method has high formation accuracy but poor flexibility.

水下机器人在三维复杂环境下编队航行时，往往会遇到各类突发障碍物或威胁，此时还要求机器人能够安全躲避障碍物，并避免机器人间发生碰撞。上述约束大大增加了编队问题的复杂度，而模型预测控制方法可灵活处理各种约束，鲁棒性高，因此将模型预测控制与上述虚拟结构法相结合是行之有效的手段。由于模型预测控制器能够稳定跟踪参考状态向量，因此如何根据编队与避障要求来定义合理的参考状态向量，是问题关键所在。而现有方法往往仅适用于规避简单障碍，无法应用于三维复杂环境下的编队与避障任务。When underwater robots navigate in formation in a complex three-dimensional environment, they often encounter various sudden obstacles or threats. At this time, the robots are also required to be able to safely avoid obstacles and avoid collisions between robots. The above constraints greatly increase the complexity of the formation problem, and the model predictive control method can flexibly deal with various constraints and has high robustness. Therefore, the combination of the model predictive control and the above virtual structure method is an effective 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 to the problem. However, existing methods are often only suitable for avoiding simple obstacles, and cannot be applied to formation and obstacle avoidance tasks in complex three-dimensional environments.

因此，有必要提供一种基于修正零空间模型预测控制的水下机器人三维编队及避障方法，以同时保证机器人稳定编队航行与安全避障，适应于复杂环境。Therefore, it is necessary to provide a three-dimensional formation and obstacle avoidance method for underwater robots based on modified zero-space model predictive control, so as to ensure stable formation navigation and safe obstacle avoidance of robots, and adapt to complex environments.

发明内容SUMMARY OF THE INVENTION

本发明在传统水下机器人编队控制方法不足的基础上提供了一种水下机器人三维编队及避障方法，基于修正零空间模型预测控制，以同时保证机器人稳定编队航行与安全避障，适应于复杂环境。The invention provides a three-dimensional formation and obstacle avoidance method for an underwater robot on the basis of the deficiency of the traditional underwater robot formation control method. complex environment.

为了实现上述目的，本发明提供了一种水下机器人三维编队及避障方法，包括以下步骤：In order to achieve the above purpose, the present invention provides a three-dimensional formation and obstacle avoidance method for an underwater robot, comprising the following steps:

(S1)构建水下机器人的三维编队系统数学模型：采用虚拟结构法定义几何队形，并根据队形中心参考轨迹，构建水下机器人的三维编队系统数学模型；(S1) Constructing the mathematical model of the three-dimensional formation system of the underwater robot: using the virtual structure method to define the geometric formation, and constructing the mathematical model of the three-dimensional formation system of the underwater robot according to the reference trajectory of the formation center;

(S2)无障碍自由环境下，基于Lyapunov稳定性原理计算各水下机器人的参考状态向量；(S2) In an unobstructed free environment, the reference state vector of each underwater robot is calculated based on the Lyapunov stability principle;

(S3)计算单个障碍对水下机器人运动的影响，以修正各水下机器人的参考运动速度；(S3) calculating the influence of a single obstacle on the motion of the underwater robot to correct the reference motion speed of each underwater robot;

(S4)根据零空间策略，考虑所有障碍物对水下机器人运动的影响，获得多障碍环境下的各水下机器人参考状态向量；(S4) According to the zero-space strategy, considering the influence of all obstacles on the motion of the underwater robot, obtain the reference state vector of each underwater robot in a multi-obstacle environment;

(S5)各水下机器人根据各自的参考状态向量，利用非线性模型预测控制器进行编队控制问题求解，获得最优控制输入。(S5) Each underwater robot uses the nonlinear model predictive controller to solve the formation control problem according to its own reference state vector, and obtains the optimal control input.

优选的，步骤(S1)采用虚拟结构法定义几何队形，并根据队形中心参考轨迹，构建水下机器人的三维编队系统数学模型的方法为：Preferably, the step (S1) adopts the virtual structure method to define the geometric formation, and according to the reference trajectory of the formation center, the method for constructing the mathematical model of the three-dimensional formation system of the underwater robot is:

不考虑洋流场影响，将第i个水下机器人R_i在惯性坐标系O-xyz下的质点运动模型定义为：Without considering the influence of the ocean current field, the particle motion model of the_i -th underwater robot Ri in the inertial coordinate system O-xyz is defined as:

其中，s_i＝[x_i,y_i,z_i,v_i,ψ_i,γ_i]^T和u_i＝[u_xi,u_yi,u_zi]^T分别表示惯性坐标系O-xyz下第i个水下机器人的状态和控制输入向量，(x_i,y_i,z_i)为机器人位置坐标，v_i、ψ_i、γ_i分别为机器人速率、艏向角、纵倾角，u_xi、u_yi、u_zi分别表示水下机器人在前进、艏向、纵倾三个方向上的过载分量；且满足u_x,min≤u_xi≤u_x,max、u_y,min≤u_yi≤u_y,max、u_z,min≤u_zi≤u_z,max、z_min≤z_i≤z_max、v_min≤v_i≤v_max、γ_min≤γ_i≤γ_max；Among them, s_i =[x_i ,y_i ,z_i ,v_i ,ψ_i ,γ_i ]^T and_ui =[u_xi ,u_yi ,u_zi ]^T respectively represent the first inertial coordinate system O-xyz The state and control input vectors of i underwater robots, (x_i , y_i , z_i ) are the robot position coordinates, vi , ψ_i , γ_i are the robot velocity, heading angle, and pitch angle_, respectively, u_xi , u_yi and u_zi represent the overload components of the underwater robot in the forward, heading and trim directions respectively; and u_x,min ≤u_xi ≤u_x,max , u_y,min ≤u_yi ≤u_y,max , u_z,min ≤u_zi ≤u_z,max , z_min ≤z_i ≤z_max , v_min ≤vi ≤v_max , γ_min ≤γ_i≤γmax ;

将所有水下机器人组队看作一个虚拟刚体，采用虚拟结构法定义几何队形，且其几何中心点为O_r，机器人编队的期望运动轨迹即为队形中心O_r的参考运动轨迹，则参考运动轨迹定义为：Considering all the underwater robot teams as a virtual rigid body, the virtual structure method is used to define the geometric formation, and its geometric center point is Or, and the desired movement_trajectory of the robot formation is the reference movement_trajectory of the formation center Or, then The reference motion trajectory is defined as:

其中,惯性坐标系O-xyz下第i个水下机器人R_i和中心点O_r的位置坐标分别为P_i＝[x_i,y_i,z_i]^T和P_r＝[x_r,y_r,z_r]^T，虚拟中心点O_r的期望速率、方位角、纵倾角分别为v_r、ψ_r、γ_r，ω_r为转弯角速率；Among them, the position coordinates of the_i -th underwater robot Ri and the center point O_r in the inertial coordinate system O-xyz are P_i =[x_i ,y_i ,z_i ]^T and P_r =[x_r ,y_r , z_r ]^T , the desired velocity, azimuth angle, and pitch angle of the virtual center point_Or are v_r , ψ_r , γ_r , respectively, and ω_r is the turning angle rate;

以虚拟中心点O_r的速度矢量在水平面内的投影为x_r轴，以垂直向上方向为z_r轴方向，建立三维编队坐标系O_r-x_ry_rz_r，则将编队坐标系O_r-x_ry_rz_r下第i个水下机器人R_i的位置P_ir＝[x_ir,y_ir,z_ir]^T表示为：Taking the projection of the velocity vector of the virtual center point Or in the horizontal plane as the x_r axis, and taking the vertical upward direction as the z_r axis direction, a three-dimensional formation coordinate system_Or -x_{ry r z risestablished} , then the formation coordinate system O The position P_ir =[x_ir ,y_ir ,z_ir ]^T of the_i -th underwater robot Ri under_r -x_r y_r z_r is expressed as:

将式(3)求导，得到：Taking the derivative of formula (3), we get:

根据期望的几何队形，确定水下机器人R_i在编队坐标系O_r-x_ry_rz_r下的期望位置P_dir＝[x_dir,y_dir,z_dir]^T，则编队误差表示为：According to the expected geometric formation, determine the expected position P_dir =[x_dir ,y_dir ,z_dir ]^T of the underwater robot_Ri in the formation coordinate system_{Or -x r y r z r,thenthe} formation error is expressed as :

P_ie＝[x_ie,y_ie,z_ie]^T＝P_ir-P_dir＝[x_ir-x_dir,y_ir-y_dir,z_ir-z_dir]^T (5)P_ie =[x_ie ,y_ie ,z_ie ]^T =P_ir -P_dir =[x_ir -x_dir ,y_ir -y_dir ,z_ir -z_dir ]^T (5)

根据式(1)-(5),构建水下机器人R_i的三维编队系统数学模型为：According to equations (1)-(5), the mathematical model of the three-dimensional formation system of the underwater robot_Ri is constructed as:

其中，第i个水下机器人R_i在编队坐标系O_r-x_ry_rz_r下的状态向量为s_i＝[x_ie,y_ie,z_ie,v_i,ψ_i,γ_i]^T。Among them, the state vector of the_i -_th underwater robot Ri in the formation coordinate system_Or -x_r y_r z_r is s_i =[x_ie ,y_ie ,z_ie ,vi ,ψ_i ,γ_i ]^T.

优选的，步骤(S2)无障碍自由环境下，基于Lyapunov稳定性原理计算各水下机器人的参考状态向量的方法为：Preferably, in step (S2), in an unobstructed free environment, the method for calculating the reference state vector of each underwater robot based on the Lyapunov stability principle is:

定义Lyapunov距离函数：Define the Lyapunov distance function:

对式(7)求导，得到：Taking the derivative of formula (7), we get:

其中，

β₁、β₂、β₃为大于0的常系数；v_i、ψ_i、γ_i分别作为水下机器人R_i的期望速率v_ir、艏向角ψ_ir、纵倾角γ_ir，期望编队误差为0，得到自由环境下第i个水下机器人R_i在编队坐标系O_r-x_ry_rz_r下的参考状态向量为s_ir＝[0,0,0,v_ir,ψ_ir,γ_ir]^T。in,

β₁ , β₂ , β₃ are constant coefficients greater than 0; vi , ψ_i , γ_i are the expected velocity_{vir , the heading angle ψ ir , the pitch angle γ irofthe} underwater robot Ri_, respectively_, and the expected formation error is 0, the reference state vector of the_i -th underwater robot Ri in the formation coordinate system_Or -x_r y_r z_r in the free environment is obtained as s_ir =[0,0,0,v_ir ,ψ_ir , γ_ir ]^T .

优选的，步骤(S3)计算单个障碍对水下机器人运动的影响，以修正各水下机器人的参考运动速度的方法为：Preferably, the step (S3) calculates the influence of a single obstacle on the motion of the underwater robot, and the method for correcting the reference motion speed of each underwater robot is:

根据公式：According to the formula:

计算第k个障碍物对水下机器人R_i运动的影响M_k；Calculate the influence Mk of the_kth obstacle on the motion of the underwater robot_Ri ;

其中，

表示第k个障碍物的表面方程，(x_k0,y_k0,z_k0)表示障碍物中心，d_k、e_k、f_k与a_k、b_k、c_k分别为障碍物形状系数和尺寸系数，k∈{1,...,K}，K表示水下机器人航行空间内障碍物数量；I为三阶单位矩阵，

表示第k个障碍物的径向法向量，ρ_k为障碍物反应系数且ρ_k＞0，n_k^TV_ir≤0表示水下机器人R_i逐渐接近第k个障碍物，n_k^TV_ir＞0表示水下机器人R_i已避开第k个障碍物；in,

represents the surface equation of the k-th obstacle, (x_k0 , y_k0 , z_k0 ) represents the center of the obstacle, d_k ,_ek , f_k and_ak , b_k ,_ck are the shape coefficient and size of the obstacle, respectively coefficient, k∈{1,...,K}, K represents the number of obstacles in the navigation space of the underwater robot; I is the third-order unit matrix,

Represents the radial normal vector of the k-th obstacle, ρ_k is the obstacle response coefficient and ρ_k > 0, n_k^T V_ir ≤0 means that the underwater robot Ri is gradually approaching the_k -th obstacle, n_k^T V_ir > 0 means that the underwater robot_Ri has avoided the kth obstacle;

根据水下机器人与障碍物间的距离，计算各障碍物的权重值ω_k，并将权重值ω_k归一化处理，得到归一化权重值

According to the distance between the underwater robot and the obstacle, the weight value ω_k of each obstacle is calculated, and the weight value ω_k is normalized to obtain the normalized weight value

根据自由环境下的水下机器人R_i的参考状态向量s_ir，计算水下机器人的参考运动速度V_ir，并在第k个障碍物影响下修正水下机器人的参考运动速度，得到参考运动速度修正值

即：According to the reference state vector s_ir of the underwater robot R_i in the free environment, calculate the reference movement speed V_ir of the underwater robot, and correct the reference movement speed of the underwater robot under the influence of the kth obstacle to obtain the reference movement speed Correction value

which is:

其中，第k个障碍物的运动速度为U_obs,k，

为指数衰减项。Among them, the moving speed of the k-th obstacle is U_obs,k ,

is an exponential decay term.

优选的，步骤(S4)根据零空间策略，考虑所有障碍物对水下机器人运动的影响，获得多障碍环境下的各水下机器人参考状态向量的方法为：Preferably, in step (S4), according to the zero-space strategy, considering the influence of all obstacles on the motion of the underwater robot, the method for obtaining the reference state vector of each underwater robot in a multi-obstacle environment is:

考虑所有障碍物对水下机器人R_i运动的影响，将所有的参考运动速度修正值

按大小进行排序，确定它们的优先级序号{1,...,K}；Considering the influence of all obstacles on the motion of the underwater robot R_i , correct all the reference motion speeds

Sort by size and determine their priority number {1,...,K};

根据公式：According to the formula:

基于零空间策略，完全保留优先级最高的参考运动速度修正值

将优先级次高的参考运动速度修正值

仅保留其在零空间内的分量，融合各参考运动速度修正值

并补偿，确定水下机器人的参考速度向量

Based on the zero-space strategy, the reference motion speed correction value with the highest priority is completely reserved

Correct the reference motion speed with the second highest priority

Only keep its components in the null space, and fuse the correction values of each reference motion speed

and compensation to determine the reference velocity vector of the underwater robot

根据水下机器人R_i的参考速度向量

计算各参考状态

确定多障碍环境下的水下机器人R_i的参考状态向量为

According to the reference velocity vector of the underwater robot R_i

Calculate each reference state

Determine the reference state vector of the underwater robot R_i in the multi-obstacle environment as

优选的，根据各水下机器人的编队位置误差

大小确定各水下机器人优先级顺序；Preferably, according to the formation position error of each underwater robot

The size determines the priority order of each underwater robot;

根据优先级协调策略，对于优先级最高的水下机器人无需考虑其他水下机器人影响；对于优先级较低的水下机器人，将优先级较高的水下机器人作为其可能会遭遇的虚拟障碍，即半径为d_min、运动速度为

的球形障碍，其中d_min表示水下机器人之间的安全距离。According to the priority coordination strategy, there is no need to consider the influence of other underwater robots for the underwater robot with the highest priority; for the underwater robot with a lower priority, the underwater robot with a higher priority is regarded as a virtual obstacle that it may encounter. That is, the radius is d_min , and the moving speed is

, where d_min represents the safe distance between underwater robots.

优选的，步骤(S5)各水下机器人根据各自的参考状态向量，利用非线性模型预测控制器进行编队控制问题求解，获得最优控制输入的方法为：Preferably, in step (S5), each underwater robot uses a nonlinear model predictive controller to solve the formation control problem according to their respective reference state vectors, and the method for obtaining the optimal control input is:

各水下机器人根据当前时刻t时的自身及其他水下机器人的状态向量、期望队形、参考轨迹、环境信息构建目标函数：Each underwater robot constructs an objective function according to the state vector, expected formation, reference trajectory, and environmental information of itself and other underwater robots at the current time t:

其中，当前时刻为t，时域长度为N，第i个水下机器人R_i的最优控制输入序列、预测状态向量序列、参考状态向量序列分别为u_i(t:t+N-1)＝{u_i(t),…,u_i(t+N-1)}、s_i(t+1:t+N)＝{s_i(t+1),…,s_i(t+N)}、s_ir(t+1:t+N)＝{s_ir(t+1),…,s_ir(t+N)}；A、B、C分别表示状态代价、终端状态代价、控制输入代价的加权矩阵；Among them, the current time is t, the length of the time domain is N, and the optimal control input sequence, predicted state vector sequence, and reference state vector sequence of the_i -th underwater robot Ri are respectively u_i (t:t+N-1) ={u_i (t),…,u_i (t+N-1)}, s_i (t+1:t+N)={s_i (t+1),…,s_i (t+N )}, s_ir (t+1:t+N)={s_ir (t+1),...,s_ir (t+N)}; A, B, C represent state cost, terminal state cost, control weighted matrix of input costs;

优化计算最优控制输入序列：The optimization computes the optimal control input sequence:

求解最优控制输入序列

Solve the optimal control input sequence

根据当前时刻t时的最优控制输入序列

更新水下机器人实际状态。According to the optimal control input sequence at the current time t

Update the actual status of the underwater robot.

与现有技术相比，本发明的优点和积极效果在于：Compared with the prior art, the advantages and positive effects of the present invention are:

本发明提供了一种水下机器人三维编队及避障方法，基于修正零空间模型预测，结合虚拟结构法，通过构建水下机器人三维编队系统数学模型，采取Lyapunov稳定性理论计算各机器人的参考状态向量，实现三维编队与轨迹跟踪；同时，采取量化计算单个障碍对水下机器人运动的影响，通过修正机器人的参考运动速度，可在不影响编队系统稳定性的同时实现安全避障；采用零空间策略来综合考虑所有障碍物对机器人运动的影响，以满足避障要求，相比于传统的加权求和方式，该策略更加合理、高效，且仍然能同时保证机器人稳定编队航行与安全避障，适应于复杂环境。The invention provides a three-dimensional formation and obstacle avoidance method of an underwater robot. Based on the prediction of the corrected zero-space model, combined with the virtual structure method, the mathematical model of the three-dimensional formation system of the underwater robot is constructed, and the Lyapunov stability theory is used to calculate the reference state of each robot. vector to realize three-dimensional formation and trajectory tracking; at the same time, quantitative calculation of the influence of a single obstacle on the movement of the underwater robot is adopted, and by correcting the reference movement speed of the robot, it can achieve safe obstacle avoidance without affecting the stability of the formation system; using zero space Compared with the traditional weighted sum method, this strategy is more reasonable and efficient, and can still ensure the stable formation navigation of the robot and safe obstacle avoidance at the same time. Adapt to complex environments.

附图说明Description of drawings

图1为本发明的水下机器人三维编队及避障方法流程图；Fig. 1 is the flow chart of the three-dimensional formation and obstacle avoidance method of underwater robot of the present invention;

图2为基于虚拟结构法的水下机器人三维编队示意图；Figure 2 is a schematic diagram of a three-dimensional formation of an underwater robot based on a virtual structure method;

图3为二维空间下的零空间策略示意图；Figure 3 is a schematic diagram of a null space strategy in two-dimensional space;

图4为多机器人三维编队与避障结果；Figure 4 shows the results of multi-robot 3D formation and obstacle avoidance;

其中：图4a显示水下机器人三维编队三维视图，图4b显示水下机器人三维编队俯视图，图4c显示编队误差曲线。Among them: Figure 4a shows the 3D view of the 3D formation of the underwater robot, Figure 4b shows the top view of the 3D formation of the underwater robot, and Figure 4c shows the formation error curve.

具体实施方式Detailed ways

以下，结合附图对本发明的具体实施方式进行进一步的描述。Hereinafter, the specific embodiments of the present invention will be further described with reference to the accompanying drawings.

由于水下机器人在三维复杂环境下编队航行时，往往会遇到各类突发障碍物或威胁，此时还要求水下机器人能够安全躲避障碍物，并避免机器人间发生碰撞。上述约束大大增加了编队问题的复杂度，而模型预测控制方法可灵活处理各种约束，鲁棒性高，因此本发明考虑将模型预测控制与虚拟结构法相结合。由于模型预测控制器能够稳定跟踪参考状态向量，因此如何根据编队与避障要求来定义合理的参考状态向量，是问题关键所在。针对编队要求，本发明考虑基于稳定性原理、空间位置或速度差值等计算出参考状态向量；在此基础上为满足避障要求，采用零空间策略来综合考虑所有障碍物对机器人运动的影响。When underwater robots navigate in formation in a complex three-dimensional environment, they often encounter various sudden obstacles or threats. At this time, underwater robots are also required to be able to safely avoid obstacles and avoid collisions between robots. The above constraints greatly increase the complexity of the formation problem, and the model predictive control method can flexibly handle various constraints and has high robustness. Therefore, the present 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 to the problem. In view of the formation requirements, the present invention considers the calculation of the reference state vector based on the stability principle, spatial position or speed difference, etc. On this basis, in order to meet the obstacle avoidance requirements, a zero-space strategy is adopted to comprehensively consider the impact of all obstacles on the robot motion .

基于上述分析，本发明提供了一种基于修正零空间模型预测控制的水下机器人三维编队及避障方法。通过构建基于虚拟结构的水下机器人三维编队模型，进而采用Lyapunov稳定性原理与修正零空间策略来确定模型预测控制器的参考状态向量，可引导水下机器人组成三维队形并精确跟踪期望轨迹，同时安全躲避障碍。方法流程如图1所示，具体包括以下步骤：Based on the above analysis, the present invention provides a three-dimensional formation and obstacle avoidance method for an underwater robot based on a modified zero-space model predictive control. By constructing a 3D formation model of underwater robot based on virtual structure, and then using Lyapunov stability principle and corrected zero space strategy to determine the reference state vector of the model predictive controller, the underwater robot can be guided to form a 3D formation and accurately track the desired trajectory. Also avoid obstacles safely. The method flow is shown in Figure 1, which specifically includes the following steps:

(1)构建水下机器人的三维编队系统数学模型：采用虚拟结构法定义几何队形，并根据队形中心参考轨迹，构建水下机器人的三维编队系统数学模型。具体为：(1) Build the mathematical model of the 3D formation system of the underwater robot: The virtual structure method is used to define the geometric formation, and according to the reference trajectory of the formation center, the mathematical model of the 3D formation system of the underwater robot is constructed. Specifically:

①不考虑洋流场影响，第i个水下机器人R_i在惯性坐标系O-xyz下的质点运动模型可定义为：① Regardless of the influence of the ocean current field, the particle motion model of the i-th underwater robot R_i in the inertial coordinate system O-xyz can be defined as:

其中s_i＝[x_i,y_i,z_i,v_i,ψ_i,γ_i]^T和u_i＝[u_xi,u_yi,u_zi]^T分别表示水下机器人状态和控制输入。(x_i,y_i,z_i)为机器人位置，v_i、ψ_i、γ_i分别为机器人速率、艏向角、纵倾角u_xi、u_yi、u_zi分别表示水下机器人在前进、艏向、纵倾三个方向上的过载分量；另外，水下机器人状态量和控制量需满足如下运动约束条件u_x,min≤u_xi≤u_x,max，u_y,min≤u_yi≤u_y,max，u_z,min≤u_zi≤u_z,max，z_min≤z_i≤z_max，v_min≤v_i≤v_max，γ_min≤γ_i≤γ_max。where s_i =[_xi ,y_i ,z_i ,_{vi ,ψ i ,γ i ]T and ui =[u xi,u}^yi_,uzi]^T represent the state and control input of the underwater robot, respectively. (_xi ,_yi ,_zi ) is the position of the robot, v_i , ψ_i , and γ_i are the speed, heading angle, and pitch angle of the robot, respectively. The overload components in the three directions of pitch and pitch; in addition, the state and control quantities of the underwater robot must satisfy the following motion constraints u_x,min ≤u_xi ≤u_x,max , u_y,min ≤u_yi ≤u_y,max , u_z,min ≤u_zi ≤u_z,max , z_min ≤zi_≤zmax , v_min ≤vi ≤v_max , γ_min ≤γ_i≤γmax .

②如图2所示，采用虚拟结构法来定义几何队形，即将所有水下机器人组队看作一个虚拟刚体，且其几何中心点为O_r，因此机器人编队的期望运动轨迹即为队形中心O_r的参考运动轨迹。惯性坐标系O-xyz下第i个水下机器人R_i和中心点O_r的位置坐标分别为P_i＝[x_i,y_i,z_i]^T和P_r＝[x_r,y_r,z_r]^T，虚拟中心点O_r的期望速率、方位角、纵倾角分别为v_r、ψ_r、γ_r，则参考运动轨迹可定义为：②As shown in Figure 2, the virtual structure method is used to define the geometric formation, that is, all the underwater robots are regarded as a virtual rigid body, and its geometric center point is_Or , so the expected movement trajectory of the robot formation is the formation The reference motion_trajectory of the center Or. The position coordinates of the_i -th underwater robot Ri and the center point Or under the inertial coordinate system O-xyz are P_i =[x_i ,y_i ,z_i ]^T and P_r =[x_r ,_{y r,} z_r ]^T , the desired velocity, azimuth, and pitch of the virtual center point_Or are v_r , ψ_r , and γ_r respectively, then the reference motion trajectory can be defined as:

其中，ω_r为转弯角速率。where ω_r is the turning angular rate.

③为了更清晰描述模型，以虚拟中心点O_r的速度矢量在水平面内的投影为x_r轴，以垂直向上方向为z_r轴方向，建立三维编队坐标系O_r-x_ry_rz_r，得到编队坐标系O_r-x_ry_rz_r下机器人R_i的位置P_ir＝[x_ir,y_ir,z_ir]^T为：③ In order to describe the model more clearly, the three-dimensional formation coordinate system_Or -x_r y_r z_r is established with the projection of the velocity_vector of the virtual center point Or in the horizontal plane as the x_r axis, and the vertical upward direction as the z_r axis direction._, the position P_ir = [x_ir , y_ir , z_ir ]^T of the robot R_{i under the formation coordinate system Or -x r y r z risobtainedas} :

对上式(3)求导，得到：Taking the derivation of the above formula (3), we get:

根据期望的几何队形，确定机器人R_i在三维编队坐标系O_r-x_ry_rz_r下的期望位置P_dir＝[x_dir,y_dir,z_dir]^T；According to the expected geometric formation, determine the expected position P_dir =[x_dir ,y_dir ,z_dir ]^T of the robot_Ri in the three-dimensional formation coordinate system Or-x_r y_r z_r;

则求得编对误差P_ie＝[x_ie,y_ie,z_ie]^T＝P_ir-P_dir＝[x_ir-x_dir,y_ir-y_dir,z_ir-z_dir]^T；Then find the alignment error P_ie =[x_ie ,y_ie ,z_ie ]^T =P_ir -P_dir =[x_ir -x_dir ,y_ir -y_dir ,z_ir -z_dir ]^T ;

由编队误差P_ie，将式(4)可重新定义为：From the formation error P_ie , formula (4) can be redefined as:

则构建第i个水下机器人R_i的三维编队系统数学模型为：Then the mathematical model of the three-dimensional formation system of the i-th underwater robot R_i is constructed as:

其中，第i个水下机器人R_i在编队坐标系O_r-x_ry_rz_r下的状态向量为s_i＝[x_ie,y_ie,z_ie,v_i,ψ_i,γ_i]^T。Among them, the state vector of the_i -_th underwater robot Ri in the formation coordinate system_Or -x_r y_r z_r is s_i =[x_ie ,y_ie ,z_ie ,vi ,ψ_i ,γ_i ]^T.

(2)无障碍自由环境下，基于Lyapunov稳定性原理计算各水下机器人的参考状态向量。具体为：(2) In an unobstructed free environment, the reference state vector of each underwater robot is calculated based on the Lyapunov stability principle. Specifically:

为使机器人组成编队队形并沿期望轨迹航行，要求各机器人编队误差逐渐趋于0即x_ie→0,y_ie→0,z_ie→0，因此可定义Lyapunov距离函数为：In order to make the robots form a formation and sail along the desired trajectory, the formation error of each robot is required to gradually tend to 0, that is, x_ie → 0, y_ie → 0, z_ie → 0, so the Lyapunov distance function can be defined as:

对式(7)求导后可得After derivation of formula (7), we can get

为实现

可定义to achieve

definable

其中，β₁、β₂、β₃为大于0的常系数；v_i、ψ_i、γ_i分别作为第i个水下机器人R_i的期望速率v_ir、艏向角ψ_ir、纵倾角γ_ir，由于水下机器人的期望编队误差均为0，得到自由环境下第i个水下机器人R_i在编队坐标系O_r-x_ry_rz_r下的参考状态向量为s_ir＝[0,0,0,v_ir,ψ_ir,γ_ir]^T。Among them, β₁ , β₂ , β₃ are constant coefficients greater than 0; vi , ψ_i , γ_i are the expected velocity_{vir , heading angle ψ ir,} pitch angle γ of the_i -_th underwater robot Ri, respectively_ir , since the expected formation error of the underwater robot is all 0, the reference state vector of the_i -th underwater robot Ri in the formation coordinate system_Or -x_r y_r z_r in the free environment is obtained as s_ir = [0 ,0,0,v_ir ,ψ_ir ,γ_ir ]^T .

(3)计算单个障碍对水下机器人运动的影响，以修正各水下机器人的参考运动速度。具体为：(3) Calculate the influence of a single obstacle on the motion of the underwater robot to correct the reference motion speed of each underwater robot. Specifically:

①假设机器人航行空间内有K个障碍物，以第k个障碍物为例，k∈{1,...,K}，其表面方程为

其中(x_k0,y_k0,z_k0)表示障碍物中心，d_k,e_k,f_k与a_k,b_k,c_k分别为障碍物形状系数和尺寸系数。①Assuming that there are K obstacles in the robot's navigation space, taking the kth obstacle as an example, k∈{1,...,K}, its surface equation is

Where (x_k0 , y_k0 , z_k0 ) represents the center of the obstacle, and d_k ,_ek , f_k and_ak , b_k ,_ck are the shape factor and size factor of the obstacle, respectively.

第k个障碍物对水下机器人R_i运动的影响为：The influence of the kth obstacle on the motion of the underwater robot_Ri is:

其中，d_k、e_k、f_k与a_k、b_k、c_k分别为第k个障碍物的形状系数和尺寸系数，k∈{1,...,K}，K表示水下机器人航行空间内障碍物数量；I为三阶单位矩阵，

表示第k个障碍物的径向法向量，ρ_k为障碍物反应系数且ρ_k＞0，n_k^TV_ir≤0表示水下机器人R_i逐渐接近第k个障碍物，n_k^TV_ir＞0表示水下机器人R_i已避开第k个障碍物。Among them, d_k ,_ek , f_k and_ak , b_k ,_ck are the shape coefficient and size coefficient of the k-th obstacle, respectively, k∈{1,...,K}, K represents the underwater robot The number of obstacles in the navigation space; I is the third-order unit matrix,

Represents the radial normal vector of the k-th obstacle, ρ_k is the obstacle response coefficient and ρ_k > 0, n_k^T V_ir ≤0 means that the underwater robot Ri is gradually approaching the_k -th obstacle, n_k^T V_ir >0 means that the underwater robot_Ri has avoided the kth obstacle.

②根据机器人与障碍物间的距离，计算各障碍物的权重值ω_k：② According to the distance between the robot and the obstacle, calculate the weight value ω_k of each obstacle:

由于

因此还需将权重值归一化处理，得到归一化权重值

because

Therefore, it is necessary to normalize the weight value to obtain the normalized weight value.

③根据自由环境下的机器人参考状态向量s_ir，可计算水下机器人R_i的参考运动速度V_ir＝[v_ircosγ_ircosψ_ir,v_ircosγ_irsinψ_ir,v_irsinγ_ir]^T，并在第k个障碍物影响下修正参考运动速度，得到参考运动速度修正值

即：③According to the robot reference state vector s_ir in the free environment, the reference motion speed of the underwater robot R_i can be calculated V_ir =[v_ir cosγ_ir cosψ_ir ,v_ir cosγ_ir sinψ_ir ,v_ir sinγ_ir ]^T , and Correct the reference motion speed under the influence of the kth obstacle, and obtain the reference motion speed correction value

which is:

其中，第k个障碍物的运动速度为U_obs,k，指数衰减项

用来适当减少远处障碍物的影响。Among them, the moving speed of the k-th obstacle is U_obs,k , and the exponential decay term

Used to properly reduce the influence of distant obstacles.

(4)根据零空间策略，考虑所有障碍物对水下机器人运动的影响，获得多障碍环境下的各水下机器人参考状态向量。具体为：(4) According to the zero-space strategy, considering the influence of all obstacles on the motion of the underwater robot, the reference state vector of each underwater robot in the multi-obstacle environment is obtained. Specifically:

由于各障碍物的形状、尺寸、距离等互不相同，它们对水下机器人运动会产生不同的影响，因此修正后的参考运动速度

互不相同，甚至可能互相矛盾。例如，为躲避某障碍物，机器人需向左运动，而为躲避另一障碍物，机器人可能需向右运动。传统的速度求和方法无法应对上述问题，因此本发明采用零空间策略来融合修正后的各项参考运动速度：首先确定各参考运动速度的优先级，然后保留优先级较高的参考运动速度，而对于优先级较低的参考运动速度，仅保留其在零空间内的分量(即与优先级较高的速度向量相垂直)，而忽略其他分量。具体为：Because the shapes, sizes, distances, etc. of the obstacles are different from each other, they will have different effects on the motion of the underwater robot, so the corrected reference motion speed

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 summation method cannot deal with the above-mentioned problems, so the present invention adopts the zero-space strategy to fuse the corrected reference motion speeds: first, the priority of each reference motion speed is determined, and then the reference motion speed with higher priority is reserved, For the reference motion velocity with lower priority, only its component in the null space (ie, perpendicular to the velocity vector with higher priority) is retained, and other components are ignored. Specifically:

①将所有的参考运动速度修正值

按从大到小进行排序，确定它们的优先级序号{1,...,K}；①Modify all reference motion speed to the correction value

Sort from large to small to determine their priority number {1,...,K};

②根据零空间策略融合各参考运动速度修正值：②Fusing the correction value of each reference motion speed according to the zero space strategy:

按照上式，将完全保留优先级最高的参考运动速度修正值，而对于优先级次高的参考运动速度修正值仅保留其在零空间内的分量，以此类推，即可融合所有参考运动速度修正值。图3表示二维空间下的零空间策略示意图，其中

的优先级高于

表示与

垂直的

分量。此外，由于部分参考运动速度修正值分量被忽略，因此融合的速率

还需进一步补偿，即

According to the above formula, the reference motion speed correction value with the highest priority will be completely retained, and the reference motion speed correction value with the second highest priority will only retain its component in the zero space, and so on, all reference motion speeds can be fused Correction value. Figure 3 shows a schematic diagram of the null space strategy in two-dimensional space, where

priority is higher than

means with

vertical

weight. In addition, since some of the reference motion velocity modifier components are ignored, the fused rate

Further compensation is required, i.e.

③根据修正后的参考速度向量

可计算出各参考状态如

因此多障碍环境下水下机器人R_i的参考状态向量为

③According to the corrected reference velocity vector

Each reference state can be calculated as

Therefore, the reference state vector of the underwater robot R_i in the multi-obstacle environment is

各水下机器人均按上述步骤计算各自的参考状态向量，但还需将其他水下机器人作为虚拟障碍物，以避免机器人间发生碰撞。具体来说，首先根据各机器人的编队位置误差

确定优先级顺序，误差越小，机器人优先级越高；然后，根据优先级协调策略，优先级最高的机器人无需考虑其他机器人影响，而对于优先级较低的机器人来说，需要将优先级较高的机器人作为它可能会遭遇的虚拟障碍，即半径为d_min、运动速度为

的球形障碍，其中d_min表示水下机器人之间的安全距离。Each underwater robot calculates its own reference state vector according to the above steps, but other underwater robots need to be used as virtual obstacles to avoid collisions between robots. Specifically, first, according to the formation position error of each robot

Determine the priority order, the smaller the error, the higher the priority of the robot; then, according to the priority coordination strategy, the robot with the highest priority does not need to consider the influence of other robots, while for the robot with lower priority, the priority needs to be higher. The tall robot is used as a virtual obstacle that it may encounter, that is, the radius is d_min and the movement speed is

, where d_min represents the safe distance between underwater robots.

(5)各水下机器人根据各自的参考状态向量，利用非线性模型预测控制器进行编队控制问题求解，获得最优控制输入。具体为：(5) Each underwater robot uses the nonlinear model predictive controller to solve the formation control problem according to its own reference state vector, and obtains the optimal control input. Specifically:

①各水下机器人利用非线性模型预测控制器跟踪参考状态，以获得最优控制输入。假设当前时刻为t，时域长度为N，机器人R_i的最优控制输入序列、预测状态向量序列、参考状态向量序列分别为u_i(t:t+N-1)＝{u_i(t),…,u_i(t+N-1)}、s_i(t+1:t+N)＝{s_i(t+1),…,s_i(t+N)}、s_ir(t+1:t+N)＝{s_ir(t+1),…,s_ir(t+N)}。① Each underwater robot uses a nonlinear model predictive controller to track the reference state to obtain the optimal control input. Assuming that the current moment is t and the time domain length is N, the optimal control input sequence, predicted state vector sequence, and reference state vector sequence of the robot R_i are respectively u_i (t:t+N-1)={u_i (t ),…,u_i (t+N-1)}, s_i (t+1:t+N)={s_i (t+1),…,s_i (t+N)}, s_ir ( t+1:t+N)={_sir (t+1),...,_sir (t+N)}.

则水下机器人R_i根据自身及其他机器人的状态向量、期望队形、参考轨迹、环境信息等构建目标函数：Then the underwater robot R_i constructs the objective function according to the state vector, expected formation, reference trajectory, and environmental information of itself and other robots:

其中，A、B、C分别表示状态代价、终端状态代价、控制输入代价的加权矩阵，它们需提前设定。Among them, A, B, and C represent the weighted matrix of state cost, terminal state cost, and control input cost, respectively, which need to be set in advance.

②然后，采取灰狼优化方法优化计算最优控制输入序列

② Then, adopt the gray wolf optimization method to optimize and calculate the optimal control input sequence

其中

in

③将当前时刻t时的最优控制输入序列

用于机器人航行，并更新水下机器人实际状态。在t+1时刻，重新计算最优控制输入序列并更新状态值，以此类推即可实现滚动优化。③ Input the optimal control sequence at the current time t

Used for robot navigation and updating the actual state of the underwater robot. Attime t+1, the optimal control input sequence is recalculated and the state value is updated, and so on to achieve rolling optimization.

图4表示某复杂环境下的水下机器人三维编队与避障结果，图4a显示水下机器人三维编队三维视图，图4b显示水下机器人三维编队俯视图，图4c显示编队误差曲线。5艘水下机器人R1-R5可快速编队组成三角队形并跟踪期望轨迹FGT，且安全躲避各类静态或动态障碍物以及其他机器人。虽然各机器人会因避障而改变期望队形，导致编队误差增大，但机器人在避开障碍后会迅速回到期望位置，编队误差重新趋于0。Figure 4 shows the results of the 3D formation and obstacle avoidance of underwater robots in a complex environment. Figure 4a shows the 3D view of the 3D formation of the underwater robot, Figure 4b shows the top view of the 3D formation of the underwater robot, and Figure 4c shows the formation error curve. The five underwater robots R1-R5 can quickly form a triangular formation and track the desired trajectory FGT, and safely avoid various static or dynamic obstacles and other robots. Although each robot will change the expected formation due to obstacle avoidance, resulting in an increase in the formation error, but the robot will quickly return to the desired position after avoiding the obstacle, and the formation error tends to zero again.

因此，综上可知，本发明提供的一种水下机器人三维编队及避障方法，基于修正零空间模型预测，基于虚拟模型结构，通过构建水下机器人三维编队系统数学模型，采取Lyapunov稳定性理论计算各机器人的参考状态向量，实现了三维编队与轨迹跟踪；同时，采取量化计算单个障碍对水下机器人运动的影响，通过修正机器人的参考运动速度，可在不影响编队系统稳定性的同时实现安全避障；采用零空间策略来综合考虑所有障碍物对机器人运动的影响，以满足避障要求，相比于传统的加权求和方式，该策略更加合理、高效，且仍然能同时保证机器人稳定编队航行与安全避障，适应于复杂环境。Therefore, it can be seen from the above that the three-dimensional formation and obstacle avoidance method of an underwater robot provided by the present invention is based on the prediction of the modified zero-space model and the virtual model structure. The reference state vector of each robot is calculated to realize three-dimensional formation and trajectory tracking; at the same time, the impact of a single obstacle on the movement of the underwater robot is quantitatively calculated, and the reference movement speed of the robot can be corrected without affecting the stability of the formation system. Safe obstacle avoidance; a zero-space strategy is used to comprehensively consider the impact of all obstacles on the robot's motion to meet the obstacle avoidance requirements. Compared with the traditional weighted summation method, this strategy is more reasonable and efficient, and can still ensure the stability of the robot at the same time. Formation navigation and safe obstacle avoidance, adapting to complex environments.

以上所述，仅是本发明的较佳实施例而已，并非是对本发明作其它形式的限制，任何熟悉本专业的技术人员可能利用上述揭示的技术内容加以变更或改型为等同变化的等效实施例应用于其它领域，但是凡是未脱离本发明技术方案内容，依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化与改型，仍属于本发明技术方案的保护范围。The above are only preferred embodiments of the present invention, and are not intended to limit the present invention in other forms. Any person skilled in the art may use the technical content disclosed above to make changes or modifications to equivalent changes. The embodiments are applied to other fields, but any simple modifications, equivalent changes and modifications made to the above embodiments according to the technical essence of the present invention still belong to the protection scope of the technical solutions of the present invention without departing from the content of the technical solutions of the present invention.

Claims (3)

Translated fromChinese

1.一种水下机器人三维编队及避障方法，其特征在于，包括以下步骤：1. an underwater robot three-dimensional formation and obstacle avoidance method, is characterized in that, comprises the following steps:(S1)构建水下机器人的三维编队系统数学模型：采用虚拟结构法定义几何队形，并根据队形中心参考轨迹，构建水下机器人的三维编队系统数学模型；(S1) Constructing the mathematical model of the three-dimensional formation system of the underwater robot: using the virtual structure method to define the geometric formation, and constructing the mathematical model of the three-dimensional formation system of the underwater robot according to the reference trajectory of the formation center;(S2)无障碍自由环境下，基于Lyapunov稳定性原理计算各水下机器人的参考状态向量；(S2) In an unobstructed free environment, the reference state vector of each underwater robot is calculated based on the Lyapunov stability principle;(S3)计算单个障碍对水下机器人运动的影响，以修正各水下机器人的参考运动速度：(S3) Calculate the influence of a single obstacle on the motion of the underwater robot to correct the reference motion speed of each underwater robot:根据公式：According to the formula:

计算第k个障碍物对水下机器人R_i运动的影响M_k；Calculate the influence Mk of the_kth obstacle on the motion of the underwater robot_Ri ;其中，

表示第k个障碍物的表面方程，(x_k0,y_k0,z_k0)表示障碍物中心，d_k、e_k、f_k与a_k、b_k、c_k分别为障碍物形状系数和尺寸系数，k∈{1,...,K}，K表示水下机器人航行空间内障碍物数量；I为三阶单位矩阵，

表示第k个障碍物的径向法向量，ρ_k为障碍物反应系数且ρ_k＞0，n_k^TV_ir≤0表示水下机器人R_i逐渐接近第k个障碍物，n_k^TV_ir＞0表示水下机器人R_i已避开第k个障碍物；in,

represents the surface equation of the k-th obstacle, (x_k0 , y_k0 , z_k0 ) represents the center of the obstacle, d_k ,_ek , f_k and_ak , b_k ,_ck are the shape coefficient and size of the obstacle, respectively coefficient, k∈{1,...,K}, K represents the number of obstacles in the navigation space of the underwater robot; I is the third-order unit matrix,

Represents the radial normal vector of the k-th obstacle, ρ_k is the obstacle response coefficient and ρ_k > 0, n_k^T V_ir ≤0 means that the underwater robot Ri is gradually approaching the_k -th obstacle, n_k^T V_ir > 0 means that the underwater robot_Ri has avoided the kth obstacle;根据水下机器人与障碍物间的距离，计算各障碍物的权重值ω_k，并将权重值ω_k归一化处理，得到归一化权重值

According to the distance between the underwater robot and the obstacle, the weight value ω_k of each obstacle is calculated, and the weight value ω_k is normalized to obtain the normalized weight value

根据自由环境下的水下机器人R_i的参考状态向量s_ir，计算水下机器人的参考运动速度V_ir，并在第k个障碍物影响下修正水下机器人的参考运动速度，得到参考运动速度修正值

即：According to the reference state vector s_ir of the underwater robot R_i in the free environment, calculate the reference movement speed V_ir of the underwater robot, and correct the reference movement speed of the underwater robot under the influence of the kth obstacle to obtain the reference movement speed Correction value

which is:

其中，第k个障碍物的运动速度为U_obs,k，

为指数衰减项；Among them, the moving speed of the k-th obstacle is U_obs,k ,

is the exponential decay term;(S4)根据零空间策略，考虑所有障碍物对水下机器人运动的影响，获得多障碍环境下的各水下机器人参考状态向量：(S4) According to the zero-space strategy, considering the influence of all obstacles on the motion of the underwater robot, the reference state vector of each underwater robot in the multi-obstacle environment is obtained:考虑所有障碍物对水下机器人R_i运动的影响，将所有的参考运动速度修正值

按大小进行排序，确定它们的优先级序号{1,...,K}；Considering the influence of all obstacles on the motion of the underwater robot R_i , correct all the reference motion speeds

Sort by size and determine their priority number {1,...,K};根据公式：According to the formula:

基于零空间策略，完全保留优先级最高的参考运动速度修正值

将优先级次高的参考运动速度修正值

仅保留其在零空间内的分量，融合各参考运动速度修正值

并补偿，确定水下机器人的参考速度向量

Based on the zero-space strategy, the reference motion speed correction value with the highest priority is completely reserved

Correct the reference motion speed with the second highest priority

Only keep its components in the null space, and fuse the correction values of each reference motion speed

and compensation to determine the reference velocity vector of the underwater robot

根据水下机器人R_i的参考速度向量

计算各参考状态

确定多障碍环境下的水下机器人R_i的参考状态向量为

According to the reference velocity vector of the underwater robot R_i

Calculate each reference state

Determine the reference state vector of the underwater robot R_i in the multi-obstacle environment as

(S5)各水下机器人根据各自的参考状态向量，利用非线性模型预测控制器进行编队控制问题求解，获得最优控制输入：(S5) Each underwater robot uses the nonlinear model predictive controller to solve the formation control problem according to its own reference state vector, and obtains the optimal control input:各水下机器人根据当前时刻t时的自身及其他水下机器人的状态向量、期望队形、参考轨迹、环境信息构建目标函数：Each underwater robot constructs an objective function according to the state vector, expected formation, reference trajectory, and environmental information of itself and other underwater robots at the current time t:

其中，当前时刻为t，时域长度为N，第i个水下机器人R_i的最优控制输入序列、预测状态向量序列、参考状态向量序列分别为u_i(t:t+N-1)＝{u_i(t),…,u_i(t+N-1)}、s_i(t+1:t+N)＝{s_i(t+1),…,s_i(t+N)}、s_ir(t+1:t+N)＝{s_ir(t+1),…,s_ir(t+N)}；A、B、C分别表示状态代价、终端状态代价、控制输入代价的加权矩阵；Among them, the current time is t, the length of the time domain is N, and the optimal control input sequence, predicted state vector sequence, and reference state vector sequence of the_i -th underwater robot Ri are respectively u_i (t:t+N-1) ={u_i (t),…,u_i (t+N-1)}, s_i (t+1:t+N)={s_i (t+1),…,s_i (t+N )}, s_ir (t+1:t+N)={s_ir (t+1),...,s_ir (t+N)}; A, B, C represent state cost, terminal state cost, control weighted matrix of input costs;优化计算最优控制输入序列：The optimization computes the optimal control input sequence:

求解最优控制输入序列

Solve the optimal control input sequence

根据当前时刻t时的最优控制输入序列

更新水下机器人实际状态。According to the optimal control input sequence at the current time t

Update the actual status of the underwater robot.2.根据权利要求1所述的水下机器人三维编队及避障方法，其特征在于，步骤(S1)采用虚拟结构法定义几何队形，并根据队形中心参考轨迹，构建水下机器人的三维编队系统数学模型的方法为：2. underwater robot three-dimensional formation and obstacle avoidance method according to claim 1, is characterized in that, step (S1) adopts virtual structure method to define geometric formation, and according to formation center reference track, constructs the three-dimensional underwater robot The method of the formation system mathematical model is:不考虑洋流场影响，将第i个水下机器人R_i在惯性坐标系O-xyz下的质点运动模型定义为：Without considering the influence of the ocean current field, the particle motion model of the_i -th underwater robot Ri in the inertial coordinate system O-xyz is defined as:

其中，s_i＝[x_i,y_i,z_i,v_i,ψ_i,γ_i]^T和u_i＝[u_xi,u_yi,u_zi]^T分别表示惯性坐标系O-xyz下第i个水下机器人的状态和控制输入向量，(x_i,y_i,z_i)为机器人位置坐标，v_i、ψ_i、γ_i分别为机器人速率、艏向角、纵倾角，u_xi、u_yi、u_zi分别表示水下机器人在前进、艏向、纵倾三个方向上的过载分量；且满足u_x,min≤u_xi≤u_x,max、u_y,min≤u_yi≤u_y,max、u_z,min≤u_zi≤u_z,max、z_min≤z_i≤z_max、v_min≤v_i≤v_max、γ_min≤γ_i≤γ_max；Among them, s_i =[x_i ,y_i ,z_i ,_{vi ,ψ i ,γ i ]T and u i =[u xi,u}^yi_,uzi]^T respectively represent the first inertial coordinate system O-xyz The state and control input vectors of i underwater robots, (x_i , y_i , z_i ) are the robot position coordinates, vi , ψ_i , γ_i are the robot velocity, heading angle, and pitch angle_, respectively, u_xi , u_yi and u_zi represent the overload components of the underwater robot in the forward, heading and trim directions respectively; and u_x,min ≤u_xi ≤u_x,max , u_y,min ≤u_yi ≤u_y,max , u_z,min ≤u_zi ≤u_z,max , z_min ≤z_i ≤z_max , v_min ≤vi ≤v_max , γ_min ≤γ_i≤γmax ;将所有水下机器人组队看作一个虚拟刚体，采用虚拟结构法定义几何队形，且其几何中心点为O_r，机器人编队的期望运动轨迹即为队形中心O_r的参考运动轨迹，则参考运动轨迹定义为：Considering all the underwater robot teams as a virtual rigid body, the virtual structure method is used to define the geometric formation, and its geometric center point is Or, and the desired movement_trajectory of the robot formation is the reference movement_trajectory of the formation center Or, then The reference motion trajectory is defined as:

其中,惯性坐标系O-xyz下第i个水下机器人R_i和中心点O_r的位置坐标分别为P_i＝[x_i,y_i,z_i]^T和P_r＝[x_r,y_r,z_r]^T，虚拟中心点O_r的期望速率、方位角、纵倾角分别为v_r、ψ_r、γ_r，ω_r为转弯角速率；Among them, the position coordinates of the_i -th underwater robot Ri and the center point O_r in the inertial coordinate system O-xyz are P_i =[x_i ,y_i ,z_i ]^T and P_r =[x_r ,y_r , z_r ]^T , the desired velocity, azimuth angle, and pitch angle of the virtual center point_Or are v_r , ψ_r , γ_r , respectively, and ω_r is the turning angle rate;以虚拟中心点O_r的速度矢量在水平面内的投影为x_r轴，以垂直向上方向为z_r轴方向，建立三维编队坐标系O_r-x_ry_rz_r，则将编队坐标系O_r-x_ry_rz_r下第i个水下机器人R_i的位置P_ir＝[x_ir,y_ir,z_ir]^T表示为：Taking the projection of the velocity vector of the virtual center point Or in the horizontal plane as the x_r axis, and taking the vertical upward direction as the z_r axis direction, a three-dimensional formation coordinate system_Or -x_{ry r z risestablished} , then the formation coordinate system O The position P_ir =[x_ir ,y_ir ,z_ir ]^T of the_i -th underwater robot Ri under_r -x_r y_r z_r is expressed as:

将上式求导，得到：Differentiating the above formula, we get:

根据期望的几何队形，确定水下机器人R_i在编队坐标系O_r-x_ry_rz_r下的期望位置P_dir＝[x_dir,y_dir,z_dir]^T，则编队误差表示为：According to the expected geometric formation, determine the expected position P_dir =[x_dir ,y_dir ,z_dir ]^T of the underwater robot_Ri in the formation coordinate system_{Or -x r y r z r,thenthe} formation error is expressed as :P_ie＝[x_ie,y_ie,z_ie]^T＝P_ir-P_dir＝[x_ir-x_dir,y_ir-y_dir,z_ir-z_dir]^TP_ie =[x_ie ,y_ie ,z_ie ]^T =P_ir -P_dir =[x_ir -x_dir ,y_ir -y_dir ,z_ir -z_dir ]^T构建水下机器人R_i的三维编队系统数学模型为：The mathematical model of the three-dimensional formation system of the underwater robot R_i is constructed as follows:

其中，第i个水下机器人R_i在编队坐标系O_r-x_ry_rz_r下的状态向量为s_i＝[x_ie,y_ie,z_ie,v_i,ψ_i,γ_i]^T。Among them, the state vector of the_i -_th underwater robot Ri in the formation coordinate system_Or -x_r y_r z_r is s_i =[x_ie ,y_ie ,z_ie ,vi ,ψ_i ,γ_i ]^T.3.根据权利要求2所述的水下机器人三维编队及避障方法，其特征在于，步骤(S2)无障碍自由环境下，基于Lyapunov稳定性原理计算各水下机器人的参考状态向量的方法为：3. underwater robot three-dimensional formation according to claim 2 and obstacle avoidance method, it is characterized in that, under step (S2) barrier-free free environment, the method for calculating the reference state vector of each underwater robot based on Lyapunov stability principle is: :定义Lyapunov距离函数：Define the Lyapunov distance function:

对上式求导，得到：Taking the derivative of the above formula, we get:

其中，

β₁、β₂、β₃为大于0的常系数；v_i、ψ_i、γ_i分别作为水下机器人R_i的期望速率v_ir、艏向角ψ_ir、纵倾角γ_ir，期望编队误差为0，得到自由环境下第i个水下机器人R_i在编队坐标系O_r-x_ry_rz_r下的参考状态向量为s_ir＝[0,0,0,v_ir,ψ_ir,γ_ir]^T。in,

β₁ , β₂ , β₃ are constant coefficients greater than 0; vi , ψ_i , γ_i are the expected velocity_{vir , the heading angle ψ ir , the pitch angle γ irofthe} underwater robot Ri_, respectively_, and the expected formation error is 0, the reference state vector of the_i -th underwater robot Ri in the formation coordinate system_Or -x_r y_r z_r in the free environment is obtained as s_ir =[0,0,0,v_ir ,ψ_ir , γ_ir ]^T .

Images