一种智能机械臂的控制方法、装置及系统与流程

本发明涉及机器人领域，尤其涉及一种机器人的智能机械臂的控制方法、装置及系统。

背景技术：

智能机械臂是目前机器人技术领域中得到最广泛实际应用的自动化机械装置，其应用于于工业制造、医学治疗、娱乐服务、军事、半导制造等等领域。其基本的工作原理就是能够接收治疗，进行相关作业。

然而目前对机械臂的控制主要通过程序来直接驱动，或者通过传感器、连杆等物理遥控器进行控制。通过人体姿态来对机械臂进行控制的主要是通过摄像头或者复杂的传感器套装进行姿态捕获和换算。而传统编程控制方式不够灵活，不同的动作设定需要编写不同的程序进行驱动，而物理遥控的方式不能很直观地反映相关的姿态动作，另外，新的人体姿态转换方式，要么采用摄像头，要么采用较复杂的传感器，导致成本较高，同时过于依赖硬件环境，计算量也很大。

另外，如图1所示为六自由度机械臂的结构示意简图，其至少包括底座1、第一臂2、第二臂3、手腕4和爪子5，底座1与第一臂2、第一臂2与第二臂3、第二臂3与手腕4之间均通过可转动装置连接，也即是第一臂2、第二臂3、手腕4可根据控制信号向一定的方向上转动，从而可以使得爪子能够抓到物品。也即是，只要能够得到第一臂2、第二臂3和手腕4的转动角度，就可以将该其对应的转动角度的数据转换为控制信号，通过对应的舵机就可以控制第一臂2、第二臂3和手腕4的转动角度。

技术实现要素：

为了克服现有技术的不足，本发明的目的之一在于提供一种智能机械臂的控制方法，其能够解决使用人体姿态转换方式来控制机械臂时，成本高、结构复杂、计算量等问题。

本发明的目的之一采用以下技术方案实现：

本发明提供了一种智能机械臂的控制方法，包括以下步骤：

S1：获取第一欧拉角、第二欧拉角和第三欧拉角，并分别存储于对应的缓存空间；所述第一欧拉角是第一MCU控制器根据安装于人手的手背上的第一传感器所测量的数据计算得到；所述第二欧拉角是第二MCU控制器根据安装于人的前臂上方的第二传感器所测量的数据计算得到；所述第三欧拉角是第三MCU控制器根据安装于人的上臂上方的第三传感器所测量的数据计算得到；

S2：根据所述第二欧拉角的方位角计算得出机械臂底盘的方位舵机角度；

S3：根据所述第二欧拉角和第三欧拉角的俯仰角计算得出手臂的垂直平面坐标，并换算为机械舵机的垂直平面坐标，并依据机械臂舵机的垂直平面坐标以及机械臂的两条臂的长度计算得出机械臂的第一臂的水平俯仰角和第二臂的水平俯仰角，以及机械臂第一臂的舵机角度和机械臂第二臂的舵机角度；

S4：根据人手的垂直平面坐标和机械臂的垂直平面坐标的映射关系，根据第一欧拉角的俯仰角和侧倾角得到机械臂手腕的俯仰角和侧倾角，并结合所述第一臂的水平俯仰角和第二臂的水平俯仰角，计算得出机械臂手腕的俯仰舵机角度；

S5：根据第一欧拉角的侧倾角得到机械臂手腕的侧倾舵机角度；

S6：根据五个舵机角度向机械臂控制器发送驱动舵机转动的数据信号，从而使得机械臂控制器根据该数据信号生成PWM信号给机械臂，以控制机械臂对应的部位运动；所述五个舵机角度包括所述机械臂底盘的方位舵机角度、机械臂第一臂的舵机角度、机械臂第二臂的舵机角度、机械臂手腕的俯仰舵机角度和机械臂手腕的侧倾舵机角度。

优选地，所述第一传感器为六轴传感器，第二传感器和第三传感器为九轴传感器。

优选地，步骤S1和步骤S2之间还包括S11：判断所述第二欧拉角和第一欧拉角是否合法，若合法，则执行S2，若不合法，则执行S1。

优选地，步骤S3还包括当所述根据第二欧拉角和第三欧拉角的俯仰角计算得出手臂的垂直坐标平面，并换算为机械臂舵机的垂直平面坐标时，判断机械臂是否能够达到机械臂的垂直坐标平面的对应坐标点，若不能，则PC端采用原有的数据作为机械臂第一臂的舵机角度和第二臂的舵机角度值，继续执行S4；所述原有的数据为智能机械臂前一次运动时计算得到的机械臂第一臂的舵机角度和第二臂的舵机角度值。

为了克服现有技术的不足，本发明的目的之二在于提供一种智能机械臂的控制装置，其能够解决使用人体姿态转换方式来控制机械臂时，成本高、结构复杂、计算量等问题。

本发明的目的之二采用以下技术方案实现：

本发明提供了一种智能机械臂的控制装置，包括：

获取欧拉角模块，用于获取第一欧拉角、第二欧拉角和第三欧拉角并分别存储于对应的缓存空间；所述第一欧拉角是第一MCU控制器根据安装于人手的手背上的第一传感器所测量的数据计算得到；所述第二欧拉角是第二MCU控制器根据安装于人的前臂上方的第二传感器所测量的数据计算得到；所述第三欧拉角是第三MCU控制器根据安装于人手的上臂上方的第三传感器所测量的数据计算得到；

获取第一舵机角度模块，用于根据第二欧拉角的方位角计算得出机械臂底盘的方位舵机角度；

获取第二和第三舵机角度模块，用于根据第二欧拉角和第三欧拉角的俯仰角计算得出手臂的垂直平面坐标，并换算为机械臂舵机的垂直平面坐标，根据坐标数据以及机械臂的两条臂长的条件计算得出机械臂的第一臂的水平俯仰角和第二臂的水平俯仰角，以及机械臂第一臂的舵机角度和机械臂第二臂的舵机角度；

获取第四舵机角度模块，用于根据人手的垂直坐标平面和机械臂的垂直坐标平面的映射关系，根据第一欧拉角的俯仰角和侧倾角得到机械臂手腕的俯仰角和侧倾角，并结合所述第一臂的水平俯仰角和第二臂的水平俯仰角，计算得出机械臂手腕的俯仰舵机角度；

获取第五舵机角度模块，用于根据第一欧拉角的侧倾角得到机械臂手腕的侧倾舵机角度；

控制机械臂工作模块，用于根据五个舵机角度向机械臂控制器发送驱动舵机转动角度的信号，从而使得机械臂控制器根据该信号发送一PWM信号控制机械臂相对应的部位运动；所述五个舵机角度包括机械臂底盘的方位舵机角度、机械臂第一臂的舵机角度、机械臂第二臂的舵机角度、机械臂手腕的俯仰舵机角度和机械臂手腕的侧倾舵机角度。

优选地，所述第一传感器为六轴传感器，第二传感器和第三传感器为九轴传感器。

为了克服现有技术的不足，本发明的目的之三在于提供一种智能机械臂的控制系统，其能够解决使用人体姿态转换方式来控制机械臂时，成本高、结构复杂、计算量等问题。

本发明的目的之三采用以下技术方案实现：

本发明提供了一种智能机械臂的控制系统，包括安装于手背上的第一传感器、安装于前臂上方的第二传感器、安装于上臂上方的第三轴传感器以及MCU控制器，所述MCU控制器有三个，分别记为第一MCU控制器、第二MCU控制器和第三MCU控制器；所述第一传感器与所述第一MCU控制器，所述第二传感器与所述第二MCU控制器电性连接，所述第三传感器与所述第三MCU控制器电性连接；第一MCU控制器用于获取第一传感器所发送的数据，以及根据所述第一传感器所发送的数据计算得到第一欧拉角；第二MCU控制器用于获取第二传感器所发送的数据，以及根据所述第二传感器所发送的数据计算得到第二欧拉角；第三MCU控制器用于获取第三传感器所发送的数据，以及根据所述第三传感器所发送的数据计算得到第三欧拉角；

还包括PC端、机械臂控制器和机械臂，所述第一MCU控制器、第二MCU控制器、第三MCU控制器分别与所述PC端连接，所述机械臂控制器与所述PC端电性连接，所述机械臂控制器与机械臂电性连接；

PC端用于接收来自第一MCU控制器、第二MCU控制器和第三MCU控制器发送的第一欧拉角、第二欧拉角和第三欧拉角，并分别存储于对应的缓存空间中，以及用于根据所述第一欧拉角、第二欧拉角和第三欧拉角生成使得机械臂运动的数据信号并发送给机械臂控制器；所述机械臂控制器用于根据所述数据信号生成PWM信号给机械臂，并控制机械臂对应的部位工作。

优选地，所述第一传感器为六轴传感器，第二传感器和第三传感器为九轴传感器。

优选地，所述第一MCU控制器、第二MCU控制器、第三MCU控制器分别与所述PC端通过串口通信连接。

优选地，所述机械臂控制器与PC端通过I2C数据总线连接。

相比现有技术，本发明的有益效果在于：本发明仅需通过三个固定的上臂上方、前臂上方和手背的加速度、陀螺仪、磁场传感器即可进行相关的姿态捕捉，通过传统的计算机即可进入换算和驱动。本发明解决了传统机械臂驱动方案不灵活的同时，还比目前复杂传感器套装和摄像头姿态捕获的方案更加节省成本和计算量，提高了通用性。

附图说明

图1为本发明提供的六自由度机械臂的结构示意简图；

图2为本发明提供智能机械臂的控制系统的结构模块图；

图3为本发明提供智能机械臂的控制方法流程图；

图4为本发明提供智能机械臂的控制装置结构模块图。

具体实施方式

下面，结合附图以及具体实施方式，对本发明做进一步描述：

如图2所示，本发明提供的实施例一是一种智能机械臂的控制系统，其包括一安装于手背上的第一传感器、安装于前臂上方的第二传感器、安装于上臂上方的第三传感器以及MCU控制器，所述MCU控制器有三个，分别记为第一MCU控制器、第二MCU控制器和第三MCU控制器，这三个MCU控制器之间没有任何关联。所述第一传感器与第一MCU控制器电性连接，所述第二传感器与所述第二MCU控制器电性连接，所述第三传感器与所述第三MCU控制器电性连接。第一MCU控制器用于获取第一传感器所发送的数据，第二MCU控制器用于获取第二传感器所发送的数据，第三MCU控制器用于获取第三传感器所发送的数据。其中第一传感器为六轴传感器，第二传感器和第三传感器均为九轴传感器。另外，由于六轴传感器以及九轴传感器是姿态传感器，比如六轴传感器是由三轴加速度和三轴陀螺仪传感器组成的，可利用基于四元数的三维算法和特殊数据融合计数，实时输出以四元数、欧拉角表示的零漂移三维姿态方位数据，也即是通过上述过程，MCU控制器可根据六轴传感器测量的数据得到本发明中所提到的第一欧拉角。该计算方法已经是比较常见的方法，是本领域的技术人员所熟知的技术。

还包括PC端、机械臂控制器和机械臂，其中，该机械臂为六自由度机械臂。所述第一MCU控制器、第二MCU控制器、第三MCU控制器分别通过串口通信与PC端通信，所述机械臂控制器与PC端电性连接，所述机械臂控制器与机械臂电性连接。PC端用于接收来自第一MCU控制器、第二MCU控制器和第三MCU控制器发送的数据并分别存储于相对应的缓存空间中，以及用于根据所述第一MCU控制器、第二MCU控制器和第三MCU控制器发送的数据生成使得机械臂运动的动作数据并发送给机械臂控制器。也即是通过检测人手的动作数据(也即是人手的运动轨迹)，然后通过PC端将该人手的动作数据转换为相对应的机械臂的动作数据(也即是机械臂的运动轨迹)，从而将机械臂的动作数据转换为控制机械臂运动的电信号，实现了通过人体姿态来控制机械臂的运动轨迹。机械臂包括手腕、爪子、第一臂、第二臂以及底座。因此，可通过测量人的手以及手臂的运动数据，在通过PC端的处理计算得到机械臂的各个部件的运动数据，从而得到控制机械臂运动轨迹的控制信号，从而使得机械臂根据该控制信号运动。

如图3所示，本发明还提供了一种智能机械臂的控制方法包括以下步骤：

S1：获取第一欧拉角、第二欧拉角和第三欧拉角，并分别存储于对应的缓存空间。所述第一欧拉角是由第一MCU控制器根据获取的第一传感器所测量的数据计算得到；所述第二欧拉角是由第二MCU控制器根据获取的第二传感器所测量的数据计算得到；所述第三欧拉角是由第三MCU控制器根据获取的第三传感器所测量的数据计算得到。其中第一传感器为六轴传感器，第二传感器和第三传感器均为九轴传感器。

首先，将六轴传感器安装在人手的手背上，用于检测人手的手背转动时的角度等数据，第一MCU控制器根据六轴传感器所检测的数据然后计算得到手背的第一欧拉角，同理可得到前臂和上臂的第二欧拉角和第三欧拉角。三个MCU控制器分别通过串口通信与PC端连接，用于将第一欧拉角、第二欧拉角以及第三欧拉角通过串口通信发送到PC端。而PC端获取到所述第一欧拉角、第二欧拉角和第三欧拉角后，分别将其存储于不同的缓存空间内。

还包括S2：判断第二欧拉角、第三欧拉角是否合法，若合法，则执行S3。

当接收到三个MCU控制器发送的数据后，首先判定第二欧拉角和第三欧拉角是否合法，因为机械臂的运动轨迹有一定的规则，因为当人们操作机械臂时，也是遵循一定的规则，而不是根据人的爱好随意控制的，因此首先判断人体前臂和手臂所对应的第二欧拉角和第三欧拉角是否合法，比如机械臂的第一臂和第二臂只能向特定方向上移动，而不是360度方向移动(并不像人类手臂一样可以自由旋转)，因此可能导致测量的数据不合法，则就需要重新获取；或者是在接收数据时，由于网络或者其它原因导致数据的不合法，若不合法，则需重新获取，若合法，则继续下一步操作。

S3：根据第二欧拉角的方位角计算得出机械臂底盘的方位舵机角度。

根据安装在前臂上方的第二传感器所检测的数据计算得到的第二欧拉角中的方位角计算得出机械臂底盘的左右运动方位，也即是机械臂底盘的舵机方位，其用于控制机械臂底盘的舵机运动的方位。另外，舵机是一种位置(角度)伺服的驱动器，适用于那些需要角度不断变化并可以保持的控制系统，一般都是在机械臂中安装的。

S4：根据第二欧拉角和第三欧拉角的俯仰角计算得出手臂的垂直平面坐标，并换算为机械臂舵机的垂直平面坐标，并依据机械臂舵机的垂直平面坐标以及机械臂的两条臂的长度计算得出机械臂的第一臂的水平俯仰角和第二臂的水平俯仰角，以及机械臂第一臂的舵机角度和机械臂第二臂的舵机角度。

其中，首先根据第二欧拉角和第三欧拉角计算得出一人手的垂直坐标平面中手腕的坐标，而该垂直平面以相关手臂为原点，水平为X轴、垂直为Y轴。根据上述人手的垂直坐标平面换算为机械臂的垂直坐标平面，该机械臂的垂直坐标平面是以机械臂根部为原点。

由于机械臂的两条臂长是一定的，根据机械臂的两条臂的长度(包括第一臂和第二臂)并结合机械臂的垂直平面坐标，从而得到机械臂第一臂的水平俯仰角和机械臂第二臂的水平俯仰角，以及机械臂第一臂的舵机角度和机械臂第二臂的舵机角度。

S5：根据人手的垂直平面坐标和机械臂的垂直平面坐标的映射关系，根据第一欧拉角的俯仰角和侧倾角得到机械臂手腕的俯仰角和侧倾角，并结合所述第一臂的水平俯仰角和第二臂的水平俯仰角，计算得出机械臂手腕的俯仰舵机角度。

首先根据上述人手的垂直坐标平面和机械臂的垂直坐标平面的映射关系，可根据第一欧拉角的俯仰角和侧倾角得到机械臂手腕的俯仰角和侧倾角，然后结合所述第一臂的水平俯仰角和第二臂的水平俯仰角，计算得出机械臂手腕的俯仰舵机角度。

S6：根据第一欧拉角的侧倾角得到机械臂手腕的侧倾舵机角度。

S7：根据五个舵机角度向机械臂控制器发送驱动舵机转动角度的信号，从而使得机械臂控制器根据该信号发送一PWM信号控制机械臂相对应的部位运动；所述五个舵机角度包括所述机械臂底盘的方位舵机角度、机械臂第一臂的舵机角度、机械臂第二臂的舵机角度、机械臂手腕的俯仰舵机角度和机械臂手腕的侧倾舵机角度。

上述五个舵机角度的数据，PC端通过串口数据线将其传输给机械臂控制器，该机械臂控制器实时将上述五个舵机角度的数据转换为对应舵机的PWM控制信号，以使得对应舵机根据相对应的PWM信号工作，从而完成了机械臂的控制。

其中所述步骤S3、步骤S4和S5、步骤S6之间没有执行的先后顺序。

优选地，所述步骤S3还包括，当所述根据第二欧拉角和第三欧拉角的俯仰角计算得出手臂的垂直坐标平面，并换算为机械臂舵机的垂直平面坐标时，判断机械臂是否能够达到机械臂的垂直坐标平面的对应坐标点，若不能，则PC端采用原有的数据作为机械臂第一臂的舵机角度和第二臂的舵机角度值，继续执行S5；所述原有的数据为智能机械臂前一次运动时计算得到的机械臂第一臂的舵机角度和第二臂的舵机角度值。

如图4所示，本发明还提供了与一种智能机械臂的控制方法相对应的智能机械臂的控制装置，其包括：

获取欧拉角模块，用于获取第一欧拉角、第二欧拉角和第三欧拉角并分别存储于对应的缓存空间；所述第一欧拉角是第一MCU控制器根据安装于人手的手背上的第一传感器所测量的数据计算得到；所述第二欧拉角是第二MCU控制器根据安装于人的前臂上方的第二传感器所测量的数据计算得到；所述第三欧拉角是第三MCU控制器根据安装于人手的上臂上方的第三传感器所测量的数据计算得到；

获取第一舵机角度模块，用于根据第二欧拉角的方位角计算得出机械臂底盘的方位舵机角度；

获取第二和第三舵机角度模块，用于根据第二欧拉角和第三欧拉角的俯仰角计算得出手臂的垂直平面坐标，并换算为机械臂舵机的垂直平面坐标，根据坐标数据以及机械臂的两条臂长的条件计算得出机械臂的第一臂的水平俯仰角和第二臂的水平俯仰角，以及机械臂第一臂的舵机角度和机械臂第二臂的舵机角度；

获取第四舵机角度模块，用于根据人手的垂直坐标平面和机械臂的垂直坐标平面的映射关系，根据第一欧拉角的俯仰角和侧倾角得到机械臂手腕的俯仰角和侧倾角，并结合所述第一臂的水平俯仰角和第二臂的水平俯仰角，计算得出机械臂手腕的俯仰舵机角度；

获取第五舵机角度模块，用于根据第一欧拉角的侧倾角得到机械臂手腕的侧倾舵机角度；

控制机械臂工作模块，用于根据五个舵机角度向机械臂控制器发送驱动舵机转动角度的信号，从而使得机械臂控制器根据该信号发送一PWM信号控制机械臂相对应的部位运动；所述五个舵机角度包括机械臂底盘的方位舵机角度、机械臂第一臂的舵机角度、机械臂第二臂的舵机角度、机械臂手腕的俯仰舵机角度和机械臂手腕的侧倾舵机角度。

优选地，所述第一传感器为六轴传感器，第二传感器和第三传感器为九轴传感器。

对本领域的技术人员来说，可根据以上描述的技术方案以及构思，做出其它各种相应的改变以及形变，而所有的这些改变以及形变都应该属于本发明权利要求的保护范围之内。