[发明专利]一种水下无人航行器和机械手系统的手艇协调控制方法

摘要

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

主权项

一种水下无人航行器和机械手系统的手艇协调控制方法，其特征在于：包括水下无人航行器控制系统和协调控制器，步骤一、协调控制器通过隔离串口板采集水下无人航行器控制系统中的多普勒测速仪和磁罗经的数据，并通过水下无人航行器控制系统中的PC104模块进行船位推算，获得水下无人航行器当前位姿，通过水下无人航行器控制系统中的can数据采集板、机械手直流伺服电机驱动器和编码器获取机械手各关节当前姿态；协调控制器是水下无人航行器和机械手的协调运动控制器，包括运动学控制律和动力学控制律两部分；步骤二、建立扰动力观测器，并设计协调控制器的两条控制律；所述建立扰动力观测器是机械手对航行器的扰动力观测器，其中，倾斜力矩观测公式是：g(q)=fgVV+fBVV+Σi=1n(fgiV+fBiV)rgVV×fgVV+rBVV×fBVV+Σi=1n(rgiV×fgiV+rBiV×fBiV)]]>式中，和分别为机械手第i个广义连杆相对于水下无人航行器的重力和浮力的矩阵表达形式，0RV为艇体坐标系到机械手基座坐标系的转换矩阵和分别为机械手第i个广义连杆的重心和浮心相对于水下无人航行器的位置，而和分别为机械手艇体重心和浮心相对于水下无人航行器的位置，mig和FBi分别为机械手第i个广义连杆相对于水下无人航行器的重力和浮力；机械手和水下无人航行器的耦合运动造成的耦合力的观测公式是：f01,0=R01f11,0n01,0=R01n11,0]]>式中：0f1,0和0n1,0是水下无人航行器在手臂基关节受到的机械手的耦合作用力，1f1,0和1n1,0是水下无人航行器在手臂基座受到的机械手的耦合作用力，0R1是机械手基关节的随动坐标系到机械手基座坐标系的转换矩阵，0f1,0和0n1,0将由下式递推得到：fii,i-1=fii+1,i-fi*i-Fii,inii,i-1=nii+1,i+rici×fii,i-1-ni*i-Mii,i+(rii-rici)×(fii+1,i+Fii,i)]]>式中：ifi,i‑1和ini,i‑1为第i个广义连杆在其第i关节对第i‑1关节的约束反力，iri和irci分别为机械手第i个广义连杆在其坐标系下的位置矢量和中心位置矢量，iFi,i和iMi,i分别为机械手第i个广义连杆在水中运动所受到的流体作用力，ifi*和分别为机械手第i个广义连杆质心的惯性力和力矩，可以通过牛顿‑欧拉方程递推得到；基于倾斜力矩观测公式、机械手和水下无人航行器的耦合运动造成的耦合力的观测公式得出扰动力矩的观测公式是：F^dis(q)=g^(q)+f^V1,Vn^V1,V=f^gVV+f^BVV+Σi=14(f^giV+f^BiV)+f^V1,Vr^gVV×fgVV+r^BVV×f^BV0+Σi=14(r^giV×f^giV+r^BiV×f^BiV)+n^V1,V;]]>步骤三、通过协调控制器的两条控制律控制并控制水下无人航行器和机械手的运动。