CN113878583B - 一种水下机械臂控制方法及系统 - Google Patents

Abstract

本发明公开了一种水下机械臂控制方法及系统，属于机械设备控制领域。现有的水下机械臂控制方法繁琐，位置控制精度低，不能快速精准地进行水下机械臂位置控制。本发明提出将水下机械臂在水流作用下所产生的末端变形融入到机械臂运动学模型中，使水下机械臂的运动学模型更加精确，从而能够实现水下机械臂的快速精准的位置控制。

Description

一种水下机械臂控制方法及系统

技术领域

本发明公开了一种水下机械臂控制方法及系统。

背景技术

水下机械臂的运动学问题主要研究的是机械臂末端执行器与机械臂关节变量之间的位置和方向关系，包括正运动学和逆运动学两个基本问题。机械臂正运动学方程可以用于机械臂的正向控制，而逆运动学是机械臂轨迹规划和运动控制的基础。所以说建立正确、准确的机械臂运动学模型对于机械臂的精准位置控制具有重要意义。

水下机械臂在靠近定点作业目标的过程中，存在着模型不确定性及水流干扰的情况。目前水下机械臂的位置控制方法主要是使用关节控制的方法对水下机械臂的关节角度进行控制，而关节角度需要通过运动学模型计算所得。

但是，目前的水下机械臂运动学模型的建立过程中都没有考虑机械臂结构受水流作用而产生的形变，这种形变在机械臂结构刚性较小，工作环境水流大的情况下更为明显，形变的出现势必会导致机械臂末端的位置出现偏移，位置偏移的存在会导致机械臂的位置精度的降低，这对于一些精度要求较高的场所影响较大，目前解决这种误差的主要方法是通过水下机械臂末端传感器的位置测量与控制算法相结合进行后置反馈调节，这一方法过于繁琐且响应迟缓。

发明内容

本发明是针对目前现有水下机械臂存在受水流作用力而产生的末端位置偏移的问题而提出的一种水下机械臂控制方法及系统，该方法和系统在建立机械臂运动学模型的过程中考虑到机械臂受水流作用而产生的形变，所建立的运动学模型的准确度大大提高，对后续的轨迹规划、精准的位置控制具有重要意义。

本发明采用的技术方案如下:

第一方面，本发明提出了一种水下机械臂控制方法，包括如下步骤：

步骤一：运用D-H法建立水下机械臂的运动学模型；

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

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

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

步骤五：将上一步所得到的末端位置偏移函数方程加入到水下机械臂运动学模型中，达到修正水下机械臂运动学模型的目的；

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

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

步骤八：将上述得到的转动角度下发至运动控制器，通过伺服驱动系统驱动各关机器转动相应角度，各连杆联动完后预定的运动轨迹，从而达到目标位置。

另一方面，本发明还提出了一种水下机械臂控制系统，包括

第一模块，被配置为建立水下机械臂的运动学模型；

第二模块，被配置为对水下机械臂的受力状况进行分析，得到各个力的值；

第三模块，被配置为对机械臂结构进行静力学分析，得到不同角度下水下机械臂末端的变形量；

第四模块，被配置为将所得的水下机械臂末端变形量数据进行曲线拟合，得到有关于关节角度与水下机械臂末端偏移量的函数方程；

第五模块，被配置为将得到的末端位置偏移函数方程加入到水下机械臂运动学模型中，达到修正水下机械臂运动学模型的目的；

第六模块，被配置为通过修正后的正运动学模型进行轨迹规划，根据曲线特点以及实际工作要求选取N个末端位置坐标P_i(x_i，y_i，z_i)；

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

第八模块：被配置为将得到的角度值θ_ij发送至运动控制器，通过伺服驱动系统驱动各关机器转动相应角度，各连杆联动完后预定的运动轨迹，从而达到目标位置。

本发明具有以下的优点与积极效果：

本发明所提出的通过修正水下机械臂运动学模型，从而达到主动，快速，精准的位置控制的方法，能够在控制时更快的响应以及精准位置控制，减少了后期位置反馈调节的时间，提高了水下机械臂末端位置精度，使水下机械臂能够更快，更准确的到达指定位置。

附图说明

图1：控制方法流程图；

图2：二连杆平面水下机械臂三维模型图；

图3：机械臂参考坐标系；

图4：水下机械臂受力分析图；

图5：静力学分析流程图；

图6：水下机械臂末端X向变形散点图；

图7：水下机械臂末端Y向变形散点图；

图8：水下机械臂末端X向变形拟合曲线图；

图9：水下机械臂末端Y向变形拟合曲线图；

图10：反函数拟合曲线图。

具体实施方式

应该指出，以下详细说明都是例示性的，旨在对本申请提供进一步的说明。除非另有指明，本实施例使用的所有技术和科学术语具有与本申请所属技术领域的普通技术人员通常理解的相同含义。

需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本申请的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。

为了解决现有技术中存在的技术问题，本实施例公开了一种水下机械臂控制方法，包括如下步骤：

步骤1：运用D-H法建立水下机械臂的运动学模型；

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

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

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

步骤5：将上一步所得到的末端位置偏移函数方程加入到水下机械臂运动学模型中，达到修正水下机械臂运动学模型的目的；

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

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

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

本发明以平面二连杆水下机械臂为对象，对所提出的步骤一至步骤五进行了解释说明。

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

1.1正运动学模型的建立

水下机械臂正运动学方程可以用于机械臂的正向控制，其对于轨迹规划与末端位置求解具有重要作用。

如附图2为二连杆水下机械臂模型图，由图可见，所建立的模型主要包括四部分，分别是基座、上臂、下臂以及关节销，每个关节均为旋转关节；对各个关节进行分析，根据D-H法则，建立如附图3所示的参考坐标系；将水下机械臂的D-H参数代入连杆齐次变换矩阵的一般表达式，可得到相邻连杆间的坐标变换矩阵：

由下式可计算出该机械臂的运动学方程，这样得到的末端坐标系到基坐标系的变换矩阵为

由于机械臂末端坐标是在坐标系{2}内，是在坐标系{2}的基础上平移a₂，不存在旋转变换，所以末端坐标到末端坐标系{2}的变换矩阵为：

那么可以由得到末端坐标相对于基座标系的位姿矩阵，也是此平面二连杆机械臂模型的运动学方程：

1.2逆运动学模型建立

逆运动学模型的建立对于水下机械臂的位置控制至关重要。对于附图2所示的水下机械臂，在已知末端坐标为(x₃，y₃)的情况下，结合正运动学方程可以得到以下两个个非线性方程，进而可以求出θ₁，θ₂。

x₃＝a₁ cosθ₁+a₂ cos(θ₁+θ₂)

y₃＝a₁ sinθ₁+a₂ sin(θ₁+θ₂)

将上面两个式子平方相加可以得到下式，

由上式求解cosθ₂，得到下式，

应用双变量反正切公式计算θ₁，得到下式，

θ₂＝Atan2(sinθ₂，cosθ₂)

最后，由下式便可以求得θ₁的值，

步骤2：水下机械臂受力分析

机械臂在水中工作时，其受到的力可以视为由两部分组成。第一部分是在静水环境下，因机械臂的主动运动搅动水导致的受力；第二部分是在水环境下，当机械臂以某个位姿静止不动时，仅受水流冲击的受力。在解释中，我们只分析机械臂在水流速度恒定的环境下，机械臂运动到不同位姿静止不动时的变形情况，所以此时的机械臂只受恒定水流冲击的力。

水下机械臂手受水流冲击作用时，流体会对水下机械臂表面产生一定的阻力，水下机械臂的连杆按照阻力的方向可以分解为法向阻力与切向阻力，因为机械臂的连杆是圆柱体，所以其切向阻力通常忽略不计。本发明中基于微元法和Morison公式对水下机械臂的单个连杆进行了受力分析，如附图4所示。

综合以上分析，在恒流环境下，处于静止状态的水下机械臂主要受水阻力，重力，浮力，阻力可以通过下式计算得到，

式中，F_di—连杆所受的水阻力，上杆为F_d1，下杆F_d2；ρ—水的密度；C_D—水阻系数，本实施例取C_D＝1.2；v—水流速度；A_i—连杆在垂直于来流速度v方向的投影面积，上连杆为A₁＝a₁Dcosθ₁，下杆为A₁＝a₂Dcos(θ₁+θ₂)，此处D为连杆的等效直径。

由于重力与浮力的方向是相反的，所以在假设连杆重心和浮心重合的情况下用一个向下的等效重力来代替重力和浮力，即：

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

步骤3：静力学分析

静力学分析一般应用于分析结构在承担恒定不变的载荷条件下的变形和应力情况。本部分要进行的是在ANSYS Workbench中利用Static Structural模块完成机械臂的静力学分析。静态分析的目的是观察不同关节角姿态下的机械臂，在受相对应的水阻力时末端位置不同的形变情况，静力学分析的流程如附图5所示。

步骤4：偏差方程拟合

经过前文所述的边界条件的添加与设置后，通过软件将37组不同姿态的模型进行仿真分析，得到了机械臂末端位置在X方向与Y方向的变形量，将所得的实验数据绘制成散点图，机械臂末端位置X方向的变形量如附图6所示，机械臂末端位置Y方向的变形量如附图7所示。

利用MATLAB的曲线拟合工具箱的曲线拟合功能对上述数据进行曲线拟合，在工具箱中选择了曲线形状与散点图近似的傅里叶拟合与三角函数拟合，综合考虑拟合优度、误差平方和公式复杂度后，X向变形的曲线的拟合方程选择为：

y＝1.14sin(2.316x-2.974)+45.95sin(0.02236x+3.137)

Y向变形的曲线的拟合方程的选择是：

y＝3.942+3.3cos(1.725x)-0.4679sin(1.725x)

这两个方程所对应的拟合曲线如附图8与图9所示。

步骤5：运动学模型修正

5.1、由前面所建立的平面二连杆机械臂的正运动学方程可知，在不受外力作用，结构没有变形的情况下，机械臂末端坐标为(x₃，y₃),此时坐标中的值分别为：

x₃＝a₁cosθ₁+a₂cos(θ₁+θ₂)

y₃＝a₁sinθ₁+a₂sin(θ₁+θ₂)

而当上臂转角θ₁＝0°不变，下臂转角θ₂∈[-90°,90°]时，机械臂在水中转动一定角度静止时，由于受外力作用，末端位置会产生形变，从而导致末端位置的坐标出现偏差。通过上述对仿真数据的处理，我们的到了末端X向偏移量和Y向偏移量与下臂转角θ₂的函数关系式，所以当机械臂在恒定水流为2m/s的环境下，当机械臂上臂转角保持θ₁＝0°不变，下臂转过一定角度θ₂时，机械臂末端真正的位置坐标应该为(x₃’，y₃’)，此时坐标中的值应该为：

x'₃＝1000+800cosθ₂+1.14sin(2.316θ₂-2.974)+45.95sin(0.02236θ₂+3.137)

y'₃＝800sinθ₂+3.942+3.3cos(1.725θ₂)-0.4679sin(1.725θ₂) (5-4)

5.2、上述中建立的运动学模型能够在已知末端坐标(x₃，y₃)的情况下，能够求得上下两臂的转角θ₁、θ₂。而在下文中的机械臂水下受力静力学分析中，我们设定上臂转角θ₁＝0，所以此时我们只要建立一个能够通过末端坐标求出下臂转角的关系式即可，则通过前面修正后的正运动学坐标方程中的式(5-4)的反函数方程就可求得下臂关节θ₂。

由于式(5-4)较为复杂，其反函数不易求解，所以此处我们将由式(5-4)计算得到的坐标值中的y₃’作为自变量，将下臂旋转角度θ₂作为因变量，利用软件MATLAB绘制散点图并进行插值函数曲线拟合，这样得到的图像便是反函数图像，其拟合优度R²＝1，拟合图像如图10所示。利用图窗中的数据游标便可以读取任意点的坐标值，坐标中y的值便是要求解的θ₂。

本申请的再一种典型的实施方式中还提出了一种控制系统，包括：第一模块，被配置为建立水下机械臂的运动学模型；第二模块，被配置为对水下机械臂的受力状况进行分析，得到各个力的值；第三模块，被配置为对机械臂结构进行静力学分析，得到不同角度下水下机械臂末端的变形量；第四模块，被配置为将所得的水下机械臂末端变形量数据进行曲线拟合，得到有关于关节角度与水下机械臂末端偏移量的函数方程；第五模块，被配置为将得到的末端位置偏移函数方程加入到水下机械臂运动学模型中，达到修正水下机械臂运动学模型的目的；第六模块，被配置为通过修正后的正运动学模型进行轨迹规划，根据曲线特点以及实际工作要求选取N个末端位置坐标P_i(x_i，y_i，z_i)；第七模块：被配置为将获取的N个点通过修正后的逆运动学模型进行运动学反解的计算；得到每一个轨迹坐标点对应的驱动关节所对应的角度值θ_ij；第八模块：被配置为将得到的角度值θ_ij发送至运动控制器，通过伺服驱动系统驱动各关机器转动相应角度，各连杆联动完后预定的运动轨迹，从而达到目标位置。

各个模块的执行的具体过程与前面方法部分每一步骤的过程对应，在此就不进行赘述了。

以上所述仅为本申请的优选实施例而已，并不用于限制本申请，对于本领域的技术人员来说，本申请可以有各种更改和变化。凡在本申请的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本申请的保护范围之内。

Claims (6)

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模块完成机械臂的静力学分析。

4.一种水下机械臂控制系统，执行权利要求1-3任一项所述的水下机械臂控制方法，其特征在于，包括：

第一模块，被配置为建立水下机械臂的运动学模型；

第二模块，被配置为对水下机械臂的受力状况进行分析，得到各个力的值；

第三模块，被配置为对机械臂结构进行静力学分析，得到不同角度下水下机械臂末端的变形量；

第四模块，被配置为将所得的水下机械臂末端变形量数据进行曲线拟合，得到有关于关节角度与水下机械臂末端偏移量的函数方程；

第五模块，被配置为将所述的函数方程加入到水下机械臂运动学模型中，达到修正水下机械臂运动学模型的目的；

第六模块，被配置为通过修正后的正运动学模型进行轨迹规划，根据曲线特点以及实际工作要求选取N个末端位置坐标P_i（x _i，y _i，z _i）；

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

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

5.如权利要求4所述的水下机械臂控制系统，其特征在于，第一模块被配置为运用D-H法建立水下机械臂的运动学模型。

6. 如权利要求4所述的水下机械臂控制系统，其特征在于，第一模块被配置为利用Static Structural模块完成机械臂的静力学分析。

Images