一种基于关节力矩的机械臂拖动引导定位的方法与流程

本发明涉及机械臂控制技术领域，具体涉及一种基于关节力矩的机械臂拖动引导定位方法。

背景技术：

机器人辅助手术在现代手术中正在发挥越来越重要的作用。机器人脱胎于现代计算机技术，具有高精度高稳定性的特点。相比传统手术效果高度依赖医生经验，机器人辅助手术通过视觉提示和机械臂引导，将手术流程数字化，极大的降低了手术效果对医生经验的依赖程度。

机械臂在机器人辅助手术中通常起到引导定位的作用，比如在骨科手术中起引导截骨位置、引导假体安装角度的作用，而在脊柱和神经外科领域常常用来定位穿刺针位置。与机械臂在工业上的应用不同，应用在手术场景的机械臂通常和人会处于同一工作空间，因此对机械臂运行的安全性就提出了比较高的要求。目前常用的技术方案是让机械臂主动运动到定位位置，并监测机械臂本体受到的外力，当外力超过一定阈值就判定为碰撞，机械臂立即进入限制运动的状态。这种方案在手术场景中依然会存在不小的安全风险，并且机械臂和医生配合度不高，使用的方便程度也常常被诟病。

技术实现要素：

本发明的目的在于克服现有技术的不足，提供一种基于关节力矩的机械臂拖动引导定位的方法，可以现有技术应用在导航辅助手术中，机械臂难以进行手动引导定位的技术难题。

为了达到上述技术目的，本发明的提供一种基于关节力矩的机械臂拖动引导定位的方法，包括以下步骤：

s1.对于不同类型的机械臂，选用合适的方法获得机械臂关节的力矩数据；

s2.确定末端工具的工作点，通过标定的方法，确定该工作点基于机械臂法兰的位置和姿态，以该工作点为原点建立工具坐标系；

s3.对机械臂建立动力学模型，使用参数辨识的方法对步骤s1中获得的关节力矩数据进行解耦，分离出施加在工具坐标系上外力导致的关节力矩，再根据关节外力矩计算出工具坐标系上的外力和外力矩；

s4.使用低通fir滤波器对步骤s3中计算得到的外力和外力矩做滤波处理；

s5.运用速度控制器，将步骤s4中得到的滤波后的力和力矩作为控制器的输入，输出基于工具坐标系的期望速度。该控制器可以实现机械臂工作空间内的自由拖动，也可通过设置限制条件，让末端工具可以沿着一定轨迹或在定义的范围内拖动；

s6.将工具坐标系的目标速度，转化为机械臂关节坐标系的控制指令，直接驱动机械臂的运动。

本发明所提出的基于关节力矩的机械臂拖动引导定位的方法，有益效果如下：与机械臂主动运动定位相比，让操作者手动引导机械臂运动，机械臂提供部分运动自由度的限制，这种方案在辅助手术领域安全性更高，也更符合医生操作习惯；这种拖动引导的方式可以完全依赖于机械臂本体已有的传感器，而不需要外装力矩传感器，降低了企业的生产成本；在控制系统的设计上，使用低通fir滤波器对数据进行预处理降低了机械臂在运行过程中发生严重抖动的风险；使用速度控制策略不仅提高了拖动的柔顺度，并且让机械臂能够在有限制条件的情况下拖动运行；引入误差阈值，可以让机械臂能够在无外力施加的情况下稳定悬停。该方法最后的控制结果直接施加于机械臂的关节驱动器，控制指令直接且明确，确保各类不同开发程度的机械臂都能够应用该方法，体现了该方法的广泛适用性。

附图说明

图1是本发明提供的基于关节力矩的机械臂拖动引导定位方法的流程图。

图2是本发明提供的基于关节力矩的机械臂拖动引导定位方法中速度控制器的设计流程图。

具体实施方式

为使本发明的技术细节阐述清楚，以下结合附图及实例，对发明的内容进行进一步详细说明。

如图1所示，本发明提供的基于关节力矩的机械臂拖动引导定位方法，包括以下步骤：

s1.机械臂关节力矩数据的获取；

为了使表述更加准确，特此申明，以下提到的关节力矩，指的是传导扭矩，即连杆和连杆之间产生的所有力矩的矢量和。根据机械臂类型的不同，关节力矩的获取方式可以分为三种，电机电流估计、关节扭转幅度估计和应变计测量。其中电机电流估计利用了电机的电流值和施加在该关节上的力矩成比例关系的特性，将测量测到的电流值和电机常量相乘可以算得电机扭矩，再选用合适的摩擦力模型补偿传动摩擦力，即可得到关节力矩。摩擦力的估计常用非线性摩擦力模型，由taghirad和belanger在1998年提出，属于本领域技术人员应当熟悉的技术，此处不再详述。关节扭转幅度估计是基于谐波减速机本身具有一定结构弹性的特性，在关节受到外力扭转时，电机端和连杆端的会发生扭转角度差，该角度差和在轻负载的情况下和减速机上的扭矩成线性关系，在重负载的情况下需要加上立方非线性刚度的表达式。关节扭转幅度估计也会有耗散作用，因此和电机电流估计方法一样，需要考虑合适的摩擦力模型来补偿。不同的是，对于这种估计方法，选用粘滞摩擦模型更加合适。第三种方法直接使用应变计测量连杆之间的扭矩，使用上更加直接方便，应变计的输出即为关节力矩，而无需再对摩擦力进行补偿。

s2.工具坐标系的建立；在机械臂辅助手术的应用场景中，机械臂通常需要安装末端工具，以适应不同操作步骤的需要。在引导定位的操作下，定位的限制条件一般不会设置在机械臂末端法兰坐标系，而会设置在末端工具的工作点上。因此，为了计算的方便，需要设置一个末端工具坐标系，这个坐标系的定义和定位限制条件相匹配。

在一些对精度要求不高的使用场景中，有时会从机械图纸直接读取工具坐标系所需要的参数值，这种方法忽略了机械加工误差和安装过程中引入的配合误差，对于手术辅助这种需要高精度控制的场景来说，并不是一个较优的选择。一个替代的方案是，将工具先安装到末端法兰上，再将末端工具的工作点以不同姿态移向一个空间中的参考点，重复四次，即可计算出工作点在法兰坐标系里的位置。工作点坐标系的姿态可以通过两点法，将自定义的某个轴上的一个点和另一个该轴所在平面上的点示教给机械臂，即可定义了工作点坐标系的姿态。至此，工作点坐标系的定义完成，以下将所有的工作点坐标系统称为工具坐标系。

s3.工具坐标系上作用外力计算。步骤s1获取了机械臂各个关节的力矩，这个力矩包含了机械臂本体的质量、末端工具的质量、外部的作用力和运动时加速度产生的惯性。而在控制系统的设计中，通常只会把外部作用力作为控制系统的输入项，所以此处需要对关节力矩进行处理，拆分出外部作用力导致的力矩部分，再通过正向运动学，将作用在关节上的外力映射到工具坐标系上，计算出作用在工具坐标系上的外力和外力矩。

参数识别技术可以很好的用来识别本体质量和运动惯性导致的关节力矩。对于不同类型的机械臂，参数辨识方法也会有差别。大致的流程为先对机械臂运动时的传感器读数进行采样，获得不同时刻的关节角度位置和关节力矩值，将数据代入动力学方程，再将待辨识的参数进行分离后解算超定方程，即可辨识出机械臂各连杆的质量、质心、长度、惯量等参数。对于安装了末端工具的机械臂，若是工具和末端法兰是刚性连接，可以把工具装载在机械臂上进行识别，识别出来的参数会将机械臂最后一个连杆和工具考虑为一个整体，计算出来的关节力矩即为本体加上工具在运动状态下施加在各个关节上的力矩值。然后用实际测得的关节力矩值减去辨识得到的力矩数值，就获得了各个关节上承受的外部力矩。

在手术辅助的应用中，外力一般是施加于末端工具上，控制器的设计也是基于工具坐标系的。将上述获得的关节外部力矩转换到工具坐标系，可以通过引入雅可比矩阵来实现。

τext＝j·fext

其中j为雅可比矩阵，fext为关节外力矩，τext为关节外力矩工具坐标系上的力和力矩值。需要澄清，所有笛卡尔空间坐标系中提到的力矩，指的是对应坐标系上xyz轴的分力矩。对于六轴或六轴以上构型的机械臂，τext应当为六维向量，分别为对应坐标系上三个轴向的力和力矩。

s4.数据滤波；在实际引用中发现，从步骤s3中获得的工具坐标系力和力矩数据偶尔会出现比较大幅度的波动，这样的波动若是直接导入控制器，会对控制器的稳定性产生比较大的考验，因此较优的做法是先对数据进行滤波处理。滑窗平均滤波器是一种低通fir滤波器，处理这种偶尔发生高频振荡的数据流有很好的效果。

首先滤波器需要设定一个滑窗的长度，滑窗内数据的初始值都设为0。当采样数据被获取时，该数据会从滑窗的一端加入队列，而另一端的一个数据将会丢失，滑窗的总长度保持不变。每个时刻滑窗滤波器都可以向外输出一个滤波后的数值，数值大小等于当前时刻滑窗内所有数值的平均值。用数学表达式表示如下：

其中n表示滑窗的总长度，这个长度永远保持不变，ai表示当前时刻滑窗内第i个数据，fout为滤波器输出的滤波处理后的数据。该滤波器只有滑窗长度一个变量可以调节，滑窗长度越大，数据的变化就越平缓，控制器的对力的变化的响应时间越长。在机械臂辅助手术的应用场景中，通常会对力和力矩采用不同的滤波器参数，可以构造两个滑窗长度不同的滤波器，来分别满足力和力矩的滤波需求。

s5.速度控制器；该系统的运动控制采用速度控制策略，以工具坐标系为参考的速度和转速作为被控制变量。首先需要对末端工具做受力分析，然而末端工具在装载情况下，受力情况受到机械臂运动情况影响，难以形成解析的数学模型。因此，这里末端工具的受力分析是处于未被装载情况下的，运动仅受到外界拉力和摩擦力的共同作用。在工具质心上的线性运动模型可以有以下表达：

上式的m表示工具的质量，和分别表示运动的加速度和速度，f表示库伦和粘性摩擦力，f表示外界施加的拉力，b是阻尼系数。由于在辅助手术应用中，机械臂不会处于速度高频变化的状态下，也就是说，惯性对受力的影响非常有限，因此上式可以被进一步简化：

由于力矩传感器的零点位置不可能像理论值一样准确，并且在做参数辨识是也会有误差引入，这些误差会导致若是直接使用上式做速度控制，在没有施加外力的情况下，机械臂也会缓慢朝着某个方向逐渐偏移。为了消除这些误差带来的影响，需要设置一个合适的阈值，让机械臂承受外力达到某个程度以后，才能开始运动。所以，速度模型的表达式被修改为：

式中的ft即为误差阈值。可以发现，在上式中不再包含摩擦力项，因为摩擦力在这也可以被看作是误差的一部分，误差阈值可以囊括摩擦力带来的影响。调节常量参数b，可以调节外力和运动速度之间的转化关系，合适的参数选择可以在保证机械臂不会振荡的同时，有较优的柔顺性。

同理，对于转动运动也可以使用类似的表达:

为转动的速度，t为外部施加的力矩，tt为转动的误差阈值，常量β用来调节力矩和转动速度间的关系。

在做引导定位的应用中，机械臂末端运动的轨迹通常需要做一些限制，以达到末端工具可以沿着预设轨迹拖动的效果。在之前的分析中，当超过误差阈值后，速度方向和外力方向始终保持一致。而在有限制的运动中，实际运动速度方向将不与施加外力方向相同。对于直线型的运动，将与外力方向相同的速度投影到预设的直线方向上，生成新的速度x^·‘，该速度即为加了限制后的工具坐标系期望速度。对于平面型的运动，也可采取类似的投影方法。较为复杂的运动条件是在非规则曲线或曲面上拖动。对于这类非规则运动，需要根据当前位置，求预设曲线或曲面上的切线或切面，再做到该切线或切面的投影，作为当前时刻的速度方向。对于旋转运动速度也可以采用相似的投影方法得到符合轨迹条件的旋转速度θ^·’，此处将不再赘述。

s6.关节运动指令。从s5中已经获得了某时刻期望达到的工具坐标系速度，而机械臂对关节空间内的指令能更直接给出反应，所以在步骤s6中，末端速度将被转化为关节空间内的运动指令，直接控制机械臂下一时刻的运动。末端速度转化到关节速度可以直接利用雅可比矩阵：

式中的为关节速度，为雅可比矩阵的伪逆。将关节速度乘以循环时间间隔，就获得了下一时刻各个关节期望到达的关节位置。

其中δq为每个循环关节期望变化的角度值，t为循环间隔的时间。将δq在每个循环时间内更新给机械臂的各个关节驱动器，即可激活机械臂的运动。

采用本发明的基于关节力矩的机械臂拖动引导定位方法，与机械臂主动运动定位相比，让操作者手动引导机械臂运动，机械臂提供部分运动自由度的限制，这种方案在辅助手术领域安全性更高，也更符合医生操作习惯；这种拖动引导的方式可以完全依赖于机械臂本体已有的传感器，而不需要外装力矩传感器，降低了企业的生产成本；在控制系统的设计上，使用低通fir滤波器对数据进行预处理降低了机械臂在运行过程中发生严重抖动的风险；使用速度控制策略不仅提高了拖动的柔顺度，并且让机械臂能够在有限制条件的情况下拖动运行；引入误差阈值，可以让机械臂能够在无外力施加的情况下稳定悬停。该方法最后的控制结果直接施加于机械臂的关节驱动器，控制指令直接且明确，确保各类不同开发程度的机械臂都能够应用该方法，体现了该方法的广泛适用性。