[发明专利]一种水下机械臂控制方法及系统

权利要求书

1.一种水下机械臂控制方法，其特征在于，包括如下步骤：

步骤1：建立水下机械臂的运动学模型；

步骤2：对水下机械臂的受力状况进行分析，得到各个力的值；

步骤2中基于微元法和Morison公式对水下机械臂的单个连杆进行了受力分析；

在恒流环境下，处于静止状态的水下机械臂的主要受水阻力，重力，浮力；

水阻力计算公式为：

式中，F_di—连杆所受的水阻力，ρ—水的密度，C_D—水阻系数，v—水流速度，A_i—连杆在垂直于来流速度v方向的投影面积，D为连杆的等效直径；

在连杆重心和浮心重合的情况下，水下机械臂的等效重力计算公式为：

其中，m—连杆的总质量，g—重力加速度，V—连杆的体积，ρ—水的密度，ρ_m—连杆材料的密度；

步骤3：对机械臂结构进行静力学分析，得到不同角度下水下机械臂末端的变形量；

步骤4：将所得的水下机械臂末端变形量数据进行曲线拟合，得到有关于关节角度与水下机械臂末端偏移量的函数方程；

步骤5：将所述的函数方程加入到水下机械臂运动学模型中，达到修正水下机械臂运动学模型的目的；

步骤6：通过修正后的正运动学模型进行轨迹规划，根据曲线特点以及实际工作要求选取N个末端位置坐标P_i（x_i，y_i，z_i）；

步骤7：将获取的N个点通过修正后的逆运动学模型进行运动学反解的计算；得到每一个轨迹坐标点对应的驱动关节所对应的角度值θ_ij；

步骤8：将得到的角度值θ_ij发送至运动控制器，通过伺服驱动系统驱动各关节转动相应角度，各连杆联动完成预定的运动轨迹，从而达到目标位置。

2.如权利要求1所述的水下机械臂控制方法，其特征在于，步骤1中运用D-H法建立水下机械臂的运动学模型。

3. 如权利要求1所述的水下机械臂控制方法，其特征在于，步骤3中利用StaticStructural模块完成机械臂的静力学分析。