CN111208845B - 一种水下机器人移动式对接回收的路径规划方法及装置 - Google Patents

一种水下机器人移动式对接回收的路径规划方法及装置 Download PDF

Info

Publication number
CN111208845B
CN111208845B CN202010198550.4A CN202010198550A CN111208845B CN 111208845 B CN111208845 B CN 111208845B CN 202010198550 A CN202010198550 A CN 202010198550A CN 111208845 B CN111208845 B CN 111208845B
Authority
CN
China
Prior art keywords
gravitation
underwater robot
point
force
positioning information
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.)
Active
Application number
CN202010198550.4A
Other languages
English (en)
Other versions
CN111208845A (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.)
Harbin Engineering University
Original Assignee
Harbin Engineering University
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 Harbin Engineering University filed Critical Harbin Engineering University
Priority to CN202010198550.4A priority Critical patent/CN111208845B/zh
Publication of CN111208845A publication Critical patent/CN111208845A/zh
Application granted granted Critical
Publication of CN111208845B publication Critical patent/CN111208845B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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/10—Simultaneous control of position or course in three dimensions

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

一种水下机器人移动式对接回收的路径规划方法及装置
技术领域
本发明涉及水下机器人领域，具体而言，涉及一种水下机器人移动式对接回收的路径规划方法及装置。
背景技术
随着我国航海事业的发展，越来越多的深海探测任务需要水下机器人(Autonomous Underwater Vehicle，简称AUV)的参与，比如水下侦查、海底施工、大范围且长航程的工作网络扩展和组建等工作。由于现有的水下通讯能力的限制性以及自身能源的有限性，AUV在深海执行任务的过程中，在能源系统警告、作业数据容量饱和或者先期任务完成之后，需要与坞站进行对接动作，以此完成AUV与坞站之间的能量交换、数据上传、任务下载、位置矫正或载体维护等任务。自主水下对接技术是完成AUV与坞站之间对接的关键技术，在种种深海探测任务中起着至关重要的作用，而AUV在与坞站的对接过程中，容易受到了如作业深度和范围受限等诸多不利因素的影响。如何排除这些影响，实现高效率、高精度的自主水下对接技术是水下机器人领域中需要探索的重要问题。
现有的、较为成熟的自主水下对接技术主要是以固定式对接为主，即AUV完成阶段性工作后，归航并对接到某一固定在水下特定位置的坞站中。固定式对接对路径规划和跟踪控制的要求相对较低，对接成功率高，但一方面，固定式对接往往需要提前布置好回收装置，灵活性不高，不适宜完成复杂的深海探测任务；另一方面，固定式对接的对接装置设计复杂，且容易受海洋附着物的影响，维护难度较大。由此，一种移动式自主水下对接技术应运而生。
移动式自主水下对接技术是指对接装置处于运动状态下，AUV与对接装置完成对接。一般而言，对接装置搭载于大型水下航行器上，移动式自主水下对接技术要求AUV能在回收过程中能准确捕捉大型水下航行器的位姿信息，同时也对两个相对运动物体的路径规划和跟踪控制提出了更高的要求。因而，在移动式自主水下对接技术中，实现准确且高效的对接路径路径规划是必不可少的。
水下机器人根据驱动特性即推力器的配置分为全驱动、欠驱动和过驱动。欠驱动AUV在绝对速度较快时，舵效高，操纵性能好，出于制造成本和能源消耗的考虑，执行探测任务时一般采用欠驱动AUV。因此，有必要针对欠驱动AUV，提出一种可靠实用的移动式自主水下对接路径规划的方法。
综上，现有的自主水下对接技术主要是以固定式对接为主，存在灵活性低、成本过高的缺陷。为了实现低成本、高准度的自主水下对接，一种针对水下机器人的移动式对接回收的路径规划方法亟待提出。
发明内容
有鉴于此，本发明旨在至少在一定程度上解决相关技术中的技术问题。为达上述目的，本发明提供了一种水下机器人移动式对接回收的路径规划方法，包括：
获取回收水下机器人的第一定位信息、移动坞站的第二定位信息以及所述待回收水下机器人的水动力参数；
根据所述第一定位信息和所述水动力参数，建立所述待回收水下机器人的空间六自由度运动模型；
根据所述第二定位信息设置位置引力点和姿态引力点，根据所述位置引力点和所述姿态引力点确定虚拟引力,其中，所述位置引力点用于划分距离范围，所述姿态引力点用于划分角度范围；
根据所述虚拟引力，确定所述待回收水下机器人的推力、垂直舵力以及水平舵力；
将所述推力、所述垂直舵力以及所述水平舵力带入所述空间六自由度运动模型，确定所述待回收水下机器人的下一时刻位姿，并获取所述移动坞站的下一时刻位姿；
判断所述待回收水下机器人的下一时刻位姿是否与所述移动坞站的下一时刻位姿达到一致，若达到，则结束路径规划任务。
由此，本发明通过通过在人工势场法中设置位置引力点和姿态引力点求得虚拟引力，并针对欠驱动的待回收水下机器人的运动特性得到推力、垂直舵力和水平舵力，进一步规划待回收水下机器人移动式对接路径，从而使得路径末端的待回收水下机器人和移动坞站的位姿趋于一致。综上，本发明充分利用了待回收水下机器人的欠驱动运动特性，利用位置引力点和姿态引力点求得虚拟引力，再将虚拟引力整合成推力、垂直舵力、水平舵力，最后利用推力、垂直舵力、水平舵力完成了准确的待回收水下机器人的末端位姿求解，实现了高效的水下移动式对接路径规划，保证了待回收水下机器人和移动坞站的高精度对接。
进一步地，所述第一定位信息包括所述待回收水下机器人的重心，所述根据所述第一定位信息和所述水动力参数，建立所述待回收水下机器人的空间六自由度运动模型，包括：
根据所述水动力参数,确定六个自由度上的静水力和动水力的合外力；
将所述水动力参数和所述合外力代入六自由度运动模型等式中，以所述重心为原点，建立所述空间六自由度运动模型。
由此，利用第一定位信息和水动力参数确定待回收机器人的空间六自由度运动模型，通过建立好的空间六自由度运动模型，充分考虑待回收水下机器人的运动特性，保证进一步准确求解待回收机器人的末端位姿。
进一步地，所述虚拟引力包括主动力和主动力矩，所述根据所述第二定位信息设置位置引力点和姿态引力点，根据所述位置引力点和所述姿态引力点确定虚拟引力，包括：
根据所述第二定位信息设置所述位置引力点，再根据所述位置引力点确定所述主动力，其中，所述主动力包括纵向引力、横向引力以及垂向引力；
根据所述第二定位信息设置所述姿态引力点，再根据所述姿态引力点确定所述主动力矩，其中，所述主动力矩包括艏向引力矩、俯仰引力矩。
由此，位置引力点产生主动力，通过将位置引力分解成坐标轴上三个方向的力，有效求得纵向引力、横向引力和垂向引力；而姿态引力点产生主动力矩，主动力矩包括艏向引力矩、俯仰引力矩。因而，本发明有效利用欠驱动水下机器人的运动特性，准确求解纵向引力、横向引力、垂向引力、艏向引力矩和俯仰引力矩，有利于进一步整合求解待回收水下机器人的主动控制力(推力、垂直舵力、水平舵力)，保证了高效准确的路径规划控制方法。
进一步地，所述位置引力点包括距离控制点，所述根据所述第二定位信息设置所述位置引力点，再根据所述位置引力点确定主动力，包括：
根据所述第二定位信息，通过坐标转换公式得到所述待回收水下机器人和所述坞站在随体坐标系下的相对矢量距离和相对标量距离；
根据所述第二定位信息，设置所述距离控制点、最大限制吸引力和第一常数；
根据所述距离控制点、所述最大限制吸引力、所述第一常数、所述相对矢量距离和所述相对标量距离，确定所述纵向引力、所述横向引力、所述垂向引力。
由此，通过计算相对标量距离，将其作为调节主动力大小的标准，充分考虑到待回收机器人和移动坞站之间的相对距离对主动力的影响，有效利用两者的相对运动特性，得到准确的主动力，进而保证了本发明路径规划的有效性。
进一步地，所述根据所述距离控制点、所述最大限制吸引力、所述第一常数、所述相对矢量距离和所述相对标量距离，确定纵向引力、横向引力以及垂向引力，包括：
根据所述距离控制点、所述相对标量距离和第一常数，确定第一权重；
判断所述距离控制点和所述相对标量距离是否满足第一预设条件；
若满足所述第一预设条件，则根据所述最大限制吸引力、所述第一权重、所述相对矢量距离确定总矢量引力；
根据所述总矢量引力确定所述纵向引力、所述横向引力和所述垂向引力，其中，所述总矢量引力包括沿Y轴方向的第一引力、沿X轴方向的第二引力和沿Z轴方向的第三引力，所述纵向引力为所述第一引力，所述横向引力为所述第二引力，所述垂向引力为所述第三引力。
由此，根据待回收水下机器人和移动坞站相对运动的特性，通过设置距离控制点，判断两者之间的相对标量距离是否超过距离控制点，即判断两者距离是否超过一定的范围。依据此，再利用最大限制吸引力、第一权重、相对矢量距离求解总矢量引力。由于总矢量引力是一个矢量，包含三个方向的力，根据这三个方向的力可以确定沿X、Y、Z轴的推力，即纵向引力、横向引力和垂向引力，进而保证了路径规划中求解各个方向的控制力的准确性。
进一步地，所述若满足所述第一预设条件，则根据所述最大限制吸引力、所述第一权重、所述相对矢量距离确定总矢量引力，包括：
当所述相对标量距离大于所述距离控制点，则根据所述最大限制吸引力和所述相对矢量距离的乘积确定所述总矢量引力；
当所述相对标量距离小于或等于所述距离控制点，则根据所述最大限制吸引力、所述相对矢量距离和所述第一权重的乘积确定所述总矢量引力。
由此，当待回收水下机器人和移动坞站的距离超过距离控制点的范围，所需总矢量引力较大，因而利用最大限制吸引力和相对矢量距离的乘积确定总矢量引力；而当待回收水下机器人和移动坞站的距离在距离控制点的范围内时，所需总矢量引力较小，此时利用第一权重有效求得合适的总矢量引力。总体上，本发明通过距离控制点的设置，依据两者相对距离的大小，设置合适范围的主控制力，使路径规划的控制方法更具合理性。
进一步地，所述姿态引力点包括艏向角引力点和俯仰角引力点；所述根据所述定位信息设置所述姿态引力点，再根据所述姿态引力点计算主动力矩，包括：
根据所述第二定位信息设置艏向角引力点和最大垂直舵力，再根据所述第一定位信息、所述第二定位信息、所述艏向角引力点和所述最大垂直舵力确定所述艏向引力矩；
根据所述第二定位信息设置俯仰角引力点和最大水平舵力，再根据所述第一定位信息、所述第二定位信息、所述俯仰角引力点和所述最大水平舵力确定所述俯仰引力矩。
由此，根据欠驱动水下机器人的运动特性，通过设置艏向角引力点以确定艏向引力矩、设置俯仰角引力点以确定俯仰引力矩，以此确定主动力炬，以便后续整合求解待回收水下机器人的主动控制力(推力、垂直舵力、水平舵力)，保证了路径规划控制方法的有效性。
进一步地，所述第一定位信息包括第一艏向角，所述第一艏向角为所述待回收水下机器人的艏向角，所述第二定位信息包括第二艏向角，所述第二艏向角为所述移动坞站的艏向角；所述再根据所述第一定位信息、所述第二定位信息、所述艏向角引力点和所述最大垂直舵力确定所述艏向引力矩，包括：
根据所述第一艏向角和所述第二艏向角之差确定相对艏向角；
设置第二常数，根据所述第二常数和所述相对艏向角确定第二权重；
若所述相对艏向角大于所述艏向角引力点，则所述艏向引力矩为所述最大垂直舵力；
若所述相对艏向角小于或所述艏向角引力点，则所述艏向引力矩为所述最大垂直舵力和所述第二权重的乘积。
由此，根据待回收水下机器人和移动坞站相对运动的特性，通过设置艏向角引力点，判断两者之间的相对艏向角是否超过艏向角引力点，即判断两者相对艏向角是否超过一定的范围。依据此，再利用最大垂直舵力、第二权重求解艏向引力矩，进而保证了路径规划中求解艏向引力矩的准确性。
进一步地，所述第一定位信息包括第一俯仰角，所述第一俯仰角为所述待回收水下机器人的俯仰角，所述第二定位信息包括第二俯仰角，所述第二俯仰角为所述移动坞站的俯仰角，所述再根据所述第一定位信息、所述第二定位信息、所述俯仰角引力点和所述最大水平舵力确定所述俯仰引力矩，包括：
根据所述第一俯仰角和所述第二俯仰角之差确定相对俯仰角；
设置第三常数，根据所述第三常数和所述相对俯仰角确定第三权重；
若所述相对俯仰角大于所述俯仰角引力点，则所述俯仰引力矩为所述最大水平舵力；
若所述相对俯仰角小于或所述俯仰角引力点，则所述俯仰引力矩为所述最大水平舵力和所述第三权重的乘积。
由此，根据待回收水下机器人和移动坞站相对运动的特性，通过设置俯仰角引力点，判断两者之间的相对俯仰角是否超过俯仰角引力点，即判断两者相对俯仰角是否超过一定的范围。依据此，再利用最大水平舵力、第三权重求解俯仰引力矩，进而保证了路径规划中求解俯仰引力矩的准确性。
进一步地，所述虚拟引力包括主动力和主动力矩，所述主动力包括纵向引力、横向引力和垂向引力，所述主动力矩包括艏向引力矩和俯仰引力矩；所述根据所述虚拟引力，确定所述待回收水下机器人的推力、垂直舵力、水平舵力，所述根据所述虚拟引力，确定所述待回收水下机器人的推力、垂直舵力、水平舵力，包括：
根据所述纵向引力确定所述推力；
根据所述横向引力和所述艏向引力矩之和确定所述垂直舵力；
根据所述垂向引力和所述俯仰引力矩之和确定所述水平舵力。
由此，根据待回收水下机器人和移动坞站相对运动的特性，通过对纵向引力、横向引力、垂向引力、艏向引力矩和俯仰引力矩的整合，得到推力、垂直舵力和水平舵力，进而保证了路径规划中求解俯仰引力矩的准确性。
进一步地，所述判断所述待回收水下机器人的下一时刻位姿是否与所述移动坞站的下一时刻位姿达到一致，包括：
判断所述待回收水下机器人和所述移动坞站下一时刻的相对距离是否达到第一预设范围，其中，所述相对距离包括相对纵向距离、相对横向距离和相对垂向距离；
和/或判断所述待回收水下机器人和所述移动坞站下一时刻的相对角度是否达到第二预设范围，其中，所述相对角度包括下一时刻的相对俯仰角、相对艏向角。
由此，从相对距离和相对角度两个方面，判断待回收水下机器人和移动坞站的末端位姿是否一致，保证待回收水下机器人能高效无误地完成归航时的对接。
本发明的第二目的在于提供一种水下机器人移动式对接回收的路径规划控制的装置，充分利用了待回收水下机器人的欠驱动运动特性，利用位置引力点和姿态引力点，完成了准确的待回收水下机器人的末端位姿求解，实现了高效的水下移动式对接路径规划，保证了待回收水下机器人和移动坞站的高精度对接。
为达到上述目的，本发明的技术方案是这样实现的：
一种水下机器人移动式对接回收的路径规划的装置，包括：
获取单元，用于获取定位信息、待回收水下机器人的水动力参数，其中，所述定位信息包括所述待回收水下机器人的第一定位信息和移动坞站的第二定位信息；
处理单元，用于根据所述第一定位信息和所述水动力参数，建立所述待回收水下机器人的空间六自由度运动模型；用于根据所述第二定位信息设置位置引力点和姿态引力点，根据所述位置引力点和所述姿态引力点确定虚拟引力,其中，所述位置引力点用于划分距离范围，所述姿态引力点用于划分角度范围；用于根据所述虚拟引力，整合所述待回收水下机器人的推力、垂直舵力、水平舵力；用于将所述推力、所述垂直舵力、所述水平舵力带入所述空间六自由度运动模型，求解得到所述待回收水下机器人的下一时刻位姿，并获取所述移动坞站的下一时刻位姿；
控制单元，用于判断所述待回收水下机器人的下一时刻位姿是否与所述移动坞站的下一时刻位姿达到一致，若达到，则结束路径规划任务。
所述水下机器人移动式对接回收的路径规划的装置与水下机器人移动式对接回收的路径规划方法相对于现有技术所具有的有益效果相同，在此不再赘述。
本发明的第三目的在于提供一种计算机可读存储介质，充分利用了待回收水下机器人的欠驱动运动特性，利用位置引力点和姿态引力点，完成了准确的待回收水下机器人的末端位姿求解，实现了高效的水下移动式对接路径规划，保证了待回收水下机器人和移动坞站的高精度对接。
为达到上述目的，本发明的技术方案是这样实现的：
一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器读取并运行时，实现上述的水下机器人移动式对接回收的路径规划方法。
所述计算机可读存储介质与上述的水下机器人移动式对接回收的路径规划方法相对于现有技术所具有的有益效果相同，在此不再赘述。
附图说明
图1所示为本发明实施例的水下机器人移动式对接回收的路径规划方法的流程示意图；
图2所示为本发明实施例的待回收水下机器人和移动坞站相对位置的示意图；
图3所示为本发明实施例的待回收水下机器人和移动坞站艏向角度差的示意图；
图4所示为本发明实施例的待回收水下机器人和移动坞站俯仰角度差的示意图；
图5所示为本发明实施例的待回收水下机器人和移动坞站对接示意图；
图6所示为本发明实施例的水下机器人移动式对接回收的路径规划装置的结构示意图。
具体实施方式
下面将参照附图详细描述根据本发明的实施例，描述涉及附图时，除非另有表示，不同附图中的相同附图标记表示相同或相似的要素。要说明的是，以下示例性实施例中所描述的实施方式并不代表本发明的所有实施方式。它们仅是与如权利要求书中所详述的、本发明公开的一些方面相一致的装置和方法的例子，本发明的范围并不局限于此。在不矛盾的前提下，本发明各个实施例中的特征可以相互组合。
此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个该特征。在本发明的描述中，“多个”的含义是至少两个，例如两个，三个等，除非另有明确具体的限定。
现有的水下机器人对接技术主要以固定式对接为主，在固定式对接技术中，路径规划的详细步骤为：
1)基于预知的基站位姿信息,控制水下机器人航行至对接起始点,预知的基站位姿信息包括对接装置在大地坐标系下的GPS纬度、GPS经度、相对水平面深度、横滚角、俯仰角及艏向角；
2)分析水下机器人的欠驱动特性带来的几何约束问题，包括任务终端约束和运动约束。
3)根据B样条曲线的特性确定通过选取控制点序列来给出3维路径曲线。其中，首先采用样板的方式确定一部分控制点使曲线满足终端约束条件，然后将水下机器人的回转和升沉运动约束写入遗传算法，通过对解空间的启发式自适应搜索确定中间控制点，两部分控制点所决定的曲线满足所有的几何约束条件时，则生成路径。
然而，现有固定式对接技术灵活性不高、对接装置设计复杂且维护难度较大，因而不适宜完成诸多复杂的深海探测任务。为了结合移动式对接技术的灵活性以及欠驱动水下机器人的可操纵性，实现低成本、高准度的自主水下对接，本发明提出了一种水下机器人移动式对接回收的路径规划控制方法。
本发明实施例提供了一种水下机器人移动式对接回收的路径规划控制方法。图1所示为本发明实施例的水下机器人移动式对接回收的路径规划方法的流程示意图，包括步骤S1至步骤S6，其中：
在步骤S1中，获取回收水下机器人的第一定位信息、移动坞站的第二定位信息以及所述待回收水下机器人的水动力参数。在本发明实施例中，水动力参数包括待回收水下机器人在静水、动水中的运动参数，包括质量和转动惯量等，以此计算静水力和动水力；第一定位信息包括待回收水下机器人的初始位置和初始姿态，比如在惯性坐标系下的坐标、横滚角、俯仰角、艏向角等；第二定位信息包括待移动坞站的初始位置和初始姿态，比如在惯性坐标系下的坐标、横滚角、俯仰角、艏向角等。由此，通过获取以上初始信息，有效地捕捉到待回收水下机器人和移动坞站的相对状态，以此建立准确的空间六自由度运动模型，保证求解末端位姿的准确性，以此进一步保证两者末端位姿一致。
在步骤S2中，根据第一定位信息和水动力参数，建立待回收水下机器人的空间六自由度运动模型。通过第一定位信息中的坐标、横滚角、俯仰角、艏向角以及在静水、动水中的水动力参数，利用待回收水下机器人自身运动特性，建立有效表征其运动特性的空间六自由度运动模型，保证后续准确求解下一时刻的位姿求解。
可选地，第一定位信息包括待回收水下机器人的重心。由此，将待回收水下机器人的重心作为原点，有效地建立空间六自由度运动模型。
在本发明实施例中，步骤2包括步骤S21至步骤S22。
在步骤S21中，根据水动力参数,确定六个自由度上的静水力和动水力的合外力。由于空间六自由度运动模型的等式右边是合外力，而合外力是由静水力和动水力组成，因而首先应根据水动力参数，求得等式右边的合外力。其中，六个自由度上的静水力分别为:
Figure BDA0002418518010000111
Figure BDA0002418518010000122
在公式组(1)中，XHS表示纵向静水力，YHS表示横向静水力，ZHS表示垂向静水力，KHS表示横滚静水力矩，MHS表示俯仰静水力矩，NHS表示转艏静水力矩，P表示AUV重力，B表示AUV浮力，θ表示俯仰角，φ表示横滚角，xG表示AUV重心纵向坐标，yG表示AUV重心横向坐标，zG表示AUV重心垂向坐标，xb表示AUV浮心纵向坐标，yb表示AUV浮心横向坐标，zb表示AUV浮心垂向坐标。
各自由度上的动水力分别为:
Figure BDA0002418518010000121
在公式组(2)中：
XHD表示纵向水动力，Xu|u|表示由纵向速度引起的纵向力水动力导数，Xu表示纵向加速度引起的附加质量，Xwq表示由垂向速度和俯仰角速度引起的纵向力水动力导数，Xqq表示由俯仰角速度引起的纵向力水动力导数，Xvr表示由横向速度和转艏角速度引起的纵向水动力导数，Xrr表示由转艏角速度引起的纵向水动力导数，XT表示推进器推力；
YHD表示横向水动力，Yv|v|表示由横向速度引起的横向水动力导数，Yr|r|表示由转艏角速度引起的横向水动力导数，Yv表示由横向加速度引起的附加质量，Yr表示由转艏角加速度引起的附加质量，Yur表示由纵向速度和转艏角速度引起的横向水动力导数，Ywp表示由垂向速度和横滚角速度引起的横向水动力导数，Ypq表示横滚角速度和俯仰角速度引起的横向水动力导数，Yuv表示由纵向速度和横向速度引起的横向水动力导数，
Figure BDA0002418518010000131
表示由横向速度和垂直舵引起的横向水动力导数；
ZHD表示垂向水动力，Zw|w|表示由垂向速度引起的垂向水动力导数，Zq|q|表示由俯仰角速度引起的垂向水动力导数，Zw表示由垂向加速度引起的附加质量，Zq表示由俯仰角加速度引起的附加质量，Zuq表示由纵向速度和俯仰角速度引起的垂向水动力导数，Zvp表示由横向速度和横滚角速度引起的垂向水动力导数，Zrp表示由转艏角速度和横滚角速度引起的垂向水动力导数，Zuw表示由纵向速度和垂向速度引起的垂向水动力导数，
Figure BDA0002418518010000134
表示由纵向速度和水平舵引起的垂向水动力导数；
KHD表示横滚水动力矩，Kp|p|表示由横滚角速度引起的横滚水动力矩导数，Kp表示由横滚角加速度引起的附加质量，Kprop表示推进器扭矩；
MHD表示俯仰水动力矩，Mw|w|表示由垂向速度引起的俯仰水动力矩导数，Mq|q|表示由俯仰角速度引起的俯仰水动力导数，Mw表示由垂向加速度引起的附加质量，Mq表示由俯仰角加速度引起的附加质量，Muq表示由纵向速度和俯仰角速度引起的俯仰水动力矩导数，Mvp表示由横向速度和横滚角速度引起的俯仰水动力矩导数，Mrp表示由转艏角速度和横滚角速度引起的俯仰水动力矩导数，Muw表示由纵向速度和垂向速度引起的俯仰水动力矩导数，
Figure BDA0002418518010000132
表示由纵向速度和水平舵引起的俯仰水动力矩导数；
NHD表示转艏水动力矩，Nv|v|表示由横向速度引起的转艏水动力矩导数，Nr|r|表示由转艏角速度引起的转艏水动力矩导数，Nv表示由横向加速度引起的附加质量，Nr表示由转艏角加速度引起的附加质量，Nur表示由纵向速度和转艏角速度引起的转艏水动力矩导数，Nwp表示由垂向速度和横滚角速度引起的转艏水动力矩导数，Npq表示由横滚角速度和俯仰角速度引起的转艏水动力矩导数，Nuv表示由纵向速度和横向速度引起的转艏水动力矩导数，
Figure BDA0002418518010000133
表示由纵向速度和垂直舵引起的转艏水动力矩导数；
u表示纵向速度，
Figure BDA0002418518010000142
表示纵向加速度，w表示垂向速度，q表示俯仰角速度，v表示横向速度，r表示转艏角速度，p表示横滚角速度，w表示垂向加速度，δh表示水平舵角，δv表示垂直舵角。
在本发明实施例中，通过公式组(1)和公式组(2)求得六个自由度上的静水力和动水力，再计算每个自由度上静水力和动水力组成的合外力，最终得到六个自由度上的合外力，分别为：∑Xext、∑Yext、∑Zext、∑Kext、∑Mext和∑Next，将其作为空间六自由度模型方程的等式右边。
在步骤S22中，根据水动力参数和合外力，以重心为坐标原点确定空间六自由度运动模型。其中，最终的空间六自由度运动模型表示为：
Figure BDA0002418518010000141
在公式组(3)中，等式右边∑Xext、∑Yext、∑Zext、∑Kext、∑Mext和∑Next分别为六个自由度上的合外力。m表示AUV质量，Ixx表示绕x轴的转动惯量，Iyy表示绕y轴的转动惯量，Izz表示绕z轴的转动惯量，u表示纵向速度，
Figure BDA0002418518010000143
表示纵向加速度，w表示垂向速度，q表示俯仰角速度，v表示横向速度，r表示转艏角速度，p表示横滚角速度，w表示垂向加速度，δh表示水平舵角，δv表示垂直舵角，xG表示AUV重心纵坐标，yG表示AUV重心横坐标，zG表示AUV重心垂向坐标，xb表示AUV浮心纵坐标，yb表示AUV浮心横坐标，zb表示AUV浮心垂向坐标。
由此，本发明实施例利用第一定位信息和水动力参数确定待回收机器人的空间六自由度运动模型，通过建立好的空间六自由度运动模型，保证进一步准确求解待回收机器人的末端位姿。
在步骤S3中，根据第二定位信息设置位置引力点和姿态引力点，根据位置引力点和姿态引力点确定虚拟引力。其中，第二定位信息包括移动坞站在惯性坐标系下的坐标、横滚角、俯仰角、艏向角，能有效表征待回收水下机器人和移动坞站的初始位置和初始姿态。可选地，虚拟引力包括主动力和主动力矩，其中，主动力包括纵向引力、横向引力和垂向引力，主动力矩包括艏向引力矩和俯仰引力矩。根据上述信息设置位置引力点和姿态引力点，充分考虑移动坞站与待回收水下机器人的相对运动的影响，从而能求得合理的虚拟引力，最终通过求得的虚拟引力保证路径规划后两者末端位姿一致，大大提高了移动对接的可靠性。
在本发明实施例中，步骤S3包括步骤S31至步骤S32。
在步骤S31中，根据第二定位信息设置位置引力点，再根据位置引力点计算主动力，其中，主动力包括纵向引力、横向引力和垂向引力。由此，位置引力点产生主动力，通过将位置引力分解成坐标轴上三个方向的力，有效求得纵向引力、横向引力和垂向引力。
可选地，步骤S31包括步骤S311至步骤S313：
在步骤S312中，根据所述第一定位信息和所述第二定位信息，通过坐标转换公式得到所述待回收水下机器人和所述坞站在随体坐标系下的相对矢量距离和相对标量距离。在本发明实施例中，第一定位信息包括待回收水下机器人在惯性坐标系下的坐标、横滚角、俯仰角、艏向角；第二定位信息包括待移动坞站在惯性坐标系下的坐标、横滚角、俯仰角、艏向角。将待回收水下机器人的重心作为随体坐标系原点，根据第一定位信息和第二定位信息，通过坐标转换公式得到移动坞站在随体坐标系内的坞站随体坐标，确定坞站随体坐标和随体坐标系原点的相对矢量距离和相对标量距离。
具体地，结合图2来看，图2是待回收水下机器人和移动坞站相对位置的示意图。图中，AUV表示待回收水下机器人，(x1,y1,z1)表示待回收水下机器人在随体坐标系下的坐标，(x2,y2,z2)表示移动坞站在随体坐标系下的坐标；X、Y、Z分别为随体坐标系下的三个坐标轴，G为随体坐标系的原点，即待回收水下机器人的重心；ξ、η、ζ分别为惯性坐标系下的三个坐标轴。在本发明实施例中，将惯性坐标系下的坐标转化为随体坐标系下的坐标，以此计算待回收水下机器人和移动坞站之间的相对矢量距离和相对标量距离，其中，坐标转换的公式为：
Figure BDA0002418518010000161
在公式(4)中，[x2,y2,z2]T表示移动坞站在随体坐标系下的坐标,[ξ2,η2,ζ2]T表示移动坞站在惯性坐标系的坐标，
Figure BDA0002418518010000162
为待回收水下机器人的横滚角，俯仰角，艏向角；J1为转换公式，其公式为：
Figure BDA0002418518010000163
在公式(5)中，θ表示俯仰角，φ表示横滚角，ψ表示艏向角。
在本发明实施例中，通过上述步骤，利用坐标转换公式得到移动坞站在随体坐标系内的坞站随体坐标，以便后续确定移动坞站和待回收机器人在随体坐标系的相对矢量距离和相对标量距离。
在步骤S312中，根据所述第二定位信息，设置所述距离控制点、最大限制吸引力和第一常数。由此，设置距离控制点以便考量待回收水下机器人和移动坞站之间的相对距离关系。若两者相对距离大于距离控制点，则将主动力调节至较大范围，若两者相对距离小于或等于距离控制点，则将主动力调节至较小范围，以此依据距离，保证主动力始终处于合理范围。同时，设置合理的最大限制吸引力以明确主动力的最大值，进一步使主动力的求解合理化；设置第一常数，以便设置相应的权重，便于有效调节主动力的大小。
在步骤S313中，根据距离控制点、最大限制吸引力、第一常数、相对矢量距离和相对标量距离，确定纵向引力、横向引力、垂向引力。由此，通过比较相对标量距离与距离控制点，以此确定主动力的大小和方向，充分考虑到待回收机器人和移动坞站之间的相对距离对主动力的影响，有效利用两者的相对运动特性，得到准确的主动力，进而保证了本发明路径规划的有效性。
可选地，步骤S313包括步骤S3131至步骤S3134。
在步骤S3131中，根据距离控制点、相对标量距离和第一常数，确定第一权重。设置第一权重，利用第一权重和最大限制吸引力的乘积有效控制主动力的大小。
在步骤S3132中，根据距离控制点和相对标量距离判断是否满足第一预设条件。由此，根据待回收水下机器人和移动坞站相对运动的特性，通过设置距离控制点，判断两者之间的相对标量距离是否超过距离控制点，即判断两者距离是否超过一定的范围。
在步骤S3133中，若满足第一预设条件，则根据最大限制吸引力、第一权重、相对矢量距离确定总矢量引力。由此，在不同的距离范围内，确定相应合理的总矢量引力，再利用最大限制吸引力、第一权重、相对矢量距离求解总矢量引力，有效地设置了总矢量引力的大小。
可选地，步骤S3133包括步骤S31331至步骤S31332。
在步骤31331中，当相对标量距离大于距离控制点，则根据最大限制吸引力和相对矢量距离的乘积确定总矢量引力。由此，当待回收水下机器人和移动坞站的距离超过距离控制点的范围，所需总矢量引力较大，因而利用最大限制吸引力和相对矢量距离的乘积确定总矢量引力。
在步骤31332中，当相对标量距离小于或等于距离控制点，则根据最大限制吸引力、相对矢量距离和第一权重的乘积确定总矢量引力。由此，当待回收水下机器人和移动坞站的距离在距离控制点的范围内时，所需总矢量引力较小，此时利用第一权重有效求得合适的总矢量引力。
具体地，结合公式(6)和公式(7)来看：
Figure BDA0002418518010000171
Figure BDA0002418518010000181
在公式(6)和公式(7)中，Fattractive表示总矢量引力，fattractivemax表示最大限制吸引力，Dswitch表示距离控制点，D表示待回收水下机器人和移动坞站之间的相对标量距离，Dir1,2表示待回收水下机器人和移动坞站之间的相对矢量距离，K1表示第一常量，(x1,y1,z1)表示待回收水下机器人在随体坐标系下的坐标，(x2,y2,z2)表示移动坞站在随体坐标系下的坐标。
当相对标量距离D小于距离控制点Dswitch时，将最大限制吸引力fattractivemax、第一权重及相对矢量距离Dir1,2相乘得到总矢量引力Fattractive，此时随着相对标量距离D的缩小，Fattractive的大小开始缩小，并最终使得相对标量距离D稳定不变。当相对标量距离D大于距离控制点Dswitch时，将最大限制吸引力fattractivemax、相对矢量距离Dir1,2相乘得到总矢量引力Fattractive，此时两者距离较大，相应的总矢量引力也可随着相对标量距离的增大而增大。综上，通过距离控制点的设置，依据两者相对距离的大小，设置合适范围的主控制力，使路径规划的控制方法更具合理性。
在步骤S3134中，根据总矢量引力确定纵向引力、横向引力和垂向引力。由于总矢量引力是一个矢量，包含三个方向的力，通过总矢量力即可求解各个方向的控制力，进而保证了路径规划中求解各个方向的控制力的准确性。可选地，总矢量引力包括沿Y轴方向的第一引力、沿X轴方向的第二引力和沿Z轴方向的第三引力，所述根据总矢量引力确定纵向引力、横向引力和垂向引力，包括：根据第一引力确定纵向引力；根据第二引力确定横向引力；根据第三引力确定垂向引力。由此，由于总矢量引力包含三个方向的力，根据这三个方向的力可以确定沿X、Y、Z轴的推力，即纵向引力、横向引力和垂向引力，保证准确求解了各个方向的控制力。
在本发明实施例中，根据待回收水下机器人和移动坞站相对运动的特性，通过设置距离控制点，判断两者之间的相对标量距离是否超过距离控制点，即判断两者距离是否超过一定的范围。依据此，再利用最大限制吸引力、第一权重、相对矢量距离求解总矢量引力，进而保证了路径规划中求解各个方向的控制力的准确性。
在步骤S32中，设置姿态引力点，根据姿态引力点计算主动力矩，其中，主动力矩包括艏向引力矩、俯仰引力矩。姿态引力点产生主动力矩，通过姿态引力点有效求得艏向引力矩、俯仰引力矩。
在本发明实施例中，步骤S32包括步骤S321至步骤S322。
在步骤S321中，设置艏向角引力点和最大垂直舵力，根据第一定位信息、第二定位信息、艏向角引力点和最大垂直舵力确定所述艏向引力矩。由此，根据欠驱动水下机器人的运动特性，通过设置艏向角引力点以确定艏向引力矩，保证了有效的路径规划控制方法。
具体地，结合图3来看，图3是待回收水下机器人和移动坞站的艏向角度差的示意图。可选地，步骤S321包括步骤S3211至步骤S3214。
在步骤S3211中，根据第一艏向角和第二艏向角之差确定相对艏向角。由此，根据待回收水下机器人和移动坞站相对运动的特性，计算两者之间的相对艏向角。
在步骤S3212中，设置第二常数，根据所述第二常数和相对艏向角确定第二权重。由此，设置第二权重达到有效调节艏向引力矩的目的。
在步骤S3213中，若相对艏向角大于艏向角引力点，则艏向引力矩为最大垂直舵力。由此，通过设置艏向角引力点，判断两者之间的相对艏向角是否超过艏向角引力点，即判断两者相对艏向角是否超过一定的范围，当相对艏向角大于艏向角引力点时，所需艏向引力矩较大，为最大垂直舵力。
在步骤S3214中，若相对艏向角小于或等于艏向角引力点，则艏向引力矩为最大垂直舵力和第二权重的乘积。由此，通过设置艏向角引力点，判断两者之间的相对艏向角是否超过艏向角引力点，即判断两者相对艏向角是否超过一定的范围，当相对艏向角小于或等于艏向角引力点时，所需艏向引力矩较小，因而为最大垂直舵力和第二权重的乘积。
在本发明实施例中，当第二常数为K2,最大垂直舵力为F最大垂直舵力，▽ψ为相对艏向角时，求得第二权重为
Figure BDA0002418518010000201
具体地，结合公式组(8)来看：
Figure BDA0002418518010000202
公式组(8)中，Fattractive_ψ表示为艏向引力矩，F最大垂直舵力表示最大垂直舵力，K2表示为第二常数，▽ψ表示为相对艏向角，ψswitch表示为艏向角引力点。
由此，当相对艏向角▽ψ大于艏向角引力点ψswitch，则艏向引力矩Fattractive_ψ为最大垂直舵力F最大垂直舵力；当相对艏向角▽ψ小于或等于艏向角引力点ψswitch时，艏向引力矩Fattractive_ψ为最大垂直舵力F最大垂直舵力和第二权重
Figure BDA0002418518010000203
的乘积。
在步骤S322中，设置俯仰角引力点和最大水平舵力，根据第一定位信息、第二定位信息、俯仰角引力点和最大水平舵力确定俯仰引力矩。由此，根据欠驱动水下机器人的运动特性，设置俯仰角引力点以确定俯仰引力矩，保证了有效的路径规划控制方法。
具体地，结合图4来看，图4是待回收水下机器人和移动坞站的俯仰角度差的示意图。可选地，步骤S322包括步骤S3221至步骤S3224。
在步骤S3221中，根据第一俯仰角和第二俯仰角之差确定相对俯仰角。由此，根据待回收水下机器人和移动坞站相对运动的特性，计算两者之间的相对俯仰角。
在步骤S3222中，设置第三常数，根据第三常数和相对俯仰角确定第三权重。由此，设置第三权重达到有效调节俯仰引力矩的目的。
在步骤S3223中，若相对俯仰角大于俯仰角引力点，则俯仰引力矩为最大水平舵力。由此，通过设置俯仰角引力点，判断两者之间的相对俯仰角是否超过俯仰角引力点，即判断两者相对俯仰角是否超过一定的范围，当相对俯仰角大于俯仰角引力点时，所需俯仰引力矩较大，为最大水平舵力。
在步骤S3224中，若相对俯仰角小于或等于俯仰角引力点，则俯仰引力矩为最大水平舵力和第三权重的乘积。由此，通过设置俯仰角引力点，判断两者之间的相对俯仰角是否超过艏向角引力点，即判断两者相对俯仰角是否超过一定的范围，当相对俯仰角小于或等于俯仰角引力点时，所需俯仰引力矩较小，因而为最大水平舵力和第三权重的乘积。
在本发明实施例中，当第三常数为K3,最大水平舵力为F最大水平舵力，▽θ为相对俯仰角时，求得第三权重为
Figure BDA0002418518010000211
具体地，结合公式组(9)来看：
Figure BDA0002418518010000212
在公式组(9)中，Fattractive_θ表示为俯仰引力矩，F最大水平舵力表示最大水平舵力，K3表示为第三常数，▽θ表示为相对俯仰角，θswitch表示为俯仰角引力点。由此，当相对俯仰角▽θ大于俯仰角引力点θswitch，则俯仰引力矩Fattractive_θ为最大水平舵力F最大水平舵力；当相对俯仰角▽θ小于或等于俯仰角引力点θswitch时，俯仰引力矩Fattractive_θ为最大水平舵力F最大水平舵力和第三权重
Figure BDA0002418518010000213
的乘积。
在步骤S4中，根据虚拟引力，确定待回收水下机器人的推力、垂直舵力、水平舵力。由此，根据待回收水下机器人和移动坞站相对运动的特性，通过对纵向引力、横向引力、垂向引力、艏向引力矩和俯仰引力矩的整合，得到推力、垂直舵力和水平舵力，进而保证了路径规划中求解俯仰引力矩的准确性。
可选地，步骤S4包括步骤S41至步骤S43。
在步骤S41中，根据纵向引力确定推力。由此，纵向引力等效于待回收水下机器人的推力，即推力F推＝纵向引力Fattractive_y。
在步骤S42中，根据横向引力和艏向引力矩之和确定垂直舵力。由此，垂直舵力等效于横向引力和艏向引力矩之和，即横向引力Fattractive_y+艏向引力矩Fattractive_ψ＝垂直舵力F垂直舵力。
在步骤S43中，根据垂向引力和俯仰引力矩之和确定水平舵力。由此，水平舵力等效于垂向引力和俯仰引力矩之和，即垂向引力Fattractive_z+俯仰引力矩Fattractive_θ＝水平舵力F水平舵力。
在步骤S5中，将推力、垂直舵力、水平舵力带入空间六自由度运动模型，求解得到待回收水下机器人的下一时刻位姿，并获取移动坞站的下一时刻位姿。由此，利用上述步骤求得的推力、垂直舵力、水平舵力，将其带入空间六自由度运动模型中求解，即可有效解得待回收水下机器人的下一时刻位姿，保证了移动对接的可靠性。
可选地，步骤S5包括步骤S51至步骤S53。
在步骤S51中，将求得的垂直舵力替换空间六自由度运动模型中的δv，求得的水平舵力替换空间六自由度运动模型中的δh。即将垂直舵力F垂直舵力替换公式组(2)中水动力项的δv，将水平舵力F水平舵力替换成公式组(2)中的δh。
在步骤S52中，使用4阶龙格库塔求解微分方程的方法计算待回收水下机器人的下一时刻位姿。由此，通过求解微分方程，准确求得下一时刻位姿，保证路径规划的可靠性。
在步骤S53中，获取下一时刻的移动坞站的第二定位信息，更新移动坞站的位姿。由此，通过重新获取移动坞站的第二定位信息，及时更新移动坞站下一时刻位姿，保证路径规划中，最终两者末端位姿一致。
在步骤S6中，判断待回收水下机器人的下一时刻位姿是否满足与移动坞站的下一时刻位姿达到一致，若满足，则结束路径规划任务。由此，判断待回收水下机器人和移动坞站的末端位姿是否一致，保证待回收水下机器人能高效无误地完成归航时的对接。
具体地，所述判断待回收水下机器人的下一时刻位姿是否满足与移动坞站的下一时刻位姿达到一致，包括步骤S61至步骤S62。
在步骤S61中，判断待回收水下机器人和移动坞站是否满足位置约束条件。其中，位置约束条件可以表示为：
Figure BDA0002418518010000231
在公式(10)中，xrelativeGoal表示待回收水下机器人和移动坞站的相对纵向距离，yrelativeGoal表示待回收水下机器人和移动坞站的相对横向距离，zrelativeGoal表示待回收水下机器人和移动坞站的相对垂向距离。
结合图5来看，图5为待回收水下机器人和移动坞站对接示意图。由于待回收水下机器人与移动坞站开口的纵向距离小于等于摄像机最大捕捉距离Dc，该最大捕捉距离因水体环境和摄像机性能相关，一般情况下为10～20米，本发明出于安全考虑设Dc＝10米；而待回收水下机器人与移动坞站开口的中心线的横向和垂向位置差应不大于0.2米。
在步骤S62中，判断待回收水下机器人和移动坞站是否满足姿态约束条件。其中，姿态约束条件可以表示为：
Figure BDA0002418518010000232
在公式(11)中，θAUV表示待回收水下机器人下一时刻的俯仰角，θGoal表示移动坞站下一时刻的俯仰角；ψAUV表示待回收水下机器人下一时刻的艏向角，ψGoal表示移动坞站下一时刻的艏向角。由此，待回收水下机器人在末端的俯仰角和艏向角应与坞站中心线的俯仰角和艏向角保持基本一致，从而使得点光源处于摄像机的视角极限内，摄像机能精确捕捉到所有点光源的位置，保证接下来光学导引对接阶段能顺利进行。
综上，本发明通过通过在人工势场法中设置位置引力点和姿态引力点求得虚拟引力，并针对欠驱动的待回收水下机器人的运动特性得到推力、垂直舵力和水平舵力，进一步规划待回收水下机器人移动式对接路径，从而使得路径末端的待回收水下机器人和移动坞站的位姿趋于一致。综上，本发明充分利用了待回收水下机器人的欠驱动运动特性，利用位置引力点和姿态引力点求得虚拟引力，再将虚拟引力整合成推力、垂直舵力、水平舵力，最后利用推力、垂直舵力、水平舵力完成了准确的待回收水下机器人的末端位姿求解，实现了高效的水下移动式对接路径规划，保证了待回收水下机器人和移动坞站的高精度对接。
图6所示为本发明实施例的水下机器人移动式对接回收的路径规划的装置800的结构示意图，包括获取单元801、处理单元802和控制单元803。
获取单元801，用于获取定位信息、待回收水下机器人的水动力参数，其中，定位信息包括待回收水下机器人的第一定位信息和移动坞站的第二定位信息；
处理单元802，用于根据第一定位信息和水动力参数，建立待回收水下机器人的空间六自由度运动模型；用于根据第二定位信息设置位置引力点和姿态引力点，根据位置引力点和姿态引力点确定虚拟引力,其中，位置引力点用于划分距离范围，姿态引力点用于划分角度范围；用于根据虚拟引力，整合待回收水下机器人的推力、垂直舵力、水平舵力；用于将推力、垂直舵力、水平舵力带入空间六自由度运动模型，求解得到待回收水下机器人的下一时刻位姿，并获取移动坞站的下一时刻位姿；
控制单元803，用于判断所述待回收水下机器人的下一时刻位姿是否与所述移动坞站的下一时刻位姿达到一致，若达到，则结束路径规划任务。
本发明提供的一种水下机器人移动式对接回收的路径规划的装置，充分利用了待回收水下机器人的欠驱动运动特性，利用位置引力点和姿态引力点，完成了准确的待回收水下机器人的末端位姿求解，实现了高效的水下移动式对接路径规划，保证了待回收水下机器人和移动坞站的高精度对接。
本发明的又一实施例还提供一种计算机可读存储介质，计算机可读存储介质存储有计算机程序，计算机程序被处理器读取并运行时，实现如上所述的水下机器人移动式对接回收的路径规划方法，充分利用了待回收水下机器人的欠驱动运动特性，利用位置引力点和姿态引力点，完成了准确的待回收水下机器人的末端位姿求解，实现了高效的水下移动式对接路径规划，保证了待回收水下机器人和移动坞站的高精度对接。
虽然本公开披露如上，但本公开的保护范围并非仅限于此。本领域技术人员，在不脱离本公开的精神和范围的前提下，可进行各种变更与修改，这些变更与修改均将落入本发明的保护范围。

Claims (12)

1.一种水下机器人移动式对接回收的路径规划方法，其特征在于，包括：
获取待回收水下机器人的第一定位信息、移动坞站的第二定位信息以及所述待回收水下机器人的水动力参数；
所述第一定位信息包括所述待回收水下机器人的重心，根据所述第一定位信息和所述水动力参数，建立所述待回收水下机器人的空间六自由度运动模型，包括：根据所述水动力参数,确定六个自由度上的静水力和动水力的合外力；将所述水动力参数和所述合外力代入所述六自由度运动模型等式中，以所述重心为原点，建立所述空间六自由度运动模型；
根据所述第二定位信息设置位置引力点和姿态引力点，根据所述位置引力点和所述姿态引力点确定虚拟引力,其中，所述位置引力点用于划分距离范围，所述姿态引力点用于划分角度范围；
根据所述虚拟引力，确定所述待回收水下机器人的推力、垂直舵力以及水平舵力；
将所述推力、所述垂直舵力以及所述水平舵力带入所述空间六自由度运动模型，确定所述待回收水下机器人的下一时刻位姿，并获取所述移动坞站的下一时刻位姿；
判断所述待回收水下机器人的下一时刻位姿是否与所述移动坞站的下一时刻位姿达到一致，若达到，则结束路径规划任务。
2.如权利要求1所述的水下机器人移动式对接回收的路径规划方法，其特征在于，所述虚拟引力包括主动力和主动力矩，所述根据所述第二定位信息设置位置引力点和姿态引力点，根据所述位置引力点和所述姿态引力点确定虚拟引力，包括：
根据所述第二定位信息设置所述位置引力点，再根据所述位置引力点确定所述主动力，其中，所述主动力包括纵向引力、横向引力和垂向引力；
根据所述第二定位信息设置所述姿态引力点，再根据所述姿态引力点确定所述主动力矩，其中，所述主动力矩包括艏向引力矩、俯仰引力矩。
3.如权利要求2所述的水下机器人移动式对接回收的路径规划方法，其特征在于，所述位置引力点包括距离控制点，所述根据所述第二定位信息设置所述位置引力点，再根据所述位置引力点确定所述主动力，包括：
根据所述第二定位信息，通过坐标转换公式得到所述待回收水下机器人和所述坞站在随体坐标系下的相对矢量距离和相对标量距离；
根据所述第二定位信息，设置所述距离控制点、最大限制吸引力和第一常数；
根据所述距离控制点、所述最大限制吸引力、所述第一常数、所述相对矢量距离和所述相对标量距离，确定所述纵向引力、所述横向引力以及所述垂向引力。
4.如权利要求3所述的水下机器人移动式对接回收的路径规划方法，其特征在于，所述根据所述距离控制点、所述最大限制吸引力、所述第一常数、所述相对矢量距离和所述相对标量距离，确定纵向引力、横向引力以及垂向引力，包括：
根据所述距离控制点、所述相对标量距离和第一常数，确定第一权重；
判断所述距离控制点和所述相对标量距离是否满足第一预设条件；
若满足所述第一预设条件，则根据所述最大限制吸引力、所述第一权重、所述相对矢量距离确定总矢量引力；
根据所述总矢量引力确定所述纵向引力、所述横向引力和所述垂向引力，其中，所述总矢量引力包括沿Y轴方向的第一引力、沿X轴方向的第二引力和沿Z轴方向的第三引力，所述纵向引力为所述第一引力，所述横向引力为所述第二引力，所述垂向引力为所述第三引力。
5.如权利要求4所述的水下机器人移动式对接回收的路径规划方法，其特征在于，所述若满足所述第一预设条件，则根据所述最大限制吸引力、所述第一权重、所述相对矢量距离确定总矢量引力，包括：
当所述相对标量距离大于所述距离控制点，则根据所述最大限制吸引力和所述相对矢量距离的乘积确定所述总矢量引力；
当所述相对标量距离小于或等于所述距离控制点，则根据所述最大限制吸引力、所述相对矢量距离和所述第一权重的乘积确定所述总矢量引力。
6.如权利要求2所述的水下机器人移动式对接回收的路径规划方法，其特征在于，所述姿态引力点包括艏向角引力点和俯仰角引力点；所述根据所述第二定位信息设置所述姿态引力点，再根据所述姿态引力点确定所述主动力矩，包括：
根据所述第二定位信息设置艏向角引力点和最大垂直舵力，再根据所述第一定位信息、所述第二定位信息、所述艏向角引力点和所述最大垂直舵力确定所述艏向引力矩；
根据所述第二定位信息设置俯仰角引力点和最大水平舵力，再根据所述第一定位信息、所述第二定位信息、所述俯仰角引力点和所述最大水平舵力确定所述俯仰引力矩。
7.如权利要求6所述的水下机器人移动式对接回收的路径规划方法，其特征在于，所述第一定位信息包括第一艏向角，所述第一艏向角为所述待回收水下机器人的艏向角，所述第二定位信息包括第二艏向角，所述第二艏向角为所述移动坞站的艏向角；所述再根据所述第一定位信息、所述第二定位信息、所述艏向角引力点和所述最大垂直舵力确定所述艏向引力矩，包括：
根据所述第一艏向角和所述第二艏向角之差确定相对艏向角；
设置第二常数，根据所述第二常数和所述相对艏向角确定第二权重；
若所述相对艏向角大于所述艏向角引力点，则所述艏向引力矩为所述最大垂直舵力；
若所述相对艏向角小于或所述艏向角引力点，则所述艏向引力矩为所述最大垂直舵力和所述第二权重的乘积。
8.如权利要求6所述的水下机器人移动式对接回收的路径规划方法，其特征在于，所述第一定位信息包括第一俯仰角，所述第一俯仰角为所述待回收水下机器人的俯仰角，所述第二定位信息包括第二俯仰角，所述第二俯仰角为所述移动坞站的俯仰角，所述再根据所述第一定位信息、所述第二定位信息、所述俯仰角引力点和所述最大水平舵力确定所述俯仰引力矩，包括：
根据所述第一俯仰角和所述第二俯仰角之差确定相对俯仰角；
设置第三常数，根据所述第三常数和所述相对俯仰角确定第三权重；
若所述相对俯仰角大于所述俯仰角引力点，则所述俯仰引力矩为所述最大水平舵力；
若所述相对俯仰角小于或所述俯仰角引力点，则所述俯仰引力矩为所述最大水平舵力和所述第三权重的乘积。
9.如权利要求1所述的水下机器人移动式对接回收的路径规划方法，其特征在于，所述虚拟引力包括主动力和主动力矩，所述主动力包括纵向引力、横向引力和垂向引力，所述主动力矩包括艏向引力矩和俯仰引力矩；所述根据所述虚拟引力，确定所述待回收水下机器人的推力、垂直舵力、水平舵力，包括：
根据所述纵向引力确定所述推力；
根据所述横向引力和所述艏向引力矩之和确定所述垂直舵力；
根据所述垂向引力和所述俯仰引力矩之和确定所述水平舵力。
10.如权利要求1-9所述的水下机器人移动式对接回收的路径规划方法，其特征在于，所述判断所述待回收水下机器人的下一时刻位姿是否与所述移动坞站的下一时刻位姿达到一致，包括：
判断所述待回收水下机器人和所述移动坞站下一时刻的相对距离是否达到第一预设范围，其中，所述相对距离包括相对纵向距离、相对横向距离和相对垂向距离；
和/或判断所述待回收水下机器人和所述移动坞站下一时刻的相对角度是否达到第二预设范围，其中，所述相对角度包括相对俯仰角、相对艏向角。
11.一种水下机器人移动式对接回收的路径规划的装置，其特征在于，包括：
获取单元，用于获取定位信息、待回收水下机器人的水动力参数，其中，所述定位信息包括所述待回收水下机器人的第一定位信息和移动坞站的第二定位信息；所述第一定位信息包括所述待回收水下机器人的重心；
处理单元，用于根据所述第一定位信息和所述水动力参数，建立所述待回收水下机器人的空间六自由度运动模型，包括：根据所述水动力参数,确定六个自由度上的静水力和动水力的合外力；将所述水动力参数和所述合外力代入所述六自由度运动模型等式中，以所述重心为原点，建立所述空间六自由度运动模型；用于根据所述第二定位信息设置位置引力点和姿态引力点，根据所述位置引力点和所述姿态引力点确定虚拟引力,其中，所述位置引力点用于划分距离范围，所述姿态引力点用于划分角度范围；用于根据所述虚拟引力，整合所述待回收水下机器人的推力、垂直舵力、水平舵力；用于将所述推力、所述垂直舵力、所述水平舵力带入所述空间六自由度运动模型，求解得到所述待回收水下机器人的下一时刻位姿，并获取所述移动坞站的下一时刻位姿；
控制单元，用于判断所述待回收水下机器人的下一时刻位姿是否与所述移动坞站的下一时刻位姿达到一致，若达到，则结束路径规划任务。
12.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器读取并运行时，实现如权利要求1-10中任一项所述的水下机器人移动式对接回收的路径规划方法。
CN202010198550.4A 2020-03-20 2020-03-20 一种水下机器人移动式对接回收的路径规划方法及装置 Active CN111208845B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010198550.4A CN111208845B (zh) 2020-03-20 2020-03-20 一种水下机器人移动式对接回收的路径规划方法及装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010198550.4A CN111208845B (zh) 2020-03-20 2020-03-20 一种水下机器人移动式对接回收的路径规划方法及装置

Publications (2)

Publication Number Publication Date
CN111208845A CN111208845A (zh) 2020-05-29
CN111208845B true CN111208845B (zh) 2023-07-07

Family

ID=70789954

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010198550.4A Active CN111208845B (zh) 2020-03-20 2020-03-20 一种水下机器人移动式对接回收的路径规划方法及装置

Country Status (1)

Country Link
CN (1) CN111208845B (zh)

Families Citing this family (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111625009B (zh) * 2020-06-05 2021-06-25 上海中车艾森迪海洋装备有限公司 水下机器人在布放回收过程中的自动运动控制方法及装置
CN112486168B (zh) * 2020-11-19 2022-05-20 哈尔滨工程大学 一种基于回转圆的移动式对接轨迹规划方法
CN113103225B (zh) * 2021-03-03 2022-06-10 重庆大学 一种基于图像测量的移动机器人多阶段平稳自主对接方法
CN113485366B (zh) * 2021-08-05 2022-03-04 泰瑞数创科技(北京)有限公司 用于机器人的导航路径生成方法及装置
CN113984045B (zh) * 2021-10-13 2024-01-16 湖南航天机电设备与特种材料研究所 水下机器人移动式对接目标运动状态估计方法及系统
CN114428507B (zh) * 2022-01-14 2023-07-07 哈工大机器人集团(杭州湾)国际创新研究院 一种用于浅航器的垂直入坞停泊算法
CN114771778A (zh) * 2022-04-12 2022-07-22 株洲中车时代电气股份有限公司 一种用于水下机器人的回收方法、回收装置及水下机器人

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105487548A (zh) * 2015-12-07 2016-04-13 哈尔滨工程大学 一种欠驱动水下机器人远距离精确自主搭载对接控制方法
CN108363400A (zh) * 2018-01-29 2018-08-03 哈尔滨工程大学 一种基于虚拟锚泊的欠驱动auv三维控制区域镇定控制方法
CN108362296A (zh) * 2017-12-29 2018-08-03 浙江大学 Auv与接驳站水下对接的末端视觉导引方法
CN109238291A (zh) * 2018-10-26 2019-01-18 河海大学 一种水面无人船导引缆回收自主水下航行器的规划方法
CN109933077A (zh) * 2017-12-15 2019-06-25 中国科学院沈阳自动化研究所 一种水下机器人自动靠泊的路径规划和控制方法

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US8881665B2 (en) * 2011-09-30 2014-11-11 Cggveritas Services Sa Deployment and recovery vessel for autonomous underwater vehicle for seismic survey
US10669000B2 (en) * 2017-07-11 2020-06-02 The Governement Of The United States Of America, As Represented By The Secretary Of The Navy Mobile underwater docking system for an underwater vehicle

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105487548A (zh) * 2015-12-07 2016-04-13 哈尔滨工程大学 一种欠驱动水下机器人远距离精确自主搭载对接控制方法
CN109933077A (zh) * 2017-12-15 2019-06-25 中国科学院沈阳自动化研究所 一种水下机器人自动靠泊的路径规划和控制方法
CN108362296A (zh) * 2017-12-29 2018-08-03 浙江大学 Auv与接驳站水下对接的末端视觉导引方法
CN108363400A (zh) * 2018-01-29 2018-08-03 哈尔滨工程大学 一种基于虚拟锚泊的欠驱动auv三维控制区域镇定控制方法
CN109238291A (zh) * 2018-10-26 2019-01-18 河海大学 一种水面无人船导引缆回收自主水下航行器的规划方法

Also Published As

Publication number Publication date
CN111208845A (zh) 2020-05-29

Similar Documents

Publication Publication Date Title
CN111208845B (zh) 一种水下机器人移动式对接回收的路径规划方法及装置
Manzanilla et al. Autonomous navigation for unmanned underwater vehicles: Real-time experiments using computer vision
CN112148022B (zh) 全驱动自主水下机器人回收三维路径跟踪控制系统及方法
CN108267953B (zh) 一种基于领航者-跟踪者水下机器人位置跟踪方法
CN107991872B (zh) 欠驱动auv的虚拟锚泊水平面区域镇定控制方法及实现方法
CN113419428B (zh) 基于3d映射制导的机/船协同路径跟踪控制器设计方法
CN111857165B (zh) 一种水下航行器的轨迹跟踪控制方法
Hai et al. Investigation on the mechanical design and manipulation hydrodynamics for a small sized, single body and streamlined I-AUV
Zhou et al. Dynamic modeling and motion control of a novel conceptual multimodal underwater vehicle for autonomous sampling
CN106840143B (zh) 一种判别水下机器人姿态稳定的方法
CN113741433B (zh) 一种水面无人船的分布式编队方法
CN113721639A (zh) 一种用于小型水下机器人入坞的路径规划方法及控制方法
CN113147438A (zh) 一种水下航行器无线充电系统及控制方法
CN108227723A (zh) 一种水下机器人及其稳定性分析及结构优化的应用方法
CN116540717A (zh) 一种基于改进dwa的auv局部路径规划方法
CN116679693A (zh) 融合螺旋桨水动力的无人艇自抗扰控制方法
CN110083941A (zh) 一种五自由度rov非线性运动学建模方法
CN114721407A (zh) 一种水下航行器无线充电系统的控制方法
Bian et al. Visual docking control of an AUV with a moving station using a cross-tracking approach with unscented Kalman filtering
CN112925222A (zh) 一种基于ros的无人艇运动控制仿真方法及装置
CN114019805B (zh) 一种欠驱动auv的模型预测对接控制方法
Lyu et al. A Snake Eel Inspired Multi-joint Underwater Inspection Robot for Undersea Infrastructure Intelligent Maintenance
Hidaka et al. System design and hardware development of autonomous underwater robot “DaryaBird”
Oh et al. A Study on the Control of AUV’s Homing and Docking
Martins et al. MARA-A modular underwater robot for confined spaces exploration

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