CN106483964A - 一种基于接触力观测器的机器人柔顺控制方法 - Google Patents

Abstract

本发明公开一种基于接触力观测器的机器人柔顺控制方法，属于机器人控制领域，不采用力传感器测量机器人与环境的接触力，而是根据运动状态采用模型估计力的大小，采用基于位置的阻抗控制器实现机器人的柔顺控制。通过编码器获得关节角速度信息后，通过状态观测器估计角度、角速度和角加速度信息。然后根据电机电流信息和关节状态信息通过扰动观测器计算关节有效驱动力矩。同时根据关节运动状态可以通过动力学模型计算出驱动机构运动所需要的关节驱动力矩。用有效驱动力矩减去动力学模型计算所得的驱动力矩就是外力作用引起的关节驱动力矩，再由雅克比矩阵映射得到环境接触力。本发明的优点在于：不需要安装价格昂贵且易损坏的多维力传感器。

Description

一种基于接触力观测器的机器人柔顺控制方法

技术领域

本发明属于机器人控制领域，具体是一种基于接触力观测器的机器人柔顺控制方法，无需多维力传感器，采用模型估计的方法估计接触力作为柔顺控制的力反馈，实现机器人的柔顺控制。

背景技术

机器人柔顺控制在切削、磨光、装配作业中应用广泛，也应用于行走机器人的行走过程中。顺性控制分为主动柔顺控制和被动柔顺控制两类。机器人凭借一些辅助柔顺机构，使其在与环境接触时能够对外部作用力产生自然顺应，称为被动柔顺控制；被动柔顺机构即利用一些可以使机器人在与环境作用时，能够吸收或存储能量的机构器件如弹簧、阻尼等而构成的机构。机器人采用被动柔顺控制在作业中存在一些问题：（1）无法根除机器人高刚度与高柔性间的矛盾。（2）被动柔顺装置的专用性强，适应能力差，适用范围受到限制。（3）无法使机器人本身产生对力的反应动作，成功率较低等。机器人利用力的反馈信息采用一定的控制策略去主动控制作用力，称为主动柔顺控制。主动柔顺控制也称为力控制。机器人主动柔顺控制实现克服了被动柔顺控制的不足。因此，机器人的主动柔顺控制研究成果具有十分广阔的应用前景。

目前实现主动柔顺控制的方法主要有两类。一为阻抗控制，另一类是力和位置的混合控制。阻抗控制不是直接控制期望的力和位置，而是通过控制力和位置之间的动态关系来实现柔顺功能。这样的动态关系类似于电路中阻抗的概念，因此称为阻抗控制。阻抗控制又可分为两类，一类是位置型阻抗控制，原理是对电机的控制采用位置控制，将力的差值通过阻抗模型转化为位置误差，修正目标位置。该方法由于内环是位置控制，所以具有较强的鲁棒性，且由于位置控制比较成熟，所以该方法应用广泛。另一类是基于力的阻抗控制，该方法的直接控制电机的驱动力矩，抗扰动能力较弱。力/位混合控制是指根据作业需要在任务空间中对不同方向上分别进行力和位置控制，通过雅克比矩阵映射到关节空间，相加后合并为统一的关节力矩，从而实现任务空间中有约束方向上进行力控制，无约束方向上实现力控制。其他主动柔顺控制方法多为这两种方法的变形和改进。

无论是阻抗控制还是力/位混合控制，都需要机器人末端与外界环境接触的力的信息作为反馈。目前多采用在机器人手腕处或者脚踝处安装多维力传感器的方法实现力的测量，再反馈给控制。但是多维力传感器往往价格昂贵，而且极易损坏，对于部分机器人还存在无法安装力传感器的情况。本发明将采用一种基于动力学模型的方法估计机器人与外界的接触力，然后采用阻抗控制实现机器人的柔顺控制。因此该方法无需力传感器，具有重要的应用价值。

发明内容

本发明提供的一种基于接触力观测器的机器人柔顺控制方法，该方法主要由位置控制器、阻抗控制器和基于动力学模型的环境接触力观测器组成，其特征在于采用接触力传感器代替多维力传感器得到接触力的反馈，采用位置型阻抗控制器实现柔顺控制，该方法包括下述步骤：

步骤1：预先建立机器人运动学模型、动力学模型和主动关节摩擦力模型；

步骤2：根据任务需要规划机器人末端运动轨迹，通过逆运动学计算得到关节运动角度；

步骤3：通过位置控制器（如PID控制器等）控制机器人的运动；

步骤4：机器人运动过程中实时采集电机电流值和电机转动角速度；

步骤5：将角速度信息输入状态观测器，得到估计的角位移、角速度和角加速度信息；

步骤6：将电流信息和步骤3中得到的角位移、角速度和角加速度信息输入到扰动观测器中，得到关节输出力矩；

步骤7：将步骤3中得到的角位移、角速度和角加速度信息输入到机器人动力学模型中，得到由动力学模型计算而来的关节驱动力矩；

步骤8：用步骤4中得到的关节输出力矩减掉步骤5中得到的由动力学模型计算得到的关节驱动力矩，得到末端接触力占关节驱动力的部分；

步骤9：根据步骤8中得到的接触力的大小判断机器人有没有跟外界环境发生接触；如果有接触，则进行阻抗控制，如果没有发生接触，则阻抗控制不起作用；

步骤10：经过步骤9中的判断，在机器人跟外界发生接触情况下，将步骤8中得到的接触力占关节驱动力的部分乘以力雅克比矩阵即可估计出接触力在三维空间中X/Y/Z三个方向上的大小；

步骤11：分别对X/Y/Z三个方向上的接触力与期望作用力作差，将差值输入阻抗控制器，输出该方向上的位移修正量；

步骤12：将步骤11中的阻抗控制器输出结果与期望位移作差作为新的轨迹，实现柔顺控制。

如上所述，本发明提出一种基于接触力观测器的机器人柔顺控制方法，具有以下有益效果：

仅依据关节驱动电机的电流和电机编码器测量得到的角速度信息，运用接触力观测器即可估计出环境接触力，作为力反馈信息完成阻抗控制，从而代替多维力传感器，节约设备开发成本，降低因力传感器损坏造成的损失。

附图说明

图1是基于接触力观测器的机器人柔顺控制方法控制框图。

图2是3自由度并联机器人机构简图。

图3是3自由度串联机器人机构简图。

图4是3自由度并联机器人柔顺控制实验示意图。

图5是柔顺控制力控制实验结果。

图6是柔顺控制位移实验结果。

具体实施方式

以下结合实施例及附图对本发明作进一步详细的说明，但本发明实施方式不限于此。该具体实施例选取三自由度并联机器人（如图2所示）作为作用对象，但实施对象不限于此，多自由度串联机器人（如图3所示）同样适用。

以下将详细叙述本发明的一种基于接触力观测器的机器人柔顺控制方法的原理及实施方式，使本领域技术性人员不需要创造性劳动即可理解本发明一种基于接触力观测器的机器人柔顺控制方法。

如图1所示，本发明提供一种基于接触力观测器的机器人柔顺控制方法具体包括以下步骤：

步骤1：图2中机器人具有一条UP支链，两条UPS支链，U代表万向节，P代表移动副，S代表球副。其中P为主动关节，由伺服电机带动滚珠丝杠副实现驱动。建立机器人运动学模型、动力学模型和关节摩擦模型，并对未知参数进行参数辨识，得到精确的机器人动力学模型。

步骤2：根据任务需求，规划机器人的末端运动轨迹 ，通过逆运动学即可求得关节转角。

步骤3：对每一个关节都采用单独的位置控制器控制关节运动，此处以PID控制器为例加以说明，PID控制器的控制率为

(1)

式中，为比例系数，为积分时间常数，为微分时间常数。对于关节i而言，采样周期k对应的位置误差由期望关节转角和编码器测得转角作差得到，

(2)

PID控制器的参数、、可以采用Z—N法整定。

步骤4：机器人运动过程中，伺服电机可以实时将电机电流信息和编码器得到的角位移信息上传至控制器，该角位移信息伴有噪声。

步骤5：动力学模型中需要用到关节的角度、角速度和角加速度，而电机编码器只能得到角位移，因此需要状态观测器估计角速度和角加速度，而且编码器直接输出的角位移是带有噪声的信号，状态观测器也可以滤除噪声。此处以卡尔曼滤波器作为状态观测器进行说明。卡尔曼滤波器是一种最优状态观测器，由预测和修正两个步骤组成。

设状态方程和观测方程为

(3)

(4)

式中为第k个采样周期的状态向量，为第k-1个采样周期的状态向量，和分别为状态矩阵和观测矩阵，和分别为过程噪声和测量噪声，它们之间是彼此独立的，而且不同k之间也是独立的。

设关节位置信号是由噪声驱动的ARMA模型，表示为如下形式

(5)

式中为频域符号。可以证明在采样频率非常小的情况下，不会影响卡尔曼滤波的特性，这样可以得到一个噪声驱动的全积分模型

(6)

令(6)式中，，，这样可将(3)和(4)式化为状态空间形式，

(7)

(8)

有了(7)式和(8)式，就可以应用卡尔曼滤波对信号进行估计。卡尔曼滤波的具体过程为四步。

第一步，基于系统的上一状态而预测出现在状态：

(9)

第二步，更新协方差：

(10)

式中是对应的协方差，是系统过程的协方差。

有了现在状态的预测结果，然后我们再收集现在状态的测量值。结合预测值和测量值，我们可以得到现在状态x(k)的最优化估算值x(k|k)。第三步就是求状态最优估计值：

(11)

其中为卡尔曼增益，可由下式求得

(12)

式中R为测量噪声协方差。至此，已经得到了状态向量的最优估计值，即，也就是角位移、角速度和角加速度的最优估计值。为了使迭代能够继续进行需要更新对应的协方差，

(13)

式中I为单位矩阵。当进入下一个采样周期后，又可以依次按照这四步求解下一个周期的最优估计状态。

对于本发明中涉及的机器人，需要分别估计一条腿中三个关节的角位移、角速度和角加速度状态。设，则通过以上卡尔曼滤波器，即可得到状态的最优估计值、和。

步骤6：采用扰动观测器计算电机输出的有效力矩。

由于电机不是直接驱动连杆的，往往是通过减速器或者其他运动副与连杆相连，而这些中间环节往往具有较大的惯量和摩擦，需要通过扰动观测器求解关节力矩。

对于关节i具有以下平衡方程：

(14)

式中为电机力矩系数，为电机电流，为折算到电机轴上的关节转动惯量，是外界所有扰动之和。定义如下，

(15)

式中即为需要求取的关节扭矩，由步骤1中建立的摩擦模型求得。值得注意的是此处将其他扰动全部合并到摩擦力中，因此摩擦模型一般不能用简单的库仑摩擦加粘性摩擦来描述，需要根据实际情况合理建立新的摩擦模型。联立(14)和(15)式即可得到关节力矩为

(16)

将步骤5中得到的代入(16)式，即可得到关节有效输出力矩。

步骤7：依据机器人腿部机构动力学模型，计算驱动腿部运动所需要的关节驱动力。步骤1中建立的腿部机构的动力学模型为

(17)

其中M为3×3的惯性矩阵，为科氏力和向心力项，G为重力项，为除驱动关节摩擦外的其他关节摩擦。将状态观测器得到的状态估计值，和代入(15)式中，即可得到。

将步骤5中得到的、和代入(17)式，即可得到关节动力学驱动力矩。

步骤8：步骤6和步骤7中得到的力矩作差即可得到外力作用占关节驱动力的部分。

关节驱动力包含两部分，一部分是无外力作用时驱动机构运动所需驱动力，另一部分是外力作用对关节驱动力的影响，即有下式

(18)

则

(19)

式(19)得到的关节空间中的力矩，因此需要步骤9进一步将该力矩转化为外力。

步骤9：根据步骤8中得到的结果，判断机器人有没有跟外界环境发生接触。由于模型估计存在一定误差，所以设定一个阈值范围判断，如果满足

(20)

则说明机器人跟外界环境发生了接触。如果发生接触则使阻抗控制起作用，如果没有发生接触，阻抗控制不起作用。

步骤10：将步骤8中得到的外力引起的关节驱动力矩映射为足底接触力。

关节空间和任务空间的映射采用雅克比矩阵完成，即有

(21)

上式中为机器人脚在笛卡尔坐标系中三个方向的运动速度，为腿部机构雅克比矩阵。则由虚功原理可得到

(22)

将步骤8的结果代入(22)式即可得

(23)

步骤11：如果步骤9的判断结果为真，则规定三个方向上的期望接触力，再分别对X/Y/Z三个方向上的接触力与期望作用力作差，将差值输入阻抗控制器，输出该方向上的位移修正量，如下式所示

(24)

步骤12：采用阻抗控制，将力的差值转化为位置量，转换关系为

(25)

其中， ，。写成传递函数形式为

(26)

式(26)求得的位移量将作为参考轨迹的修正量，从而改变参考轨迹，使特定方向上的接触力控制到期望的力的大小，实现柔顺控制。

图4为采用本发明所提供的柔顺控制方法进行的验证性试验。将本发明所提控制方法应用到图2所示的3自由度并联机器人上。机器人末端跟踪一空间轨迹，只有y方向和z方向有运动。增加一障碍物，障碍物会阻碍机器人的运动。令z方向上的期望接触力为300N。图5和图6显示了力的控制效果和位移变化情况。图5显示当机器人接触到障碍物后z方向上的力可以快速控制到300N；图6显示，在遇到障碍物之前，机器人末端轨迹很好的跟踪了期望轨迹，遇到障碍物后为实现力的控制改变了期望轨迹。因此实验结果表明本发明所提一种基于接触力观测器的机器人柔顺控制方法是可行有效的。

Claims (9)

1.一种基于接触力观测器的机器人柔顺控制方法，其特征在于，该方法包括下述步骤：

(1) 预先建立机器人运动学模型、动力学模型和主动关节摩擦力模型；

(2) 根据任务需要规划机器人末端运动轨迹，通过逆运动学计算得到关节运动角度；

(3) 通过位置控制器（如PID控制器等）控制机器人的运动；

(4) 机器人运动过程中实时采集电机电流值和电机转动角速度；

(5) 将角速度信息输入状态观测器，得到估计的角位移、角速度和角加速度信息；

(6) 将电流信息和步骤(3)中得到的角位移、角速度和角加速度信息输入到扰动观测器中，得到关节输出力矩；

(7) 将步骤(3)中得到的角位移、角速度和角加速度信息输入到机器人动力学模型中，得到由动力学模型计算而来的关节驱动力矩；

(8) 用步骤(4)中得到的关节输出力矩减掉步骤(5)中得到的由动力学模型计算得到的关节驱动力矩，得到末端接触力占关节驱动力的部分；

(9) 根据步骤(8)中得到的接触力的大小判断机器人有没有跟外界环境发生接触；如果有接触，则进行阻抗控制，如果没有发生接触，则阻抗控制不起作用；

(10) 经过步骤(9)中的判断，在机器人跟外界发生接触情况下，将步骤(8)中得到的接触力占关节驱动力的部分乘以力雅克比矩阵即可估计出接触力在三维空间中X/Y/Z三个方向上的大小；

(11) 分别对X/Y/Z三个方向上的接触力与期望作用力作差，将差值输入阻抗控制器，输出该方向上的位移修正量；

(12) 将步骤(11)中的阻抗控制器输出结果与期望位移作差作为新的轨迹，实现柔顺控制。

2.根据权利要求1所述的基于接触力观测器的机器人柔顺控制方法，其特征在于，步骤(5)、(6)、(7)、(8)组成的接触力观测器可以估计外界环境接触力。

3.根据权利要求1所述的基于接触力观测器的机器人柔顺控制方法，其特征在于，步骤(3)中位置控制器不限于PID控制器，其他位置控制器也同样适用。

4.根据权利要求1所述的基于接触力观测器的机器人柔顺控制方法，其特征在于，步骤(5)中，由状态观测器估计关节角度、角速度和角加速度，后续所有用到的状态值都是该步采用状态观测器估计的结果。

5.根据权利要求1所述的多足机器人动态稳定性定量判定方法，其特征在于，步骤(6)中，采用扰动观测器计算关节有效输出力矩，对于第i个关节的输出力矩计算公式如下

(1)

式中为电机力矩系数，为电机电流，为折算到电机轴上的关节转动惯量，即为需要求取的关节扭矩，由步骤1中建立的摩擦模型求得。

6.根据权利要求1所述的多足机器人动态稳定性定量判定方法，其特征在于，步骤(7)中，由动力学模型计算驱动机构运动所需要的关节驱动力：

(2)

其中M为3×3的惯性矩阵，为科氏力和向心力项，G为重力项，为除驱动关节摩擦外的其他关节摩擦。

7.根据权利要求1所述的多足机器人动态稳定性定量判定方法，其特征在于，步骤(8)中，按照以下公式计算外界作用力占驱动扭矩的部分

(3)

上式得到外界接触力在关节空间中的映射。

8.根据权利要求1所述的多足机器人动态稳定性定量判定方法，其特征在于，步骤(7)中，采用雅克比矩阵将外力作用所致的关节驱动力映射为足底接触力

(4)

式中即为足底接触力在笛卡尔坐标系中的表示。

9.根据权利要求1所述的多足机器人动态稳定性定量判定方法，其特征在于，步骤(11)中，采用阻抗控制器将X/Y/Z三个方向上的力误差通过阻抗控制器转换为X/Y/Z三个方向上的位移量，以此修正期望轨迹，

(5)

式中， ，。