水下移动作业机器人的协调规划与控制方法技术

本发明专利技术公开一种水下移动作业机器人的协调规划与控制方法。该方法包括：通过动态追踪微分器，实时规划当前期望的速度与状态；用迭代任务优先方法，将笛卡尔空间的任务规划转化到随体坐标系和各关节坐标系的速度与加速度规划；根据速度与加速度规划，利用动力学方法来控制潜器和作业臂，从而使得水下移动作业机器人进行巡游与作业。本发明专利技术实例采用动态追踪微分器，实施规划当前各任务的期望速度与期望状态，然后，采用状态观测器，估计当前任务状态与速度，并反馈至控制输入，最后通过鲁棒的迭代任务优先方法实现闭环控制，提高了水下移动作业机器人的自主化水平，最终解决了如何实现水下移动作业机器人全自主巡游与作业的技术问题。

Coordinated planning and control method for underwater mobile robot

The invention discloses a coordinated planning and control method for an underwater mobile robot. The method includes: tracking differentiator through dynamic, real-time planning speed and the current state of expectation; priority using iterative method, the Descartes space mission planning into the speed and acceleration of planning with the body coordinate system and the joint coordinate system; according to the speed and acceleration of planning, using the dynamic method to control submersible and manipulator thus, parade and operation of mobile robot under water. The examples of the use of dynamic tracking differentiator, the implementation of each task in the planning of the desired speed and expected condition, then, the observer estimates the current state of the task and speed, and feedback to the control input, finally through the iterative priority method of robust implementation of closed loop control, independent of the level of mobile robot to improve underwater finally, to solve the technical problem how to realize underwater mobile robot autonomous cruise and operation.

【技术实现步骤摘要】
水下移动作业机器人的协调规划与控制方法
本专利技术涉及机器人
，尤其是涉及一种水下移动作业机器人的协调规划与控制方法。
技术介绍
广阔的海洋为人类提供了丰富的资源，对海洋资源的高效利用和可持续开发对人类具有重要的意义。人类对海洋的探索从未停止，从最原始的载人水下航行器，到遥控型水下航行器，再到如今的全自主无人水下航行器的研究，这些极大地推动了人类对海洋的认识和开发。特别是在海产养殖中，我国近浅海水产养殖业迅猛发展，各类水产品产量巨大，由于捕捞技术落后，却面临着效率低、浅海环境污染、植被破坏及资源消耗严重的窘境。利用潜水员在水下作业采捕海珍品的人工采捕方式对环境无破坏，但这种方式的效率低，作业时间有限，作业环境恶劣，而且潜在危险多等。随着对海洋环境保护和工作效率要求的不断提高，采用水下移动作业机器人等采捕设备替代人工水下作业的需求愈发迫切。水下移动作业机器人一般由移动平台和一个或多个机械臂组成，移动平台和机械臂按需求设计、制造后组装集成在一起，通过高度集成的感知、规划与控制系统，实现机器人的水下自主作业。在非结构化未知的水下环境，水下移动作业机器人实现全自主作业主要分为自主巡游与自主作业两个阶段。欧美及其日本研制各类型的水下机器人作业系统，这些作业机器人普遍通过遥控或者半自主的方式进行水下作业，普遍存在自主化水平不足、操作技术要求高等弊端。有鉴于此，特提出本专利技术。
技术实现思路
为了解决现有技术中的上述问题，即为了解决如何实现水下移动作业机器人全自主巡游与作业的技术问题，提供一种水下移动作业机器人的协调规划与控制方法。为了实现上述目的，提供以下技术方案：一种水下移动作业机器人的协调规划与控制方法，该方法包括：通过动态追踪微分器，实时规划当前期望的速度与状态；采用迭代任务优先方法，将笛卡尔空间的任务规划转化到随体坐标系和各关节坐标系的速度与加速度规划；根据速度与加速度规划，利用动力学方法来控制潜器和作业臂，从而使得水下移动作业机器人进行巡游与作业。优选地，通过动态追踪微分器，实时规划当前期望的速度与状态具体包括：利用自抗扰技术，通过动态追踪微分器将阶跃型任务信号转化为连续的光滑信号，并得到当前期望的速度和状态。优选地，根据下式来建立动态追踪微分器：f＝f(v1-v0,v2,r0,h0)v1＝v1+h0v2v2＝v2+h0f其中，v0表示给定的期望目标点；v1表示状态规划；v2表示速度规划；r0、h0分别表示影响微分器曲线的速度和光滑程度的微分器参数；其中，f根据下式确定：优选地，采用迭代任务优先方法，将笛卡尔空间的任务规划转化到随体坐标系和各关节坐标系的速度与加速度规划，具体包括：通过迭代任务优先方法进行冗余迭代，对笛卡尔空间世界坐标系下任务规划进行转化，求解在随体坐标和各关节坐标系下的期望速度与期望加速度。优选地，方法还包括：通过状态观测器观测估计当前的任务速度与任务状态，并进行实时反馈，以更新速度与加速度规划。优选地，实时反馈中的反馈信号通过以下方式来确定：根据当前规划的期望速度和期望加速度、当前速度误差和当前状态误差，并且与规划的速度加权结合，来确定反馈信号。优选地，状态观测器通过下式来建立：e＝z1-z0z1＝z1+h0(z2-β01e)z2＝z2+h0(z3-β02e+Jζ)z3＝z3+h0(-β03e)其中，β01、β02、β03表示设定的保证收敛的参数；z0表示期望的目标状态；z1、z2、z3分别表示对状态、速度和加速度的观测估计；J表示相对应的雅克比矩阵；ζ由任务速度规划与误差(状态误差与速度误差)共同组成的反馈项；h0表示影响微分器曲线的光滑程度的微分器参数。本专利技术实施例提供一种水下移动作业机器人的协调规划与控制方法。该方法包括：通过动态追踪微分器，实时规划当前期望的速度与状态；用迭代任务优先方法，将笛卡尔空间的任务规划转化到随体坐标系和各关节坐标系的速度与加速度规划；根据速度与加速度规划，利用动力学方法来控制潜器和作业臂，从而使得水下移动作业机器人进行巡游与作业。本专利技术实施例将自主巡游和自主作业过程的各个子任务统一起来，采用动态追踪微分器，实施规划当前各任务的期望速度与期望状态，然后，采用状态观测器，估计当前任务状态与速度，并反馈至控制输入，最后通过鲁棒的迭代任务优先方法实现闭环控制，提高了水下移动作业机器人的自主化水平，最终实现了水下移动作业机器人全自主巡游与作业。附图说明图1为根据本专利技术实施例的水下移动机器人作业臂系统的示意图；图2为根据本专利技术实施例的水下移动作业机器人坐标系示意图；图3为根据本专利技术实施例的水下移动作业机器人的协调规划与控制方法的流程示意图；图4为根据本专利技术实施例的带有跟踪微分器和状态观测器的鲁棒迭代任务优先算法的流程示意图。具体实施方式下面参照附图来描述本专利技术的优选实施方式。本领域技术人员应当理解的是，这些实施方式仅仅用于解释本专利技术的技术原理，并非旨在限制本专利技术的保护范围。本专利技术实施例基本构思是将航行器舰体和机械臂做统一的规划和控制，充分考虑艇体与机械臂的耦合作用，将自主作业的任务划分为若干个子任务，并将子任务做优先级排列，然后，按照优先级顺序依次融合各子任务控制策略与方案，从而完成各子任务并实现最终的作业任务。图1是水下移动机器人作业臂系统，该系统主要由两侧对称装配的仿生推动器、作业臂和艇体三部分组成。对称的仿生推进器可以提供机器人前进、后退、侧移、上浮、下潜、正逆旋转所需要的力和力矩，即在本体坐标系中具有四个自由度，如图2所示，分别沿Ob-x,y,z方向的平移运动以及绕Ob-z的旋转运动。在机械臂上，也存在四个自由度，分别是腰关节、肩关节、肘关节、腕关节，分别可以做左右摆动上下摆动、上下摆动和旋转运动，机械臂末端为双指夹器，通过钢丝牵动实现目标的夹取等，各关节分别建立Oi-x，y，z，i＝0，1，2，3坐标系。借助于机械臂运动学模型，给定任务与系统速度的关系可以表示为：其中，是给定任务的时间微分；ζ为系统速度；J是相应的雅克比矩阵。在横滚和俯仰角的控制上，仅仅通过长鳍波动实现，相当困难。除此之外，当自主作业时，如果带有俯仰和横滚角，自主作业的控制和实现也比较困难，因此在实际规划和作业过程中，需要限制水下移动作业机器人的俯仰角和横滚角始终为零。为此本专利技术实施例提供一种水下移动作业机器人的协调规划与控制方法。如图3所示该方法可以包括：S1：通过动态追踪微分器，实时规划当前期望的速度与状态。具体地，本步骤可以包括：利用自抗扰技术，通过动态追踪微分器将阶跃型任务信号转化为连续的光滑信号并得到当前期望的速度和状态。在实际应用中，控制目标往往是一种阶跃信号形式，例如，到达指定的位置等，因此，输出信号存在跳跃，严格的追踪阶跃轨迹非常困难，另外，系统反馈信号并不精确，因此基于自适应抗扰动技术，来根据当前的状态与期望目标，规划当前的速度、状态等信息。在一个优选的实施例中，可以根据下式来建立动态追踪微分器：f＝f(v1-v0,v2,r0,h0)v1＝v1+h0v2v2＝v2+h0f其中，v0表示给定的期望目标点；v1表示状态规划；v2表示速度规划或任务速度规划；r0、h0分别表示影响微分器曲线的速度和光滑程度的微分器参数；其中，f(·)根据下式确定：a0＝h0v2y＝本文档来自技高网...

【技术保护点】
一种水下移动作业机器人的协调规划与控制方法，其特征在于，所述方法包括：通过动态追踪微分器，实时规划当前期望的速度与状态；采用迭代任务优先方法，将笛卡尔空间的任务规划转化到随体坐标系和各关节坐标系的速度与加速度规划；根据所述速度与所述加速度规划，利用动力学方法来控制潜器和作业臂，从而使得所述水下移动作业机器人进行巡游与作业。

【技术特征摘要】
1.一种水下移动作业机器人的协调规划与控制方法，其特征在于，所述方法包括：通过动态追踪微分器，实时规划当前期望的速度与状态；采用迭代任务优先方法，将笛卡尔空间的任务规划转化到随体坐标系和各关节坐标系的速度与加速度规划；根据所述速度与所述加速度规划，利用动力学方法来控制潜器和作业臂，从而使得所述水下移动作业机器人进行巡游与作业。2.根据权利要求1所述的方法，其特征在于，所述通过动态追踪微分器，实时规划当前期望的速度与状态具体包括：利用自抗扰技术，通过所述动态追踪微分器将阶跃型任务信号转化为连续的光滑信号，并得到当前期望的速度和状态。3.根据权利要求1所述的方法，其特征在于，根据下式来建立所述动态追踪微分器：f＝f(v1-v0,v2,r0,h0)v1＝v1+h0v2v2＝v2+h0f其中，所述v0表示给定的期望目标点；所述v1表示状态规划；所述v2表示速度规划；所述r0、h0分别表示影响微分器曲线的速度和光滑程度的微分器参数；其中，所述f根据下式确定：

【专利技术属性】
技术研发人员：王宇，唐冲，王睿，王硕，谭民，
申请(专利权)人：中国科学院自动化研究所，
类型：发明
国别省市：北京,11