CN113448338B - 机器人控制方法、机器人、计算机程序产品和存储介质 - Google Patents

机器人控制方法、机器人、计算机程序产品和存储介质 Download PDF

Info

Publication number
CN113448338B
CN113448338B CN202110575994.XA CN202110575994A CN113448338B CN 113448338 B CN113448338 B CN 113448338B CN 202110575994 A CN202110575994 A CN 202110575994A CN 113448338 B CN113448338 B CN 113448338B
Authority
CN
China
Prior art keywords
virtual
motion
robot
underwater robot
formation
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
CN202110575994.XA
Other languages
English (en)
Other versions
CN113448338A (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.)
Peng Cheng Laboratory
Original Assignee
Peng Cheng Laboratory
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 Peng Cheng Laboratory filed Critical Peng Cheng Laboratory
Priority to CN202110575994.XA priority Critical patent/CN113448338B/zh
Publication of CN113448338A publication Critical patent/CN113448338A/zh
Application granted granted Critical
Publication of CN113448338B publication Critical patent/CN113448338B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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

机器人控制方法、机器人、计算机程序产品和存储介质
技术领域
本发明涉及机器人控制技术领域，尤其涉及一种机器人控制方法、机器人、计算机程序产品和存储介质。
背景技术
近年来随着国家领土主权意识、海洋监测与保护意识的增强，对水下非合作目标的定位与追踪需求越来越大，利用携带声纳等传感设备的多水下机器人对非合作目标进行全方位观测与追踪，在维护国家海洋权益、观测海洋环境等方面具有重要意义。随着传感器技术、水下通信技术以及现代制造技术的发展，水下机器人正朝着小型化、智能化、集群化方向发展，小型化、低成本的AUV(水下机器人)机动能力与感知能力不断得到提升，利用编队的多个AUV完成对水下目标的定位与追踪成为现实。
然而上述编队追踪方案存在的技术问题是：在追踪过程中，当非合作目标依靠外部环境躲避追踪，水下机器人的队形无法自适应变换。
发明内容
本发明主要目的在于提供一种机器人控制方法、机器人、计算机程序产品和存储介质，旨在解决现有技术中当非合作目标依靠外部环境躲避追踪，水下机器人的队形无法自适应变换的问题。
为实现上述目的，本发明提供一种机器人控制，在一实施例中，所述机器人控制包括以下步骤：
获取每个水下机器人的运动环境参数、每个水下机器人所在编队的机器人数量以及非合作目标的运动信息，所述运动环境参数包括运动轨道的半径；
根据所述运动环境参数、所述所在编队的机器人数量、所述非合作目标的运动信息以及预设的路径预测模型确定所述水下机器人的运动轨迹；
控制所述水下机器人根据所述运动轨迹移动。
在一实施例中，所述获取每个水下机器人的运动环境参数、每个水下机器人所在编队的机器人数量以及非合作目标的运动信息的步骤之前，还包括：
获取预先设置的每个虚拟领航者的路径参数与非合作目标的运动信息，所述路径参数包括每个虚拟领航者运动轨道的半径、虚拟领航者的标识信息以及虚拟领航者所在编队的机器人数量，所述虚拟领航者与所述水下机器人对应；
根据每个所述路径参数得到每个虚拟领航者的当前位置；
根据每个虚拟领航者的当前位置以及所述非合作目标的运动信息确定所述每个虚拟领航者的当前位置，其中，所述非合作目标的运动信息包括：非合作目标的运动速度及角速度；
根据所述每个虚拟领航者的当前位置生成预设的路径预测模型。
在一实施例中，所述根据每个所述路径参数得到每个虚拟领航者在目标坐标系下的位置的步骤包括：
根据第一计算公式得到每个虚拟领航者在目标坐标系下的位置，所述第一计算公式为：
其中，i为第i个虚拟领航者的标识信息，Rd为第i个虚拟领航者的运动轨道的半径，γi为引导变量，φi为第i个虚拟领航者所在编队的编队相位且φi＝(i-1)·2·π/NZ，NZ为第i个虚拟领航者所在编队的机器人数量。
在一实施例中，所述根据每个虚拟领航者在目标坐标系下的位置以及所述非合作目标的运动信息确定所述每个虚拟领航者的当前位置的步骤，包括：
根据第二计算公式确定所述每个虚拟领航者的当前位置，所述第二计算公式为：
其中，pd,i(γi,t)为第i个水下机器人的虚拟领航者在地球坐标系下的期望路径，为非合作目标的当前位置，Rt为非合作目标的地球坐标系和艇体坐标系间坐标转换矩阵，/>为二阶反对称矩阵，/>为非合作目标的角速度，/>符号表示相对于γi的微分，/>为非合作目标的速度。
在一实施例中，所述确定所述每个虚拟领航者的当前位置之后，还包括：
构建每个虚拟领航者之间的通信拓扑关系；
基于所述通信拓扑关系生成所述每个虚拟领航者的编队协调控制信号；
根据所述编队协调控制信号控制所述每个虚拟领航者移动。
在一实施例中，所述基于所述通信拓扑关系生成所述每个虚拟领航者的编队协调控制信号，包括：
获取并保存边的连接关系的邻接矩阵A＝[aij]，其中，aij为相邻两个虚拟领航者之间的连接关系；
基于所述邻接矩阵获取度量矩阵，所述度量矩阵为D＝diag(|N1|,|N2|,...,|NN|)，其中，为第i个虚拟领航者所在编队的机器人数量；
根据所述度量矩阵并结合增益矩阵Kc＝diag(kc,1,kc,2,...,kc,N)生成虚拟领航者的编队协调控制信号。
在一实施例中，所述控制所述水下机器人根据所述运动轨迹移动，包括：
获取水下机器人与虚拟领航者的路径跟踪误差；
采用一阶滑模控制方法对所述跟踪误差处理以得到水下机器人的控制律；
根据所述控制律控制所述水下机器人根据所述运动轨迹移动。
为实现上述目的，本发明还提供了一种计算机程序产品，所述计算机程序产品包括机器人控制程序，所述机器人控制程序被所述处理器执行时实现上述的机器人控制方法的步骤。
为实现上述目的，本发明还提供一种水下机器人，所述水下机器人包括存储器、处理器以及存储在所述存储器并可在所述处理器上运行的机器人控制程序，所述机器人控制程序被所述处理器执行时实现如上所述的机器人控制的各个步骤。
为实现上述目的，本发明还提供一种存储介质，所述存储介质存储有机器人控制程序，所述机器人控制程序被处理器执行时实现如上所述的机器人控制的各个步骤。
本发明提供的机器人控制方法及装置、水下机器人和存储介质，至少具有以下技术效果：
由于采用了获取每个水下机器人的运动环境参数、每个水下机器人所在编队的机器人数量以及非合作目标的运动信息，所述运动环境参数包括运动轨道的半径；根据所述运动环境参数、所述所在编队的机器人数量、所述非合作目标的运动信息以及预设的路径预测模型进行编队确定所述水下机器人的运动轨迹，控制所述水下机器人根据所述运动轨迹进行移动，解决现有技术中当非合作目标依靠外部环境躲避追踪，水下机器人的队形无法自适应变换的问题，提高水下机器人编队追踪过程的自适应能力。
附图说明
图1为本发明实施例涉及的水下机器人架构示意图；
图2为本发明机器人控制的第一实施例的流程示意图；
图3为本发明机器人控制的第二实施例的流程示意图；
图4为本发明机器人控制的第三实施例的流程示意图；
图5为本发明机器人控制的第四实施例中步骤S320的细化流程示意图；
图6为本发明机器人控制的第五实施例中步骤S130的细化流程示意图；
本发明目的的实现、功能特点及优点将结合实施例，参照附图做进一步说明。
具体实施方式
应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。
本申请为解决现有技术中当非合作目标依靠外部环境躲避追踪，水下机器人的队形无法自适应变换的问题，采用了获取每个水下机器人的运动环境参数、每个水下机器人所在编队的机器人数量以及非合作目标的运动信息，所述运动环境参数包括运动轨道的半径；根据所述运动环境参数、所述所在编队的机器人数量、所述非合作目标的运动信息以及预设的路径预测模型确定所述水下机器人的运动轨迹；控制所述水下机器人根据所述运动轨迹移动，提高水下机器人编队追踪过程的自适应能力。
为了更好地理解上述技术方案，下面将参照附图更详细地描述本申请的示例性实施例。虽然附图中显示了本申请的示例性实施例，然而应当理解，可以以各种形式实现本申请而不应被这里阐述的实施例所限制。相反，提供这些实施例是为了能够更透彻地理解本申请，并且能够将本申请的范围完整的传达给本领域的技术人员。
如图1所示，图1为本发明实施例方案涉及的硬件运行环境的结构示意图。
需要说明的是，图1即可为水下机器人的硬件运行环境的架构示意图。
如图1所示，该水下机器人可以包括：处理器1001，例如CPU，存储器1005，用户接口1003，网络接口1004，通信总线1002。其中，通信总线1002用于实现这些组件之间的连接通信。所述用户接口1003可以包括标准的有线接口、无线接口。网络接口1004可选的可以包括标准的有线接口、无线接口例如WI-FI接口、无线电接口。存储器1005可以是高速RAM存储器，也可以是稳定的存储器(non-volatile memory)，例如磁盘存储器。存储器1005可选的还可以是独立于前述处理器1001的存储装置。
可选地，水下机器人还可以包括摄像头、传感器、无线传输模块等等，其中，所述传感器包括运动传感器或者其他传感器；所述无线传输模块主要用于数据通信；当然，移动终端还可配置陀螺仪、红外线传感器等其他传感器，在此不再赘述。
本领域技术人员可以理解，图1所示的水下机器人结构并不构成对水下机器人的限定，水下机器人可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件布置。
如图1所示，作为一种存储介质的存储器1005中可以包括操作***、网络通信模块、用户接口模块以及机器人控制程序。其中，操作***是管理和控制水下机器人硬件和软件资源的程序，机器人控制程序以及其它软件或程序的运行。
在图1所示的水下机器人中，用户接口1003主要用于连接终端，与终端进行数据通信；网络接口1004主要用于进行数据传输；处理器1001可以用于调用存储器1005中存储的机器人控制程序。
在本实施例中，水下机器人包括：存储器1005、处理器1001及存储在所述存储器上并可在所述处理器上运行的机器人控制程序，其中：
在本实施例中，处理器1001可以用于调用存储在存储器1005中的机器人控制程序，并执行以下操作：
获取每个水下机器人的运动环境参数、每个水下机器人所在编队的机器人数量以及非合作目标的运动信息，所述运动环境参数包括运动轨道的半径；
根据所述运动环境参数、所述所在编队的机器人数量、所述非合作目标的运动信息以及预设的路径预测模型确定所述水下机器人的运动轨迹；
控制所述水下机器人根据所述运动轨迹移动。
在本实施例中，处理器1001可以用于调用存储在存储器1005中的机器人控制程序，并执行以下操作：
获取预先设置的每个虚拟领航者的路径参数与非合作目标的运动信息，所述路径参数包括每个虚拟领航者运动轨道的半径、虚拟领航者的标识信息以及虚拟领航者所在编队的机器人数量，所述虚拟领航者与所述水下机器人对应；
根据每个所述路径参数得到每个虚拟领航者在目标坐标系下的位置；
根据每个虚拟领航者在目标坐标系下的位置以及所述非合作目标的运动信息确定所述每个虚拟领航者的当前位置，其中，所述非合作目标的运动信息包括：非合作目标的运动速度及角速度；
根据所述每个虚拟领航者的当前位置生成预设的路径预测模型。
在本实施例中，处理器1001可以用于调用存储在存储器1005中的机器人控制程序，并执行以下操作：
构建每个虚拟领航者之间的通信拓扑关系；
基于所述通信拓扑关系生成所述每个虚拟领航者的编队协调控制信号；
根据所述编队协调控制信号控制所述每个虚拟领航者移动。
在本实施例中，处理器1001可以用于调用存储在存储器1005中的机器人控制程序，并执行以下操作：
获取并保存边的连接关系的邻接矩阵A＝[aij]，其中，aij为相邻两个虚拟领航者之间的连接关系；
基于所述邻接矩阵获取度量矩阵，所述度量矩阵为D＝diag(|N1|,|N2|,...,|NN|)，其中，为第i个虚拟领航者所在编队的机器人数量；
根据所述度量矩阵并结合增益矩阵Kc＝diag(kc,1,kc,2,...,kc,N)生成虚拟领航者的编队协调控制信号。
在本实施例中，处理器1001可以用于调用存储在存储器1005中的机器人控制程序，并执行以下操作：
获取水下机器人与虚拟领航者的路径跟踪误差；
采用一阶滑模控制方法对所述跟踪误差处理以得到水下机器人的控制律；
根据所述控制律控制所述水下机器人根据所述运动轨迹移动。
由于本申请实施例提供的水下机器人，为实施本申请实施例的方法所采用的水下机器人，故而基于本申请实施例所介绍的方法，本领域所属人员能够了解该水下机器人的具体结构及变形，故而在此不再赘述。凡是本申请实施例的方法所采用的水下机器人都属于本申请所欲保护的范围。上述本发明实施例序号仅仅为了描述，不代表实施例的优劣。
对于软件实现，可通过执行本发明实施例所述功能的模块(例如过程、函数等)来实现本发明实施例所述的技术。软件代码可存储在存储器中并通过处理器执行。存储器可以在处理器中或在处理器外部实现。
基于上述结构，提出本发明的实施例。
参照图2，图2为本发明机器人控制的第一实施例的流程示意图，包括以下步骤：
步骤S110，获取每个水下机器人的运动环境参数、每个水下机器人所在编队的机器人数量以及非合作目标的运动信息。
在本实施例中，所述运动环境参数包括水下机器人所在运动轨道的半径，在运动环境参数即运动轨道半径发生变化时，水下机器人编队缩小覆盖区域通过轨道狭窄区域，在运动轨道变宽时，水下机器人编队扩大覆盖区域保持更好的追踪效果，在实际水下运动情况下，所述水下机器人运动环境参数的获取可以借助一些距离测量传感器，例如激光或者声呐等，将这些距离测量传感器搭载于水机器人上，根据发送信号与接收信号的时间实现运动轨道半径的测量，在仿真过程中，所述水下机器人的运动环境参数可以根据实际情况进行设定；当运动环境发生变化时，需要动态的改变并获取水下机器所在编队的机器人数量，且每个机器人都有对应的机器人标识信息；所述非合作目标为水下机器人需要跟踪的对象，所述非合作目标为水下非合作目标，所述非合作目标是一种需要被探测的目标，所述非合作目标包括水下不明潜器、失效潜器、海洋动物以及由于石油泄漏、海底火山喷发导致的羽流等；所述非合作目标的运动参数包括非合作目标的速度或角速度。
步骤S120，根据所述运动环境参数、所述所在编队的机器人数量、所述非合作目标的运动信息以及预设的路径预测模型确定所述水下机器人的运动轨迹。
在本实施例中，所述编队的机器人数量根据所述运动环境参数的变化会实时发生变化，根据所述运动环境参数、所述水下机器人所在编队的机器人数量，采用顶层决策规划器生成对应的路径参数，将所述路径参数以及非合作目标的运动角度、速度、位置作为输入数据输入预设的路径预测模型中，从而获取匹配的水下机器人对应的运动轨迹。
步骤S130，控制所述水下机器人根据所述运动轨迹移动。
在本实施例中，所述虚拟领航者与所述水下机器人一一对应，即虚拟领航者与水下机器人具有映射关系，在得知虚拟领航者的运动参数、所在编队的机器人数量、所述非合作目标的运动信息之后，将这些参数输入路径预测模型中，根据所述映射关系即可得到水下机器人的实时变化的运动轨迹，底层运动控制器控制各水下机器人根据该运动轨迹进行相应的移动。
由于采用了在得知虚拟领航者的运动参数、所在编队的机器人数量、所述非合作目标的运动信息之后，将这些参数输入路径预测模型中，根据虚拟领航者与水下机器人的映射关系即可得到水下机器人的实时变化的运动轨迹，底层运动控制器控制各水下机器人根据该运动轨迹进行相应的移动的技术方案，解决现有技术中当非合作目标依靠外部环境躲避追踪，水下机器人的队形无法自适应变换的问题，提高水下机器人编队追踪过程的自适应能力。
参照图3，图3为本发明机器人控制的第二实施例的流程示意图，在本实施例中，第一实施例中步骤S110之前包括：
步骤S210，获取预先设置的每个虚拟领航者的路径参数与非合作目标的运动信息。
在本实施例中，所述路径参数是由顶层决策规划器根据运动环境参数以及任务需求生成的，所述路径参数包括每个虚拟领航者运动轨道的半径、虚拟领航者的标识信息以及虚拟领航者所在编队的机器人数量，所述虚拟领航者的标识信息为虚拟领航者的编号，所述虚拟领航者与所述水下机器人对应。
步骤S220，根据每个所述路径参数得到每个虚拟领航者在目标坐标系下的位置。
在本实施例中，根据第一计算公式得到每个虚拟领航者在目标坐标系下的位置，所述第一计算公式为：
其中，i为第i个虚拟领航者标识信息，Rd为第i个虚拟领航者的运动轨道的半径，γi为引导变量，φi为第i个虚拟领航者所在编队的编队相位且φi＝(i-1)·2·π/NZ，NZ为第i个虚拟领航者所在编队的机器人数量。
步骤S230，根据每个虚拟领航者在目标坐标系下的位置以及所述非合作目标的运动信息确定所述每个虚拟领航者的当前位置。
在本实施例中，所述非合作目标的运动信息包括：非合作目标的运动速度及角速度，根据第二计算公式确定所述虚拟领航者的当前位置，所述第二计算公式为：
其中，pd,i(γi,t)为第i个水下机器人的虚拟领航者在地球坐标系下的期望路径，为非合作目标的当前位置，Rt为非合作目标的地球坐标系和艇体坐标系间坐标转换矩阵，/>为二阶反对称矩阵，/>为非合作目标的角速度，/>符号表示相对于γi的微分，/>为非合作目标的速度。
步骤S240，根据所述每个虚拟领航者的当前位置生成预设的路径预测模型。
在本实施例中，所述路径预测模型有每个虚拟领航者的当前位置生成，而所述每个虚拟领航者的当前位置由两部分进行确定，一部分是由顶层决策规划器根据运动环境参数以及任务需求生成的路径参数，根据该路径参数生成虚拟领航者的当前位置；另一部分需结合当前非合作目标的运动信息，根据这两部分数据最终生成虚拟领航者的当前位置。
由于采用了获取预先设置的每个虚拟领航者的路径参数与非合作目标的运动信息，根据每个所述路径参数得到每个虚拟领航者在目标坐标系下的位置，根据每个虚拟领航者在目标坐标系下的位置以及所述非合作目标的运动信息确定所述每个虚拟领航者的当前位置，根据所述每个虚拟领航者的当前位置生成预设的路径预测模型的技术方案，不仅实时改变路径生成器参数，可以根据任务需求快速地改变多机器人编队的覆盖区域，通过狭窄航道或缩小目标距离，还实时改变编队参数，根据任务需求灵活地增减编队中机器人的数目，实现多台水下机器人协作追踪任务中的航行编队自适应动态变化的效果。
参照图4，图4为本发明机器人控制的第三实施例步骤的流程示意图，在本实施例中，第二实施例中步骤S230之后包括：
步骤S310，构建每个虚拟领航者之间的通信拓扑关系。
在本实施例中，所述通信拓扑关系用于建立各虚拟领航者之间的通信关系，将虚拟领航者中的每个虚拟领航者用节点表示，将节点间的通信连接用节点连线构成的边来表示，将虚拟领航者的通信关系用无向代数拓扑图表示，所述无向代数拓扑图包含节点、边和边的连接关系。
步骤S320，基于所述通信拓扑关系生成所述每个虚拟领航者的编队协调控制信号。
在本实施例中，所述编队协调控制信号用于控制虚拟领航者进行有序的运动从而避免发生碰撞，通过计算所述无向代数拓扑图中每个虚拟领航者边的连接关系，根据该边的连接关系确定邻接矩阵，基于所述邻接矩阵计算获取度量矩阵，通过所述度量矩阵以及增益矩阵生成每个虚拟领航者的编队协调控制信号。
步骤S330，根据所述编队协调控制信号控制所述每个虚拟领航者移动。
在本实施例中，在编队过程中，每个虚拟领航者在进行移动的过程中，同时会根据对应生成的编队协调控制信号控制虚拟领航者有序移动，避免相邻虚拟领航者在运动过程中发生碰撞，实现协作追踪任务中的编队自适应动态变化的效果。
由于采用了构建每个虚拟领航者之间的通信拓扑关系，通过计算所述无向代数拓扑图中每个虚拟领航者边的连接关系，并根据边的连接关系邻接矩阵，基于所述邻接矩阵计算获取度量矩阵，通过所述度量矩阵以及增益矩阵生成每个虚拟领航者的编队协调控制信号，根据对应生成的编队协调控制信号控制虚拟领航者有序移动，从而避免发生碰撞。
参照图5，图5为本发明机器人控制的第四实施例中步骤S320的细化流程示意图，在本实施例中，第三实施例中步骤S320包括：
步骤S321，获取并保存边的连接关系的邻接矩阵。
在本实施例中，所述邻接矩阵用于表示定点之间相邻关系的矩阵，假设V和E集合，其中，V是顶点，E是边，用一个一维数组存放图中所有顶点数据，用一个二维数组存放顶点间关系(边或弧)的数据，这个二维数组称为邻接矩阵，本申请采用的邻接矩阵为A＝[aij]，其中，aij为相邻两个虚拟领航者之间的连接关系，1为连接，0为不连接，根据边的连接关系确定邻接矩阵，该邻接矩阵是对称的且主对角线一定为零，因此所述邻接矩阵为无向图邻接矩阵。
步骤S322，基于所述邻接矩阵获取度量矩阵。
在本实施例中，所述度量矩阵为D＝diag(|N1|,|N2|,...,|NN|)，其中，为第i个虚拟领航者所在编队的机器人数量，在无向图中，任一顶点i的度为第i列(或第i行)所有非零元素的个数，在有向图中顶点i的出度为第i行所有非零元素的个数，而入度为第i列所有非零元素的个数，例如在四个水下机器人编队时，
动态地变为三个水下机器人编队时，
动态地变为两个水下机器人编队时，
步骤S323，根据所述度量矩阵并结合增益矩阵生成虚拟领航者的编队协调控制信号。
在本实施例中，所述增益矩阵为对所述各虚拟领航者进行协调控制，所述增益矩阵为Kc＝diag(kc,1,kc,2,...,kc,N)，根据所述度量矩阵与增益矩阵，采用公式υr,i(t)＝[-KC(D-A)γ]i计算各虚拟领航者的编队协调控制信号，其中，为作用在第i个水下机器人的虚拟领航者的编队协调控制信号，根据所述编队协调控制信号控制各虚拟领航者运动，以避免发生碰撞。
由于采用了获取并保存边的连接关系的邻接矩阵，基于所述邻接矩阵获取度量矩阵，根据所述度量矩阵并结合增益矩阵生成虚拟领航者的编队协调控制信号的技术方案，解决现有技术中水下机器人在编队运动过程中碰撞问题，通过接收编队协调控制信号控制各虚拟领航者运动，以避免发生碰撞。
参照图6，图6为本发明机器人控制的第五实施例中步骤S130的细化流程示意图，在本实施例中，第一实施例中步骤S130包括：
步骤S131，获取水下机器人与虚拟领航者的路径跟踪误差。
在本实施例中，根据水下机器人的当前位置与虚拟领航者的当前位置的差获取水下机器人的位置误差，以及水下机器人之间在队形保持时的同步误差，由位置误差和同步误差获得每个水下机器人的路径跟踪误差，定义水下机器人与虚拟领航者的路径跟踪误差为其中/>为第i个水下机器人艇体坐标系中ε＝[ε1,ε2]T的参考点。
步骤S132，采用一阶滑模控制方法对所述跟踪误差处理以得到水下机器人的控制律。
在本实施例中，所述一阶滑模控制是一种非线性控制，可以在动态过程中，根据***当前的状态有目的地不断变化，迫使***按照预定的状态轨迹运动，所述一阶滑模控制是通过设计一种动态非线性滑模面方程实现的，而所述滑膜面由水下机器人的当前位置与虚拟领航者的当前位置的误差，即路径跟踪误差组成；采用一阶滑模控制设计第i个水下机器人的路径跟踪控制律ui＝[vf,i,ωi]T，式中vf,i,ωi分别为该水下机器人的前向速度指令及航速令，控制律由下式给出：
式中ρi为一标量因子，保证该路径跟踪控制器在外干扰及底层运动控制偏差下的鲁棒性，Δ由下式给出，为Δ的伪逆：
步骤S133，根据所述控制律控制所述水下机器人根据所述运动轨迹移动。
在本实施例中，根据得到的控制输入ui，设计底层运动控制器跟踪前向速度指令及航速指令。前向速度指令采用航速反馈的增量式PID控制，如下：
uv＝u0+Δu
ev＝vt-vc
其中，u0为与航速指令配平的控制指令，er为当前航速与指令航速的误差。指令航向速率采用航向速率反馈的PD控制器，如下：
eω＝ωt-ωc
从而实现根据虚拟领航者的前向速度指令及航速指令，控制所述水下机器人根据运动轨迹进行运动。
由于采用了获取水下机器人与虚拟领航者的路径跟踪误差，采用一阶滑模控制方法对所述跟踪误差处理以得到水下机器人的控制律，根据所述控制律控制所述水下机器人根据所述运动轨迹移动的技术方案，实现了通过一阶滑模控制器及底层运动控制器分别实现协同移动路径跟踪及运动指令跟踪，从而完成整个***闭环，获得追踪过程中编队的自适应性。
基于同一发明构思，本申请实施例还提供了一种计算机程序产品，所述计算机程序产品包括机器人控制程序，所述机器人控制程序被处理器执行时实现如上所述的机器人控制的各个步骤，且能达到相同的技术效果，为避免重复，这里不再赘述。
由于本申请实施例提供的计算机程序产品，为实施本申请实施例的方法所采用的计算机程序产品，故而基于本申请实施例所介绍的方法，本领域所属人员能够了解该计算机程序产品的具体结构及变形，故而在此不再赘述。凡是本申请实施例的方法所采用的计算机程序产品都属于本申请所欲保护的范围。
基于同一发明构思，本申请实施例还提供了一种存储介质，所述存储介质存储有机器人控制程序，所述机器人控制程序被处理器执行时实现如上所述的机器人控制的各个步骤，且能达到相同的技术效果，为避免重复，这里不再赘述。
由于本申请实施例提供的存储介质，为实施本申请实施例的方法所采用的存储介质，故而基于本申请实施例所介绍的方法，本领域所属人员能够了解该存储介质的具体结构及变形，故而在此不再赘述。凡是本申请实施例的方法所采用的存储介质都属于本申请所欲保护的范围。
上述本发明实施例序号仅仅为了描述，不代表实施例的优劣。
本领域内的技术人员应明白，本发明的实施例可提供为方法、***、或计算机程序产品。因此，本发明可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本发明可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。
本发明是参照根据本发明实施例的方法、设备(***)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
应当注意的是，在权利要求中，不应将位于括号之间的任何参考符号构造成对权利要求的限制。单词“包含”不排除存在未列在权利要求中的部件或步骤。位于部件之前的单词“一”或“一个”不排除存在多个这样的部件。本发明可以借助于包括有若干不同部件的硬件以及借助于适当编程的计算机来实现。在列举了若干装置的单元权利要求中，这些装置中的若干个可以是通过同一个硬件项来具体体现。单词第一、第二、以及第三等的使用不表示任何顺序。可将这些单词解释为名称。
尽管已描述了本发明的优选实施例，但本领域内的技术人员一旦得知了基本创造性概念，则可对这些实施例作出另外的变更和修改。所以，所附权利要求意欲解释为包括优选实施例以及落入本发明范围的所有变更和修改。
显然，本领域的技术人员可以对本发明进行各种改动和变型而不脱离本发明的精神和范围。这样，倘若本发明的这些修改和变型属于本发明权利要求及其等同技术的范围之内，则本发明也意图包含这些改动和变型在内。

Claims (7)

1.一种机器人控制方法，其特征在于，所述方法包括：
获取预先设置的每个虚拟领航者的路径参数与非合作目标的运动信息，所述路径参数包括每个虚拟领航者运动轨道的半径、虚拟领航者的标识信息以及虚拟领航者所在编队的机器人数量，所述虚拟领航者与水下机器人对应；
根据第一计算公式得到每个虚拟领航者在目标坐标系下的位置，所述第一计算公式为：
其中，i为第i个虚拟领航者的标识信息，Rd为第i个虚拟领航者的运动轨道的半径，γi为引导变量，φi为第i个虚拟领航者所在编队的编队相位且φi＝(i-1)·2·π/NZ，NZ为第i个虚拟领航者所在编队的机器人数量；
根据每个虚拟领航者在目标坐标系下的位置以及所述非合作目标的运动信息确定所述每个虚拟领航者的当前位置，其中，所述非合作目标的运动信息包括：非合作目标的运动速度及角速度；
根据所述每个虚拟领航者的当前位置生成预设的路径预测模型；
获取每个水下机器人的运动环境参数、每个水下机器人所在编队的机器人数量以及非合作目标的运动信息，所述运动环境参数包括运动轨道的半径；
根据所述运动环境参数、所述所在编队的机器人数量、所述非合作目标的运动信息以及预设的路径预测模型确定所述水下机器人的运动轨迹；
控制所述水下机器人根据所述运动轨迹移动。
2.如权利要求1所述的机器人控制方法，其特征在于，所述根据每个虚拟领航者在目标坐标系下的位置以及所述非合作目标的运动信息确定所述每个虚拟领航者的当前位置的步骤，包括：
根据第二计算公式确定所述每个虚拟领航者的当前位置，所述第二计算公式为：
其中，pd,i(γi,t)为第i个水下机器人的虚拟领航者在地球坐标系下的期望路径，为非合作目标的当前位置，Rt为非合作目标的地球坐标系和艇体坐标系间坐标转换矩阵，/>为二阶反对称矩阵，/>为非合作目标的角速度，/>符号表示相对于γi的微分，/>为非合作目标的速度。
3.如权利要求1所述的机器人控制方法，其特征在于，所述确定所述每个虚拟领航者的当前位置之后，还包括：
构建每个虚拟领航者之间的通信拓扑关系；
基于所述通信拓扑关系生成所述每个虚拟领航者的编队协调控制信号；
根据所述编队协调控制信号控制所述每个虚拟领航者移动。
4.如权利要求3所述的机器人控制方法，其特征在于，所述基于所述通信拓扑关系生成所述每个虚拟领航者的编队协调控制信号，包括：
获取并保存边的连接关系的邻接矩阵A＝[aij]，其中，aij为相邻两个虚拟领航者之间的连接关系；
基于所述邻接矩阵获取度量矩阵，所述度量矩阵为D＝diag(|Ν1|,|Ν2|,...,|ΝN|)，其中，为第i个虚拟领航者所在编队的机器人数量；
根据所述度量矩阵并结合增益矩阵Kc＝diag(kc,1,kc,2,...,kc,N)生成虚拟领航者的编队协调控制信号。
5.如权利要求1所述的机器人控制方法，其特征在于，所述控制所述水下机器人根据所述运动轨迹移动，包括：
获取水下机器人与虚拟领航者的路径跟踪误差；
采用一阶滑模控制方法对所述跟踪误差处理以得到水下机器人的控制律；
根据所述控制律控制所述水下机器人根据所述运动轨迹移动。
6.一种水下机器人，其特征在于，所述水下机器人包括存储器、处理器以及存储在所述存储器并可在所述处理器上运行的机器人控制程序，所述机器人控制程序被所述处理器执行时实现如权利要求1-5任一项所述的机器人控制方法。
7.一种存储介质，其特征在于，所述存储介质存储有机器人控制程序，所述机器人控制程序被处理器执行时实现如权利要求1-5任一项所述的机器人控制方法。
CN202110575994.XA 2021-05-25 2021-05-25 机器人控制方法、机器人、计算机程序产品和存储介质 Active CN113448338B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110575994.XA CN113448338B (zh) 2021-05-25 2021-05-25 机器人控制方法、机器人、计算机程序产品和存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110575994.XA CN113448338B (zh) 2021-05-25 2021-05-25 机器人控制方法、机器人、计算机程序产品和存储介质

Publications (2)

Publication Number Publication Date
CN113448338A CN113448338A (zh) 2021-09-28
CN113448338B true CN113448338B (zh) 2023-11-28

Family

ID=77810235

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110575994.XA Active CN113448338B (zh) 2021-05-25 2021-05-25 机器人控制方法、机器人、计算机程序产品和存储介质

Country Status (1)

Country Link
CN (1) CN113448338B (zh)

Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111522351A (zh) * 2020-05-15 2020-08-11 中国海洋大学 水下机器人三维编队及避障方法

Patent Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111522351A (zh) * 2020-05-15 2020-08-11 中国海洋大学 水下机器人三维编队及避障方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
基于多虚拟领航者的多机器人编队控制方法;王钦钊等;《装甲兵工程学院学报》;第31卷(第5期);第49-54页 *
多AUV队形控制的新方法;吴小平等;舰船科学技术;30(2);第128-134页 *

Also Published As

Publication number Publication date
CN113448338A (zh) 2021-09-28

Similar Documents

Publication Publication Date Title
Hasberg et al. Simultaneous localization and mapping for path-constrained motion
CN110554376A (zh) 用于运载工具的雷达测程法
CN111338383B (zh) 基于gaas的自主飞行方法及***、存储介质
Karakaya et al. A new mobile robot toolbox for MATLAB
Abichandani et al. Mixed integer nonlinear programming framework for fixed path coordination of multiple underwater vehicles under acoustic communication constraints
CN111618861A (zh) 一种基于四轴结构的双随动智能手臂控制方法
Shi et al. Distributed circumnavigation control of autonomous underwater vehicles based on local information
Lu et al. Real-time perception-limited motion planning using sampling-based MPC
Fahimi et al. An alternative closed-loop vision-based control approach for Unmanned Aircraft Systems with application to a quadrotor
Lyu et al. Vision-based plane estimation and following for building inspection with autonomous UAV
Guo et al. Intelligent assistance positioning methodology based on modified iSAM for AUV using low-cost sensors
CN113448338B (zh) 机器人控制方法、机器人、计算机程序产品和存储介质
Nazarzehi et al. Decentralized three dimensional formation building algorithms for a team of nonholonomic mobile agents
CN113984045B (zh) 水下机器人移动式对接目标运动状态估计方法及***
Filaretov et al. Formation control of AUV on the base of visual tracking of AUV-leader
EP3797939B1 (en) Control command based adaptive system and method for estimating motion parameters of differential drive vehicles
CN112747752A (zh) 基于激光里程计的车辆定位方法、装置、设备和存储介质
Zolghadr et al. Locating a two-wheeled robot using extended Kalman filter
Tsai et al. Cooperative localization using fuzzy decentralized extended information filtering for homogenous omnidirectional mobile multi-robot system
Khorrami et al. A hierarchical path planning and obstacle avoidance system for an autonomous underwater vehicle
Demim et al. Simultaneous localization and mapping algorithm based on 3D laser for unmanned aerial vehicle
CN114265406B (zh) 一种基于机器视觉的智能车编队控制***及其控制方法
Winkens et al. Optical truck tracking for autonomous platooning
Kelly et al. Simultaneous mapping and stereo extrinsic parameter calibration using GPS measurements
Uyulan et al. Mobile robot localization via sensor fusion algorithms

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