CN109202873A - 一种柔性协作机械臂及其控制方法 - Google Patents

Abstract

本发明属于协作机械臂技术领域，公开了一种柔性协作机械臂及其控制方法，包括底座、机械臂主体和执行夹具，机械臂主体的底端与底座转动连接，执行夹具设于机械臂主体的顶端，底座内设置有控制板和底部舵机；机械臂主体包括转动连接的第一机臂和第二机臂；执行夹具包括多个挠性夹指，执行夹具由执行舵机驱动夹取。本发明减少了传统协作机械臂的复杂结构，降低了对驱动力的要求，本发明采用柔性的执行夹具，能够从多种角度进行抓取，通过将形变读数引入到位置速度控制环中，实现了舵机主动抵消手臂震动，提升了机械臂的控制精度；通过对舵机进行电流检测的方式，低成本地实现了碰撞检测与拖动示教。因此适合推广使用。

Description

一种柔性协作机械臂及其控制方法

技术领域

本发明属于协作机械臂技术领域，具体涉及一种柔性协作机械臂及其控制方法。

背景技术

目前的机械臂可以分为两大类：需要安全隔离区的传统工业机械臂以及可以与人共处的协作机械臂。机械臂是一个多输入多输出、高度非线性、强耦合的复杂***。因其独特的操作灵活性,已在工业装配、安全防爆等领域得到广泛应用；随着机械臂的深入研发，其结构和精度均得到极大的改善，已不仅仅在目前的领域应用，还拓展至诸多直接辅助人进行操作的技术领域，甚至能部分代替人的工作。

协作机械臂要能与人安全共处，且能让使用者方便的进行编程，需要实现碰撞检测与拖动示教两大功能。为了提高机械臂的运动精度，目前的协作机械臂均设计成体积较为庞大的结构，并采用高硬度的材料制成，以此减小因材料属性导致机械臂本身的变形；这样的结果是导致机械臂质量偏高，且由于材质非常坚硬，一旦机械臂与人发生碰撞，导致人受伤的几率较高。因此，目前的协作机械臂厂商在机械臂的控制算法和紧急刹车等模块上大量投入改进，尽量使机械臂不出现意外导致失控碰撞，此种做法可以称为主动安全措施。主动安全措施如果经过充分的研发与测试，可以保证机械臂的安全，但是有研发难度高，研发周期长的缺点，也会导致产品成本较高。这就让协作机械臂真正进入消费者市场变得困难。

在工业领域，目前的协作机械臂的主要用于自动化生产流水线，售价一般在十万到二十万人民币。如此高的价格使得协作机械臂的应用领域非常受限，然而消费者市场对协作机械臂有着广大的潜在需求，例如对老人的护理、为肌肉萎缩患者增强行动力、家庭做饭辅助等。这类需求不需要协作机械臂达到工业领域应用标准，然而目前这块市场仍然缺乏有力的低成本的产品存在。

因此，在本领域为了提高协作机械臂的主动安全措施能动性，同时降低协作机械臂的成本，需要提出更为合理的技术方案，以更好地解决当下的技术问题。

发明内容

本发明提供了一种柔性协作机械臂，旨在精简协作机械臂的结构，减小其质量和体积，改进协作机械臂的执行末端的结构，使协作机械臂与人的配合更加顺畅，协作安全性能大大提高，同时大大降低协作机械臂的成本。

为了实现上述效果，本发明所采用的技术方案为：

一种柔性协作机械臂，包括底座、机械臂主体和执行夹具，所述机械臂主体的底端与底座转动连接，执行夹具设于机械臂主体的顶端。具体地说，所述的底座内设置有控制板和底部舵机，底部舵机用于驱动机械臂水平转动；所述的机械臂主体包括转动连接的第一机臂和第二机臂，第一机臂和第二机臂由机臂舵机驱动后在竖直方向相对转动；所述的执行夹具包括多个挠性夹指，执行夹具由执行舵机驱动夹取；所述的底部舵机、机臂舵机和执行舵机均电连接至控制板，且每个舵机与控制板之间均连接有电流传感器。

本发明通过底座和机械臂主体驱动执行夹具实现夹取操作，底座实现机械臂在水平方向的转动，机械臂主体上的机臂舵机实现竖直方向上的运动，执行夹具实现挠性夹取，避免了在夹取过程中造成损坏，同时机械臂整体在运动过程中进行碰撞检测和震荡消除，避免了伤害的扩大，提高了机械臂的与人协作性和安全性。

进一步的，第一机臂和第二机臂通过机臂舵机对机械臂进行驱动，结合机械臂本体的结构，机臂舵机的数量及位置设置能够影响整个机械臂的状态，对上述技术方案进行优化，所述的机臂舵机包括第一舵机、第二舵机和第三舵机，所述第一舵机设置于第一机臂的前端处，所述第二舵机和第三舵机分别设置于第二机臂的前端和末端处。本优化方案将传统协作机械臂的六自由度结构进行精简，减小了机械臂的整体体积，减少了机械臂自身的重量，减轻了舵机的负担，使机械臂运行要求降低，实用性更强。

再进一步，所述的机械臂主体与底座相对水平转动，机械臂主体通过其前端实现与底座的转动连接，在保证机械臂本体其他自由度不受影响的情况下，对上述技术方案进行优化，所述的第一机臂的前端设置有第一机箱，第一机箱与底部舵机的输出轴连接并同轴转动；所述第一舵机位于第一机箱内，第一舵机的输出轴与第一机臂的前端连接固定，第一舵机带动第一机臂绕轴转动。第一机箱起到保护底部舵机和固定第一舵机的作用。

再进一步，机械臂主体在竖直方向上实现升降，也在前后方向上实现位移，第一机臂和第二机臂的形态转换机臂舵机控制，在上述技术方案的基础上进行优化，通过所述的第一机臂和第二机臂的末端分别设置有第二机箱和第三机箱，所述的第二舵机设置于第二机箱内，第二舵机的输出轴与第二机臂固定连接并带动其绕轴转动；所述的第三舵机设置于第三机箱内，第三舵机的输出轴与执行夹具固定连接并带动其同轴转动。

进一步的，一般情况下，机械臂采用具有柔性属性的TPU材料(Thermoplasticpolyurethanes，热塑性聚氨酯弹性体橡胶)制成，其本身可发生一定程度的形变，控制和消减形变的影响可提高机械臂的运动精度，因此在上述技术方案的基础上进行优化，所述的第二机臂为方形，第一机臂上设置有上、下、左和右四个应变计，并分别设置在第一机臂的四个侧表面上，每一对应变计均采用Wheatstone半桥连接。上、下两个应变计为一对，用于检测第二机臂在竖直方向上的形变程度；左、右两个应变计为一对，用于检测第二机臂在水平方向上的形变程度。

再进一步，同理可对第一机臂进行应变检测设计，所述的第一机臂上设置有左和右两个应变计，左和右两个应变计采用Wheatstone半桥连接(直流单比电桥)。第一机臂上左、右两个应变计为一对，用于检测第一机臂在水平方向上的形变程度。

进一步的，执行夹具用于实现柔性夹取，减少执行时产生的损伤，其具体的形状结构按照如下设置，所述的执行夹具包括座体，所述的座体上设置有贯穿其前端面和后端面的轴孔，所述的挠性夹指设置于座体的前端面，所述的执行舵机设置于座体的后端面，执行舵机的输出轴通过螺纹连接有推送头，执行舵机带动推送头往复运动使挠性夹指实现开合夹取。

再进一步，为了实现柔性夹取，可采用一种优选的技术方案，所述的挠性夹指包括外层挠性件和内层挠性件，两个挠性件的前端连接后组合成V形，其中外层挠性件的末端与座体转动连接，内层挠性件的末端与推送头转动连接，推送头的往复运动带动夹持组件张开和合拢。

进一步的，挠性夹指实现挠性形变可采用一种优选的连接方式，夹指所述的座体上设置有连接脚，连接脚上设置有外层连接孔和外层连接轴，所述的外层挠性件设有与外层连接孔对应的连接孔，所述外层连接轴穿过外层连接孔和外层挠性件上的连接孔使外层挠性件相对连接脚转动；所述的推送头上设置有内层连接孔和内层连接轴，所述内层挠性件上设有与内层连接孔对应的连接孔，所述内层连接轴穿过内层连接孔和内层挠性件上的连接孔使内层挠性件相对推送头转动。

进一步的，舵机是一个闭环的整体，只支持PWM(Pulse Width Modulation，脉冲宽度调制)占空比信号实现位置单环PID(比例proportion、积分integral、微分differential)控制，并不支持外部扩展编程。为了实现协作机械臂必备的高级特性，对上述技术方案进行改进，本发明将每一个舵机的控制电路中原本的控制芯片金子那个拆除，并在所述的控制板上集成STM32系列的芯片，芯片用于根据各个舵机的电路电流变化控制其运行，如此就实现了对舵机的自由控制。

优选的，可选用STM32F4芯片。

上述内容公开说明了该协作机械臂的结构组成，本发明还提供一种柔性协作机械臂的控制方法，旨在通过电流感应判断机械臂是否发生碰撞，并对机械臂发生碰撞后进行及时调整，减少造成的损害；同时根据应变计检测值判断机械臂的动态形变，对机械臂的形变进行静态滤除，实现机械臂的震荡消除，从而使得协作机械臂在工作过程中更加稳定可靠。

本发明公开的该协作机械臂的控制方法具体包括如下步骤：

一种柔性机械臂的碰撞检测方法，用于控制上文所述的一种柔性机械臂，包括如下步骤：

S01：检测机臂舵机物理尺寸并确定其的质心和转动惯量，使用Denavit–Hartenberg参数形式进行建模，得到机械臂的完整物理模型；

S02：利用反向动力学方程，根据底部舵机、机臂舵机或执行舵机的输出轴转动角度分别计算出对应舵机维持该转动角度所需的理论扭矩值；

S03：根据关系式T＝I_estimated*Kt*i得到I_estimated，其中T为步骤S02中的理论扭矩值，Kt为电机常数，i为每个舵机对应的传动比；

S04：在机械臂运转的任意时刻，对实测电流进行一阶过滤，得到的电流值为I_filtered；

S05：预设一个threshold，并根据关系式|I_filtered-I_estimated|＞threshold进行判断，若成立则判断为发生碰撞事件，若不成立则判断为未发生碰撞事件；

S06：从底部舵机开始将每个舵机依次编号，满足发生碰撞事件条件且编号最大的舵机即刻反转，以此实现碰撞后的反弹，减少碰撞造成的伤害。

在上述步骤S03中，求得理论扭矩值，一般通过下列解析式进行表示：

Torque_estimated＝[(7766577*cos(conj(q2))*sin(q2))/200000+(1168371*sin(q3+conj(q2))*cos(conj(q3))*cos(q2))/312500+(8164863*sin(q3+conj(q2))*cos(conj(q3))*sin(conj(q4))*cos(q2)*cos(q4))/50000000+(18783207*sin(q3+conj(q2))*cos(conj(q3))*sin(conj(q4))*cos(q2)*sin(q4))/25000000+(7376271879476340519521767333347834587637*sin(q3+conj(q2))*cos(conj(q3))*sin(conj(q4))*cos(q4)*sin(q2))/2028240960365167042394725128601600000000+(7376271879476340519521767333347834587637*sin(q3+conj(q2))*sin(conj(q3))*sin(conj(q4))*cos(q2)*cos(q4))/2028240960365167042394725128601600000000+(86837139*sin(q3+conj(q2))*cos(conj(q3))^2*cos(conj(q4))*cos(q2)*sin(q4))/25000000,(1450899*cos(q2))/125000+(256041*sin(q2))/250000,0,0]。

进一步的，运行上述解析式造成较大的运算负担，因此进一步优化，考虑到机械臂不同姿态承受的重力是连续变化的，且不同角度的变化不是非常剧烈，本发明采用将舵机输出轴角度以5度为单位离散化，并将sin和cos这两个三角函数值直接事先以5度为增量计算出具体值，并以数组常量形式存储在控制芯片内。如此一来，以上力矩表达式就只涉及到简单的加减乘除，可以直接在控制板中用C语言实现，可以达到50Hz的控制频率。

进一步的，如果处在拖动示教模式，那么一旦检测并判断到以上碰撞事件，只需要根据实际电流值与理论电流值差值的大小来得到拖动示教人给出的力的大小，并从差值的正负号得到外力方向。得到外力方向与大小后，只需要向相应关节舵机发送同比例同方向的运动指令，即可实现拖动示教功能。

以上是在理想情况下对机械臂的碰撞进行检测的方法，实际情况是机械臂在运动中产生竖直方向和水平方向的震动，该震动导致机械臂的动态变形，同时机械臂采用柔性材质制成，会造成静态形变；进行震动消除可提高机械臂的精度，在震动消除中需要将该静态形变进行滤除，而静态形变往往与动态形变相互叠加，因此需要对机械臂的动态变形进行变量滤除，具体包括如下步骤：

1)使用低频一阶滤波器过滤应变计读数，记为S_lowpass；

2)使用高频一阶滤波器过滤应变计读数，记为S_highpass；

3)根据上述结果计算经过滤除的动态应变值S_filtered＝S_highpass-S_lowpass；

4)根据上述的动态应变值对舵机速度环反馈处引入应变计读数的比例控制，即：V_i＝V-V_s+S_filtered*p，其中，V_i为速度环输入速度，V为目标速度，V_s为当前速度，p为常数。

与现有技术相比，本发明的有益效果为：

1.本发明重量轻，且具有柔性，发生碰撞将吸收冲击，减少了人员损伤和机械臂损坏。

2.本发明减少了传统协作机械臂的复杂结构，降低了对驱动力的要求，使用舵机即可实现驱动，降低了协作机械臂的成本。

3.本发明采用柔性的执行夹具，能够从多种角度进行抓取，抓取的成功率高。

4.本发明使用应变计测量机械臂形变，并将形变读数引入到位置速度控制环中，实现了舵机主动抵消手臂震动，提升了机械臂的控制精度。

5.通过对舵机进行电流检测的方式，低成本地实现了碰撞检测与拖动示教。

附图说明

为了更清楚地说明本发明实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅表示出了本发明的部分实施例，因此不应看作是对范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其它相关的附图。

图1是本发明的整体结构示意图。

图2是本发明的分解结构示意图。

图3是执行夹具的整体结构示意图。

图4是本协作机械臂的碰撞检测过程示意图。

图5是本协作机械臂进行消震的过程原理图。

图中：1-底座；2-第一机箱；3-第一机臂；4-第二机箱；5-第二机臂；6-第三机箱；7-执行舵机；8-座体；9-挠性夹指；901-外层挠性件；902-内层挠性件；10-第一舵机；11-第二舵机；12-第三舵机；13-推送头；14-连接脚。

具体实施方式

下面结合附图及具体实施例对本发明做进一步阐释。

实施例1：

如图1、图2、图3所示，本实施例公开了一种柔性协作机械臂，包括底座1、机械臂主体和执行夹具，所述机械臂主体的底端与底座转动连接，执行夹具设于机械臂主体的顶端。具体地说，所述的底座内设置有控制板和底部舵机，底部舵机用于驱动机械臂水平转动；所述的机械臂主体包括转动连接的第一机臂3和第二机臂5，第一机臂和第二机臂由机臂舵机驱动后在竖直方向相对转动；所述的执行夹具包括多个挠性夹指9，执行夹具由执行舵机7驱动夹取；所述的底部舵机、机臂舵机和执行舵机均电连接至控制板，且每个舵机与控制板之间均连接有电流传感器。

本实施例通过底座和机械臂主体驱动执行夹具实现夹取操作，底座实现机械臂在水平方向的转动，机械臂主体上的机臂舵机实现竖直方向上的运动，执行夹具实现挠性夹取，避免了在夹取过程中造成损坏，同时机械臂整体在运动过程中进行碰撞检测和震荡消除，避免了伤害的扩大，提高了机械臂的与人协作性和安全性。

优选的，本实施例中，整个协作机械臂通过3D打印技术进行制作。

第一机臂和第二机臂通过机臂舵机对机械臂进行驱动，结合机械臂本体的结构，机臂舵机的数量及位置设置能够影响整个机械臂的状态，对上述技术方案进行优化，所述的机臂舵机包括第一舵机10、第二舵机11和第三舵机12，所述第一舵机设置于第一机臂的前端处，所述第二舵机和第三舵机分别设置于第二机臂的前端和末端处。本优化方案将传统协作机械臂的六自由度结构进行精简，减小了机械臂的整体体积，减少了机械臂自身的重量，减轻了舵机的负担，使机械臂运行要求降低，实用性更强。

本实施例中，所述的机械臂主体与底座相对水平转动，机械臂主体通过其前端实现与底座的转动连接，在保证机械臂本体其他自由度不受影响的情况下，对上述技术方案进行优化，所述的第一机臂的前端设置有第一机箱2，第一机箱与底部舵机的输出轴连接并同轴转动；所述第一舵机位于第一机箱内，第一舵机的输出轴与第一机臂的前端连接固定，第一舵机带动第一机臂绕轴转动。第一机箱起到保护底部舵机和固定第一舵机的作用。

机械臂主体在竖直方向上实现升降，也在前后方向上实现位移，第一机臂和第二机臂的形态转换机臂舵机控制，在上述技术方案的基础上进行优化，通过所述的第一机臂和第二机臂的末端分别设置有第二机箱4和第三机箱6，所述的第二舵机设置于第二机箱内，第二舵机的输出轴与第二机臂固定连接并带动其绕轴转动；所述的第三舵机设置于第三机箱内，第三舵机的输出轴与执行夹具固定连接并带动其同轴转动。

本实施例中，机械臂采用具有柔性属性的TPU材料(Thermoplasticpolyurethanes，热塑性聚氨酯弹性体橡胶)制成，其本身可发生一定程度的形变，控制和消减形变的影响可提高机械臂的运动精度，因此在上述技术方案的基础上进行优化，所述的第二机臂为方形，第一机臂上设置有上、下、左和右四个应变计，并分别设置在第一机臂的四个侧表面上，每一对应变计均采用Wheatstone半桥连接。上、下两个应变计为一对，用于检测第二机臂在竖直方向上的形变程度；左、右两个应变计为一对，用于检测第二机臂在水平方向上的形变程度。

同理可对第一机臂进行应变检测设计，所述的第一机臂上设置有左和右两个应变计，左和右两个应变计采用Wheatstone半桥连接。第一机臂上左、右两个应变计为一对，用于检测第一机臂在水平方向上的形变程度。

执行夹具用于实现柔性夹取，减少执行时产生的损伤，其具体的形状结构按照如下设置，所述的执行夹具包括座体8，所述的座体上设置有贯穿其前端面和后端面的轴孔，所述的挠性夹指设置于座体的前端面，所述的执行舵机设置于座体的后端面，执行舵机的输出轴通过螺纹连接有推送头13，执行舵机带动推送头往复运动使挠性夹指实现开合夹取。

为了实现柔性夹取，可采用一种优选的技术方案，所述的挠性夹指包括外层挠性件和内层挠性件，两个挠性件的前端连接后组合成V形，其中外层挠性件901的末端与座体转动连接，内层挠性件902的末端与推送头转动连接，推送头的往复运动带动夹持组件张开和合拢。

挠性夹指实现挠性形变可采用一种优选的连接方式，夹指所述的座体上设置有连接脚，连接脚上设置有外层连接孔和外层连接轴，所述的外层挠性件设有与外层连接孔对应的连接孔，所述外层连接轴穿过外层连接孔和外层挠性件上的连接孔使外层挠性件相对连接脚转动；所述的推送头上设置有内层连接孔和内层连接轴，所述内层挠性件上设有与内层连接孔对应的连接孔，所述内层连接轴穿过内层连接孔和内层挠性件上的连接孔使内层挠性件相对推送头转动。

舵机是一个闭环的整体，只支持PWM占空比信号实现位置单环PID控制，并不支持外部扩展编程。为了实现协作机械臂必备的高级特性，对上述技术方案进行改进，本发明将每一个舵机的控制电路中原本的控制芯片金子那个拆除，并在所述的控制板上集成STM32系列的芯片，芯片用于根据各个舵机的电路电流变化控制其运行，如此就实现了对舵机的自由控制。

本实施例中，可选用STM32F4芯片。

实施例2：

如图4、图5所示，实施例1公开说明了该协作机械臂的结构组成，本实施例提供一种柔性协作机械臂的控制方法，旨在通过电流感应判断机械臂是否发生碰撞，并对机械臂发生碰撞后进行及时调整，减少造成的损害；同时根据应变计检测值判断机械臂的动态形变，对机械臂的形变进行静态滤除，实现机械臂的震荡消除，从而使得协作机械臂在工作过程中更加稳定可靠。

本实施例公开的该协作机械臂的控制方法具体包括如下步骤：

一种柔性机械臂的碰撞检测方法，用于控制实施例1所述的一种柔性机械臂，包括如下步骤：

S01：检测机臂舵机物理尺寸并确定其的质心和转动惯量，使用Denavit–Hartenberg参数形式进行建模，得到机械臂的完整物理模型；

S02：利用反向动力学方程，根据底部舵机、机臂舵机或执行舵机的输出轴转动角度分别计算出对应舵机维持该转动角度所需的理论扭矩值；

S03：根据关系式T＝I_estimated*Kt*i得到I_estimated，其中T为步骤S02中的理论扭矩值，Kt为电机常数，i为每个舵机对应的传动比；

S04：在机械臂运转的任意时刻，对实测电流进行一阶过滤，得到的电流值为I_filtered；

S05：预设一个threshold，并根据关系式|I_filtered-I_estimated|＞threshold进行判断，若成立则判断为发生碰撞事件，若不成立则判断为未发生碰撞事件；

S06：从底部舵机开始将每个舵机依次编号，满足发生碰撞事件条件且编号最大的舵机即刻反转，以此实现碰撞后的反弹，减少碰撞造成的伤害。

在上述步骤S03中，求得理论扭矩值，一般通过下列解析式进行表示：

Torque_estimated＝[(7766577*cos(conj(q2))*sin(q2))/200000+(1168371*sin(q3+conj(q2))*cos(conj(q3))*cos(q2))/312500+(8164863*sin(q3+conj(q2))*cos(conj(q3))*sin(conj(q4))*cos(q2)*cos(q4))/50000000+(18783207*sin(q3+conj(q2))*cos(conj(q3))*sin(conj(q4))*cos(q2)*sin(q4))/25000000+(7376271879476340519521767333347834587637*sin(q3+conj(q2))*cos(conj(q3))*sin(conj(q4))*cos(q4)*sin(q2))/2028240960365167042394725128601600000000+(7376271879476340519521767333347834587637*sin(q3+conj(q2))*sin(conj(q3))*sin(conj(q4))*cos(q2)*cos(q4))/2028240960365167042394725128601600000000+(86837139*sin(q3+conj(q2))*cos(conj(q3))^2*cos(conj(q4))*cos(q2)*sin(q4))/25000000,(1450899*cos(q2))/125000+(256041*sin(q2))/250000,0,0]。其中q1、q2、q3、q4分别为底部舵机、第一舵机、第二舵机、第三舵机的输出轴转动角度。

运行上述解析式造成较大的运算负担，因此进一步优化，考虑到机械臂不同姿态承受的重力是连续变化的，且不同角度的变化不是非常剧烈，本发明采用将舵机输出轴角度以5度为单位离散化，并将sin和cos这两个三角函数值直接事先以5度为增量计算出具体值，并以数组常量形式存储在控制芯片内。如此一来，以上力矩表达式就只涉及到简单的加减乘除，可以直接在控制板中用C语言实现，可以达到50Hz的控制频率。

如果处在拖动示教模式，那么一旦检测并判断到以上碰撞事件，只需要根据实际电流值与理论电流值差值的大小来得到拖动示教人给出的力的大小，并从差值的正负号得到外力方向。得到外力方向与大小后，只需要向相应关节舵机发送同比例同方向的运动指令，即可实现拖动示教功能。

以上是在理想情况下对机械臂的碰撞进行检测的方法，实际情况是机械臂在运动中产生竖直方向和水平方向的震动，该震动导致机械臂的动态变形，同时机械臂采用柔性材质制成，会造成静态形变；进行震动消除可提高机械臂的精度，在震动消除中需要将该静态形变进行滤除，而静态形变往往与动态形变相互叠加，因此需要对机械臂的动态变形进行变量滤除，具体包括如下步骤：

1)使用低频一阶滤波器过滤应变计读数，记为S_lowpass；

2)使用高频一阶滤波器过滤应变计读数，记为S_highpass；

3)根据上述结果计算经过滤除的动态应变值S_filtered＝S_highpass-S_lowpass；

4)根据上述的动态应变值对舵机速度环反馈处引入应变计读数的比例控制，即：V_i＝V-V_s+S_filtered*p，其中，V_i为速度环输入速度，V为目标速度，V_s为当前速度，p为常数。

以上即为本发明列举的几种实施方式，但本发明不局限于上述可选的实施方式，本领域技术人员可根据上述方式相互任意组合得到其他多种实施方式，任何人在本发明的启示下都可得出其他各种形式的实施方式。上述具体实施方式不应理解成对本发明的保护范围的限制，本发明的保护范围应当以权利要求书中界定的为准，并且说明书可以用于解释权利要求书。

Claims (10)

1.一种柔性协作机械臂，包括底座(1)、机械臂主体和执行夹具，所述机械臂主体的底端与底座转动连接，执行夹具设于机械臂主体的顶端；其特征在于：所述的底座内设置有控制板和底部舵机，底部舵机用于驱动机械臂水平转动；所述的机械臂主体包括转动连接的第一机臂(3)和第二机臂(5)，第一机臂和第二机臂由机臂舵机驱动后在竖直方向相对转动；所述的执行夹具包括多个挠性夹指(9)，执行夹具由执行舵机(7)驱动夹取；所述的底部舵机、机臂舵机和执行舵机均电连接至控制板，且每个舵机与控制板之间均连接有电流传感器。

2.根据权利要求1所述的柔性协作机械臂，其特征在于：所述的机臂舵机包括第一舵机(10)、第二舵机(11)和第三舵机(12)，所述第一舵机设置于第一机臂的前端处，所述第二舵机和第三舵机分别设置于第二机臂的前端和末端处。

3.根据权利要求2所述的柔性协作机械臂，其特征在于：所述的第一机臂的前端设置有第一机箱(2)，第一机箱与底部舵机的输出轴连接并同轴转动；所述第一舵机位于第一机箱内，第一舵机的输出轴与第一机臂的前端连接固定，第一舵机带动第一机臂绕轴转动。

4.根据权利要求2所述的柔性协作机械臂，其特征在于：所述的第一机臂和第二机臂的末端分别设置有第二机箱(4)和第三机箱(6)，所述的第二舵机设置于第二机箱内，第二舵机的输出轴与第二机臂固定连接并带动其绕轴转动；所述的第三舵机设置于第三机箱内，第三舵机的输出轴与执行夹具固定连接并带动其同轴转动。

5.根据权利要求2所述的柔性协作机械臂，其特征在于：所述的第二机臂为方形，第一机臂上设置有上、下、左和右四个应变计，并分别设置在第一机臂的四个侧表面上，每一对应变计均采用直流单比电桥连接。

6.根据权利要求2所述的柔性协作机械臂，其特征在于：所述的第一机臂上设置有左和右两个应变计，左和右两个应变计采用直流单比电桥连接。

7.根据权利要求1所述的柔性协作机械臂，其特征在于：所述的执行夹具包括座体(8)，所述的座体上设置有贯穿其前端面和后端面的轴孔，所述的挠性夹指设置于座体的前端面，所述的执行舵机设置于座体的后端面，执行舵机的输出轴通过螺纹连接有推送头(13)，执行舵机带动推送头往复运动使挠性夹指实现开合夹取。

8.根据权利要求1所述的柔性协作机械臂，其特征在于：所述的控制板上集成STM32系列的芯片，芯片用于控制各个舵机的运行。

9.一种柔性机械臂的碰撞检测方法，用于控制权利要求1～8任一项所述的一种柔性机械臂，其特征在于，包括如下步骤：

S01：检测机臂舵机物理尺寸并确定其的质心和转动惯量，使用Denavit–Hartenberg参数形式进行建模，得到机械臂的完整物理模型；

S02：利用反向动力学方程，根据底部舵机、机臂舵机或执行舵机的输出轴转动角度分别计算出对应舵机维持该转动角度所需的理论扭矩值；

S03：根据关系式T＝I_estimated*Kt*i得到I_estimated，其中T为步骤S02中的理论扭矩值，Kt为电机常数，i为每个舵机对应的传动比；

S04：在机械臂运转的任意时刻，对实测电流进行一阶过滤，得到的电流值为I_filtered；

S05：预设一个threshold，并根据关系式|I_filtered-I_estimated|＞threshold进行判断，若成立则判断为发生碰撞事件，若不成立则判断为未发生碰撞事件；

S06：从底部舵机开始将每个舵机依次编号，满足发生碰撞事件条件且编号最大的舵机即刻反转，以此实现碰撞后的反弹，减少碰撞造成的伤害。

10.根据权利要求9所述的柔性机械臂的碰撞检测方法，其特征在于，机械臂在运动中产生竖直方向和水平方向的震动，该震动导致机械臂的动态变形，对机械臂的动态变形进行变量滤除，包括如下步骤：

1)使用低频一阶滤波器过滤应变计读数，记为S_lowpass；

2)使用高频一阶滤波器过滤应变计读数，记为S_highpass；

3)根据上述结果计算经过滤除的动态应变值S_filtered＝S_highpass-S_lowpass；

4)根据上述的动态应变值对舵机速度环反馈处引入应变计读数的比例控制，即：V_i＝V-V_s+S_filtered*p，其中，V_i为速度环输入速度，V为目标速度，V_s为当前速度，p为常数。