CN110716557A - 基于先验动力学知识的机器人参数识别及接触力监测方法 - Google Patents

Abstract

本发明为基于先验动力学知识的机器人参数识别及接触力监测方法，该方法通过编码器获得机器人的关节位置和速度信息、关节加速度信息，数据采集卡实时采集电机驱动器模拟量监测端的电流信息处理后得到的实际关节力矩；控制机器人在空间内做尽量多的不同轨迹的运动，同时采集其在不同时刻的角度、角速度、角加速度、实际力矩作为样本，通过梯度下降法离线学习，辨识出机器人的动力学参数，得到机器人的理想动力学模型；在实时监测过程中，通过采集和处理的电流信息，带入理想动力学模型，得到机器人在此时刻的理论力矩；将实际关节力矩和理论力矩比较，若相差超出阈值范围内则表示发生了碰撞。该方法能实现在低成本下准确获取接触力数据。

Description

基于先验动力学知识的机器人参数识别及接触力监测方法

技术领域

本发明涉及共融机器人与外界的接触力监测方法技术领域，更具体地，涉及一种基于先验动力学知识的机器人参数识别及接触力监测方法。

背景技术

机器人具有负载大、响应快、精度高等优点，通常被用来替代人类从事重复、繁重或者危险的任务，在工业制造的各领域得到了广泛应用。随着传感器、人工智能、自动控制等学科和技术的进步，机器人正逐步发展成具有感知、认知和自主行动能力的智能化装备，特别是共融机器人概念的提出极大地丰富了机器人的任务内涵。所谓共融机器人，是指机器与人共享同一时间下的工作空间和生产活动，利用人类更为直接的认知和高度智能的决策能力，发挥机器大承载、高精度执行的优势，通过人机协同作业完成非结构化环境下的非确定性任务。机器人与工作人员处于同一工作空间中并通常需要在操作上相互配合，在这种情况下，机器人的安全性显得尤为重要，若机器人不采取必要的防撞措施，则会造成设备损坏或人员伤亡，因此，必须首先解决机器人与外界环境的接触安全性问题。为了保证机器人的安全性，需要对机器人与外界环境的接触力进行检测，并及时采取必要的控制策略，避免发生严重碰撞，并控制碰撞接触力在完全可承受的范围内。

解决机器人安全性问题可以从碰撞检测开始，目前已有一些学者在这方面做出努力并取得了一定成果。检测机器人与外界环境接触力的方法有添加外部传感器检测法，如在机器人手腕添加力传感器法，中国专利CN201510006024.2在机械臂末端法兰与负载之间设置六维力传感器，按照设定的采样频率不断采集传感器的测量数据，进行机械臂的碰撞接触力检测与响应，该方法虽然能够精确检测出接触力的大小，但仅限于安装了外部传感器的部位。此外，还有在机器人外表面包裹敏感皮肤检测法，该方法能够较好地检测出碰撞接触力及其碰撞部位，中国专利CN201480076246.5用可容纳气体的软性覆盖构件包裹机器人，通过检测其内部空间的气体压强和所述软性覆盖构件的外部压强之间的差压变化检测碰撞接触力，该方法增加了对外部信息处理的难度，增加了机器人布线的复杂度，增加了机器人的体积、降低了灵活性。也有学者提出视觉传感器检测法，中国专利CN201810365980.3基于多维视觉传感器获得真空机械手的位置信息，并实时判断其处于危险状态。该方法可以比较全面的掌握外部环境信息，但图像信息量大，实时性难以保证。

机器人发生与外界环境发生接触后，接触力会瞬间增大，因此，碰撞检测算法需具备实时性、准确性以及一定的接触方位识别能力，以便更好地控制接触力，减小碰撞伤害。

发明内容

针对现有技术的不足，本发明拟解决的技术问题是，提供一种基于先验动力学知识的机器人参数识别及接触力监测方法。该方法先通过梯度下降法离线学习，辨识得到了机器人在自由空间即与包括人在内的周围环境没有碰撞和接触时的理想动力学模型，下文将某时刻在此条件下对应的关节力矩称为理论力矩。再将机器人实际关节力矩与理论力矩比较，通过上位机解算程序得到机器人的碰撞信息，该方法可以实现在低成本的情况下简单、准确的接触力数据获取，并将接触力信息作为控制依据驱动机器人进行运动调整保证共融机器人工作过程中的安全性和柔顺性。

本发明的目的是方便快捷的识别出机器人与外界环境的接触力和接触位置，从而为机器人的控制提供参考。

本发明解决所述技术问题采用的技术方案是，提供一种基于先验动力学知识的机器人参数识别及接触力监测方法，该方法通过编码器获得机器人的关节位置和速度信息并通过中值滤波去除噪声干扰，将滤波后的速度信息微分并均值滤波，从而得到机器人的关节加速度信息，数据采集卡实时采集电机驱动器模拟量监测端的电流信息并通过中值滤波和比例放大得到机器人的实际关节力矩；

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

在实时监测过程中，按照上述方法采集和处理编码器和电机驱动器的电流信息，带入自由空间的理想动力学模型，得到机器人在此时刻的理论力矩；实时监测时，通过将驱动器模拟量监测端采集的电流信号通过中值滤波和比例放大得到此时刻的实际关节力矩；

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

一种基于机器人动力学的实时碰撞检测***，包括机器人本体、数据采集模块和上位机，其中机器人本体是一个两自由度关节型机械臂，机器人本体具有两个机械臂，单个机械臂有两个关节连杆，一个关节连杆一端连接机器人肩部，该关节连杆的另一端连接另一个关节连杆，在连接位置设置电机和减速器、电机驱动器驱动；

数据采集模块包括编码器和数据采集卡，编码器用于测量机器人关节角度q＝[q₁,q₂]^T和关节角速度

数据采集卡用于采集上述编码器测量得到的关节角度q＝[q₁,q₂]^T和关节角速度以及驱动器的模拟量监测端检测电流I并传输给上位机，编码器安装在两个关节连杆连接位置及关节连杆与机器人肩部连接位置处，编码器和数据采集卡之间通过信号线实现通讯和数据传输；上位机和数据采集卡之间通过PCI总线通信，上位机用于接收数据采集卡的数据并进行相应的处理以及控制电机运动；

上位机中包括主控制器、数据处理单元、机器人动力学参数辨识单元和实时监测单元；

所述主控制器用于控制机器人实现指定轨迹运动，控制机器人使其在自由空间内做尽量多的不同轨迹的运动以为上述动力学参数辨识单元提供大量的样本数据；

数据处理单元用于在线实时处理数据采集卡传输上来的信息，包括对速度信息均值滤波以弥补采样频率低而导致的速度信息波动，再对上述均值滤波后的各关节角速度一阶微分得到各关节的角加速度

再根据电流和力矩的比例关系等比例放大电流信号得到各关节的实际力矩；

τ＝n·v·η·i

其中τ表示机器人关节的实际转矩，n表示机器人关节减速器减速比，v是额定转矩和额定电流之比，η是减速器效率，i是实际监测电流；

动力学参数辨识单元以数据处理单元同时采集并统计其在不同时刻的角度、角速度、角加速度、实际力矩作为样本，得到的大量的数据样本，利用梯度下降法辨识机器人的动力学参数，从而得到机器人的理想动力学模型；

实时监测单元用于判断碰撞是否发生，并计算出接触发生的位置和接触力的大小和方向。

与现有技术相比，本发明的有益效果是：

本发明提出了一种基于机器人本体信息，即从编码器获得的各关节的角度和角速度信息，和由驱动器模拟量监测端采集的电流信号，角速度经过均值滤波后再微分得到角加速度信息，电流信号经过中值滤波和比例放大后得到机器人各关节的实时实际力矩。控制机器人使其在自由空间内做尽量多的不同轨迹的运动，同时采集其在不同时刻的角度、角速度、角加速度、实际力矩作为样本，基于机器人先验动力学知识，采用梯度下降法，通过离线学习的方式辨识出机器人的动力学参数，从而得到了机器人在自由空间的理想动力学模型，在此条件下对应的关节力矩称为理论力矩。实时监测时，通过将驱动器模拟量监测端采集的电流信号按照上述方法换算为实际关节力矩，将从编码器获得的各关节的角度和角速度信息按上述方法得到各关节的角度、角速度、角加速度信息，再带入上述机器人在自由空间的理想动力学模型，从而得到机器人此时的理论力矩，再将机器人实际力矩与理论力矩比较，如果相差超过一定的阈值则警示发生碰撞。再根据机器人关节力矩与末端力之间的对应关系，换算出力的大小。传统的基于神经网络辨识机器人动力学仅在观测空间内准确，对于未观测空间内其准确性无法保证，而在共融机器人控制算法中，可能随时伴随着根据机器人与外界接触力的大小而进行的对机器人的位姿调整，这种基于先验知识的神经网络可以泛化到整个机器人工作空间，有效的对未观测的空间进行预测。

进一步的，设计中值滤波器去除环境噪声的干扰，设计均值滤波器处理编码器反馈回来的速度，并微分处理得到关节加速度，使接触力监测结果更为准确。

进一步的，基于机器人动力学先验知识，通过梯度下降法，辨识出机器人动力学参数，从而得到机器人在自由空间的精确动力学模型(理想动力学模型)，其准确性在98％以上，而且参数逼近以后辨识的参数结果可以直接使用，而不必在控制过程中不断修正，即其适用性不仅仅局限在已观测的空间内，可以泛化到整个机器人工作空间，有效的对未观测的空间进行预测。

本发明采用的是神经网络离线学习辨识相应的动力学参数，辨识包括机器人本身如关节连杆质量、转动惯量混合两连杆的质心到各自旋转轴的距离，通过用粘性摩擦加库伦摩擦模型进行建模获得摩擦参数，学习样本为机器人运动的真实数据，更接近于真实模型，更精确。

进一步的，通过将电机电流信号换算成对应的实际关节力矩，再和理论力矩比较相差是否超过阈值，若是突然超过阈值则表示和外界环境发生了碰撞和接触，再根据机器人力雅克比矩阵将关节力矩换算到末端，从而得到末端的接触力方位和大小。

综上所述，本发明通过梯度下降法辨识机器人动力学参数，建立了机器人在自由空间的精确动力学模型，再将编码器角度和角速度滤波和微分处理后的实时信息输入上述机器人精确动力学模型，得到此条件下与外界环境无接触时对应的理论力矩，同时将电机驱动器的电流信号按比例转换为关节实时实际力矩，再将实际力矩与理论力矩比较，如果差值超过阈值则表示发生了碰撞和接触，再根据机器人力雅克比矩阵将关节力矩换算到末端，从而得到末端的接触力方位和大小。本发明无需末端或关节安装力/力矩传感器或是视觉传感器，监测机器人是否发生碰撞，降低了成本和复杂的布线与通讯，缩短了控制周期，提高了***响应速度，保证了监测的实时性，并且能实现全臂接触监测，将力信息输入到机器人控制器，成为机器人的运动调整依据，能更好的保证共融机器人的安全，更易被行业人员与使用者所接受。

附图说明

图1为基于先验动力学知识的机器人参数识别及接触力监测方法所用***组成图；

图2为基于先验动力学知识的机器人参数识别及接触力监测方法实施流程示意图；

图3为动力学参数辨识单元流程示意图；

图4为实时碰撞力监测模块示意图；

图5(a)为样本的实际力矩与将辨识的参数值带入动力学方程后得到的理论力矩1轴的对照图；

图5(b)为样本的实际力矩与将辨识的参数值带入动力学方程后得到的理论力矩2轴的对照图；

图6(a)为非样本数据即观测空间外数据的实际力矩与将辨识的参数值带入动力学方程后得到的理论力矩1轴的对照图；

图6(b)为非样本数据即观测空间外数据的实际力矩与将辨识的参数值带入动力学方程后得到的理论力矩2轴的对照图；

图7为采用本专利所述方法得到的估计接触力与六维力传感器测量的接触力对照图。

具体实施方式

下面给出本发明的具体实施例。具体实施例仅用于详细说明本发明，不限制本申请专利的要求的保护范围。

本发明是一种基于先验动力学知识的机器人参数识别及接触力监测方法，这种方法无需力传感器和视觉传感器，根据先验动力学知识获得接触力方位和大小。

本发明通过梯度下降法辨识机器人动力学参数，建立了机器人在自由空间的精确动力学模型，再将编码器角度和角速度滤波和微分处理后的实时信息输入上述机器人精确动力学模型，得到此条件下与外界环境无接触时对应的理论力矩，同时将电机驱动器的电流信号按比例转换为关节实时实际力矩，再将实际力矩与理论力矩比较，如果差值超过阈值则表示发生了碰撞和接触，再根据机器人力雅克比矩阵将关节力矩换算到末端，从而得到末端的接触力方位和大小。将接触力信息作为控制依据调整机器人运动达到保证共融机器人安全工作的目的。

本发明一种基于先验动力学知识的机器人参数识别及接触力监测方法，所使用的基于机器人动力学的实时碰撞检测***包括机器人本体、数据采集模块和上位机。其中机器人本体是一个两自由度关节型机械臂，主要包括驱动装置及连杆，图1中为机器人本体一个机械臂的示意图，单个机械臂有两个关节连杆，连杆1和连杆2，连杆1连接机器人肩部，二者连接位置设置编码器1和电机1、减速器1，在连杆1和连杆2之间安装减速器2、电机2及编码器2驱动装置包括交流同步电机、伺服驱动器；数据采集模块包括编码器和数据采集卡，编码器用于测量机器人关节角度q＝[q₁,q₂]^T和关节角速度

数据采集卡用于采集上述编码器测量得到的关节角度q＝[q₁,q₂]^T和关节角速度

以及驱动器的模拟量监测端检测电流I并传输给上位机，上述编码器和上述数据采集卡之间通过信号线实现通讯和数据传输；上位机和数据采集卡之间通过PCI总线通信，上位机用于接收数据采集卡的数据并进行相应的处理以及控制驱动装置运动，上位机中包括主控制器、数据处理单元、机器人动力学参数辨识单元和实时监测单元。

更具体的上述主控制器用于控制机器人实现指定轨迹运动，控制机器人使其在自由空间内做尽量多的不同轨迹的运动以为上述动力学参数辨识单元提供大量的样本数据，主控制器实现机器人不同轨迹运动的控制，优选采用基于RBF神经网络补偿动力学建模误差的自适应位置控制器。

更具体的上述数据处理单元用于在线实时处理数据采集卡传输上来的信息，包括对速度信息均值滤波以弥补采样频率低而导致的速度信息波动，再对上述均值滤波后的各关节角速度一阶微分得到各关节的角加速度

通过对信号进行观察分析发现环境干扰主要为不定时的幅值很大超过电流信号2倍的噪声干扰，设计了简单有效的中值滤波器对各关节的电流信号进行滤波以去除环境中不定时的大幅值的噪声干扰。再根据电流和力矩的比例关系等比例放大电流信号得到各关节的实际力矩。

τ＝n·v·η·i

其中τ表示机器人关节的实际转矩，n表示机器人关节减速器减速比，v是额定转矩和额定电流之比，η是减速器效率，i是实际监测电流。

更具体的上述机器人动力学参数辨识单元主要是利用上述数据处理单元同时采集并统计其在不同时刻的角度、角速度、角加速度、实际力矩作为样本，得到的大量的数据，利用梯度下降法辨识机器人的动力学参数，从而得到机器人精确的动力学模型。

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

θ^*＝[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表示连杆质心到轴的长度。机器人的摩擦力很复杂，本申请中用粘性摩擦加库伦摩擦模型进行建模，即

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

由上述可知机器人的动力学是关于各关节质量m_i、转动惯量I_i，连杆长度l_i、连杆质心到轴的长度p_i、粘性系数f_v、库伦摩擦系数f_c、角度q₁,q₂、角速度

角加速度的函数，其中各连杆的长度l_i可以通过测量获得，角度信息q,

和实际力矩τ可通过上述数据采集卡采集并通过上述数据处理单元处理后得到，由此可基于机器人动力学方程，采用梯度下降法辨识其他未知参数。设所需辨识的参数矢量

为：

其中

分别为两连杆的质量的辨识值，I₁ ^*,I₂ ^*分别为两连杆绕各自旋转轴转动的转动惯量的辨识值，p₁ ^*,p₂ ^*分别为两连杆的质心到各自旋转轴的距离的辨识值。

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

采用梯度下降法获得未知参数，

的初始参数为人工估计的

该人工估计值为与真实值比较接近的数值，可以通过操作者凭经验估计或者通过计算机辅助三维模型获得，参数的学习速率为β，损失函数采用二次代价函数：

则损失函数L的相对于

的梯度为

此时参数更新法则为：

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

此单元采用的是离线学习的方式，将上述数据处理单元处理完的大量的数据作为学习的样本。从而按照上述优化方式辨识出机器人的动力学参数

进而得到了机器人在自由空间即与包括人在内的周围环境没有碰撞和接触时的理想动力学模型，下文将某时刻在此条件下对应的关节力矩称为理论力矩。

更具体的上述实时监测单元用于判断碰撞是否发生，并计算出接触发生的位置和接触力的大小和方向。上述数据处理单元的处理完的数据输入到上述实时监测单元，实时监测单元将各关节角度q，角速度

角加速度

带入上述动力学参数辨识单元得到的机器人理想动力学模型，得到机器人在该位置、速度、加速度的条件下对应的理论力矩，将上述数据处理单元传送上来的实际力矩与理论力矩比较，求出力矩矢量差值，即

其中τ表示实际力矩矢量，

表示理论力矩矢量，Δτ表示实际力矩与理论力矩差值矢量。

若某一轴相差超出阈值范围(理论力矩的10％左右)内则表示发生了接触，即

Δτ_n＞Δτ^* _n

其中，n＝1或2，Δτ_n表示第n轴实际力矩与理论力矩之差，Δτ^* _n表示第n轴的差值阈值。若Δτ_n＞Δτ^* _n，且Δτ_n+1≤Δτ^* _n+1则表示接触发生在第n关节。

由

其中τ表示实际力矩矢量，

表示理论力矩矢量，τ_e是外界碰撞在机器人关节产生的力矩矢量，且有

τ_e＝J^TF_e

其中F_e是外界和机器人的接触力，J^T是机器人力雅克比矩阵。

由上述可得

从而得到了机器人与外界的接触力矢量(包含大小和方向)，再将机器人与外界的接触力矢量(接触力的大小和方向)输入到上述主控制器，后期按照具体控制策略控制机器人做出相应的柔顺响应，停止运动或根据阻抗控制策略将力信息转换为机器人的位置调整信息。

本方法实施流程如图2所示，控制机器人按照不同轨迹运动，上述数据采集模块采集其关节角度和角速度以及电流信息，上述数据处理单元实时在线处理，并将所得数据统计和保存，约得到3万组数据作为样本，动力学参数辨识单元离线辨识得到该机器人的动力学参数。图5(a)和图5(b)分别为样本中第一关节(1轴)和第二关节(2轴)的电机电流经过换算后得到的实际力矩和将辨识得到的动力学参数以及各关节角度、角速度和角加速度带入自由空间理想动力学方程后得到的对应的关节理论力矩值的对照图，其中实线表示实际力矩的大小，虚线表示理论力矩的大小，由图5(a)和图5(b)图可看出两条曲线几乎重合，误差较小，表明辨识所得的动力学参数的准确性高。图6(a)和图6(b)分别为控制机器人进行新的轨迹的运动，所获得的上述参与动力学参数辨识的样本数据之外即观测空间外的数据，第一关节和第二关节的实际力矩与将辨识的参数值带入动力学方程后得到的理论力矩对照图，其中实线表示实际力矩的大小，虚线表示理论力矩的大小，由图6(a)和图6(b)可看出两条曲线几乎重合，误差较小，表明辨识所得的动力学参数在观测空间外仍然适用且准确，即其适用性不仅仅局限在已观测的空间内，可以泛化到整个机器人工作空间。在机器人末端安装六维力传感器，控制机器人运动，并在机器人末端施加外在接触力，按照本专利上述基于先验动力学知识的机器人接触力监测方法估计机器人与外界环境的接触力。图7为六维力传感器测量得到的机器人末端接触力和采用本专利方法估计得到的接触力的对照图，其中实线表示六维力传感器测量所得接触力的大小，虚线表示采用本方法估计得到的接触力的大小，由图7可看出两条曲线相差不大，说明了采用本方法估计机器人与外界环境接触力的准确性。

综上可说明本方法在辨识机器人参数及接触力监测方面的可行性和准确性。

本发明未述及之处适用于现有技术。

Claims (4)

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

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

在实时监测过程中，按照上述方法采集和处理编码器和电机驱动器的电流信息，带入自由空间的理想动力学模型，得到机器人在此时刻的理论力矩；实时监测时，通过将驱动器模拟量监测端采集的电流信号通过中值滤波和比例放大得到此时刻的实际关节力矩；

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

2.根据权利要求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表示连杆质心到轴的长度；

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

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

设所需辨识的参数矢量

为：

其中

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

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

采用梯度下降法获得未知参数，

的初始参数为人工估计的

参数的学习速率为β，损失函数采用二次代价函数：

则损失函数L的相对于

的梯度为

此时参数更新法则为：

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

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

3.根据权利要求1所述的监测方法，其特征在于，接触力矢量计算：若某一轴相差超出阈值范围(理论力矩的10％左右)内则表示发生了接触，即

Δτ_n＞Δτ^* _n

其中，n＝1或2，Δτ_n表示第n轴实际力矩与理论力矩之差，Δτ^* _n表示第n轴的差值阈值；

若Δτ_n＞Δτ^* _n，且Δτ_n+1≤Δτ^* _n+1则表示接触发生在第n关节；

由

其中τ表示实际力矩矢量，表示理论力矩矢量，τ_e是外界碰撞在机器人关节产生的力矩矢量，且有

τ_e＝J^TF_e

其中F_e是外界和机器人的接触力，J^T是机器人力雅克比矩阵；

由上述可得

从而得到了机器人与外界的接触力矢量，包含大小和方向。

4.一种基于机器人动力学的实时碰撞检测***，包括机器人本体、数据采集模块和上位机，其中机器人本体是一个两自由度关节型机械臂，机器人本体具有两个机械臂，单个机械臂有两个关节连杆，一个关节连杆一端连接机器人肩部，该关节连杆的另一端连接另一个关节连杆，在连接位置设置电机和减速器、电机驱动器驱动；

数据采集模块包括编码器和数据采集卡，编码器用于测量机器人关节角度q＝[q₁,q₂]^T和关节角速度数据采集卡用于采集上述编码器测量得到的关节角度q＝[q₁,q₂]^T和关节角速度

以及驱动器的模拟量监测端检测电流I并传输给上位机，编码器安装在两个关节连杆连接位置及关节连杆与机器人肩部连接位置处，编码器和数据采集卡之间通过信号线实现通讯和数据传输；上位机和数据采集卡之间通过PCI总线通信，上位机用于接收数据采集卡的数据并进行相应的处理以及控制电机运动；

上位机中包括主控制器、数据处理单元、机器人动力学参数辨识单元和实时监测单元；

所述主控制器用于控制机器人实现指定轨迹运动，控制机器人使其在自由空间内做尽量多的不同轨迹的运动以为上述动力学参数辨识单元提供大量的样本数据；

数据处理单元用于在线实时处理数据采集卡传输上来的信息，包括对速度信息均值滤波以弥补采样频率低而导致的速度信息波动，再对上述均值滤波后的各关节角速度一阶微分得到各关节的角加速度

再根据电流和力矩的比例关系等比例放大电流信号得到各关节的实际力矩；

τ＝n·v·η·i

其中τ表示机器人关节的实际转矩，n表示机器人关节减速器减速比，v是额定转矩和额定电流之比，η是减速器效率，i是实际监测电流；

动力学参数辨识单元以数据处理单元同时采集并统计其在不同时刻的角度、角速度、角加速度、实际力矩作为样本，得到的大量的数据样本，利用梯度下降法辨识机器人的动力学参数，从而得到机器人的理想动力学模型；

实时监测单元用于判断碰撞是否发生，并计算出接触发生的位置和接触力的大小和方向。

Images