CN114779712A - 一种水下机器人控制系统 - Google Patents

一种水下机器人控制系统 Download PDF

Info

Publication number
CN114779712A
CN114779712A CN202210700810.2A CN202210700810A CN114779712A CN 114779712 A CN114779712 A CN 114779712A CN 202210700810 A CN202210700810 A CN 202210700810A CN 114779712 A CN114779712 A CN 114779712A
Authority
CN
China
Prior art keywords
state
underwater robot
module
communication
main control
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202210700810.2A
Other languages
English (en)
Other versions
CN114779712B (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.)
Daoli Zhiyuan Technology Qingdao Co ltd
Original Assignee
Ocean University of China
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Ocean University of China filed Critical Ocean University of China
Priority to CN202210700810.2A priority Critical patent/CN114779712B/zh
Publication of CN114779712A publication Critical patent/CN114779712A/zh
Application granted granted Critical
Publication of CN114779712B publication Critical patent/CN114779712B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B19/00—Programme-control systems
    • G05B19/02—Programme-control systems electric
    • G05B19/04—Programme control other than numerical control, i.e. in sequence controllers or logic controllers
    • G05B19/042—Programme control other than numerical control, i.e. in sequence controllers or logic controllers using digital processors
    • G05B19/0423—Input/output
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B2219/00—Program-control systems
    • G05B2219/20—Pc systems
    • G05B2219/24—Pc safety
    • G05B2219/24215—Scada supervisory control and data acquisition
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02P—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN THE PRODUCTION OR PROCESSING OF GOODS
    • Y02P90/00—Enabling technologies with a potential contribution to greenhouse gas [GHG] emissions mitigation
    • Y02P90/02—Total factory control, e.g. smart factories, flexible manufacturing systems [FMS] or integrated manufacturing systems [IMS]

Landscapes

  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Engineering & Computer Science (AREA)
  • Automation & Control Theory (AREA)
  • Manipulator (AREA)

Abstract

本发明涉及水下机器人控制技术领域，且公开一种水下机器人控制系统，包括：传感器模块；控制器模块，其包括主控单元和FPGA单元；主控单元接收传感器模块检测的信息并对其解析处理，以获取到水下机器人的实际状态信息，主控单元计算水下机器人的参考目标值和实际状态信息之间的误差，作为鲁棒控制器的输入值；FPGA单元包括FPGA逻辑端和HPS端，FPGA逻辑端通过HPS端与主控单元双向通信，且FPGA逻辑端用于并行执行鲁棒控制器的矩阵运算；多个推进器，主控单元接收鲁棒控制器的输出值、根据输出值进行力矩分配、且将所分配力矩对应的多个控制信号分别输出至多个推进器。本发明能够使水下机器人实时且稳定的鲁棒控制。

Description

一种水下机器人控制系统
技术领域
本发明涉及水下机器人控制技术领域，尤其涉及一种水下机器人控制系统。
背景技术
水下机器人是复杂的机械电子综合体，严苛的控制目标对水下机器人控制硬件平台的实时性和可靠性提出了极高的要求。
与陆地机器人不同，水下机器人具有布放难度大、运行作业成本高的特点，因此，水下机器人一般采用压缩空间的紧凑设计方案来降低布放和运维难度。而且为了提高水下机器人的航行作业时间，并增强机器人的观测与作业能力，水下机器人内部的大部分空间被电池和传感器单元所占据，因此，机器人内部空间十分紧张。
由于上述原因，水下机器人的主控单元为了压缩硬件尺寸，降低主控功耗，通常选择功能裁剪的嵌入式计算板卡作为其主控硬件，但是这种主控计算单元通常只有有限的接口和计算能力，许多先进的控制理论方法无法直接有效的应用在这些水下机器人上。此外，采用连续信号表述的控制器从理论算法转变为实际的离散控制器需要硬件平台提供高速的计算能力和准确的时钟输出，例如多输入多输出鲁棒控制算法中存在大量的浮点数矩阵运算，高维度控制器若采用非实时的串行软件编程方式实现，则难以保证实际控制器在准确的时刻产生控制信号，系统控制性能可能出现较大损失。
面对如上问题，设计一套满足实时性要求且稳定可靠的水下机器人控制系统是必要的。
发明内容
为了解决如上技术问题，本发明的实施例提供一种水下机器人控制系统，采用分布式、模块化功能设计，提高控制系统的可维护性及向后拓展能力；利用FPGA单元的并行计算能力，对水下机器人的鲁棒控制器进行实时且快速地运算，实现对水下机器人实时且可靠的稳定控制；采用HPS端在FPGA单元和主控单元之间进行双线程通信，确保通信快速且可靠。
为实现上述发明，本发明采用下述技术方案予以实现：
本申请涉及一种水下机器人控制系统，其特征在于，包括：
传感器模块，其用于检测水下机器人的信息；
控制器模块，其包括主控单元和FPGA单元；主控单元接收传感器模块检测的信息并对其解析处理，以获取到水下机器人的实际状态信息；主控单元计算参考目标值和实际状态信息之间的误差，作为鲁棒控制器的输入值；FPGA单元包括FPGA逻辑端和HPS端，FPGA逻辑端通过HPS端与主控单元双向通信，且FPGA逻辑端用于并行鲁棒控制器的矩阵运算；
多个推进器，所述主控单元接收鲁棒控制器的输出值、根据输出值进行力矩分配、且将所分配力矩对应的多个控制信号分别输出至多个推进器；
能源供应模块，其通过电力总线向所述水下机器人控制系统中用电部件供电。
为实现鲁棒控制器的并行矩阵计算，在本申请的一些实施例中，所述鲁棒控制器为多输入多输出控制器，且状态空间表达式离散化表示为：
x(k+1)=A(k)*x(k)+ B(k)*e(k)
y(k)=C(k)*x(k)；
所述FPGA逻辑端包括数据流执行模块、状态控制模块和状态机；
所述状态控制模块接收系统时钟信号，并根据所述状态机的多个状态中每个状态的计算时长，设定每个状态的状态切换信号，所述状态切换信号包括数据载入切换信号load_c、相乘切换信号MVs_c、加和切换信号add_c和状态量更新切换信号reload_c；
所述状态机连接系统时钟信号，且多个状态包括输出load_end信号的数据载入状态、输出MVs_end信号的乘法运算状态、输出add_end信号的加法运算状态以及输出reload_flag信号的状态量更新状态;
在load_c和reload_flag使能时，进入数据载入状态，在load_end和MVs_c使能时，进入乘法运算状态，在add_c和MVs_end使能时，进入加法运算状态，在reload_c和add_end使能时，进入状态量更新状态；
其中，在分别执行完乘法运算状态、加法运算状态和状态量更新状态时，分别对应输出使能信号MV_en、Y_en和用于更新状态的Xk_en；
所述数据流执行模块包括并行执行的用于计算A(k)*X(k)的第一乘积单元、用于计算B(k)*E(k)的第二乘积单元和用于计算C(k)*X(k)的第三乘积单元，且包括并行执行的用于计算A(k)*X(k)+ B(k)*E(k)的加和单元和用于存储C(k)*X(k)的存储单元；
所述第一乘积单元、第二乘积单元和第三乘积单元分别接收使能信号MV_en，所述加和单元和存储单元分别接收使能信号Y_en；
其中，X(k)表示水下机器人当前状态向量x(k)的状态矩阵，E(k)表示鲁棒控制器输入的误差e(k)的误差矩阵，Y(k)表示鲁棒控制器的输出值y(k)的输出矩阵，x(k+1)为更新后的状态向量。
在本申请中，为了实现FPGA单元和主控单元之间双线通信，设置了自定义协议，HPS端和主控单元之间的通信通过自定义协议进行，所述自定义协议包括：
协议头；
协议尾；
协议体，其位于协议头和协议尾之间，且至少包括信息种类标识和消息负载信息，所述信息种类标识包括用于表征鲁棒控制器的输出值的标识和用于表征鲁棒控制器的输入值的标识，所述消息负载信息为鲁棒控制器的输出值或输入值。
在本申请中，为了满足多种控制模式，所述协议体还包括:
标志ARM_flag，其用于表征是否运行鲁棒控制器。
在本申请中，为了确保水下机器人与地面站的可靠通讯，且能够模块化设计通信接口部分，所述水下机器人控制系统还包括：
地面站；
通信模块，其设置在水下机器人上，且与所述主控单元通信；
通信工具箱，其设置在地面上，且分别与所述通信模块和地面站通信。
在本申请中，为了能够实现多种通讯方式之间的信号通信，所述通信模块包括：
通信控制板，其以树莓派为核心，且与所述主控单元通信连接；
第一电力载波模块，其通过以太网与所述通信控制板通信；
所述通信工具箱包括：
第二电力载波模块，所述第一电力载波模块与所述第二电力载波模块连接，所述第二电力载波模块与所述地面站通过以太网通信。
为了满足水下机器人水下的视频监控需求以及视觉处理需求，所述水下机器人控制系统还包括视频模块，所述视频模块包括：
视频控制板，其以树莓派为核心，且与所述通信控制板通信连接；
至少一个摄像机，其分别通过以太网与所述视频控制板和通信控制板通信连接。
为了满足水下机器人的多种仪器设备接入，所述水下机器人控制系统还包括：
附加仪器设备模块，其通过以太网与所述通信控制板通信连接。
为了丰富对水下机器人的控制，所述地面站包括：
模式选择模块，其用于选择手动控制模式、PID控制模式和鲁棒控制模式中任一种。
在本申请中，水下机器人至少包括控制舱，控制器模块位于所述控制舱内；通信舱，水下机器人用于传输信息的通信部分位于所述通信舱内；能源舱，能源供应模块位于能源舱内。
本申请提供的水下机器人控制系统，具有如下有益效果和优点：
（1）将控制系统分布式、模块化设计成传感器模块、控制器模块、多个推进器和能源供应模块，形成水下机器人的多个相对独立的节点，减轻控制器模块的压力，便于系统布设及维护；
（2）FPGA单元能够通过HPS端接收主控单元输出的鲁棒控制器的输入值，并能够利用FPGA单元的并行特性实现鲁棒控制器的高维度矩阵运算，解决了传统控制单元计算高维度矩阵带来的实时性问题，实时性及准确性高，确保水下机器人可靠控制；
（3）FPGA单元计算出的鲁棒控制器的输出值也会通过HPS端发送至主控单元，实现FPGA单元和主控单元之间的双线程通信，提高通信速度及通信可靠性。
结合附图阅读本发明的具体实施方式后，本发明的其他特点和优点将变得更加清楚。
附图说明
为了更清楚地说明本发明实施例中的技术方案，下面将对实施例中所需要使用的附图作一简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1是本发明提出的水下机器人控制系统一实施例的原理框图；
图2是本发明提出的水下机器人控制系统一实施例的闭环控制系统框图；
图3是本发明提出的水下机器人控制系统一实施例中FPGA逻辑端进行矩阵计算的逻辑原理图；
图4为本发明提出的水下机器人控制系统一实施例中FPGA逻辑端进行矩阵计算的逻辑实现图；
图5是本发明提出的水下机器人控制系统一实施例中HPS端和主控单元之间自定义协议的布局图；
图6是本发明提出的水下机器人控制系统再一实施例的原理框图；
图7是本发明提出的水下机器人控制系统再一实施例的层级关系图。
附图标记：
1-控制器模块；11-主控单元；12-FPGA单元；121-FPGA逻辑端；122-HPS端；
2-传感器模块；
3-能源供应模块；
4-通信部分；41-通信模块；411-通信控制板；42-通信工具箱；
5-地面站；
6-附加仪器设备模块；61-机械臂；62-DVL；63-通信转换模块；
7-视频模块；71-视频控制板；72-摄像机。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。
基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。在本发明的描述中，需要理解的是，术语“中心”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。
在本发明的描述中，需要说明的是，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接。对于本领域的普通技术人员而言，可以具体情况理解上述术语在本发明中的具体含义。在上述实施方式的描述中，具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。
术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括一个或者更多个该特征。在本发明的描述中，除非另有说明，“多个”的含义是两个或两个以上。
为了提高水下机器人的实时性且提高计算能力，本申请涉及一种水下机器人控制系统，其能够确保在水下机器人水下控制实时性和稳定性。
参见图1，水下机器人控制系统包括控制器模块1、传感器模块2、多个推进器（未标注）和能源供应模块3，该些模块均设置在水下机器人的本体上。
传感器模块2用于检测水下机器人的信息，以确保水下机器人可控稳定运行。
例如，传感器模块2包括水声定位系统（例如，超短基线USBL（Ultra-ShortBaseline）定位系统）、测速仪（例如，速度传感器或多普勒测速仪）、测高单元、压力传感器、深度传感器、声呐传感器、惯导（包含有磁罗盘）、泄露传感器等，用于获取水下机器人的信息，例如深度信息、速度信息、艏摇角度信息、位置信息等。
控制器模块1采用主控单元11和FPGA（Field Programmable Gate Array，现场可编程逻辑门阵列）单元12的双核处理，且还包括与主控单元11连接的推力分配单元13。
其中，主控单元11用于将运算条件发送至FPGA单元12，FPGA单元12用于执行水下机器人的鲁棒控制器的矩阵计算，并将计算结果再发送回主控单元11。
传感器模块2将检测到的信息发送至主控单元11，并由主控单元11对该信息进行解析处理，获取到水下机器人的实际状态信息。
因此，如上所述的运算条件包括水下机器人的实际状态信息和参考目标值（即，目标航点）。
参考图2，主控单元11计算参考目标值和实际状态信息之间的误差e(k)，作为鲁棒控制器的输入值，经由鲁棒控制器后输出输出值y(k)。
在鲁棒控制器计算输出输出值y(k)时，即获取到需要向水下机器人输出该输出值y(k)的力/力矩大小，如此，主控单元11根据输出值y(k)分配力矩，并输出所分配力矩对应的多个控制信号（例如，该控制信号为PWM驱动信号）至水下机器人的多个推进器，实现由多个推进器带动水下机器人运动。
水下机器人的运动例如，在垂荡自由度方向上下运动，表现为下降运动；在横摇自由度方向运动，表现为顺时针或逆时针横滚；在纵摇自由度方向运动，表现为抬头或者沉头运动；在纵荡自由度方向上运动，表现为前进或后退运动；在艏摇自由度方向运动，表现为顺时针或逆时针旋转运动，等等。
在本申请中，主控单元11采用ARM嵌入式系统（例如，ARM cortex-M7芯片系列），且ARM嵌入式系统内设置有STM系列微控制器（例如，STM32微控制器）。
在本申请中，FPGA单元12包括FPGA逻辑端121和HPS（Hard Processor System，硬核处理器）端122。
FPGA逻辑端121和HPS端122之间的通信采用AXI bridge协议，AXI bridge协议能够处理带宽适应和时钟控制，支持HPS端122与FPGA逻辑端121之间双向的逻辑和数据交互。
FPGA逻辑端121设计包括鲁棒控制器的逻辑实现与引脚分配等，其中鲁棒控制器的输入值e(k)和输出值y(k)均存储在FPGA逻辑端121的RAM存储器中，按照地址与位宽依次存放。
HPS端122配置搭建HPS模块、HPS端的软件设计，用于导入鲁棒控制器的运算条件且导出鲁棒控制器的计算输出值，实现与主控单元11的通信。
在本申请中，HPS端122和主控单元11采用RS232串口通信，且通信协议采用自定义通讯协议，如下将详细介绍。
参见图3和图4，示出FPGA逻辑端121对鲁棒控制器的矩阵的并行计算。
结合图3和图4，具体描述鲁棒控制器的矩阵并行计算。
在本申请中，鲁棒控制器为多输入多输出控制器，且状态空间表达式离散化表示为：
x(k+1)=A(k)*x(k)+ B(k)*e(k)
y(k)=C(k)*x(k)。
其中，x(k)表示水下机器人当前状态向量，e(k)为参考目标值和实际状态信息之间的误差，也即鲁棒控制器的输入值，y(k)表示鲁棒控制器的输出值，x(k+1)为更新后的状态向量。
图3和图4中X(k)表示状态向量x(k)的状态矩阵，E(k)表示误差e(k)的误差矩阵，Y(k)表示输出值y(k)的输出矩阵。
A(k)、B(k)和C(k)分别为离散化的系数矩阵。
需要说明的是，由于鲁棒控制器是多输入多输出控制器，因此，参考目标值为多个自由度（例如，纵荡、横荡、垂荡、艏摇、纵摇、横摇等多自由度）上的目标值，对应地，所获取到的实际状态信息为对应自由度上的状态信息。
例如，参考目标值可以包括纵荡、横荡、垂荡、艏摇、纵摇和横摇六个自由度的目标值，对应的实际状态信息也是对应纵荡、横荡、垂荡、艏摇、纵摇和横摇的实际值。
参见图4，FPGA逻辑端121包括数据流执行模块、状态控制模块和状态机。
数据流执行模块用于并行执行A(k)*X(k)、B(k)*E(k)、C(k)*X(k)，且并行执行A(k)*X(k)+ B(k)*E(k)和存储C(k)*X(k)。
例如，在对A(k)*X(k)运算时，可以将矩阵A(k)按行可以划分为矢量A 1 , A 2 ,..., A n 等n个矢量，将一个1×n的矢量和n×1的状态矩阵X(k)作之间的乘法封装为模块PE，通过设计并行计算的PE模块，简化了常规串行算法计算的冗余，当计算两个维度为n的矢量时，PE模块中只需要n个乘法器和（n-1）个加法器，且计算数据乘法时相邻组数据不存在耦合，因此，可以同时并行计算。
参见图3，矩阵A(k)按行可以划分为矢量A 1 , A 2 ,..., A n 等n个子模块，将每个按行划分的1×n的子模块A i 分别与n×1的状态矩阵X(k)作为PE模块的输入端进行相乘计算，如模块A'所示，矩阵A(k)与状态矩阵X(k)进行相乘计算时，可以通过n个PE模块同时并行计算。
同理，矩阵B(k)按行可以划分为矢量B 1 , B 2 ,..., B n 等n个子模块，将每个按行划分的1×m的子模块B i 分别与m×1的误差矩阵E(k)作为PE模块的输入端进行相乘计算，如模块B'所示，矩阵B(k)与误差矩阵E(k)进行相乘计算时，可以通过m个PE模块同时并行计算。
将A(k)*X(k)和B(k)*E(k)的乘法计算结果在模块C'中进行累加操作，得出下一时刻的状态x(k+1)，新输出的状态随即又返回至状态矩阵X(k)中对矩阵中的各个状态进行实时更新，并且作为新的状态向量成为下一次计算结果的输入。
矩阵C(k)按行可以划分为矢量C 1 , C 2 ,..., C m 等m个子模块，将每个按行划分的1×n的子模块Ci分别n×1的状态矩阵X(k)作为PE模块的输入端进行相乘计算，如模块D'所示，矩阵C(k)与X(k)进行相乘计算时，可以通过m个PE模块同时并行计算，在模块D'中与矩阵C(k)进行相乘操作后输出，作为鲁棒控制器最终输出结果进行存储，即获取到输出矩阵Y(k)。
周而复始的实现以上步骤，即可完成鲁棒控制器在以FPGA为硬件基础上的逻辑实现。
在本申请中，参见图4，状态控制模块根据状态机的多个状态中每个状态的计算时长，设定每个状态的状态切换信号，用于触发状态机的多个状态进行切换，在状态机输出对应使能信号时，使数据流执行模块执行上述并行计算。
参见图4，状态控制模块负责鲁棒控制器中迭代计算频率的设定，且状态控制模块接收系统时钟信号clk，且设定的状态切换信号包括数据载入切换信号load_c、相乘切换信号MVs_c、加和切换信号add_c和状态量更新切换信号reload_c，该些信号作为状态机中多个状态之间的切换信号，从而将数据流和状态机两部分紧密的结合在一起。
在本申请中，状态机采用三段式进行设计，其中设定四个状态：分别为数据载入状态Loaddata、乘法运算状态MVs、加法运算状态Adds、状态量更新状态Reload，四个状态分别代表了整个鲁棒控制器的运算过程。
其中，数据载入状态Loaddata用于数据载入，即，矩阵A(k)、矩阵B(k)、矩阵C(k)、误差矩阵E(k)及状态矩阵X(k)。
乘法运算状态MVs主要负责进行鲁棒控制器中矩阵与向量的浮点数乘法运算，例如，A(k)*X(k)、B(k)*E(k)和C(k)*X(k)。
加法运算状态Adds主要负责完成将乘法运算得到的结果进行累加运算。
状态量更新状态Reload主要负责将鲁棒控制器得到的新状态量加载至状态向量中，从而进行状态实时更新。
在完成数据载入状态Loaddata后，输出load_end信号使能（即，load_end=1），在完成乘法运算状态MVs后，输出MVs_end信号使能（即，MVs_end=1），在完成加法运算状态Adds后，输出add_end信号使能（即，add_end=1），在完成状态量更新状态Reload后，输出reload_flag信号使能（即，reload_flag=1）。
在load_c使能和reload_flag=1时，进入数据载入状态Loaddata；在MVs_c使能和load_end=1时，进入乘法运算状态MVs，并输出使能信号MV_en；在add_c使能和MVs_end=1时，进入加法运算状态Adds，在reload_c使能和add_end=1时，进入状态量更新状态Reload。
在执行完乘法运算状态MVs后输出的使能信号MV_en，驱动在第一乘法单元执行A(k)*X(k)，在第二乘法单元执行B(k)*E(k)，且在第三乘法单元执行C(k)*X(k)。
在执行完加法运算状态Adds后，输出使能信号Y_en，驱动在加和单元执行A(k)*X(k)+ B(k)*E(k)，并在存储单元中存储C(k)*X(k)的值，该值可以存储至FPGA逻辑端的RAM存储器中。
在执行完状态量更新状态Reload后，输出使能信号Xk_en，将A(k)*X(k)+ B(k)*E(k)的结果x(k+1)更新状态。
如上，通过状态机输出使能信号MV_en、Y_en和Xk_en对数据流进行使能控制，以此实现状态机与数据流的有机结合，保证了整个鲁棒控制器周而复始的有序执行。
如此，完成了鲁棒控制器的并行计算，并获得了鲁棒控制器的输出值。
如下，将介绍FPGA单元中HPS端122和主控单元11之间的自定义协议。
通过该自定义协议，能够实现主控单元11至HPS端122的通信以及HPS端122至主控单元11的通信。
如上所述的HPS端122的软件设计包括HPS端122和主控单元11之间的自定义通信协议设计、HPS端122输入输出设计。
参见图5，自定义协议包括协议头H1/H2、协议体和协议尾E1/E2。
协议头H1/H2和协议尾E1/E2作为自定义协议的开始和结束的标志。
协议体至少包括信息种类标识MSG和消息负载信息PAYLOAD；或者协议体可以包括协议的负载长度LEN、信息种类标识MSG、消息负载信息PAYLOAD及标志ARM_flag.
其中，ARM_flag用于表征是否运行鲁棒控制器，例如，ARM_flag=1表征运行鲁棒控制器，否则标识不运行鲁棒控制器。
其中，信息种类标识MSG包括用于表征鲁棒控制器的输出值的标识和用于表征鲁棒控制器的输入值的标识，消息负载信息PAYLOAD为待通信传输的信息，可以为鲁棒控制器的输出值或鲁棒控制器的输入值。
例如，信息种类标识MSG可以为y（例如，表征鲁棒控制器计算得到的艏摇力矩）、d（例如，表征鲁棒控制器计算得到的垂荡方向力）、e（例如，表征艏摇目标值与实际值之间的误差（即，鲁棒控制器的输入值）），等等。
此时，消息负载信息PAYLOAD可以为鲁棒控制器的输出值、或者鲁棒控制器的输入值。
在本申请中，协议头H1/H2和协议尾E1/E2可分别占用两个字节，协议的负载长度LEN可占一个字节，信息种类标识MSG可占用一个字节，ARM_flag可占用一个字节，PAYLOAD可占用0字节到16个字节之间。
因此，此自定义协议的协议总长度在7-23个字节，属于轻量级协议，能够以极少的资源实现通信。
对于HPS端122输入输出设计，本申请采用双线程设计。
其中一条线程为HPS端122接收主控单元11下发的包含运算条件（包括参考目标值和误差e(k)）的消息，并对该消息进行解析，最后送到FPGA逻辑端121的RAM中。
另一条线程为主控单元11获取FPGA逻辑端121所计算的鲁棒控制器的输出值，并对此输出值进行打包，最后传输至主控单元11中（可参考图7）。
例如，在信息种类标识MSG为e，此时艏摇目标值与实际值之间的误差（即，鲁棒控制器的输入值）加载到消息负载信息PAYLOAD中，构成数据包，此后，通过例如串口RS232从主控单元11发送至HPS端122，进而发送至FPGA逻辑端121，并存储在FPGA逻辑端121的RAM中，实现通信。
例如，在信息种类标识MSG为y，此时鲁棒控制器计算得到的艏摇力矩（即，鲁棒控制器的输出值）加载到消息负载信息PAYLOAD中，构成数据包，此后，通过例如串口RS232将数据包发送至主控单元11。
在主控单元11解析到该自定义协议中的相关数据时，会将消息负载信息PAYLOAD中的信息赋值到主控单元11的艏摇力矩的相关变量中，实现通信。
参见图6，水下机器人控制系统还包括地面站5和通信部分4，其中通信部分4包括通信模块41和通信工具箱42。
通信模块41设置在水下机器人上，且通信工具箱42设置在地面上。
通信模块41分别与主控单元11和通信工具箱通信42，且通信工具箱42与地面站5通信。
在本申请中，通信模块41执行两部分的内容：第一部分是通信任务；第二部分是路径规划任务。
通信任务是将水下机器人状态信息、传感器模块2反馈的信息等，通过通信模块41和通信工具箱42发送至地面站5，以实现地面站5对水下机器人的监控功能。
路径规划任务是通信模块41输出运动控制指令，即，水下机器人下一时刻的参考目标值，用于指导水下机器人的自主控制运动。
在本申请中，通信模块41和通信工具箱42通过电力载波进行通信。
具体地，通信模块41包括通信控制板411和第一电力载波模块（未示出）；通信工具箱42包括第二电力载波模块（未示出）。
通信控制板411以树莓派为核心，且与主控单元11例如采用RS232串口通信连接。通信控制板411与第一电力载波模块通过以太网通信。
第二电力载波模块与第一电力载波模块电力载波通信，其与地面站5通过以太网通信。
为了满足地面站的多种协议通信方式，通信工具箱42还可以包括通信转换模块（未示出），该通信转换模块与第二电力载波模块通过以太网通信，用于转换以太网通信方式。
例如，通信转换模块可以为以太网转RS485转换模块，其用于将以太网协议转换为RS4585通信协议，以实现通信工具箱42与地面站5的RS485通信。
如上所述的传感器模块2中的传感器可以根据需求及实际布置方式，可以与主控单元11连接，也可以与通信模块41连接，也可以一部分与主控单元11连接且一部分与通信模块41连接。
此外，在通信工具箱42中还会设置供电单元（未示出），用于为通信工具箱42中用电部件供电。
在本申请中，参见图6，水下机器人控制系统还包括视频模块7。
视频模块7包括视频控制板71和至少一个摄像机72，其中视频控制板71以树莓派为核心，且与通信控制板411例如通过以太网通信连接。
至少一个摄像机72（例如广角相机）支持以太网协议，且分别通过以太网协议与视频控制板71及通信控制板411通信连接，用于将视频流传输至地面站5，实现地面站5对水下机器人的视频监控。
参见图7，视频控制板71接收通信控制板411下发的处理指令，用于运行视觉处理程序（例如，目标识别程序），并且将视觉处理结果（例如，距障碍物的距离、相对位置等信息）通过以太网发送至通信控制板411。
之后，根据如上所述的传感器模块2检测的信息、附加仪器设备模块6（如下介绍）中各仪器设备检测的信息以及视频模块7的视频处理结果，通信控制板411做路径规划任务（例如，往右走），输出运动控制指令，即水下机器人下一时刻的参考目标值，之后，可以将参考目标值发送至主控单元11。
在本申请中，参见图6，水下机器人控制系统还包括附加仪器设备模块6，该附加仪器设备模块6通过以太网与通信控制板411通信。
在本申请中，附加仪器设备模块6包括机械臂61和DVL（Doppler Velocity Log，多普勒测速仪）62。
DVL 62通过以太网与通信控制板411通信连接，机械臂61通过通信转换模块63将机械臂61的串口协议转换为以太网协议，实现通过以太网与通信控制板411通信连接，进行数据信息的传递。
此外，在附加仪器设备模块6中，还可以设置用于满足其他多种科学仪器及传感器（例如，测量海水温盐深的传感器设备、扫描海底地形的声呐传感器等），利用此模块实现其他传感器的安装和拓展，提高该水下机器人控制系统的延展性。
在本申请中，地面站5包括PC机（未示出）和上位机软件。
上位机软件具有显示水下机器人状态、传感器数据、视频流画面、水下机器人模式控制等功能。
地面站5执行水下机器人模式控制功能时，是通过模式选择模块（未示出）来执行的。
模式选择模块用于选择手动控制模式、PID控制模式和鲁棒控制模式中一种。
通过上位机选择机器人要执行的控制模式，例如选择鲁棒控制模式时，才会执行如上所述的水下机器人控制系统。
在选择手动控制模式时，则根据上位机发出的遥控指令，水下机器人实现相关的动作。
例如，在水下机器人进行作业过程中，操作员通过鼠标、键盘或者手柄等对上位机下达期望的遥控命令。
在选择PID控制模式时，由主控单元11计算，因为此PID控制器计算简单，不存在矩阵运算，不需要FPGA单元12并行计算来加速。
至于使用哪种控制模式，需要通过上位机进行选择。
在选择了相应的模式之后，水下机器人在作业过程中会以相应的控制模式执行任务。
在本申请中，若水下机器人（未示出）采用如上所述的水下机器人控制系统，通过将该控制系统中的多个模块分布式设置在水下机器人的不同舱内，便于实现水下机器人分布式结构布置。
水下机器人至少包括控制舱、通信舱和能源舱；控制器模块1位于控制舱内，水下机器人中用于传输信息的通信部分（例如通信模块41）位于通信舱内，能源供应模块3位于能源舱内。
例如，如上所述的通信模块41位于通信舱内。
此外，水下机器人还可以包括图像处理舱和科学仪器舱。
如上所述的视频模块7中的视频控制板71可以设置在图像处理舱中，附加仪器设备模块6中需要密封设置的器件可以设置在科学仪器舱中。
通过设置多个舱，用于分布式、模块化设置水下机器人控制系统，在确保水下机器人实时且准确稳定控制的同时，确保水下机器人整体结构的重量均衡，实现提高水下机器人的整体悬浮稳定性同时提高其灵活机动性。
以上实施例仅用以说明本发明的技术方案，而非对其进行限制；尽管参照前述实施例对本发明进行了详细的说明，对于本领域的普通技术人员来说，依然可以对前述实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或替换，并不使相应技术方案的本质脱离本发明所要求保护的技术方案的精神和范围。

Claims (10)

1.一种水下机器人控制系统，其特征在于，包括：
传感器模块，其用于检测水下机器人的信息；
控制器模块，其包括主控单元和FPGA单元；主控单元接收传感器模块检测的信息并对其解析处理，以获取到水下机器人的实际状态信息；主控单元计算参考目标值和实际状态信息之间的误差，作为鲁棒控制器的输入值；FPGA单元包括FPGA逻辑端和HPS端；FPGA逻辑端通过HPS端与主控单元双向通信，且FPGA逻辑端用于并行执行鲁棒控制器的矩阵运算；
多个推进器，所述主控单元接收鲁棒控制器的输出值、根据输出值进行力矩分配、且将所分配力矩对应的多个控制信号分别输出至多个推进器；
能源供应模块，其通过电力总线向所述水下机器人控制系统中用电部件供电。
2.根据权利要求1所述的水下机器人控制系统，其特征在于，所述鲁棒控制器为多输入多输出控制器，且状态空间表达式离散化表示为：
x(k+1)=A(k)*x(k)+ B(k)*e(k)
y(k)=C(k)*x(k)；
所述FPGA逻辑端包括数据流执行模块、状态控制模块和状态机；
所述状态控制模块接收系统时钟信号，并根据所述状态机的多个状态中每个状态的计算时长，设定每个状态的状态切换信号，所述状态切换信号包括数据载入切换信号load_c、相乘切换信号MVs_c、加和切换信号add_c和状态量更新切换信号reload_c；
所述状态机连接系统时钟信号，且多个状态包括输出load_end信号的数据载入状态、输出MVs_end信号的乘法运算状态、输出add_end信号的加法运算状态以及输出reload_flag信号的状态量更新状态;
在load_c和reload_flag使能时，进入数据载入状态，在load_end和MVs_c使能时，进入乘法运算状态，在add_c和MVs_end使能时，进入加法运算状态，在reload_c和add_end使能时，进入状态量更新状态；
其中，在分别执行完乘法运算状态、加法运算状态和状态量更新状态时，分别对应输出使能信号MV_en、Y_en和用于更新状态的Xk_en；
所述数据流执行模块包括并行执行的用于计算A(k)*X(k)的第一乘积单元、用于计算B(k)*E(k)的第二乘积单元和用于计算C(k)*X(k)的第三乘积单元，且包括并行执行的用于计算A(k)*X(k)+ B(k)*E(k)的加和单元和用于存储C(k)*X(k) 的存储单元；
所述第一乘积单元、第二乘积单元和第三乘积单元分别接收使能信号MV_en，所述加和单元和存储单元分别接收使能信号Y_en；
其中，X(k)表示水下机器人当前状态向量x(k)的状态矩阵，E(k)表示鲁棒控制器输入的误差e(k)的误差矩阵，Y(k)表示鲁棒控制器的输出值y(k)的输出矩阵，x(k+1)为更新后的状态向量。
3.根据权利要求1所述的水下机器人控制系统，其特征在于，HPS端和主控单元之间的通信通过自定义协议进行，所述自定义协议包括：
协议头；
协议尾；
协议体，其位于协议头和协议尾之间，且至少包括信息种类标识和消息负载信息，所述信息种类标识包括用于表征鲁棒控制器的输出值的标识和用于表征鲁棒控制器的的输入值的标识，所述消息负载信息为鲁棒控制器的输出值或输入值。
4.根据权利要求3所述的水下机器人控制系统，其特征在于，所述协议体还包括:
标志ARM_flag，其用于表征是否运行鲁棒控制器。
5.根据权利要求1所述的水下机器人控制系统，其特征在于，所述水下机器人控制系统还包括：
地面站；
通信模块，其设置在水下机器人上，且与所述主控单元通信；
通信工具箱，其设置在地面上，且分别与所述通信模块和地面站通信。
6.根据权利要求5所述的水下机器人控制系统，其特征在于，所述通信模块包括：
通信控制板，其以树莓派为核心，且与所述主控单元通信连接；
第一电力载波模块，其通过以太网与所述通信控制板通信；
所述通信工具箱包括：
第二电力载波模块，所述第一电力载波模块与所述第二电力载波模块连接，所述第二电力载波模块与所述地面站通过以太网通信。
7.根据权利要求5所述的水下机器人控制系统，其特征在于，所述水下机器人控制系统还包括视频模块，所述视频模块包括：
视频控制板，其以树莓派为核心，且与所述通信控制板通信连接；
至少一个摄像机，其分别通过以太网与所述视频控制板和通信控制板通信连接。
8.根据权利要求5或7所述的水下机器人控制系统，其特征在于，所述水下机器人控制系统还包括：
附加仪器设备模块，其通过以太网与所述通信控制板通信连接。
9.根据权利要求5所述的水下机器人控制系统，其特征在于，所述地面站包括：
模式选择模块，其用于选择手动控制模式、PID控制模式和鲁棒控制模式中任一种。
10.根据权利要求1所述的水下机器人控制系统，其特征在于，水下机器人至少包括：
控制舱，所述控制器模块位于所述控制舱内；
通信舱，水下机器人用于传输信息的通信部分位于所述通信舱内；
能源舱，所述能源供应模块位于能源舱内。
CN202210700810.2A 2022-06-21 2022-06-21 一种水下机器人控制系统 Active CN114779712B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210700810.2A CN114779712B (zh) 2022-06-21 2022-06-21 一种水下机器人控制系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210700810.2A CN114779712B (zh) 2022-06-21 2022-06-21 一种水下机器人控制系统

Publications (2)

Publication Number Publication Date
CN114779712A true CN114779712A (zh) 2022-07-22
CN114779712B CN114779712B (zh) 2022-09-20

Family

ID=82420621

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210700810.2A Active CN114779712B (zh) 2022-06-21 2022-06-21 一种水下机器人控制系统

Country Status (1)

Country Link
CN (1) CN114779712B (zh)

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106249658A (zh) * 2016-08-31 2016-12-21 中国船舶重工集团公司第七〇二研究所 一种基于SoC‑FPGA的电机单片控制装置及方法
CN207067783U (zh) * 2017-08-18 2018-03-02 江苏信息职业技术学院 一种水下机器人
CN111913489A (zh) * 2019-05-10 2020-11-10 欧舶智能科技（上海）有限公司 水下机器人多推进器耦合控制方法及系统
CN111984224A (zh) * 2020-08-24 2020-11-24 中国海洋大学 用于auv运动控制器的浮点数乘加ip核及控制方法
US20210316828A1 (en) * 2020-04-08 2021-10-14 Bae Systems Information And Electronic Systems Integration Inc. Adaptable control for autonomous maritime vehicles
CN114397834A (zh) * 2021-12-06 2022-04-26 埃夫特智能装备股份有限公司 一种基于虚拟机器人控制器模拟fpga系统及其使用方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106249658A (zh) * 2016-08-31 2016-12-21 中国船舶重工集团公司第七〇二研究所 一种基于SoC‑FPGA的电机单片控制装置及方法
CN207067783U (zh) * 2017-08-18 2018-03-02 江苏信息职业技术学院 一种水下机器人
CN111913489A (zh) * 2019-05-10 2020-11-10 欧舶智能科技（上海）有限公司 水下机器人多推进器耦合控制方法及系统
US20210316828A1 (en) * 2020-04-08 2021-10-14 Bae Systems Information And Electronic Systems Integration Inc. Adaptable control for autonomous maritime vehicles
CN111984224A (zh) * 2020-08-24 2020-11-24 中国海洋大学 用于auv运动控制器的浮点数乘加ip核及控制方法
CN114397834A (zh) * 2021-12-06 2022-04-26 埃夫特智能装备股份有限公司 一种基于虚拟机器人控制器模拟fpga系统及其使用方法

Also Published As

Publication number Publication date
CN114779712B (zh) 2022-09-20

Similar Documents

Publication Publication Date Title
Encarnacao et al. 3D path following for autonomous underwater vehicle
CN111687821B (zh) 转动式并联型飞行机械臂系统及期望转角解算方法
Pascoal et al. Vehicle and mission control of single and multiple autonomous marine robots
CN108415460B (zh) 一种组合分离式旋翼与足式移动操作机器人集中-分布式控制方法
Duecker et al. HippoCampusX–A hydrobatic open-source micro AUV for confined environments
Hu et al. A new ROS-based hybrid architecture for heterogeneous multi-robot systems
CN111438691B (zh) 仿生六足机器蟹控制系统
CN111460723A (zh) 一种基于粒子群算法的水下航行器内部组件分层布局方法
CN114779712B (zh) 一种水下机器人控制系统
CN114089634B (zh) 一种基于多电机主从轴协同算法的水下机器人控制方法
CN113968326A (zh) 一种矢量框架式水下机器人
CN114089761A (zh) 一种基于ros的领航跟随者多无人船编队控制系统及其方法
CN114047744A (zh) 基于采样通信的自适应反步滑模多无人艇编队控制方法
CN116700299A (zh) 一种基于数字孪生的auv集群控制系统及方法
CN211943686U (zh) 一种水下仿生机器人及其运动控制系统
Su et al. Real-time dynamic-consistent motion planning for over-actuated uavs
WO2023201896A1 (zh) 一种水下机器人矢量推进器的控制系统、控制方法及矢量角度选择方法
CN210616555U (zh) 一种六自由度rov模拟驾驶系统
CN114019915A (zh) 多移动机器人协同运动控制系统及其控制方法
Parra-Vega et al. Toward force control of a quadrotor UAV in SE (3)
Zavari et al. Propulsion system development and power consumption in an autonomous underwater vehicle
CN113126645A (zh) 轻型长航程auv的控制系统
Tran et al. Development of a new underwater disk robot
CN117850245B (zh) 一种软体机器鱼及其控制方法
CN115107964B (zh) 一种水下工程测量子母式潜航器系统

Legal Events

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

Effective date of registration: 20231013

Address after: 266000 room 105, B1, zone B, Qingdao Institute of industrial technology, No. 17, Songyuan Road, high tech Zone, Qingdao, Shandong

Patentee after: Daoli Zhiyuan Technology (Qingdao) Co.,Ltd.

Address before: 266100 Shandong Province, Qingdao city Laoshan District Songling Road No. 238

Patentee before: OCEAN University OF CHINA

TR01 Transfer of patent right