CN111618859A - 一种对于静止或低速工况下机械臂高精度力反馈的方法 - Google Patents

Abstract

本发明公开了一种对于静止或低速工况下机械臂高精度力反馈的方法，包括以下步骤：S100，构建机械臂的各个关节坐标系，并计算关节的雅可比矩阵；S200，根据上述的关节坐标系，构建原点位于机械臂的每一段质心位置的坐标系，并计算质心位置的雅可比矩阵；S300，构建动力学方程中重力项的矩阵；S400，在机械臂运动过程中采集得到关节位置和力矩数据；S500，将S400的数据代入到S300中，计算出结果；S600，得到施加外力导致的额外关节扭矩；S700，结合S100中得到的关节雅可比矩阵和S600中得到的额外关节扭矩，计算得到机械臂末端外力的大小和方向。本发明提供的一种对于静止或低速工况下机械臂高精度力反馈的方法，简化计算步骤，提高力反馈精确性。

Description

一种对于静止或低速工况下机械臂高精度力反馈的方法

技术领域

本发明涉及机器人技术领域，具体地说，涉及一种对于静止或低速工况下机械臂高精度力反馈的方法。

背景技术

近年来，工业机械臂在医疗领域的应用正在快速发展中。在机械臂的帮助下，原本难度高、精度差、用时长的手术，得到了不同程度的改善。协作型机械臂作为工业机械臂的一个分支，凭借其人机协作性好、不用设置安全防护等优势，获得了医疗行业的青睐。不同于在工业上应用的机械臂，医疗机械臂更加注重安全性而非效率，因此医疗机械臂的运动基本是维持在静止或者缓慢移动的状态。然而为保证操作的灵活性和安全性，医疗机械臂对外力的感知要求会比工业场景要求更高。如何提升机械臂力反馈的精度，也是很多外科医生关心的问题。

目前主流的方法是先对机械臂本体进行数学建模，通常需要辨识机械臂每段的质量、惯性量、质心位置等参数，然后再将辨识的参数应用于力反馈的计算。文献“Modelingand Dynamic Parameter Identification of the Schunk Powerball Robotic Arm”(A.H.Memar,E.T.Esfahani)和“Dynamic Identification of the Franka Emika PandaRobot with Retrieval of Feasible Parameters Using Penalty-based Optimization”(C.Gaz,M. Cognetti,etc.)分别对3轴和7轴的机械臂进行了参数辨识，辨识的参数包括质量、质心、惯性量和摩擦力。传统的方法在计算上需要占用大量资源，运算成本很高。

发明内容

本发明的目的在于提供一种对于静止或低速工况下机械臂高精度力反馈的方法，简化计算步骤，提高力反馈精确性。

本发明公开的一种对于静止或低速工况下机械臂高精度力反馈的方法所采用的技术方案是:

一种对于静止或低速工况下机械臂高精度力反馈的方法，包括以下步骤：

S100，构建机械臂的各个关节坐标系，并计算关节的雅可比矩阵；

S200，根据上述的关节坐标系，构建原点位于机械臂的每一段质心位置的坐标系，并计算质心位置的雅可比矩阵；

S300，构建动力学方程中重力项的矩阵；

S400，设置激励运动轨迹，在机械臂运动过程中采集得到关节位置和力矩数据；

S500，将S400的数据代入到S300中，计算出结果；

S600，将辨识的参数代入到动力学方程中，得到机械臂在特定位姿下由于本体产生的关节扭矩，将计算所得与实时测量的关节扭矩相减，得到施加外力导致的额外关节扭矩；

S700，结合S100中得到的关节雅可比矩阵和S600中得到的额外关节扭矩，计算得到机械臂末端外力的大小和方向。

作为优选方案，所述S100步骤中，根据平移和欧拉角结合的方式构建机械臂各个关节坐标系。

作为优选方案，所述S300步骤中，进行参数分离，将与关节位置有关的参数放入一个矩阵，将与质量和质心有关的参数组合后放入另一个矩阵。

作为优选方案，所述S400步骤中，采样时保证机械臂处于静止状态，且采样点布满整个工作空间。

本发明公开的对于静止或低速工况下机械臂高精度力反馈的方法的有益效果是：对医疗机械臂工况的特殊性进行了分析，将方法限定于静止或低速条件下的力反馈，在动力学方程中的关节速度和加速度在该种条件下接近于零，仅保留重力项以适应静止或低速的工况，得到动力学方程中重力项的矩阵，大大减少了需要辨识的参数数量，降低了对计算能力的要求，并且使在此种工况下的力反馈更加精确。

附图说明

图1是本发明对于静止或低速工况下机械臂高精度力反馈的方法的流程图。

图2是本发明对于静止或低速工况下机械臂高精度力反馈的方法的关节坐标系示意图。

图3是本发明对于静止或低速工况下机械臂高精度力反馈的方法的质心坐标系示意图。

具体实施方式

下面结合具体实施例和说明书附图对本发明做进一步阐述和说明:

请参考图1，为更具体解释步骤中包含的技术细节，下面将结合附图以及在KUKALBR Med 7 R800七轴协作型机械臂上的实际操作对本发明进行进一步解释。

完整的动力学方程应当包含惯性项B(q)，离心力与科氏力项

重力项 G(q)，粘滞摩擦力项F_v和库伦摩擦力项F_c。方程中的q,

分别表示关节的位置、速度和加速度。当工况处于静止或低速运动状态下，关节速度

和加速度

接近于0，仅保留重力项以适应静止或低速的工况，因此动力学方程可以做如下简化：

方程右边的τ表示的是各个关节上的力矩。经过这样的简化，方程中只剩下重力项和关节力矩的关系，可以极大减少单个采样点的运算量，允许更多采样点加入运算，提高整个系统在特定工况下的辨识精度。

以KUKA LBR Med 7 R800为例，本发明构建的关节坐标系遵循坐标轴z轴平行于关节轴线且符合右手定则。请参考图1，使用平移和欧拉角结合的方式构建坐标系，所需要的参数与传统的D-H法构建所需参数有所不同，所需参数如表1所列：

表1：关节坐标系构建参数表

关节1-2

关节2-3

关节3-4

关节4-5

关节5-6

关节6-7

α<sub>x</sub>

π/2

π/2

π/2

π/2

π/2

π/2

α<sub>y</sub>

π

π

0

π

0

π

由于z轴与关节轴同向，因此z轴的变换角度即是关节转动过的角度，因此未被列入参数表内。该参数表只表示的是x和y坐标轴之间的转换关系。

以关节1和关节2的转换关系为例，坐标系2在坐标系1的基础上先进行平移变换，移动的距离为d₁₂。然后将关节1转过的角度代入z轴应转动的角度，再依次旋转x和y轴，转动角度对应表1中对应列所示角度。其余各关节的坐标系构建均基于前一关节坐标系进行构建。

使用公式可表示为：

由此公式，可以计算得到各关节坐标系相对上一坐标系的转换矩阵：

在实际运用中，基坐标系和末端执行器坐标系往往发挥重要作用，这两个坐标系关于关节坐标系的转换关系如下：

将上文所述所有转换关系进行整合，即可得到任意关节或末端执行器坐标系，相对于基坐标系的转换关系：

雅可比矩阵在机械臂正运动学中承担了将关节空间的运动速度转换到笛卡尔坐标系中的运动速度，即：

其中v_e、

ω_e分别表示末端执行器的笛卡尔坐标系速度、速度的平移分量和速度的旋转分量。右半部分的雅可比矩阵J与当前关节角有关，θ_i和

分别表示第i关节转过的角度和角速度。

雅可比矩阵的求解可以通过如下方法求偏导得到：

雅可比矩阵的前三行与笛卡尔坐标系下的平移量相对应，后三行与旋转量对应，总列数应当等于机械臂的关节个数。

在构建机械臂的数学模型时，由于机械臂的每一段都是刚体，因此可以将每一段刚体的质量集中在各段的质心处，再用合适的变换关系将各质心处的坐标系串联起来，构成简化的机械臂质量分布模型，从而用来分析机械臂的动力学特征。在以KUKA LBR Med 7R800为基础的说明中，规定了第i段的机械臂质心坐标系旋转矩阵与i+1段保持一致，平移量是未知的，先使用符号表示。采用与上述相同的方法，可以建立如附图2所示的机械臂质心坐标系，以及计算出对应的雅可比矩阵。

动力学方程中的重力项中含有大量未知参数，包括每一段的质量(m)、质心位置(cm)和长度(d)。

质量m：m₁，m₂，m₃，……，m_n

质心cm：cm₁，cm₂，cm₃，……，cm_n

长度d：d₁₂，d₂₃，d₃₄，……，d_(n-1)n

为了将未知参数辨识出来，可将重力项G拆分成两项，如下所示：

拆分后矩阵Y只与已知的关节角θ相关，因此对于特定位姿，方程中仅有矩阵U未知，可以用解线性方程的方式来求解。然而由于未知量数量大，且未知量之间又存在潜在联系，若直接求解，则忽略了其内在联系，会导致线性方程的解陷入局部最优的困境。因此需要对未知量进行重新组合，将内部数学联系充分展示，降低需要辨识的参数个数。重组参数如下所示：

表2：辨识参数重组表

在重组的参数表中，只有对机械臂的动力学建模会产生影响的基础参数得到了保留。经过参数重组后，将不再需要辨识原先的大量参数，而只需要辨识如上所列的12个重组后的参数，即可完成动力学的建模。

在采集采样点时，需保证采样点尽可能分布在整个工作空间内，以提高辨识精度。在对KUKA LBR Med 7 R800的运动轨迹进行采样时，可采用对各个关节的工作空间进行均匀采样，获得随机关节位置。采样时刻应保证机械臂基本处于静止状态，采集当前时刻的关节位置和关节力矩信息，然后运动到下一位置。运动过程中可能会遇到奇异点位置，由于在奇异点位置，机械臂会缺失部分自由度，因此需要从数据集中去除奇异点附近的采样点。

将采集到的数据以如下方式堆叠到线性方程组里：

上式中只有包含待辨识参数的矩阵u是未知的，因此可使用最小二乘法解算上式，将矩阵u解算出来，即辨识出了所有与动力学模型相关的参数。

在步骤(1)中，参数已经得到了辨识，步骤(2)将基于辨识出来的参数，完成末端执行器上外力的测量工作。

对于测量步骤，参数矩阵u为已知，矩阵Y与测量时的关节角有关，得到的矩阵τ_int表示的是由机械臂本体重量提供给各关节的力矩，计算公式为：

Y(θ₁,θ₂,…θ_n)·u＝τ_int

将测量的关节力矩值与本体提供的力矩相减，即可得到外力导致的关节力矩值：

τ_ext＝τ_measure-τ_int

外力在关节空间和笛卡尔空间之间有如下关系：

τ_ext＝J·F_ext

此处的F_ext包含了笛卡尔坐标系下的力和力矩值，因此是六维的向量。通过对雅可比矩阵求伪逆，即可得到末端执行器上基于笛卡尔坐标系表示的外力F_ext。采用关节力矩传感器的读数作为辨识程序的输入，较现有的多采用测电机电流估算关节力矩的方案，能有效提升辨识精度。

将该方法与传统的机械臂参数辨识方法进行对比，传统的方法适用范围更广，即使在高速和加速度变化较大的情况下也能较好的估计力的大小，但是缺点在于计算量较大，运算成本较高。该方法在传统方法基础上做了改进，对医疗机械臂工况的特殊性进行了分析，将方法限定于静止或低速条件下的力反馈，大大减少了需要辨识的参数数量，降低了对计算能力的要求，并且使在此种工况下的力反馈更加精确。可用于多种机械臂辅助的外科手术，包括骨科手术、肿瘤切除手术、腹腔镜手术、胸腔镜手术等，还可应用于医疗检查，包括B超、放射检查。

最后应当说明的是，以上实施例仅用以说明本发明的技术方案，而非对本发明保护范围的限制，尽管参照较佳实施例对本发明作了详细地说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或者等同替换，而不脱离本发明技术方案的实质和范围。

Claims (4)

1.一种对于静止或低速工况下机械臂高精度力反馈的方法，其特征在于，包括以下步骤：

S100，构建机械臂的各个关节坐标系，并计算关节的雅可比矩阵；

S200，根据上述的关节坐标系，构建原点位于机械臂的每一段质心位置的坐标系，并计算质心位置的雅可比矩阵；

S300，构建动力学方程中重力项的矩阵；

S400，设置激励运动轨迹，在机械臂运动过程中采集得到关节位置和力矩数据；

S500，将S400的数据代入到S300中，计算出结果；

S600，将辨识的参数代入到动力学方程中，得到机械臂在特定位姿下由于本体产生的关节扭矩，将计算所得与实时测量的关节扭矩相减，得到施加外力导致的额外关节扭矩；

S700，结合S100中得到的关节雅可比矩阵和S600中得到的额外关节扭矩，计算得到机械臂末端外力的大小和方向。

2.如权利要求1所述的对于静止或低速工况下机械臂高精度力反馈的方法，其特征在于，所述S100步骤中，根据平移和欧拉角结合的方式构建机械臂各个关节坐标系。

3.如权利要求1所述的对于静止或低速工况下机械臂高精度力反馈的方法，其特征在于，所述S300步骤中，进行参数分离，将与关节位置有关的参数放入一个矩阵，将与质量和质心有关的参数组合后放入另一个矩阵。

4.如权利要求1所述的对于静止或低速工况下机械臂高精度力反馈的方法，其特征在于，所述S400步骤中，采样时保证机械臂处于静止状态，且采样点布满整个工作空间。

Images