CN106737659B - 一种水下无人航行器和机械手系统的手艇协调控制方法 - Google Patents

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

Info

Publication number
CN106737659B
CN106737659B CN201611009723.3A CN201611009723A CN106737659B CN 106737659 B CN106737659 B CN 106737659B CN 201611009723 A CN201611009723 A CN 201611009723A CN 106737659 B CN106737659 B CN 106737659B
Authority
CN
China
Prior art keywords
mrow
mmultiscripts
mover
msub
manipulator
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
CN201611009723.3A
Other languages
English (en)
Other versions
CN106737659A (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.)
Harbin Engineering University
Original Assignee
Harbin Engineering University
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 Harbin Engineering University filed Critical Harbin Engineering University
Priority to CN201611009723.3A priority Critical patent/CN106737659B/zh
Publication of CN106737659A publication Critical patent/CN106737659A/zh
Application granted granted Critical
Publication of CN106737659B publication Critical patent/CN106737659B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1602—Programme controls characterised by the control system, structure, architecture
    • B25J9/1607—Calculation of inertia, jacobian matrixes and inverses
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1602—Programme controls characterised by the control system, structure, architecture
    • B25J9/1605—Simulation of manipulator lay-out, design, modelling of manipulator

Landscapes

  • Engineering & Computer Science (AREA)
  • Automation & Control Theory (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Physics & Mathematics (AREA)
  • Mathematical Physics (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Manipulator (AREA)

Abstract

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

Description

一种水下无人航行器和机械手系统的手艇协调控制方法
技术领域
本发明涉及一种水下无人航行器和机械手系统的手艇协调控制方法。
背景技术
随着海洋开发与研究的日益蓬勃开展，水下无人航行器搭载机械手在进行水下打捞、援潜救生、海底设施的维护与装置的回收、海底生物及岩石标本的采样等方面得到了越来越广泛地应用。
首先，虽然目前搭载机械手的水下航行器通过遥控操作可以完成大量的作业工作，但其在结构化环境中的作业能力等方面存在较大缺陷，作业范围也受到较大限制，并且在水下机器人的作业中如果单纯依赖人的遥控，将难以实现海流扰动中机械手末端的准确定位和作业。专利文献“一种ROV水下机器人的运动控制方法(CN105404303A)”和“浮游式水下机器人的S面控制方法(CN1718378A)”，涉及到的水下机器人运动控制方法，虽然也可以完成水下机器人的运动控制，但由于作业过程中机械手的运动会给航行器带来较大的扰动作用，并且扰动作用力根据作业姿态随时变化，所以上述根据干扰误差反馈进行补偿的控制方式难以实现作业过程中的航行器稳定控制。
发明内容
本发明的目的是为了提供一种水下无人航行器和机械手系统的手艇协调控制方法，能够通过航行器和机械手当前的位置和姿态的反馈实时估计作业过程中水下无人航行器所受到的机械手的扰动力，并在控制中进行补偿，从而提高作业精度。该方法用于水下无人航行器的遥控作业可以减小操作人员的操作时间，提高遥控精度，用于水下无人航行器的自主作业可以提高控制精度，尤其是机械手末端的控制精度，实现航行器的自主作业。
本发明的目的是这样实现的：包括水下无人航行器控制系统和协调控制器，
步骤一、协调运动控制器通过隔离串口板采集水下无人航行器控制系统中的多普勒测速仪和磁罗经的数据，并通过水下无人航行器控制系统中的PC104模块进行船位推算，获得水下无人航行器当前位姿，通过水下无人航行器控制系统中的can数据采集板、机械手直流伺服电机驱动器和编码器获取机械手各关节当前姿态；
步骤二、建立扰动力观测器，并设计协调控制器的两条控制率；
步骤三、通过协调控制器的两条控制率控制并控制水下无人航行器和机械手的运动。
本发明还包括这样一些结构特征：
1.步骤二中所述建立扰动力观测器是机械手对航行器的扰动力观测器，其中，倾斜力矩观测公式是：
式中，和分别为机械手第i个广义连杆相对于水下无人航行器的重力和浮力的矩阵表达形式，0RV为艇体坐标系到机械手基座坐标系的转换矩阵 和分别为机械手第i个广义连杆的重心和浮心相对于水下无人航行器的位置，而和分别为机械手艇体重心和浮心相对于水下无人航行器的位置，mig和FBi分别为机械手第i个广义连杆相对于水下无人航行器的重力和浮力；
机械手和水下无人航行器的耦合运动造成的耦合力的观测公式是：
式中：0f1,0和0n1,0是水下无人航行器在手臂基关节受到的机械手的耦合作用力，1f1,0和1n1,0是水下无人航行器在手臂基座受到的机械手的耦合作用力，0R1是机械手基关节的随动坐标系到机械手基座坐标系的转换矩阵，0f1,0和0n1,0将由下式递推得到：
式中：ifi,i-1和ini,i-1为第i个广义连杆在其第i关节对第i-1关节的约束反力，iri和irci分别为机械手第i个广义连杆在其坐标系下的位置矢量和中心位置矢量，iFi,i和iMi,i分别为机械手第i个广义连杆在水中运动所受到的流体作用力，ifi *和分别为机械手第i个广义连杆质心的惯性力和力矩，可以通过牛顿-欧拉方程递推得到；
基于倾斜力矩观测公式、机械手和水下无人航行器的耦合运动造成的耦合力的观测公式得出扰动力矩的观测公式是：
2.所设计协调运动控制器是水下无人航行器和机械手的协调运动控制器，包括运动学控制律和动力学控制律两部分。
3.所述动力学控制率为：
式中：代表质量、惯性以及附加质量的观测矩阵，代表由质量和附加质量引起的科氏力的观测矩阵，代表由流体粘性引起的阻力的观测矩阵，Kd和Kp代表微分系数和比例系数，q代表实际位置量，qd代表期望位置量，代表位置偏差。
与现有技术相比，本发明的有益效果是：本发明根据水下机器人精确作业控制的需要设计一种水下无人航行器和机械手系统的手艇协调运动控制方法，通过测量和估计作业过程中水下无人航行器所受到的机械手的扰动力，并进行补偿，保证水下无人航行器的平稳运动，实现精确的定位和作业控制。该发明具有控制准确、适应性强、鲁棒性高等优点。
附图说明
图1是水下无人航行器控制系统的内部硬件系统框图；
图2是水下无人航行器协调控制的流程图；
图3是水下无人航行器和机械手的协调运动控制器框图；
图4(a)至图4(d)分别是无协调控制时观测到机械手的各关节位移、艇体产生的纵摇和横摇、机械手运动产生的倾斜力矩以及机械手运动产生的耦合力对水下无人航行器的干扰力；
图5是应用本发明对水下无人航行器和机械手协调运动的控制结果；
图6(a)至图6(f)分别是观测得到的协调运动过程中机械手各关节位移、艇体姿态、机械手运动中观测到的扰动力、机械手运动中观测到的扰动力矩、机械手运动中产生的耦合力、机械手运动中产生的倾斜力矩对水下无人航行器的扰动力；
具体实施方式
下面结合附图与具体实施方式对本发明作进一步详细描述。
本发明为：
a、水下无人航行器机械手系统的手艇协调控制方法由水下无人航行器控制系统和协调控制器两部分组成。水下无人航行器控制系统主要由PC104核心模块、I/O板、can数据采集板、隔离串口板、数据采集板、直流伺服电机控制板、机械手直流伺服电机驱动器、编码器、机械手电机、推进器螺旋桨及其电机驱动器、磁罗经、深度计、多普勒测速仪组成。协调控制器通过隔离串口板采集多普勒测速仪和磁罗经的数据，并通过PC104模块进行船位推算，获得水下无人航行器当前位姿；进而建立扰动力观测器，并设计协调控制器控制机械手的运动；通过can数据采集板、机械手直流伺服电机驱动器和编码器获取机械手各关节当前姿态，并控制机械手的运动；通过直流伺服电机控制板、推进器螺旋桨及其电机驱动器控制水下无人航行器螺旋桨的运动。
b、所建立扰动力观测器为机械手对航行器的扰动力观测器。扰动力主要包括机械手和水下无人航行器的耦合运动造成的耦合力，作业过程中系统浮心和重心变化派生的倾斜力矩两部分。其中倾斜力矩可以结合水下无人航行器和机械手广义连杆的当前位置姿态，通过下面的公式来观测：
式中，和分别为机械手第i个广义连杆相对于水下无人航行器的重力和浮力的矩阵表达形式，0RV为艇体坐标系到机械手基座坐标系的转换矩阵； 和分别为机械手第i个广义连杆的重心和浮心相对于水下无人航行器的位置，而和分别为机械手艇体重心和浮心相对于水下无人航行器的位置，mig和FBi分别为机械手第i个广义连杆相对于水下无人航行器的重力和浮力。机械手和水下无人航行器的耦合运动造成的耦合力可以通过下面的公式来观测：
式中0f1,0和0n1,0是水下无人航行器在手臂基座受到的机械手的耦合作用力，1f1,0和1n1,0是水下无人航行器在手臂基座受到的机械手的耦合作用力。0R1是机械手基关节的随动坐标系到机械手基座坐标系的转换矩阵。0f1,0和0n1,0将由下式递推得到：
式中ifi,i-1和ini,i-1为第i个广义连杆在其第i关节对第i-1关节的约束反力(力矩)。iri和irci分别为机械手第i个广义连杆在其坐标系下的位置矢量和中心位置矢量。iFi,i和iMi,i分别为机械手第i个广义连杆在水中运动所受到的流体作用力。ifi *和分别为机械手第i个广义连杆质心的惯性力和力矩，可以通过牛顿-欧拉方程递推得到。
由于航行器受到的扰动力(矩)主要由上述两种组成，所以扰动力矩可按如下公式进行观测：
c、所设计协调控制器为水下无人航行器和机械手的协调运动控制器。协调控制器包括运动学控制律和动力学控制律两部分。将专利文献“基于浮游式水下机器人的S面控制方法(CN1718378A)”作为运动学控制律，主要作用是保证水下无人航行器和机械手的协调控制器全局收敛；在动力学控制律的设计中，将基于系统的动力学模型，观测水下无人航行器在作业过程中受到的机械手扰动力，并控制推进器运动进行补偿，实现作业过程中航行器稳定和精确控制。协调控制器如下：
式中：代表质量，惯性以及附加质量的观测矩阵,代表由质量和附加质量引起的科氏力的观测矩阵，代表由流体粘性引起的阻力的观测矩阵，Kd和Kp代表微分系数和比例系数，q代表实际位置量，qd代表期望位置量，代表位置偏差。
下面结合附图进行说明：
结合图1，水下无人航行器控制系统的核心是PC104核心模块，PC104模块通过隔离串口板采集多普勒测速仪和磁罗经的数据，并进行船位推算，获得水下无人航行器当前位姿；PC104模块通过can数据采集板、由机械手直流伺服电机驱动器和编码器获取机械手各关节的当前姿态，并发送控制指令控制机械手的运动；PC104模块根据协调控制器的结果，通过直流伺服电机控制板向电机驱动器发送控制指令，控制水下无人航行器螺旋桨运动实现作业过程中水下无人航行器的稳定精确运动。
结合图2，水下无人航行器机械手系统的手艇协调控制的流程是：首先，传感器采集航行器位姿和机械手的运动姿态数据；然后，通过运动学控制律调整位置偏差使其收敛，同时，扰动力观测器按照式(4)观测扰动力，并通过动力学控制律对其进行补偿；最后，两种控制律通过推进器同时作用在航行器上，传感器获取航行器当前时刻运动姿态数据，继续进行下一轮的协调控制。
结合图3协调控制器将S面控制方法(CN1718378A)作为运动学控制律，以保证水下无人航行器和机械手的协调控制器全局收敛；并基于系统的动力学模型，观测水下无人航行器在作业过程中受到的机械手扰动力，并控制推进器运动进行补偿，实现作业过程中航行器稳定和精确控制。
结合图4(a)至图4(d)，在没有经过协调控制器的实时控制条件下，机械手三个关节的运动对艇体产生了较大的倾斜力和耦合力矩，其结果是对艇造成了较大的横倾和纵倾。
结合图5，水下无人航行器机械手系统按照轨迹规划的对航行器和机械手各关节的规划轨迹，通过协调控制精确地实现了航行器和机械手末端的位姿控制，说明控制器可以有效地应用于在水下无人航行器机械手系统的水下作业精确控制。
结合图6(a)至图6(f)，表示经过动态控制后，观测到的机械手产生的干扰力(矩)，与图4对比可发现，协调控制器显著地减小了倾斜力(矩)对艇体的影响，较好地控制了机械手运动造成的航行器本体在横倾和纵倾方向的运动。
综上，本发明提供的是水下无人航行器机械手系统(Underwater VehicleManipulator System-UVMS)的手艇协调控制方法。用以实现水下无人航行器的稳定的高精度作业。为实现上述目的，水下无人航行器和机械手系统的手艇协调控制方法包括：通过PC104、数据采集板、机械手电机、螺旋桨推进器、磁罗经、多普勒测速仪等设备构建水下无人航行器控制系统，获取系统各自由度的姿态信息，同时进行硬件层面的控制；根据航行器和机械手的位置姿态建立内部扰动力观测器，依据牛顿-欧拉方法观测各时刻因系统姿态变化产生的倾斜力矩和耦合力矩；建立航行器和机械手的协调控制器，在控制中对航行器作业过程中受到的机械手扰动力进行补偿，实现作业过程中航行器稳定和精确控制。该方法可实际应用于UVMS控制系统设计，对于水下机器人在自主作业和遥控作业方面具有重要意义。

Claims (2)

1.一种水下无人航行器和机械手系统的手艇协调控制方法，其特征在于：包括水下无人航行器控制系统和协调控制器，
步骤一、协调控制器通过隔离串口板采集水下无人航行器控制系统中的多普勒测速仪和磁罗经的数据，并通过水下无人航行器控制系统中的PC104模块进行船位推算，获得水下无人航行器当前位姿，通过水下无人航行器控制系统中的can数据采集板、机械手直流伺服电机驱动器和编码器获取机械手各关节当前姿态；
协调控制器是水下无人航行器和机械手的协调运动控制器，包括运动学控制律和动力学控制律两部分；
步骤二、建立扰动力观测器，并设计协调控制器的两条控制律；
所述建立扰动力观测器是机械手对航行器的扰动力观测器，其中，倾斜力矩观测公式是：
<mrow> <mi>g</mi> <mrow> <mo>(</mo> <mi>q</mi> <mo>)</mo> </mrow> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <mmultiscripts> <mi>f</mi> <mi>g</mi> <mi>V</mi> <mi>V</mi> </mmultiscripts> <mi>+</mi> <mmultiscripts> <mi>f</mi> <mi>B</mi> <mi>V</mi> <mi>V</mi> </mmultiscripts> <mo>+</mo> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>n</mi> </munderover> <mrow> <mo>(</mo> <mmultiscripts> <mi>f</mi> <mi>g</mi> <mi>i</mi> <mi>V</mi> </mmultiscripts> <mo>+</mo> <mmultiscripts> <mi>f</mi> <mi>B</mi> <mi>i</mi> <mi>V</mi> </mmultiscripts> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mmultiscripts> <mi>r</mi> <mi>g</mi> <mi>V</mi> <mi>V</mi> </mmultiscripts> <mo>&amp;times;</mo> <mmultiscripts> <mi>f</mi> <mi>g</mi> <mi>V</mi> <mi>V</mi> </mmultiscripts> <mo>+</mo> <mmultiscripts> <mi>r</mi> <mi>B</mi> <mi>V</mi> <mi>V</mi> </mmultiscripts> <mo>&amp;times;</mo> <mmultiscripts> <mi>f</mi> <mi>B</mi> <mi>V</mi> <mi>V</mi> </mmultiscripts> <mo>+</mo> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>n</mi> </munderover> <mrow> <mo>(</mo> <mmultiscripts> <mi>r</mi> <mi>g</mi> <mi>i</mi> <mi>V</mi> </mmultiscripts> <mo>&amp;times;</mo> <mmultiscripts> <mi>f</mi> <mi>g</mi> <mi>i</mi> <mi>V</mi> </mmultiscripts> <mo>+</mo> <mmultiscripts> <mi>r</mi> <mi>B</mi> <mi>i</mi> <mi>V</mi> </mmultiscripts> <mo>&amp;times;</mo> <mmultiscripts> <mi>f</mi> <mi>B</mi> <mi>i</mi> <mi>V</mi> </mmultiscripts> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> </mtable> </mfenced> </mrow>
式中，和分别为机械手第i个广义连杆相对于水下无人航行器的重力和浮力的矩阵表达形式，0RV为艇体坐标系到机械手基座坐标系的转换矩阵 和分别为机械手第i个广义连杆的重心和浮心相对于水下无人航行器的位置，而和分别为机械手艇体重心和浮心相对于水下无人航行器的位置，mig和FBi分别为机械手第i个广义连杆相对于水下无人航行器的重力和浮力；
机械手和水下无人航行器的耦合运动造成的耦合力的观测公式是：
<mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <msub> <mmultiscripts> <mi>f</mi> <mn>0</mn> </mmultiscripts> <mrow> <mn>1</mn> <mo>,</mo> <mn>0</mn> </mrow> </msub> <mo>=</mo> <msub> <mmultiscripts> <mi>R</mi> <mn>0</mn> </mmultiscripts> <mn>1</mn> </msub> <msub> <mmultiscripts> <mi>f</mi> <mn>1</mn> </mmultiscripts> <mrow> <mn>1</mn> <mo>,</mo> <mn>0</mn> </mrow> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mmultiscripts> <mi>n</mi> <mn>0</mn> </mmultiscripts> <mrow> <mn>1</mn> <mo>,</mo> <mn>0</mn> </mrow> </msub> <mo>=</mo> <msub> <mmultiscripts> <mi>R</mi> <mn>0</mn> </mmultiscripts> <mn>1</mn> </msub> <msub> <mmultiscripts> <mi>n</mi> <mn>1</mn> </mmultiscripts> <mrow> <mn>1</mn> <mo>,</mo> <mn>0</mn> </mrow> </msub> </mrow> </mtd> </mtr> </mtable> </mfenced>
式中：0f1,0和0n1,0是水下无人航行器在手臂基关节受到的机械手的耦合作用力，1f1,0和1n1,0是水下无人航行器在手臂基座受到的机械手的耦合作用力，0R1是机械手基关节的随动坐标系到机械手基座坐标系的转换矩阵，0f1,0和0n1,0将由下式递推得到：
<mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <msub> <mmultiscripts> <mi>f</mi> <mi>i</mi> </mmultiscripts> <mrow> <mi>i</mi> <mo>,</mo> <mi>i</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <msub> <mmultiscripts> <mi>f</mi> <mi>i</mi> </mmultiscripts> <mrow> <mi>i</mi> <mo>+</mo> <mn>1</mn> <mo>,</mo> <mi>i</mi> </mrow> </msub> <mo>-</mo> <mmultiscripts> <mi>f</mi> <mi>i</mi> <mo>*</mo> <mi>i</mi> </mmultiscripts> <mo>-</mo> <msub> <mmultiscripts> <mi>F</mi> <mi>i</mi> </mmultiscripts> <mrow> <mi>i</mi> <mo>,</mo> <mi>i</mi> </mrow> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mmultiscripts> <mi>n</mi> <mi>i</mi> </mmultiscripts> <mrow> <mi>i</mi> <mo>,</mo> <mi>i</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <msub> <mmultiscripts> <mi>n</mi> <mi>i</mi> </mmultiscripts> <mrow> <mi>i</mi> <mo>+</mo> <mn>1</mn> <mo>,</mo> <mi>i</mi> </mrow> </msub> <mo>+</mo> <msub> <mmultiscripts> <mi>r</mi> <mi>i</mi> </mmultiscripts> <mrow> <mi>c</mi> <mi>i</mi> </mrow> </msub> <mo>&amp;times;</mo> <msub> <mmultiscripts> <mi>f</mi> <mi>i</mi> </mmultiscripts> <mrow> <mi>i</mi> <mo>,</mo> <mi>i</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>-</mo> <mmultiscripts> <mi>n</mi> <mi>i</mi> <mo>*</mo> <mi>i</mi> </mmultiscripts> <mo>-</mo> <msub> <mmultiscripts> <mi>M</mi> <mi>i</mi> </mmultiscripts> <mrow> <mi>i</mi> <mo>,</mo> <mi>i</mi> </mrow> </msub> <mo>+</mo> <mrow> <mo>(</mo> <msub> <mmultiscripts> <mi>r</mi> <mi>i</mi> </mmultiscripts> <mi>i</mi> </msub> <mo>-</mo> <msub> <mmultiscripts> <mi>r</mi> <mi>i</mi> </mmultiscripts> <mrow> <mi>c</mi> <mi>i</mi> </mrow> </msub> <mo>)</mo> </mrow> <mo>&amp;times;</mo> <mrow> <mo>(</mo> <msub> <mmultiscripts> <mi>f</mi> <mi>i</mi> </mmultiscripts> <mrow> <mi>i</mi> <mo>+</mo> <mn>1</mn> <mo>,</mo> <mi>i</mi> </mrow> </msub> <mo>+</mo> <msub> <mmultiscripts> <mi>F</mi> <mi>i</mi> </mmultiscripts> <mrow> <mi>i</mi> <mo>,</mo> <mi>i</mi> </mrow> </msub> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> </mtable> </mfenced>
式中：ifi,i-1和ini,i-1为第i个广义连杆在其第i关节对第i-1关节的约束反力，iri和irci分别为机械手第i个广义连杆在其坐标系下的位置矢量和中心位置矢量，iFi,i和iMi,i分别为机械手第i个广义连杆在水中运动所受到的流体作用力，ifi *和分别为机械手第i个广义连杆质心的惯性力和力矩，可以通过牛顿-欧拉方程递推得到；
基于倾斜力矩观测公式、机械手和水下无人航行器的耦合运动造成的耦合力的观测公式得出扰动力矩的观测公式是：
<mrow> <msub> <mover> <mi>F</mi> <mo>^</mo> </mover> <mrow> <mi>d</mi> <mi>i</mi> <mi>s</mi> </mrow> </msub> <mrow> <mo>(</mo> <mi>q</mi> <mo>)</mo> </mrow> <mo>=</mo> <mover> <mi>g</mi> <mo>^</mo> </mover> <mrow> <mo>(</mo> <mi>q</mi> <mo>)</mo> </mrow> <mo>+</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <msub> <mmultiscripts> <mover> <mi>f</mi> <mo>^</mo> </mover> <mi>V</mi> </mmultiscripts> <mrow> <mn>1</mn> <mo>,</mo> <mi>V</mi> </mrow> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mmultiscripts> <mover> <mi>n</mi> <mo>^</mo> </mover> <mi>V</mi> </mmultiscripts> <mrow> <mn>1</mn> <mo>,</mo> <mi>V</mi> </mrow> </msub> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <mmultiscripts> <mover> <mi>f</mi> <mo>^</mo> </mover> <mi>g</mi> <mi>V</mi> <mi>V</mi> </mmultiscripts> <mi>+</mi> <mmultiscripts> <mover> <mi>f</mi> <mo>^</mo> </mover> <mi>B</mi> <mi>V</mi> <mi>V</mi> </mmultiscripts> <mo>+</mo> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mn>4</mn> </munderover> <mrow> <mo>(</mo> <mmultiscripts> <mover> <mi>f</mi> <mo>^</mo> </mover> <mi>g</mi> <mi>i</mi> <mi>V</mi> </mmultiscripts> <mo>+</mo> <mmultiscripts> <mover> <mi>f</mi> <mo>^</mo> </mover> <mi>B</mi> <mi>i</mi> <mi>V</mi> </mmultiscripts> <mo>)</mo> </mrow> <mo>+</mo> <msub> <mmultiscripts> <mover> <mi>f</mi> <mo>^</mo> </mover> <mi>V</mi> </mmultiscripts> <mrow> <mn>1</mn> <mo>,</mo> <mi>V</mi> </mrow> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mmultiscripts> <mover> <mi>r</mi> <mo>^</mo> </mover> <mi>g</mi> <mi>V</mi> <mi>V</mi> </mmultiscripts> <mo>&amp;times;</mo> <mmultiscripts> <mi>f</mi> <mi>g</mi> <mi>V</mi> <mi>V</mi> </mmultiscripts> <mo>+</mo> <mmultiscripts> <mover> <mi>r</mi> <mo>^</mo> </mover> <mi>B</mi> <mi>V</mi> <mi>V</mi> </mmultiscripts> <mo>&amp;times;</mo> <mmultiscripts> <mover> <mi>f</mi> <mo>^</mo> </mover> <mi>B</mi> <mi>V</mi> <mi>0</mi> </mmultiscripts> <mo>+</mo> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>4</mi> </munderover> <mrow> <mo>(</mo> <mmultiscripts> <mover> <mi>r</mi> <mo>^</mo> </mover> <mi>g</mi> <mi>i</mi> <mi>V</mi> </mmultiscripts> <mo>&amp;times;</mo> <mmultiscripts> <mover> <mi>f</mi> <mo>^</mo> </mover> <mi>g</mi> <mi>i</mi> <mi>V</mi> </mmultiscripts> <mo>+</mo> <mmultiscripts> <mover> <mi>r</mi> <mo>^</mo> </mover> <mi>B</mi> <mi>i</mi> <mi>V</mi> </mmultiscripts> <mo>&amp;times;</mo> <mmultiscripts> <mover> <mi>f</mi> <mo>^</mo> </mover> <mi>B</mi> <mi>i</mi> <mi>V</mi> </mmultiscripts> <mo>)</mo> </mrow> <mo>+</mo> <msub> <mmultiscripts> <mover> <mi>n</mi> <mo>^</mo> </mover> <mi>V</mi> </mmultiscripts> <mrow> <mn>1</mn> <mo>,</mo> <mi>V</mi> </mrow> </msub> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>;</mo> </mrow>
步骤三、通过协调控制器的两条控制律控制并控制水下无人航行器和机械手的运动。
2.根据权利要求1所述的一种水下无人航行器和机械手系统的手艇协调控制方法，其特征在于：所述动力学控制律为：
<mrow> <mi>&amp;tau;</mi> <mo>=</mo> <mover> <mi>M</mi> <mo>^</mo> </mover> <mrow> <mo>(</mo> <mi>q</mi> <mo>)</mo> </mrow> <mo>&amp;lsqb;</mo> <msub> <mover> <mi>q</mi> <mo>&amp;CenterDot;&amp;CenterDot;</mo> </mover> <mi>d</mi> </msub> <mo>+</mo> <mfrac> <mrow> <mn>2</mn> <mi>I</mi> </mrow> <mrow> <mo>(</mo> <mi>I</mi> <mo>+</mo> <mi>exp</mi> <mo>(</mo> <mo>-</mo> <msub> <mi>K</mi> <mi>d</mi> </msub> <mover> <mi>q</mi> <mo>~</mo> </mover> <mo>-</mo> <msub> <mi>K</mi> <mi>d</mi> </msub> <mover> <mover> <mi>q</mi> <mo>~</mo> </mover> <mo>&amp;CenterDot;</mo> </mover> <mo>)</mo> <mo>)</mo> </mrow> </mfrac> <mo>-</mo> <mi>I</mi> <mo>&amp;rsqb;</mo> <mo>+</mo> <mover> <mi>C</mi> <mo>^</mo> </mover> <mrow> <mo>(</mo> <mover> <mi>q</mi> <mo>&amp;CenterDot;</mo> </mover> <mo>)</mo> </mrow> <mover> <mi>q</mi> <mo>&amp;CenterDot;</mo> </mover> <mo>+</mo> <mover> <mi>D</mi> <mo>^</mo> </mover> <mrow> <mo>(</mo> <mover> <mi>q</mi> <mo>&amp;CenterDot;</mo> </mover> <mo>)</mo> </mrow> <mover> <mi>q</mi> <mo>&amp;CenterDot;</mo> </mover> <mo>+</mo> <msub> <mover> <mi>F</mi> <mo>^</mo> </mover> <mrow> <mi>d</mi> <mi>i</mi> <mi>s</mi> </mrow> </msub> <mrow> <mo>(</mo> <mi>q</mi> <mo>)</mo> </mrow> <mo>+</mo> <mo>+</mo> <msub> <mi>K</mi> <mi>d</mi> </msub> <mover> <mover> <mi>q</mi> <mo>~</mo> </mover> <mo>&amp;CenterDot;</mo> </mover> <mo>+</mo> <msub> <mi>K</mi> <mi>p</mi> </msub> <mover> <mi>q</mi> <mo>~</mo> </mover> </mrow>
式中：代表质量、惯性以及附加质量的观测矩阵，代表由质量和附加质量引起的科氏力的观测矩阵，代表由流体粘性引起的阻力的观测矩阵，Kd和Kp代表微分系数和比例系数，q代表实际位置量，qd代表期望位置量，代表位置偏差。
CN201611009723.3A 2016-11-15 2016-11-15 一种水下无人航行器和机械手系统的手艇协调控制方法 Active CN106737659B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201611009723.3A CN106737659B (zh) 2016-11-15 2016-11-15 一种水下无人航行器和机械手系统的手艇协调控制方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201611009723.3A CN106737659B (zh) 2016-11-15 2016-11-15 一种水下无人航行器和机械手系统的手艇协调控制方法

Publications (2)

Publication Number Publication Date
CN106737659A CN106737659A (zh) 2017-05-31
CN106737659B true CN106737659B (zh) 2017-11-21

Family

ID=58969334

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201611009723.3A Active CN106737659B (zh) 2016-11-15 2016-11-15 一种水下无人航行器和机械手系统的手艇协调控制方法

Country Status (1)

Country Link
CN (1) CN106737659B (zh)

Families Citing this family (15)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109839866A (zh) * 2017-11-29 2019-06-04 中国科学院沈阳自动化研究所 一种用于自主潜水器的探测载荷设备的管理系统及方法
CN108045530A (zh) * 2017-12-04 2018-05-18 国网山东省电力公司电力科学研究院 一种海底电缆探测水下机器人及作业方法
CN108045531A (zh) * 2017-12-04 2018-05-18 国网山东省电力公司电力科学研究院 用于海底电缆巡检的水下机器人控制系统及方法
CN108674613B (zh) * 2018-04-16 2020-02-14 哈尔滨工程大学 一种水下机器人重心辅助调节系统及控制方法
CN109062230B (zh) * 2018-08-06 2021-05-04 江苏科技大学 水下辅助采油机器人控制系统及动力定位方法
CN109366490A (zh) * 2018-12-10 2019-02-22 美钻深海能源科技研发（上海）有限公司 水下履带智能机器人总控单元
CN109558694B (zh) * 2018-12-26 2023-03-17 哈尔滨工程大学 一种水下机器人和机械手系统抓取运动过程的水动力分析方法
CN110032198A (zh) * 2019-04-30 2019-07-19 苏州蓝智特种机器人有限公司 一种水下特种机器人艇-手多体动力学建模方法及装置
CN110555254B (zh) * 2019-08-22 2023-06-06 天津大学 一种磁极分组降低永磁电机面外振动不稳定的方法
CN111136687B (zh) * 2019-12-31 2023-06-20 哈尔滨工程大学 一种水下机器人视觉控制目标抓取测试系统与方法
CN112318508A (zh) * 2020-08-14 2021-02-05 大连海事大学 一种水下机器人-机械手系统受海流扰动强弱评估方法
CN112327634B (zh) * 2020-11-27 2022-04-26 江苏科技大学 基于bp神经网络s面控制的水下机器人姿态控制方法
CN113459089B (zh) * 2021-06-09 2022-04-29 华中科技大学 一种面向水下无人艇-双机械臂作业系统的动力学耦合效应评价方法
CN113467501B (zh) * 2021-07-20 2023-03-28 福州大学 作业飞行机器人动态滑翔抓取与力位混合控制方法
CN115195974B (zh) * 2022-06-21 2024-01-30 广东海洋大学 水下航行器打捞作业机械手

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103240739A (zh) * 2013-05-07 2013-08-14 武汉科技大学 一种移动机械臂分散集中自主切换控制方法
CN103901776A (zh) * 2014-04-04 2014-07-02 哈尔滨工程大学 一种工业机械手抗干扰鲁棒自适应pid控制方法
CN105468020A (zh) * 2015-12-29 2016-04-06 天津海之声科技有限公司 基于分布式控制的水下机器人系统
WO2016120071A1 (en) * 2015-01-29 2016-08-04 Norwegian University Of Science And Technology (Ntnu) Underwater manipulator arm robot
US9452528B1 (en) * 2012-03-05 2016-09-27 Vecna Technologies, Inc. Controller device and method

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9452528B1 (en) * 2012-03-05 2016-09-27 Vecna Technologies, Inc. Controller device and method
CN103240739A (zh) * 2013-05-07 2013-08-14 武汉科技大学 一种移动机械臂分散集中自主切换控制方法
CN103901776A (zh) * 2014-04-04 2014-07-02 哈尔滨工程大学 一种工业机械手抗干扰鲁棒自适应pid控制方法
WO2016120071A1 (en) * 2015-01-29 2016-08-04 Norwegian University Of Science And Technology (Ntnu) Underwater manipulator arm robot
CN105468020A (zh) * 2015-12-29 2016-04-06 天津海之声科技有限公司 基于分布式控制的水下机器人系统

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
漂浮基空间机械臂T－S模糊鲁棒控制;洪昭斌等;《厦门理工学院学报》;20160229;第24卷(第1期);全文 *

Also Published As

Publication number Publication date
CN106737659A (zh) 2017-05-31

Similar Documents

Publication Publication Date Title
CN106737659B (zh) 一种水下无人航行器和机械手系统的手艇协调控制方法
CN107168312B (zh) 一种补偿uuv运动学和动力学干扰的空间轨迹跟踪控制方法
CN103955218B (zh) 一种基于非线性控制理论的无人艇轨迹跟踪控制装置及方法
Abbeel et al. Learning vehicular dynamics, with application to modeling helicopters
CN107957727B (zh) 水下机器人控制系统及动力定位方法
CN104076689B (zh) 一种全驱动式自主水下航行器协同控制方法
CN106527491B (zh) 一种固定翼无人机控制系统及横侧向飞行轨迹控制方法
Rodic et al. Control of a Quadrotor Flight
CN103942383B (zh) 一种深海作业型水下机器人的动力学和运动学估计方法
Rodić et al. Qualitative evaluation of flight controller performances for autonomous quadrotors
CN106227223A (zh) 一种基于动态滑模控制的uuv轨迹跟踪方法
CN109828570A (zh) 一种自适应边界层水面无人艇控制导引方法
CN105807789A (zh) 基于t-s模糊观测器补偿的uuv控制方法
CN104199303B (zh) 一种基于矢量场制导的平流层卫星平面路径跟踪控制方法
CN109974706A (zh) 一种基于双运动模型的主从式多auv协同导航方法
CN111857165B (zh) 一种水下航行器的轨迹跟踪控制方法
CN109946976B (zh) 一种宽航速auv运动控制方法
CN108121338B (zh) 一种usv的航迹闭环控制方法
CN109470248A (zh) 一种水下航行器导航系统及导航方法
CN107656530A (zh) 变参数开架式海洋水下机器人的轨迹跟踪控制方法、装置和系统
CN106896817A (zh) 一种基于粘滞阻尼振荡模型的多auv编队控制方法
CN107491083A (zh) 一种基于饱和自适应滑模控制的四旋翼自主着船方法
CN113093742A (zh) 一种自动规避多障碍物的无人船路径跟踪系统
CN108829132A (zh) 一种考虑大时滞的多uuv空间机动控制方法
CN110428715A (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