CN110682290B - 一种基于动量观测器的闭环机械臂***碰撞检测方法 - Google Patents

Abstract

本发明涉及一种闭环机械臂***的碰撞检测方法，特别涉及一种基于动量观测器的闭环机械臂***碰撞检测方法，属于机器人技术领域。本发明通过在闭环机械臂***关节处布置运动传感器测量关节运动信息、布置六维力/力矩传感器测量每个闭环中任意至少一个关节处约束力和约束力矩，并基于动量观测器实现对闭环机械臂***的碰撞识别和碰撞信息的实时检测。本发明的方法所需传感器均配置于关节处，装配简单、可靠性和灵活性高；本发明的方法无需关节角加速度信息或对高维时变质量特性矩阵求逆，碰撞信息观测结果精度高、时间延迟低；本发明的方法可通过改变观测器增益和碰撞判断阈值，调节观测器性能和检测灵敏度。

Description

一种基于动量观测器的闭环机械臂***碰撞检测方法

技术领域

本发明涉及一种闭环机械臂***的碰撞检测方法，特别涉及一种基于动量观测器的闭环机械臂***碰撞检测方法，属于机器人技术领域。

背景技术

随着机器人技术的发展，机械臂的力控制在工业机器人、协作机器人、航天器在轨服务与操作等领域得到越来越多的发展和应用。要实现机械臂的力控制，实时获取***中的接触碰撞信息是必不可少的一环。对于这一问题，现有解决方案仅涉及树形机械臂***，其结构中无闭环约束，因此仅通过安装在关节处的传感器测量信息，即可识别外部碰撞的发生并解算碰撞引入各关节的广义碰撞力大小。此类仅在关节处配置传感器的方案具有装配简单、可靠性高的特点，然而对于闭环机械臂***，目前尚无能够通过仅在关节处布置传感器即实现外部碰撞信息检测的方法。针对这一现状，本发明提出一种基于动量观测器且仅需在关节处布置传感器的闭环机械臂***碰撞检测方法。

发明内容

为了克服现有闭环机械臂***碰撞检测方式的不足，提供一种基于动量观测器的闭环机械臂***碰撞检测方法；该方法基于所设计的动量观测器，仅在闭环机械臂***关节处配置运动传感器和有限个六维力/力矩传感器，即可实现对闭环机械臂***接触碰撞信息的实时获取，且配置六维力/力矩传感器的最小数量与闭环约束数量一致。

本发明的目的通过下述技术方案实现。

一种基于动量观测器的闭环机械臂***碰撞检测方法，在闭环机械臂***关节处配置运动传感器用于测量各关节转角、角速度大小；在闭环约束任意至少一个关节处配置六维力/力矩传感器，用于测量闭环约束力、约束力矩大小。根据第一类拉格朗日方程理论得到闭环机械臂***的动力学模型。以该动力学模型为基础，设计动量观测器用于观测各关节广义碰撞力。将六维力/力矩传感器测量的闭环约束力及约束力矩、运动传感器测量的关节转角及转角速度和利用驱动电机电流估算的输入***的主动力信息代入所设计的动量观测器，并根据闭环机械臂***内、外部干扰因素和控制精度需求设计观测阈值，则可通过该观测器输出进行碰撞检测，并获取实际碰撞引入各关节的广义碰撞力。

所述六维力/力矩传感器配置方法中，所选择的配置关节可根据具体结构确定，但需保证能够在每一闭环约束中至少配置一个。

一种基于动量观测器的闭环机械臂***碰撞检测方法，具体步骤为：

步骤一：在闭环机械臂***中配置运动传感器，所述运动传感器用于采集关节的转角

及转角速度

在闭环机械臂***中配置六维力/力矩传感器，所述六维力/力矩传感器用于采集每一闭环约束中任意至少一个关节处作用的约束力和约束力矩大小，记为

步骤二：建立闭环机械臂***的动力学模型。

根据第一类拉格朗日方程理论得到闭环机械臂***动力学模型：

其中，M(q)为***的质量特性矩阵；q、

为各关节处的转角、角速度、角加速度，即***的广义坐标、广义速率以及广义加速度；A(q)、

分别为***中六维力/力矩传感器配置关节的约束雅各比矩阵及其导数；μ为拉格朗日乘子，实际物理意义为六维力/力矩传感器配置关节处对应的约束力和约束力矩；F^C、F^gc、

g(q)分别为驱动机构输入对各关节的广义主动力、碰撞引入各关节的广义碰撞力、各体运动耦合产生的广义惯性力非线性项以及重力对各关节的广义力。

步骤三：设计动量观测器用于观测各关节广义碰撞力。

定义***广义动量为

同时由多体动力学理论得：

则根据式(1)所述闭环机械臂***动力学模型，广义动量的导数可表述为：

其中中间变量

构造观测器如下：

其中，

为广义动量及其导数的观测量；类似地，各带有^符号的量均为对应物理量的观测量或测量量；γ为观测器输出；K_MO为观测器增益，用于调节观测器跟踪性能和灵敏度。

根据式(3)以及式(2)，有

当测量量足够准确，有

且

同时若模型足够准确，

则有

此时式 (4)表示为

则能够看出理论上观测器的输出γ以指数接近广义碰撞力F^gc；通过调节观测器增益K_MO可调节观测器跟踪性能和灵敏度。

步骤四：对闭环机械臂***进行碰撞检测。

将步骤一实时测量的关节约束力

运动信息

和

以及由关节驱动电机的电流值估算的

带入步骤三所设计的动量观测器(3)，则可通过观测器输出γ对闭环机械臂***进行碰撞检测。由于实际***内部、外部存在摩擦以及环境等不确定因素干扰，需根据各类干扰的具体情况以及控制精度需求设定判断碰撞是否发生的阈值γ_C。当γ≤γ_C时，认为观测到的信息是由不确定干扰因素引起的；当γ＞γ_C时，认为碰撞发生，且观测到的关节广义碰撞力为

本发明公开的一种闭环机械臂***的碰撞检测方法，通过在闭环机械臂***关节处配置运动传感器和六维力/力矩传感器测定闭环约束力和约束力矩、利用动量观测器实现对***碰撞的检测和碰撞广义力的观测；解决了闭环机械臂***碰撞检测问题，传感器配置要求低、***可靠性高。

有益效果：

1、该方法所需配置的传感器位于机械臂关节处，装配简单、可靠性高；

2、该方法的传感器配置方案在满足每个闭环约束中配置一个六维力/力矩传感器的原则下，六维力/力矩传感器具体安装关节有多种选择，灵活性高；

3、该方法所公开的基于动量观测器的碰撞检测方法，在工作过程中无需机械臂关节角加速度信息，避免了对运动传感器测量结果进行数值微分所引入的高频噪声、碰撞信息观测结果精度高；

4、该方法所提出的动量观测器，在碰撞检测过程中无需对高维时变的质量特性矩阵求逆，计算量小；在碰撞观测和控制过程中引入的时间延迟低；

5、根据该方法中动量观测器输出，能够判断机器人是否发生碰撞；通过调节碰撞判断阈值和观测器增益，可以调节碰撞检测的灵敏度和对碰撞引入各关节的广义力的跟踪性能；

附图说明

图1为本发明公开动量观测器结构及碰撞检测方法示意图；

图2为实施例中的闭环机械臂***结构及六维力/力矩传感器配置关节示意图；

图3为实施例中关节11、21、31处待观测的碰撞力信息；

图4为实施例中使用本发明给出方法观测到关节11、21、31处的广义碰撞力信息。

具体实施方式

为了更好地说明本发明的目的和优点，下面结合附图对本发明的具体实施方式作进一步详细的说明。

图1为本发明公开的动量观测器结构及碰撞检测方法示意图。本实施例中所述闭环机械臂***结构见图2，其上平台1在空间中固定，下平台为一Bricard机构，由6个臂杆组成，编号为12A，12B，22A，22B，32A，32B，相邻关节间夹角为30°；上下平台间通过3个 RRS分支进行连接，包含6个臂杆11，12，21，22，31，32。该闭环机械臂***中所有臂杆为匀质直杆，长度为0.5m，质量为0.6kg。

本发明公开的一种基于动量观测器的闭环机械臂***碰撞检测方法包括如下步骤：

步骤一：在图2所示的研究对象各关节处配置运动传感器，所述运动传感器用于采集关节的转角

及转角速度

考虑到***对称性，选择图2所示的三个关节C1、C2、C3(12A与22B连接关节、22A与32B连接关节、32A与12B连接关节)配置六维力/力矩传感器，用于采集每一闭环中至少一个关节处作用的约束力和约束力矩大小，记为

步骤二：建立闭环机械臂***的动力学模型。

对图2所示本实施例研究对象，定义广义速率如下

三个RRS并联分支运动状态可由6个转角速度进行描述：以各臂杆与自身内接体间柱铰关节的转角速率作为对应的广义速率

i＝11,12,21,22,31,32；

Bricard机构臂杆12A、22A、32A相对自身内接体12、22、32的运动状态各以3个转角速度进行描述：以各杆件与内接体间球铰的角速度作为对应的广义速率ω_12A、ω_22A、ω_32A；

Bricard机构杆12B、22B、32B相对自身内接体12A、22A、32A的运动状态各以1个转角描述：选取各杆件与内接体间柱铰的转角速率作为对应的广义速率

利用以上所取各广义速率，描述闭环机械臂***运动的所有广义速率可记为

写出如下闭环约束方程

，其中A(q)为约束雅各比矩阵。通过一次求导后得加速度级约束方程

从而闭环机械臂***动力学模型可写为

其中，M(q)为***的质量特性矩阵；q、

为各关节处的转角、角速度、角加速度，即***的广义坐标、广义速率以及广义加速度；A(q)、

分别为***中六维力/力矩传感器配置关节的约束雅各比矩阵及其导数；μ为拉格朗日乘子、实际物理意义为六维力/力矩传感器配置关节处对应的约束力和约束力矩；F^C、F^gc、

g(q)分别为驱动机构输入对各关节的广义主动力、碰撞引入各关节的广义碰撞力、各体运动耦合产生的广义惯性力非线性项以及重力对各关节的广义力。

步骤三：设计动量观测器用于观测各关节广义碰撞力。

定义***广义动量为

由于

则

其中中间变量

构造观测器如下

其中，

为广义动量及其导数的观测量；类似地，各带有^符号的量均为对应物理量的观测量或测量量；γ为观测器输出；K_MO为观测器增益，用于调节观测器跟踪性能和灵敏度。则当测量量和模型足够准确时，观测器的输出以指数接近广义碰撞力F^gc，且通过调节观测器增益K_MO可调节观测器跟踪性能和灵敏度，观测器结构见图1。

步骤四：对闭环机械臂***进行碰撞检测。

将步骤四实时测量信息

和

带入步骤三所设计的动量观测器，得到观测器实时输出γ。根据各类干扰的具体情况以及控制精度需求设定判断碰撞是否发生的阈值γ_C。将γ与γ_C比较，实现对闭环机械臂***的碰撞检测：当γ≤γ_C时，认为观测到的信息是由不确定干扰因素引起的；当γ＞γ_C时，认为碰撞发生，且观测到碰撞引入各关节的广义力为

采用本发明给出的以上步骤，以数学仿真模拟实际机械臂***、以通过力学理论解算得到的关节C1、C2、C3的实时约束力和约束力矩大小模拟实际***中六维力/力矩传感器测量值，对本方法的观测效果进行验证。选择观测器增益K_MO＝200；由于数学仿真中未引入不确定因素的干扰，故设定判断阈值γ_C＝0。对该闭环机械臂***中关节11、21、31输入如图3 所示的待观测广义碰撞力，利用本发明公开的方法观测到的碰撞信息如图4所示，可见本发明公开的方法可以实现对闭环机械臂***的碰撞识别并获取关节广碰撞义力信息。

以上所述的具体描述，对发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施例而已，并不用于限定本发明的保护范围，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (2)

1.一种基于动量观测器的闭环机械臂***碰撞检测方法，其特征在于：具体步骤为：

步骤一：在闭环机械臂***中配置运动传感器，所述运动传感器用于采集关节的转角

及转角速度

在闭环机械臂***中配置六维力或六维力矩传感器，所述六维力或六维力矩传感器用于采集每一闭环约束中任一关节处作用的约束力或约束力矩大小，记为

步骤二：建立闭环机械臂***动力学模型；

根据第一类拉格朗日方程理论得到闭环机械臂***动力学模型：

其中，M(q)为***的质量特性矩阵；q、

为各关节处的转角、角速度、角加速度，即***的广义坐标、广义速率以及广义加速度；A(q)、

分别为***中六维力或六维力矩传感器配置关节的约束雅各比矩阵及其导数；μ为拉格朗日乘子，实际物理意义为六维力或六维力矩传感器配置关节处对应的约束力或约束力矩；F^C、F^gc、

g(q)分别为关节驱动电机输入对各关节的广义主动力、碰撞引入各关节的广义碰撞力、各体运动耦合产生的广义惯性力非线性项以及重力对各关节的广义力；

步骤三：设计动量观测器用于观测各关节的广义碰撞力；

定义***广义动量为

同时由多体动力学理论得：

则根据式(1)所述闭环机械臂***动力学模型，广义动量的导数表述为：

其中中间变量

构造动量观测器如下：

其中，

为广义动量及其导数的观测量；带有^符号的量均为对应物理量的观测量或测量量；γ为观测器输出；K_MO为观测器增益，用于调节观测器跟踪性能和灵敏度；

步骤四：对闭环机械臂***进行碰撞检测；

将步骤一实时测量的关节约束力或约束力矩

关节的转角

转角速度

以及由关节驱动电机的电流值估算的

带入步骤三所设计的动量观测器，则可通过观测器输出γ对闭环机械臂***进行碰撞检测；由于实际***内部、外部存在摩擦以及环境的不确定因素干扰，需根据各类干扰的具体情况以及控制精度需求设定判断碰撞是否发生的阈值γ_C；当γ≤γ_C时，认为观测到的信息是由不确定因素干扰引起的；当γ＞γ_C时，认为碰撞发生，且观测到的关节的广义碰撞力为

2.如权利要求1所述的一种基于动量观测器的闭环机械臂***碰撞检测方法，其特征在于：所述观测器能够用于观测的校验方法为：

根据式(3)以及式(2)，有

当测量量足够准确，有

且

同时若

则有

此时式(4)表示为

则能够看出理论上观测器的输出γ以指数接近广义碰撞力F^gc；通过调节观测器增益K_MO可调节观测器跟踪性能和灵敏度。

Images