[go: up one dir, main page]

CN110879600A - A Coordinated Control Method for Multi-Underwater Robot System Based on Distributed Predictive Control - Google Patents

A Coordinated Control Method for Multi-Underwater Robot System Based on Distributed Predictive Control Download PDF

Info

Publication number
CN110879600A
CN110879600A CN201911223257.2A CN201911223257A CN110879600A CN 110879600 A CN110879600 A CN 110879600A CN 201911223257 A CN201911223257 A CN 201911223257A CN 110879600 A CN110879600 A CN 110879600A
Authority
CN
China
Prior art keywords
underwater robot
underwater
robot system
module
target object
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.)
Pending
Application number
CN201911223257.2A
Other languages
Chinese (zh)
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.)
Nanjing Institute of Technology
Original Assignee
Nanjing Institute of Technology
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 Nanjing Institute of Technology filed Critical Nanjing Institute of Technology
Priority to CN201911223257.2A priority Critical patent/CN110879600A/en
Publication of CN110879600A publication Critical patent/CN110879600A/en
Pending legal-status Critical Current

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)
  • Manipulator (AREA)

Abstract

本发明公开了一种基于分布式预测控制的多水下机器人系统协调控制方法，属于机器人技术领域，建立多水下机器人系统、数据采集服务器和控制策略服务器，解决了在带有静态障碍物的受限工作空间中，多水下机器人系统的分布式协调控制策略的技术问题，本发明提出了一个分布式非线性模型预测控制系统NMPC，实现在受限工作空间中让N个水下机器人系统牢固地抓住一个目标物体，通过利用机器人与物体之间的耦合动力学并使用一定的负载分配系数，将水下机器人从初始位置导航到最终位置，通过协调控制策略控制水下机器人将其沿着在工作空间中计算出的路径运动。

Figure 201911223257

The invention discloses a coordinated control method for a multi-underwater robot system based on distributed predictive control, belonging to the field of robotics technology. The invention establishes a multi-underwater robot system, a data acquisition server and a control strategy server, and solves the problem of a multi-underwater robot system with static obstacles. In the restricted working space, the technical problem of the distributed coordinated control strategy of the multi-underwater robot system, the present invention proposes a distributed nonlinear model predictive control system NMPC , which realizes N underwater robot systems in the restricted working space. Firmly grasp a target object, navigate the underwater robot from the initial position to the final position by utilizing the coupled dynamics between the robot and the object and use a certain load distribution coefficient, and control the underwater robot to move it along the path through a coordinated control strategy. moves along the path calculated in the workspace.

Figure 201911223257

Description

一种基于分布式预测控制的多水下机器人系统协调控制方法A Coordinated Control Method for Multi-Underwater Robot System Based on Distributed Predictive Control

技术领域technical field

本发明属于机器人技术领域，涉及一种基于分布式预测控制的多水下机器人系统协调控制方法。The invention belongs to the technical field of robots, and relates to a coordinated control method for multiple underwater robot systems based on distributed predictive control.

背景技术Background technique

水下机器人已广泛用于各种应用，例如海洋科学、船舶维护，油气设施检查等。多个水下机器人系统协同控制，对群体控制方法要求高效准确。另一方面，多水下机器人工作条件苛刻，最困难的是严格的通信约束要求，由于水下声学设备的带宽和通信速率有限，因此在水下环境中采用基于通信的控制结构可能会导致严重的性能问题。此外，由于声音通信设备的带宽较窄，多水下机器人系统的个体数量受到严格限制。Underwater robots have been widely used in various applications such as marine science, ship maintenance, inspection of oil and gas facilities, etc. The collaborative control of multiple underwater robot systems requires efficient and accurate group control methods. On the other hand, the working conditions of multiple underwater robots are harsh, and the most difficult one is the strict communication constraint requirements. Due to the limited bandwidth and communication rate of underwater acoustic devices, the adoption of communication-based control structures in the underwater environment may lead to serious problems. performance issues. In addition, due to the narrow bandwidth of acoustic communication devices, the number of individuals in multiple underwater robot systems is strictly limited.

对水下机器人协调控制分为集中式控制策略和分布式控制策略。集中式控制策略效率高，但是稳定性较差，随着水下机器人的数量增多，系统的复杂性也迅速增加。而分布式控制策略通常依靠于多水下机器人之间的数据通信，通过在多水下机器人之间传输数据来协调实现工作空间速度，但是由于通信带宽的限制，参与协同水下工作的水下机器人数量也受到限制。Coordinated control of underwater robots is divided into centralized control strategy and distributed control strategy. The centralized control strategy has high efficiency, but poor stability. With the increase of the number of underwater robots, the complexity of the system also increases rapidly. The distributed control strategy usually relies on the data communication between multiple underwater robots, and the speed of the workspace is achieved by coordinating the transmission of data among the multiple underwater robots. The number of bots is also limited.

发明内容SUMMARY OF THE INVENTION

本发明的目的是提供一种基于分布式预测控制的多水下机器人系统协调控制方法，解决了在带有静态障碍物的受限工作空间中，多水下机器人系统的分布式协调控制策略的技术问题。The purpose of the present invention is to provide a coordinated control method for multiple underwater robot systems based on distributed predictive control, which solves the problem of the distributed coordinated control strategy of multiple underwater robot systems in a restricted workspace with static obstacles. technical problem.

为实现上述目的，本发明采用如下技术方案：To achieve the above object, the present invention adopts the following technical solutions:

一种基于分布式预测控制的多水下机器人系统协调控制方法，包括如下步骤：A coordinated control method for multiple underwater robot systems based on distributed predictive control, comprising the following steps:

步骤1：建立多水下机器人系统、数据采集服务器和控制策略服务器，多水下机器人系统包括多个水下机器人系统，每一个水下机器人系统均设有水下机器人传感器组，水下机器人传感器组包括惯性测量单元IMU，超短基线定位系统USBL和多普勒测速仪DVL，水下机器人传感器组将采集到的数据全部传输到数据采集服务器进行数据融合，最终生成水下机器人的位置信息和速度信息；Step 1: Establish a multi-underwater robot system, a data acquisition server and a control strategy server. The multi-underwater robot system includes multiple underwater robot systems. Each underwater robot system is provided with an underwater robot sensor group and an underwater robot sensor. The group includes the inertial measurement unit IMU, the ultra-short baseline positioning system USBL and the Doppler velocimeter DVL. The underwater robot sensor group transmits all the collected data to the data acquisition server for data fusion, and finally generates the position information and speed information;

步骤2：在控制策略服务器中建立工作区模块、约束模块、导航模块、控制策略生成模块和数学建模模块；Step 2: establish a workspace module, a constraint module, a navigation module, a control strategy generation module and a mathematical modeling module in the control strategy server;

步骤3：数据采集服务器预设采样时刻，在每一个采样时刻，数据采集服务器就采集一次水下机器人传感器组的数据，并生成水下机器人的位置信息和速度信息，数学建模模块读取水下机器人的位置信息和速度信息，根据位置信息和速度信息建立每个水下机器人系统的分布式动力学数学模型；Step 3: The data collection server presets the sampling time. At each sampling time, the data collection server collects the data of the underwater robot sensor group once, and generates the position information and speed information of the underwater robot, and the mathematical modeling module reads the water. According to the position information and speed information of the robot, the distributed dynamic mathematical model of each underwater robot system is established according to the position information and speed information;

步骤4：工作区模块对障碍物、水下机器人和工作空间均采用球体世界表示法进行球体建模，生成工作空间W：Step 4: The workspace module uses the spherical world representation to model obstacles, underwater robots and workspaces to generate a workspace W:

步骤A1：设定B(xO,r0)是一个封闭的球体，覆盖物体的体积，半径为r0；Step A1: Set B(x O , r 0 ) to be a closed sphere covering the volume of the object with a radius of r 0 ;

步骤A2：定义以每个水下机器人系统的末端执行器为中心的闭合球体

Figure BDA0002301438430000021
该闭合球体
Figure BDA0002301438430000022
涵盖了所有可能配置的水下机器人体积；Step A2: Define a closed sphere centered on the end effector of each underwater robot system
Figure BDA0002301438430000021
the closed sphere
Figure BDA0002301438430000022
Covers all possible configurations of underwater robot volumes;

步骤A3：定义目标物体，目标物体上的抓取点之间的距离至少为

Figure BDA0002301438430000023
Step A3: Define the target object, the distance between the grab points on the target object is at least
Figure BDA0002301438430000023

步骤A4：定义位于半径为

Figure BDA0002301438430000031
的x0处的球区域B(xO,R)，在球区域B(xO,R)中包含了多水下机器人系统的体积和目标物体的体积；Step A4: Define where the radius is
Figure BDA0002301438430000031
The sphere area B(x O , R) at x 0 contains the volume of the multi-underwater robot system and the volume of the target object in the sphere area B(x O , R);

步骤A5：将球区域B(xO,R)中存在的M个静态障碍定义为由

Figure BDA0002301438430000032
其中
Figure BDA0002301438430000033
是中心，
Figure BDA0002301438430000034
是障碍物πm的半径，M取值正整数；Step A5: Define the M static obstacles existing in the ball area B(x O , R) as
Figure BDA0002301438430000032
in
Figure BDA0002301438430000033
is the center,
Figure BDA0002301438430000034
is the radius of the obstacle π m , and M is a positive integer;

步骤5：在工作空间W中，导航模块根据以下公式设计出目标物体的期望运动轨迹和速度：Step 5: In the workspace W, the navigation module designs the desired motion trajectory and speed of the target object according to the following formula:

Figure BDA0002301438430000035
Figure BDA0002301438430000035

其中

Figure BDA0002301438430000036
表示在工作空间
Figure BDA0002301438430000037
内得出安全运动矢量场的电势，k是设计常数且k＞1，
Figure BDA0002301438430000038
表示对目标位置
Figure BDA0002301438430000039
的吸引势场且γ(0)＝0；in
Figure BDA0002301438430000036
represented in the workspace
Figure BDA0002301438430000037
The electric potential of the safe motion vector field is obtained within, k is a design constant and k > 1,
Figure BDA0002301438430000038
Indicates the target location
Figure BDA0002301438430000039
The attractive potential field of and γ(0)=0;

目标物体的所需运动轨迹路径设计如下：The required motion trajectory path of the target object is designed as follows:

Figure BDA00023014384300000310
Figure BDA00023014384300000310

其中KNF为正增益。定义一个采样时刻tj的序列，其中采样时刻为h，h∈(0,TP)为常数：tj+1＝tj+h；where KNF is the positive gain. Define a sequence of sampling time t j , where the sampling time is h, h∈(0, T P ) is a constant: t j+1 =t j +h;

xO(tj)是给定目标物体在时间tj的当前位置，vO(tj)是给定目标物体在时间tj的当前速度，每个水下机器人系统可传播的时间间隔S∈[tj,tj+TP]，其中TP是预测范围；x O (t j ) is the current position of the given target object at time t j , v O (t j ) is the current velocity of the given target object at time t j , the time interval S that each underwater robot system can propagate ∈[t j ,t j +T P ], where T P is the prediction horizon;

步骤6：约束模块为每个水下机器人系统提出一组系统状态约束集Xi；Step 6: The constraint module proposes a set of system state constraints X i for each underwater robot system;

步骤7：在约束模块中规定每个水下机器人系统的输入约束为：

Figure BDA0002301438430000041
和
Figure BDA0002301438430000042
其中τi是一个矢量，包括每个受驱动关节
Figure BDA0002301438430000043
的相应极限范围，
Figure BDA0002301438430000044
τn是被激活的关节数；Step 7: Specify the input constraints of each underwater robot system in the constraint module as:
Figure BDA0002301438430000041
and
Figure BDA0002301438430000042
where τ i is a vector including each driven joint
Figure BDA0002301438430000043
The corresponding limit range of ,
Figure BDA0002301438430000044
τ n is the number of activated joints;

定义水下机器人系统控制输入约束集为Ti：Define the control input constraint set of the underwater robot system as T i :

τi(t)∈Ti；τ i (t)∈T i ;

其中：

Figure BDA0002301438430000045
in:
Figure BDA0002301438430000045

步骤8：控制策略生成模块根据以下方法生成最优控制输入轨迹：Step 8: The control strategy generation module generates the optimal control input trajectory according to the following method:

步骤B1：设定当前状态测量值为xi(tj)，离散采样时刻为tj；Step B1: set the current state measurement value as x i (t j ), and the discrete sampling time as t j ;

步骤B2：对于多水下机器人系统，在采样时刻之间施加的开环输入信号由以下公式求解给出：Step B2: For multiple underwater robot systems, the open-loop input signal applied between sampling moments is given by the following equation:

Figure BDA0002301438430000046
Figure BDA0002301438430000046

其中：in:

Figure BDA0002301438430000047
Figure BDA0002301438430000047

公式中：formula:

F和E分别是运行和终端成本函数，都是二次型形式，即

Figure BDA0002301438430000048
和
Figure BDA0002301438430000049
Px、Qx、Qv和R分别是要进行适当调整的正定矩阵，
Figure BDA00023014384300000410
是在时间实例tj处的状态，
Figure BDA00023014384300000411
是输入轨迹，
Figure BDA00023014384300000412
是最优控制输入轨迹；F and E are the running and terminal cost functions, respectively, both in quadratic form, i.e.
Figure BDA0002301438430000048
and
Figure BDA0002301438430000049
P x , Q x , Q v and R are positive definite matrices to be properly adjusted, respectively,
Figure BDA00023014384300000410
is the state at time instance t j ,
Figure BDA00023014384300000411
is the input trajectory,
Figure BDA00023014384300000412
is the optimal control input trajectory;

步骤B3：控制策略生成模块将最优控制输入轨迹发送给多水下机器人系统；Step B3: The control strategy generation module sends the optimal control input trajectory to the multi-underwater robot system;

步骤9：在下一个采样时刻tj+1，tj+1＝tj+h，根据步骤3到步骤8的方法继续生成在采样时刻tj+1时的最优控制输入轨迹。Step 9: At the next sampling time t j+1 , t j+1 =t j +h, continue to generate the optimal control input trajectory at the sampling time t j+1 according to the methods from steps 3 to 8 .

优选的，在执行步骤3时，所述数学建模模块根据以下公式建立每个水下机器人系统的分布式动力学数学模型：Preferably, when performing step 3, the mathematical modeling module establishes a distributed dynamic mathematical model of each underwater robot system according to the following formula:

Figure BDA0002301438430000051
Figure BDA0002301438430000051

其中，in,

Figure BDA0002301438430000052
Figure BDA0002301438430000052

Figure BDA0002301438430000053
Figure BDA0002301438430000053

Figure BDA0002301438430000054
Figure BDA0002301438430000054

优选的，在执行步骤6时，所述系统状态约束集Xi包括多个约束项，具体表现为：Preferably, when step 6 is executed, the system state constraint set X i includes multiple constraint items, which are specifically represented as:

这些约束由以下公式表示：These constraints are represented by the following formulas:

xi(t)∈Xi；其中，Xi由以下约束组成：x i (t)∈X i ; where X i consists of the following constraints:

Figure BDA0002301438430000055
Figure BDA0002301438430000055

其中，

Figure BDA0002301438430000056
是多水下机器人系统的奇异位置集合，
Figure BDA0002301438430000057
是机械手的关节极限集合：in,
Figure BDA0002301438430000056
is a collection of singular positions of multiple underwater robot systems,
Figure BDA0002301438430000057
is the set of joint limits of the manipulator:

Figure BDA0002301438430000058
Figure BDA0002301438430000058

其中，

Figure BDA0002301438430000061
是对应关节
Figure BDA0002301438430000062
的极限范围，
Figure BDA0002301438430000063
是对应关节
Figure BDA0002301438430000064
的关节速度，集合Xi集中了多水下机器人系统的所有状态约束。in,
Figure BDA0002301438430000061
is the corresponding joint
Figure BDA0002301438430000062
limit range,
Figure BDA0002301438430000063
is the corresponding joint
Figure BDA0002301438430000064
The joint velocities of , the set Xi gathers all the state constraints of the multi-AUV system.

本发明所述的一种基于分布式预测控制的多水下机器人系统协调控制方法，解决了在带有静态障碍物的受限工作空间中，多水下机器人系统的分布式协调控制策略的技术问题，本发明提出了一个分布式非线性模型预测控制系统NMPC，实现在受限工作空间中让N个水下机器人系统牢固地抓住一个目标物体，通过利用机器人与物体之间的耦合动力学并使用一定的负载分配系数，将水下机器人从初始位置导航到最终位置，通过协调控制策略控制水下机器人将其沿着在工作空间中计算出的路径运动，本发明所提出的控制策略可以使多水下机器人系统在协作水下工作执行期间减少的机器人之间通信数据，从而减少了所需的通信带宽，提高了多水下机器人系统工作效率。The coordinated control method for multiple underwater robot systems based on distributed predictive control described in the present invention solves the technology of the distributed coordinated control strategy of multiple underwater robot systems in a restricted working space with static obstacles. Problem, the present invention proposes a distributed nonlinear model predictive control system NMPC, which enables N underwater robot systems to firmly grasp a target object in a restricted workspace, by utilizing the coupled dynamics between the robot and the object And use a certain load distribution coefficient to navigate the underwater robot from the initial position to the final position, and control the underwater robot to move along the path calculated in the workspace through a coordinated control strategy. The control strategy proposed by the present invention can Communication data between robots is reduced during the execution of cooperative underwater work by the multi-underwater robot system, thereby reducing the required communication bandwidth and improving the work efficiency of the multi-underwater robot system.

附图说明Description of drawings

图1为本发明的多水下机器人系统运行轨迹示意图。FIG. 1 is a schematic diagram of the running trajectory of the multi-underwater robot system of the present invention.

具体实施方式Detailed ways

如图1所示的一种基于分布式预测控制的多水下机器人系统协调控制方法，包括如下步骤：As shown in Figure 1, a coordinated control method for multiple underwater robot systems based on distributed predictive control includes the following steps:

步骤1：建立多水下机器人系统、数据采集服务器和控制策略服务器，多水下机器人系统包括多个水下机器人系统，每一个水下机器人系统均设有水下机器人传感器组，水下机器人传感器组包括惯性测量单元IMU，超短基线定位系统USBL和多普勒测速仪DVL，水下机器人传感器组将采集到的数据全部传输到数据采集服务器进行数据融合，最终生成水下机器人的位置信息和速度信息；Step 1: Establish a multi-underwater robot system, a data acquisition server and a control strategy server. The multi-underwater robot system includes multiple underwater robot systems. Each underwater robot system is provided with an underwater robot sensor group and an underwater robot sensor. The group includes the inertial measurement unit IMU, the ultra-short baseline positioning system USBL and the Doppler velocimeter DVL. The underwater robot sensor group transmits all the collected data to the data acquisition server for data fusion, and finally generates the position information and speed information;

步骤2：在控制策略服务器中建立工作区模块、约束模块、导航模块、控制策略生成模块和数学建模模块；Step 2: establish a workspace module, a constraint module, a navigation module, a control strategy generation module and a mathematical modeling module in the control strategy server;

步骤3：数据采集服务器预设采样时刻，在每一个采样时刻，数据采集服务器就采集一次水下机器人传感器组的数据，并生成水下机器人的位置信息和速度信息，数学建模模块读取水下机器人的位置信息和速度信息，根据位置信息和速度信息建立每个水下机器人系统的分布式动力学数学模型；Step 3: The data collection server presets the sampling time. At each sampling time, the data collection server collects the data of the underwater robot sensor group once, and generates the position information and speed information of the underwater robot, and the mathematical modeling module reads the water. According to the position information and speed information of the robot, the distributed dynamic mathematical model of each underwater robot system is established according to the position information and speed information;

针对在有界工作空间W∈R3中操作的N个水下机器人系统，首先，用

Figure BDA0002301438430000071
表示每个水下机器人系统的末端效应器的坐标，其
Figure BDA0002301438430000072
中和
Figure BDA0002301438430000073
是以欧拉角表示w.r.t惯性坐标系的位置和方向。设
Figure BDA0002301438430000074
是每个水下机器人系统的关节状态变量，其中
Figure BDA0002301438430000075
是包含位置
Figure BDA0002301438430000076
和水下机器人方向
Figure BDA0002301438430000077
的向量，Bi是操纵器关节角位置的向量。具体来说，
Figure BDA0002301438430000078
和
Figure BDA0002301438430000079
是以欧拉角表示w.r.t惯性坐标系的位置和方向。还可以用
Figure BDA00023014384300000710
来定义水下机器人系统的末端效应器广义速度，其中
Figure BDA00023014384300000711
和
Figure BDA00023014384300000712
分别表示线性速度和角速度。For N underwater robot systems operating in a bounded workspace W ∈ R 3 , first, use
Figure BDA0002301438430000071
represent the coordinates of the end-effector of each underwater robot system, which
Figure BDA0002301438430000072
neutralize
Figure BDA0002301438430000073
is the Euler angle to represent the position and orientation of the wrt inertial coordinate system. Assume
Figure BDA0002301438430000074
are the joint state variables of each underwater robot system, where
Figure BDA0002301438430000075
is the include location
Figure BDA0002301438430000076
and the direction of the underwater robot
Figure BDA0002301438430000077
, and B i is the vector of the angular positions of the manipulator joints. Specifically,
Figure BDA0002301438430000078
and
Figure BDA0002301438430000079
is the Euler angle to represent the position and orientation of the wrt inertial coordinate system. can also be used
Figure BDA00023014384300000710
to define the end-effector generalized velocity of the underwater robot system, where
Figure BDA00023014384300000711
and
Figure BDA00023014384300000712
represent the linear and angular velocities, respectively.

优选的，在执行步骤3时，所述数学建模模块根据以下公式建立每个水下机器人系统的分布式动力学数学模型：Preferably, when performing step 3, the mathematical modeling module establishes a distributed dynamic mathematical model of each underwater robot system according to the following formula:

Figure BDA00023014384300000713
Figure BDA00023014384300000713

其中，in,

Figure BDA00023014384300000714
Figure BDA00023014384300000714

Figure BDA00023014384300000715
Figure BDA00023014384300000715

Figure BDA00023014384300000716
Figure BDA00023014384300000716

Figure BDA0002301438430000081
是水下机器人系统的分布式动力学数学模型，
Figure BDA0002301438430000082
xi表示水下机器人的坐标位置，其中
Figure BDA0002301438430000083
和qi是局部测量坐标值，ui表示控制变量；
Figure BDA0002301438430000081
is the distributed dynamic mathematical model of the underwater robot system,
Figure BDA0002301438430000082
x i represents the coordinate position of the underwater robot, where
Figure BDA0002301438430000083
and qi are the local measurement coordinate values, and ui represents the control variable;

Figure BDA0002301438430000084
是水下机器人系统耦合动力学方程，其中
Figure BDA0002301438430000085
是科里奥利矩阵，
Figure BDA0002301438430000086
是重力和浮力效应的向量，
Figure BDA0002301438430000087
是耗散效应模型，
Figure BDA0002301438430000088
是将欧拉角速率转换成速的目标物体表示雅可比定律。
Figure BDA0002301438430000084
is the coupled dynamics equation of the underwater robot system, where
Figure BDA0002301438430000085
is the Coriolis matrix,
Figure BDA0002301438430000086
are the vectors of gravity and buoyancy effects,
Figure BDA0002301438430000087
is the dissipation effect model,
Figure BDA0002301438430000088
is the target object that converts the Euler angular rate to velocity representing Jacobi's law.

Figure BDA0002301438430000089
其中
Figure BDA00023014384300000810
是正定惯性矩阵。
Figure BDA0002301438430000089
in
Figure BDA00023014384300000810
is the positive definite inertia matrix.

本发明中，首先，制定多水下机器人系统的整体动力学，然后通过使用一定的负载分配系数实现在目标物体和水下机器人之间解耦。每个水下机器人系统在每个采样时间根据其整体动力学的相应部分和许多不等式约束来求解NMPC，水下机器人协同驱动目标物体并沿着工作空间内运动，通过计算出的可行路径引导水下机器人，可行路径的计算基于多水下机器人系统的导航功能。In the present invention, firstly, the overall dynamics of the multiple underwater robot system is formulated, and then the decoupling between the target object and the underwater robot is realized by using a certain load distribution coefficient. Each underwater robot system solves the NMPC according to the corresponding part of its overall dynamics and many inequality constraints at each sampling time, the underwater robot cooperatively drives the target object and moves along the workspace, guiding the water through the calculated feasible path Under the robot, the calculation of the feasible path is based on the navigation function of the multi-underwater robot system.

步骤4：工作区模块对障碍物、水下机器人和工作空间均采用球体世界表示法进行球体建模，生成工作空间W：Step 4: The workspace module uses the spherical world representation to model obstacles, underwater robots and workspaces to generate a workspace W:

步骤A1：设定B(xO,r0)是一个封闭的球体，覆盖物体的体积，半径为r0；Step A1: Set B(x O , r 0 ) to be a closed sphere covering the volume of the object with a radius of r 0 ;

步骤A2：定义以每个水下机器人系统的末端执行器为中心的闭合球体

Figure BDA00023014384300000811
该闭合球体
Figure BDA00023014384300000812
涵盖了所有可能配置的水下机器人体积；Step A2: Define a closed sphere centered on the end effector of each underwater robot system
Figure BDA00023014384300000811
the closed sphere
Figure BDA00023014384300000812
Covers all possible configurations of underwater robot volumes;

步骤A3：定义目标物体，目标物体上的抓取点之间的距离至少为

Figure BDA0002301438430000091
Step A3: Define the target object, the distance between the grab points on the target object is at least
Figure BDA0002301438430000091

本实施例中，

Figure BDA0002301438430000092
为每个水下机器人末端执行器的工作范围，目标物体上会有多个抓取点，其中任意两个抓取点之间距离至少为
Figure BDA0002301438430000093
In this embodiment,
Figure BDA0002301438430000092
For the working range of the end effector of each underwater robot, there will be multiple grab points on the target object, and the distance between any two grab points is at least
Figure BDA0002301438430000093

步骤A4：定义位于半径为

Figure BDA0002301438430000094
的x0处的球区域B(xO,R)，在球区域B(xO,R)中包含了多水下机器人系统的体积和目标物体的体积；Step A4: Define where the radius is
Figure BDA0002301438430000094
The sphere area B(x O , R) at x 0 contains the volume of the multi-underwater robot system and the volume of the target object in the sphere area B(x O , R);

步骤A5：将球区域B(xO,R)中存在的M个静态障碍定义为由

Figure BDA0002301438430000095
其中
Figure BDA0002301438430000096
是中心，
Figure BDA0002301438430000097
是障碍物πm的半径，M取值正整数；Step A5: Define the M static obstacles existing in the ball area B(x O , R) as
Figure BDA0002301438430000095
in
Figure BDA0002301438430000096
is the center,
Figure BDA0002301438430000097
is the radius of the obstacle π m , and M is a positive integer;

步骤5：在工作空间W中，导航模块根据以下公式设计出目标物体的期望运动轨迹和速度：Step 5: In the workspace W, the navigation module designs the desired motion trajectory and speed of the target object according to the following formula:

Figure BDA0002301438430000098
Figure BDA0002301438430000098

其中

Figure BDA0002301438430000099
表示在工作空间
Figure BDA00023014384300000910
内得出安全运动矢量场的电势，k是设计常数且k＞1，
Figure BDA00023014384300000911
表示对目标位置
Figure BDA00023014384300000912
的吸引势场且γ(0)＝0；in
Figure BDA0002301438430000099
represented in the workspace
Figure BDA00023014384300000910
The electric potential of the safe motion vector field is obtained within, k is a design constant and k > 1,
Figure BDA00023014384300000911
Indicates the target location
Figure BDA00023014384300000912
The attractive potential field of and γ(0)=0;

目标物体的所需运动轨迹路径设计如下：The required motion trajectory path of the target object is designed as follows:

Figure BDA00023014384300000913
Figure BDA00023014384300000913

其中KNF为正增益。定义一个采样时刻tj的序列，其中采样时刻为h，h∈(0,TP)为常数：tj+1＝tj+h；where KNF is the positive gain. Define a sequence of sampling time t j , where the sampling time is h, h∈(0, T P ) is a constant: t j+1 =t j +h;

xO(tj)是给定目标物体在时间tj的当前位置，vO(tj)是给定目标物体在时间tj的当前速度，每个水下机器人系统可传播的时间间隔S∈[tj,tj+TP]，其中TP是预测范围；x O (t j ) is the current position of the given target object at time t j , v O (t j ) is the current velocity of the given target object at time t j , the time interval S that each underwater robot system can propagate ∈[t j ,t j +T P ], where T P is the prediction horizon;

步骤6：约束模块为每个水下机器人系统提出一组系统状态约束集Xi，这些约束由以下公式表示：Step 6: The constraint module proposes a set of system state constraints X i for each underwater robot system, these constraints are represented by the following formulas:

xi(t)∈Xi；其中，Xi由以下约束组成：x i (t)∈X i ; where X i consists of the following constraints:

Figure BDA0002301438430000101
Figure BDA0002301438430000101

其中，

Figure BDA0002301438430000102
是多水下机器人系统的奇异位置集合，
Figure BDA0002301438430000103
是机械手的关节极限集合：in,
Figure BDA0002301438430000102
is a collection of singular positions of multiple underwater robot systems,
Figure BDA0002301438430000103
is the set of joint limits of the manipulator:

Figure BDA0002301438430000104
Figure BDA0002301438430000104

其中，

Figure BDA0002301438430000105
是对应关节
Figure BDA0002301438430000106
的极限范围，
Figure BDA0002301438430000107
是对应关节
Figure BDA0002301438430000108
的关节速度，集合Xi集中了多水下机器人系统的所有状态约束；in,
Figure BDA0002301438430000105
is the corresponding joint
Figure BDA0002301438430000106
limit range,
Figure BDA0002301438430000107
is the corresponding joint
Figure BDA0002301438430000108
The joint velocities of , the set X i concentrates all the state constraints of the multi-underwater robot system;

步骤7：在约束模块中规定每个水下机器人系统的输入约束为：

Figure BDA0002301438430000109
和
Figure BDA00023014384300001010
其中τi是一个矢量，包括每个受驱动关节
Figure BDA00023014384300001011
的相应极限范围，
Figure BDA00023014384300001012
τn是被激活的关节数；Step 7: Specify the input constraints of each underwater robot system in the constraint module as:
Figure BDA0002301438430000109
and
Figure BDA00023014384300001010
where τ i is a vector including each driven joint
Figure BDA00023014384300001011
The corresponding limit range of ,
Figure BDA00023014384300001012
τ n is the number of activated joints;

定义水下机器人系统控制输入约束集为Ti：Define the control input constraint set of the underwater robot system as T i :

τi(t)∈Ti；τ i (t)∈T i ;

其中：

Figure BDA0002301438430000111
in:
Figure BDA0002301438430000111

本发明在每个采样时间内，水下机器人系统通过数学建模模块得到的分布式动力学数学模型，再通过约束模块，求解NMPC控制策略方案，在通过导航模块求解时间间隔s∈[tj,tj+TP]的轨迹

Figure BDA0002301438430000112
和速度
Figure BDA0002301438430000113
最后在通过以下步骤8制定最优控制输入轨迹。In the present invention, in each sampling time, the underwater robot system obtains the distributed dynamic mathematical model through the mathematical modeling module, and then through the constraint module, solves the NMPC control strategy scheme, and solves the time interval s ∈ [t j through the navigation module ,t j +T P ] trajectory
Figure BDA0002301438430000112
and speed
Figure BDA0002301438430000113
Finally, the optimal control input trajectory is formulated through the following step 8.

步骤8：控制策略生成模块根据以下方法生成最优控制输入轨迹：Step 8: The control strategy generation module generates the optimal control input trajectory according to the following method:

步骤B1：设定当前状态测量值为xi(tj)，离散采样时刻为tj；Step B1: set the current state measurement value as x i (t j ), and the discrete sampling time as t j ;

步骤B2：对于多水下机器人系统，在采样时刻之间施加的开环输入信号由以下公式求解给出：Step B2: For multiple underwater robot systems, the open-loop input signal applied between sampling moments is given by the following equation:

Figure BDA0002301438430000114
Figure BDA0002301438430000114

其中：in:

Figure BDA0002301438430000115
Figure BDA0002301438430000115

公式中：formula:

F和E分别是运行和终端成本函数，都是二次型形式，即

Figure BDA0002301438430000116
和
Figure BDA0002301438430000117
Px、Qx、Qv和R分别是要进行适当调整的正定矩阵，
Figure BDA0002301438430000118
是在时间实例tj处的状态，
Figure BDA0002301438430000119
是输入轨迹，
Figure BDA00023014384300001110
是最优控制输入轨迹；F and E are the running and terminal cost functions, respectively, both in quadratic form, i.e.
Figure BDA0002301438430000116
and
Figure BDA0002301438430000117
P x , Q x , Q v and R are positive definite matrices to be properly adjusted, respectively,
Figure BDA0002301438430000118
is the state at time instance t j ,
Figure BDA0002301438430000119
is the input trajectory,
Figure BDA00023014384300001110
is the optimal control input trajectory;

步骤B3：控制策略生成模块将最优控制输入轨迹发送给多水下机器人系统；Step B3: The control strategy generation module sends the optimal control input trajectory to the multi-underwater robot system;

步骤9：在下一个采样时刻tj+1，tj+1＝tj+h，根据步骤3到步骤8的方法继续生成在采样时刻tj+1时的最优控制输入轨迹。Step 9: At the next sampling time t j+1 , t j+1 =t j +h, continue to generate the optimal control input trajectory at the sampling time t j+1 according to the methods from steps 3 to 8 .

本发明的控制输入τi具有反馈形式，可以根据每个采样时刻的当前状态重新计算。The control input τ i of the present invention has a feedback form and can be recalculated according to the current state at each sampling moment.

本发明所述的一种基于分布式预测控制的多水下机器人系统协调控制方法，解决了在带有静态障碍物的受限工作空间中，多水下机器人系统的分布式协调控制策略的技术问题，本发明提出了一个分布式非线性模型预测控制系统NMPC，实现在受限工作空间中让N个水下机器人系统牢固地抓住一个目标物体，通过利用机器人与物体之间的耦合动力学并使用一定的负载分配系数，将水下机器人从初始位置导航到最终位置，通过协调控制策略控制水下机器人将其沿着在工作空间中计算出的路径运动，本发明所提出的控制策略可以使多水下机器人系统在协作水下工作执行期间减少的机器人之间通信数据，从而减少了所需的通信带宽，提高了多水下机器人系统工作效率。The coordinated control method for multiple underwater robot systems based on distributed predictive control described in the present invention solves the technology of the distributed coordinated control strategy of multiple underwater robot systems in a restricted working space with static obstacles. Problem, the present invention proposes a distributed nonlinear model predictive control system NMPC, which enables N underwater robot systems to firmly grasp a target object in a restricted workspace, by utilizing the coupled dynamics between the robot and the object And use a certain load distribution coefficient to navigate the underwater robot from the initial position to the final position, and control the underwater robot to move along the path calculated in the workspace through a coordinated control strategy. The control strategy proposed by the present invention can Communication data between robots is reduced during the execution of cooperative underwater work by the multi-underwater robot system, thereby reducing the required communication bandwidth and improving the work efficiency of the multi-underwater robot system.

Claims (3)

1. A multi-underwater robot system coordination control method based on distributed predictive control is characterized in that: the method comprises the following steps:
step 1: establishing a multi-underwater robot system, a data acquisition server and a control strategy server, wherein the multi-underwater robot system comprises a plurality of underwater robot systems, each underwater robot system is provided with an underwater robot sensor group, each underwater robot sensor group comprises an inertial measurement unit IMU, an ultra-short baseline positioning system USBL and a Doppler velocimeter DVL, and the underwater robot sensor groups transmit all acquired data to the data acquisition server for data fusion to finally generate position information and speed information of the underwater robot;
step 2: establishing a working area module, a constraint module, a navigation module, a control strategy generation module and a mathematical modeling module in a control strategy server;
and step 3: the method comprises the steps that a data acquisition server presets sampling moments, at each sampling moment, the data acquisition server acquires data of a primary underwater robot sensor group and generates position information and speed information of an underwater robot, a mathematical modeling module reads the position information and the speed information of the underwater robot, and a distributed dynamic mathematical model of each underwater robot system is established according to the position information and the speed information;
and 4, step 4: the working area module carries out sphere modeling on the obstacles, the underwater robot and the working space by adopting a sphere world representation method to generate a working space W:
step A1: setting B (x)O,r0) Is a closed sphere covering the volume of the object and having a radius r0；
Step A2: defining a closed sphere centered on an end effector of each underwater robotic system
Figure FDA0002301438420000011
The closed sphere
Figure FDA0002301438420000012
The underwater robot volume of all possible configurations is covered;
step A3: defining a target object, the distance between the grabbing points on the target object being at least
Figure FDA0002301438420000021
Step A4: is defined at a radius of
Figure FDA0002301438420000022
X of0Ball region B (x)OR) in the spherical region B (x)OR) comprises the volume of the multi-underwater robot system and the volume of the target object;
step A5: will sphere region B (x)OAnd M static obstacles present in R) are defined as being composed of
Figure FDA0002301438420000023
Wherein
Figure FDA0002301438420000024
Is the center of the image, and the center of the image,
Figure FDA0002301438420000025
is a barrier pimM is a positive integer;
and 5: in the working space W, the navigation module designs the desired motion trajectory and velocity of the target object according to the following formulas:
Figure FDA0002301438420000026
wherein
Figure FDA0002301438420000027
Is represented in a workspace
Figure FDA0002301438420000028
The potential of the safe motion vector field is internally derived, k is a design constant and k > 1,
Figure FDA0002301438420000029
indicating the position of the target
Figure FDA00023014384200000210
And γ (0) is 0;
the required motion trail path of the target object is designed as follows:
Figure FDA00023014384200000211
wherein KNFIs a positive gain. Defining a sampling instant tjWherein the sampling time is h, h e (0, T)P) Is constant: t is tj+1＝tj+h；
xO(tj) Is given target object at time tjCurrent position of vO(tj) Is given target object at time tjCurrent speed of each underwater robotic system, a time interval S e [ t ] that each underwater robotic system can propagatej,tj+TP]Wherein T isPIs the prediction horizon;
step 6: the constraint module provides a group of system state constraint sets X for each underwater robot systemi；
And 7: in a constraint moduleInput constraints of each underwater robot system are determined as follows:
Figure FDA0002301438420000031
and
Figure FDA0002301438420000032
wherein tau isiIs a vector comprising each of the driven joints
Figure FDA0002301438420000033
In the respective limit ranges of (a) and (b),
Figure FDA0002301438420000034
τnis the number of joints that are activated;
defining a control input constraint set of an underwater robot system as Ti：
τi(t)∈Ti；
Wherein:
Figure FDA0002301438420000035
and 8: the control strategy generation module generates an optimal control input track according to the following method:
step B1: setting the current state measurement value to xi(tj) Discrete sampling time tj；
Step B2: for a multi-underwater robotic system, the open-loop input signal applied between sampling instants is given by solving the following equation:
Figure FDA0002301438420000036
wherein:
Figure FDA0002301438420000037
in the formula:
f and E are running and terminal costs, respectivelyThe functions being all of quadratic form, i.e.
Figure FDA0002301438420000038
And
Figure FDA0002301438420000039
Px、Qx、Qvand R are respectively positive definite matrices to be properly adjusted,
Figure FDA00023014384200000310
is at time instance tjIn the state of (a) in (b),
Figure FDA00023014384200000311
is the input trajectory of the input object,
Figure FDA0002301438420000041
is the optimal control input trajectory;
step B3: the control strategy generation module sends the optimal control input track to the multi-underwater robot system;
and step 9: at the next sampling instant tj+1，tj+1＝tj+ h, continuing the generation according to the method from step 3 to step 8 at the sampling instant tj+1The input trajectory is optimally controlled.
2. The coordinated control method of the multi-underwater robot system based on the distributed predictive control as claimed in claim 1, wherein: in performing step 3, the mathematical modeling module builds a distributed dynamical mathematical model for each underwater robotic system according to the following formula:
Figure FDA0002301438420000042
wherein,
Figure FDA0002301438420000043
Figure FDA0002301438420000044
Figure FDA0002301438420000045
3. the coordinated control method of the multi-underwater robot system based on the distributed predictive control as claimed in claim 1, wherein: in step 6, the system state constraint set XiComprising a plurality of constraint terms, embodied as:
these constraints are represented by the following equations:
xi(t)∈Xi(ii) a Wherein, XiConsists of the following constraints:
Figure FDA0002301438420000051
wherein,
Figure FDA0002301438420000052
is a singular set of positions for a multi-underwater robotic system,
Figure FDA0002301438420000053
is the set of joint limits of the manipulator:
Figure FDA0002301438420000054
wherein,
Figure FDA0002301438420000055
is corresponding to the joint
Figure FDA0002301438420000056
In the range of the limit (c) of (c),
Figure FDA0002301438420000057
is corresponding to the joint
Figure FDA0002301438420000058
Joint velocity of (1), set XiAll state constraints of the multi-underwater robotic system are centralized.
CN201911223257.2A 2019-12-03 2019-12-03 A Coordinated Control Method for Multi-Underwater Robot System Based on Distributed Predictive Control Pending CN110879600A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201911223257.2A CN110879600A (en) 2019-12-03 2019-12-03 A Coordinated Control Method for Multi-Underwater Robot System Based on Distributed Predictive Control

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201911223257.2A CN110879600A (en) 2019-12-03 2019-12-03 A Coordinated Control Method for Multi-Underwater Robot System Based on Distributed Predictive Control

Publications (1)

Publication Number Publication Date
CN110879600A true CN110879600A (en) 2020-03-13

Family

ID=69730078

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201911223257.2A Pending CN110879600A (en) 2019-12-03 2019-12-03 A Coordinated Control Method for Multi-Underwater Robot System Based on Distributed Predictive Control

Country Status (1)

Country Link
CN (1) CN110879600A (en)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111730593A (en) * 2020-05-27 2020-10-02 南京工程学院 A model-based multi-axis robot control device and method
CN113741461A (en) * 2021-09-06 2021-12-03 中国人民解放军国防科技大学 Multi-robot obstacle avoidance method in complex scene facing limited communication

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106773689A (en) * 2016-12-16 2017-05-31 西北工业大学 AUV formation cooperative control methods based on layered distribution type Model Predictive Control
CN109343350A (en) * 2018-11-20 2019-02-15 清华大学 A Path Tracking Control Method for Underwater Robots Based on Model Predictive Control

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106773689A (en) * 2016-12-16 2017-05-31 西北工业大学 AUV formation cooperative control methods based on layered distribution type Model Predictive Control
CN109343350A (en) * 2018-11-20 2019-02-15 清华大学 A Path Tracking Control Method for Underwater Robots Based on Model Predictive Control

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
SHAHAB HESHMATI-ALAMDARI 等: "A Distributed Predictive Control Approach for Cooperative Manipulation of Multiple Underwater Vehicle Manipulator Systems", 《2019 INTERNATIONAL CONFERENCE ON ROBOTICS AND AUTOMATION (ICRA)》 *

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111730593A (en) * 2020-05-27 2020-10-02 南京工程学院 A model-based multi-axis robot control device and method
CN113741461A (en) * 2021-09-06 2021-12-03 中国人民解放军国防科技大学 Multi-robot obstacle avoidance method in complex scene facing limited communication
CN113741461B (en) * 2021-09-06 2023-10-03 中国人民解放军国防科技大学 Multi-robot obstacle avoidance method oriented to limited communication under complex scene

Similar Documents

Publication Publication Date Title
US11845186B2 (en) Inverse kinematics solving method for redundant robot and redundant robot and computer readable storage medium using the same
CN106737659B (en) A kind of hand ship control method for coordinating of underwater unmanned vehicle and arm-and-hand system
CN104076689B (en) A kind of full drive-type Autonomous Underwater Vehicle cooperative control method
CN106708069B (en) Coordinated planning and control method of underwater movement robot
CN108319138A (en) A kind of sliding formwork of drive lacking unmanned boat-contragradience double loop Trajectory Tracking Control method
CN114721275B (en) An adaptive tracking control method for visual servo robot based on preset performance
An et al. Task planning and collaboration of jellyfish-inspired multiple spherical underwater robots
CN108663939A (en) Consider the UUV level of constellation face path tracking control method of communication packet loss
CN109634307A (en) Unmanned underwater vehicle composite track tracking control method
CN108267953A (en) One kind is based on pilotage people-follower's underwater robot location tracking method
CN107263466A (en) Pedestal unperturbed control method of the robot for space based on quadratic programming problem
CN115431271B (en) A method for anti-interference pointing control at the end of a flying manipulator
CN112327892A (en) Anti-interference control method with AUV (autonomous Underwater vehicle) error limited
CN110879600A (en) A Coordinated Control Method for Multi-Underwater Robot System Based on Distributed Predictive Control
CN118672287A (en) Hybrid self-adaptive underwater robot track tracking control method
CN115237117A (en) Path Planning Method for Mobile Robot Based on Mutation Adaptive Particle Swarm Optimization
Yu et al. Minimum jerk trajectory generation of a quadrotor based on the differential flatness
CN118219260A (en) A motion control method for a mobile robotic arm in a dynamic scene and its application
Qazani et al. A Real-Time Motion Control Tracking Mechanism for Satellite Tracking Antenna Using Serial Robot
Li et al. An Improved Motion Strategy with Uncertainty Perception for the Underwater Robot Based on Thrust Allocation Model
CN116175585A (en) UDE control method for multi-joint manipulator with input saturation and output constraints
CN112286227B (en) Underwater robot trajectory control method and system based on differential tracker and disturbance observer
Pang et al. A novel time-varying formation obstacle avoidance algorithm for multiple AUVs
Farooq et al. Controlling and stabilisation of remotely operated underwater vehicle
Morton et al. Generalized trajectory control for tree-structured aerial manipulators

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
RJ01 Rejection of invention patent application after publication
RJ01 Rejection of invention patent application after publication

Application publication date: 20200313