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

权利要求书

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

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

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

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

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

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

S600，将辨识的参数代入到动力学方程中，得到机械臂在特定位姿下由于本体产生的关节扭矩，将计算所得与实时测量的关节扭矩相减，得到施加外力导致的额外关节扭矩，所述辨识的参数为S400步骤中的关节位置和力矩数据带入到S300步骤中动力学方程中重力项的矩阵中，计算所得出的参数，即是S500步骤中所计算出的结果；

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

其中，所述S600步骤中，所述辨识的参数通过如下方法获得：

动力学方程中的重力项中含有大量未知参数，包括每一段的质量(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未知，可以用解线性方程的方式来求解；然而由于未知量数量大，且未知量之间又存在潜在联系，若直接求解，则忽略了其内在联系，会导致线性方程的解陷入局部最优的困境；因此需要对未知量进行重新组合，将内部数学联系充分展示，降低需要辨识的参数个数；重组参数如下所示：

在重组的参数表中，只有对机械臂的动力学建模会产生影响的基础参数得到了保留；经过参数重组后，将不再需要辨识原先的大量参数，而只需要辨识如上所列的12个重组后的参数，即可完成动力学的建模；在采集采样点时，需保证采样点尽可能分布在整个工作空间内，以提高辨识精度；在对KUKA LBR Med 7 R800的运动轨迹进行采样时，可采用对各个关节的工作空间进行均匀采样，获得随机关节位置；采样时刻应保证机械臂基本处于静止状态，采集当前时刻的关节位置和关节力矩信息，然后运动到下一位置；运动过程中可能会遇到奇异点位置，由于在奇异点位置，机械臂会缺失部分自由度，因此需要从数据集中去除奇异点附近的采样点；

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

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

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

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

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