CN111805538A - 一种基于力反馈的机器人实时运动规划方法 - Google Patents

Abstract

一种基于力反馈的机器人实时运动规划方法，针对在较大周期姿态修正量下，机械臂不能平滑加/减运动的问题，修正了现有的分段线性力位控制方法。采用六维力传感器感知外部作用力，获取坐标系各轴每周期姿态目标修正量；并通过将各坐标轴稳定运行时的最大周期姿态修正量增量确定各轴每周期姿态修正量插补步幅基数，最后通过力位转换、插补逼近各轴的周期姿态目标修正量，避免了机械臂启动或停止时，运动状态的改变导致的震荡，实现机械臂力控过程中的平滑运动。

Description

一种基于力反馈的机器人实时运动规划方法

技术领域

本发明涉及一种基于力反馈的机器人实时运动规划方法，属于控制工程技术领域。

背景技术

机器人是一种大承载、高精度的自动化机械系统，并越来越多的应用到加工及装配领域。但在某些加工装配领域，传统的示教难以满足精度要求、耗时长、环境适应性差。基于力反馈的机器人主动柔顺控制系统由机械臂、机械臂控制柜、安装于机械臂末端的力传感器以及上位机控制系统组成，能够跟随外部作用力进行柔性运动，实现机械臂末端部件姿态的灵活调整。

在机器人柔顺力控制研究中发现：为了实现机械臂的运行速度大小与外部作用力的大小正相关，采用传统的线性映射方法，将外部作用力与机械臂周期姿态修正量进行映射。当外部作用力较小时，通过线性映射得到的姿态周期修正量较小，机械臂可以平稳启动并运行；当外部作用力较大时，得到的姿态周期修正量较大，不仅导致机械臂启动出现震荡以及加/减速器力矩超过额定值而故障停机，同时力传感器由于机械震荡出现数据采集波动，整个系统的鲁棒性较差。

同时，三个坐标轴的加/减速力矩额定值不同。将外部作用力分解到传感器坐标系的X、Y、Z轴，得到各个轴的受力比分别通过线性映射得到对应的轴周期姿态修正量。那么，在一定的作用力取值区域，三个轴不能按照既定占比进行运动平滑加、减速。

发明内容

本发明解决的技术问题是：针对目前现有技术中，采用机械臂末端受力与周期姿态修正量分段线性映射对机械臂进行柔顺力随动控制过程中，容易出现的运动震荡以及故障停机，单纯依赖理论模型未考虑机械臂各轴的实际额定力矩约束的问题，提出了一种基于力反馈的机器人实时运动规划方法。

本发明解决上述技术问题是通过如下技术方案予以实现的：

一种基于力反馈的机器人实时运动规划方法，步骤如下：

(1)利用六维力传感器进行数据采集，经重力补偿及坐标变换后获取机械臂基坐标系下X，Y，Z轴的分作用力f_X，f_Y，f_Z，并根据分作用力f_X，f_Y，f_Z获取机械臂基坐标系下的外部作用力f；

(2)根据步骤(1)所得外部作用力f建立外部作用力与机械臂基坐标系下周期姿态目标修正量分段映射关系f(f、v_a、f₁、f₂)；

(3)根据步骤(2)所得映射关系f(f、v_a、f₁、f₂)确定周期修正速度v_a，根据步骤(1)所得分作用力f_X，f_Y，f_Z与外部作用力f的比值，分别映射至X、Y、Z轴上，获取各轴力与周期姿态目标修正量的映射关系f₁(f、f_x、v_ax)、f₂(f、f_Y、v_aY)、f₃(f、f_Z、v_aZ)；

(4)确定机械臂基坐标系下，机械臂三轴各自稳定运行情况下周期姿态修正量增量Δv_X，Δv_Y，Δv_Z，选取最小值Δv并确定对应轴的外部作用力值f_A；

(5)根据步骤(4)中选定的最小值Δv及其外部作用力值f_A、步骤(1)中分作用力f_X，f_Y，f_Z计算X、Y、Z轴的周期姿态修正量插补步幅基数v_bX，v^b _Y，v^b _Z；

(6)根据各轴每周期累计插补步数Step_X，Step_Y，Step_Z与步骤(5)所得周期姿态修正量插补步幅基数计算三轴当前周期姿态累积插补修正量v_sX，v_sY，v_sZ；

(7)对步骤(6)所得三轴当前周期姿态累积插补修正量分别进行判断，若各轴当前周期姿态累积插补修正量大于该轴周期姿态目标修正量，则通过插补步数随运行周期逐步递减至实时周期姿态目标修正量，否则逐步递增至实时周期姿态目标修正量。

所述步骤(1)中，所述六维力传感器坐标系与机械臂工具坐标系X、Y轴分别重合，所述外部作用力的计算方法为：利用基坐标系下六维力传感器测量值减去机械臂末端负载重力、六维力传感器传感器静态误差值后获得。

所述步骤(2)中，映射关系f(f、v_a、f₁、f₂)具体为：

式中，k1，k2，k3分别为力位转换比例因子，f₁，f₂分别为函数分段数据阈值，v_a为周期修正速度，其中：

若外部作用力f小于数据计算阈值f₁，则舍弃该数值并返回步骤(1)重新采集下一组数据，否则根据步骤(2)中映射关系建立力位映射转换。

所述步骤(3)中，映射关系f₁(f、f_x、v_ax)、f₂(f、f_Y、v_aY)、f₃(f、f_Z、v_aZ)具体为：

X轴周期姿态目标修正量为：

Y轴周期姿态目标修正量为：

Z轴周期姿态目标修正量为：

所述步骤(5)中，X、Y、Z轴的周期姿态修正量插补步幅基数v_bX，v_bY，v_bZ的计算方法具体为：

X轴方向的周期姿态修正量插补步幅基数分段函数如下：

Y轴方向的周期姿态修正量插补步幅基数分段函数如下：

Z轴方向的周期姿态修正量插补步幅基数分段函数如下：

所述步骤(6)中，三轴当前周期姿态累积插补修正量v_sX，v_sY，v_sZ的计算方法为：

X轴方向当前周期姿态累积插补修正量：v_sX＝v_bX*Step_X

Y轴方向当前周期姿态累积插补修正量：v_sY＝v_bY*Step_Y

Z轴方向当前周期姿态累积插补修正量：v_sz＝v_bz*Step_Z。

所述步骤(7)中，当前周期姿态累积插补修正量随运行周期逐步递增或递减的方法具体为：

(7a)确定三轴各自当前周期姿态修正目标插补步数，具体为：

(7b)于逐步递增或递减，拟合逼近周期姿态目标修正量的过程中，X轴方向的实时周期姿态累积插补修正量具体为：

v_sX＝Step_X'*v_bX

Y轴方向的实时周期姿态累积插补修正量具体为：

v_sY＝Step_Y'*v_bY

Z轴方向的实时周期姿态累积插补修正量具体为：

本发明与现有技术相比的优点在于：

本发明提供的一种基于力反馈的机器人实时运动规划方法，采用六维力传感器感知外部作用力，建立了力与周期姿态目标修正量的三段分段函数，并设定力控触发阈值，通过函数映射获取各轴周期姿态目标修正量并比较获得各轴中的最小值及其外部作用力，在此基础上根据三轴的外部作用力相互关系获取各轴的周期姿态修正量插补步幅基数，并按照各自修正插补步幅进行逼近拟合加速，实现了机械臂的平滑加速、减速，能够在较大周期姿态修正量下机械臂力控过程中的平滑运动，不仅消除了力传感器因为加速度较大突变导致的数据采集噪声，同时实现了加工装配的稳定性，提高了加工装配质量。

附图说明

图1为发明提供的运动规划方法流程图；

具体实施方式

一种基于力反馈的机器人实时运动规划方法，针对在较大周期姿态修正量情况下，机械臂不能平滑运动的问题，修正了现有的分段线性力位控制方法，如图1所示，具体步骤为：

(1)利用六维力传感器进行数据采集，经重力补偿及坐标变换后获取机械臂基坐标系下X，Y，Z轴的分作用力f_X，f_Y，f_Z，并根据分作用力f_X，f_Y，f_Z获取机械臂基坐标系下的外部作用力f；

其中，六维力传感器坐标系与机械臂工具坐标系X、Y轴分别重合，所述外部作用力的计算方法为：利用基坐标系下六维力传感器测量值减去机械臂末端负载重力、六维力传感器传感器静态误差值后获得，机械臂基坐标系原点一般位于机械臂足部，Z轴垂直向上。工具坐标系的原点一般位于机械臂末端法兰，Z轴垂直向外。两个坐标系均为出厂已定义坐标系；

(2)根据步骤(1)所得外部作用力f建立外部作用力与机械臂基坐标系下周期姿态目标修正量分段映射关系f(f、v_a、f₁、f₂)；

其中，映射关系f(f、v_a、f₁、f₂)具体为：

式中，k1，k2，k3分别为力位转换比例因子，f₁，f₂分别为函数分段数据阈值，v_a为周期修正速度，其中：

若外部作用力f小于数据计算阈值f₁，则舍弃该数值并返回步骤(1)重新采集下一组数据，否则根据步骤(2)中映射关系建立力位映射转换并进入步骤(3)；

(3)根据步骤(2)所得映射关系f(f、v_a、f₁、f₂)确定周期修正速度v_a，根据步骤(1)所得分作用力f_X，f_Y，f_Z与外部作用力f的比值，分别映射至X、Y、Z轴上，获取各轴力与周期姿态目标修正量的映射关系f₁(f、f_x、v_ax)、f₂(f、f_Y、v_aY)、f₃(f、f_Z、v_aZ)，其中：

映射关系f₁(f、f_x、v_ax)、f₂(f、f_Y、v_aY)、f₃(f、f_Z、v_aZ)具体为：

X轴周期姿态目标修正量为：

Y轴周期姿态目标修正量为：

Z轴周期姿态目标修正量为：

(4)确定机械臂基坐标系下，机械臂三轴各自稳定运行情况下周期姿态修正量增量Δv_X，Δv_Y，Δv_Z，选取最小值Δv并确定对应轴的外部作用力值f_A；

(5)根据步骤(4)中选定的最小值Δv及其外部作用力值f_A、步骤(1)中分作用力f_X，f_Y，f_Z计算X、Y、Z轴的周期姿态修正量插补步幅基数v_bX，v_bY，v_bZ；

其中，X、Y、Z轴的周期姿态修正量插补步幅基数v_bX，v_bY，v_bZ的计算方法具体为：

X轴方向的周期姿态修正量插补步幅基数分段函数如下：

Y轴方向的周期姿态修正量插补步幅基数分段函数如下：

Z轴方向的周期姿态修正量插补步幅基数分段函数如下：

(6)根据各轴每周期累计插补步数Step_X，Step_Y，Step_Z与步骤(5)所得周期姿态修正量插补步幅基数计算三轴当前周期姿态累积插补修正量v_sX，v_sY，v_sZ；

三轴当前周期姿态累积插补修正量v_sX，v_sY，v_sZ的计算方法为：

X轴方向当前周期姿态累积插补修正量：v_sX＝v_bX*Step_X

Y轴方向当前周期姿态累积插补修正量：v_sY＝v_bY*Step_Y

Z轴方向当前周期姿态累积插补修正量：v_sz＝v_bz*Step_Z。

(7)对步骤(6)所得三轴当前周期姿态累积插补修正量分别进行判断，若各轴当前周期姿态累积插补修正量大于该轴周期姿态目标修正量，则通过插补步数随运行周期逐步递减至实时周期姿态目标修正量，否则逐步递增至实时周期姿态目标修正量，具体为：

(7a)确定三轴各自当前周期姿态修正目标插补步数，具体为：

(7b)于逐步递增或递减，拟合逼近周期姿态目标修正量的过程中，X轴方向的实时周期姿态累积插补修正量具体为：

v_sX＝Step_X'*v_bX

Y轴方向的实时周期姿态累积插补修正量具体为：

v_sY＝Step_Y'*v_bY

Z轴方向的实时周期姿态累积插补修正量具体为：

v_sZ＝Step_Z'*v_bZ

下面结合具体实施例进行进一步说明：

对六维力传感器进行数据采集，通过重力补偿及坐标转换，获取机械臂基坐标系下X，Y，Z轴的分作用力f_X，f_Y，f_Z，并根据分作用力f_X，f_Y，f_Z获取机械臂基坐标系下的外部作用力f。

建立外部作用力f与机械臂基坐标系下周期姿态目标修正量分段映射关系，f(f、v_a、f₁、f₂)；

计算周期修正速度v_a，按照步骤(1)中获得的三个轴的作用力f_X，f_Y，f_Z与合作用力f的比值关系，分别映射到三轴，得到三轴与周期姿态目标修正量的映射关系f₁(f、f_x、v_ax)、f₂(f、f_Y、v_aY)、f₃(f、f_Z、v_aZ)；

确定机械臂X，Y，Z轴各自均能稳定运行的周期姿态修正量增量Δv_X，Δv_Y，Δv_Z，三者中的最小值Δv以及对应轴的外部作用力值f_A；在本实施例中，Δv_Y最小；

计算三轴的作用力大小建立三轴的周期姿态修正量插补步幅基数v_bX，v_bY，v_bZ，其中：

X轴、Y轴、Z轴方向的周期姿态修正量插补步幅基数分段函数如下：

v_bY＝Δv_Y

在上述计算所得周期姿态修正量插补步幅基数基础上，由各轴每周期累计插补步数Step_X，Step_Y，Step_Z与各轴每周期姿态修正量插补步幅基数v_bX，v_bY，v_bZ分别相乘得到各轴当前周期姿态累积插补修正量v_sX，v_sY，v_sZ，在拟合逼近周期姿态目标修正量的过程中，先对各轴周期姿态累积插补修正量与对应周期姿态目标修正量进行大小关系判断，再判断各轴周期姿态累积插补修正量是逐步递增还是递减。

尽管本发明的内容已经通过上述优选实施例作了详细介绍，但应当认识到上述的描述不应被认为是对本发明的限制。在本领域技术人员阅读了上述内容后，对于本发明的多种修改和替代都将是显而易见的。因此，本发明的保护范围应由所附的权利要求来限定。

Claims (7)

1.一种基于力反馈的机器人实时运动规划方法，其特征在于步骤如下：

(1)利用六维力传感器进行数据采集，经重力补偿及坐标变换后获取机械臂基坐标系下X，Y，Z轴的分作用力f_X，f_Y，f_Z，并根据分作用力f_X，f_Y，f_Z获取机械臂基坐标系下的外部作用力f；

(2)根据步骤(1)所得外部作用力f建立外部作用力与机械臂基坐标系下周期姿态目标修正量分段映射关系f(f、v_a、f₁、f₂)；

(3)根据步骤(2)所得映射关系f(f、v_a、f₁、f₂)确定周期修正速度v_a，根据步骤(1)所得分作用力f_X，f_Y，f_Z与外部作用力f的比值，分别映射至X、Y、Z轴上，获取各轴力与周期姿态目标修正量的映射关系f₁(f、f_x、v_ax)、f₂(f、f_Y、v_aY)、f₃(f、f_Z、v_aZ)；

(4)确定机械臂基坐标系下，机械臂三轴各自稳定运行情况下周期姿态修正量增量Δv_X，Δv_Y，Δv_Z，选取最小值Δv并确定对应轴的外部作用力值f_A；

(5)根据步骤(4)中选定的最小值Δv及其外部作用力值f_A、步骤(1)中分作用力f_X，f_Y，f_Z计算X、Y、Z轴的周期姿态修正量插补步幅基数v_bX，v_bY，v_bZ；

(6)根据各轴每周期累计插补步数Step_X，Step_Y，Step_Z与步骤(5)所得周期姿态修正量插补步幅基数计算三轴当前周期姿态累积插补修正量v_sX，v_sY，v_sZ；

(7)对步骤(6)所得三轴当前周期姿态累积插补修正量分别进行判断，若各轴当前周期姿态累积插补修正量大于该轴周期姿态目标修正量，则通过插补步数随运行周期逐步递减至实时周期姿态目标修正量，否则逐步递增至实时周期姿态目标修正量。

2.根据权利要求1所述的一种基于力反馈的机器人实时运动规划方法，其特征在于：所述步骤(1)中，所述六维力传感器坐标系与机械臂工具坐标系X、Y轴分别重合，所述外部作用力的计算方法为：利用基坐标系下六维力传感器测量值减去机械臂末端负载重力、六维力传感器传感器静态误差值后获得。

3.根据权利要求1所述的一种基于力反馈的机器人实时运动规划方法，其特征在于：所述步骤(2)中，映射关系f(f、v_a、f₁、f₂)具体为：

式中，k1，k2，k3分别为力位转换比例因子，f₁，f₂分别为函数分段数据阈值，v_a为周期修正速度，其中：

若外部作用力f小于数据计算阈值f₁，则舍弃该数值并返回步骤(1)重新采集下一组数据，否则根据步骤(2)中映射关系建立力位映射转换。

4.根据权利要求1所述的一种基于力反馈的机器人实时运动规划方法，其特征在于：所述步骤(3)中，映射关系f₁(f、f_x、v_ax)、f₂(f、f_Y、v_aY)、f₃(f、f_Z、v_aZ)具体为：

X轴周期姿态目标修正量为：

Y轴周期姿态目标修正量为：

Z轴周期姿态目标修正量为：

5.根据权利要求1所述的一种基于力反馈的机器人实时运动规划方法，其特征在于：所述步骤(5)中，X、Y、Z轴的周期姿态修正量插补步幅基数v_bX，v_bY，v_bZ的计算方法具体为：

X轴方向的周期姿态修正量插补步幅基数分段函数如下：

Y轴方向的周期姿态修正量插补步幅基数分段函数如下：

Z轴方向的周期姿态修正量插补步幅基数分段函数如下：

6.根据权利要求1所述的一种基于力反馈的机器人实时运动规划方法，其特征在于：所述步骤(6)中，三轴当前周期姿态累积插补修正量v_sX，v_sY，v_sZ的计算方法为：

X轴方向当前周期姿态累积插补修正量：v_sX＝v_bX*Step_X

Y轴方向当前周期姿态累积插补修正量：v_sY＝v_bY*Step_Y

Z轴方向当前周期姿态累积插补修正量：v_sz＝v_bz*Step_Z。

7.根据权利要求1所述的一种基于力反馈的机器人实时运动规划方法，其特征在于：所述步骤(7)中，当前周期姿态累积插补修正量随运行周期逐步递增或递减的方法具体为：

(7a)确定三轴各自当前周期姿态修正目标插补步数，具体为：

(7b)于逐步递增或递减，拟合逼近周期姿态目标修正量的过程中，X轴方向的实时周期姿态累积插补修正量具体为：

v_sX＝Step_X'*v_bX

Y轴方向的实时周期姿态累积插补修正量具体为：

v_sY＝Step_Y'*v_bY

Z轴方向的实时周期姿态累积插补修正量具体为：

v_sZ＝Step_Z'*v_bZ。

Images