一种基于关节位置控制器的机器人末端力控制方法
【技术领域】
[0001] 本发明涉及一种基于位置控制的力控制方法，尤其涉及一种基于关节位置控制器 的机器人末端力控制方法。
【背景技术】
[0002] 机器人的多数操作任务是通过各关节的位置控制使其末端与目标接触完成的，如 果接触力过大，会导致机器人本体或目标损坏，因此在机器人作业过程中，如何保持末端与 目标接触并控制接触力是机器人能否顺利完成操作任务的关键问题之一。
[0003] 为了使机器人末端与目标接触并保持一定的接触力，以往的方法可以分为三类。 一类方法是根据目标的几何形状设计具有一定柔性的机器人末端专用装置;另一类方法是 利用具有力控制功能的机器人关节控制器，控制机器人末端与目标之间的接触力;还有一 类方法是直接测量机器人末端与目标之间的接触力，通过实时的机器人逆运动学或者逆动 力学解算，对机器人末端的位置或速度进行修正。
[0004] 现有的机器人末端力控制方法中，为机器人末端加装专用柔性装置由于要求装置 的柔性必须与目标匹配，所以该方法比较局限，而且研制成本较高;具有力控制功能的机器 人关节控制器技术尚不成熟，系统实现困难;基于机器人逆运动学或者动力学实时解算的 力控制方法严重依赖于算法的可靠性，并且运算量较大。
【发明内容】
[0005] 本发明解决的技术问题:克服现有机器人力控制技术不能直接应用于一般机器人 的问题，提供一种利用机器人自身的关节位置控制器实现机器人末端力控制方法。
[0006] 本发明的技术解决方案是：一种基于关节位置控制器的机器人末端力控制方法， 包括以下步骤：
[0007] 1)将期望的机器人末端运动的起点位置矢量p。和终点位置矢量pt，代入机器人的 逆运动学方程，分别计算得出机器人每个关节运动的起始角度矢量Θ。和终止角度矢量0 t;
[0008] 2)根据步骤1)得到的每个关节运动的起始角度矢量Θ。和终止角度矢量0*，通过多 项式插值计算得出连续的关节运动角度函数，函数输入为时间计数t，函数输出为关节角度 0(t)；
[0009] 3)设定期望接触力fd，其中期望接触力fd包括最大期望接触力fdmax和最小期望接 触力fdmin;
[0010] 4)接触力控制环节将机器人末端与目标的实际接触力f与步骤3设定的期望接触 力fd比较，如果f>f dmax，则时间计数t增加 At，At为机器人控制指令的执行周期；如果f〈 fdmin，则时间计数t减小Δ t;如果fdmin < f < fdmax，则时间计数t不变，BP
[0011]
[0012] 5)将步骤4)得到的时间计数t，代入到步骤2)得到的关节运动角度函数，计算得出 期望关节角度Θ;
[0013] 6)将步骤5)得到期望关节角度Θ作为控制指令发送给机器人的关节位置控制器， 关节位置控制器根据控制指令控制机器人各关节运动。
[0014] 步骤1)中机器人的逆运动学方程为0=1^(?)，式中为机器人逆运动学函数。
[0015] 通过多项式插值计算得出连续的关节运动角度函数的具体方法为：
[0016] 0(t) =ao+ait+a2t2+-,-+antn；
[0017] 式中，η为多项式次数，ao, ai, a2，…，an为待确定的多项式系数，其中函数输入为时 间计数t，函数输出为关节角度Θ (t)。
[0018] 本发明与现有技术相比具有以下优点：
[0019] (1)本发明从软件上实现了对机器人末端的力控制，并实现了根据设定的期望接 触力调整接触力大小，设定的时间增量参数调整机器人末端等效刚度，而不需要对硬件进 行任何改造或者升级，降低了系统成本。
[0020] (2)本发明充分利用了机器人集成的关节位置控制器，由于关节位置控制技术比 较成熟，所以基于关节位置控制器的系统响应速度快，鲁棒性好。
[0021] (3)本发明整个控制过程中，只需要根据期望的机器人末端运动路径的起点和终 点位置，预先进行两次逆运动学解算，避免了高强度的机器人逆运动学或者逆动力学实时 解算，系统易于实现。
【附图说明】
[0022]图1为本发明实施例不意图；
[0023]图2为本发明控制原理图；
[0024]图3为本发明控制流程图。
【具体实施方式】
[0025] 下面结合附图和具体实施例对本发明作进一步详细地描述：
[0026] 如图2、图3所示，本发明由逆运动学环节、多项式插值环节和接触力控制环节组 成。逆运动学环节根据机器人末端运动的起点和终点位置，及机器人逆运动学方程，计算与 机器人末端位置对应的机器人各关节起始和终止角度。多项式插值环节根据得到的机器人 各关节起始和终止角度，及多项式插值方程，计算机器人各关节运动的连续角度函数。接触 力控制环节根据得到的各关节运动的连续角度函数，及机器人末端所受的接触力，计算机 器人关节位置控制器控制信号。
[0027]实施例
[0028] 本实施例以平面机器人为例，如图1所示，其臂杆1长度为h，臂杆2长度为12,关节1 角度为91，关节2角度为02，末端位置从P。= (X。，y。)运动到pt = (Xt，yt)，运动时间为T，且运动 过程中与人为设置的目标接触，那么平面机器人运动学方程可以表示为
[0029]
[0030] (1)逆运动学环节:根据上式可得，平面机器人逆运动学方程为
[0031]
[0032]
[0033] 对于期望的机器人末端运动的起点位置矢量(x。，y。)和终点位置矢量(xt，yt)，分 别代入上式，得到各关节运动的起始角度矢量θ〇θ。和终止角度矢量0*为
[0034]
[0035] L 叫 」
[0036] (2)多项式插值环节:本实施例采用三次多项式插值，其表达式为
[0037] 0(t) =ao+ait+a2t2+a3t3
[0038] 根据约束条件，可以得到
[0039]
[0040]求解上式，确定三次多项式系数为
[0041] ao= θ〇
[0042] ai = 0
[0043]
[0044]
[0045] 因此，关节运动角度函数为
[0046]
[0047] (3)设定控制参数:本实施例设定At为机器人控制指令的执行周期，即时间增量 At = 0.001s，最大期望接触力f<w=5.1N，最小期望接触力f<hin = 4.9N。
[0048] (4)比较接触力大小:时间变量t从0开始计数，每隔0 · 001 s比较一次实际接触力和 期望接触力大小。当实际接触力大于最大期望接触力fdmax=5.1N，时间变量t增加 Δ t = 0 · 001S;当实际接触力小于最小期望接触力f dmin = 4 · 9N，时间变量t减小Δ t = 0 · 001S;当实 际接触力介于最大期望接触力fdmax = 5 · 1N与最小期望接触力fdmin = 4 · 9N之间时，时间变量t 保持不变。
[0049] (5)计算期望关节角度:将时间变量t代入到关节运动角度函数中，计算得出期望 关节角度Θ。
[0050] (6)控制机器人运动:将期望关节角度Θ作为控制指令发送给机器人的关节位置控 制器，关节位置控制器根据控制指令控制机器人各关节运动。
[0051] 本实施例的工作效果为:机器人从期望的起点位置向期望的终点位置运动，运动 误差小于0.1mm，在运动过程中机器人末端与物体接触，并施加一定的作用力，作用力大小 为 5·0Ν±0·5Ν。
[0052] 本发明所公开的机器人末端力控制方法创造性的利用关节位置控制器实现机器 人末端力控制，可以推广到其他具有位置控制器的执行机构实现力控制功能。
[0053]本发明未详细描述内容为本领域技术人员公知技术。
【主权项】
1. 一种基于关节位置控制器的机器人末端力控制方法，其特征在于:包括以下步骤： 1) 将期望的机器人末端运动的起点位置矢量P。和终点位置矢量Pt，代入机器人的逆运 动学方程，分别计算得出机器人每个关节运动的起始角度矢量Θ。和终止角度矢量9 t; 2) 根据步骤1)得到的每个关节运动的起始角度矢量Θ。和终止角度矢量0*，通过多项式 插值计算得出连续的关节运动角度函数，函数输入为时间计数t，函数输出为关节角度Θ (t)； 3) 设定期望接触力fd，其中期望接触力fd包括最大期望接触力fdmax和最小期望接触力 fdmin ； 4) 接触力控制环节将机器人末端与目标的实际接触力f与步骤3设定的期望接触力fd比 较，如果f>fdmax，则时间计数t增加 Δ t，△ t为机器人控制指令的执行周期;如果f〈fdmin，则时 间计数t减小Δ t;如果fdmin < f < fdmax，则时间计数t不变，即5) 将步骤4)得到的时间计数t，代入到步骤2)得到的关节运动角度函数，计算得出期望 关节角度Θ; 6) 将步骤5)得到期望关节角度Θ作为控制指令发送给机器人的关节位置控制器，关节 位置控制器根据控制指令控制机器人各关节运动。2. 根据权利要求1所述的一种基于关节位置控制器的机器人末端力控制方法，其特征 在于:步骤1)中机器人的逆运动学方程为9 = 1^(?)，式中1^()为机器人逆运动学函数。3. 根据权利要求1所述的一种基于关节位置控制器的机器人末端力控制方法，其特征 在于:通过多项式插值计算得出连续的关节运动角度函数的具体方法为： 9(t) =ao+ait+a2t2+-,-+antn； 式中，η为多项式次数，ao, ai, a2，…，an为待确定的多项式系数，其中函数输入为时间计 数t，函数输出为关节角度9(t)。
【专利摘要】本发明一种基于关节位置控制器的机器人末端力控制方法，将机器人末端与目标之间的接触力作为输入，基于机器人的关节位置控制器，通过对机器人各关节的角度控制实现对机器人末端的力控制，包括根据机器人末端运动的起点和终点位置，及机器人逆运动学方程，计算与机器人末端位置对应的机器人各关节起始和终止角度；根据得到的机器人各关节起始和终止角度，及多项式插值方程，计算机器人各关节运动的连续角度函数；根据得到的各关节运动的连续角度函数，及机器人末端所受的接触力，计算机器人关节位置控制器控制信号。该方法既保证了机器人末端运动的位置精度，又避免了机器人末端与外界的接触力过大，并可以通过控制参数设定接触力大小。
【IPC分类】G05D15/00
【公开号】CN105676898
【申请号】CN201610160345
【发明人】周扬, 魏春岭, 朱志斌, 王勇, 徐拴锋
【申请人】北京控制工程研究所
【公开日】2016年6月15日
【申请日】2016年3月21日