CN106951658A - 一种串联机器人逆动力学快速计算方法 - Google Patents

Abstract

本发明公开了一种串联机器人逆动力学快速计算方法，包括步骤：使用牛顿‑欧拉方程建立相对简洁的串联机器人逆动力学模型，解耦得到标准动力学参数和对应的回归矩阵，使用基于数值的简化方法对解耦后模型进行简化，得到基本动力学参数和对应的基本回归矩阵，借助符号运算工具对基本回归矩阵进行合并和简化，并提取反复出现的项作为中间项，优先计算以减少计算量。本发明采用牛顿‑欧拉方程、简化动力学参数和提取重复项三个步骤，大大减少串联机器人逆动力学运算量，解决串联机器人逆动力学运算过于复杂，难以满足实时计算要求的问题。

Description

一种串联机器人逆动力学快速计算方法

技术领域

本发明涉及串联机器人逆动力学计算方法，特别涉及一种串联机器人逆动力学快速计算方法。

背景技术

串联机器人在制造业、农业、娱乐业等行业得到越来越广泛的应用，许多应用场景需要串联机器人拥有高速、高精度的运动性能。基于串联机器人动力学模型的运动控制能赋予机器人高速、高精度的运动性能，这种方法需要不断实时进行串联机器人逆动力学计算。

目前串联机器人逆动力学模型多以拉格朗日方程法进行建模，这种方法得到的串联机器人逆动力学模型十分复杂，以6自由度的串联机器人为例，使用拉格朗日方程法得到的逆动力学模型，总共需要计算66271次乘法和51548次加法。而串联机器人控制器的伺服控制周期一般为1ms，显然如此复杂的模型无法在1ms之内完成计算。因此，需要更简洁的模型和更快速的计算方法才能实现串联机器人逆动力学实时计算。

发明内容

本发明的目的是提供一种串联机器人逆动力学快速计算方法，旨在解决串联机器人逆动力学实时计算问题。

本发明的目的通过下述技术方案来实现：

一种串联机器人逆动力学快速计算方法，包括如下步骤：

S1、使用牛顿-欧拉方程建立相对简洁的串联机器人逆动力学模型；

S2、解耦得到标准动力学参数和对应的回归矩阵；

S3、使用基于数值的简化方法对解耦后模型进行简化，得到基本动力学参数和对应的基本回归矩阵；

S4、借助符号运算工具对基本回归矩阵进行合并和简化，并提取反复出现的项作为中间项，优先计算以减少计算量。

进一步地，所述步骤S2包含如下步骤：

S2.1、使用迭代牛顿-欧拉动力学算法得到串联机器人的逆动力学表达式

式中，Γ为各关节力矩；分别为各关节的转角、角速度和角加速度；δ为各连杆的标准动力学参数，且δ＝[δ₁ δ₂ … δ_n]^T，δ_k为连杆k的标准动力学参数且δ_k＝[L_xxk L_xykL_xzk L_yyk L_yzk L_zzk l_xk l_yk l_zk m_k f_vk f_ck I_ak]^T；InvDYN(·)为机器人逆动力学模型函数；

为了便于进行串联机器人动力学参数辨识，需要将式(1)改写成如式(2)所示的线性形式

式中，被称作回归矩阵，是关于关节的转角、角速度和角加速度的函数；

S2.2、依次令矢量δ的第i个动力学参数等于1，其余动力学参数等于0，然后代入式(1)所示的InvDYN(·)函数中，则除去零项后，剩余项均为第i个动力学参数的相关项，将其赋值给回归矩阵H的相关列，最终得到完整的回归矩阵H。

进一步地，所述步骤S3包含如下步骤：

S3.1、可以将矩阵和机器人标准动力学参数δ分成两部分，则动力学方程可以写成如下的形式

式中，H_b为矩阵所有n_b个线性无关列组成的子矩阵；H_d为剩下的n_d个全零列向量和线性相关列组成的子矩阵；δ_b为基本动力学参数；δ_d为对动力学不起作用的动力学参数；

H_d可以用H_b线性表示，写作

H_d＝H_bK_d； (4)

S3.2、设置换矩阵P＝[P_b P_d]满足如下式

HP＝[H_b H_d]， (5)

则有

联立式(3)到(6)可以得到

H_b(δ_b+K_dδ_d)＝Γ， (7)

写成含基本动力学参数项的动力学方程，则为

式中，β为基本动力学参数，β＝Kδ，K＝P_b ^T+K_dP_d ^T；

S3.3、随机生成S组且满足S远远大于机器人标准动力学参数δ的个数；将S组代入回归矩阵函数可以得到回归矩阵H_S；对H_S使用QR分解可以得到正则上三角矩阵R

遍历矩阵R的对角元素，记录其中非零对角元素在矩阵中的列号，依次写入数组db；定义行列数等于机器人标准动力学参数δ个数的单位矩阵P'；依照数组db记录的列号，按顺序从单位矩阵P'中取出对应的列组成新矩阵，该矩阵为P_b，剩余的列组成新矩阵，该矩阵为P_d，则P＝[P_b P_d]。

对矩阵H_b,H_d分别使用QR分解，可得

则有

联立式(5)和式(10)，可以得到

K_d＝R_b ^-1R_d， (12)

综上所述，可以得到P_b,K_d,P_d，进而得到基本动力学参数β。

本发明相对于现有技术具有如下的优点及效果：

本发明采用牛顿-欧拉方程、简化动力学参数和提取重复项三个步骤，大大减少串联机器人逆动力学运算量。以6自由度串联机器人逆动力学模型为例，使用本发明的计算方法最终只需要计算551次乘法和312次加法，而市面常见的运动控制系统基本都能在1ms内完成1000次以内的乘法和加法。因此，本发明能解决串联机器人逆动力学运算过于复杂，难以满足实时计算要求的问题。

附图说明

图1是本发明实施例的串联机器人逆动力学快速计算方法流程示意图。

具体实施方式

下面结合实施例及附图对本发明作进一步的详细描述，但本发明的实施方式不限于此。

如图1所示，现以某六自由度垂直关节串联机器人为对象，按照以下步骤进行串联机器人逆动力学快速计算：

一种串联机器人逆动力学快速计算方法，包括如下步骤：

S1、使用牛顿-欧拉方程建立相对简洁的串联机器人逆动力学模型；

S2、解耦得到标准动力学参数和对应的回归矩阵；

S3、使用基于数值的简化方法对解耦后模型进行简化，得到基本动力学参数和对应的基本回归矩阵；

S4、借助符号运算工具对基本回归矩阵进行合并和简化，并提取反复出现的项作为中间项，优先计算以减少计算量。

具体而言，所述步骤S2包含如下步骤：

S2.1、使用迭代牛顿-欧拉动力学算法得到串联机器人的逆动力学表达式

式中，Γ为各关节力矩；分别为各关节的转角、角速度和角加速度；δ为各连杆的标准动力学参数，且δ＝[δ₁ δ₂ … δ_n]^T，δ_k为连杆k的标准动力学参数且δ_k＝[L_xxk L_xykL_xzk L_yyk L_yzk L_zzk l_xk l_yk l_zk m_k f_vk f_ck I_ak]^T；InvDYN(·)为机器人逆动力学模型函数；

为了便于进行串联机器人动力学参数辨识，需要将式(1)改写成如式(2)所示的线性形式

式中，被称作回归矩阵，是关于关节的转角、角速度和角加速度的函数；

S2.2、依次令矢量δ的第i个动力学参数等于1，其余动力学参数等于0，然后代入式(1)所示的InvDYN(·)函数中，则除去零项后，剩余项均为第i个动力学参数的相关项，将其赋值给回归矩阵H的相关列，最终得到完整的回归矩阵H。

具体而言，所述步骤S3包含如下步骤：

S3.1、可以将矩阵和机器人标准动力学参数δ分成两部分，则动力学方程可以写成如下的形式

式中，H_b为矩阵所有n_b个线性无关列组成的子矩阵；H_d为剩下的n_d个全零列向量和线性相关列组成的子矩阵；δ_b为基本动力学参数；δ_d为对动力学不起作用的动力学参数；

H_d可以用H_b线性表示，写作

H_d＝H_bK_d； (4)

S3.2、设置换矩阵P＝[P_b P_d]满足如下式

HP＝[H_b H_d]， (5)

则有

联立式(3)到(6)可以得到

H_b(δ_b+K_dδ_d)＝Γ， (7)

写成含基本动力学参数项的动力学方程，则为

式中，β为基本动力学参数，β＝Kδ，K＝P_b ^T+K_dP_d ^T；

S3.3、随机生成S组且满足S远远大于机器人标准动力学参数δ的个数；将S组代入回归矩阵函数可以得到回归矩阵H_S；对H_S使用QR分解可以得到正则上三角矩阵R

遍历矩阵R的对角元素，记录其中非零对角元素在矩阵中的列号，依次写入数组db；定义行列数等于机器人标准动力学参数δ个数的单位矩阵P'；依照数组db记录的列号，按顺序从单位矩阵P'中取出对应的列组成新矩阵，该矩阵为P_b，剩余的列组成新矩阵，该矩阵为P_d，则P＝[P_b P_d]。

对矩阵H_b,H_d分别使用QR分解，可得

则有

联立式(5)和式(10)，可以得到

K_d＝R_b ^-1R_d， (12)

综上所述，可以得到P_b,K_d,P_d，进而得到基本动力学参数β。

上述实施例为本方面较佳的实施方式，但本方明的实施方式并不受上述实施例的限制，其他的任何背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。

Claims (3)

1.一种串联机器人逆动力学快速计算方法，其特征在于，包括如下步骤：

S1、使用牛顿-欧拉方程建立相对简洁的串联机器人逆动力学模型；

S2、解耦得到标准动力学参数和对应的回归矩阵；

S3、使用基于数值的简化方法对解耦后模型进行简化，得到基本动力学参数和对应的基本回归矩阵；

S4、借助符号运算工具对基本回归矩阵进行合并和简化，并提取反复出现的项作为中间项，优先计算以减少计算量。

2.根据权利要求1所述的一种串联机器人逆动力学快速计算方法，其特征在于，所述步骤S2包含如下步骤：

S2.1、使用迭代牛顿-欧拉动力学算法得到串联机器人的逆动力学表达式

$\mathrm{\Γ}=InvDYN(q,\stackrel{\·}{q},\stackrel{\·\·}{q},\mathrm{\δ}),---\left(1\right)$

式中，Γ为各关节力矩；q,分别为各关节的转角、角速度和角加速度；δ为各连杆的标准动力学参数，且δ＝[δ₁ δ₂ … δ_n]^T，δ_k为连杆k的标准动力学参数且δ_k＝[L_xxk L_xyk L_xzkL_yyk L_yzk L_zzk l_xk l_yk l_zk m_k f_vk f_ck I_ak]^T；InvDYN(·)为机器人逆动力学模型函数；

为了便于进行串联机器人动力学参数辨识，需要将式(1)改写成如式(2)所示的线性形式

$\mathrm{\Γ}=H(q,\stackrel{\·}{q},\stackrel{\·\·}{q})\mathrm{\δ},---\left(2\right)$

式中，被称作回归矩阵，是关于关节的转角、角速度和角加速度q,的函数；

S2.2、依次令矢量δ的第i个动力学参数等于1，其余动力学参数等于0，然后代入式(1)所示的InvDYN(·)函数中，则除去零项后，剩余项均为第i个动力学参数的相关项，将其赋值给回归矩阵H的相关列，最终得到完整的回归矩阵H。

3.根据权利要求1所述的一种串联机器人逆动力学快速计算方法，其特征在于，所述步骤S3包含如下步骤：

S3.1、可以将矩阵和机器人标准动力学参数δ分成两部分，则动力学方程可以写成如下的形式

$\left[\begin{array}{cc}{H}_b& H_d\end{array}\right]\left[\begin{array}{c}{\mathrm{\δ}}_b\\ {\mathrm{\δ}}_d\end{array}\right]=\mathrm{\Γ},---\left(3\right)$

式中，H_b为矩阵所有n_b个线性无关列组成的子矩阵；H_d为剩下的n_d个全零列向量和线性相关列组成的子矩阵；δ_b为基本动力学参数；δ_d为对动力学不起作用的动力学参数；

H_d可以用H_b线性表示，写作

H_d＝H_bK_d； (4)

S3.2、设置换矩阵P＝[P_b P_d]满足如下式

HP＝[H_b H_d]， (5)

则有

$\begin{array}{c}{H}_b={\mathrm{HP}}_b\\ H_d={\mathrm{HP}}_d\\ {\mathrm{\δ}}_b={P_b}^T\mathrm{\δ}\\ {\mathrm{\δ}}_d={P_d}^T\mathrm{\δ}\end{array},---\left(6\right)$

联立式(3)到(6)可以得到

H_b(δ_b+K_dδ_d)＝Γ， (7)

写成含基本动力学参数项的动力学方程，则为

$H_b(q,\stackrel{\·}{q},\stackrel{\·\·}{q})\mathrm{\β}=\mathrm{\Γ},---\left(8\right)$

式中，β为基本动力学参数，β＝Kδ，

S3.3、随机生成S组且满足S远远大于机器人标准动力学参数δ的个数；将S组代入回归矩阵函数可以得到回归矩阵H_S；对H_S使用QR分解可以得到正则上三角矩阵R

$H_S=Q\left[\begin{array}{c}R\\ 0\end{array}\right],---\left(9\right)$

遍历矩阵R的对角元素，记录其中非零对角元素在矩阵中的列号，依次写入数组db；定义行列数等于机器人标准动力学参数δ个数的单位矩阵P'；依照数组db记录的列号，按顺序从单位矩阵P'中取出对应的列组成新矩阵，该矩阵为P_b，剩余的列组成新矩阵，该矩阵为P_d，则P＝[P_b P_d]。

对矩阵H_b,H_d分别使用QR分解，可得

$HP=\left[\begin{array}{cc}{H}_b& H_d\end{array}\right]=\left[\begin{array}{cc}{Q}_b& Q_d\end{array}\right]\left[\begin{array}{cc}{R}_b& R_d\\ 0& 0\end{array}\right]=\left[\begin{array}{cc}{Q}_b{R}_d& Q_b{R}_d\end{array}\right],---\left(10\right)$

则有

$\begin{array}{c}{H}_b=Q_b{P}_b\\ H_d=Q_b{P}_d\end{array},---\left(11\right)$

联立式(5)和式(10)，可以得到

K_d＝R_b ^-1R_d， (12)

综上所述，可以得到P_b,K_d,P_d，进而得到基本动力学参数β。