CN108247638B - 多自由度旋转机械臂的控制方法 - Google Patents

Abstract

本发明公开了一种多自由度旋转机械臂的控制方法，采用此方法，既可以实现多自由度旋转机械臂自动到给定的三维空间坐标点抓取物体，又可以实现多自由度旋转机械臂机械爪在要求考虑机械爪抓取物体方向时和不要求考虑机械爪抓取物体方向时的直线运动。根据目标点在三维直角坐标系的坐标，添加约束条件并计算出机械臂各个肢节的姿态角度，使机械爪直接到达目标点，从而实现多自由度旋转机械爪定点自动抓取物体。本发明主要解决了多自由度旋转机械臂操作复杂的问题，简化了原有多自由度旋转机械臂的控制方法。

Description

多自由度旋转机械臂的控制方法

技术领域

本发明涉及一种机械臂的控制方法，尤其是一种旋转机械臂的控制方法。

背景技术

当下人工捕捞海参存在成本高、危险性高、人员短缺等问题，用机械臂代替人捕捞海参已成为必然的发展趋势。而目前工业用的机械臂大都属于第一代机械臂，即人工开环控制下的机械臂，且大多数机械臂只能完成单一重复性的工作。由于海底捕捞环境复杂、海参位置不固定、机械臂防水程度不够、抓取操作复杂等因素，第一代机械臂不能直接用于捕捞海参。为了满足水下捕捞作业的需求，需要对现有机械臂从结构、控制算法到控制系统进行改进。

现有由舵机旋转控制运动的机械臂大多以单舵机控制为主，即机械臂上所有舵机被独立控制。以主流的六自由度旋转机械臂来说，需要控制6路舵机。控制输入较多，且操作复杂，因为每调整一路舵机时，机械爪都会以不同的轴按不同的半径旋转，这样到达目标点之前需要对多个舵机进行反复调整，既影响工作效率，又不符合人们直观控制的思维习惯，即上、下、左、右、前、后直线调整。现有的流水线上的多自由度旋转机械臂大多以动作组的方式进行作业，本质是将动作预先编成指令进行存储，再让机械臂一遍遍读取指令进行动作。这种方式只适用于重复性较高，且目标点固定的场合，不适用于类似海参捕捞作业场景下人机交互和目标点不确定的情况。

发明内容

本发明要解决的技术问题是提供一种具有普适意义的多自由度旋转机械臂的控制方法，根据目标点在三维直角坐标系的坐标，添加约束条件并计算出机械臂各个肢节的姿态角度，使机械爪直接到达目标点，从而实现多自由度旋转机械爪定点自动抓取物体，即根据当前指令对前一时刻机械爪三维空间内的坐标进行调整，更新目标点坐标，从而实现直线运动控制。

本发明的技术方案是这样实现的：

多自由度旋转机械臂的控制方法，包括以下步骤，步骤S1：根据目标点在三维直角坐标系的坐标，添加约束条件计算出机械臂各个肢节的姿态角度；步骤S2：根据步骤S1所述姿态角度确定机械臂上各舵机的旋转角度；步骤S3：根据步骤S2所述的旋转角度确定各舵机PWM值；步骤S4：将步骤S3中所述各舵机PWM值的数值信号同时发送给各舵机，使各舵机转动到步骤S1中所述的姿态角度，实现机械爪中心与目标点的重合，即实现机械爪到给定的三维空间坐标点的作业。

较佳的，所述步骤S1中添加的约束条件有两种，分别为机械臂臂长相等的第一约束条件和机械臂臂长不相等且无明显规律的第二约束条件。

较佳的，所述多自由度旋转机械臂有N路舵机(N≥6)，其中舵机N-1控制机械爪张合，舵机N-2控制机械爪旋转，舵机N-N控制底盘旋转，其余舵机控制竖直平面内机械臂肢节运动；所述三维直角坐标系的坐标以舵机N-(N-1)旋转轴中心为坐标原点O，坐标系的XOY平面始终与机械臂底座平面平行；舵机N-(N-2)和舵机N-(N-1)旋转轴距离OD₁为L₁，舵机N-(N-3)和舵机N-(N-2)旋转轴距离D₁D₂为L₂，以此类推，机械爪中心到舵机N-3旋转轴距离D_nD_n-1为L_n。

较佳的，机械臂臂长相等的第一约束条件下，即L_i＝L_n+1-i(1≤i≤n-1,i为整数),当n为奇数时，在OD_n线段上取n-1个点H₁、H₂、……H_n-1，使

于H_i(1≤i≤n-1,i为整数)；当n为偶数时，在OD_n线段上取n-1个点H₁、H₂、……H_n-1，使

于H_i(1≤i≤n-1,i为整数)。

较佳的，机械臂臂长不相等且无明显规律时的第二约束条件下，在多边形OD₁D₂…D_n中连接D_n与其他不相邻顶点可将该多边形分割成互不重叠的n-1个三角形，形成有线段D_nD₁，D_nD₂，…，D_nD_n-2，且每段线段长度为：D_nD_i＝k_i×D_nD_i-1，其中

1≤i≤n-2,i为整数，i＝1时D₀即为原点O。

技术方案中所述的舵机采用PWM脉冲宽度调节舵机旋转角度，周期20ms，0.5ms～2.5ms的脉宽高电平对应舵机0度～180度角度范围，且成线性关系。舵机驱动控制板采用500～2500数值对应舵机控制输出角度的0.5ms～2.5ms高电平脉冲，舵机驱动板数值与舵机旋转的角度有如下关系：θ为舵机旋转角度，PWM为舵机驱动板数值，θ＝0.09×PW M-45，这样舵机的控制精度是3μs，在2000个脉宽范围内最小控制精度能达到0.3度。

本发明的有益效果在于：

1、通过添加约束条件计算机械臂各个肢节的姿态角度，并通过同时给各舵机发送其PWM值的数值信号使各舵机能协同工作，实现了机械爪中心到目标点的快速平滑移动。

2、通过对约束条件的改变，用同一种技术方案解决了机械臂臂长相等和不相等两种不同情形下的不同问题。

3、以N(N≥6)作为机械臂舵机的数量给出约束条件和相应参数，为舵机数大于6的各种机械臂控制方法，提供了统一的解决方案，便于对多种机械臂的统一设置与操作。

附图说明

附图1六自由度旋转机械臂舵机及建系示意图。

附图2七自由度旋转机械臂舵机及建系示意图。

附图3六自由度旋转机械臂第一约束条件下模型简化示意图。

附图4七自由度旋转机械臂第一约束条件下模型简化示意图。

附图5六自由度旋转机械臂第二约束条件下模型简化示意图。

附图6七自由度旋转机械臂第二约束条件下模型简化示意图。

具体实施方式

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

如附图1、2、3、4、5、6所示，多自由度旋转机械臂的控制方法，包括以下步骤，步骤S1：根据目标点在三维直角坐标系的坐标，添加约束条件计算出机械臂各个肢节的姿态角度；步骤S2：根据步骤S1所述姿态角度确定机械臂上各舵机的旋转角度；步骤S3：根据步骤S2所述的旋转角度确定各舵机PWM值；步骤S4：将步骤S3中所述各舵机PWM值的数值信号同时发送给各舵机，使各舵机转动到步骤S1中所述的姿态角度，实现机械爪中心与目标点的重合，即实现机械爪到给定的三维空间坐标点的作业。

如附图3、4、5、6所示，所述步骤S1中添加的约束条件有两种，分别为机械臂臂长相等的第一约束条件和机械臂臂长不相等且无明显规律的第二约束条件。

如附图1、2所示，所述多自由度旋转机械臂有N路舵机(N≥6)，其中舵机N-1控制机械爪张合，舵机N-2控制机械爪旋转，舵机N-N控制底盘旋转，其余舵机控制竖直平面内机械臂肢节运动；所述三维直角坐标系的坐标以舵机N-(N-1)旋转轴中心为坐标原点O，坐标系的XOY平面始终与机械臂底座平面平行；舵机N-(N-2)和舵机N-(N-1)旋转轴距离OD₁为L₁，舵机N-(N-3)和舵机N-(N-2)旋转轴距离D₁D₂为L₂，以此类推，机械爪中心到舵机N-3旋转轴距离D_nD_n-1为L_n。当目标点已知为(x₀,y₀,z₀)时，即D_n点坐标为(x₀,y₀,z₀)，将此坐标转换为三维空间内的极坐标(r₀,θ₀,φ₀)(r₀≤L₁+L₂+L₃)。此时，多边形OD₁D₂…D_n形状不固定，每一种形状都对应着机械臂的机械爪到达目标点时的姿态。有无数种多边形OD₁D₂…D_n的形状，也有无数种抓取姿态，无法进行相应的姿态计算和姿态控制。所以，我们需要添加约束条件，使多边形OD₁D₂…D_n全约束，即每个目标点的对应姿态唯一确定，这样才能对其进行计算控制。

如附图3、4所示，机械臂臂长相等的第一约束条件下，即L_i＝L_n+1-i(1≤i≤n-1,i为整数),当n为奇数时，在OD_n线段上取n-1个点H₁、H₂、……H_n-1，使

于H_i(1≤i≤n-1,i为整数)；当n为偶数时，在OD_n线段上取n-1个点H₁、H₂、……H_n-1，使

于H_i(1≤i≤n-1,i为整数)。此时，多边形OD₁D₂…D_n全约束，且线段D_nO对称轴即为线段

所在直线。当多边形OD₁D₂…D_n全约束后，可以求出控制竖直平面内机械臂肢节运动的舵机的旋转角度，即多边形OD₁D₂…D_n的各个内角。舵机N-N旋转角度即为φ₀。根据舵机N-3到舵机N-N的旋转角度，便可以确定这N-2个舵机的PWM值，将信号同时发送给这N-2个舵机，使其各自转动到对应的理论计算角度，此时机械爪中心与目标点重合，实现了多自由度旋转机械臂自动到给定的三维空间坐标点的作业。

如附图5、6所示，机械臂臂长不相等且无明显规律时的第二约束条件下，在多边形OD₁D₂…D_n中连接D_n与其他不相邻顶点可将该多边形分割成互不重叠的n-1个三角形，并对连接的这些线段的长度加以约束，使各个三角形都全约束，那么多边形OD₁D₂…D_n也被全约束。分割为三角形的方案有多种，由于∠D_n-1D_nO不涉及舵机旋转角度，也不用计算其大小，为了简化后期计算，连接D_n与其他不相邻顶点，则有线段D_nD₁，D_nD₂，…，D_nD_n-2，且每段线段长度为：D_nD_i＝k_i×D_nD_i-1，其中

1≤i≤n-2,i为整数，i＝1时D₀即为原点O。这里k_i的选择是有原因的，因为当r₀＝L₁+L₂+…+L_n时，即机械臂机械爪要达到最大工作半径，只有k_i这样取值才能使机械臂每个肢节完全伸展开。此种方案还有隐形的限制条件，即D_nD_i-1+D_nD_i≥D_i-1D_i,(1≤i≤n-2,i为整数，且i＝1时D₀即为原点O),计算得出如采用方案二，目标点到原点的坐标需大于某一定值，关于k_i的选取和隐形限制将在后面举例说明。添加线段并约束其长度后，每个三角形均被全约束，且每个三角形边长都已知，根据边长即可求出三角形各个内角，进而求得了多边形OD₁D₂…D_n的内角，即控制竖直平面内机械臂肢节运动的舵机的旋转角度。

如附图1、3所示，以六自由度旋转机械臂第一约束条件下的控制方法为例。这里指的六自由度旋转机械臂有6路舵机，舵机6-1控制机械爪张合，舵机6-2控制机械爪旋转，舵机6-6控制底盘旋转，舵机6-3、舵机6-4和舵机6-5控制竖直平面内机械臂肢节运动。以舵机6-5旋转轴中心为原点坐标O，建立如图所示的三维直角坐标系，其中XOY平面始终与机械臂底座平面平行。机械臂各个肢节的长度已知，舵机6-4和舵机6-5旋转轴距离OD₁为L₁，舵机6-3和舵机6-4旋转轴距离D₁D₂为L₂，机械爪中心到舵机6-3旋转轴距离D₂D₃为L₃，由方案一前提条件知L₁＝L₃。

当目标点已知为(x₀,y₀,z₀)时，即D₃点坐标为(x₀,y₀,z₀)，将此坐标转换为三维空间内的极坐标(r₀,θ₀,φ₀)(r₀≤L₁+L₂+L₃)。在OD₃线段上取两点H₁和H₂，使

加入约束条件D₂H₂⊥D₃O于H₂，D₁H₁⊥D₃O于H₁，此时，四边形OD₁D₂D₃全约束,且H₁H₂平行且等于D₁D₂，如图2所示，求舵机6-3、舵机6-4和舵机6-5的旋转角度即求∠D₃D₂D₁、∠D₂D₁O和∠D₁OD₃(θ-∠D₁OD₃即为舵机6-5的旋转角，而θ已知)，结果如下：

舵机6-6旋转角度即为φ₀。根据舵机6-3到舵机6-6的旋转角度，便可以确定这四个舵机的PWM值，将信号同时发送给这四个舵机，使其各自转动到对应的理论计算角度，此时机械爪中心与目标点重合，实现了六自由度旋转机械臂自动到给定的三维空间坐标点的作业。

如附图2、4所示，以七自由度旋转机械臂第一约束条件下的控制方法为例。这里指的七自由度旋转机械臂有7路舵机，舵机7-1控制机械爪张合，舵机7-2控制机械爪旋转，舵机7-7控制底盘旋转，舵机7-3、舵机7-4、舵机7-5和舵机7-6控制竖直平面内机械臂肢节运动。以舵机7-6旋转轴中心为原点坐标O，建立如图所示的三维直角坐标系，其中XOY平面始终与机械臂底座平面平行。机械臂各个肢节的长度已知，舵机7-5和舵机7-6旋转轴距离OD₁为L₁，舵机7-4和舵机7-5旋转轴距离D₁D₂为L₂，舵机7-3和舵机7-4旋转轴距离D₂D₃为L₃，机械爪中心到舵机7-3旋转轴距离D₃D₄为L₄，由方案一前提条件知L₁＝L₄，L₂＝L₃。

当目标点已知为(x₀,y₀,z₀)时，即D₄点坐标为(x₀,y₀,z₀)，将此坐标转换为三维空间内的极坐标(r₀,θ₀,φ₀)(r₀≤L₁+L₂+L₃)。在OD₄线段上取三点H₃、H₂和H₁，使

加入约束条件D₃H₃⊥D₄O于H₃，D₂H₂⊥D₄O于H₂，D₁H₁⊥D₄O于H₁，此时，五边形OD₁D₂D₃D₄全约束，且线段D₄O对称轴即为线段D₂H₂所在直线，如图4所示。求舵机7-3、舵机7-4、舵机7-5和舵机7-6的旋转角度即求∠D₄D₃D₂、∠D₃D₂D₁、∠D₂D₁O和∠D₁OD₃(θ-∠D₁OD₃即为舵机7-6的旋转角，而θ已知)，结果如下：

舵机7-7旋转角度即为φ₀。根据舵机7-3到舵机7-7的旋转角度，便可以确定这五个舵机的PWM值，将信号同时发送给这五个舵机，使其各自转动到对应的理论计算角度，此时机械爪中心与目标点重合，实现了七自由度旋转机械臂自动到给定的三维空间坐标点的作业。

添加第一约束条件，多自由度旋转机械臂的控制方法计算简单，误差较小，工作空间与单舵机控制的最大工作区间相同，即没有工作区间上的损减，且经过实验验证此方法是可以实现的，但是需要在机械臂设计制造过程中注意对称机械臂肢节长度相等，其相等程度将影响机械爪到目标点的误差大小。

如附图1、5所示，以六自由度旋转机械臂第二约束条件下的控制方法为例。连接D₃D₁，加入约束条件D₃D₁＝k×r₀,其中

此时四边形OD₁D₂D₃全约束，如图5所示。求舵机6-3、舵机6-4和舵机6-5的旋转角度即求∠D₃D₂D₁、∠D₂D₁O和∠D₁OD₃(θ-∠D₁OD₃即为舵机6-5的旋转角，而θ已知)，结果如下：

舵机6-6旋转角度即为φ₀。根据舵机6-3到舵机6-6的旋转角度，便可以确定这四个舵机的PWM值，将信号同时发送给这四个舵机，使其各自转动到对应的理论计算角度，此时机械爪中心与目标点重合，实现了六自由度旋转机械臂自动到给定的三维空间坐标点的作业。当r₀＝L₁+L₂+L₃时，D₃D₁＝k×r₀＝L₂+L₃＝D₁D₂+D₂D₃＝D₃O-D₁O,即D₃、D₂、D₁和O四点共线，机械臂得以完全伸展。如k取其他值，则机械臂的机械爪不能完全伸展，即不能在最大工作半径下作业，缩小了其作业空间。应注意D₁D₃+D₃O≥D₁O,即

也就是说，目标点到原点的距离不能小于

如附图2、6所示，以七自由度旋转机械臂第二约束条件下的控制方法为例。连接D₄D₁和D₄D₂，加入约束条件D₄D₁＝k₁×r₀，D₄D₂＝k₂×D₄D₁，其中

此时五边形OD₁D₂D₃D₄全约束，如图6所示。求舵机7-3、舵机7-4、舵机7-5和舵机7-6的旋转角度即求∠D₄D₃D₂、∠D₃D₂D₁、∠D₂D₁O和∠D₁OD₃(θ-∠D₁OD₃即为舵机6-5的旋转角，而θ已知)，结果如下：

舵机7-7旋转角度即为φ₀。根据舵机7-3到舵机7-7的旋转角度，便可以确定这五个舵机的PWM值，将信号同时发送给这五个舵机，使其各自转动到对应的理论计算角度，此时机械爪中心与目标点重合，实现了七自由度旋转机械臂自动到给定的三维空间坐标点的作业。应注意D₁D₄+D₄O≥D₁O,D₂D₄+D₄D₁≥D₂D₁，即

且

添加第二约束条件，多自由度旋转机械臂的控制方法适用性好，计算简单，误差几乎为0，尤其适用于各肢节长度无规律的机械臂控制。工作空间比单舵机控制下的工作区间小，因为存在隐性约束条件，所以使目标点到原点的距离不能小于一个与机械臂肢节长度相关的定值。但是因为这一定值可以人为改变机械臂某肢节长度进行调整，且通常这个机械爪不能到达的目标点的区域较小，不影响机械臂的主要工作区域，所以第二约束条件不失为添加约束条件的较好的选择。

当已知当前多自由度旋转机械臂机械爪的中心坐标(x₀,y₀,z₀)时，读取输入状态，如收到z₀减少的指令，则目标点坐标为(x₀,y₀,z_0-P)(P为步长，可以编程调节步长大小实现机械爪运动速度的调节)，再通过上述的多自由度旋转机械臂定点运动的算法，使机械臂的机械爪到达目标点。此时目标点将作为z₀减少后多自由度旋转机械臂机械爪的中心坐标(x₀,y₀,z₀)，即刷新坐标，之后每一时刻都重复上述步骤，沿其余坐标轴增减同理，从而实现了多自由度旋转机械臂机械爪中心沿直线运动。

当考虑机械爪抓取物体方向时，此时应将用于竖直平面内机械臂肢节运动的舵机N-3空置出来，用于机械爪姿态的调整，这样机械爪有两个空间自由度，可在其工作平面内完成任一方向的抓取。舵机N-1控制机械爪张合，舵机N-2和舵机N-3控制机械爪旋转，舵机N-N控制底盘旋转，其余舵机控制竖直平面内机械臂肢节运动。舵机N-1、舵机N-2和舵机N-3为机械爪抓取控制，不影响机械爪运动，所以单独考虑，不在本发明研究范围内。考虑机械爪抓取方向时，机械臂控制运动的目标是使目标点坐标与舵机N-3旋转轴中心重合，此时竖直平面内机械臂肢节运动的舵机数量由n变为n-1，即由多边形OD₁D₂…D_n变为多边形OD₁D₂…D_n-1，控制方法仍可参照上述两种方法，从而完成考虑机械爪抓取物体方向的多自由度旋转机械臂运动控制。

以上所述，仅为发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何根据目标点在三维直角坐标系的坐标，添加本发明所述的约束条件，计算出机械臂各个肢节的姿态角度，使机械爪直接到达目标点，从而实现多自由度旋转机械爪定点自动抓取物体的设计思路均属于本发明技术构思的保护范围，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，根据本发明的技术方案及其构思加以等同替换或改变，都应涵盖在本发明的保护范围之内。

Claims (1)

1.多自由度旋转机械臂的控制方法，其特征在于，包括以下步骤：

步骤S1：根据目标点在三维直角坐标系的坐标，添加约束条件计算出机械臂各个肢节的姿态角度；

步骤S2：根据步骤S1所述的姿态角度确定机械臂上各舵机的旋转角度；

步骤S3：根据步骤S2所述的旋转角度确定各舵机PWM值；

步骤S4：将步骤S3中所述各舵机PWM值的数值信号同时发送给各舵机，使各舵机转动到步骤S1中所述的姿态角度，实现机械爪中心与目标点的重合，即实现机械爪到给定的三维空间坐标点的作业；

其中，所述多自由度旋转机械臂有n路舵机，其中n≥6，其中舵机n__1控制机械爪张合，舵机n__2控制机械爪旋转，舵机n__n控制底盘旋转，其余舵机控制竖直平面内机械臂肢节运动；所述三维直角坐标系的坐标以舵机n__(n-1)旋转轴中心为坐标原点O，坐标系的XOY平面始终与机械臂底座平面平行；舵机n__(n-2)和舵机n__(n-1)旋转轴距离OD₁为L₁，舵机n__(n-3)和舵机n__(n-2)旋转轴距离D₁D₂为L₂，以此类推，机械爪中心到舵机n__3旋转轴距离D_nD_n-1为L_n；

所述步骤S1中添加的约束条件有两种，分别为机械臂臂长相等的第一约束条件和机械臂臂长不相等且无明显规律的第二约束条件；

机械臂臂长相等的第一约束条件下，即L_i＝L_n+1-i，其中1≤i≤n-1,i为整数,当目标点已知为(x₀,y₀,z₀)时，即D_n点坐标为(x₀,y₀,z₀)，将此坐标转换为三维空间内的极坐标(r₀,θ₀,φ₀)，其中r₀≤L₁+L₂+L₃；当n为奇数时，在OD_n线段上取n-1个点H₁、H₂、……H_n-1，使

其中

D_iH_i⊥D_nO于H_i,其中1≤i≤n-1,i为整数；当n为偶数时，在OD_n线段上取n-1个点H₁、H₂、……H_n-1，使

D_iH_i⊥D_nO于H_i，其中1≤i≤n-1,i为整数；

机械臂臂长不相等且无明显规律的第二约束条件下，在多边形OD₁D₂…D_n中连接D_n与其他不相邻顶点可将该多边形分割成互不重叠的n-1个三角形，形成有线段D_nD₁，D_nD₂，…，D_nD_n-2，且每段线段长度为：D_nD_i＝k_i×D_nD_i-1，其中

i为整数，i＝1时D₀即为原点O。

Images