[发明专利]基于先验动力学知识的机器人参数识别及接触力监测方法

权利要求书

1.一种基于先验动力学知识的机器人参数识别及接触力监测方法，该方法通过编码器获得机器人的关节角度和角速度并通过中值滤波去除噪声干扰，将滤波后的速度信息微分并均值滤波，从而得到机器人的关节角加速度，数据采集卡实时采集电机驱动器模拟量监测端的电流信息并通过中值滤波和比例放大得到机器人的实际关节力矩；

控制机器人在空间内做尽量多的不同轨迹的运动，同时采集其在不同时刻的角度、角速度、角加速度、实际力矩作为样本，基于机器人先验动力学知识通过梯度下降法离线学习，辨识出机器人的动力学参数，包括关节连杆质量、转动惯量、连杆的质心到各自旋转轴的距离，得到机器人在自由空间即与包括人在内的周围环境没有碰撞和接触时的理想动力学模型；

在实时监测过程中，将所述机器人的关节角度和角速度、机器人的关节角加速度带入自由空间的理想动力学模型，得到机器人在此时刻的理论力矩；实时监测时，将在电机驱动器模拟量监测端采集的电流信息通过中值滤波和比例放大得到此时刻的实际关节力矩；

将实际关节力矩和理论力矩比较，若相差超出阈值范围内则表示发生了碰撞，根据机器人关节力矩和末端接触力的关系，用力的雅克比矩阵换算得到机器人的接触力大小和方位，将接触力信息作为控制依据调整机器人运动达到保证人机协作安全的目的；

动力学参数的辨识过程是：

假设机器人动力学参数理想值θ^*已知，组成的矢量为：

θ^*＝[m₁^*,m₂^*,I₁^*,I₂^*,p₁^*,p₂^*]

其中m₁^*,m₂^*分别为两连杆的质量的理想值，I₁^*,I₂^*分别为两连杆绕各自旋转轴转动的转动惯量的理想值，p₁^*,p₂^*分别为两连杆的质心到各自旋转轴的距离的理想值；

则根据牛顿-欧拉法建立机器人动力学模型为

其中，τ^*是理想情况下对应的关节力矩，q＝[q₁,q₂]^T是各关节角度矢量，是各关节角速度，是各关节角加速度，D(q)是惯性矩阵，是离心力和哥氏力矢量，G(q)是重力矢量，τ_f是关节摩擦力矩，τ_e是克服与外界接触力的力矩；

对于二自由度旋转型机器人有

其中c_i＝cos(q_i)，s_i＝sin(q_i)，m_i表示各关节臂的质量，I_i表示各关节转动惯量，l_i表示各连杆长度，p_i表示连杆质心到轴的长度；g表示重力加速度；i＝1或2；c₁₂＝cos(q1+q2)；

用粘性摩擦加库伦摩擦模型进行建模得到关节摩擦力矩，即

其中f_v是粘性摩擦系数，f_c是库伦摩擦系数；

设所需辨识的参数矢量为：

其中分别为两连杆的质量的辨识值，分别为两连杆绕各自旋转轴转动的转动惯量的辨识值，分别为两连杆的质心到各自旋转轴的距离的辨识值；

采用机器人动力学的先验知识构建的神经网络为：

采用梯度下降法获得未知参数，的初始参数为人工估计的参数的学习速率为β，损失函数采用二次代价函数：

则损失函数L的相对于的梯度为

此时参数更新法则为：

参数更新和优化的最终结果为：

从而获得辨识后的各动力学参数，得到了机器人在自由空间即与包括人在内的周围环境没有碰撞和接触时的理想动力学模型。