CN114516050B - 基于位形雅可比条件数优化的机器人外力估计方法及系统 - Google Patents

Abstract

一种基于位形雅可比条件数优化的机器人外力估计方法及系统，使用基于反向传播神经网络的半参数摩擦力模型构建广义动量观测器估计机器人受到的外部作用力，通过提升机器人低关节速度下的摩擦力建模准确度，改善了观测器的外力估计精度，结合基于位形雅可比条件数的优化，得到了能有效抑制关节力矩噪声和建模误差对外力估计产生不良影响的机器人位形。满足了在不使用力/力矩传感器的条件下机器人准确感知外力的需求。

Description

基于位形雅可比条件数优化的机器人外力估计方法及系统

技术领域

本发明涉及的是一种自动化控制领域的技术，具体是一种基于位形雅可比条件数优化的机器人外力估计方法及系统。

背景技术

随着智能制造实施现场的普及，机器人的外力感知能力在碰撞检测、人机交互、机器人加工与装配中发挥着重大的作用。准确的外力感知能在以上应用中带来良好的控制性能。因此，提升机器人的外力感知能力具有非常重要的意义。

现有的机器人力感知改进技术将重点聚焦在无附加传感器下的机器人外力估计方法，这些方法大都基于机器人动力学模型。然而，上述方法的准确度均依赖于动力学模型的精度，由于机器人动力学模型高度非线性化，并且在机器人运动过程中还存在着迟滞、关节反冲和弹性变形等现象，导致准确的机器人动力学模型，特别是其中的摩擦力模型难以建立并解析表达。此外工业机器人关节力矩的噪声在外力估计的过程中也不容忽视。在建模误差和关节力矩噪声无法消除的前提下，通过机器人位形的优化来抑制这些误差与噪声对外力估计的影响还未见报导。

发明内容

本发明针对现有技术采用关节电流反馈信号的工业机器人外力估计精度较低且容易受到机器人动力学建模误差和关节力矩噪声影响的问题，提出一种基于位形雅可比条件数优化的机器人外力估计方法及系统，使用基于反向传播神经网络的半参数摩擦力模型构建广义动量观测器估计机器人受到的外部作用力，通过提升机器人低关节速度下的摩擦力建模准确度，改善了观测器的外力估计精度，结合基于位形雅可比条件数的优化，得到了能有效抑制关节力矩噪声和建模误差对外力估计产生不良影响的机器人位形。满足了在不使用力/力矩传感器的条件下机器人准确感知外力的需求。

本发明是通过以下技术方案实现的：

本发明涉及一种基于位形雅可比条件数优化的机器人外力估计方法，根据拉格朗日法建立工业机器人的动力学模型；针对无附加力/力矩传感器的工业机器人设计动力学参数激励轨迹；采集机器人在沿激励轨迹运动时的关节信息并结合机器人动力学模型，生成相应的观测矩阵和转矩矩阵并求解机器人动力学参数。通过近零速度下的特定对称位形机器人运动轨迹设计，得到关节摩擦力矩随关节速度变化的数据集，利用数据集训练反向传播神经网络，使得训练后的反向传播神经网络输出在近零速度附近的对应关节速度下的摩擦力矩，然后结合求解出的库伦-粘滞摩擦力模型构建半参数摩擦力模型，并通过数值计算方法得到机器人动力学模型中的惯性矩阵、科里奥利力、离心力矩阵和重力矩阵，结合半参数摩擦力模型构建得到广义动量观测器用于针对给定任务下估计机器人受到的外部作用力。

所述的动力学参数激励轨迹，在其中加入位置、速度和加速度等运动学约束；

所述的机器人动力学参数，依据观测矩阵和转矩矩阵，采用加入物理可行性约束后的最小二乘法求解得到。

所述的关节摩擦力矩随关节速度变化的数据集，根据机器人动力学参数结合机器人动力学模型设计运动轨迹，生成机器人各关节摩擦力矩在关节速度较低状态下随关节速度变化的数据集。

所述的运动轨迹，具体为：机器人在沿着轨迹往返运动的过程中，保持其位形具有对称性，在机器人经过某一位形时，关节角速度具备大小相等，方向相反的条件，便于实现后续的关节摩擦力矩解耦。

所述的机器人动力学模型为：其中：M(q)为惯性矩阵，/>为科里奥利力和离心力矩阵，G(q)为重力矢量，f为摩擦力矩矢量，τ为机器人关节力矩，/>为关节角加速度矢量，/>为关节角速度矢量，q为关节角度矢量，也可以被称之为机器人的位形；经线性化简化后/>其中：/>即为单个采样点对应的观测矩阵，β为待辨识的动力学参数。

所述的动力学参数激励轨迹由五次傅里叶级数构成，具体为：

其中：i表示第i关节，q_i表示第i关节的关节角度，ω为傅里叶级数的基频，a_ik，b_ik分别为第i关节的k阶正弦、余弦系数，q_i0为i关节对应的常数项。

所述的动力学参数激励轨迹满足的约束包括：其中：f_obj为优化目标函数，其物理意义在于使得各采样点观测矩阵H的时间平均条件数最小，并且满足各关节的角度、角速度和角加速度不超过极限值。

所述的观测矩阵转矩矩阵/>其中：/>为第i个采样点的观测矩阵，/>为第i个采样点的关节转矩矩阵。

所述的求解机器人动力学参数，具体为：首先将各采样点的机器人动力学模型组合成如下形式：然后引入约束矩阵D作为物理可行性约束，在满足约束矩阵D正定的条件下求解得到待辨识的动力学参数β。

所述的约束矩阵D＝diag(D₁(δ₁),D₂(δ₂),…,D_N(δ_N),f_v1,f_c1,f_v2,f_c2,…f_vN,f_cN)，其中：D_i(δ_i)定义为：其中：L为惯性矩，l为质心位置，m为连杆质量，ε为任意一个小的正常数，E为单位矩阵。

所述的库伦-粘滞摩擦力模型为：其中：/>为第i关节在关节速度为/>时的关节摩擦力矩大小，f_vi和f_ci为库伦-粘滞摩擦力模型中的粘滞摩擦系数和库仑摩擦系数。

所述的关节摩擦力矩随关节速度变化的数据集，通过以下方式得到：

①对于科里奥利力和离心力矩阵中的第u行第v列的元素/>其满足：/>

由选定的摩擦力模型知，关节摩擦力矩满足则用于生成摩擦力数据集的轨迹设定为：q₁、q₂为两个不同的机器人位形，在轨迹上任意选取一个机器人位形q₁，轨迹上都存在机器人位形q₂满足/>

②分离出关节摩擦力矩，具体为：将两条轨迹的角度、角速度与角加速度代入动力学方程，得到：两式相减，得到：

由于在低速运动的情况下，/>可以忽略不计，因此有/>即为分离出的摩擦力矩。

所述的半参数摩擦力模型为：其中：/>为临界速度，在临界速度内用反向传播神经网络建立摩擦力模型，在临界速度外使用现有的摩擦力模型。

所述的反向传播神经网络包括：输入层，隐藏层和输出层，该反向传播神经网络的输入为关节角速度，输出为摩擦力。其训练方式具体为：采用梯度下降的方式，优化各层神经元的输入权值和偏置，使得神经网络的输出尽可能地接近期望输出，直到最后收敛。

所述的广义动量观测器，通过以下方式得到：将机器人关节力矩分离成τ_ext由外力产生和τ_d由机器人本体运动产生的两部分，即τ_J＝τ_ext+τ_d，其中：根据机器人动力学模型和半参数摩擦力模型f，外力由于关节加速度信号的信噪比较小从而导致直接采用上式无法得到准确的外力估计结果，改用观测器的形式为：由动力学性质，/>为对称矩阵，因此有/>综合以上各式得到广义动量观测器的形式为：其中：τ_ext由外力产生，K_o为观测器增益，τ_J为由关节电流反馈得到的关节力矩，f为半参数摩擦力模型，/>为机器人的广义动量。

所述的估计机器人受到的外部作用力，具体是指：针对给定任务，将机器人任务路径上的平均位形雅可比条件数作为优化目标，基于插值法和机器人逆运动学方程求解机器人优化位形，降低机器人位形对应雅克比矩阵条件数，并指导机器人任务路径与末端姿态的设计与选择；

所述的给定任务包括：机器人抛光、打磨和铣削，工业机器人所具备的自由度(6个)往往大于任务所需的自由度(5个)，因此机器人可采用不同的位形/路径来完成指定任务。

所述的求解机器人优化位形，具体包括：

1)机器人末端在笛卡尔空间中受到的外力在关节空间下映射产生的力矩J(q)^TF_ext＝τ_ext；外力估计的误差满足：

2)当任务路径是一条直线，则路径可由方向向量m和中点坐标n来唯一确定：当路径长度为a，用k+1个点将其平均分成k段，则每个点的坐标为：当机器人在任务中的末端姿态不变，则旋转矩阵为常量R_i＝R，因此对应的轴角也为常量θr＝θ[r_x r_y r_z]^T。

3)则第i个插值点的齐次变换矩阵可以为根据误差不等式，优化问题变为/>

技术效果

本发明在存在机器人动力学建模误差和关节力矩噪声的前提条件下，利用机器人位形优化实现了机器人外力估计中的精度提升，抑制了上述干扰因素对机器人外力估计的影响；在辨识得到的关节摩擦力模型的基础上，采用反向传播神经网络建立了半参数摩擦力模型，显著提升了低关节速度下的摩擦力建模准确度；通过优化机器人在进行任务过程中的位形，提高了存在关节力矩噪声和动力学建模误差时的机器人外力估计的精度，适用于无附加力/力矩传感器的通用工业机器人，仅利用关节电流信息和关节运动状态信息完成机器人外力的估计从而无需安装额外的力传感器，节省了系统成本。

附图说明

图1为本发明流程图；

图2为半参数摩擦力模型与现有摩擦力模型的对比图；

图3为广义动量观测器结构图；

图4为仿真中位形优化前后机器人的轨迹；

图5为仿真中经过位形优化后的机器人估计恒定外力结果；

图6为仿真中经过位形优化后的机器人估计恒定外力结果；

图7为仿真中位形优化前后外力估计的对比结果；

图8-10为实验验证中世界坐标系下X，Y，Z方向的外力估计结果与真实值之间的对比结果。

具体实施方式

如图1所示，为本实施例涉及一种基于位形雅可比条件数优化的机器人外力估计方法，基于自主搭建的通用多关节工业机器人，其六个关节均为旋转关节，伺服电机和驱动器选用松下MINAS A6系列，采用倍福控制器对伺服电机进行控制。

本实施例外力估计方法具体包括如下步骤：

步骤一，建立工业机器人的动力学模型，动力学模型中包含的运动学参数由激光跟踪仪提前进行标定，在本实施例中，采用拉格朗日方法建立动力学模型并进行线性化，得到：其中：/>即为单个采样点对应的观测矩阵，β为待辨识的动力学参数，τ为关节力矩；

步骤二，采用五次傅里叶级数作为激励轨迹，使五次傅里叶级数满足观测矩阵的时域平均条件数约束并加入位置、速度和加速度约束，包括：其中：f_obj为优化目标函数，其物理意义在于使得各采样点观测矩阵H的时间平均条件数最小，并且满足各关节的角度、角速度和角加速度不超过极限值，在本实施例中，激励轨迹的周期设为10s；

根据上述方法得到的激励轨迹采集各个关节的电流和编码器信息，并且用巴特沃斯低通滤波器对上述关节信息进行滤波并通过对角速度进行数值微分得到各个关节的加速度。在本实施例中，采样时间设置为1ms，巴特沃斯低通滤波器的截止频率设置为2Hz，将连续运动五个周期的关节信息进行平均处理后再进行滤波和数值微分；

步骤三，通过机器人动力学模型计算各个经过平均处理后各采样点对应的观测矩阵和转矩矩阵并且组成总的观测矩阵转矩矩阵/>然后将各采样点的机器人动力学模型组合成/>其中：/>为第i个采样点的观测矩阵，/>为第i个采样点的关节转矩矩阵；

在本实施例中，H_M为60000行72列的矩阵，τ_M为60000行的列向量。

步骤四，引入约束矩阵D作为物理可行性约束，在满足D正定的条件下求解得到待辨识的动力学参数β，该约束矩阵D＝diag(D₁(δ₁),D₂(δ₂),…,D_N(δ_N),f_v1,f_c1,f_v2,f_c2,…f_vN,f_cN)，

其中：L为惯性矩，l为质心位置，m为连杆质量，ε为任意一个小的正常数，E为单位矩阵，f_vi和f_ci为如下摩擦力模型中的粘滞摩擦系数和库仑摩擦系数，摩擦力模型为：/>

步骤五，当计用于获取摩擦力数据集的轨迹，与步骤二类似地得到关节信息。在本实施例中，当q₁、q₂为轨迹上两个不同的机器人位形，在轨迹上任意选取一个机器人位形q₁，都存在机器人位形q₂满足如下关系，且要满足低速运动的条件，即将本步骤中机器人处于两种位形时对应的关节力矩记作τ₁、τ₂，将两者相减即为分离到的摩擦力矩：

步骤六，针对每个关节训练反向传播神经网络，用于构建半参数摩擦力模型，该模型具体为：其中：BPNN为反向传播神经网络，该神经网络包括输入层、隐藏层和输出层，输入层大小为1，接受的输入为关节角速度，隐藏层数量为1，神经元个数为10，输出层为关节摩擦力矩。

如图2所示，为一个关节对应的半参数摩擦力模型与现有摩擦力模型的对比。

步骤七，建立广义动量观测器估计机器人受到的外部作用力，并将步骤八中得到的半参数摩擦力模型应用于观测器中，如图3所示，广义动量观测器具体为：

步骤八，针对实际具体任务建立位形优化问题，求解机器人完成任务时的运动轨迹和对应位形，该优化问题为：

本实施例面向真实的机器人恒力打磨场景，针对该任务进行数值仿真，在仿真环境下验证有效性。

在仿真环境中设置机器人末端被施加了一个大小和方向均恒定的外力，同时机器人沿着任务路径运动，并采用构建的广义动量观测器来估计外力。

如图4所示，为位形优化前后机器人的轨迹，如图5所示，为经过位形优化前的机器人估计恒定外力的仿真结果，如图6所示，为经过位形优化后的估计恒定外力的仿真结果，如图7所示，为位形优化前后外力估计的最大误差和均方根误差。

步骤十，搭建实验平台并进行人机交互实验验证所提出的方法的有效性。如图8-图10所示，为使用了所提出方法后在世界坐标系下X，Y，Z方向的外力估计结果与真实测量值之间的对比，位形优化前和优化后各方向外力估计值和真实值之间的均方根误差结果如下所示：

与现有技术相比，本发明在存在机器人动力学建模误差和关节力矩噪声的前提条件下，提升了机器人进行外力估计的精度，在X，Y，Z三个方向上分别提升了48.28％，46.53％和6.721％。

上述具体实施可由本领域技术人员在不背离本发明原理和宗旨的前提下以不同的方式对其进行局部调整，本发明的保护范围以权利要求书为准且不由上述具体实施所限，在其范围内的各个实现方案均受本发明之约束。

Claims (10)

1.一种基于位形雅可比条件数优化的机器人外力估计方法，其特征在于，根据拉格朗日法建立工业机器人的动力学模型；针对无附加力/力矩传感器的工业机器人设计动力学参数激励轨迹；采集机器人在沿激励轨迹运动时的关节信息并结合机器人动力学模型，生成相应的观测矩阵和转矩矩阵并求解机器人动力学参数；通过近零速度下的特定对称位形机器人运动轨迹设计，得到关节摩擦力矩随关节速度变化的数据集，利用数据集训练反向传播神经网络，使得训练后的反向传播神经网络输出在近零速度附近的对应关节速度下的摩擦力矩，然后结合求解出的库伦-粘滞摩擦力模型构建半参数摩擦力模型，并通过数值计算方法得到机器人动力学模型中的惯性矩阵、科里奥利力、离心力矩阵和重力矩阵，结合半参数摩擦力模型构建得到广义动量观测器用于针对给定任务下估计机器人受到的外部作用力；

所述的机器人动力学模型为：其中：M(q)为惯性矩阵，为科里奥利力和离心力矩阵，G(q)为重力矢量，f为摩擦力矩矢量，τ为机器人关节力矩，/>为关节角加速度矢量，/>为关节角速度矢量，q为关节角度矢量，也可以被称之为机器人的位形；经线性化简化后/>其中：/>即为单个采样点对应的观测矩阵，β为待辨识的动力学参数；

所述的库伦-粘滞摩擦力模型为：其中：/>为第i关节在关节速度为/>时的关节摩擦力矩大小，f_vi和f_ci为库伦-粘滞摩擦力模型中的粘滞摩擦系数和库仑摩擦系数；

所述的半参数摩擦力模型为：其中：/>为临界速度，在临界速度内用反向传播神经网络建立摩擦力模型，在临界速度外使用现有的摩擦力模型。

2.根据权利要求1所述的基于位形雅可比条件数优化的机器人外力估计方法，其特征是，所述的机器人动力学参数，依据观测矩阵和转矩矩阵，采用加入物理可行性约束后的最小二乘法求解得到；

所述的关节摩擦力矩随关节速度变化的数据集，根据机器人动力学参数结合机器人动力学模型设计运动轨迹，生成机器人各关节摩擦力矩在关节速度较低状态下随关节速度变化的数据集。

3.根据权利要求1所述的基于位形雅可比条件数优化的机器人外力估计方法，其特征是，所述的运动轨迹，具体为：机器人在沿着轨迹往返运动的过程中，保持其位形具有对称性，在机器人经过某一位形时，关节角速度具备大小相等，方向相反的条件，便于实现后续的关节摩擦力矩解耦。

4.根据权利要求1所述的基于位形雅可比条件数优化的机器人外力估计方法，其特征是，所述的动力学参数激励轨迹由五次傅里叶级数构成，具体为：

其中：i表示第i关节，q_i表示第i关节的关节角度，ω为傅里叶级数的基频，a_ik，b_ik分别为第i关节的k阶正弦、余弦系数，q_i0为i关节对应的常数项；

所述的动力学参数激励轨迹满足的约束包括：其中：f_obj为优化目标函数，其物理意义在于使得各采样点观测矩阵H的时间平均条件数最小，并且满足各关节的角度、角速度和角加速度不超过极限值。

5.根据权利要求1或2所述的基于位形雅可比条件数优化的机器人外力估计方法，其特征是，所述的观测矩阵转矩矩阵/>其中：/>为第i个采样点的观测矩阵，/>为第i个采样点的关节转矩矩阵。

6.根据权利要求1所述的基于位形雅可比条件数优化的机器人外力估计方法，其特征是，所述的求解机器人动力学参数，具体为：首先将各采样点的机器人动力学模型组合成如下形式：然后引入约束矩阵D作为物理可行性约束，在满足约束矩阵D正定的条件下求解得到待辨识的动力学参数β；

所述的约束矩阵D＝diag(D₁(δ₁),D₂(δ₂),…,D_N(δ_N),f_v1,f_c1,f_v2,f_c2,…f_vN,f_cN)，其中：D_i(δ_i)定义为：其中：L为惯性矩，l为质心位置，m为连杆质量，ε为任意一个小的正常数，E为单位矩阵。

7.根据权利要求1所述的基于位形雅可比条件数优化的机器人外力估计方法，其特征是，所述的关节摩擦力矩随关节速度变化的数据集，通过以下方式得到：

①对于科里奥利力和离心力矩阵中的第u行第v列的元素/>其满足：由选定的摩擦力模型知，关节摩擦力矩满足/>则用于生成摩擦力数据集的轨迹设定为：q₁、q₂为两个不同的机器人位形，在轨迹上任意选取一个机器人位形q₁，轨迹上都存在机器人位形q₂满足/>

②分离出关节摩擦力矩，具体为：将两条轨迹的角度、角速度与角加速度代入动力学方程，得到：两式相减，得到：

由于在低速运动的情况下，/>可以忽略不计，因此有即为分离出的摩擦力矩。

8.根据权利要求1所述的基于位形雅可比条件数优化的机器人外力估计方法，其特征是，所述的广义动量观测器，通过以下方式得到：将机器人关节力矩分离成τ_ext由外力产生和τ_d由机器人本体运动产生的两部分，即τ_J＝τ_ext+τ_d，其中：根据机器人动力学模型和半参数摩擦力模型f，外力由于关节加速度信号的信噪比较小从而导致直接采用上式无法得到准确的外力估计结果，改用观测器的形式为：由动力学性质，/>为对称矩阵，因此有/>综合以上各式得到广义动量观测器的形式为：其中：τ_ext由外力产生，K_o为观测器增益，τ_J为由关节电流反馈得到的关节力矩，f为半参数摩擦力模型，/>为机器人的广义动量。

9.根据权利要求1所述的基于位形雅可比条件数优化的机器人外力估计方法，其特征是，所述的估计机器人受到的外部作用力，具体是指：针对给定任务，将机器人任务路径上的平均位形雅可比条件数作为优化目标，基于插值法和机器人逆运动学方程求解机器人优化位形，降低机器人位形对应雅克比矩阵条件数，并指导机器人任务路径与末端姿态的设计与选择。

10.根据权利要求9所述的基于位形雅可比条件数优化的机器人外力估计方法，其特征是，所述的求解机器人优化位形，具体包括：

1)机器人末端在笛卡尔空间中受到的外力在关节空间下映射产生的力矩J(q)^TF_ext＝τ_ext；外力估计的误差满足：

2)当任务路径是一条直线，则路径可由方向向量m和中点坐标n来唯一确定：当路径长度为a，用k+1个点将其平均分成k段，则每个点的坐标为：i＝0,1,2,...,k；当机器人在任务中的末端姿态不变，则旋转矩阵为常量R_i＝R，因此对应的轴角也为常量θr＝θ[r_x r_y r_z]^T；

3)则第i个插值点的齐次变换矩阵可以为根据误差不等式，优化问题变为/>