一种多水下机器人协同跟踪控制算法验证系统的制作方法

1.本发明涉及一种无人系统集群协同跟踪控制算法验证系统，属于控制技术领域。


背景技术：

2.无人系统集群协同工作可广泛应用于编队护航、搜索巡逻、区域控制和平战结合的信息保障等军民用任务。欧美一些国家正在积极推进无人系统集群演示验证试验，以加快推进技术的实战化应用。由于当前无人系统集群的关键技术尚未成熟，因此需要开展大量的测试验证和优化。但是，海上试验环境复杂，试验成本高，长期利用无人系统集群实装测试并不现实。
3.目前的多水下机器人协同跟踪控制算法验证基本都是依赖于针对控制算法单独开发的水下机器人进行的，或者是针对某些特定型号的水下机器人进行控制算法开发时，利用开发的控制算法针对之前的对象记性验证。所以目前并没有一种针对不同控制算法记性统一验证的系统或设备，因此单独开发验证的方式存在效率低、成本高的问题。
4.同时目前有些控制算法是针对水下机器人运动控制的，有些控制是针对水下机器人运动和作业任务控制(需要机械臂参与控制)的，因此现有的水下机器人也不能够用于不同算法的验证，同时用于验证现有控制算法的水下机器人还存在rov驱动效率较低的问题。


技术实现要素：

5.本发明是为了解决目前验证控制算法存在效率低、成本高的问题，以及现有的水下机器人不能适用于不同算法验证的问题。
6.一种多水下机器人协同跟踪控制算法验证系统，包括rov和上位机，rov即水下机器人，rov和上位机之间通过电力载波的通信方式进行通信，且上位机通过指令对控制至少两个rov的运动及任务执行；
7.所述rov包括rov本体和水下抓取机械臂；
8.所述rov本体包括前板、后板、两个侧板、上板、下板、舱体；所述前板、后板、两个侧板、上板、下板构成rov本体框架，本体框架内设置舱体，电机和舱内元件通过舱盖封闭在舱体内部；
9.前板、后板上各设置4个推进器，前板上的4个推进器设置在前板的四角附近，其中位于前板上方的2个推进器的推进方向与前板平行，且2个推进器的推进方向相对成夹角，位于前板下方的2个推进器推进方向与前板成夹角，且2个推进器的推进方向相对成夹角；后板上的4个推进器设置在前板的四角附近，其中位于后板下方的2个推进器的推进方向与后板平行且2个推进器的推进方向相反成夹角，位于后板上方的2个推进器的推进方向与前板成夹角，且2个推进器的推进方向相对成夹角；前板、后板上的8个推进器构成rov的动力系统，这种推进器布置方式不仅可以为rov提供良好的动力，而且具有优秀的机动性；
10.所述上位机包括机器人通信单元、抓取机械臂显示单元、航迹显示单元、航线添加单元、机器人数据显示单元、数据发送单元；
11.机器人通信单元，用于根据输入的不同rov的ip通过port端口号连接rov，并与相应的rov进行通信；
12.抓取机械臂显示单元，根据连接机械手的串口接收和显示机械臂数据；
13.航迹显示单元，根据uwb串口接收uwb数据，解析数据并在地图上显示rov当前位置；
14.航线添加单元，根据用户的输入在地图上添加、删除航线并在当前地图上显示航线；
15.机器人数据显示单元，接收并显示连接状态的rov数据；
16.数据发送单元，用于根据用户选择对连接状态下的rov发送操作指令数据，进而控制rov的运动。
17.进一步地，所述rov还包括uwb定位天线，用于获取rov的位置信息。
18.进一步地，所述rov还包括姿态传感器和深度计，用于计算rov的姿态信息。
19.进一步地，所述rov本体上设置有摄像头和照明设备。
20.进一步地，水下机械臂采用三关节式结构设计，水下机械臂端的底部搭载360度旋转的旋转云台，机械臂每个关节处配有数字舵机，通过控制线输入一个宽度可调的周期性方波脉冲信号，方波脉冲信号的周期为20ms；当方波的脉冲宽度改变时，舵机转轴的角度发生改变，角度变化与脉冲宽度的变化成正比。
21.进一步地，水下机械臂端的机械臂部分采用圆形包裹设计，线缆布放在包裹中。
22.进一步地，所述舱内元件包括主控制器板、电机驱动板，主控制器板通过串口与电机驱动板进行通信；
23.主控制器板用于传感器数据的处理，rov控制命令的处理与发送，rov状态的监测；
24.电机驱动板用于电机控制，即对推进器对应的电机和机械臂对应的舵机进行控制。
25.进一步地，所述主控制器板型号为stm32f407vet6，电机驱动板的芯片型号为stm32f103rct6。
26.进一步地，所述主控制器板和电机驱动板采用分板式设计，即主控制器板和电机驱动板为两块电路板。
27.10、根据权利要求9所述的一种多水下机器人协同跟踪控制算法验证系统，其特征在于，所述舱内元件还包括电源板，电源板用于电调转接。
28.有益效果：
29.本发明采用上位机和机器人通信的方式进行控制，可以适用于不同控制算法的验证，利用上位机，可以通过指令发送的方式验证水下机器人的控制效果，也可以通过控制算法设定的pid控制指令进行控制，既能验证水下机器人的控制效果，还能够验证控制算法的有效性，因此本发明可以实现不同输入的方式、不同控制算法的验证。同时本发明的rov采用水下抓取机械臂，并且在上位机中设置单独的控制单元，因此本发明即可以适用于针对rov运动进行控制的算法的验证，还可以用于针对rov运动和作业任务进行的控制，所以本发明不仅通用性更强，而且省去了不同控制算法需要单独开发机器人或/和控制界面的存在的效率低、成本高的问题。
30.此外，本发明的rov结构还可以为rov提供良好的动力，使得rov具有优秀的机动
性。
附图说明
31.图1为rov本体实物图；
32.图2为前板推进器布置方式示意图；
33.图3为后板推进器布置方式示意图；
34.图4为舵机输出轴转角与输入信号脉冲宽度关系图；
35.图5为柔性鳍条式机械爪实物图；
36.图6为控制器板、推进器驱动板、机械臂驱动板安装实物图；
37.图7a
‑
图7e为stm32f407vet6局部引脚关系示意图；
38.图8a
‑
图8j为与stm32f407vet6引脚连接的接口示意图；
39.图9a为与stm32f407vet6的40、42、44、45号引脚连接的电路示意图；图9b为与stm32f407vet6的3号引脚连接的电路示意图；
40.图10为与can通信收发器电路示意图；
41.图11为stm32f103rct6引脚关系示意图；
42.图12a
‑
图12h为与stm32f103rct6引脚连接的电路及接口示意图；
43.图13a
‑
图13i为上位机功能单元的截图；其中图13a为机器人通信单元截图，图13b为抓取机械臂显示单元截图，图13c为航迹显示单元截图，图13d
‑
图13g为航线添加单元截图，图13h为机器人数据显示单元截图，图13i为数据发送单元截图。
具体实施方式
44.具体实施方式一：
45.本实施方式为一种多水下机器人协同跟踪控制算法验证系统，所述的验证系统包括rov和上位机，rov即水下机器人，rov和上位机之间通过电力载波的通信方式进行通信，且上位机通过指令对控制至少两个rov的运动及任务执行。
46.所述rov包括一个rov本体，rov本体上搭载摄像头、照明灯、uwb定位天线、水下抓取机械臂，以及姿态传感器和深度计；
47.姿态传感器与深度计作为rov的感知系统，通过计算得到rov的姿态信息。
48.本发明采用uwb定位技术作为rov的导航定位系统，来获取rov的位置信息。
49.如图1所示，所述rov本体包括前板、后板、两个侧板101、上板、下板102、舱体；所述前板、后板、两个侧板101、上板、下板(102)构成rov本体框架，本体框架内设置舱体，电机和舱内元件通过舱盖封闭在舱体内部；
50.前板、后板上各设置4个推进器，如图2
‑
图3所示，前板上的4个推进器设置在前板的四角附近，其中位于前板上方的2个推进器的推进方向与前板平行，且2个推进器的推进方向相对成夹角，位于前板下方的2个推进器推进方向与前板成夹角(小于90
°
)，且2个推进器的推进方向相对成夹角(以推进方向汇聚点为夹角的顶点，夹角小于180
°
)；后板上的4个推进器设置在前板的四角附近，其中位于后板下方的2个推进器的推进方向与后板平行且2个推进器的推进方向相反成夹角，位于后板上方的2个推进器的推进方向与前板成夹角(小于90
°
)，且2个推进器的推进方向相对成夹角(以推进方向反方向汇聚点为夹角的顶点，夹
角小于180
°
)；前板、后板上的8个推进器构成rov的动力系统，这种推进器布置方式不仅可以为rov提供良好的动力，而且具有优秀的机动性；
51.通过对rov整机的浮心与重心的稳性分析，本实施方式中rov的总重量为27.66kg，总排水量为36.98kg，因此，rov应该携带的配重10.6kg。
52.表1 rov各部件参数表
[0053][0054]
表2 rov总重量与总排水体积
[0055] 理论值实际值总重量26.36627.66总排水量36.98——
[0056]
通过对rov整机的浮心与重心的稳性分析，本实施方式中rov的总重量为27.66kg，总排水量为36.98kg，因此，rov应该携带的配重10.6kg。
[0057]
水下机械臂采用三关节式结构设计，底部搭载可实现360度旋转的旋转云台，机械臂每个关节处配有数字舵机，通过控制线输入一个宽度可调的周期性方波脉冲信号，方波脉冲信号的周期为20ms。当方波的脉冲宽度改变时，舵机转轴的角度发生改变，角度变化与脉冲宽度的变化成正比。舵机的输出轴转角与输入信号的脉冲宽度之间的关系如图4所示；
[0058]
机械臂部分采用圆形包裹设计，线缆布放在包裹中，避免线路裸露在外面导致的以外损坏，同时，通过浮力调节，使机械臂在水中呈现零浮力状态，极大地降低了由于机械臂运动对rov自身姿态影响的程度。
[0059]
机械臂末端的机械爪部分采用柔性的鳍条式设计，充分将柔性材料的优势与结构设计的原理相结合，实现在保证拥有足够大的抓取力的同时还能进行一定的变形，即满足对不同形状物体(异形物体)的抓取，如图5所示。
[0060]
舱内元件包括主控制器板、电机驱动板和电源板，在一些实施例中，主控制器板、电机驱动板和电源板设置为一张电路；在一些实施例中主控制器板、电机驱动板可以设置为一张电路板，电源板单独设置为一张电路板；在另一些实施例中，可以采用分板式设计，即主控制器板、电机驱动板和电源板为三张电路板，实物图如图6所示，这种设计方式可以在一定程度上减少体积，更为重要的是这种设计方式能够极大地减少电机驱动板与主控器的之间的干扰，保证rov的稳定性和机械臂的控制准确性，同时这种设计方法也能够提高机械臂的控制效率；而且这种方式还有开发设计简单，维护更换成本较低的优点。
[0061]
主控制器板用于传感器数据的处理，rov控制命令的处理与发送，rov状态的监测等等。同时，主控制器上集成了电力通信模块，路由器模块，可以实现rov与上位机之间的网
络通信。主控制器是整个rov硬件电路中最核心的部分。主控制器板的型号为stm32f407vet6，电路图如图7a
‑
图7e所示，主控制器芯片的引脚主要起着与其它芯片或者传感器通信的作用，通信以串口通信为主，其中串口1(图8a)与电机驱动板的芯片进行通信，控制其输出pwm信号以驱动电机；串口2(图8b)与由wifi模块与电力猫组成的通信板进行通信，接收来自上位机的指令信号；串口5接收深度传感器的数据(图8c)；串口6接收姿态传感器陀螺仪的数据(图8d)；另外该主控芯片还具备can通信(图8f)，spi通信(图8e)与iic通信功能。
[0062]
图7a
‑
图7e为stm32f407vet6局部引脚关系示意图；图8a
‑
图8i为与stm32f407vet6引脚连接的接口示意图，其中图8a对应68
‑
69(实际就是pa9、pa10，为了表示方便，本发明用引脚号进行表示)号引脚，图8b对应25
‑
26号引脚、图8c对应80、84号引脚，图8d对应63、64号引脚，图8f对应70、71号引脚，图8e对应89
‑
91号引脚，图8g对应92
‑
93号引脚，图8h对应32、35、36、33、34号引脚，图8i对应59
‑
62号引脚，图8j对应72、76号引脚；
[0063]
图9a为与stm32f407vet6的40、42、44、45号引脚连接的电路示意图；图9b为与stm32f407vet6的3号引脚(pe4)连接的电路(进水检测电路)示意图；
[0064]
图10为与can通信收发器电路示意图。
[0065]
电机驱动板是电机控制部分，根据串口2(16、17号引脚，即pa2和pa3)接收到的主控制器板的信号对推进器对应的电机和机械臂对应的舵机进行控制；主要包括一片stm32f1系列芯片和多片光耦芯片，起到控制电与动力电隔离的作用，不会使控制部分的电路受电机功率变化的影响；本实施方式选择stm32f1系列芯片的型号为stm32f103rct6，电路图如图11所示，图12a为与14、15号引脚和58、59、61、62号引脚连接的电路示意图，图12f为与图12a连接的串口，该串口为电机m5
‑
m8的接口；图12b为与50、55、29、30号引脚连接的电路示意图，图12h为与图12b连接的串口，该串口为舵机t1
‑
t4接口；图12c为与26、27、56、57号引脚连接的电路示意图，该电路同时还与37
‑
40号引脚相连接，图12g为与图12c连接的串口，该串口为电机m1
‑
m4的接口；图12d为与41号引脚连接的电路示意图；图12e为与44、45号引脚连接的电路示意图；这块主控芯片也具备can通信功能。
[0066]
电源板是电调转接部分，将电调模块嵌入pcb中，精简了舱内结构，电调24v供电。
[0067]
所述上位机包括机器人通信单元、抓取机械臂显示单元、航迹显示单元、航线添加单元、机器人数据显示单元、数据发送单元；
[0068]
机器人通信单元，用于根据输入的不同rov的ip通过port端口号连接rov，并与相应的rov进行通信；如图13a所示；
[0069]
本实施方式中，rov1、rov2可同时连接，连接成功后上位机开启定时器向不同rov定时发送数据，定时器间隔为1s(方便调试，此事件可随意调整)。连接、断开按钮可变色提示。
[0070]
抓取机械臂显示单元，根据连接机械手的串口接收和显示机械臂数据，如图13b所示。
[0071]
航迹显示单元，根据uwb串口接收uwb数据，解析数据并在地图上显示rov当前位置，如图13c所示。
[0072]
航线添加单元，根据用户的输入在地图上添加、删除航线并在当前地图上显示航线，如图13d、图13e、图13f、图13g所示；
[0073]
本实施方式中，点击添加航线按钮，按钮变蓝色，鼠标左键在地图上选择航线点，规划完成后，点击鼠标右键弹出对话框，输入航线名称后，点击ok按钮完成添加航线，点击编辑航线按钮，可看到当前航线的相关数据，点击“删除航线”按钮删除当前航线，当已经设置了航线后，点击“发送航线”按钮会弹出发送成功对话框，若当前无航线会提示无航线需先添加再发送；鼠标右键，点击“清楚航线”可清楚当前地图上的航线。
[0074]
机器人数据显示单元，接收并显示连接状态的rov数据，如图13h所示；
[0075]
数据发送单元，用于根据用户选择对连接状态下的rov发送操作指令数据，进而控制rov的运动，如图13i所示；
[0076]
本实施方式中可以通过点击按钮或使用按钮对应键盘上的相应按钮发送操作指令，例如油门按钮为滑动条，通过拖动滑动条或点击按钮改变油门大小。也可以通过设定好的pid控制指令进行控制。
[0077]
本发明采用上位机和机器人通信的方式进行控制，可以适用于不同控制算法的验证，利用上位机，可以通过指令发送的方式验证水下机器人的控制效果，也可以通过控制算法设定的pid控制指令进行控制，既能验证水下机器人的控制效果，还能够验证控制算法的有效性，因此本发明可以实现不同输入的方式、不同控制算法的验证。同时本发明的rov采用水下抓取机械臂，并且在上位机中设置单独的控制单元，因此本发明即可以适用于针对rov运动进行控制的算法的验证，还可以用于针对rov运动和作业任务进行的控制，所以本发明不仅通用性更强，而且省去了不同控制算法需要单独开发机器人或/和控制界面的存在的效率低、成本高的问题。
[0078]
此外，本发明的rov结构还可以为rov提供良好的动力，使得rov具有优秀的机动性。
[0079]
本发明还可有其它多种实施例，在不背离本发明精神及其实质的情况下，本领域技术人员当可根据本发明作出各种相应的改变和变形，但这些相应的改变和变形都应属于本发明所附的权利要求的保护范围。