CN111645046B - 一种主随动机械臂系统及其控制方法 - Google Patents

Abstract

本发明公开一种主随动机械臂系统及其控制方法，主随动机械臂系统包括机械臂、感知系统、数据采集系统、随动系统、主控系统和供电系统，其控制方法包括S1:设置主动机械臂的运动姿态；S2:获取主动机械臂的运动姿态以及各电位器采集的数据以获得主动机械臂姿态信息；S3:根据主动机械臂的反馈量计算得到机械臂各关节的控制信号量；S4:对随动机械臂各关节的舵机发送控制命令，一个同步跟随控制周期结束，重复步骤S1‑S4，完成对主动机械臂的同步跟随控制。随动机械臂模仿跟随主动机械臂的动作，使机械臂脱离了传统的操作平台，让操作员可以更灵活地控制机械臂以完成一些需要精细控制的工作。

Description

一种主随动机械臂系统及其控制方法

技术领域

本发明应用于机械臂领域，具体是一种主随动机械臂系统及其控制方法。

背景技术

机械臂因其独特的操作灵活性，已在工业装配、安全防爆等领域得到广泛应用。现有的机械臂大多结构复杂且造价不菲，同时使用时需要预先设置，难以根据操作者的意图和动作实时灵活运动，缩小了其使用场景，即现有的机械臂无法在造价低廉的同时做到可根据使用者的动作灵活控制运动。

发明内容

本发明所要解决的技术问题是针对现有技术的不足，提供一种主随动机械臂系统及其控制方法。

为解决上述技术问题，本发明的一种主随动机械臂系统，其包括：

机械臂，包括主动机械臂和随动机械臂；

感知系统，安装于主动机械臂上，用于感知主动机械臂运动姿态，其包括小臂齿轮、肘关节传动齿轮和上臂齿轮；

数据采集系统，安装于主动机械臂上，用于采集感知系统所感知出的运动姿态，其包括径向腕关节旋动电位器、肘关节旋动电位器、径向肘关节旋动电位器和MPU6050电子陀螺仪；

随动系统，安装于随动机械臂上，其包括径向腕关节舵机、肘关节舵机、径向肘关节舵机、第一肩关节舵机、第二肩关节舵机及舵机供电模块；

主控系统，与数据采集系统连接采集信息并与随动系统连接控制舵机；

供电系统。

作为一种可能的实施方式，进一步的，所述主动机械臂包括小臂外壳和上臂外壳，所述小臂外壳和上臂外壳一端转动连接。

作为一种可能的实施方式，进一步的，所述随动机械臂包括机械臂小臂和机械臂上臂，所述机械臂小臂与机械臂上臂通过径向腕关节舵机和肘关节舵机相连接。

作为一种可能的实施方式，进一步的，所述径向腕关节旋动电位器、肘关节旋动电位器和径向肘关节旋动电位器均通过电位器齿轮分别与小臂齿轮、肘关节传动齿轮和上臂齿轮对应传动连接。

作为一种较优的选择，优选的，所述主控系统为STM32F103VET6单片机。

一种主随动机械臂系统的控制方法，其包括如下步骤：

S1，操作员佩戴主动机械臂并根据需要变换姿态，以进行主动机械臂运动姿态的设置；

S2，主控系统从主动机械臂上的数据采集系统中获取数据以获得主动机械臂的运动姿态信息；

S3，主控系统根据主动机械臂的反馈量计算得到机械臂各关节的控制信号量；

S4，主控系统对随动机械臂上的随动系统发送控制命令，一个同步跟随控制周期结束，重复步骤S1-S4，完成对主动机械臂的同步跟随控制。

作为一种可能的实施方式，进一步的，所述步骤S2具体包括如下步骤：

S21，实时提取陀螺仪全姿态信号，并将之通过算法换算后赋值于定时器输出PWM的占空比变量以实现对舵机的角度控制；

S22，实时提取电位器中间引脚电压值，经数模转换与算法换算后赋值于定时器输出PWM的占空比变量以实现对舵机的角度控制。

本发明采用以上技术方案，具有以下有益效果：

1.本发明通过设置主动机械臂并配合其上的数据采集系统和感知系统，使得使用者能够通过佩戴主动机械臂并根据需要变换姿态的方式来控制随动机械臂做出所需要的动作运动，极大的简化传统随动机械臂通过按键或遥控器的控制方式，使得整个控制过程更加合理便捷；数据采集系统能够配合感知系统对使用者佩戴主动机械臂后做出的动作信息进行精确的采集以供主控系统获取精准数据。

2.本发明通过设置随动系统和随动机械臂利用五组舵机对随动机械臂进行控制，使得随动机械臂能够在主控系统的控制下跟随主动机械臂的动作信息进行与主动机械臂动作相似的高精度灵活动作。主控系统接收主动机械臂做出的动作信息后转化为控制信号对随动机械臂发送控制命令控制其做出实时动作。

附图说明

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

图1为本发明实施方法的流程示意图；

图2为本发明主动机械臂结构示意图；

图3为本发明主动机械臂结构侧视图；

图4为本发明主动机械臂结构径向立体示意图；

图5为本发明随动机械臂结构示意图；

图6为本发明随动机械臂结构内侧示意图。

具体实施方式

为使本发明实施方式的目的、技术方案和优点更加清楚，下面将结合本发明实施方式中的附图，对本发明实施方式中的技术方案进行清楚、完整地描述。

如图1-6所示，本发明提供了一种主随动机械臂系统，其包括：

机械臂，包括主动机械臂100和随动机械臂200；主随动机械臂外壳均通过3D打印机制成，材料为PLA。

感知系统，安装于主动机械臂100上，用于表示主动机械臂100运动姿态，其包括小臂齿轮、肘关节传动齿轮和上臂齿轮；

数据采集系统，安装于主动机械臂100上，用于采集感知系统所感知出的运动姿态，其包括径向腕关节旋动电位器、肘关节旋动电位器、径向肘关节旋动电位器和MPU6050电子陀螺仪；电子陀螺仪由单片机供电，旋动电位器由供电系统直接供电；随动系统，安装于随动机械臂200上，其包括径向腕关节舵机、肘关节舵机、径向肘关节舵机、第一肩关节舵机、第二肩关节舵机及舵机供电模块；各舵机由供电系统直接供电。

主控系统，与数据采集系统连接采集信息并与随动系统连接控制舵机；即主控系统一侧连接数据采集系统以采集信息，另一侧连接随动机械臂舵机以控制舵机。

供电系统，供电系统包括3片锂离子聚合物电池(12.4V)和2个DC-DC直流降压模块(输出电压分别为5V、6V)。供电系统降压后5V供主控系统使用，6V供舵机使用。

其中的主动机械臂100为一种佩戴式主动外骨骼机械臂，其用于装佩在使用者手臂进行动作采样以控制外部随动机械臂，其包括一端相互转动连接的小臂外壳101和上臂外壳102，其转动连接可采用关节连接钉配合关节钉套进行转动连接，也可采用销连接等转动连接方式，此处可采用的转动连接方式均为本领域技术人员的公知常识，在此不做赘述；且小臂外壳101和上臂外壳102均采用弧形外壳设计，使用者使用时能够将手伸入佩戴；小臂外壳101另一端转动连接有手持部103，且手持部103上固定连接有小臂齿轮104，小臂齿轮104为环形齿轮，使用时使用者能够将手伸入贯穿小臂齿轮104从而实现佩戴；小臂外壳101上安装有通过电位器齿轮与小臂齿轮104传动连接的径向腕关节旋动电位器105；小臂外壳101包括小臂下壳101a和小臂上壳101b，且小臂下壳101a一端设有插条111，小臂上壳101b上对应插条111设有插槽112，所述插条111插入插槽112并可相对其滑动，插条111与插槽112上贯穿开设有三组以上的定位孔113，所述定位孔113内插设有用于固定小臂下壳101a和小臂上壳101b的定位棒114，且定位棒114与定位孔113间隙配合使得定位棒114插入定位孔113进行定位后使用时不易滑出，使用时可通过插条111在插槽112上滑动并改变插入定位棒114的位置与数量达到改变小臂下壳101a和小臂上壳101b之间间距的目的，从而使其适用于不同臂长的使用者。上臂外壳102包括相互径向转动连接的上臂下壳102a和上臂上壳102b，且上臂下壳102a上固定连接有上臂齿轮106，上臂齿轮106为环形齿轮，使用时使用者能够将手伸入贯穿上臂齿轮106从而实现佩戴。所述上臂上壳102b上安装有通过电位器齿轮与上臂齿轮106传动连接的径向肘关节旋动电位器107和电子陀螺仪108；电子陀螺仪108的型号为MPU6050。小臂外壳101和上臂外壳102转动连接处设置有肘关节传动齿轮109，且肘关节传动齿轮109与小臂外壳101固定连接并跟随其转动，所述上臂下壳102a上安装有通过电位器齿轮与肘关节传动齿轮109传动连接的肘关节旋动电位器110。上臂上壳102b一端转动连接有护肩外壳115。腕关节旋动电位器105采集以手臂为轴手腕径向转动的数据、肘关节旋动电位器109采集以手肘关节为轴转动的数据、径向肘关节旋动电位器107采集以手臂为轴手肘径向转动的数据、电子陀螺仪108采集肩关节转动的两个方向的数据，分别以左右肩连线和前后肩连线为轴。主动臂能够装佩穿戴在使用者手臂上，通过活动连接的手持部103、小臂下壳101a、小臂上壳101b、上臂下壳102a和上臂上壳102b，使得使用者穿戴主动机械臂时能够灵活活动做出所需的手臂动作，为控制外界随动臂活动提供了良好的可动性。通过设置小臂齿轮104、上臂齿轮106、肘关节传动齿轮109、多组电位器和电子陀螺仪108，使得使用者穿戴主动机械臂所做出的动作能够被精确的采集转换为动作信息以供外部的主控系统采集从而转化为控制外界随动臂的控制信号，精确的动作信息保证了控制外界随动臂时动作的实时性和精准性。通过设置插条111和插槽112实现可拆卸并可调节连接的小臂下壳101a和小臂上壳101b，使得装置能够适用于不同臂长的使用人群。过程中外界主控系统会实时提取电子陀螺仪108全姿态信号，并将之通过算法换算后赋值于定时器输出PWM的占空比变量以实现对舵机的角度控制；此电子陀螺仪108的信号传输协议为I²C协议，通过I²C协议直接提取陀螺仪的六个基础数据：三方向加速度与三方向角速度。同时实时提取各电位器中间引脚电压值，经数模转换与算法换算后赋值于定时器输出PWM的占空比变量以实现对外界随动机械臂上的舵机的角度控制。

其中的随动臂为一种随动式机械臂，可通过主控系统发送控制信号对其进行控制，其包括：安装轴承座201，内部通过舵机安装架安装有径向肩关节舵机204，径向肩关节舵机204用于带动机械臂上臂202围绕安装轴承座201旋转；径向肩关节舵机204固定安装于安装轴承座201内部，且径向肩关节舵机204的转轴贯穿安装轴承座201轴承孔并与其侧壁紧配合，径向肩关节舵机204转轴裸露在安装轴承座201轴承孔外侧的一端固定安装有肩关节舵机206。肩关节舵机206的输出轴与机械臂上臂202固定连接带动其摆动。使用时整体组装过程可采用如下安装方式组装安装轴承座201，将安装轴承座201轴承通过四片固定片固定，径向肩关节舵机204从中孔穿入，加第五片固定片固定舵机，以铜柱，螺母固定四根承重钉，完成安装轴承座201的安装。此时安装轴承座201分为内部可旋转固定片区域与外部与承重钉相连的外部静止固定片区域。其中可旋转固定片区域为由径向肩关节舵机204驱动的随动臂的第一个关节，肩关节的第一部分。机械臂上臂202，两端分别安装有径向肘关节舵机205和肩关节舵机206，且肩关节舵机206与径向肩关节舵机204转轴一端固定连接；径向肘关节舵机205固定安装于机械臂上臂202。机械臂小臂203，两端分别安装有径向腕关节舵机207和肘关节舵机208，且肘关节舵机208与径向肘关节舵机205转轴一端固定连接。肘关节舵机208输出轴与机械臂小臂203固定连接带动其摆动。径向腕关节舵机207固定连接于机械臂小臂203一端，径向腕关节舵机207转轴处安装有机械手209。通过设置径向肩关节舵机204和肩关节舵机206将安装轴承座201和机械臂上臂202相连接，使得机械臂上臂202能够在安装轴承座201旋转与摆动，使得机械臂上臂202的活动范围大且活动灵活。通过设置径向肘关节舵机205和肘关节舵机208将机械臂上臂202和机械臂小臂203进行连接，使得机械臂小臂203能够在机械臂上臂202前端旋转与摆动，使得机械臂小臂203的活动范围大且活动灵活。通过设置径向腕关节舵机207使得机械手209能够多角度进行旋转，使得装置整体造价低廉的同时做到能够灵活控制运动。其中的径向肩关节舵机204、径向肘关节舵机205、肩关节舵机206、径向腕关节舵机207、肘关节舵机208均可采用型号为PDI-6221MG-180的数字舵机。

作为一种较优的选择，优选的，所述主控系统为STM32F103VET6单片机。根据从主动机械臂各个关节获得的同步跟随数据转换成控制信号量，通过单片机下发给随动机械臂的各个关节的舵机以实现随动控制。

一种主随动机械臂系统的控制方法，其包括如下步骤：

S1，操作员佩戴主动机械臂并根据需要变换姿态，以进行主动机械臂运动姿态的设置；

S2，主控系统从主动机械臂上的数据采集系统中获取数据以获得主动机械臂的运动姿态信息；

S3，主控系统根据主动机械臂的反馈量计算得到机械臂各关节的控制信号量；

S4，主控系统对随动机械臂上的随动系统发送控制命令，一个同步跟随控制周期结束，重复步骤S1-S4，完成对主动机械臂的同步跟随控制。

作为一种可能的实施方式，进一步的，所述步骤S2具体包括如下步骤：

S21，实时提取陀螺仪全姿态信号，并将之通过算法换算后赋值于定时器输出PWM的占空比变量以实现对舵机的角度控制；此陀螺仪的信号传输协议为I²C协议，通过I²C协议直接提取陀螺仪的六个基础数据：三方向加速度与三方向角速度；

S22，实时提取电位器中间引脚电压值，经数模转换与算法换算后赋值于定时器输出PWM的占空比变量以实现对舵机的角度控制。

进一步的，步骤S3中根据主动机械臂的反馈量计算得到机械臂各关节的控制信号量包括以下过程：

在已知单片机时钟频率υ₁，时钟分频因子c与自动重装载寄存器m数据可调的情况下，可得到任意频率的方波，同时通过更改输出比较寄存器的值可改变方波的占空比。

所需频率为：

而舵机需要的控制信号为厂家提供的t∈0.5～2.5ms对应θ∈0～180°，故舵机控制信号的数值为：

经计算，n∈50～250对应舵机θ∈0～180°，此时控制信号变化量对应舵机角度的比例为：

陀螺仪随动算法相对简便，是因为陀螺仪给出的欧拉角数据即为角度数据，故以陀螺仪为信号源的控制信号的数据即为：

n＝kηθ+C (4)

其中k与C为调试参数，数值与舵机安装方式及其机械误差有关。

旋动电位器随动算法相对较复杂。

首先，根据电位器总圈数N＝10可得当前阻值r的角度θ为：

其中R为旋动电位器总阻值。

其次，由调压范围可得当前角度的电压U₁为：

再由齿轮齿数比K可得姿态运动角度θ_z与电压值的换算公式为：

最后，由公式(3)可得以旋动电位器为信号源的控制信号的数据n为：

以上所述为本发明的实施例，对于本领域的普通技术人员而言，根据本发明的教导，在不脱离本发明的原理和精神的情况下凡依本发明申请专利范围所做的均等变化、修改、替换和变型，皆应属本发明的涵盖范围。

Claims (6)

1.一种主随动机械臂系统，其特征在于：其包括：

机械臂，包括主动机械臂和随动机械臂；所述主动机械臂包括小臂外壳和上臂外壳，所述小臂外壳和上臂外壳一端转动连接；

感知系统，安装于主动机械臂上，用于感知主动机械臂运动姿态，其包括小臂齿轮、肘关节传动齿轮和上臂齿轮；

数据采集系统，安装于主动机械臂上，用于采集感知系统所感知出的运动姿态，其包括径向腕关节旋动电位器、肘关节旋动电位器、径向肘关节旋动电位器和MPU6050电子陀螺仪；所述径向腕关节旋动电位器与小臂齿轮传动连接采集数据，所述肘关节旋动电位器与肘关节传动齿轮传动连接采集数据，所述径向肘关节旋动电位器与上臂齿轮传动连接采集数据；

随动系统，安装于随动机械臂上，其包括径向腕关节舵机、肘关节舵机、径向肘关节舵机、第一肩关节舵机、第二肩关节舵机及舵机供电模块；

主控系统，与数据采集系统连接采集信息并与随动系统连接控制舵机；

供电系统。

2.根据权利要求1所述的一种主随动机械臂系统，其特征在于：所述随动机械臂包括机械臂小臂和机械臂上臂，所述机械臂小臂与机械臂上臂通过径向腕关节舵机和肘关节舵机相连接。

3.根据权利要求1所述的一种主随动机械臂系统，其特征在于：所述径向腕关节旋动电位器、肘关节旋动电位器和径向肘关节旋动电位器均通过电位器齿轮分别与小臂齿轮、肘关节传动齿轮和上臂齿轮对应传动连接。

4.根据权利要求1所述的一种主随动机械臂系统，其特征在于：所述主控系统为STM32F103VET6单片机。

5.一种如权利要求1所述的主随动机械臂系统的控制方法，其特征在于：其包括如下步骤：

S1，操作员佩戴主动机械臂并根据需要变换姿态，以进行主动机械臂运动姿态的设置；

S2，主控系统从主动机械臂上的数据采集系统中获取数据以获得主动机械臂的运动姿态信息；

S3，主控系统根据主动机械臂的反馈量计算得到机械臂各关节的控制信号量；

S4，主控系统对随动机械臂上的随动系统发送控制命令，一个同步跟随控制周期结束，重复步骤S1-S4，完成对主动机械臂的同步跟随控制。

6.根据权利要求5所述的一种主随动机械臂系统的控制方法，其特征在于：步骤S2具体包括如下步骤：

S21，实时提取陀螺仪全姿态信号，并将之通过算法换算后赋值于定时器输出PWM的占空比变量以实现对舵机的角度控制；

S22，实时提取电位器中间引脚电压值，经数模转换与算法换算后赋值于定时器输出PWM的占空比变量以实现对舵机的角度控制。

Images