一种水下无人航行器和机械手系统的手艇协调控制方法与流程

本发明涉及一种水下无人航行器和机械手系统的手艇协调控制方法。

背景技术：

随着海洋开发与研究的日益蓬勃开展，水下无人航行器搭载机械手在进行水下打捞、援潜救生、海底设施的维护与装置的回收、海底生物及岩石标本的采样等方面得到了越来越广泛地应用。

首先，虽然目前搭载机械手的水下航行器通过遥控操作可以完成大量的作业工作，但其在结构化环境中的作业能力等方面存在较大缺陷，作业范围也受到较大限制，并且在水下机器人的作业中如果单纯依赖人的遥控，将难以实现海流扰动中机械手末端的准确定位和作业。专利文献“一种ROV水下机器人的运动控制方法(CN105404303A)”和“浮游式水下机器人的S面控制方法(CN1718378A)”，涉及到的水下机器人运动控制方法，虽然也可以完成水下机器人的运动控制，但由于作业过程中机械手的运动会给航行器带来较大的扰动作用，并且扰动作用力根据作业姿态随时变化，所以上述根据干扰误差反馈进行补偿的控制方式难以实现作业过程中的航行器稳定控制。

技术实现要素：

本发明的目的是为了提供一种水下无人航行器和机械手系统的手艇协调控制方法，能够通过航行器和机械手当前的位置和姿态的反馈实时估计作业过程中水下无人航行器所受到的机械手的扰动力，并在控制中进行补偿，从而提高作业精度。该方法用于水下无人航行器的遥控作业可以减小操作人员的操作时间，提高遥控精度，用于水下无人航行器的自主作业可以提高控制精度，尤其是机械手末端的控制精度，实现航行器的自主作业。

本发明的目的是这样实现的：包括水下无人航行器控制系统和协调控制器，

步骤一、协调运动控制器通过隔离串口板采集水下无人航行器控制系统中的多普勒测速仪和磁罗经的数据，并通过水下无人航行器控制系统中的PC104模块进行船位推算，获得水下无人航行器当前位姿，通过水下无人航行器控制系统中的can数据采集板、机械手直流伺服电机驱动器和编码器获取机械手各关节当前姿态；

步骤二、建立扰动力观测器，并设计协调控制器的两条控制率；

步骤三、通过协调控制器的两条控制率控制并控制水下无人航行器和机械手的运动。

本发明还包括这样一些结构特征：

1.步骤二中所述建立扰动力观测器是机械手对航行器的扰动力观测器，其中，倾斜力矩观测公式是：

式中，和分别为机械手第i个广义连杆相对于水下无人航行器的重力和浮力的矩阵表达形式，⁰R_V为艇体坐标系到机械手基座坐标系的转换矩阵和分别为机械手第i个广义连杆的重心和浮心相对于水下无人航行器的位置，而和分别为机械手艇体重心和浮心相对于水下无人航行器的位置，m_ig和F_Bi分别为机械手第i个广义连杆相对于水下无人航行器的重力和浮力；

机械手和水下无人航行器的耦合运动造成的耦合力的观测公式是：

式中：⁰f_1,0和⁰n_1,0是水下无人航行器在手臂基关节受到的机械手的耦合作用力，¹f_1,0和¹n_1,0是水下无人航行器在手臂基座受到的机械手的耦合作用力，⁰R₁是机械手基关节的随动坐标系到机械手基座坐标系的转换矩阵，⁰f_1,0和⁰n_1,0将由下式递推得到：

式中：ⁱf_i,i-1和ⁱn_i,i-1为第i个广义连杆在其第i关节对第i-1关节的约束反力，ⁱr_i和ⁱr_ci分别为机械手第i个广义连杆在其坐标系下的位置矢量和中心位置矢量，ⁱF_i,i和ⁱM_i,i分别为机械手第i个广义连杆在水中运动所受到的流体作用力，ⁱf_i^*和分别为机械手第i个广义连杆质心的惯性力和力矩，可以通过牛顿-欧拉方程递推得到；

基于倾斜力矩观测公式、机械手和水下无人航行器的耦合运动造成的耦合力的观测公式得出扰动力矩的观测公式是：

2.所设计协调运动控制器是水下无人航行器和机械手的协调运动控制器，包括运动学控制律和动力学控制律两部分。

3.所述动力学控制率为：

式中：代表质量、惯性以及附加质量的观测矩阵，代表由质量和附加质量引起的科氏力的观测矩阵，代表由流体粘性引起的阻力的观测矩阵，K_d和K_p代表微分系数和比例系数，q代表实际位置量，q_d代表期望位置量，代表位置偏差。

与现有技术相比，本发明的有益效果是：本发明根据水下机器人精确作业控制的需要设计一种水下无人航行器和机械手系统的手艇协调运动控制方法，通过测量和估计作业过程中水下无人航行器所受到的机械手的扰动力，并进行补偿，保证水下无人航行器的平稳运动，实现精确的定位和作业控制。该发明具有控制准确、适应性强、鲁棒性高等优点。

附图说明

图1是水下无人航行器控制系统的内部硬件系统框图；

图2是水下无人航行器协调控制的流程图；

图3是水下无人航行器和机械手的协调运动控制器框图；

图4(a)至图4(d)分别是无协调控制时观测到机械手的各关节位移、艇体产生的纵摇和横摇、机械手运动产生的倾斜力矩以及机械手运动产生的耦合力对水下无人航行器的干扰力；

图5是应用本发明对水下无人航行器和机械手协调运动的控制结果；

图6(a)至图6(f)分别是观测得到的协调运动过程中机械手各关节位移、艇体姿态、机械手运动中观测到的扰动力、机械手运动中观测到的扰动力矩、机械手运动中产生的耦合力、机械手运动中产生的倾斜力矩对水下无人航行器的扰动力；

具体实施方式

下面结合附图与具体实施方式对本发明作进一步详细描述。

本发明为：

a、水下无人航行器机械手系统的手艇协调控制方法由水下无人航行器控制系统和协调控制控制器两部分组成。水下无人航行器控制系统主要由PC104核心模块、I/O板、can数据采集板、隔离串口板、数据采集板、直流伺服电机控制板、机械手直流伺服电机驱动器、编码器、机械手电机、推进器螺旋桨及其电机驱动器、磁罗经、深度计、多普勒测速仪组成。控制器通过隔离串口板采集多普勒测速仪和磁罗经的数据，并通过PC104模块进行船位推算，获得水下无人航行器当前位姿；进而建立扰动力观测器，并设计协调运动控制器控制机械手的运动；通过can数据采集板、机械手直流伺服电机驱动器和编码器获取机械手各关节当前姿态，并控制机械手的运动；通过直流伺服电机控制板、推进器螺旋桨及其电机驱动器控制水下无人航行器螺旋桨的运动。

b、所建立扰动力观测器为机械手对航行器的扰动力观测器。扰动力主要包括机械手和水下无人航行器的耦合运动造成的耦合力，作业过程中系统浮心和重心变化派生的倾斜力矩两部分。其中倾斜力矩可以结合水下无人航行器和机械手广义连杆的当前位置姿态，通过下面的公式来观测：

式中，和分别为机械手第i个广义连杆相对于水下无人航行器的重力和浮力的矩阵表达形式，⁰R_V为艇体坐标系到机械手基座坐标系的转换矩阵；和分别为机械手第i个广义连杆的重心和浮心相对于水下无人航行器的位置，而和分别为机械手艇体重心和浮心相对于水下无人航行器的位置，m_ig和F_Bi分别为机械手第i个广义连杆相对于水下无人航行器的重力和浮力。机械手和水下无人航行器的耦合运动造成的耦合力可以通过下面的公式来观测：

式中⁰f_1,0和⁰n_1,0是水下无人航行器在手臂基座受到的机械手的耦合作用力，¹f_1,0和¹n_1,0是水下无人航行器在手臂基座受到的机械手的耦合作用力。⁰R₁是机械手基关节的随动坐标系到机械手基座坐标系的转换矩阵。⁰f_1,0和⁰n_1,0将由下式递推得到：

式中ⁱf_i,i-1和ⁱn_i,i-1为第i个广义连杆在其第i关节对第i-1关节的约束反力(力矩)。ⁱr_i和ⁱr_ci分别为机械手第i个广义连杆在其坐标系下的位置矢量和中心位置矢量。ⁱF_i,i和ⁱM_i,i分别为机械手第i个广义连杆在水中运动所受到的流体作用力。ⁱf_i^*和分别为机械手第i个广义连杆质心的惯性力和力矩，可以通过牛顿-欧拉方程递推得到。

由于航行器受到的扰动力(矩)主要由上述两种组成，所以扰动力矩可按如下公式进行观测：

c、所设计协调运动控制器为水下无人航行器和机械手的协调运动控制器。控制器包括运动学控制律和动力学控制律两部分。将专利文献“基于浮游式水下机器人的S面控制方法(CN1718378A)”作为运动学控制律，主要作用是保证水下无人航行器和机械手的协调控制器全局收敛；在动力学控制律的设计中，将基于系统的动力学模型，观测水下无人航行器在作业过程中受到的机械手扰动力，并控制推进器运动进行补偿，实现作业过程中航行器稳定和精确控制。协调控制器如下：

式中：代表质量，惯性以及附加质量的观测矩阵,代表由质量和附加质量引起的科氏力的观测矩阵，代表由流体粘性引起的阻力的观测矩阵，K_d和K_p代表微分系数和比例系数，q代表实际位置量，q_d代表期望位置量，代表位置偏差。

下面结合附图进行说明：

结合图1，水下无人航行器控制系统的核心是PC104核心模块，PC104模块通过隔离串口板采集多普勒测速仪和磁罗经的数据，并进行船位推算，获得水下无人航行器当前位姿；PC104模块通过can数据采集板、由机械手直流伺服电机驱动器和编码器获取机械手各关节的当前姿态，并发送控制指令控制机械手的运动；PC104模块根据协调运动控制器的结果，通过直流伺服电机控制板向电机驱动器发送控制指令，控制水下无人航行器螺旋桨运动实现作业过程中水下无人航行器的稳定精确运动。

结合图2，水下无人航行器机械手系统的手艇协调控制的流程是：首先，传感器采集航行器位姿和机械手的运动姿态数据；然后，通过运动学控制律调整位置偏差使其收敛，同时，扰动力观测器按照式(4)观测扰动力，并通过动力学控制律对其进行补偿；最后，两种控制律通过推进器同时作用在航行器上，传感器获取航行器当前时刻运动姿态数据，继续进行下一轮的协调控制。

结合图3协调运动控制器将S面控制方法(CN1718378A)作为运动学控制律，以保证水下无人航行器和机械手的协调控制器全局收敛；并基于系统的动力学模型，观测水下无人航行器在作业过程中受到的机械手扰动力，并控制推进器运动进行补偿，实现作业过程中航行器稳定和精确控制。

结合图4(a)至图4(d)，在没有经过协调运动控制器的实时控制条件下，机械手三个关节的运动对艇体产生了较大的倾斜力和耦合力矩，其结果是对艇造成了较大的横倾和纵倾。

结合图5，水下无人航行器机械手系统按照轨迹规划的对航行器和机械手各关节的规划轨迹，通过协调控制精确地实现了航行器和机械手末端的位姿控制，说明控制器可以有效地应用于在水下无人航行器机械手系统的水下作业精确控制。

结合图6(a)至图6(f)，表示经过动态控制后，观测到的机械手产生的干扰力(矩)，与图4对比可发现，协调运动控制器显著地减小了倾斜力(矩)对艇体的影响，较好地控制了机械手运动造成的航行器本体在横倾和纵倾方向的运动。

综上，本发明提供的是水下无人航行器机械手系统(Underwater Vehicle Manipulator System-UVMS)的手艇协调控制方法。用以实现水下无人航行器的稳定的高精度作业。为实现上述目的，水下无人航行器和机械手系统的手艇协调控制方法包括：通过PC104、数据采集板、机械手电机、螺旋桨推进器、磁罗经、多普勒测速仪等设备构建水下无人航行器控制系统，获取系统各自由度的姿态信息，同时进行硬件层面的控制；根据航行器和机械手的位置姿态建立内部扰动力观测器，依据牛顿-欧拉方法观测各时刻因系统姿态变化产生的倾斜力矩和耦合力矩；建立航行器和机械手的协调运动控制器，在控制中对航行器作业过程中受到的机械手扰动力进行补偿，实现作业过程中航行器稳定和精确控制。该方法可实际应用于UVMS控制系统设计，对于水下机器人在自主作业和遥控作业方面具有重要意义。