CN107363832B - 一种工业机器人前馈力矩实时计算方法 - Google Patents

一种工业机器人前馈力矩实时计算方法 Download PDF

Info

Publication number
CN107363832B
CN107363832B CN201710454107.7A CN201710454107A CN107363832B CN 107363832 B CN107363832 B CN 107363832B CN 201710454107 A CN201710454107 A CN 201710454107A CN 107363832 B CN107363832 B CN 107363832B
Authority
CN
China
Prior art keywords
point
joint
interpolation
time
real
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Expired - Fee Related
Application number
CN201710454107.7A
Other languages
English (en)
Other versions
CN107363832A (zh
Inventor
张铁
龚文涛
邹焱飚
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
South China University of Technology SCUT
Zhongshan Institute of Modern Industrial Technology of South China University of Technology
Original Assignee
South China University of Technology SCUT
Zhongshan Institute of Modern Industrial Technology of South China University of Technology
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by South China University of Technology SCUT, Zhongshan Institute of Modern Industrial Technology of South China University of Technology filed Critical South China University of Technology SCUT
Priority to CN201710454107.7A priority Critical patent/CN107363832B/zh
Publication of CN107363832A publication Critical patent/CN107363832A/zh
Application granted granted Critical
Publication of CN107363832B publication Critical patent/CN107363832B/zh
Expired - Fee Related legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/10—Programme-controlled manipulators characterised by positioning means for manipulator elements
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Manipulator (AREA)
  • Numerical Control (AREA)

Abstract

本发明公开了一种工业机器人前馈力矩实时计算方法，包括步骤：(1)建立机器人各关节转角q*的实时插补点队列Q；(2)在实时插补点队列Q中每次取五个点建立动态的插补缓存队列Qc；(3)然后根据五次中心差分法，在已知五个点的关节转角q*的前提下，实时计算插补缓存队列Qc的中间点的关节角速度
Figure DDA0001323276850000011
和关节角加速度
Figure DDA0001323276850000012
(4)计算实时插补点队列Q中其他点的关节角速度
Figure DDA0001323276850000013
和关节角加速度
Figure DDA0001323276850000014
(5)将计算所得的各插补点的关节转角q*、关节角速度
Figure DDA0001323276850000015
和关节角加速度
Figure DDA0001323276850000016
代入机器人的逆动力学模型，即可在插补时实时计算各关节的前馈驱动力矩。本发明能够实时而快速的计算机器人的前馈补偿力矩，保证机器人各关节伺服电机有足够大的力和力矩来驱动机器人的连杆和关节，保证精度。

Description

一种工业机器人前馈力矩实时计算方法
技术领域
本发明涉及工业机器人的动力学控制应用领域，尤其涉及一种工业机器人前馈力矩实时计算方法。
背景技术
因为机器人的复杂非线性、时变不确定性、强耦合性(特别是在高速运动时)，要使机器人能以期望的速度和加速度运动，机器人各关节伺服电机必须有足够大的力和力矩来驱动机器人的连杆和关节，否则，连杆将因运动迟缓而影响机器人的定位和轨迹跟踪精度，为此必须建立基于动力学模型的前馈力矩控制。而实时快速地计算前馈补偿力矩，则是本发明的主要内容。
发明内容
本发明的目的在于克服现有技术的不足，提供一种计算简单方便，满足工业机器人实时插补要求的前馈力矩计算方法。
上述目的是通过以下技术方案实现的：
一种工业机器人前馈力矩实时计算方法，包括步骤：
(1)建立机器人各关节转角q*的实时插补点队列Q；
(2)在实时插补点队列Q中每次取五个点建立动态的插补缓存队列Qc；
(3)然后根据五次中心差分法，在已知五个点的关节转角q*的前提下，实时计算插补缓存队列Qc的中间点的关节角速度
Figure BDA0001323276830000011
和关节角加速度
Figure BDA0001323276830000012
(4)计算实时插补点队列Q中第一个点、第二个点、倒数第二个点、倒数第一个点的关节角速度
Figure BDA0001323276830000013
和关节角加速度
Figure BDA0001323276830000014
(5)将计算所得的各插补点的关节转角q*、关节角速度
Figure BDA0001323276830000015
和关节角加速度
Figure BDA0001323276830000016
代入机器人的逆动力学模型，即可在插补时实时计算各关节的前馈驱动力矩，实现前馈力矩控制。
进一步地，所述步骤(1)具体包括：
在工业机器人的实时插补中，实时地每次计算出一个插补周期的各关节转角q*，放入实时插补点队列Q中。
进一步地，所述步骤(2)具体包括：
(21)从第一个点开始，从实时插补点队列Q中取五个点，放入插补缓存队列Qc，使用五次中心差分法计算第三个点的关节角速度和关节角加速度；
(22)计算完之后，去掉插补缓存队列Qc中位于队尾的第一个点，再从队列Q中取一个点加入插补缓存队列Qc的队头，组成新的插补缓存队列Qc；
(23)然后计算新的插补缓存队列Qc第三个点的关节角速度和关节角加速度；
(24)重复上述步骤，直至将队列Q最后一个点加入插补缓存队列Qc，计算完成。
进一步地，所述步骤(3)具体包括：
(31)设定五个插补点的时间间隔为机器人的理论插补周期T，设第三个点的关节转角时间序列为q(x)，则其他几个点的时间序列分别为q(x-2T)、q(x-T)、q(x+T)、q(x+2T)；
(32)分别对q(x-2T)、q(x-T)、q(x+T)、q(x+2T)进行泰勒四阶展开，并相减可以得到：
Figure BDA0001323276830000021
Figure BDA0001323276830000022
式中，c1和c2是常数；
(33)对上两式进行数学转化可以得到：
Figure BDA0001323276830000031
(34)找到一个常数c∈[x-2T,x+2T]，使得下式成立：
16q(5)(c1)-64q(5)(c2)＝-48q(5)(c)；
整理可得：
Figure BDA0001323276830000032
(35)具有o(T4)精度的一阶中心差分算法计算q'(x)：
Figure BDA0001323276830000033
同理，用二阶中心差分算法计算q”(x)：
Figure BDA0001323276830000034
进一步地，所述步骤(4)具体包括：
(41)对于实时插补点队列Q中的第二个点和倒数第二个点，通过其前后两点，根据三点中心差分原理求解，即：
Figure BDA0001323276830000035
Figure BDA0001323276830000036
Figure BDA0001323276830000037
Figure BDA0001323276830000038
(42)对于实时插补点队列Q中的第一个点和倒数第一个点，其理论关节角速度
Figure BDA0001323276830000039
理论关节角加速度
Figure BDA00013232768300000310
不能通过中心差分得到，因此令第一个点和倒数第一个点的关节角速度和关节角加速度为零，即相当于这两个插补点只补偿前馈力矩的重力项。
因为插补周期很短,同时机器人启动和停止阶段第一个周期内速度和加速度较小，所以不会有太大的计算误差。
进一步地，所述步骤(5)具体包括：
将计算所得的各插补点的关节转角q*、关节角速度
Figure BDA0001323276830000041
和关节角加速度
Figure BDA0001323276830000042
代入机器人的逆动力学模型，即可在插补时根据公式：
Figure BDA0001323276830000043
实时计算各关节的前馈驱动力矩，实现前馈力矩控制，其中：Hb为机器人逆动力学模型中的回归矩阵，β为机器人的基础动力学参数。
相比现有技术，本发明能够实时而快速的计算机器人的前馈补偿力矩，保证机器人各关节伺服电机有足够大的力和力矩来驱动机器人的连杆和关节，防止连杆因运动迟缓而影响机器人的定位和轨迹跟踪精度。
附图说明
图1是本发明实施例的前馈力矩控制示意图。
图2是本发明实施例的建立插补动态缓存队列的示意图。
具体实施方式
下面结合附图和实施例对本发明作进一步的说明。
一种工业机器人前馈力矩实时计算方法，包括步骤：
(1)建立机器人各关节转角q*的实时插补点队列Q；
(2)在实时插补点队列Q中每次取五个点建立动态的插补缓存队列Qc；
(3)然后根据五次中心差分法，在已知五个点的关节转角q*的前提下，实时计算插补缓存队列Qc的中间点的关节角速度
Figure BDA0001323276830000044
和关节角加速度
Figure BDA0001323276830000045
(4)计算实时插补点队列Q中第一个点、第二个点、倒数第二个点、倒数第一个点的关节角速度
Figure BDA0001323276830000046
和关节角加速度
Figure BDA0001323276830000047
(5)将计算所得的各插补点的关节转角q*、关节角速度
Figure BDA0001323276830000048
和关节角加速度
Figure BDA0001323276830000049
代入机器人的逆动力学模型，即可在插补时实时计算各关节的前馈驱动力矩，实现前馈力矩控制(见图1)。
具体而言，所述步骤(1)具体包括：
在工业机器人的实时插补中，实时地每次计算出一个插补周期的各关节转角q*，放入实时插补点队列Q中，从而根据q*实时计算
Figure BDA0001323276830000051
和
Figure BDA0001323276830000052
工业机器人动力学控制中常用的前馈力矩补偿控制中，前馈力矩的计算表达式为：
Figure BDA0001323276830000053
要想实现实时前馈力矩控制，必须已知各关节的关节转角q*、角速度
Figure BDA0001323276830000054
和角加速度
Figure BDA0001323276830000055
具体而言，如图2所示，所述步骤(2)具体包括：
(21)从第一个点开始，从实时插补点队列Q中取五个点，放入插补缓存队列Qc，使用五次中心差分法计算第三个点的关节角速度和关节角加速度；
(22)计算完之后，去掉插补缓存队列Qc中位于队尾的第一个点，再从队列Q中取一个点加入插补缓存队列Qc的队头，组成新的插补缓存队列Qc；
(23)然后计算新的插补缓存队列Qc第三个点的关节角速度和关节角加速度；
(24)重复上述步骤，直至将队列Q最后一个点加入插补缓存队列Qc，计算完成。
具体而言，所述步骤(3)具体包括：
(31)设定五个插补点的时间间隔为机器人的理论插补周期T，设第三个点的关节转角时间序列为q(x)，则其他几个点的时间序列分别为q(x-2T)、q(x-T)、q(x+T)、q(x+2T)；
(32)分别对q(x-2T)、q(x-T)、q(x+T)、q(x+2T)进行泰勒四阶展开，并相减可以得到：
Figure BDA0001323276830000056
Figure BDA0001323276830000061
式中，c1和c2是常数；
(33)对上两式进行数学转化可以得到：
Figure BDA0001323276830000062
(34)找到一个常数c∈[x-2T,x+2T]，使得下式成立：
16q(5)(c1)-64q(5)(c2)＝-48q(5)(c)；
整理可得：
Figure BDA0001323276830000063
(35)具有o(T4)精度的一阶中心差分算法计算q'(x)：
Figure BDA0001323276830000064
同理，用二阶中心差分算法计算q”(x)：
Figure BDA0001323276830000065
进一步地，所述步骤(4)具体包括：
(41)对于实时插补点队列Q中的第二个点和倒数第二个点，通过其前后两点，根据三点中心差分原理求解，即：
Figure BDA0001323276830000066
Figure BDA0001323276830000067
Figure BDA0001323276830000068
Figure BDA0001323276830000069
(42)对于实时插补点队列Q中的第一个点和倒数第一个点，其理论关节角速度
Figure BDA0001323276830000071
理论关节角加速度
Figure BDA0001323276830000072
不能通过中心差分得到，因此令第一个点和倒数第一个点的关节角速度和关节角加速度为零，即相当于这两个插补点只补偿前馈力矩的重力项。因为插补周期很短,同时机器人启动和停止阶段第一个周期内速度和加速度较小，所以不会有太大的计算误差。
具体而言，所述步骤(5)具体包括：
将实时计算所得的各插补点的关节转角q*、关节角速度
Figure BDA0001323276830000073
和关节角加速度
Figure BDA0001323276830000074
代入机器人的逆动力学模型，即可在插补时根据公式：
Figure BDA0001323276830000075
实时计算各关节的前馈驱动力矩，实现前馈力矩控制，其中：Hb为机器人逆动力学模型中的回归矩阵，β为机器人的基础动力学参数。
本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。

Claims (5)

1.一种工业机器人前馈力矩实时计算方法，其特征在于，包括步骤：
(1)建立机器人各关节转角q*的实时插补点队列Q；
(2)在实时插补点队列Q中每次取五个点建立动态的插补缓存队列Qc；
(3)然后根据五次中心差分法，在已知五个点的关节转角q*的前提下，实时计算插补缓存队列Qc的中间点的关节角速度
Figure FDA0002224743150000011
和关节角加速度
Figure FDA0002224743150000012
(4)计算实时插补点队列Q中第一个点、第二个点、倒数第二个点、倒数第一个点的关节角速度
Figure FDA0002224743150000013
和关节角加速度
Figure FDA0002224743150000014
(5)将计算所得的各插补点的关节转角q*、关节角速度
Figure FDA0002224743150000015
和关节角加速度
Figure FDA0002224743150000016
代入机器人的逆动力学模型，即可在插补时实时计算各关节的前馈驱动力矩，实现前馈力矩控制；
所述步骤(3)具体包括：
(31)设定五个插补点的时间间隔为机器人的理论插补周期T，设第三个点的关节转角时间序列为q(x)，则其他几个点的时间序列分别为q(x-2T)、q(x-T)、q(x+T)、q(x+2T)；
(32)分别对q(x-2T)、q(x-T)、q(x+T)、q(x+2T)进行泰勒四阶展开，并相减可以得到：
Figure FDA0002224743150000017
Figure FDA0002224743150000018
式中，c1和c2是常数；
(33)对上两式进行数学转化可以得到：
Figure FDA0002224743150000021
(34)找到一个常数c∈[x-2T,x+2T]，使得下式成立：
16q(5)(c1)-64q(5)(c2)＝-48q(5)(c)；
整理可得：
Figure FDA0002224743150000022
(35)具有o(T4)精度的一阶中心差分算法计算q'(x)：
Figure FDA0002224743150000023
同理，用二阶中心差分算法计算q”(x)：
Figure FDA0002224743150000024
2.根据权利要求1所述的一种工业机器人前馈力矩实时计算方法，其特征在于，所述步骤(1)具体包括：
在工业机器人的实时插补中，实时地每次计算出一个插补周期的各关节转角q*，放入实时插补点队列Q中。
3.根据权利要求1所述的一种工业机器人前馈力矩实时计算方法，其特征在于，所述步骤(2)具体包括：
(21)从第一个点开始，从实时插补点队列Q中取五个点，放入插补缓存队列Qc，使用五次中心差分法计算第三个点的关节角速度和关节角加速度；
(22)计算完之后，去掉插补缓存队列Qc中位于队尾的第一个点，再从队列Q中取一个点加入插补缓存队列Qc的队头，组成新的插补缓存队列Qc；
(23)然后计算新的插补缓存队列Qc第三个点的关节角速度和关节角加速度；
(24)重复上述步骤，直至将队列Q最后一个点加入插补缓存队列Qc，计算完成。
4.根据权利要求1所述的一种工业机器人前馈力矩实时计算方法，其特征在于，所述步骤(4)具体包括：
(41)对于实时插补点队列Q中的第二个点和倒数第二个点，通过其前后两点，根据三点中心差分原理求解，即：
Figure FDA0002224743150000031
Figure FDA0002224743150000032
Figure FDA0002224743150000033
Figure FDA0002224743150000034
(42)对于实时插补点队列Q中的第一个点和倒数第一个点，其理论关节角速度
Figure FDA0002224743150000035
理论关节角加速度
Figure FDA0002224743150000036
不能通过中心差分得到，因此令第一个点和倒数第一个点的关节角速度和关节角加速度为零，即相当于这两个插补点只补偿前馈力矩的重力项。
5.根据权利要求1所述的一种工业机器人前馈力矩实时计算方法，其特征在于，所述步骤(5)具体包括：
将计算所得的各插补点的关节转角q*、关节角速度
Figure FDA0002224743150000037
和关节角加速度
Figure FDA0002224743150000038
代入机器人的逆动力学模型，即可在插补时根据公式：
Figure FDA0002224743150000041
实时计算各关节的前馈驱动力矩，实现前馈力矩控制，其中：Hb为机器人逆动力学模型中的回归矩阵，β为机器人的基础动力学参数。
CN201710454107.7A 2017-06-15 2017-06-15 一种工业机器人前馈力矩实时计算方法 Expired - Fee Related CN107363832B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201710454107.7A CN107363832B (zh) 2017-06-15 2017-06-15 一种工业机器人前馈力矩实时计算方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201710454107.7A CN107363832B (zh) 2017-06-15 2017-06-15 一种工业机器人前馈力矩实时计算方法

Publications (2)

Publication Number Publication Date
CN107363832A CN107363832A (zh) 2017-11-21
CN107363832B true CN107363832B (zh) 2020-04-28

Family

ID=60305599

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201710454107.7A Expired - Fee Related CN107363832B (zh) 2017-06-15 2017-06-15 一种工业机器人前馈力矩实时计算方法

Country Status (1)

Country Link
CN (1) CN107363832B (zh)

Families Citing this family (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109760048B (zh) * 2018-12-28 2021-04-23 深圳市越疆科技有限公司 一种机器人的伺服控制方法、装置及机器人
CN110394802B (zh) * 2019-08-06 2021-03-30 前元运立(北京)机器人智能科技有限公司 一种打磨机器人及位置补偿方法
CN112338909A (zh) * 2020-09-18 2021-02-09 珠海格力电器股份有限公司 一种机器人力矩前馈控制方法、装置、存储介质及机器人
CN112462694B (zh) * 2020-12-03 2022-02-08 上海维宏电子科技股份有限公司 基于总线伺服控制系统实现速度前馈补偿处理的方法、系统、装置、处理器及存储介质
CN113910244B (zh) * 2021-11-15 2022-12-20 武汉联影智融医疗科技有限公司 用于神经外科的基于力矩前馈的机械臂拖动混合控制方法

Family Cites Families (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US7212886B2 (en) * 2002-12-12 2007-05-01 Kabushiki Kaisha Yaskawa Denki Robot control apparatus and method
CN201559195U (zh) * 2009-12-11 2010-08-25 Abb技术有限公司 机器人
JP4850956B2 (ja) * 2010-02-19 2012-01-11 ファナック株式会社 学習制御機能を備えたロボット
JP6097174B2 (ja) * 2013-08-05 2017-03-15 株式会社東芝 ロボット制御装置
JP6209176B2 (ja) * 2014-04-14 2017-10-04 オークマ株式会社 位置制御装置
CN106313044B (zh) * 2016-09-20 2018-09-14 华南理工大学 一种工业机器人前馈力矩补偿方法

Also Published As

Publication number Publication date
CN107363832A (zh) 2017-11-21

Similar Documents

Publication Publication Date Title
CN107363832B (zh) 一种工业机器人前馈力矩实时计算方法
CN110421547B (zh) 一种基于估计动力学模型的双臂机器人协同阻抗控制方法
CN109159151B (zh) 一种机械臂空间轨迹跟踪动态补偿方法和系统
CN106647282B (zh) 一种考虑末端运动误差的六自由度机器人轨迹规划方法
CN107901041A (zh) 一种基于图像混合矩的机器人视觉伺服控制方法
CN106313044A (zh) 一种工业机器人前馈力矩补偿方法
CN111347423A (zh) 工业机器人传送带动态跟踪涂胶方法
CN113601512B (zh) 一种机械臂奇异点的通用规避方法与系统
CN105739537B (zh) 一种小天体表面附着运动主动控制方法
CN110948504B (zh) 机器人加工作业法向恒力跟踪方法和装置
CN109606753A (zh) 一种空间双臂机器人协同抓捕目标的控制方法
CN111805547B (zh) 一种轨迹动态跟踪实现方法
CN109857100B (zh) 一种基于反演法和快速终端滑模的复合轨迹跟踪控制算法
CN108227506A (zh) 一种基于自适应最优化方法的机器人导纳控制系统
CN110861090B (zh) 一种力矩前馈控制系统及方法
CN114536334B (zh) 一种柔性机械臂系统高阶滑模抗干扰控制方法
CN109760047B (zh) 一种基于视觉传感器的舞台机器人预测控制方法
CN111098299B (zh) 一种空间机器人扰动补偿的方法
CN110394802B (zh) 一种打磨机器人及位置补偿方法
CN108724191A (zh) 一种机器人运动轨迹控制方法
CN110936374A (zh) 一种柔性双关节机械臂命令滤波反步控制方法
WO2023116129A1 (zh) 一种协作机器人的柔顺力控制方法及系统
CN107085432B (zh) 一种移动机器人的目标轨迹跟踪方法
CN114378827A (zh) 一种基于移动机械臂整体控制的动态目标跟踪抓取方法
CN110026986B (zh) 多臂协同作业柔顺控制方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant
CF01 Termination of patent right due to non-payment of annual fee
CF01 Termination of patent right due to non-payment of annual fee

Granted publication date: 20200428