FORCE检测与反馈实例

1. 文档概述
   1. 介绍

本文大体上分为三个部分，一是阐述gripper夹物判断的实现方法和物理原理。二是利用gripper夹物判断和Omega配合后实现的力反馈系统的结构和原理。三是现有的问题。

一、gripper夹物判断的实现方法和物理原理

         先从底层硬件驱动电路的角度出发，对gripper电机进行了动力学分析，得出了电机匀速时，电机速度、PWM占空比和检测电流的数学关系，并利用此数学模型结合实测结果对该速度电流曲线做标定，然后做一些理论上可以允许的简化和假设。据此，我们可以从电机速度和驱动PWM占空比的信息算出器械在空载时的加速度理论值，利用该值和从编码器获得的加速度实际值做对比，作为gripper夹物判断的依据

• 力反馈系统的结构和原理

有了夹物判断的方法后，将其实现在PC端的下位机中，同时且实时地将夹物判断的结果上传到PC端的上位机(Omega控制程序)、下发到器械端的单片机(器械Tool控制程序)中。上位机通过夹物判断的结果和Omega-Gripper的开合角度动态地调整反馈到手上的作用力，单片机通过夹物判断的结果和Omega下发的位置控制指令实时地切换电机控制方式(力控制和位置控制)，整个系统便是一个带有力反馈效果的主从跟随系统。

1. 1. 适用范围

研发部软件、硬件驱动相关研发成员。

1. Gripper夹物判断方法
   1. 电机驱动电路分析

要着手分析电机的力学模型，要先了解电机的驱动芯片的结构。

图二 直流电机驱动芯片MAX14871数据手册中的推荐电路

表一 直流电机驱动芯片MAX14871数据手册中的控制逻辑表

图二和表一均为器械控制板所选用的直流电机驱动芯片MAX14871的数据手册中的内容。不妨设流经电机线圈的电流为，流经电阻上的电流为，该电流可被单片机的ADC采样得到。我们看到电机的驱动电路为H全桥结构，当芯片的使能脚使能后，若此时PWM控制信号为1(高电位)，电机线圈一端接Vdd另一端接GND，此时=±(符号由DIR脚的电平决定)。相反的，若此时PWM控制信号为0(低电位)，电机线圈两端都接GND，电机线圈短接，此时电机线圈的电流很大，该电流阻止着电机的进一步转动，电机处于高阻尼的刹车状态会马上停下。而这个过程中的=0，意味着我们得不到此时电机线圈的电流。而在理论上，我们可以知道=，其中为电机转动引起的反向电动势，为电机线圈内阻，为线圈电感的复阻抗。我们又知道正比于电机旋转角速度，比例系数为，即=(取决于电机内部磁场、线圈结构。同样的，电机产生的输出力矩与线圈电流也成正比，比例系数也为这个，=，我们在电机的选型手册中可以找到这个，如图三，对于我们使用的24V电机，其Torque constant即是 = 16mNm/A，同样也可以由Speed constant这一项求出:= 60/(2*Speed constatnt)=60/(2*595)=0.0160Nm/A )。

图三

1. 1. Gripper匀速开合，电机速度与电流的关系

由上节我们知道，当PWM信号为1时，电机有个正向加速力矩=*，此时电机加速。当PWM信号为0时，电机有个反向力矩==（为此时的），此时电机刹车。而PWM控制信号的频率很快（目前BMR手术系统中为15.69KHz）使得电机不停在加速减速下切换以至于从宏观上来看，当PWM信号占空比一定时，电机匀速转动。为了保持电机匀速，由角动量守恒可得：

               ①            其中d为PWM占空比（0≤d≤1）

从电学角度：

                           ②            其中为驱动电压24V，=

单片机的ADC测到的电流值为在整个PWM周期内的平均值

=                                    ③

由①②③可以得出电机匀速运动时单片机获取到的电机电流与电机转速和占空比之间的关系：

                    ④            其中为驱动电压24V，=

 =                                              ⑤  

将此处的记为。

对于目前15.69KHz的PWM控制频率，我们取其一阶基频，近似认为==

再由图三得到电机的线圈电阻和电感分别为，，可以画出如下曲线

图四 恒定负载且匀速转动时电机速度电流理论曲线

图五 恒定负载且匀速转动时PWM占空比和速度理论曲线

我们可以得出：随着电机转速的加快先增大，最大时为，此时电机发热最大，效率最低。转速继续增大，电流开始下降，直到电机产生的反向电动势和激励电压相抵消，电流为0，此时转速最快。该曲线结果中的电流趋势和最大值和实际测量结果非常符合。

而PWM占空比和电机转速呈简单的线性关系。

而实际中，还要考虑摩擦力的影响，包括电机内部的转动摩擦力矩，器械传动时带来的摩擦力矩等等，把他们一并记为(由于器械加工精度和不同开合角度下力传导的非线性，摩擦力不仅和角速度有关，还和角位置有关，为了简化问题，我们假设只与角速度有关)

①式改写为

                ❶

结合②③可以推出：

                     ❹

                                        ❺

由于此时的是电机匀速转动时的电流测量值，将其改记作。并将写成两部分的组合=+，其中即等于④式，为非摩擦力部分。为摩擦力部分。

由于对摩擦力的数学建模比较复杂而且一致性较差，我们采用实验的办法进行标定。

图五 5mm持针器驱动电机转速与电流曲线

考虑到实际应用场景中电机转速很少会高于6000rpm/min，因此我们只测量了电机转速在7000rpm/min以下的情形，可以看到，在低速情况下，实际与理论值电流值很接近，摩擦力项很小。当电机转速提升，实际与理论值差距变大，项影响不可忽视。

1. 1. gripper夹物判断

由于实际的操作器械过程中，电机不可能处于匀速状态，始终有个加速度，记为。即不可能永远等于，他们的差值反映了电机加速度，由电机的动力学关系可知：

      ⑥  

其中为考虑到不同占空比下的的修正值：

为加入器械后电机的等效转动惯量（如同，为了简化问题，我们假定其为常量，并由实验测量标定得出）。

由此，我们可以根据电机的转速、测量得到的电机电流、PWM控制信号的占空比这三个已知量算出电机在这一时刻的加速度，与通过编码器获得的电机在这一时刻的实际加速度做比较，作为器械夹物判断的依据。

  ⑦

如果大于某个门限，如>=0.5，就意味着电机实际加速度还没有达到计算得出的加速度值的一半，认为器械前端gripper受到了外物阻挡，使得整体的电机等效转动惯量变大了。

综上，给出了一种实时性较高且较灵敏的判断gripper是否夹到物体的方法。

1. 1. Gripper夹物判断方法的实验验证

我们使用二自由度5mm持针器做了实际测试。实际使用中，我们只考虑gripper闭合时有没有碰到物体而受阻，不考虑gripper张开时的情况。我们将的门限设置为60%，并且只有当理论值大于某个门限（否则，容易产生误判）时，才进行夹物判断。实验结果表明，夹物判断算法运行良好，可以非常灵敏地检测到一个很小的外力，空载时在器械的整个自由行程范围内不会产生夹物误判。夹物检测的结果有50~100ms的判断延时。

1. 力反馈系统的结构
   1. 总体结构

图六为目前实现的力反馈系统的总体结构

图六 力反馈系统的总体结构

夹物检测的运算在PC端的下位机中实现，它将判断结果同时上传给上位机、下发给单片机。

1. 1. 单片机控制电机部分逻辑

图七 驱动板MCU控制逻辑

单片机把和电机有关的电流、PWM占空比、电机速度(加速度)等信息上传给下位机。下位机根据此信息结合夹物检测算法判断gripper有无夹物并回传给单片机。

单片机根据gripper位置指令和夹物判断的结果判断电机控制模块应该工作在哪个模式。

如果gripper位置指令在gripper行程范围之内并且没有检测到夹到物体，单片机对电机进行位置控制。如果位置指令小于gripper的零位，或者检测到gripper夹到物体，进入电流环控制电机，电流值的设置取决于从上位机下发的Omega_Force。

1. 1. 上位机逻辑

图八 上位机逻辑

图八中的SpringGate是上位机为Omega设置弹性力的参考原点，只有当Omega的Grip自由度的开合角度OmegaGripperPosition小于SpringGate时，上位机才会为Omega的Grip自由度设置一个正比于他们差值的弹簧力。

当检测到gripper夹到物体时，SpringGate会被设置成当前Omega的Grip自由度的开合角度。此后，若操纵者继续往内压Gripper，操纵者的手上会感受到一个反作用力，会有压到弹簧的感觉。并且，此时的反作用力大小还会下发给单片机，单片机以此为依据设置电流环的电流值。这样，可以把操作者手上的力和电机进入电流模式下的输出转矩对应起来。

1. 遇到的问题

1. 如何让Omega施加给操作者手上的反作用力OmegaGripperForce正比于器械前端gripper的夹持力。

目前让Omega产生力是通过Omega SDK中提供的API ，SetGripperForce（...）（单位牛顿）实现的，其准确度有待验证。这个力的值经过线性放大对应电机的设置电流值，电流值的设置精度取决于电流环的性能。这个电流值使得器械前端产生夹持力，我们认为当器械夹到物体静止后，电流值和末端夹持力成正比。

整个过程中，在理论上都是线性的，即操作者施加了一个力F1后，经过放大，在器械前端产生一个夹持力F2，F2=kF1，K为常数。具体是否线性，线性度如何，需要工装去测量。

1. 实验中发现，当gripper夹到物体后omega继续向内挤压，器械前端夹持力会增大。但是，当操作者慢慢松开OmegaGripper，即将手上力慢慢撤掉的过程中（但不完全撤掉），器械前端的夹持力还是会保持在之前的最大值，并不会和手上的力一起慢慢变小。直到操作者将手上的力完全撤掉，电机进入位置模式，器械前端的夹持力才会消失。

这可以说是一个问题，也可以说是一种特性。这是由于电机驱动芯片在不工作时（PWM信号为低时）使得电机处在线圈短接的刹车模式下。即使不给电机驱动电流，电机也会维持不转，导致gripper因夹到物体导致的弹性形变不能恢复进而力也依旧保持着。对于持针器而言，这个特性可以使医生的手上不必全程保持最大的力就能达到最大夹持力的效果，是个优点。而对于其他器械而言，这个特性会使得医生发现自己用力过大后难以通过减小手上的力使器械端的力变小，是个缺点。

1. 夹物判断有延迟。

目前夹物判断的算法有50ms左右的延迟，导致的问题在操作者闭合OmegaGripper的过程中，如果操作速度太快，即使器械gripper夹到了物体，已经被卡住，而上位机的SpringGate无法捕捉到真正夹到物体时的器械角度，使得操作者误以为gripper闭合的过程中并没有夹到物体。

解决的办法是减小夹物判断门限，使其变得更灵敏，但是如果把夹物检测设置得太灵敏，有可能会在器械空载的自由行程中触发夹物误判，影响操作者使用感受。