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

本发明涉及机器人技术领域，尤其是涉及一种水下移动作业机器人的协调规划与控制方法。

背景技术：

广阔的海洋为人类提供了丰富的资源，对海洋资源的高效利用和可持续开发对人类具有重要的意义。人类对海洋的探索从未停止，从最原始的载人水下航行器，到遥控型水下航行器，再到如今的全自主无人水下航行器的研究，这些极大地推动了人类对海洋的认识和开发。特别是在海产养殖中，我国近浅海水产养殖业迅猛发展，各类水产品产量巨大，由于捕捞技术落后，却面临着效率低、浅海环境污染、植被破坏及资源消耗严重的窘境。利用潜水员在水下作业采捕海珍品的人工采捕方式对环境无破坏，但这种方式的效率低，作业时间有限，作业环境恶劣，而且潜在危险多等。随着对海洋环境保护和工作效率要求的不断提高，采用水下移动作业机器人等采捕设备替代人工水下作业的需求愈发迫切。水下移动作业机器人一般由移动平台和一个或多个机械臂组成，移动平台和机械臂按需求设计、制造后组装集成在一起，通过高度集成的感知、规划与控制系统，实现机器人的水下自主作业。在非结构化未知的水下环境，水下移动作业机器人实现全自主作业主要分为自主巡游与自主作业两个阶段。欧美及其日本研制各类型的水下机器人作业系统，这些作业机器人普遍通过遥控或者半自主的方式进行水下作业，普遍存在自主化水平不足、操作技术要求高等弊端。

有鉴于此，特提出本发明。

技术实现要素：

为了解决现有技术中的上述问题，即为了解决如何实现水下移动作业机器人全自主巡游与作业的技术问题，提供一种水下移动作业机器人的协调规划与控制方法。

为了实现上述目的，提供以下技术方案：

一种水下移动作业机器人的协调规划与控制方法，该方法包括：

通过动态追踪微分器，实时规划当前期望的速度与状态；

采用迭代任务优先方法，将笛卡尔空间的任务规划转化到随体坐标系和各关节坐标系的速度与加速度规划；

根据速度与加速度规划，利用动力学方法来控制潜器和作业臂，从而使得水下移动作业机器人进行巡游与作业。

优选地，通过动态追踪微分器，实时规划当前期望的速度与状态具体包括：

利用自抗扰技术，通过动态追踪微分器将阶跃型任务信号转化为连续的光滑信号，并得到当前期望的速度和状态。

优选地，根据下式来建立动态追踪微分器：

f＝f(v₁-v₀,v₂,r₀,h₀)

v₁＝v₁+h₀v₂

v₂＝v₂+h₀f

其中，v₀表示给定的期望目标点；v₁表示状态规划；v₂表示速度规划；r₀、h₀分别表示影响微分器曲线的速度和光滑程度的微分器参数；

其中，f根据下式确定：

优选地，采用迭代任务优先方法，将笛卡尔空间的任务规划转化到随体坐标系和各关节坐标系的速度与加速度规划，具体包括：

通过迭代任务优先方法进行冗余迭代，对笛卡尔空间世界坐标系下任务规划进行转化，求解在随体坐标和各关节坐标系下的期望速度与期望加速度。

优选地，方法还包括：

通过状态观测器观测估计当前的任务速度与任务状态，并进行实时反馈，以更新速度与加速度规划。

优选地，实时反馈中的反馈信号通过以下方式来确定：

根据当前规划的期望速度和期望加速度、当前速度误差和当前状态误差，并且与规划的速度加权结合，来确定反馈信号。

优选地，状态观测器通过下式来建立：

e＝z₁-z₀

z₁＝z₁+h₀(z₂-β₀₁e)

z₂＝z₂+h₀(z₃-β₀₂e+Jζ)

z₃＝z₃+h₀(-β₀₃e)

其中，β₀₁、β₀₂、β₀₃表示设定的保证收敛的参数；z₀表示期望的目标状态；z₁、z₂、z₃分别表示对状态、速度和加速度的观测估计；J表示相对应的雅克比矩阵；ζ由任务速度规划与误差(状态误差与速度误差)共同组成的反馈项；h₀表示影响微分器曲线的光滑程度的微分器参数。

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

附图说明

图1为根据本发明实施例的水下移动机器人作业臂系统的示意图；

图2为根据本发明实施例的水下移动作业机器人坐标系示意图；

图3为根据本发明实施例的水下移动作业机器人的协调规划与控制方法的流程示意图；

图4为根据本发明实施例的带有跟踪微分器和状态观测器的鲁棒迭代任务优先算法的流程示意图。

具体实施方式

下面参照附图来描述本发明的优选实施方式。本领域技术人员应当理解的是，这些实施方式仅仅用于解释本发明的技术原理，并非旨在限制本发明的保护范围。

本发明实施例基本构思是将航行器舰体和机械臂做统一的规划和控制，充分考虑艇体与机械臂的耦合作用，将自主作业的任务划分为若干个子任务，并将子任务做优先级排列，然后，按照优先级顺序依次融合各子任务控制策略与方案，从而完成各子任务并实现最终的作业任务。

图1是水下移动机器人作业臂系统，该系统主要由两侧对称装配的仿生推动器、作业臂和艇体三部分组成。对称的仿生推进器可以提供机器人前进、后退、侧移、上浮、下潜、正逆旋转所需要的力和力矩，即在本体坐标系中具有四个自由度，如图2所示，分别沿O_b-x,y,z方向的平移运动以及绕O_b-z的旋转运动。在机械臂上，也存在四个自由度，分别是腰关节、肩关节、肘关节、腕关节，分别可以做左右摆动上下摆动、上下摆动和旋转运动，机械臂末端为双指夹器，通过钢丝牵动实现目标的夹取等，各关节分别建立O_i-x，y，z，i＝0，1，2，3坐标系。

借助于机械臂运动学模型，给定任务与系统速度的关系可以表示为：

其中，是给定任务的时间微分；ζ为系统速度；J是相应的雅克比矩阵。

在横滚和俯仰角的控制上，仅仅通过长鳍波动实现，相当困难。除此之外，当自主作业时，如果带有俯仰和横滚角，自主作业的控制和实现也比较困难，因此在实际规划和作业过程中，需要限制水下移动作业机器人的俯仰角和横滚角始终为零。

为此本发明实施例提供一种水下移动作业机器人的协调规划与控制方法。如图3所示该方法可以包括：

S1：通过动态追踪微分器，实时规划当前期望的速度与状态。

具体地，本步骤可以包括：利用自抗扰技术，通过动态追踪微分器将阶跃型任务信号转化为连续的光滑信号并得到当前期望的速度和状态。

在实际应用中，控制目标往往是一种阶跃信号形式，例如，到达指定的位置等，因此，输出信号存在跳跃，严格的追踪阶跃轨迹非常困难，另外，系统反馈信号并不精确，因此基于自适应抗扰动技术，来根据当前的状态与期望目标，规划当前的速度、状态等信息。

在一个优选的实施例中，可以根据下式来建立动态追踪微分器：

f＝f(v₁-v₀,v₂,r₀,h₀)

v₁＝v₁+h₀v₂

v₂＝v₂+h₀f

其中，v₀表示给定的期望目标点；v₁表示状态规划；v₂表示速度规划或任务速度规划；r₀、h₀分别表示影响微分器曲线的速度和光滑程度的微分器参数；其中，f(·)根据下式确定：

a₀＝h₀v₂

y＝v₁+a₀

a₂＝a₀+sign(y)(a₁-d)/2

s_y＝(sign(y+d)-sign(y-d))/2

a＝(a₀+y-a₂)s_y+a₂

s_a＝(sign(a+d)-sign(a-d))/2

f＝-r₀(a/d-sign(a))s_a-r₀sign(a)

本发明实施例通过采用动态追踪微分器，将阶跃型任务信号转化为连续的光滑信号，使得控制信号连续光滑，实现了信号的连续性和可微性，使得控制信号更加细致化，避免了信号的阶跃型跳跃和震动；而且动态追踪微分器将信号平滑化的同时，给出了当前的速度规划，并且实时计算出当前的速度和状态信息。

图4示例性地示出了得到任务的状态与速度规划的示意图。沿用前例，以3个任务(i＝1,2,3)进行示例性说明。其中，对任务集σ_z并结合机器人的位姿η和机械臂各关节角q进行运动学控制，得到系统参考速度v_d与系统参考加速度然后根据v_d、进行动力学控制，得到期望的力和力矩τ_d；然后，期望的力和力矩τ_d输入至驱动控制部分；驱动控制部分输出机器人艇体的速度v与加速度速度v与加速度再反馈至动力学控制部分；σ₁、σ₂、σ₃是给定任务中三个子任务，通过追踪微分器(TD1、TD2、TD3)之后，可以得到各个子任务的状态与速度规划。其中，i＝1,2,3；σ_i为给定的第i个任务，TDi为追踪微分器(Tracking Differentiator)，σ_id,为第i个任务的状态规划和速度规划；K_ip,K_id为状态误差和速度误差的加权系数；ζ_i是满足第i个任务所需要的系统速度；Q_i-1是零空间映射算子；ζ为满足所有任务的系统速度；dζ/dt对速度进行时间微分，得到加速度信息；v,为机器人艇体的速度与加速度；为机械臂各关节的旋转速度和加速度，q为机械臂各关节角；η是机器人的位姿；SOi为状态观测器(State Observer)；为第i个任务的状态和速度的观测。

S2：采用迭代任务优先方法，将笛卡尔空间的任务规划转化到随体坐标系和各关节坐标系的速度与加速度规划。

其中，迭代任务优先方法具有很好的鲁棒性。目标任务可以是在笛卡尔空间世界坐标系中给定的。

作为另一个实施例，具体地，本步骤可以包括：通过迭代任务优先方法进行冗余迭代，对笛卡尔空间世界坐标系下任务规划进行转化，求解在随体坐标和各关节坐标系下的期望速度与期望加速度。

本步骤采用任务优先方法框架，将笛卡尔空间世界坐标系下任务规划转化到随体坐标系和各关节坐标系的速度与加速度规划，并作为动力学控制的信息输入。

沿用图4所示示例，σ₁处于最高优先级，σ₂次之，σ₃优先级最低，根据下式通过任务优先方法进行依次迭代：

ζ＝v₂+k₁(v₁-z₁)+k₂(v₂-z₂)

其中，ζ表示由任务速度规划v₂与误差(状态误差与速度误差)共同组成的反馈项；Q表示零空间映射算子；k₁表示状态误差加权系数；k₂表示速度误差加权系数；z₁表示对状态的观测估计；z₂表示对速度的观测估计；I表示单位矩阵；i表示当前任务索引，即表示将所有给定任务依次进行迭代，其最大值是给定的任务总数；表示第i个任务的相应雅克比矩阵的广义逆矩阵；J_i表示第i个任务的相应雅克比矩阵；ρ_i表示迭代输出结果。

图4示例性地示出了带有跟踪微分器和状态观测器的鲁棒迭代任务优先算法的流程示意图。通过图4所示方法，消除了经典任务优先算法的运动学奇异和算法奇异现象，保证了控制算法的鲁棒性，本发明实施例可以将所有的任务依次迭代，计算出满足所有任务需求的在随体坐标系和各关节坐标系中的速度与加速度。

S3：根据速度与加速度规划，利用动力学方法来控制潜器和作业臂，从而产生水下移动作业机器人的巡游与作业。

在一个优选的实施例中，本发明实施例提供的方法还可以包括：通过状态观测器观测估计当前的任务速度与任务状态，并进行实时反馈，以更新速度与加速度规划。

其中，通过状态观测器观测估计当前的速度与状态，并实时反馈至输入端，构成闭环控制，从而可以提高本发明实施例的稳定性。

在一个优选的实施例中，实时反馈中的反馈信号通过以下方式来确定：根据当前规划的期望速度和期望加速度、当前速度误差和当前状态误差，并且与规划的速度加权结合，来确定反馈信号。

在一个优选的实施例中，状态观测器通过下式来建立：

e＝z₁-z₀

z₁＝z₁+h₀(z₂-β₀₁e)

z₂＝z₂+h₀(z₃-β₀₂e+Jζ)

z₃＝z₃+h₀(-β₀₃e)

其中，β₀₁、β₀₂、β₀₃表示设定的保证收敛的参数；z₁、z₂、z₃分别表示对状态、速度和加速度的观测估计；z₀表示期望的目标状态；J表示相对应的雅克比矩阵；ζ由任务速度规划与误差(状态误差与速度误差)共同组成的反馈项；h₀表示影响微分器曲线的光滑程度的微分器参数。

本发明实施例通过采用上述技术方案，实现了潜器-作业臂协调规划与运动控制的闭环控制，解决了如何实现水下移动作业机器人的自主巡游与作业的技术问题。通过状态观测器可以实时地观测估计系统的状态和速度信息，从而大大提高了在末端自主作业时的稳定性和精准度；而且，反馈信号综合考虑了当前规划的期望速度、当前速度误差及当前状态误差，相比于单一的状态误差方法而言，其收敛速度更快；将自主巡游和自主作业两个阶段的不同任务做统一考虑，无需进行阶段划分和状态切换机制的设计，可以同时考虑多个任务的同时执行而且消除任务之间的相互影响，从而保证迭代更多的任务，将所有的任务都统一在本框架之下，通过自适应调节机制，能够保证在不同阶段对不同的性能指标的要求，可以统一实现自主的高速高机动自主巡游和高稳定高精准自主作业，例如：在巡游阶段的高速高机动性，作业阶段的高稳定高精度性，消除了经典任务优先方法的运动学奇异和方法奇异，保证了方法的鲁棒性。

至此，已经结合附图所示的优选实施方式描述了本发明的技术方案，但是，本领域技术人员容易理解的是，本发明的保护范围显然不局限于这些具体实施方式。在不偏离本发明的原理的前提下，本领域技术人员可以对相关技术特征作出等同的更改或替换，这些更改或替换之后的技术方案都将落入本发明的保护范围之内。