CN105676898B - 一种基于关节位置控制器的机器人末端力控制方法 - Google Patents

Abstract

本发明一种基于关节位置控制器的机器人末端力控制方法，将机器人末端与目标之间的接触力作为输入，基于机器人的关节位置控制器，通过对机器人各关节的角度控制实现对机器人末端的力控制，包括根据机器人末端运动的起点和终点位置，及机器人逆运动学方程，计算与机器人末端位置对应的机器人各关节起始和终止角度；根据得到的机器人各关节起始和终止角度，及多项式插值方程，计算机器人各关节运动的连续角度函数；根据得到的各关节运动的连续角度函数，及机器人末端所受的接触力，计算机器人关节位置控制器控制信号。该方法既保证了机器人末端运动的位置精度，又避免了机器人末端与外界的接触力过大，并可以通过控制参数设定接触力大小。

Description

一种基于关节位置控制器的机器人末端力控制方法

技术领域

本发明涉及一种基于位置控制的力控制方法，尤其涉及一种基于关节位置控制器的机器人末端力控制方法。

背景技术

机器人的多数操作任务是通过各关节的位置控制使其末端与目标接触完成的，如果接触力过大，会导致机器人本体或目标损坏，因此在机器人作业过程中，如何保持末端与目标接触并控制接触力是机器人能否顺利完成操作任务的关键问题之一。

为了使机器人末端与目标接触并保持一定的接触力，以往的方法可以分为三类。一类方法是根据目标的几何形状设计具有一定柔性的机器人末端专用装置；另一类方法是利用具有力控制功能的机器人关节控制器，控制机器人末端与目标之间的接触力；还有一类方法是直接测量机器人末端与目标之间的接触力，通过实时的机器人逆运动学或者逆动力学解算，对机器人末端的位置或速度进行修正。

现有的机器人末端力控制方法中，为机器人末端加装专用柔性装置由于要求装置的柔性必须与目标匹配，所以该方法比较局限，而且研制成本较高；具有力控制功能的机器人关节控制器技术尚不成熟，系统实现困难；基于机器人逆运动学或者动力学实时解算的力控制方法严重依赖于算法的可靠性，并且运算量较大。

发明内容

本发明解决的技术问题：克服现有机器人力控制技术不能直接应用于一般机器人的问题，提供一种利用机器人自身的关节位置控制器实现机器人末端力控制方法。

本发明的技术解决方案是：一种基于关节位置控制器的机器人末端力控制方法，包括以下步骤：

1)将期望的机器人末端运动的起点位置矢量p_o和终点位置矢量p_t，代入机器人的逆运动学方程，分别计算得出机器人每个关节运动的起始角度矢量θ_o和终止角度矢量θ_t；

2)根据步骤1)得到的每个关节运动的起始角度矢量θ_o和终止角度矢量θ_t，通过多项式插值计算得出连续的关节运动角度函数，函数输入为时间计数t，函数输出为关节角度θ(t)；

3)设定期望接触力f_d，其中期望接触力f_d包括最大期望接触力f_dmax和最小期望接触力f_dmin；

4)接触力控制环节将机器人末端与目标的实际接触力f与步骤3设定的期望接触力f_d比较，如果f>f_dmax，则时间计数t增加Δt，Δt为机器人控制指令的执行周期；如果f<f_dmin，则时间计数t减小Δt；如果f_dmin≤f≤f_dmax，则时间计数t不变，即

5)将步骤4)得到的时间计数t，代入到步骤2)得到的关节运动角度函数，计算得出期望关节角度θ；

6)将步骤5)得到期望关节角度θ作为控制指令发送给机器人的关节位置控制器，关节位置控制器根据控制指令控制机器人各关节运动。

步骤1)中机器人的逆运动学方程为θ＝k^-1(p)，式中k^-1()为机器人逆运动学函数。

通过多项式插值计算得出连续的关节运动角度函数的具体方法为：

θ(t)＝a₀+a₁t+a₂t²+…+a_ntⁿ；

式中，n为多项式次数，a₀,a₁,a₂,…,a_n为待确定的多项式系数，其中函数输入为时间计数t，函数输出为关节角度θ(t)。

本发明与现有技术相比具有以下优点：

(1)本发明从软件上实现了对机器人末端的力控制，并实现了根据设定的期望接触力调整接触力大小，设定的时间增量参数调整机器人末端等效刚度，而不需要对硬件进行任何改造或者升级，降低了系统成本。

(2)本发明充分利用了机器人集成的关节位置控制器，由于关节位置控制技术比较成熟，所以基于关节位置控制器的系统响应速度快，鲁棒性好。

(3)本发明整个控制过程中，只需要根据期望的机器人末端运动路径的起点和终点位置，预先进行两次逆运动学解算，避免了高强度的机器人逆运动学或者逆动力学实时解算，系统易于实现。

附图说明

图1为本发明实施例示意图；

图2为本发明控制原理图；

图3为本发明控制流程图。

具体实施方式

下面结合附图和具体实施例对本发明作进一步详细地描述：

如图2、图3所示，本发明由逆运动学环节、多项式插值环节和接触力控制环节组成。逆运动学环节根据机器人末端运动的起点和终点位置，及机器人逆运动学方程，计算与机器人末端位置对应的机器人各关节起始和终止角度。多项式插值环节根据得到的机器人各关节起始和终止角度，及多项式插值方程，计算机器人各关节运动的连续角度函数。接触力控制环节根据得到的各关节运动的连续角度函数，及机器人末端所受的接触力，计算机器人关节位置控制器控制信号。

实施例

本实施例以平面机器人为例，如图1所示，其臂杆1长度为l₁，臂杆2长度为l₂，关节1角度为θ₁，关节2角度为θ₂，末端位置从p_o＝(x_o,y_o)运动到p_t＝(x_t,y_t)，运动时间为T，且运动过程中与人为设置的目标接触，那么平面机器人运动学方程可以表示为

(1)逆运动学环节：根据上式可得，平面机器人逆运动学方程为

对于期望的机器人末端运动的起点位置矢量(x_o,y_o)和终点位置矢量(x_t,y_t)，分别代入上式，得到各关节运动的起始角度矢量θ_oθ_o和终止角度矢量θ_t为

(2)多项式插值环节：本实施例采用三次多项式插值，其表达式为

θ(t)＝a₀+a₁t+a₂t²+a₃t³

根据约束条件，可以得到

求解上式，确定三次多项式系数为

a₀＝θ_o

a₁＝0

因此，关节运动角度函数为

(3)设定控制参数：本实施例设定Δt为机器人控制指令的执行周期，即时间增量Δt＝0.001s，最大期望接触力f_dmax＝5.1N，最小期望接触力f_dmin＝4.9N。

(4)比较接触力大小：时间变量t从0开始计数，每隔0.001s比较一次实际接触力和期望接触力大小。当实际接触力大于最大期望接触力f_dmax＝5.1N，时间变量t增加Δt＝0.001s；当实际接触力小于最小期望接触力f_dmin＝4.9N，时间变量t减小Δt＝0.001s；当实际接触力介于最大期望接触力f_dmax＝5.1N与最小期望接触力f_dmin＝4.9N之间时，时间变量t保持不变。

(5)计算期望关节角度：将时间变量t代入到关节运动角度函数中，计算得出期望关节角度θ。

(6)控制机器人运动：将期望关节角度θ作为控制指令发送给机器人的关节位置控制器，关节位置控制器根据控制指令控制机器人各关节运动。

本实施例的工作效果为：机器人从期望的起点位置向期望的终点位置运动，运动误差小于0.1mm，在运动过程中机器人末端与物体接触，并施加一定的作用力，作用力大小为5.0N±0.5N。

本发明所公开的机器人末端力控制方法创造性的利用关节位置控制器实现机器人末端力控制，可以推广到其他具有位置控制器的执行机构实现力控制功能。

本发明未详细描述内容为本领域技术人员公知技术。

Claims (3)

1.一种基于关节位置控制器的机器人末端力控制方法，其特征在于：包括以下步骤：

1)将期望的机器人末端运动的起点位置矢量p_o和终点位置矢量p_t，代入机器人的逆运动学方程，分别计算得出机器人每个关节运动的起始角度矢量θ_o和终止角度矢量θ_t；

2)根据步骤1)得到的每个关节运动的起始角度矢量θ_o和终止角度矢量θ_t，通过多项式插值计算得出连续的关节运动角度函数，函数输入为时间计数t，函数输出为关节角度θ(t)；

3)设定期望接触力f_d，其中期望接触力f_d包括最大期望接触力f_dmax和最小期望接触力f_dmin；

4)接触力控制环节将机器人末端与目标的实际接触力f与步骤3设定的期望接触力f_d比较，如果f>f_dmax，则时间计数t增加Δt，Δt为机器人控制指令的执行周期；如果f<f_dmin，则时间计数t减小Δt；如果f_dmin≤f≤f_dmax，则时间计数t不变，即

5)将步骤4)得到的时间计数t，代入到步骤2)得到的关节运动角度函数，计算得出期望关节角度θ；

6)将步骤5)得到期望关节角度θ作为控制指令发送给机器人的关节位置控制器，关节位置控制器根据控制指令控制机器人各关节运动。

2.根据权利要求1所述的一种基于关节位置控制器的机器人末端力控制方法，其特征在于：步骤1)中机器人的逆运动学方程为θ＝k^-1(p)，式中k^-1( )为机器人逆运动学函数。

3.根据权利要求1所述的一种基于关节位置控制器的机器人末端力控制方法，其特征在于：通过多项式插值计算得出连续的关节运动角度函数的具体方法为：

θ(t)＝a₀+a₁t+a₂t²+…+a_ntⁿ；

式中，n为多项式次数，a₀,a₁,a₂,…,a_n为待确定的多项式系数，其中函数输入为时间计数t，函数输出为关节角度θ(t)。