CN114063624A - 一种爬游无人潜水器多模式规划运动控制器及其控制方法 - Google Patents
一种爬游无人潜水器多模式规划运动控制器及其控制方法 Download PDFInfo
- Publication number
- CN114063624A CN114063624A CN202111229823.8A CN202111229823A CN114063624A CN 114063624 A CN114063624 A CN 114063624A CN 202111229823 A CN202111229823 A CN 202111229823A CN 114063624 A CN114063624 A CN 114063624A
- Authority
- CN
- China
- Prior art keywords
- mode
- crawling
- motion
- submersible
- speed
- 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
Links
- 230000033001 locomotion Effects 0.000 title claims abstract description 154
- 230000009193 crawling Effects 0.000 title claims abstract description 118
- 238000000034 method Methods 0.000 title claims abstract description 63
- 238000013507 mapping Methods 0.000 claims abstract description 25
- 230000007246 mechanism Effects 0.000 claims abstract description 23
- 230000001360 synchronised effect Effects 0.000 claims abstract description 22
- 230000006870 function Effects 0.000 claims description 32
- 238000005259 measurement Methods 0.000 claims description 31
- 230000008569 process Effects 0.000 claims description 28
- 238000006243 chemical reaction Methods 0.000 claims description 27
- 238000001514 detection method Methods 0.000 claims description 15
- 238000010586 diagram Methods 0.000 claims description 15
- 230000005021 gait Effects 0.000 claims description 15
- 230000001133 acceleration Effects 0.000 claims description 13
- 230000009466 transformation Effects 0.000 claims description 13
- 210000001503 joint Anatomy 0.000 claims description 12
- 239000002245 particle Substances 0.000 claims description 9
- 238000012937 correction Methods 0.000 claims description 7
- 238000001914 filtration Methods 0.000 claims description 7
- 239000011159 matrix material Substances 0.000 claims description 7
- 238000004891 communication Methods 0.000 claims description 6
- 238000012545 processing Methods 0.000 claims description 6
- 238000004364 calculation method Methods 0.000 claims description 3
- 230000000694 effects Effects 0.000 claims description 3
- 230000010365 information processing Effects 0.000 claims description 3
- 230000000630 rising effect Effects 0.000 claims description 3
- 238000013519 translation Methods 0.000 claims description 3
- 239000013598 vector Substances 0.000 claims description 3
- 235000018453 Curcuma amada Nutrition 0.000 claims 1
- 241001512940 Curcuma amada Species 0.000 claims 1
- 230000009189 diving Effects 0.000 description 6
- 238000012544 monitoring process Methods 0.000 description 2
- 230000007704 transition Effects 0.000 description 2
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 2
- 238000011161 development Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 238000011835 investigation Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 239000013589 supplement Substances 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/04—Control of altitude or depth
- G05D1/06—Rate of change of altitude or depth
- G05D1/0692—Rate of change of altitude or depth specially adapted for under-water vehicles
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明公开了一种爬游无人潜水器多模式规划运动控制器及其控制方法，所述控制器包括规划决策主板和运动控制主板；所述规划决策主板用于水下多波束声纳的同步定位与建图，并且能够执行爬游多种运动模式规划和控制指令生成；所述运动控制主板连接爬行和巡游所需的多种传感器和作动机构，执行爬游多模式运动控制功能；本发明能够构建分级、模块化的规划运动构架，满足多模式运动规划与冗余执行机构的协调分配要求，提升爬游无人潜水器运动自主性和可靠性。
Description
技术领域
本发明属于水下无人潜水器的技术领域，具体涉及一种爬游无人潜水器多模式规划运动控制器及其控制方法。
背景技术
无人潜水器是能够搭载作业载荷，在海洋中进行自主作业的潜水器，作为有人船舶及海洋工程装备的重要补充，在海洋开发中具有重要的应用价值。
当前无人潜水器主要有水下自主无人航行器、遥控型无人航行器、海底无人履带车等。水下自主无人航行器一般采用鱼雷形状，能够通过艉推、舵机在水中巡游；遥控型无人航行器由操控手通过脐带缆控制垂推和艉推，开展水下作业；海底无人履带车采用履带式机构在海底运动。爬游无人潜航器是一种新概念潜水器，既可以通过垂推和艉推进行巡游，也具有由多个水密关节组成的机械腿进行海底爬行，既保证了高速的巡游能力，又能实现精确的海底爬行，适用于海底科学考察、环境监测、打捞搜救等需求。
爬游无人潜水器多样化的执行机构增加了控制模式和运动功能，同时也给潜水器的运动规划和控制带来较大困难。由于该潜水器的传感器、执行机构的种类繁多复杂，在运动上既要考虑爬行、巡游每个模式下的运动功能，还需要考虑不同模式之间的转换，同时为了提高潜水器在海底爬行或者巡游的自主性及可靠性，还需要对于海底环境进行感知，并且根据本身的运动状态进行路线规划，从而规避障碍物。爬游无人潜水器扩展了常规的无人潜航器不具备的功能，也导致传统的控制器无法满足该类潜水器的规划运动控制的要求。
因此，需要针对爬游无人潜水器的特点，设计相应的硬件组成结构和软件系统工作流程，构造多模式规划运动控制器，通过分级、模块化构架，实现多种模式下统筹规划与控制分配，引导机器人有序的运动，从而完成爬行规划控制功能。
发明内容
有鉴于此，本发明提供了一种爬游无人潜水器多模式规划运动控制器及其控制方法，能够构建分级、模块化的规划运动构架，满足多模式运动规划与执行机构的协调分配要求，提升爬游无人潜水器运动自主性和可靠性。
实现本发明的技术方案如下：
一种爬游无人潜水器多模式规划运动控制器，包括规划决策主板和运动控制主板；所述规划决策主板用于水下多波束声纳的同步定位与建图，并且能够执行爬游多种运动模式规划和控制指令生成；所述运动控制主板连接爬行和巡游所需的多种传感器和作动机构，执行爬游多模式运动控制功能。
进一步地，所述规划决策主板采用英伟达ARM主板，运行ubuntu操作系统，并安装ROS机器人操作系统。
进一步地，所述规划决策主板通过交换机连接运动控制主板，接收运动控制主板发送的位姿状态估计数据(位置、姿态、航向角)，通过交换机连接多波束声纳，接收声纳探测数据，并执行水下同步定位与建图、路径规划功能，从而根据用户设定的目标点解算潜水器的运动模式(巡游模式、转换模式、爬行模式)和运动指令(对应运动模式下的速度指令、角速度指令)，并通过交换机发送给运动控制主板。
进一步地，所述运动控制主板采用ARM+FPGA主板，通过FPGA扩展多路串口及总线接口，加载Linux操作系统ETHCAT通信主站功能库SOME。
进一步地，所述运动控制主板通过RS232接口连接超短基线水声通信定位应答器，接收用户设定的目标点；通过RS232接口连接多普勒里程计，接收潜水器的速度数据；通过RS232接口连接惯性测量单元，接收潜水器加速度和角速度数据；通过RS485接口连接高深度计，接收潜水器的高度和深度数据；运动控制主板接收规划决策主板的运动模式和运动控制指令，执行状态测量与估计及运动控制功能，解算推进器指令和腿部关节角指令，并通过CAN总线控制前垂推、后垂推、左艉推、右艉推的转速，通过ETHCAT总线控制水下机械腿关节的位置，完成爬行、巡游以及模式切换的运动。
一种爬游无人潜水器多模式规划运动控制方法，以用户设定的目标点作为输入，经过爬游多种传感器的状态测量与估计，完成潜水器位姿状态的精确估计，通过水下声纳的同步定位与建图，获取潜水器在环境地图中的位置，采用路径规划方法求解运动模式(巡游模式、转换模式、爬行模式)和运动指令(对应运动模式下的速度指令、角速度指令)，最终根据运动模式和运动指令，完成巡游推进器、爬行过程机械腿的作动机构的控制分配，各个作动机构协调动作，确保潜水器到达设定的目标点。
进一步地，具体包括以下步骤：
步骤一、状态测量与估计
状态测量与估计运行在运动控制主板，所述运动控制主板以惯性、里程计及步态数据作为测量输入，为同步定位与建图、运动控制提供精确的位姿估计；通过引入步态数据，能够提高爬行过程中的位置姿态估计的准确性；其中，惯性数据为惯性测量单元的加速度和角速度，里程计的速度数据由多普勒里程计提供，步态数据为足端位置数据，所述足端位置数据来源于编码器测量的关节角度，通过机械腿的D-H运动学方程来求解获得；
状态估计算法是以潜水器本体位置、速度、姿态、航向角、足端位置作为状态向量，通过爬游无人潜水器的平移、转动运动学方程构造扩展卡尔曼滤波方程，以测量数据的噪声作为观测误差，采用状态预测、状态更新、增益阵更新、协方差矩阵预测与更新，从而获得滤波后的潜水器位置、速度、姿态、航向角以及足端位置信息；
步骤二、同步定位与建图
同步定位与建图运行在规划决策主板，所述规划决策主板以多波束声纳作为测量输入，并接收潜水器估计的位姿信息，采用ROS系统中openslam建图功能包，实时构建声纳探测地图，并定位潜水器在地图中的位置，作为路径规划的输入信息；
同步定位与建图需要输入声纳探测图像和潜水器的位姿信息；声纳探测数据处理过程为：首先将多波束声纳数据进行极坐标变换，描述为距离方位坐标下的目标强度声纳扇形图，通过对比增强图像处理方式提高探测质量，按照ROS的雷达数据消息机制发布给建图功能包；潜水器的位姿信息处理过程为：将潜水器的位姿估计数据转换为相对于惯性坐标系原点的坐标变换矩阵，并按照ROS坐标变换数据消息机制发布给建图功能包；建图功能包收到上述消息后，采用粒子滤波器方法，设定一系列粒子携带不同时刻下的声纳图像，结合潜水器坐标变换进行微小移动粒子来获取最佳匹配效果，从而更新声纳地图和潜水器在地图中的位置；
步骤三、路径规划
爬游潜水器运行在规划决策主板，所述规划决策主板是以用户设定的目标位置作为输入，根据潜水器在声纳地图中的位置，以运动性能作为约束，实时规划潜水器运动模式(巡游模式、转换模式、爬行模式)和运动控制指令(速度指令、角速度指令)输出给运动控制模块；该过程中首先执行模式转换规划，平稳切换3种运动模式，然后采用ROS系统中导航功能包进行爬行或者巡游模式下的路径规划；
模式转换规划是根据潜水器当前的运动模式以及目标位置来规划高度轨迹；当潜水器正在海底爬行模式，而设定的目标位置高于爬行站立高度时，首先采用匀减速降低爬行速度直至停止；然后进入转换模式，采用梯形速度法生成上升的速度指令，引导潜水器到达指定高度；最后启动巡游模式的路径规划；当潜水器处在巡游模式，而设定的目标位置处于海底时，首先采用匀减速降低巡游速度为0；然后进入转换模式，采用梯形速度法生成下降的速度指令，引导潜水器到达海底；最后启动爬行模式路径规划；
爬行路径规划是以潜水器海底爬行过程中最大速度、最大角速度、最大加速度、障碍物最小距离作为约束，使用ROS系统导航工具包中A*算法全局规划获得潜水器由起点到目标点的全局路径，然后采用动态窗口算法实时规划局部路径，输出爬行速度和角速度指令；
巡游路径规划以潜水器巡游过程中最大速度、最大角速度、最大加速度和障碍物最小距离作为约束，采用与爬行一样的全局和局部路径规划方法，输出巡游速度和角速度指令；
步骤四、运动控制
运动控制模块运行在运动控制主板，所述运动控制主板是以路径规划的运动模式、运动指令作为输入，进行模式选择，并执行对应的巡游控制、模式转换控制或者爬行控制算法，输出推进器的转速指令、机械腿关节角度指令，由各执行机构伺服控制完成指令跟踪；
巡游控制包括4个控制回路：前向速度控制、航向角速度控制、垂直速度控制、俯仰角控制；前三者均采用跟踪误差的比例积分控制器，分别解算艉推转速中值、艉推转速差值、垂推转速中值；俯仰角控制采用跟踪误差的比例积分微分控制器，解算垂推转速差值；左艉推转速指令为艉推转速中值与差值之和，右艉推为两者之差；同理，利用垂推转速中值和差值，得到前垂推和后垂推的转速指令；并且垂直速度指令和俯仰角指令均为0，机械腿各个关节处于潜水器巡游模式下的固定关节角度；
转换控制包括2个控制回路：垂直速度控制、俯仰角控制；所述垂直速度控制指令由模式转换规划生成，俯仰角控制指令为0；当潜水器由爬行转到巡游指定高度后，机械腿各关节角度由爬行模式关节角度采用线性插值转换到巡游模式关节角度；同理，在巡游转爬行过程中，当速度降低为0后，机械腿各关节角度由巡游模式关节角度采用线性插值转换到爬行模式关节角度；
爬行控制包含足端轨迹生成以及逆运动学两部分；足端轨迹生成是根据前向速度指令，采用基本步态规划的抛物线轨迹生成算法，生成摆动腿和支撑腿的足端初始轨迹，同时通过航向角速度指令，采用转向步态修正方法，按照航向角速度指令的比例积分算法计算足端的前向、侧向轨迹的修正值，足端初始轨迹位置与修正值叠加，从而获得最终的足端轨迹位置；逆运动学是以水下机械腿关节连杆D-H动力学为基础，对于摆动腿、支撑腿足端轨迹进行逆运动学解算，求解每个水下机械腿n(n≥2)个关节所需的关节角度，确保爬游潜水器按照设定的爬行速度、角速度运行；该过程中，潜水器艉推和垂推的转速指令均为0。
有益效果：
1.本发明提出一种爬游无人潜水器的多模式规划运动控制器，该控制器构建了一种分级、模块化的规划运动控制器构架，有效协调两种运动模式的运动功能，实现了爬行、巡游平稳转换，避免了潜水器的运动冲突。
2.本发明提出一种爬游无人潜水器的多模式规划运动控制器，该控制器创新性提出了基于多波束声纳的水下同步定位与建图的方法，提升爬游潜水器的环境感知能力，为不同运动模式下的运动规划和避障提供了重要基础。
3.本发明提出一种爬游无人潜水器的多模式规划运动控制器，该控制器采用惯性、里程计及步态数据进行组合滤波来估计潜水器的位置和姿态，充分利用多种测量信息进行数据融合，从而为运动控制和路径规划提供精确的状态信息，确保潜水器运动的准确性。
4.本发明提出一种爬游无人潜水器的多模式规划运动控制器，该控制器结合爬游无人潜水器的工作特点，设计爬行、巡游以及模式转换不同模式下的运动控制方法，实现多模式作动机构的协调控制，确保了系统的可靠、有序的工作。
附图说明
图1是本发明的爬游无人潜水器多模式规划控制器硬件组成结构图。
图2是本发明的爬游无人潜水器多模式规划控制方法原理图。
图3是本发明的爬游无人潜水器多模式规划控制器状态测量与估计原理图。
图4是本发明的爬游无人潜水器多模式规划控制器同步定位与建图工作原理图。
图5是本发明的爬游无人潜水器爬行模式转巡游模式的模式转换规划过程图。
图6是本发明的爬游无人潜水器巡游模式转爬行模式的模式转换规划过程图。
图7是本发明的爬游无人潜水器多模式规划控制器的巡游控制原理图。
图8是本发明的爬游无人潜水器多模式规划控制器的爬行控制原理图。
具体实施方式
下面结合附图并举实施例，对本发明进行详细描述。
本发明所述的一种爬游无人潜水器多模式规划控制器，主要包含控制器的硬件组成结构(如图1所示)和规划控制软件工作流程(如图2所示)。
如图1所示，控制硬件中枢由规划决策主板和运动控制主板两部分组成。
规划决策主板采用英伟达ARM主板，运行ubuntu操作系统，并安装ROS机器人操作系统。该主板通过交换机连接运动控制主板，接收运动控制主板发送的位姿状态估计数据(位置、姿态、航向角)，通过网络交换机连接多波束声纳，接收声纳探测数据，并执行水下同步定位与建图、路径规划功能，然后向运动控制主板发送运动模式和运动控制指令。
运动控制主板为ARM+FPGA主板，通过FPGA扩展多路串口及总线接口，加载Linux操作系统ETHCAT通信主站功能库SOME。该主板通过RS232接口连接超短基线水声通信定位应答器，接收水面监控系统设定的目标位置；通过RS232接口连接多普勒里程计，接收潜水器的速度数据；通过RS232接口连接惯性测量单元，接收加速度和角速度数据；通过RS485接口连接高深度计，接收高度和深度数据。运动控制主板接收规划决策主板的运动模式和运动控制指令，执行状态测量与估计、运动控制功能，并通过CAN总线控制前垂推、后垂推、左艉推、右艉推的转速，通过ETHCAT总线控制水下机械腿关节的位置。
爬游无人潜水器多模式规划控制方法流程如图2所示，以用户设定的目标点作为输入，潜水器经过状态测量与估计完成位姿状态的精确估计，通过声纳同步定位与建图，获取潜水器在环境地图中的位置，采用路径规划方法求解运动模式和运动指令，最终由运动控制完成作动机构的控制分配，确保潜水器到达设定的目标。具体包含以下步骤：
(1)状态测量与估计
由图3所示，状态测量与估计运行在运动控制主板，该模块以惯性、里程计及步态数据作为测量输入，为同步定位与建图、运动控制提供精确的位姿估计。其中，惯性数据为惯性测量单元的加速度和角速度，里程计的速度数据由多普勒里程计提供，步态数据为足端位置数据，足端位置数据来源于编码器测量的关节角度，通过机械腿的D-H运动学方程来求解获得。
状态估计算法是以潜水器本体位置、速度、姿态、航向角、足端位置作为状态向量，通过爬游无人潜水器的平移、转动运动学方程构造扩展卡尔曼滤波方程，以测量数据的噪声作为观测误差，采用状态预测、状态更新、增益阵更新、协方差矩阵预测与更新，从而更新滤波后的潜水器位置、速度、姿态、航向角以及足端位置信息。
(2)同步定位与建图
同步定位与建图运行在规划决策主板，该模块以多波束声纳作为测量输入，并接收潜水器估计的位姿信息，采用ROS系统中openslam建图功能包，实时构建声纳探测地图，并定位潜水器在地图中的位置，作为路径规划的输入信息。
如图4所示，同步定位与建图的功能需要两部分信息：声纳探测图像和潜水器的位姿信息。声纳探测数据处理过程为：首先将多波束声纳数据进行极坐标变换，描述为距离方位坐标下的目标强度声纳扇形图，通过对比增强图像处理方式提高探测质量，按照ROS的雷达数据消息机制发布给建图功能包。潜水器的位姿信息处理过程为：将潜水器的位姿估计数据转换为相对于惯性坐标系原点的坐标变换矩阵，并按照ROS坐标变换数据消息机制发布给建图功能包。建图功能包收到上述消息后，采用粒子滤波器方法，设定一系列粒子携带不同时刻下的声纳图像，结合潜水器坐标变换进行微小移动粒子来获取最佳匹配效果，从而更新声纳地图和潜水器在地图中的位置。
(3)路径规划
爬游潜水器运行在规划决策主板，该模块是以用户设定的目标位置作为输入，根据潜水器在声纳地图中的位置，以运动性能作为约束，实时规划潜水器运动模式和运动控制指令(速度指令、角速度指令)输出给运动控制模块。该过程中首先执行模式转换规划，平稳切换巡游模式、转换模式、爬行模式3种运动模式，然后采用ROS系统中导航功能包进行爬行或者巡游模式下的路径规划。
模式转换规划是根据潜水器当前的运动模式以及目标位置来规划高度轨迹。如图5所示，当潜水器正在海底爬行模式，而设定的目标位置高于爬行站立高度时，首先采用匀减速降低爬行速度直至停止；然后进入转换模式，采用梯形速度法生成上升的速度指令，引导潜水器到达指定高度；最后启动巡游模式的路径规划。如图6所示，当潜水器处在巡游模式，而设定的目标位置处于海底时，首先采用匀减速降低巡游速度为0；然后进入转换模式，采用梯形速度法生成下降速度指令，引导潜水器到达海底；最后启动爬行模式路径规划。
爬行路径规划器是以潜水器海底爬行过程中最大速度、最大角速度、最大加速度、障碍物最小距离作为约束，使用ROS系统导航工具包中A*算法全局规划获得潜水器由起点到目标点的全局路径，然后采用动态窗口算法实时规划局部路径，输出爬行速度和角速度指令。
巡游路径规划器以潜水器巡游过程中最大速度、最大角速度、最大加速度和障碍物最小距离作为约束，采用与爬行一样的全局和局部路径规划方法，输出巡游速度和角速度指令。
(4)运动控制
运动控制模块运行在运动控制主板，该模块是以路径规划的运动模式、运动指令作为输入，进行模式选择，并执行对应的巡游控制、转换控制或者爬行控制算法，输出推进器的转速指令、机械腿关节角度指令，由各执行机构伺服控制完成指令跟踪。
如图7所示，巡游控制主要包括4个控制回路：前向速度控制、航向角速度控制、垂直速度控制、俯仰角控制。前三者均采用跟踪误差的比例积分控制器，分别解算艉推转速中值、艉推转速差值、垂推转速中值；俯仰角控制采用跟踪误差的比例积分微分控制器，解算垂推转速差值。左艉推转速指令为艉推转速中值与差值之和，右艉推为两者之差。同理，可以由垂推转速中值和差值，得到前垂推和后垂推的转速指令。并且垂直速度指令和俯仰角指令均为0，机械腿各个关节处于潜水器巡游模式下的固定关节角度。
转换控制主要包括2个控制回路：垂直速度控制、俯仰角控制。该俯仰角控制与巡游控制过程中的俯仰角控制一致；区别在于垂直速度控制，具体为：垂直速度指令是由模式转换规划生成，俯仰角控制指令为0。当潜水器由爬行转到巡游指定高度后，机械腿各关节角度由爬行模式关节角度采用线性插值转换到巡游模式关节角度。同理，在巡游转爬行过程中，当速度降低为0后，机械腿各关节角度由巡游模式关节角采用线性插值转换到爬行模式关节角度。
如图8所示，爬行控制主要包含了足端轨迹生成以及逆运动学两部分。足端轨迹生成是根据前向速度指令，采用常规基本步态规划的抛物线轨迹生成算法，生成摆动腿和支撑腿的足端初始轨迹，同时通过航向角速度指令，采用转向步态修正方法，按照航向角速度指令的比例积分算法计算足端的前向、侧向轨迹的修正值，足端初始轨迹位置与修正值叠加，从而获得最终的足端轨迹位置。逆运动学是以水下机械腿关节连杆D-H动力学为基础，对于摆动腿、支撑腿足端轨迹进行逆运动学解算，求解每个水下机械腿n(n≥2)个关节所需的关节角度，确保爬游潜水器按照设定的爬行速度、角速度运行。该过程中，潜水器艉推和垂推的转速指令均为0。
综上所述，以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。
Claims (7)
1.一种爬游无人潜水器多模式规划运动控制器，其特征在于，包括规划决策主板和运动控制主板；所述规划决策主板用于水下多波束声纳的同步定位与建图，并且能够执行爬游多种运动模式规划和控制指令生成；所述运动控制主板连接爬行和巡游所需的多种传感器和作动机构，执行爬游多模式运动控制功能。
2.如权利要求1所述的一种爬游无人潜水器多模式规划运动控制器，其特征在于，所述规划决策主板采用英伟达ARM主板，运行ubuntu操作系统，并安装ROS机器人操作系统。
3.如权利要求1所述的一种爬游无人潜水器多模式规划运动控制器，其特征在于，所述规划决策主板通过交换机连接运动控制主板，接收运动控制主板发送的位姿状态估计数据，所述位姿状态估计数据包括位置、姿态、航向角，通过交换机连接多波束声纳，接收声纳探测数据，并执行水下同步定位与建图、路径规划功能，从而根据用户设定的目标点解算潜水器的运动模式和运动指令，所述运动模式包括巡游模式、转换模式、爬行模式，所述运动指令包括对应运动模式下的速度指令、角速度指令，并通过交换机发送给运动控制主板。
4.如权利要求1所述的一种爬游无人潜水器多模式规划运动控制器，其特征在于，所述运动控制主板采用ARM+FPGA主板，通过FPGA扩展多路串口及总线接口，加载Linux操作系统ETHCAT通信主站功能库SOME。
5.如权利要求1所述的一种爬游无人潜水器多模式规划运动控制器，其特征在于，所述运动控制主板通过RS232接口连接超短基线水声通信定位应答器，接收用户设定的目标点；通过RS232接口连接多普勒里程计，接收潜水器的速度数据；通过RS232接口连接惯性测量单元，接收潜水器加速度和角速度数据；通过RS485接口连接高深度计，接收潜水器的高度和深度数据；运动控制主板接收规划决策主板的运动模式和运动控制指令，执行状态测量与估计及运动控制功能，解算推进器指令和腿部关节角指令，并通过CAN总线控制前垂推、后垂推、左艉推、右艉推的转速，通过ETHCAT总线控制水下机械腿关节的位置，完成爬行、巡游以及模式切换的运动。
6.一种爬游无人潜水器多模式规划运动控制方法，其特征在于，以用户设定的目标点作为输入，经过爬游多种传感器的状态测量与估计，完成潜水器位姿状态的精确估计，通过水下声纳的同步定位与建图，获取潜水器在环境地图中的位置，采用路径规划方法求解运动模式和运动指令，所述运动模式包括巡游模式、转换模式、爬行模式，所述运动指令包括对应运动模式下的速度指令、角速度指令，最终根据运动模式和运动指令，完成巡游推进器、爬行过程机械腿的作动机构的控制分配，各个作动机构协调动作，确保潜水器到达设定的目标点。
7.如权利要求6所述的一种爬游无人潜水器多模式规划运动控制方法，其特征在于，具体包括以下步骤：
步骤一、状态测量与估计
状态测量与估计运行在运动控制主板，所述运动控制主板以惯性、里程计及步态数据作为测量输入，为同步定位与建图、运动控制提供精确的位姿估计；通过引入步态数据，能够提高爬行过程中的位置姿态估计的准确性；其中，惯性数据为惯性测量单元的加速度和角速度，里程计的速度数据由多普勒里程计提供，步态数据为足端位置数据，所述足端位置数据来源于编码器测量的关节角度，通过机械腿的D-H运动学方程来求解获得；
状态估计算法是以潜水器本体位置、速度、姿态、航向角、足端位置作为状态向量，通过爬游无人潜水器的平移、转动运动学方程构造扩展卡尔曼滤波方程，以测量数据的噪声作为观测误差，采用状态预测、状态更新、增益阵更新、协方差矩阵预测与更新，从而获得滤波后的潜水器位置、速度、姿态、航向角以及足端位置信息；
步骤二、同步定位与建图
同步定位与建图运行在规划决策主板，所述规划决策主板以多波束声纳作为测量输入，并接收潜水器估计的位姿信息，采用ROS系统中openslam建图功能包，实时构建声纳探测地图，并定位潜水器在地图中的位置，作为路径规划的输入信息；
同步定位与建图需要输入声纳探测图像和潜水器的位姿信息；声纳探测数据处理过程为：首先将多波束声纳数据进行极坐标变换，描述为距离方位坐标下的目标强度声纳扇形图，通过对比增强图像处理方式提高探测质量，按照ROS的雷达数据消息机制发布给建图功能包；潜水器的位姿信息处理过程为：将潜水器的位姿估计数据转换为相对于惯性坐标系原点的坐标变换矩阵，并按照ROS坐标变换数据消息机制发布给建图功能包；建图功能包收到上述消息后，采用粒子滤波器方法，设定一系列粒子携带不同时刻下的声纳图像，结合潜水器坐标变换进行微小移动粒子来获取最佳匹配效果，从而更新声纳地图和潜水器在地图中的位置；
步骤三、路径规划
爬游潜水器运行在规划决策主板，所述规划决策主板是以用户设定的目标位置作为输入，根据潜水器在声纳地图中的位置，以运动性能作为约束，实时规划潜水器运动模式和运动控制指令输出给运动控制模块；该过程中首先执行模式转换规划，平稳切换3种运动模式，然后采用ROS系统中导航功能包进行爬行或者巡游模式下的路径规划；
模式转换规划是根据潜水器当前的运动模式以及目标位置来规划高度轨迹；当潜水器正在海底爬行模式，而设定的目标位置高于爬行站立高度时，首先采用匀减速降低爬行速度直至停止；然后进入转换模式，采用梯形速度法生成上升的速度指令，引导潜水器到达指定高度；最后启动巡游模式的路径规划；当潜水器处在巡游模式，而设定的目标位置处于海底时，首先采用匀减速降低巡游速度为0；然后进入转换模式，采用梯形速度法生成下降的速度指令，引导潜水器到达海底；最后启动爬行模式路径规划；
爬行路径规划是以潜水器海底爬行过程中最大速度、最大角速度、最大加速度、障碍物最小距离作为约束，使用ROS系统导航工具包中A*算法全局规划获得潜水器由起点到目标点的全局路径，然后采用动态窗口算法实时规划局部路径，输出爬行速度和角速度指令；
巡游路径规划以潜水器巡游过程中最大速度、最大角速度、最大加速度和障碍物最小距离作为约束，采用与爬行一样的全局和局部路径规划方法，输出巡游速度和角速度指令；
步骤四、运动控制
运动控制模块运行在运动控制主板，所述运动控制主板是以路径规划的运动模式、运动指令作为输入，进行模式选择，并执行对应的巡游控制、模式转换控制或者爬行控制算法，输出推进器的转速指令、机械腿关节角度指令，由各执行机构伺服控制完成指令跟踪；
巡游控制包括4个控制回路：前向速度控制、航向角速度控制、垂直速度控制、俯仰角控制；前三者均采用跟踪误差的比例积分控制器，分别解算艉推转速中值、艉推转速差值、垂推转速中值；俯仰角控制采用跟踪误差的比例积分微分控制器，解算垂推转速差值；左艉推转速指令为艉推转速中值与差值之和，右艉推为两者之差；同理，利用垂推转速中值和差值，得到前垂推和后垂推的转速指令；并且垂直速度指令和俯仰角指令均为0，机械腿各个关节处于潜水器巡游模式下的固定关节角度；
转换控制包括2个控制回路：垂直速度控制、俯仰角控制；所述垂直速度控制指令由模式转换规划生成，俯仰角控制指令为0；当潜水器由爬行转到巡游指定高度后，机械腿各关节角度由爬行模式关节角度采用线性插值转换到巡游模式关节角度；同理，在巡游转爬行过程中，当速度降低为0后，机械腿各关节角度由巡游模式关节角度采用线性插值转换到爬行模式关节角度；
爬行控制包含足端轨迹生成以及逆运动学两部分；足端轨迹生成是根据前向速度指令，采用基本步态规划的抛物线轨迹生成算法，生成摆动腿和支撑腿的足端初始轨迹，同时通过航向角速度指令，采用转向步态修正方法，按照航向角速度指令的比例积分算法计算足端的前向、侧向轨迹的修正值，足端初始轨迹位置与修正值叠加，从而获得最终的足端轨迹位置；逆运动学是以水下机械腿关节连杆D-H动力学为基础，对于摆动腿、支撑腿足端轨迹进行逆运动学解算，求解每个水下机械腿n个关节所需的关节角度，n≥2，确保爬游潜水器按照设定的爬行速度、角速度运行；该过程中，潜水器艉推和垂推的转速指令均为0。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111229823.8A CN114063624A (zh) | 2021-10-22 | 2021-10-22 | 一种爬游无人潜水器多模式规划运动控制器及其控制方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111229823.8A CN114063624A (zh) | 2021-10-22 | 2021-10-22 | 一种爬游无人潜水器多模式规划运动控制器及其控制方法 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN114063624A true CN114063624A (zh) | 2022-02-18 |
Family
ID=80235172
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111229823.8A Pending CN114063624A (zh) | 2021-10-22 | 2021-10-22 | 一种爬游无人潜水器多模式规划运动控制器及其控制方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114063624A (zh) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114932555A (zh) * | 2022-06-14 | 2022-08-23 | 如你所视(北京)科技有限公司 | 机械臂协同作业系统及机械臂控制方法 |
| CN114998491A (zh) * | 2022-08-01 | 2022-09-02 | 阿里巴巴（中国）有限公司 | 数字人驱动方法、装置、设备及存储介质 |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106394833A (zh) * | 2016-05-20 | 2017-02-15 | 中国船舶重工集团公司第七〇九研究所 | 一种爬游混合型无人潜水器及其使用方法 |
| CN207502724U (zh) * | 2017-12-07 | 2018-06-15 | 中国船舶重工集团公司第七一九研究所 | 一种海生物多声纳组网探测定位系统 |
| CN109189057A (zh) * | 2018-07-03 | 2019-01-11 | 中国船舶重工集团公司第七〇九研究所 | 一种深海爬游机器人自主避障仿真实验系统 |
| CN110262495A (zh) * | 2019-06-26 | 2019-09-20 | 山东大学 | 可实现移动机器人自主导航与精确定位的控制系统及方法 |
| CA3067575A1 (en) * | 2019-01-14 | 2020-07-14 | Harbin Engineering University | Self-learning autonomous navigation systems and methods for unmanned underwater vehicle |
| CN112034735A (zh) * | 2020-08-31 | 2020-12-04 | 浙江大学 | 一种多auv水下协同作业的模拟实验平台 |
| JP2021034050A (ja) * | 2019-08-21 | 2021-03-01 | 哈爾浜工程大学 | 強化学習に基づくａｕｖ行動計画及び動作制御方法 |
-
2021
- 2021-10-22 CN CN202111229823.8A patent/CN114063624A/zh active Pending
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106394833A (zh) * | 2016-05-20 | 2017-02-15 | 中国船舶重工集团公司第七〇九研究所 | 一种爬游混合型无人潜水器及其使用方法 |
| CN207502724U (zh) * | 2017-12-07 | 2018-06-15 | 中国船舶重工集团公司第七一九研究所 | 一种海生物多声纳组网探测定位系统 |
| CN109189057A (zh) * | 2018-07-03 | 2019-01-11 | 中国船舶重工集团公司第七〇九研究所 | 一种深海爬游机器人自主避障仿真实验系统 |
| CA3067575A1 (en) * | 2019-01-14 | 2020-07-14 | Harbin Engineering University | Self-learning autonomous navigation systems and methods for unmanned underwater vehicle |
| CN110262495A (zh) * | 2019-06-26 | 2019-09-20 | 山东大学 | 可实现移动机器人自主导航与精确定位的控制系统及方法 |
| JP2021034050A (ja) * | 2019-08-21 | 2021-03-01 | 哈爾浜工程大学 | 強化学習に基づくａｕｖ行動計画及び動作制御方法 |
| CN112034735A (zh) * | 2020-08-31 | 2020-12-04 | 浙江大学 | 一种多auv水下协同作业的模拟实验平台 |
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114932555A (zh) * | 2022-06-14 | 2022-08-23 | 如你所视(北京)科技有限公司 | 机械臂协同作业系统及机械臂控制方法 |
| CN114932555B (zh) * | 2022-06-14 | 2024-01-05 | 如你所视(北京)科技有限公司 | 机械臂协同作业系统及机械臂控制方法 |
| CN114998491A (zh) * | 2022-08-01 | 2022-09-02 | 阿里巴巴（中国）有限公司 | 数字人驱动方法、装置、设备及存储介质 |
| CN114998491B (zh) * | 2022-08-01 | 2022-11-18 | 阿里巴巴（中国）有限公司 | 数字人驱动方法、装置、设备及存储介质 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN102768539B (zh) | 基于迭代的自主水下航行器三维曲线路径跟踪控制方法 | |
| CN109050835B (zh) | 全驱动自主水下机器人结构及回收三维路径跟踪方法 | |
| CN114063624A (zh) | 一种爬游无人潜水器多模式规划运动控制器及其控制方法 | |
| Huang et al. | Vehicle-manipulator system dynamic modeling and control for underwater autonomous manipulation | |
| Wang et al. | A simultaneous planning and control method integrating APF and MPC to solve autonomous navigation for USVs in unknown environments | |
| Mas et al. | Centralized and decentralized multi-robot control methods using the cluster space control framework | |
| CN112148022A (zh) | 全驱动自主水下机器人回收三维路径跟踪控制系统及方法 | |
| Duecker et al. | HippoCampusX–A hydrobatic open-source micro AUV for confined environments | |
| CN113296505B (zh) | 一种基于速变los的无人船多模式路径跟踪控制方法 | |
| Yang et al. | A hybrid autonomous recovery scheme for AUV based dubins path and non-singular terminal sliding mode control method | |
| Sans-Muntadas et al. | Spiral path planning for docking of underactuated vehicles with limited FOV | |
| Borges de Sousa et al. | A verified hierarchical control architecture for co‐ordinated multi‐vehicle operations | |
| CN111872938B (zh) | 空间三维大尺度运动学仿真系统及方法 | |
| Rife et al. | Design and validation of a robotic control law for observation of deep-ocean jellyfish | |
| CN116027796A (zh) | 一种多自治水下机器人编队控制系统及方法 | |
| An et al. | Underwater Motion Characteristics Evaluation of a Bio-inspired Father-son Robot | |
| Gavrilina et al. | Singularity-free attitude control of the unmanned underwater vehicle | |
| Caccia et al. | Guidance of unmanned underwater vehicles: Experimental results | |
| Emrani et al. | An adaptive leader-follower formation controller for multiple AUVs in spatial motions | |
| Renjie et al. | Underwater robot formation control based on leader-follower model | |
| Wang et al. | Stepwise cooperative trajectory planning for multiple BUVs based on temporal–spatial Bezier curves | |
| Khorrami et al. | A hierarchical path planning and obstacle avoidance system for an autonomous underwater vehicle | |
| Caccia et al. | Sonar-based guidance of unmanned underwater vehicles | |
| Chen et al. | Improved multi-uuv formation control for artificial potential fields and virtual navigators | |
| Wan et al. | Modeling and motion control strategy for AUV |
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 | ||
| WD01 | Invention patent application deemed withdrawn after publication |
Application publication date: 20220218 |
|
| WD01 | Invention patent application deemed withdrawn after publication |