CN116382320A - 水下机器人姿态控制方法及装置 - Google Patents

Abstract

本发明公开一种水下机器人姿态控制方法及装置。通过获取待控制水下机器人在机器人坐标系下的第一位姿信息和在世界坐标系下的第二位姿信息，然后根据第一位姿信息和第二位姿信息判断待控制水下机器人的姿态是否需要调整，若是，则获取待控制水下机器人的各个机械结构中结点的结点位置，并根据结点位置对待控制水下机器人的姿态进行调整，在姿态调整完成后，获取待控制水下机器人的工作参数，并根据工作参数判断待控制水下机器人的姿态是否符合预设姿态标准。本发明能够及时有效地对待控制水下机器人的姿态进行调整，在姿态调整完成后，有效地判断调整后的姿态是否符合预设姿态标准，以判断是否需要再次进行调整，能够有效地控制水下机器人的姿态。

Description

水下机器人姿态控制方法及装置

技术领域

本发明涉及水下机器人技术领域，尤其涉及一种水下机器人姿态控制方法及装置。

背景技术

近年来，无论是海洋资源开发，还是陆地水资源的利用，水下机器人的研究逐渐成为机器人研究应用中的热点问题。水下机器人在水下搜救打捞、考古科研、海洋生物观测、调查取证、海洋油气管道调查、能源勘探等方面发挥着重要的作用。灵活、稳定、可靠的水下机器人姿态控制方法是水下机器人进行多种水下作业的重要保证。目前，水下机器人的姿态控制多采用PID控制方法，PID控制器简单易操作，鲁棒性强，对非线性系统有良好的控制效果，但水下机器人属于MIMO二阶非线性不确定系统，传统方法无法取得良好的控制效果。因此，如何有效地控制水下机器人的姿态，保证水下机器人的正常工作，成为一个亟待解决的问题。

上述内容仅用于辅助理解本发明的技术方案，并不代表承认上述内容是现有技术。

发明内容

本发明的主要目的在于提供了一种水下机器人姿态控制方法及装置，旨在解决如何有效地控制水下机器人的姿态，保证水下机器人的正常工作的技术问题。

为实现上述目的，本发明提供了一种水下机器人姿态控制方法，所述水下机器人姿态控制方法包括以下步骤：

获取待控制水下机器人在机器人坐标系下的第一位姿信息和在世界坐标系下的第二位姿信息；

根据所述第一位姿信息和所述第二位姿信息判断所述待控制水下机器人的姿态是否需要调整；

若是，则获取所述待控制水下机器人的各个机械结构中结点的结点位置，并根据所述结点位置对所述待控制水下机器人的姿态进行调整；

在姿态调整完成后，获取所述待控制水下机器人的工作参数，并根据所述工作参数判断所述待控制水下机器人的姿态是否符合预设姿态标准。

可选地，所述根据所述第一位姿信息和所述第二位姿信息判断所述待控制水下机器人的姿态是否需要调整的步骤，具体包括：

获取所述第一位姿信息和所述第二位姿信息中处于同一机械结构的第一目标位姿信息和第二目标位姿信息；

确定所述待控制水下机器人在所述机器人坐标系和所述世界坐标系之间的转换矩阵；

根据所述第一目标位姿信息和所述转换矩阵确定所述第一目标位姿信息在所述世界坐标系下的第三目标位姿信息；

根据所述第二目标位姿信息和所述第三目标位姿信息判断所述待控制水下机器人的姿态是否需要调整。

可选地，所述根据所述第二目标位姿信息和所述第三目标位姿信息判断所述待控制水下机器人的姿态是否需要调整的步骤，具体包括：

判断所述第二目标位姿信息和所述第三目标位姿信息之间是否存在重合部分；

若是，则确定所述重合部分对应的所述待控制水下机器人的目标机械结构；

获取所述目标机械结构在工作状态下的预设位姿信息；

根据所述第二目标位姿信息、所述第三目标位姿信息以及所述预设位姿信息判断所述待控制水下机器人的姿态是否需要调整。

可选地，所述若是，则获取所述待控制水下机器人的各个机械结构中结点的结点位置，并根据所述结点位置对所述待控制水下机器人的姿态进行调整的步骤，具体包括：

若是，则获取所述待控制水下机器人的各个机械结构中结点在所述机器人坐标系下的第一结点位置和所述世界坐标系下的第二结点位置；

确定采集所述第二位姿信息的相机对应的相机参数；

根据所述第一结点位置和所述第二结点位置对所述相机参数进行调整，获得调整后的相机参数；

基于所述调整后的相机参数确定所述结点在所述世界坐标系下的第三结点位置；

根据所述第一结点位置和所述第三结点位置对各结点进行调整，以对所述待控制水下机器人的姿态进行调整。

可选地，所述调整后的相机参数包括：调整后的相机外参数和调整后的相机内参数；

所述根据所述第一结点位置和所述第二结点位置对所述相机参数进行调整，获得调整后的相机参数的步骤，具体包括：

获取所述第二结点位置的变化曲线，并对所述变化曲线进行拟合处理，获得拟合曲线；

将所述拟合曲线与所述待控制水下机器人的移动轨迹进行对比，并根据对比结果对所述相机参数中的相机内参数进行调整，获得调整后的相机内参数；

根据所述第一结点位置和所述转换矩阵确定所述第一结点位置在所述世界坐标系下的第四结点位置；

根据所述第四结点位置和所述第一结点位置对所述相机参数中的相机外参数进行调整，获得调整后的相机外参数。

可选地，所述根据所述第一结点位置和所述第三结点位置对各结点进行调整，以对所述待控制水下机器人的姿态进行调整的步骤，具体包括：

根据所述第一结点位置和所述第三结点位置确定所述待控制水下机器人的当前工作状态和在所述机器人坐标系下的目标结点位置；

获取所述当前工作状态下的所述待控制水下机器人在所述机器人坐标系下的预设结点位置；

将所述目标结点位置和所述预设结点位置进行对比，获得位置对比结果；

根据所述位置对比结果对各结点进行调整。

可选地，所述在姿态调整完成后，获取所述待控制水下机器人的工作参数，并根据所述工作参数判断所述待控制水下机器人的姿态是否符合预设姿态标准的步骤，具体包括：

在姿态调整完成后，获取所述待控制水下机器人的工作参数；

通过相机确定所述待控制水下机器人的工作环境；

根据所述工作环境和所述工作参数判断所述待控制水下机器人是否处于正常工作状态；

若是，则判定所述待控制水下机器人的姿态是否符合预设姿态标准。

此外，为实现上述目的，本发明还提供一种水下机器人姿态控制装置，所述水下机器人姿态控制装置包括：

信息获取模块，用于获取待控制水下机器人在机器人坐标系下的第一位姿信息和在世界坐标系下的第二位姿信息；

姿态判断模块，用于根据所述第一位姿信息和所述第二位姿信息判断所述待控制水下机器人的姿态是否需要调整；

姿态调整模块，用于在所述待控制水下机器人的姿态需要调整时，则获取所述待控制水下机器人的各个机械结构中结点的结点位置，并根据所述结点位置对所述待控制水下机器人的姿态进行调整；

所述姿态判断模块，还用于在姿态调整完成后，获取所述待控制水下机器人的工作参数，并根据所述工作参数判断所述待控制水下机器人的姿态是否符合预设姿态标准。

可选地，所述姿态判断模块，还用于获取所述第一位姿信息和所述第二位姿信息中处于同一机械结构的第一目标位姿信息和第二目标位姿信息；确定所述待控制水下机器人在所述机器人坐标系和所述世界坐标系之间的转换矩阵；根据所述第一目标位姿信息和所述转换矩阵确定所述第一目标位姿信息在所述世界坐标系下的第三目标位姿信息；根据所述第二目标位姿信息和所述第三目标位姿信息判断所述待控制水下机器人的姿态是否需要调整。

可选地，所述姿态判断模块，还用于判断所述第二目标位姿信息和所述第三目标位姿信息之间是否存在重合部分；若是，则确定所述重合部分对应的所述待控制水下机器人的目标机械结构；获取所述目标机械结构在工作状态下的预设位姿信息；根据所述第二目标位姿信息、所述第三目标位姿信息以及所述预设位姿信息判断所述待控制水下机器人的姿态是否需要调整。

本发明通过获取待控制水下机器人在机器人坐标系下的第一位姿信息和在世界坐标系下的第二位姿信息，然后根据第一位姿信息和第二位姿信息判断待控制水下机器人的姿态是否需要调整，若是，则获取待控制水下机器人的各个机械结构中结点的结点位置，并根据结点位置对待控制水下机器人的姿态进行调整，在姿态调整完成后，获取待控制水下机器人的工作参数，并根据工作参数判断待控制水下机器人的姿态是否符合预设姿态标准。本发明根据第一位姿信息和第二位姿信息判断待控制水下机器人的姿态是否需要调整，能够及时判断姿态是否需要调整，在姿态需要调整时，根据待控制水下机器人的各个机械结构中的结点位置对待控制水下机器人的姿态进行调整，能够及时有效地对待控制水下机器人的姿态进行调整，在姿态调整完成后，根据工作参数判断待控制水下机器人的姿态是否符合预设姿态标准，能够在姿态调整完成后，有效地判断调整后的姿态是否符合预设姿态标准，以判断是否需要再次进行调整，能够有效地控制水下机器人的姿态，保证水下机器人的正常工作。

附图说明

图1为本发明水下机器人姿态控制方法第一实施例的流程示意图；

图2为本发明水下机器人姿态控制方法第二实施例的流程示意图；

图3为本发明水下机器人姿态控制方法第三实施例的流程示意图；

图4为本发明水下机器人姿态控制方法一实施例的待控制水下机器人的示意图；

图5为本发明水下机器人姿态控制装置第一实施例的结构框图。

本发明目的的实现、功能特点及优点将结合实施例，参照附图做进一步说明。

具体实施方式

应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。

本发明实施例提供了一种水下机器人姿态控制方法，参照图1，图1为本发明水下机器人姿态控制方法第一实施例的流程示意图。

本实施例中，所述水下机器人姿态控制方法包括以下步骤：

步骤S10：获取待控制水下机器人在机器人坐标系下的第一位姿信息和在世界坐标系下的第二位姿信息；

需要说明的是，本实施例的执行主体可以是一种具有数据处理、网络通信以及程序运行功能的计算服务设备，例如平板电脑、个人电脑等。以下以计算机为例，对本实施例及下述各实施例进行说明。

可理解的是，机器人坐标系是以机器人安装基座为基准、用来描述机器人本体运动的直角坐标系，具体可设置为面对待控制水下机器人的前后为X轴，待控制水下机器人的左右为Y轴，待控制水下机器人的上下为Z轴。世界坐标系是固定的坐标系，其X轴是横轴，Y轴是纵轴，Z轴垂直于XY平面，可以以垂直于地面的平面作为Z轴。

应理解的是，由于待控制水下机器人在水下的状态可能不是直立状态，因此在待控制水下机器人在机器人坐标系下的第一位姿信息和在世界坐标系下的第二位姿信息可能不相同。位姿信息是指位置信息和姿态信息，位置信息可包括待控制水下机器人上的各个机械结构的坐标，姿态信息可包括待控制水下机器人的姿势，例如，直立、俯冲等。

在具体实现中，可通过待控制水下机器人上安装的传感器得到在机器人坐标系下的第一位姿信息，传感器可包括姿态传感器、位置传感器等。还可通过相机得到在机器人坐标系下的第二位姿信息，具体可以通过相机采集待控制水下机器人的图像，然后根据相机与世界坐标系之间的角度、相机与待控制水下机器人正面的角度对相机进行标定，再根据该图像得到在世界坐标系下的第二位姿信息。

步骤S20：根据所述第一位姿信息和所述第二位姿信息判断所述待控制水下机器人的姿态是否需要调整；

可理解的是，可根据第一位姿信息和第二位姿信息判断待控制水下机器人的姿态是否需要调整，具体可先获取待控制水下机器人预设的标准姿态，然后将第一位姿信息和第二位姿信息与预设标准姿态进行对比，若预设标准姿态中不存在上述第一位姿信息和第二位姿信息，则判定待控制水下机器人的姿态需要调整。

步骤S30：若是，则获取所述待控制水下机器人的各个机械结构中结点的结点位置，并根据所述结点位置对所述待控制水下机器人的姿态进行调整；

应理解的是，在需要对待控制水下机器人的姿态进行调整时，可获取待控制水下机器人的各个机械结构中结点的结点位置，机械结构可包括：头部、肩膀、肘部、手腕、下肢、臀部、膝关节、踝关节等。各个机械结构中可包含多个结点，具体的结点个数本实施例不做具体限制。

在具体实现中，可根据结点位置对待控制水下机器人的姿态进行调整，具体可先通过操作人员确定待控制水下机器人的当前工作状态，然后获取当前工作状态下的各结点之间的预设连接关系，并获取已经得到的待控制水下机器人中的结点位置连成的连接关系，将预设连接关系对结点位置构成的连接关系进行对比，以调整各结点的结点位置。还可以获取各结点在世界坐标系下的结点位置和机器人坐标系下的结点位置，再根据上述两个结点位置对待控制水下机器人的姿态进行调整。

步骤S40：在姿态调整完成后，获取所述待控制水下机器人的工作参数，并根据所述工作参数判断所述待控制水下机器人的姿态是否符合预设姿态标准。

可理解的是，在姿态调整完成后，调整后的姿态不一定是正常姿态，有可能还是需要再次调整，所以需要判断待控制水下机器人的姿态是否符合预设姿态标准，预设姿态标准可包括待控制水下机器人所有可能存在的姿态。

在具体实现中，本实施例可获取待控制水下机器人的工作参数，工作参数可包括：工作速度、工作载荷等，然后根据工作参数判断待控制水下机器人的姿态是否符合预设姿态标准，具体可根据获取到的工作参数的大小判断当前处于何种工作状态，然后获取该工作状态下的姿态标准，再将该姿态标准与上述调整后的姿态进行对比，以判断待控制水下机器人的姿态是否符合预设姿态标准。

进一步地，为了判断待控制水下机器人的姿态是否符合预设姿态标准，在本实施例中，所述步骤S40包括：在姿态调整完成后，获取所述待控制水下机器人的工作参数；通过相机确定所述待控制水下机器人的工作环境；根据所述工作环境和所述工作参数判断所述待控制水下机器人是否处于正常工作状态；若是，则判定所述待控制水下机器人的姿态是否符合预设姿态标准。

可理解的是，在姿态调整完成后，可通过相机确定待控制水下机器人的工作环境，工作环境可以是没有任何障碍物的环境，也可以是存在障碍物的环境，例如存在水草等植物妨碍待控制水下机器人的姿态调整。

应理解的是，本实施例可根据工作环境和工作参数判断待控制水下机器人是否处于正常工作状态，具体可将工作参数与预设的正常工作参数范围进行对比，还可根据工作环境判断是否处于正常工作状态。若处于正常工作状态，则判断待控制水下机器人调整后的姿态符合预设姿态标准。

本实施例通过获取待控制水下机器人在机器人坐标系下的第一位姿信息和在世界坐标系下的第二位姿信息，然后根据第一位姿信息和第二位姿信息判断待控制水下机器人的姿态是否需要调整，若是，则获取待控制水下机器人的各个机械结构中结点的结点位置，并根据结点位置对待控制水下机器人的姿态进行调整，在姿态调整完成后，获取待控制水下机器人的工作参数，并根据工作参数判断待控制水下机器人的姿态是否符合预设姿态标准。本发明根据第一位姿信息和第二位姿信息判断待控制水下机器人的姿态是否需要调整，能够及时判断姿态是否需要调整，在姿态需要调整时，根据待控制水下机器人的各个机械结构中的结点位置对待控制水下机器人的姿态进行调整，能够及时有效地对待控制水下机器人的姿态进行调整，在姿态调整完成后，根据工作参数判断待控制水下机器人的姿态是否符合预设姿态标准，能够在姿态调整完成后，有效地判断调整后的姿态是否符合预设姿态标准，以判断是否需要再次进行调整，能够有效地控制水下机器人的姿态，保证水下机器人的正常工作。

参考图2，图2为本发明水下机器人姿态控制方法第二实施例的流程示意图。

基于上述第一实施例，在本实施例中，所述步骤S20包括：

步骤S201：获取所述第一位姿信息和所述第二位姿信息中处于同一机械结构的第一目标位姿信息和第二目标位姿信息；

可理解的是，第一目标位姿信息为机器人坐标系下的位姿信息，第二目标位姿信息为世界坐标系下的位姿信息，第一目标位姿信息与第二目标位姿信息对应的机械结构相同，例如，获取头部在机器人坐标系下的第一目标位姿信息和头部在世界坐标系下的第二目标位姿信息，还需要获取其他机械结构的第一目标位姿信息和第二目标位姿信息。

步骤S202：确定所述待控制水下机器人在所述机器人坐标系和所述世界坐标系之间的转换矩阵；

应理解的是，本实施例可预先获取机器人坐标系和世界坐标系之间的转换矩阵，具体可获取在待控制水下机器人在直立状态下的机器人坐标系和世界坐标系之间的转换矩阵，还可获取在待控制水下机器人在侧卧状态下的机器人坐标系和世界坐标系之间的转换矩阵，还可获取其他倾斜状态下的机器人坐标系和世界坐标系之间的转换矩阵。

步骤S203：根据所述第一目标位姿信息和所述转换矩阵确定所述第一目标位姿信息在所述世界坐标系下的第三目标位姿信息；

可理解的是，转换矩阵是机器人坐标系和世界坐标系之间的转换矩阵，因此可以根据机器人坐标系下的第一目标位姿信息和转换矩阵确定第一目标位姿信息在世界坐标系下的第三目标位姿信息。

步骤S204：根据所述第二目标位姿信息和所述第三目标位姿信息判断所述待控制水下机器人的姿态是否需要调整。

应理解的是，第二目标位姿信息和第三目标位姿信息都是世界坐标系下的位姿信息，因此可根据同一坐标系下的位姿信息判断待控制水下机器人的姿态是否需要调整，具体可在第二目标位姿信息和第三目标位姿信息之间的差异过大时，需要对待控制水下机器人的姿态进行调整。

进一步地，为了判断是否需要对待控制水下机器人的姿态进行调整，在本实施例中，所述步骤S204包括：判断所述第二目标位姿信息和所述第三目标位姿信息之间是否存在重合部分；若是，则确定所述重合部分对应的所述待控制水下机器人的目标机械结构；获取所述目标机械结构在工作状态下的预设位姿信息；根据所述第二目标位姿信息、所述第三目标位姿信息以及所述预设位姿信息判断所述待控制水下机器人的姿态是否需要调整。

可理解的是，在世界坐标系下的第二目标位姿信息和第三目标位姿信息之间存在重合部分时，确定该重合部分对应的目标机械结构，由于该目标机械结构在机器人坐标系和世界坐标系下的位姿信息基本一致，说明该目标机械结构的位姿信息是正确的，该重合部分可能经过多个机械结构，因此上述目标机械结构可能存在多个，本实施例对此目标机械结构的个数不做具体限制。

应理解的是，在世界坐标系下的第二目标位姿信息和第三目标位姿信息之间不存在重合部分时，则说明第二目标位姿信息和第三目标位姿信息中存在错误的位姿信息，可将第二目标位姿信息和第三目标位姿信息分别与预设标准位姿信息进行对比，若第二目标位姿信息或第三目标位姿信息中存在与预设标准位姿信息相同或相似的信息，则认定待控制水下机器人的姿态不需要调整，若不存在，则认定待控制水下机器人的姿态需要调整。

在具体实现中，在确定目标机械结构后，可获取该目标机械结构在工作状态下的预设位姿信息，例如，头部在工作状态下的预设位姿信息可以是在10米范围内前后移动。然后将该目标机械结构对应的第二目标位姿信息或第三目标位姿信息与预设位姿信息进行对比，若第二目标位姿信息或第三目标位姿信息与预设位姿信息相同，则说明该目标机械结构对应的位姿信息不需要调整。

本实施例通过判断第二目标位姿信息和所述第三目标位姿信息之间是否存在重合部分，能够对重合部分对应的目标机械结构进行判断是否需要调整姿态的过程更加简单，如果不存在重合部分，则需要进行两种位姿信息的对比，如果存在重合部分，只需要进行一种位姿信息的对比，从而减少判断待控制水下机器人的姿态是否需要调整的计算量。

本实施例通过获取第一位姿信息和第二位姿信息中处于同一机械结构的第一目标位姿信息和第二目标位姿信息，然后确定待控制水下机器人在机器人坐标系和世界坐标系之间的转换矩阵，然后根据第一目标位姿信息和转换矩阵确定第一目标位姿信息在世界坐标系下的第三目标位姿信息，再根据第二目标位姿信息和第三目标位姿信息判断待控制水下机器人的姿态是否需要调整。本实施例通过获取同一坐标系下的处于同一机械结构的第一目标位姿信息和第二目标位姿信息，能够精确地判断待控制水下机器人的姿态是否需要调整，并减少上述判断过程的计算量。

参考图3，图3为本发明水下机器人姿态控制方法第三实施例的流程示意图。

基于上述各实施例，在本实施例中，所述步骤S30包括：

步骤S301：若是，则获取所述待控制水下机器人的各个机械结构中结点在所述机器人坐标系下的第一结点位置和所述世界坐标系下的第二结点位置；

需要说明的是，第一结点位置是指待控制水下机器人上的各个机械结构中的所有结点在机器人坐标系下的位置，对于一个机械结构可包含多个结点，例如头部包含两个结点，肩膀包含四个结点，参照图4，图4为本发明水下机器人姿态控制方法一实施例的待控制水下机器人的示意图，图4中的黑色圆形表示结点，图4还说明了本实施例中的世界坐标系和机器人坐标系的表示。具体可由操作人员预先对该待控制水下机器人上的结点进行标注。第二结点位置是指待控制水下机器人上的各个机械结点中的所有结点在世界坐标系下的位置。

步骤S302：确定采集所述第二位姿信息的相机对应的相机参数；

应理解的是，在世界坐标系下的第二位姿信息可通过相机获取，相机参数可包括相机内参数和相机外参数，相机内参数可包含焦距、像素的物理大小、图像的中心和原点相差的像素数量等，相机外参数可包含空间坐标系三个轴的旋转参数、三个轴的平移参数等。

步骤S303：根据所述第一结点位置和所述第二结点位置对所述相机参数进行调整，获得调整后的相机参数；

可理解的是，可根据结点在机器人坐标系下的第一结点位置和结点在世界坐标系下的第二结点位置对相机内参数和相机外参数进行调整，获得调整后的相机内参数和调整后的相机外参数。

进一步地，为了有效地对相机参数进行调整，在本实施例中，所述步骤S303包括：获取所述第二结点位置的变化曲线，并对所述变化曲线进行拟合处理，获得拟合曲线；将所述拟合曲线与所述待控制水下机器人的移动轨迹进行对比，并根据对比结果对所述相机参数中的相机内参数进行调整，获得调整后的相机内参数；根据所述第一结点位置和所述转换矩阵确定所述第一结点位置在所述世界坐标系下的第四结点位置；根据所述第四结点位置和所述第一结点位置对所述相机参数中的相机外参数进行调整，获得调整后的相机外参数。

应理解的是，由于待控制水下机器人可能在不停移动，因此待控制水下机器人上的结点也在不停移动，可获取世界坐标系下的第二结点位置的坐标变化情况，将坐标变化情况连接起来可以形成变化曲线，将变化曲线进行拟合处理后，可以获得较平滑的变化曲线，即本实施例中的拟合曲线。

可理解的是，在确定拟合曲线后，即可得到各个结点在待控制水下机器人移动的过程中的结点位置变化曲线，相当于各个结点的移动曲线，此时可将拟合曲线和待控制水下机器人的移动轨迹进行对比，若对比结果基本一致，则说明通过相机采集的第二结点位置是基本正确的，若对比结果不一致，则说明第二结点位置可能存在异常，也就是采集第二结点位置的相机存在异常，此时可以对相机内参数进行调整，具体可根据对比结果进行调整，如果拟合曲线相较于移动轨迹均向某一个方向移动，此时可将上述相机参数中的图像中心向反方向调整，具体调整方式可根据相机内参数的类型和对比结果进行调整，本实施例可由操作人员预先设定，对此不做具体限制。

在具体实现中，转换矩阵为待控制水下机器人在机器人坐标系和世界坐标系之间的转换矩阵，在得到待控制水下机器人上的各个机械结构中的所有结点在机器人坐标系下的第一结点位置后，可根据转换矩阵确定在世界坐标系下的第四结点位置。然后根据第一结点位置和第四结点位置对相机外参数进行调整，本实施例以机器人坐标系为基准，对相机与世界坐标系之间的关系进行调整，也就是对相机外参数进行调整，具体可以第一结点位置和以第一结点位置得到的第四结点位置为基准，在同一结点的第四结点位置和第二结点位置相差较大时，可根据第四结点位置对相机外参数进行调整，例如，在第四结点位置与第二结点位置在X轴的坐标差别过大时，可根据第四结点位置和第二结点位置的X坐标的差值对相机外参数中的X轴的旋转参数和平移参数进行调整。

步骤S304：基于所述调整后的相机参数确定所述结点在所述世界坐标系下的第三结点位置；

可理解的是，在相机参数进行调整后，可根据调整后的相机参数对结点的位置重新进行定位，得到指待控制水下机器人上的各个机械结构中的所有结点在世界坐标系下的位置，即第三结点位置。

步骤S305：根据所述第一结点位置和所述第三结点位置对各结点进行调整，以对所述待控制水下机器人的姿态进行调整。

进一步地，为了有效地对各结点进行调整，在本实施例中，所述步骤S305包括：根据所述第一结点位置和所述第三结点位置确定所述待控制水下机器人的当前工作状态和在所述机器人坐标系下的目标结点位置；获取所述当前工作状态下的所述待控制水下机器人在所述机器人坐标系下的预设结点位置；将所述目标结点位置和所述预设结点位置进行对比，获得位置对比结果；根据所述位置对比结果对各结点进行调整，以对所述待控制水下机器人的姿态进行调整。

应理解的是，可根据第一结点位置和第三结点位置确定待控制水下机器人的当前工作状态，即确定待控制水下机器人是否在正常工作，例如，如果第一结点位置和第三结点位置在较长时间内没有变化，则说明待控制水下机器人的当前工作状态为未工作。

可理解的是，可获取当前工作状态下的待控制水下机器人在机器人坐标系下的预设结点位置，还可根据工作环境、工作参数共同确定预设结点位置，并根据机器人坐标系下的第一结点位置和世界坐标系下的第三结点位置得到机器人坐标系下的目标结点位置，具体可将第三结点位置转换到机器人坐标系下，得到第五结点位置，再将第一结点位置和第五结点位置求均值得到目标结点位置。

在具体实现中，将目标结点位置和预设结点位置进行对比后，可获得位置对比结果，并根据位置对比结果对各结点进行调整，具体可将目标结点位置调整到预设结点位置，即可以对待控制水下机器人的姿态进行调整。

本实施例通过获取待控制水下机器人的各个机械结构中结点在机器人坐标系下的第一结点位置和所述世界坐标系下的第二结点位置，然后确定采集第二位姿信息的相机对应的相机参数，然后根据第一结点位置和所第二结点位置对相机参数进行调整，获得调整后的相机参数，再基于调整后的相机参数确定结点在所述世界坐标系下的第三结点位置，并根据所述第一结点位置和第三结点位置对各结点进行调整。本实施例通过对相机参数进行调整，能够确定更加精确的世界坐标系下的第三结点位置，并根据第一结点位置和第三结点位置对各结点进行调整，从而能够有效地控制水下机器人的姿态，保证水下机器人的正常工作。

参照图5，图5为本发明水下机器人姿态控制装置第一实施例的结构框图。

如图5所示，本发明实施例提出的水下机器人姿态控制装置包括：

信息获取模块10，用于获取待控制水下机器人在机器人坐标系下的第一位姿信息和在世界坐标系下的第二位姿信息；

姿态判断模块20，用于根据所述第一位姿信息和所述第二位姿信息判断所述待控制水下机器人的姿态是否需要调整；

姿态调整模块30，用于在所述待控制水下机器人的姿态需要调整时，则获取所述待控制水下机器人的各个机械结构中结点的结点位置，并根据所述结点位置对所述待控制水下机器人的姿态进行调整；

所述姿态判断模块20，还用于在姿态调整完成后，获取所述待控制水下机器人的工作参数，并根据所述工作参数判断所述待控制水下机器人的姿态是否符合预设姿态标准。

本实施例通过获取待控制水下机器人在机器人坐标系下的第一位姿信息和在世界坐标系下的第二位姿信息，然后根据第一位姿信息和第二位姿信息判断待控制水下机器人的姿态是否需要调整，若是，则获取待控制水下机器人的各个机械结构中结点的结点位置，并根据结点位置对待控制水下机器人的姿态进行调整，在姿态调整完成后，获取待控制水下机器人的工作参数，并根据工作参数判断待控制水下机器人的姿态是否符合预设姿态标准。本发明根据第一位姿信息和第二位姿信息判断待控制水下机器人的姿态是否需要调整，能够及时判断姿态是否需要调整，在姿态需要调整时，根据待控制水下机器人的各个机械结构中的结点位置对待控制水下机器人的姿态进行调整，能够及时有效地对待控制水下机器人的姿态进行调整，在姿态调整完成后，根据工作参数判断待控制水下机器人的姿态是否符合预设姿态标准，能够在姿态调整完成后，有效地判断调整后的姿态是否符合预设姿态标准，以判断是否需要再次进行调整，能够有效地控制水下机器人的姿态，保证水下机器人的正常工作。

需要说明的是，以上所描述的工作流程仅仅是示意性的，并不对本发明的保护范围构成限定，在实际应用中，本领域的技术人员可以根据实际的需要选择其中的部分或者全部来实现本实施例方案的目的，此处不做限制。

另外，未在本实施例中详尽描述的技术细节，可参见本发明任意实施例所提供的水下机器人姿态控制方法，此处不再赘述。

基于本发明上述水下机器人姿态控制装置第一实施例，提出本发明水下机器人姿态控制装置的第二实施例。

在本实施例中，所述姿态判断模块20，还用于获取所述第一位姿信息和所述第二位姿信息中处于同一机械结构的第一目标位姿信息和第二目标位姿信息；确定所述待控制水下机器人在所述机器人坐标系和所述世界坐标系之间的转换矩阵；根据所述第一目标位姿信息和所述转换矩阵确定所述第一目标位姿信息在所述世界坐标系下的第三目标位姿信息；根据所述第二目标位姿信息和所述第三目标位姿信息判断所述待控制水下机器人的姿态是否需要调整。

进一步地，所述姿态判断模块20，还用于判断所述第二目标位姿信息和所述第三目标位姿信息之间是否存在重合部分；若是，则确定所述重合部分对应的所述待控制水下机器人的目标机械结构；获取所述目标机械结构在工作状态下的预设位姿信息；根据所述第二目标位姿信息、所述第三目标位姿信息以及所述预设位姿信息判断所述待控制水下机器人的姿态是否需要调整。

进一步地，所述姿态调整模块30，还用于若是，则获取所述待控制水下机器人的各个机械结构中结点在所述机器人坐标系下的第一结点位置和所述世界坐标系下的第二结点位置；确定采集所述第二位姿信息的相机对应的相机参数；根据所述第一结点位置和所述第二结点位置对所述相机参数进行调整，获得调整后的相机参数；基于所述调整后的相机参数确定所述结点在所述世界坐标系下的第三结点位置；根据所述第一结点位置和所述第三结点位置对各结点进行调整，以对所述待控制水下机器人的姿态进行调整。

进一步地，所述调整后的相机参数包括：调整后的相机外参数和调整后的相机内参数；所述姿态调整模块30，还用于获取所述第二结点位置的变化曲线，并对所述变化曲线进行拟合处理，获得拟合曲线；将所述拟合曲线与所述待控制水下机器人的移动轨迹进行对比，并根据对比结果对所述相机参数中的相机内参数进行调整，获得调整后的相机内参数；根据所述第一结点位置和所述转换矩阵确定所述第一结点位置在所述世界坐标系下的第四结点位置；根据所述第四结点位置和所述第一结点位置对所述相机参数中的相机外参数进行调整，获得调整后的相机外参数。

进一步地，所述姿态调整模块30，还用于根据所述第一结点位置和所述第三结点位置确定所述待控制水下机器人的当前工作状态和在所述机器人坐标系下的目标结点位置；获取所述当前工作状态下的所述待控制水下机器人在所述机器人坐标系下的预设结点位置；将所述目标结点位置和所述预设结点位置进行对比，获得位置对比结果；根据所述位置对比结果对各结点进行调整，以对所述待控制水下机器人的姿态进行调整。

进一步地，所述姿态判断模块20，还用于在姿态调整完成后，获取所述待控制水下机器人的工作参数；通过相机确定所述待控制水下机器人的工作环境；根据所述工作环境和所述工作参数判断所述待控制水下机器人是否处于正常工作状态；若是，则判定所述待控制水下机器人的姿态是否符合预设姿态标准。

本发明水下机器人姿态控制装置的其他实施例或具体实现方式可参照上述各方法实施例，此处不再赘述。

需要说明的是，在本文中，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者系统不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者系统所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括该要素的过程、方法、物品或者系统中还存在另外的相同要素。

上述本发明实施例序号仅仅为了描述，不代表实施例的优劣。

通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到上述实施例方法可借助软件加必需的通用硬件平台的方式来实现，当然也可以通 过硬件，但很多情况下前者是更佳的实施方式。基于这样的理解，本发明的 技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质（如只读存储器/随机存取存储器、磁碟、光盘）中，包括若干指令用以使得一台终端设备(可以是手机，计算机，服务器，或者网络设备等)执行本发明各个实施例所述的方法。

以上仅为本发明的优选实施例，并非因此限制本发明的专利范围，凡是利用本发明说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本发明的专利保护范围内。

Claims (10)

1.一种水下机器人姿态控制方法，其特征在于，所述水下机器人姿态控制方法包括以下步骤：

获取待控制水下机器人在机器人坐标系下的第一位姿信息和在世界坐标系下的第二位姿信息；

根据所述第一位姿信息和所述第二位姿信息判断所述待控制水下机器人的姿态是否需要调整；

若是，则获取所述待控制水下机器人的各个机械结构中结点的结点位置，并根据所述结点位置对所述待控制水下机器人的姿态进行调整；

在姿态调整完成后，获取所述待控制水下机器人的工作参数，并根据所述工作参数判断所述待控制水下机器人的姿态是否符合预设姿态标准。

2.如权利要求1所述的水下机器人姿态控制方法，其特征在于，所述根据所述第一位姿信息和所述第二位姿信息判断所述待控制水下机器人的姿态是否需要调整的步骤，具体包括：

获取所述第一位姿信息和所述第二位姿信息中处于同一机械结构的第一目标位姿信息和第二目标位姿信息；

确定所述待控制水下机器人在所述机器人坐标系和所述世界坐标系之间的转换矩阵；

根据所述第一目标位姿信息和所述转换矩阵确定所述第一目标位姿信息在所述世界坐标系下的第三目标位姿信息；

根据所述第二目标位姿信息和所述第三目标位姿信息判断所述待控制水下机器人的姿态是否需要调整。

3.如权利要求2所述的水下机器人姿态控制方法，其特征在于，所述根据所述第二目标位姿信息和所述第三目标位姿信息判断所述待控制水下机器人的姿态是否需要调整的步骤，具体包括：

判断所述第二目标位姿信息和所述第三目标位姿信息之间是否存在重合部分；

若是，则确定所述重合部分对应的所述待控制水下机器人的目标机械结构；

获取所述目标机械结构在工作状态下的预设位姿信息；

根据所述第二目标位姿信息、所述第三目标位姿信息以及所述预设位姿信息判断所述待控制水下机器人的姿态是否需要调整。

4.如权利要求2所述的水下机器人姿态控制方法，其特征在于，所述若是，则获取所述待控制水下机器人的各个机械结构中结点的结点位置，并根据所述结点位置对所述待控制水下机器人的姿态进行调整的步骤，具体包括：

若是，则获取所述待控制水下机器人的各个机械结构中结点在所述机器人坐标系下的第一结点位置和所述世界坐标系下的第二结点位置；

确定采集所述第二位姿信息的相机对应的相机参数；

根据所述第一结点位置和所述第二结点位置对所述相机参数进行调整，获得调整后的相机参数；

基于所述调整后的相机参数确定所述结点在所述世界坐标系下的第三结点位置；

根据所述第一结点位置和所述第三结点位置对各结点进行调整，以对所述待控制水下机器人的姿态进行调整。

5.如权利要求4所述的水下机器人姿态控制方法，其特征在于，所述调整后的相机参数包括：调整后的相机外参数和调整后的相机内参数；

所述根据所述第一结点位置和所述第二结点位置对所述相机参数进行调整，获得调整后的相机参数的步骤，具体包括：

获取所述第二结点位置的变化曲线，并对所述变化曲线进行拟合处理，获得拟合曲线；

将所述拟合曲线与所述待控制水下机器人的移动轨迹进行对比，并根据对比结果对所述相机参数中的相机内参数进行调整，获得调整后的相机内参数；

根据所述第一结点位置和所述转换矩阵确定所述第一结点位置在所述世界坐标系下的第四结点位置；

根据所述第四结点位置和所述第一结点位置对所述相机参数中的相机外参数进行调整，获得调整后的相机外参数。

6.如权利要求4所述的水下机器人姿态控制方法，其特征在于，所述根据所述第一结点位置和所述第三结点位置对各结点进行调整，以对所述待控制水下机器人的姿态进行调整的步骤，具体包括：

根据所述第一结点位置和所述第三结点位置确定所述待控制水下机器人的当前工作状态和在所述机器人坐标系下的目标结点位置；

获取所述当前工作状态下的所述待控制水下机器人在所述机器人坐标系下的预设结点位置；

将所述目标结点位置和所述预设结点位置进行对比，获得位置对比结果；

根据所述位置对比结果对各结点进行调整，以对所述待控制水下机器人的姿态进行调整。

7.如权利要求1~6中任一项所述的水下机器人姿态控制方法，其特征在于，所述在姿态调整完成后，获取所述待控制水下机器人的工作参数，并根据所述工作参数判断所述待控制水下机器人的姿态是否符合预设姿态标准的步骤，具体包括：

在姿态调整完成后，获取所述待控制水下机器人的工作参数；

通过相机确定所述待控制水下机器人的工作环境；

根据所述工作环境和所述工作参数判断所述待控制水下机器人是否处于正常工作状态；

若是，则判定所述待控制水下机器人的姿态是否符合预设姿态标准。

8.一种水下机器人姿态控制装置，其特征在于，所述水下机器人姿态控制装置包括：

信息获取模块，用于获取待控制水下机器人在机器人坐标系下的第一位姿信息和在世界坐标系下的第二位姿信息；

姿态判断模块，用于根据所述第一位姿信息和所述第二位姿信息判断所述待控制水下机器人的姿态是否需要调整；

姿态调整模块，用于在所述待控制水下机器人的姿态需要调整时，则获取所述待控制水下机器人的各个机械结构中结点的结点位置，并根据所述结点位置对所述待控制水下机器人的姿态进行调整；

所述姿态判断模块，还用于在姿态调整完成后，获取所述待控制水下机器人的工作参数，并根据所述工作参数判断所述待控制水下机器人的姿态是否符合预设姿态标准。

9.如权利要求8所述的水下机器人姿态控制装置，其特征在于，所述姿态判断模块，还用于获取所述第一位姿信息和所述第二位姿信息中处于同一机械结构的第一目标位姿信息和第二目标位姿信息；确定所述待控制水下机器人在所述机器人坐标系和所述世界坐标系之间的转换矩阵；根据所述第一目标位姿信息和所述转换矩阵确定所述第一目标位姿信息在所述世界坐标系下的第三目标位姿信息；根据所述第二目标位姿信息和所述第三目标位姿信息判断所述待控制水下机器人的姿态是否需要调整。

10.如权利要求9所述的水下机器人姿态控制装置，其特征在于，所述姿态判断模块，还用于判断所述第二目标位姿信息和所述第三目标位姿信息之间是否存在重合部分；若是，则确定所述重合部分对应的所述待控制水下机器人的目标机械结构；获取所述目标机械结构在工作状态下的预设位姿信息；根据所述第二目标位姿信息、所述第三目标位姿信息以及所述预设位姿信息判断所述待控制水下机器人的姿态是否需要调整。

Images