[发明专利]一种串联机器人逆动力学快速计算方法

权利要求书

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，进而得到基本动力学参数β。