CN116337331B - 水下机器人平衡检测方法、控制装置及设备 - Google Patents
水下机器人平衡检测方法、控制装置及设备 Download PDFInfo
- Publication number
- CN116337331B CN116337331B CN202310622833.0A CN202310622833A CN116337331B CN 116337331 B CN116337331 B CN 116337331B CN 202310622833 A CN202310622833 A CN 202310622833A CN 116337331 B CN116337331 B CN 116337331B
- Authority
- CN
- China
- Prior art keywords
- underwater robot
- detected
- angle
- connecting rod
- balance detection
- 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
Links
- 238000001514 detection method Methods 0.000 title claims abstract description 137
- 230000008859 change Effects 0.000 claims description 34
- 238000000034 method Methods 0.000 claims description 33
- 238000005070 sampling Methods 0.000 claims description 24
- 230000001133 acceleration Effects 0.000 claims description 13
- 230000005484 gravity Effects 0.000 claims description 13
- 238000005259 measurement Methods 0.000 claims description 10
- 238000006073 displacement reaction Methods 0.000 claims 1
- 210000001503 joint Anatomy 0.000 description 8
- 238000010586 diagram Methods 0.000 description 7
- 238000004891 communication Methods 0.000 description 6
- 230000008569 process Effects 0.000 description 5
- 210000000544 articulatio talocruralis Anatomy 0.000 description 3
- 210000000629 knee joint Anatomy 0.000 description 3
- 239000012530 fluid Substances 0.000 description 2
- 238000005096 rolling process Methods 0.000 description 2
- 230000002411 adverse Effects 0.000 description 1
- 239000000470 constituent Substances 0.000 description 1
- 230000009189 diving Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000006870 function Effects 0.000 description 1
- 230000003993 interaction Effects 0.000 description 1
- 238000010606 normalization Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01M—TESTING STATIC OR DYNAMIC BALANCE OF MACHINES OR STRUCTURES; TESTING OF STRUCTURES OR APPARATUS, NOT OTHERWISE PROVIDED FOR
- G01M1/00—Testing static or dynamic balance of machines or structures
- G01M1/12—Static balancing; Determining position of centre of gravity
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01M—TESTING STATIC OR DYNAMIC BALANCE OF MACHINES OR STRUCTURES; TESTING OF STRUCTURES OR APPARATUS, NOT OTHERWISE PROVIDED FOR
- G01M1/00—Testing static or dynamic balance of machines or structures
- G01M1/14—Determining imbalance
-
- 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
- Y02E—REDUCTION OF GREENHOUSE GAS [GHG] EMISSIONS, RELATED TO ENERGY GENERATION, TRANSMISSION OR DISTRIBUTION
- Y02E30/00—Energy generation of nuclear origin
- Y02E30/30—Nuclear fission reactors
Landscapes
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明公开了一种水下机器人平衡检测方法、控制装置及设备。本发明通过确定待检测水下机器人中处于同一关节位置的连杆，并获取连杆的连杆坐标，然后根据连杆坐标对待检测水下机器人进行初次平衡检测，在初次平衡检测通过时，获取待检测水下机器人在当前时刻下的姿态角，再根据姿态角对待检测水下机器人进行平衡检测。本发明先根据连杆坐标对待检测水下机器人进行初次平衡检测，能够粗略地判断待检测水下机器人是否处于平衡状态，在初次平衡检测通过时，根据姿态角对待检测水下机器人再次进行平衡检测，本发明通过对待检测水下机器人进行两次平衡检测，能够有效地对待检测水下机器人进行平衡检测，从而能够得到更加精确的平衡检测结果。
Description
技术领域
本发明涉及水下机器人技术领域，尤其涉及一种水下机器人平衡检测方法、控制装置及设备。
背景技术
水下机器人是一种工作于水下的极限作业机器人，水下环境恶劣危险，人的潜水深度有限，所以水下机器人已成为开发海洋的重要工具。水下机器人在水下工作过程中，由于障碍物及暗流、漩涡等影响会失去平衡，导致水下机器人的机身严重摇摆、晃动。如果对水下机器人的平衡状态未直接监测，或者在水下机器人失去平衡时被动等待的措施，则会导致水下机器人的工作中断。因此，如何有效地水下机器人进行平衡检测，成为一个亟待解决的问题。
上述内容仅用于辅助理解本发明的技术方案，并不代表承认上述内容是现有技术。
发明内容
本发明的主要目的在于提供了一种水下机器人平衡检测方法、控制装置、及设备，旨在解决如何有效地水下机器人进行平衡检测的技术问题。
为实现上述目的，本发明提供了一种水下机器人平衡检测方法，所述水下机器人平衡检测方法包括以下步骤：
确定待检测水下机器人中处于同一关节位置的连杆，并获取所述连杆的连杆坐标；
根据所述连杆坐标对所述待检测水下机器人进行初次平衡检测；
在初次平衡检测通过时，获取所述待检测水下机器人在当前时刻下的姿态角；
根据所述姿态角对所述待检测水下机器人进行平衡检测。
可选地，所述根据所述连杆坐标对所述待检测水下机器人进行初次平衡检测的步骤，具体包括：
获取所述待检测水下机器人的重心坐标和浮心坐标；
根据所述重心坐标和所述浮心坐标确定所述待检测水下机器人的中心坐标；
获取所述连杆坐标对应的连杆在所述待检测水下机器人移动时产生的连杆角度偏移的变化量；
根据所述中心坐标和所述连杆角度偏移的变化量对所述待检测水下机器人进行初次平衡检测。
可选地，所述获取所述连杆坐标对应的连杆在所述待检测水下机器人移动时产生的连杆角度偏移的变化量的步骤，具体包括：
获取所述连杆坐标对应的连杆在预设时间段内的初始连杆角度偏移；
从所述初始连杆角度偏移中选取处于同一关节位置的目标连杆角度偏移；
对所述目标连杆角度偏移进行归一化处理，获得归一化的连杆角度偏移；
根据所述归一化的连杆角度偏移确定连杆角度偏移的变化量。
可选地，所述根据所述中心坐标和所述连杆角度偏移的变化量对所述待检测水下机器人进行初次平衡检测的步骤，具体包括：
确定所述中心坐标处于所述待检测水下机器人的目标关节；
从所述连杆角度偏移的变化量中选取所述目标关节对应的连杆的目标变化量和剩余连杆的剩余变化量；
从所述目标变化量中选取处于所述待检测水下机器人的同一关节位置的第一变化量，并从所述剩余变化量中选取处于所述待检测水下机器人的同一关节位置的第二变化量；
在所述第一变化量满足第一预设条件且所述第二变化量满足第二预设条件时，判定所述待检测水下机器人初次平衡检测通过。
可选地，所述姿态角包括：俯仰角、偏航角以及滚转角；
所述在初次平衡检测通过时，获取所述待检测水下机器人在当前时刻下的姿态角的步骤，具体包括：
在初次平衡检测通过时，获取所述待检测水下机器人在前一采样时刻的初始偏航角；
通过陀螺仪采集所述待检测水下机器人在前一采样时刻的角速度，并获取所述陀螺仪在当前时刻下的测量误差；
根据所述初始偏航角、所述角速度和所述测量误差确定所述偏航角；
获取所述待检测水下机器人在前一采样时刻的初始俯仰角和初始滚转角；
通过加速度计采集所述待检测水下机器人在前一采样时刻的加速度；
根据所述初始俯仰角和所述加速度确定所述俯仰角，并根据所述初始滚转角和所述加速度确定所述滚转角。
可选地，所述根据所述姿态角对所述待检测水下机器人进行平衡检测的步骤，具体包括：
确定所述俯仰角中的正俯仰角和负俯仰角，确定所述偏航角中的正偏航角和负偏航角，确定所述滚转角中的正滚转角和负滚转角；
根据所述正俯仰角和所述负俯仰角确定俯仰角分布情况，根据所述正偏航角和所述负偏航角确定偏航角分布情况，根据所述正滚转角和所述负滚转角确定滚转角分布情况；
根据所述俯仰角分布情况、所述偏航角分布情况以及所述滚转角分布情况对所述待检测水下机器人进行平衡检测。
可选地，所述根据所述俯仰角分布情况、所述偏航角分布情况以及所述滚转角分布情况对所述待检测水下机器人进行平衡检测的步骤，具体包括：
获取所述待检测水下机器人受到的驱动力矩和所述驱动力矩的驱动方向；
根据所述驱动方向确定所述待检测水下机器人的预设运动轨迹；
根据所述俯仰角分布情况、所述偏航角分布情况以及所述滚转角分布情况确定所述待检测水下机器人在各个轴向的运动轨迹；
根据所述各个轴向的运动轨迹和所述预设运动轨迹对所述待检测水下机器人进行平衡检测。
此外，为实现上述目的，本发明还提供一种水下机器人平衡检测装置，所述水下机器人平衡检测装置包括：
坐标获取模块，用于确定待检测水下机器人中处于同一关节位置的连杆，并获取所述连杆的连杆坐标；
平衡检测模块，用于根据所述连杆坐标对所述待检测水下机器人进行初次平衡检测；
姿态角获取模块，用于在初次平衡检测通过时，获取所述待检测水下机器人在当前时刻下的姿态角；
所述平衡检测模块，还用于根据所述姿态角对所述待检测水下机器人进行平衡检测。
可选地，所述平衡检测模块，还用于获取所述待检测水下机器人的重心坐标和浮心坐标；根据所述重心坐标和所述浮心坐标确定所述待检测水下机器人的中心坐标；获取所述连杆坐标对应的连杆在所述待检测水下机器人移动时产生的连杆角度偏移的变化量；根据所述中心坐标和所述连杆角度偏移的变化量对所述待检测水下机器人进行初次平衡检测。
此外，为实现上述目的，本发明还提出一种水下机器人平衡检测设备，所述水下机器人平衡检测设备包括：存储器、处理器及存储在所述存储器上并可在所述处理器上运行的水下机器人平衡检测程序，所述水下机器人平衡检测程序配置为实现如上文所述的水下机器人平衡检测方法的步骤。
本发明通过确定待检测水下机器人中处于同一关节位置的连杆，并获取连杆的连杆坐标，然后根据连杆坐标对待检测水下机器人进行初次平衡检测，在初次平衡检测通过时，获取待检测水下机器人在当前时刻下的姿态角，再根据姿态角对待检测水下机器人进行平衡检测。本发明先根据连杆坐标对待检测水下机器人进行初次平衡检测，能够粗略地判断待检测水下机器人是否处于平衡状态，在初次平衡检测通过时，根据姿态角对待检测水下机器人再次进行平衡检测，本发明通过对待检测水下机器人进行两次平衡检测，能够有效地对待检测水下机器人进行平衡检测，从而能够得到更加精确的平衡检测结果。
附图说明
图1是本发明实施例方案涉及的硬件运行环境的水下机器人平衡检测设备的结构示意图；
图2为本发明水下机器人平衡检测方法第一实施例的流程示意图；
图3为本发明水下机器人平衡检测方法一实施例的俯仰角、偏航角以及滚转角的示意图；
图4为本发明水下机器人平衡检测方法第二实施例的流程示意图；
图5为本发明水下机器人平衡检测方法第三实施例的流程示意图；
图6为本发明水下机器人平衡检测的控制装置第一实施例的结构框图。
本发明目的的实现、功能特点及优点将结合实施例，参照附图做进一步说明。
具体实施方式
应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。
参照图1，图1为本发明实施例方案涉及的硬件运行环境的水下机器人平衡检测设备结构示意图。
如图1所示，该水下机器人平衡检测设备可以包括：处理器1001，例如中央处理器（Central Processing Unit，CPU），通信总线1002、用户接口1003，网络接口1004，存储器1005。其中，通信总线1002用于实现这些组件之间的连接通信。用户接口1003可以包括显示屏（Display）、输入单元比如键盘（Keyboard），可选用户接口1003还可以包括标准的有线接口、无线接口。网络接口1004可选的可以包括标准的有线接口、无线接口（如无线保真（Wireless-Fidelity，Wi-Fi）接口）。存储器1005可以是高速的随机存取存储器（RandomAccess Memory，RAM），也可以是稳定的非易失性存储器（Non-Volatile Memory，NVM），例如磁盘存储器。存储器1005可选的还可以是独立于前述处理器1001的存储装置。
本领域技术人员可以理解，图1中示出的结构并不构成对水下机器人平衡检测设备的限定，可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件布置。
如图1所示，作为一种存储介质的存储器1005中可以包括操作系统、网络通信模块、用户接口模块以及水下机器人平衡检测程序。
在图1所示的水下机器人平衡检测设备中，网络接口1004主要用于与网络服务器进行数据通信；用户接口1003主要用于与用户进行数据交互；本发明水下机器人平衡检测设备中的处理器1001、存储器1005可以设置在水下机器人平衡检测设备中，所述水下机器人平衡检测设备通过处理器1001调用存储器1005中存储的水下机器人平衡检测程序，并执行本发明实施例提供的水下机器人平衡检测方法。
基于上述水下机器人平衡检测设备，本发明实施例提供了一种水下机器人平衡检测方法，参照图2，图2为本发明水下机器人平衡检测方法第一实施例的流程示意图。
本实施例中，所述水下机器人平衡检测方法包括以下步骤：
步骤S10：确定待检测水下机器人中处于同一关节位置的连杆，并获取所述连杆的连杆坐标；
需要说明的是，本实施例的执行主体可以是一种具有数据处理、网络通信以及程序运行功能的计算服务设备，例如平板电脑、个人电脑等。以下以计算机为例，对本实施例及下述各实施例进行说明。
可理解的是，待检测水下机器人运动的姿态、位置等是通过控制水下机器人的各关节的运动实现的，待检测水下机器人中可包含多个关节，例如，膝关节、踝关节等，具体可将待检测水下机器人能够拆卸下来的部分作为关节。
应理解的是，待检测水下机器人的各个连杆的连接处为关节，可确定同一关节位置的连杆，即连接相同关节的连杆，例如，连接左膝关节和左踝关节之间的连杆和连接右膝关节和右踝关节之间的连杆可作为处于同一关节位置的连杆。
在具体实现中，可获取连杆的连杆坐标，即连杆上两个端点的坐标，具体可获取机器人坐标系下的连杆坐标，也可以获取世界坐标系下的连杆坐标，本实施例对此不做具体限制。
步骤S20：根据所述连杆坐标对所述待检测水下机器人进行初次平衡检测；
可理解的是，可根据连杆坐标对待检测水下机器人进行初次平衡检测，具体平衡检测方法可以是对同一关节位置的连杆坐标在预设时间段内的变化情况，由于机器人左右两侧的关节的变化情况在平衡状态下是基本一致的，可根据变化情况对待检测水下机器人进行初次平衡检测。
步骤S30：在初次平衡检测通过时，获取所述待检测水下机器人在当前时刻下的姿态角；
需要说明的是，本实施例中的姿态角可包括俯仰角、偏航角以及滚转角，俯仰角是指机器人围绕X轴旋转的夹角，偏航角是指机器人围绕Y轴旋转的夹角，滚转角是指机器人围绕Z轴的夹角。参照图3，图3为本发明水下机器人平衡检测方法一实施例的俯仰角、偏航角以及滚转角的示意图。
应理解的是，在初次平衡检测通过时，可获取待检测水下机器人在当前时刻下的姿态角，即可获取待检测水下机器人在当前时刻下的俯仰角、偏航角以及滚转角，具体可通过角度传感器采集，或者通过各个关节上安装的电机的驱动力确定姿态角。
进一步地，为了精确确定姿态角，在本实施例中，所述步骤S30包括：在初次平衡检测通过时，获取所述待检测水下机器人在前一采样时刻的初始偏航角；通过陀螺仪采集所述待检测水下机器人在前一采样时刻的角速度，并获取所述陀螺仪在当前时刻下的测量误差；根据所述初始偏航角、所述角速度和所述测量误差确定所述偏航角；获取所述待检测水下机器人在前一采样时刻的初始俯仰角和初始滚转角；通过加速度计采集所述待检测水下机器人在前一采样时刻的加速度；根据所述初始俯仰角和所述加速度确定所述俯仰角，并根据所述初始滚转角和所述加速度确定所述滚转角。
可理解的是，在初始平衡检测通过时，可获取待检测水下机器人在前一采样时刻的初始偏航角，前一采样时刻可以是前1秒、前2秒等，本实施例对此不做具体限制。还可以通过待检测水下机器人上设置的陀螺仪采集前一采样时刻的角速度，上述两个前一采样时刻为同一时刻。
应理解的是，可获取陀螺仪在当前时刻下的测量误差，即陀螺仪在采集角速度时产生的误差，该测量误差是陀螺仪本身产生的误差，可由陀螺仪的出厂参数得到。当前时刻下的偏航角可以由初始偏航角+角速度乘以采样间隔时间+测量误差得到。
在具体实现中，还可获取待检测水下机器人在前一采样时刻的初始俯仰角和初始滚转角，并通过待检测水下机器人上设置的加速度计采集所述待检测水下机器人在前一采样时刻的加速度。当前时刻下的俯仰角可以是初始俯仰角+角速度乘以采样间隔时间得到，当前时刻下的滚转角可以是初始滚转角+角速度乘以采样间隔时间得到。
步骤S40：根据所述姿态角对所述待检测水下机器人进行平衡检测。
可理解的是，本实施例可根据姿态角对待检测水下机器人再次进行平衡检测，具体可根据俯仰角、偏航角以及滚转角的变化情况进行平衡检测，例如，在俯仰角、偏航角以及滚转角的变化过快时，待检测水下机器人可能没有平衡，在俯仰角、偏航角或者滚转角始终处于一个方向，待检测水下机器人可能没有平衡。
本实施例通过确定待检测水下机器人中处于同一关节位置的连杆，并获取连杆的连杆坐标，然后根据连杆坐标对待检测水下机器人进行初次平衡检测，在初次平衡检测通过时，获取待检测水下机器人在当前时刻下的姿态角，再根据姿态角对待检测水下机器人进行平衡检测。本实施例先根据连杆坐标对待检测水下机器人进行初次平衡检测，能够粗略地判断待检测水下机器人是否处于平衡状态，在初次平衡检测通过时，根据姿态角对待检测水下机器人再次进行平衡检测，本实施例通过对待检测水下机器人进行两次平衡检测，能够有效地对待检测水下机器人进行平衡检测，从而能够得到更加精确的平衡检测结果。
参考图4，图4为本发明水下机器人平衡检测方法第二实施例的流程示意图。
基于上述第一实施例，在本实施例中，所述步骤S20包括：
步骤S201：获取所述待检测水下机器人的重心坐标和浮心坐标；
需要说明的是，重心是指待检测水下机器人处于任何方位时所有各组成支点的重力的合力都通过的那一点，浮心是指待检测水下机器人的水下部分体积的形心。
应理解的是，本实施例可获取待检测水下机器人在同一坐标系下的重心坐标和浮心坐标，获取浮心坐标的方式可以是先可得到待检测水下机器人受到的浮力大小，该浮力大小为待检测水下机器人排开那部分流体所受到的重力，而浮心位置为被排开流体的重心的位置。
步骤S202：根据所述重心坐标和所述浮心坐标确定所述待检测水下机器人的中心坐标；
可理解的是，待检测水下机器人的中心坐标可根据重心坐标和浮心坐标确定，具体可将重心坐标与浮心坐标之间的中点作为中心坐标，中心坐标的X轴坐标为（重心坐标的X轴坐标+浮心坐标的X轴坐标）/2，中心坐标的Y轴坐标为（重心坐标的Y轴坐标+浮心坐标的Y轴坐标）/2，中心坐标的Z轴坐标为（重心坐标的Z轴坐标+浮心坐标的Z轴坐标）/2。
步骤S203：获取所述连杆坐标对应的连杆在所述待检测水下机器人移动时产生的连杆角度偏移的变化量；
应理解的是，在待检测水下机器人的移动过程中，关节的位置会不断发生改变，因此连接各个关节之间的连杆坐标也会不断发生改变，连杆角度也会不断发生偏移，此时可获取连杆角度偏移的变化量，例如，可先获取第一连杆坐标和第二连杆坐标，再确定第一连杆坐标和第二连杆坐标之间的第一连杆角度偏移，还可确定第二连杆坐标和第三连杆坐标之间的第二角度偏移，第一连杆角度偏移与第二连杆角度偏移之间的差值可作为连杆角度偏移的变化量。
进一步地，为了精确确定连杆角度偏移的变化量，在本实施例中，所述步骤S203包括：获取所述连杆坐标对应的连杆在预设时间段内的初始连杆角度偏移；从所述初始连杆角度偏移中选取处于同一关节位置的目标连杆角度偏移；对所述目标连杆角度偏移进行归一化处理，获得归一化的连杆角度偏移；根据所述归一化的连杆角度偏移确定连杆角度偏移的变化量。
可理解的是，预设时间段为预先设置的时间段，例如，5秒、6秒、7秒等，本实施例对此不做具体限制。可获取预设时间段内的初始连杆角度偏移，例如预设时间段为5秒，可获取连杆坐标对应的连杆在第0秒的第一连杆坐标和第5秒的第二连杆坐标，再根据第一连杆坐标和第二连杆坐标确定初始连杆角度偏移，还可获取该连杆在第10秒的第三连杆坐标，并根据第二连杆坐标和第三连杆坐标确定初始连杆角度偏移。
应理解的是，可从初始连杆角度偏移中选取处于同一关节位置的目标连杆角度偏移，即连接相同关节的连杆的目标连杆角度偏移。再对目标连杆角度偏移进行归一化处理，获得归一化的连杆角度偏移，能够对目标连杆角度偏移被限定在一定的范围内，从而消除奇异样本数据导致的不良影响。
在具体实现中，可根据归一化的连杆角度偏移确定连杆角度偏移的变化量，即可获取相邻连杆角度偏移之间的差值作为变化量，例如，第0秒到第5秒的连杆角度偏移为0.8，第5秒到第10秒的连杆角度偏移为0.7，第10秒到第15秒的连杆角度偏移为0.8，连杆角度偏移的变化量为-0.1，0.1。
步骤S204：根据所述中心坐标和所述连杆角度偏移的变化量对所述待检测水下机器人进行初次平衡检测。
进一步地，为了有效地进行初次平衡检测，在本实施例中，所述步骤S204包括：确定所述中心坐标处于所述待检测水下机器人的目标关节；从所述连杆角度偏移的变化量中选取所述目标关节对应的连杆的目标变化量和剩余连杆的剩余变化量；从所述目标变化量中选取处于所述待检测水下机器人的同一关节位置的第一变化量，并从所述剩余变化量中选取处于所述待检测水下机器人的同一关节位置的第二变化量；在所述第一变化量满足第一预设条件且所述第二变化量满足第二预设条件时，判定所述待检测水下机器人初次平衡检测通过。
可理解的是，本实施例可确定中心坐标在待检测水下机器人中所处的目标关节，由于中心坐标可能不断变化，因此所处的目标关节也可能不断变化，具体可获取待检测水下机器人中的各个关节中的所有点的坐标构成的坐标范围，然后确定中心坐标处于哪个坐标范围中，即可确定目标关节。
应理解的是，可先确定目标关节对应的连杆，该连杆可以连接目标关节与另一个关节，因此可能存在两个连杆，然后从连杆角度偏移的变化量中选取上述两个连杆对应的目标变化量和除上述两个连杆的剩余连杆对应的剩余变化量。再从目标变化量中选取同一关节位置的第一变化量，并从剩余变化量中选取同一关节位置的第二变化量。
在具体实现中，第一预设条件和第二预设条件为预先设置的条件，例如，第一预设条件包括A关节与B关节之间的变化量为0.3~0.5，B关节与C关节之间的变化量为0.4~0.5等，第二预设条件包括A关节与B关节之间的变化量为0.2~0.6，B关节与C关节之间的变化量为0.3~0.6等，本实施例可设置第二预设条件中各个关节与关节之间的变化量范围大于第一预设条件中相同的关节与关节之间的变化范围，也就是中心所处的目标关节的变化量范围大于其余关节的变化量范围，对于具体值本实施例不做具体限制。在第一变化量满足第一预设条件且第二变化量满足第二预设条件时，判定待检测水下机器人初次平衡检测通过。
本实施例通过获取待检测水下机器人的重心坐标和浮心坐标，然后根据重心坐标和浮心坐标确定待检测水下机器人的中心坐标，然后获取连杆坐标对应的连杆在待检测水下机器人移动时产生的连杆角度偏移的变化量，再根据中心坐标和连杆角度偏移的变化量对待检测水下机器人进行初次平衡检测。本实施例根据重心坐标和浮心坐标确定待检测水下机器人的中心坐标，能够同时考虑重力和浮力的影响，得到更加精确的中心坐标，再根据中心坐标和连杆角度偏移的变化量对待检测水下机器人进行初次平衡检测，能够根据中心坐标和中心坐标所处的连杆的角度偏移的变化量共同进行初次平衡检测，从而能够有效地对待检测水下机器人进行初次平衡检测。
参考图5，图5为本发明水下机器人平衡检测方法第三实施例的流程示意图。
基于上述各实施例，在本实施例中，所述步骤S40包括：
步骤S401：确定所述俯仰角中的正俯仰角和负俯仰角，确定所述偏航角中的正偏航角和负偏航角，确定所述滚转角中的正滚转角和负滚转角；
可理解的是，本实施例可当X轴的正半轴位于过坐标原点的水平面之上（抬头）时，设置俯仰角为正，否则为负；在待检测水下机器人的机体向右偏航时，设置偏航角为正，反之为负；在待检测水下机器人的机体向右滚时，设置滚转角为正，反之为负。因此，可确定俯仰角中的正俯仰角和负俯仰角，确定偏航角中的正偏航角和负偏航角，确定滚转角中的正滚转角和负滚转角。
步骤S402：根据所述正俯仰角和所述负俯仰角确定俯仰角分布情况，根据所述正偏航角和所述负偏航角确定偏航角分布情况，根据所述正滚转角和所述负滚转角确定滚转角分布情况；
应理解的是，可根据正俯仰角和负俯仰角确定俯仰角分布情况，即确定俯仰角的正值的数量和分布情况，以及负值的数量和分布情况；还可根据正偏航角和负偏航角确定偏航角分布情况，即确定偏航角的正值的数量和分布情况，以及负值的数量和分布情况；还可根据正滚转角和负滚转角确定滚转角分布情况，即确定滚转角的正值的数量和分布情况，以及负值的数量和分布情况。
步骤S403：根据所述俯仰角分布情况、所述偏航角分布情况以及所述滚转角分布情况对所述待检测水下机器人进行平衡检测。
可理解的是，可根据俯仰角分布情况、偏航角分布情况以及滚转角分布情况对待检测水下机器人进行平衡检测，具体平衡检测方法可以是在俯仰角分布情况、偏航角分布情况以及滚转角分布情况均分布均匀时，可认定待检测水下机器人处于平衡状态。
进一步地，为了有效地进行平衡检测，在本实施例中，所述步骤S403包括：获取所述待检测水下机器人受到的驱动力矩和所述驱动力矩的驱动方向；根据所述驱动方向确定所述待检测水下机器人的预设运动轨迹；根据所述俯仰角分布情况、所述偏航角分布情况以及所述滚转角分布情况确定所述待检测水下机器人在各个轴向的运动轨迹；根据所述各个轴向的运动轨迹和所述预设运动轨迹对所述待检测水下机器人进行平衡检测。
应理解的是，为了使待检测水下机器人能够移动，需要给待检测水下机器人施加多个驱动力矩，本实施例可获取待检测水下机器人受到的所有驱动力矩和各驱动力矩对应的驱动方向。然后根据驱动方向可确定待检测水下机器人的预设运动轨迹，驱动方向的变化会导致运动轨迹的变化，驱动方向可以是在认定待检测水下机器人在平衡状态下时，对待检测水下机器人施加的驱动方向，因此得到的预设运动轨迹也是在认定待检测水下机器人在平衡状态下时的运动轨迹。
可理解的是，可根据俯仰角分布情况、偏航角分布情况以及滚转角分布情况确定待检测水下机器人在各个轴向的运动轨迹，由于俯仰角与X轴有关，因此可根据俯仰角变化情况确定待检测水下机器人在X轴向的运动轨迹，具体可根据俯仰角的正负和具体值确定待检测水下机器人在X轴向的运动轨迹，还可根据偏航角变化情况确定待检测水下机器人在Y轴向的运动轨迹，还可根据滚转角变化情况确定待检测水下机器人在Z轴向的运动轨迹。
在具体实现中，可根据各个轴向的运动轨迹形成待检测水下机器人的实际运动轨迹，然后将实际运动轨迹与预设运动轨迹进行对比，如果对比结果基本一致，则说明待检测水下机器人平衡检测通过，即待检测水下机器人处于平衡状态。
本实施例通过确定俯仰角中的正俯仰角和负俯仰角，确定偏航角中的正偏航角和负偏航角，确定滚转角中的正滚转角和负滚转角，然后根据正俯仰角和负俯仰角确定俯仰角分布情况，根据正偏航角和负偏航角确定偏航角分布情况，根据正滚转角和负滚转角确定滚转角分布情况，再根据俯仰角分布情况、偏航角分布情况以及滚转角分布情况对待检测水下机器人进行平衡检测。本实施例根据俯仰角分布情况、偏航角分布情况以及滚转角分布情况对待检测水下机器人进行平衡检测，能够考虑到俯仰角、偏航角以及滚转角带来的影响，对待检测水下机器人有效地进行平衡检测。
参照图6，图6为本发明水下机器人平衡检测的控制装置第一实施例的结构框图。
如图6所示，本发明实施例提出的水下机器人平衡检测的控制装置包括：
坐标获取模块10，用于确定待检测水下机器人中处于同一关节位置的连杆，并获取所述连杆的连杆坐标；
平衡检测模块20，用于根据所述连杆坐标对所述待检测水下机器人进行初次平衡检测；
姿态角获取模块30，用于在初次平衡检测通过时，获取所述待检测水下机器人在当前时刻下的姿态角；
所述平衡检测模块20，还用于根据所述姿态角对所述待检测水下机器人进行平衡检测。
本实施例通过确定待检测水下机器人中处于同一关节位置的连杆，并获取连杆的连杆坐标，然后根据连杆坐标对待检测水下机器人进行初次平衡检测，在初次平衡检测通过时，获取待检测水下机器人在当前时刻下的姿态角，再根据姿态角对待检测水下机器人进行平衡检测。本实施例先根据连杆坐标对待检测水下机器人进行初次平衡检测，能够粗略地判断待检测水下机器人是否处于平衡状态，在初次平衡检测通过时，根据姿态角对待检测水下机器人再次进行平衡检测，本实施例通过对待检测水下机器人进行两次平衡检测，能够有效地对待检测水下机器人进行平衡检测，从而能够得到更加精确的平衡检测结果。
需要说明的是，以上所描述的工作流程仅仅是示意性的，并不对本发明的保护范围构成限定，在实际应用中，本领域的技术人员可以根据实际的需要选择其中的部分或者全部来实现本实施例方案的目的，此处不做限制。
另外，未在本实施例中详尽描述的技术细节，可参见本发明任意实施例所提供的水下机器人平衡检测方法，此处不再赘述。
基于本发明上述水下机器人平衡检测的控制装置第一实施例，提出本发明水下机器人平衡检测装置的第二实施例。
在本实施例中，所述平衡检测模块20，还用于获取所述待检测水下机器人的重心坐标和浮心坐标；根据所述重心坐标和所述浮心坐标确定所述待检测水下机器人的中心坐标；获取所述连杆坐标对应的连杆在所述待检测水下机器人移动时产生的连杆角度偏移的变化量；根据所述中心坐标和所述连杆角度偏移的变化量对所述待检测水下机器人进行初次平衡检测。
进一步地，所述平衡检测模块20，还用于获取所述连杆坐标对应的连杆在预设时间段内的初始连杆角度偏移；从所述初始连杆角度偏移中选取处于同一关节位置的目标连杆角度偏移；对所述目标连杆角度偏移进行归一化处理，获得归一化的连杆角度偏移；根据所述归一化的连杆角度偏移确定连杆角度偏移的变化量。
进一步地，所述平衡检测模块20，还用于确定所述中心坐标处于所述待检测水下机器人的目标关节；从所述连杆角度偏移的变化量中选取所述目标关节对应的连杆的目标变化量和剩余连杆的剩余变化量；从所述目标变化量中选取处于所述待检测水下机器人的同一关节位置的第一变化量，并从所述剩余变化量中选取处于所述待检测水下机器人的同一关节位置的第二变化量；在所述第一变化量满足第一预设条件且所述第二变化量满足第二预设条件时，判定所述待检测水下机器人初次平衡检测通过。
进一步地，所述姿态角包括：俯仰角、偏航角以及滚转角；所述姿态角获取模块30，还用于在初次平衡检测通过时，获取所述待检测水下机器人在前一采样时刻的初始偏航角；通过陀螺仪采集所述待检测水下机器人在前一采样时刻的角速度，并获取所述陀螺仪在当前时刻下的测量误差；根据所述初始偏航角、所述角速度和所述测量误差确定所述偏航角；获取所述待检测水下机器人在前一采样时刻的初始俯仰角和初始滚转角；通过加速度计采集所述待检测水下机器人在前一采样时刻的加速度；根据所述初始俯仰角和所述加速度确定所述俯仰角，并根据所述初始滚转角和所述加速度确定所述滚转角。
进一步地，所述平衡检测模块20，还用于确定所述俯仰角中的正俯仰角和负俯仰角，确定所述偏航角中的正偏航角和负偏航角，确定所述滚转角中的正滚转角和负滚转角；根据所述正俯仰角和所述负俯仰角确定俯仰角分布情况，根据所述正偏航角和所述负偏航角确定偏航角分布情况，根据所述正滚转角和所述负滚转角确定滚转角分布情况；根据所述俯仰角分布情况、所述偏航角分布情况以及所述滚转角分布情况对所述待检测水下机器人进行平衡检测。
进一步地，所述平衡检测模块20，还用于获取所述待检测水下机器人受到的驱动力矩和所述驱动力矩的驱动方向；根据所述驱动方向确定所述待检测水下机器人的预设运动轨迹；根据所述俯仰角分布情况、所述偏航角分布情况以及所述滚转角分布情况确定所述待检测水下机器人在各个轴向的运动轨迹；根据所述各个轴向的运动轨迹和所述预设运动轨迹对所述待检测水下机器人进行平衡检测。
本发明水下机器人平衡检测的控制装置的其他实施例或具体实现方式可参照上述各方法实施例，此处不再赘述。
需要说明的是，在本文中，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者系统不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者系统所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括该要素的过程、方法、物品或者系统中还存在另外的相同要素。
上述本发明实施例序号仅仅为了描述，不代表实施例的优劣。
通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到上述实施例方法可借助软件加必需的通用硬件平台的方式来实现，当然也可以通 过硬件，但很多情况下前者是更佳的实施方式。基于这样的理解，本发明的 技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质（如只读存储器/随机存取存储器、磁碟、光盘）中，包括若干指令用以使得一台终端设备(可以是手机，计算机，服务器，或者网络设备等)执行本发明各个实施例所述的方法。
以上仅为本发明的优选实施例，并非因此限制本发明的专利范围，凡是利用本发明说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本发明的专利保护范围内。
Claims (7)
1.一种水下机器人平衡检测方法，其特征在于，所述水下机器人平衡检测方法包括以下步骤：
确定待检测水下机器人中处于同一关节位置的连杆，并获取所述连杆的连杆坐标；
根据所述连杆坐标对所述待检测水下机器人进行初次平衡检测；
在初次平衡检测通过时，获取所述待检测水下机器人在当前时刻下的姿态角，所述姿态角包括：俯仰角、偏航角以及滚转角；
根据所述姿态角对所述待检测水下机器人进行平衡检测；
所述根据所述连杆坐标对所述待检测水下机器人进行初次平衡检测的步骤，具体包括：
获取所述待检测水下机器人的重心坐标和浮心坐标；
根据所述重心坐标和所述浮心坐标确定所述待检测水下机器人的中心坐标；
获取所述连杆坐标对应的连杆在所述待检测水下机器人移动时产生的连杆角度偏移的变化量；
根据所述中心坐标和所述连杆角度偏移的变化量对所述待检测水下机器人进行初次平衡检测；
所述根据所述中心坐标和所述连杆角度偏移的变化量对所述待检测水下机器人进行初次平衡检测的步骤，具体包括：
确定所述中心坐标处于所述待检测水下机器人的目标关节；
从所述连杆角度偏移的变化量中选取所述目标关节对应的连杆的目标变化量和剩余连杆的剩余变化量；
从所述目标变化量中选取处于所述待检测水下机器人的同一关节位置的第一变化量，并从所述剩余变化量中选取处于所述待检测水下机器人的同一关节位置的第二变化量；
在所述第一变化量满足第一预设条件且所述第二变化量满足第二预设条件时，判定所述待检测水下机器人初次平衡检测通过。
2.如权利要求1所述的水下机器人平衡检测方法，其特征在于，所述获取所述连杆坐标对应的连杆在所述待检测水下机器人移动时产生的连杆角度偏移的变化量的步骤，具体包括：
获取所述连杆坐标对应的连杆在预设时间段内的初始连杆角度偏移；
从所述初始连杆角度偏移中选取处于同一关节位置的目标连杆角度偏移；
对所述目标连杆角度偏移进行归一化处理，获得归一化的连杆角度偏移；
根据所述归一化的连杆角度偏移确定连杆角度偏移的变化量。
3.如权利要求1所述的水下机器人平衡检测方法，其特征在于，所述在初次平衡检测通过时，获取所述待检测水下机器人在当前时刻下的姿态角的步骤，具体包括：
在初次平衡检测通过时，获取所述待检测水下机器人在前一采样时刻的初始偏航角；
通过陀螺仪采集所述待检测水下机器人在前一采样时刻的角速度，并获取所述陀螺仪在当前时刻下的测量误差；
根据所述初始偏航角、所述角速度和所述测量误差确定所述偏航角；
获取所述待检测水下机器人在前一采样时刻的初始俯仰角和初始滚转角；
通过加速度计采集所述待检测水下机器人在前一采样时刻的加速度；
根据所述初始俯仰角和所述加速度确定所述俯仰角，并根据所述初始滚转角和所述加速度确定所述滚转角。
4.如权利要求3所述的水下机器人平衡检测方法，其特征在于，所述根据所述姿态角对所述待检测水下机器人进行平衡检测的步骤，具体包括：
确定所述俯仰角中的正俯仰角和负俯仰角，确定所述偏航角中的正偏航角和负偏航角，确定所述滚转角中的正滚转角和负滚转角；
根据所述正俯仰角和所述负俯仰角确定俯仰角分布情况，根据所述正偏航角和所述负偏航角确定偏航角分布情况，根据所述正滚转角和所述负滚转角确定滚转角分布情况；
根据所述俯仰角分布情况、所述偏航角分布情况以及所述滚转角分布情况对所述待检测水下机器人进行平衡检测。
5.如权利要求4所述的水下机器人平衡检测方法，其特征在于，所述根据所述俯仰角分布情况、所述偏航角分布情况以及所述滚转角分布情况对所述待检测水下机器人进行平衡检测的步骤，具体包括：
获取所述待检测水下机器人受到的驱动力矩和所述驱动力矩的驱动方向；
根据所述驱动方向确定所述待检测水下机器人的预设运动轨迹；
根据所述俯仰角分布情况、所述偏航角分布情况以及所述滚转角分布情况确定所述待检测水下机器人在各个轴向的运动轨迹；
根据所述各个轴向的运动轨迹和所述预设运动轨迹对所述待检测水下机器人进行平衡检测。
6.一种水下机器人平衡检测的控制装置，其特征在于，所述水下机器人平衡检测装置包括：
坐标获取模块，用于确定待检测水下机器人中处于同一关节位置的连杆，并获取所述连杆的连杆坐标；
平衡检测模块，用于根据所述连杆坐标对所述待检测水下机器人进行初次平衡检测；
姿态角获取模块，用于在初次平衡检测通过时，获取所述待检测水下机器人在当前时刻下的姿态角；
所述平衡检测模块，还用于根据所述姿态角对所述待检测水下机器人进行平衡检测；
所述平衡检测模块，还用于获取所述待检测水下机器人的重心坐标和浮心坐标；根据所述重心坐标和所述浮心坐标确定所述待检测水下机器人的中心坐标；获取所述连杆坐标对应的连杆在所述待检测水下机器人移动时产生的连杆角度偏移的变化量；根据所述中心坐标和所述连杆角度偏移的变化量对所述待检测水下机器人进行初次平衡检测；
所述平衡检测模块，还用于确定所述中心坐标处于所述待检测水下机器人的目标关节；从所述连杆角度偏移的变化量中选取所述目标关节对应的连杆的目标变化量和剩余连杆的剩余变化量；从所述目标变化量中选取处于所述待检测水下机器人的同一关节位置的第一变化量，并从所述剩余变化量中选取处于所述待检测水下机器人的同一关节位置的第二变化量；在所述第一变化量满足第一预设条件且所述第二变化量满足第二预设条件时，判定所述待检测水下机器人初次平衡检测通过。
7.一种水下机器人平衡检测设备，其特征在于，所述设备包括：存储器、处理器及存储在所述存储器上并可在所述处理器上运行的水下机器人平衡检测程序，所述水下机器人平衡检测程序配置为实现如权利要求1至5中任一项所述的水下机器人平衡检测方法的步骤。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310622833.0A CN116337331B (zh) | 2023-05-30 | 2023-05-30 | 水下机器人平衡检测方法、控制装置及设备 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310622833.0A CN116337331B (zh) | 2023-05-30 | 2023-05-30 | 水下机器人平衡检测方法、控制装置及设备 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN116337331A CN116337331A (zh) | 2023-06-27 |
| CN116337331B true CN116337331B (zh) | 2023-09-05 |
Family
ID=86880813
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310622833.0A Active CN116337331B (zh) | 2023-05-30 | 2023-05-30 | 水下机器人平衡检测方法、控制装置及设备 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116337331B (zh) |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104516353A (zh) * | 2013-09-28 | 2015-04-15 | 南京专创知识产权服务有限公司 | 一种分布式水下仿生机器人姿态稳定系统 |
| KR20160034493A (ko) * | 2014-09-19 | 2016-03-30 | 삼성중공업 주식회사 | 자세보정방법 및 이를 수행하는 수중청소로봇 |
| CN106527453A (zh) * | 2016-12-06 | 2017-03-22 | 北京臻迪机器人有限公司 | 一种水下无人船定深航行控制系统及方法 |
| CN107065898A (zh) * | 2016-12-06 | 2017-08-18 | 北京臻迪科技股份有限公司 | 一种水下无人船航行控制方法及系统 |
| CN110816792A (zh) * | 2019-11-26 | 2020-02-21 | 南方电网调峰调频发电有限公司 | 一种水下机器人的稳定性调节系统及方法 |
-
2023
- 2023-05-30 CN CN202310622833.0A patent/CN116337331B/zh active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104516353A (zh) * | 2013-09-28 | 2015-04-15 | 南京专创知识产权服务有限公司 | 一种分布式水下仿生机器人姿态稳定系统 |
| KR20160034493A (ko) * | 2014-09-19 | 2016-03-30 | 삼성중공업 주식회사 | 자세보정방법 및 이를 수행하는 수중청소로봇 |
| CN106527453A (zh) * | 2016-12-06 | 2017-03-22 | 北京臻迪机器人有限公司 | 一种水下无人船定深航行控制系统及方法 |
| CN107065898A (zh) * | 2016-12-06 | 2017-08-18 | 北京臻迪科技股份有限公司 | 一种水下无人船航行控制方法及系统 |
| CN110816792A (zh) * | 2019-11-26 | 2020-02-21 | 南方电网调峰调频发电有限公司 | 一种水下机器人的稳定性调节系统及方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN116337331A (zh) | 2023-06-27 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US8200452B2 (en) | Attitude-angle detecting apparatus and attitude-angle detecting method | |
| Sheng et al. | MEMS-based low-cost strap-down AHRS research | |
| CN107433590A (zh) | 机械臂负载质量和传感器零漂在线辨识的重力补偿方法 | |
| CN108731664B (zh) | 机器人状态估计方法、装置、计算机设备和存储介质 | |
| US8897919B2 (en) | Robot controller which conducts a force control by using a three-axial force sensor | |
| CN106767673B (zh) | 一种卫星高精度光学敏感载荷的指向测量方法 | |
| Li et al. | A dynamic pre-travel error prediction model for the kinematic touch trigger probe | |
| JP2012040634A (ja) | 力制御ロボットのキャリブレーション装置と方法 | |
| CN113510698B (zh) | 一种机械臂控制方法、装置、机械臂和可读存储介质 | |
| CN113031639A (zh) | 机器人步长处理方法、装置、机器人控制设备及存储介质 | |
| CN109866217B (zh) | 机器人里程定位方法、装置、终端设备及计算机存储介质 | |
| CN111504314A (zh) | Imu与刚体的位姿融合方法、装置、设备及存储介质 | |
| CN113654747B (zh) | 机器人的关节刚度的检测方法、装置和机器人 | |
| CN116337331B (zh) | 水下机器人平衡检测方法、控制装置及设备 | |
| JP2009186244A (ja) | 傾斜角度推定システム、相対角度推定システム及び角速度推定システム | |
| CN112720450B (zh) | 机器人关节角度检验方法、装置、设备及介质 | |
| US20240134033A1 (en) | Method for determining a movement state of a rigid body | |
| CN106931992B (zh) | 用于检测物体翻滚的方法和装置 | |
| CN114137840A (zh) | 足式机器人全局平衡控制方法、装置和足式机器人 | |
| JP5424224B2 (ja) | 相対角度推定システム | |
| KR100512963B1 (ko) | 관성항법시스템을 이용한 펜형 공간 입력시스템 및 그궤적 복원 방법 | |
| CN112346468A (zh) | 一种自动引导运输车运动监控方法、系统及电子设备 | |
| JP5424226B2 (ja) | 傾斜角度推定システム | |
| JP5958920B2 (ja) | 傾斜角度推定システム | |
| CN113771044B (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 |