一种快速收敛的机器人动力学参数辨识轨迹优化方法与流程
本发明涉及机器人动力学参数辨识激励轨迹的优化方法，特别涉及一种快速收敛的有限傅里叶级数机器人动力学参数辨识激励轨迹非线性优化方法。
背景技术：
随着劳动力成本的逐年上升，各行各业对于机器人替代人进行工作的需求也逐年上升。许多应用场景对机器人的运动性能有着较高的要求，这就需要对机器人进行基于动力学模型的运动控制。精确的机器人动力学参数是基于动力学模型运动控制的前提。
一般情况下，机器人的动力学参数不能直接获得，需要通过实施辨识实验获取。辨识实验的主要过程为，使用优化方法获得一条最优激励轨迹，机器人沿着激励轨迹进行运动，采样机器人各关节的角度和力矩，采样数据经过一定处理之后，代入机器人逆动力学模型，得到一个超定的线性方程组，解这个线性方程组即可得到机器人动力学模型。
激励轨迹通常使用有限傅里叶级数激励轨迹，现有的有限傅里叶级数激励轨迹优化方法主要的问题，目标函数计算量大，约束条件过多，优化搜索空间过大，优化容易失败，优化时间过长等。
技术实现要素：
本发明的目的是提供一种快速收敛的机器人动力学参数辨识轨迹优化方法，旨在解决现有的有限傅里叶级数激励轨迹优化方法目标函数计算量大，约束条件过多，优化搜索空间过大，优化容易失败，优化时间过长等问题。
本发明的目的通过下述技术方案来实现：
一种快速收敛的机器人动力学参数辨识轨迹优化方法，包括如下步骤：
S1、定义有限傅里叶级数机器人动力学参数辨识激励轨迹非线性优化的表达式：
式中，目标函数为矩阵H的条件数；优化变量为有限傅里叶级数系数约束条件为机器人各关节角度、角速度和角加速度范围约束；
S2、简化有限傅里叶级数机器人动力学参数辨识激励轨迹非线性优化的目标函数计算量；
S3、在不改变搜索空间大小的前提下，简化有限傅里叶级数机器人动力学参数辨识激励轨迹非线性优化的约束条件数目；
S4、计算有限傅里叶级数系数的取值范围减小非线性优化的搜索空间，加快搜索速度；
S5、使用内部点算法进行有限傅里叶级数机器人动力学参数辨识激励轨迹非线性优化，加快非线性优化的收敛速度。
进一步地，所述步骤S2包括如下步骤：
S2.1、使用算法时间复杂度为线性阶O(n)的递归牛顿欧拉方法进行机器人逆动力学建模，可以得到如下的机器人逆动力学模型：
式中，Γ为各关节力矩；q,分别为各关节的转角、角速度和角加速度；δ为各连杆的标准动力学参数，且δ＝[δ 1δ2 … δn]T，δk为连杆k的标准动力学参数且δk＝[Lxxk LxykLxzk Lyyk Lyzk Lzzk lxk lyk lzk mk fvk fck Iak]T；
S2.2、对机器人动力学方程进行回归矩阵解耦；
S2.3、使用数值方法将回归矩阵简化为基础回归矩阵
S2.4、使用中间变量替代重复出现项，减少基础回归矩阵的计算量；
S2.5、在一个有限傅里叶级数周期T内，以一定的间隔Δt生成时间序列t，满足：
t＝[t1 t2 … tT/Δt]T， (3)
将时间序列t代入式(4)所示的有限傅里叶级数公式，得到T/Δt组
式中，qi，和分别为关节i的角度、角速度和角加速度；ωf是傅里叶级数的基础角频率；和是傅里叶级数的系数，其中l＝1,2,…,Ni；qio是关节i的偏置；i＝1,2,…,n，n为机器人的关节数目；
S2.6、将T/Δt组代入基础回归矩阵合并成一个矩阵HT/Δt，计算矩阵HT/Δt的条件数作为有限傅里叶级数激励轨迹非线性优化的目标函数。
进一步地，所述步骤S2.2具体包括：
S2.2.1、依次令矢量δ的第i个动力学参数等于1，其余动力学参数等于0，然后代入所示的InvDYN(·)函数中，则除去零项后，剩余项均为第i个动力学参数的相关项；
S2.2.2、将剩余项赋值给回归矩阵H的相关列，最终得到完整的回归矩阵H，从而得到解耦之后的机器人逆动力学模型：
式中，被称作回归矩阵，是关于各关节的转角、角速度和角加速度q,的函数。
进一步地，所述的步骤S2.3具体包括步骤：
S2.3.1、将矩阵和机器人标准动力学参数δ分成两部分，则式(5)逆动力学方程可以写成如下式(6)的形式：
式中，Hb为矩阵所有nb个线性无关列组成的子矩阵；Hd为剩下的nd个全零列向量和线性相关列组成的子矩阵；δb为基础动力学参数；δd为对动力学不起作用的动力学参数；
S2.3.2、设置换矩阵P＝[Pb Pd]满足如下式(7)：
HP＝[Hb Hd]， (7)
则有：
Hb＝HPb， (8)
随机生成M组且满足M>>len(δ)，将M组代入回归矩阵函数可以得到回归矩阵HM，如式(9)所示，对HM使用QR分解可以得到正则上三角矩阵R，
S2.3.3、遍历矩阵R的对角元素，记录其中非零对角元素在矩阵中的列号，依次写入数组db，定义行列数等于机器人标准动力学参数δ个数的单位矩阵P'，依照数组db记录的列号，按顺序从单位矩阵P'中取出对应的列组成新矩阵，该矩阵为Pb，将Pb代入式(8)则可以算得基础回归矩阵
进一步地，所述步骤S2.4具体包括：遍历基础回归矩阵的符号表达式，提取重复出现的项，使用中间变量替换，计算的时候，先计算中间变量，再计算各项基础回归矩阵的元素。
进一步地，所述步骤S3包括如下步骤：
S3.1将所述步骤S2.5得到的T/Δt组代入机器人各关节角度、角速度和角加速度的约束条件中：
可以得到6×T/Δt×n组不等式约束，式中，qmin,qmax,分别是各关节角度、角速度、角加速度的最小值和最大值，均为n×1的矢量，n为机器人的关节数目；
S3.2将所述步骤S3.16×T/Δt×n组不等式约束进行合并简化成6×n不等式约束：
式中，min(·)为求最小值函数；max(·)为求最大值函数。
进一步地，所述步骤S4包括如下步骤：
S4.1在式(4)的约束范围内，随机生成(2×N+1)组代入式(4)所示的有限傅里叶级数公式，得到一个关于[(2×N+1)×n]个有限傅里叶级数系数的线性方程组，可以解得一组n个关节的有限傅里叶级数系数
S4.2重复所述步骤S4.1K次，K尽可能地大，则可以得到K组有限傅里叶级数系数那么有限傅里叶级数系数的约束范围为：
将式(12)添加入非线性优化的约束条件中。
进一步地，所述步骤S5包括如下步骤：
S5.1重新定义有限傅里叶级数机器人动力学参数辨识激励轨迹非线性优化的表达式：
S5.2使用内部点算法计算式(13)所示的非线性优化问题。
本发明相对于现有技术具有如下的优点及效果：
本发明使用一系列方法加快有限傅里叶级数机器人动力学参数辨识激励轨迹非线性优化的收敛速度：包括使用递归牛顿欧拉方法进行机器人逆动力学建模，计算基础动力学参数回归矩阵和使用基于中间变量的回归矩阵计算方法简化非线性优化的目标函数计算量；合并关节角度、角速度、角加速度约束条件以减少约束条件的数目；计算傅里叶级数系数的取值范围减小非线性优化的搜索空间，加快搜索速度；使用内部点算法加快非线性优化的收敛速度。本发明解决有限傅里叶级数机器人动力学参数辨识激励轨迹非线性优化计算量大，收敛时间长的问题。
附图说明
图1是本发明实施例的一种快速收敛的机器人动力学参数辨识轨迹优化方法流程示意图。
图2是本发明实施例的一种快速收敛的机器人动力学参数辨识轨迹优化方法步骤S2的流程示意图。
图3是本发明实施例的一种快速收敛的机器人动力学参数辨识轨迹优化方法步骤S3的流程示意图。
图4是本发明实施例的一种快速收敛的机器人动力学参数辨识轨迹优化方法步骤S4的流程示意图。
图5是本发明实施例的一种快速收敛的机器人动力学参数辨识轨迹优化方法步骤S5的流程示意图。
具体实施方式
下面结合实施例及附图对本发明作进一步的详细描述，但本发明的实施方式不限于此。
现以某六自由度垂直关节串联机器人为动力学参数辨识对象，按照以下步骤进行机器人辨识实验所需要的有限傅里叶级数激励轨迹非线性优化：
一种快速收敛的机器人动力学参数辨识轨迹优化方法，包括如下步骤：
S1、定义有限傅里叶级数机器人动力学参数辨识激励轨迹非线性优化的表达式：
式中，目标函数为矩阵H的条件数；优化变量为为式(4)所示有限傅里叶级数系数约束条件为机器人各关节角度、角速度和角加速度范围约束；
S2、简化有限傅里叶级数机器人动力学参数辨识激励轨迹非线性优化的目标函数计算量；
S3、在不改变搜索空间大小的前提下，简化有限傅里叶级数机器人动力学参数辨识激励轨迹非线性优化的约束条件数目；
S4、计算有限傅里叶级数系数的取值范围减小非线性优化的搜索空间，加快搜索速度；
S5、使用内部点算法进行有限傅里叶级数机器人动力学参数辨识激励轨迹非线性优化，加快非线性优化的收敛速度。
具体而言，如图2所示，所述步骤S2包括如下步骤：
S2.1、使用算法时间复杂度为线性阶O(n)的递归牛顿欧拉方法进行机器人逆动力学建模，可以得到如下的机器人逆动力学模型：
式中，Γ为各关节力矩；q,分别为各关节的转角、角速度和角加速度；δ为各连杆的标准动力学参数，且δ＝[δ1δ2…δ6]T，δk为连杆k的标准动力学参数且δk＝[Lxxk Lxyk LxzkLyyk Lyzk Lzzk lxk lyk lzk mk fvk fck Iak]T；
S2.2、对机器人动力学方程进行回归矩阵解耦；
S2.3、使用数值方法将回归矩阵简化为基础回归矩阵
S2.4、使用中间变量替代重复出现项，减少基础回归矩阵的计算量；
S2.5、在一个有限傅里叶级数周期12.5秒内，以一定的间隔Δt＝0.001秒生成时间序列t,满足：
t＝[t1 t2 … t12500]T，(3)
将时间序列t代入式(4)所示的有限傅里叶级数公式，得到12500组
式中，Ni＝6，qi，和分别为关节i的角度、角速度和角加速度；ωf是傅里叶级数的基础角频率，取ωf＝0.16π；和是傅里叶级数的系数，其中l＝1,2,…,6；qio是关节i的偏置；i＝1,2,…,6；
S2.6、将12500组代入基础回归矩阵合并成一个矩阵H12500，计算矩阵H12500的条件数作为有限傅里叶级数激励轨迹非线性优化的目标函数。
具体而言，所述步骤S2.2具体包括：
S2.2.1、依次令矢量δ的第i个动力学参数等于1，其余动力学参数等于0，然后代入式(2)所示的InvDYN(·)函数中，则除去零项后，剩余项均为第i个动力学参数的相关项；
S2.2.2、将剩余项赋值给回归矩阵H的相关列，最终得到完整的回归矩阵H，从而得到解耦之后的机器人逆动力学模型：
式中，被称作回归矩阵，是关于各关节的转角、角速度和角加速度q,的函数。
具体而言，所述的步骤S2.3具体包括步骤：
S2.3.1、将矩阵和机器人标准动力学参数δ分成两部分，则式(5)逆动力学方程可以写成如下式(6)的形式：
式中，Hb为矩阵所有nb个线性无关列组成的子矩阵；Hd为剩下的nd个全零列向量和线性相关列组成的子矩阵；δb为基础动力学参数；δd为对动力学不起作用的动力学参数；
S2.3.2、设置换矩阵P＝[Pb Pd]满足如下式(7)：
HP＝[Hb Hd]， (7)
则有：
Hb＝HPb， (8)
随机生成10000组且满足M>>len(δ)，将M组代入回归矩阵函数可以得到回归矩阵HM，如式(9)所示，对HM使用QR分解可以得到正则上三角矩阵R，
S2.3.3、遍历矩阵R的对角元素，记录其中非零对角元素在矩阵中的列号，依次写入数组db，定义行列数等于机器人标准动力学参数δ个数的单位矩阵P'，依照数组db记录的列号，按顺序从单位矩阵P'中取出对应的列组成新矩阵，该矩阵为Pb，将Pb代入式(8)则可以算得基础回归矩阵
具体而言，所述步骤S2.4具体包括：遍历基础回归矩阵的符号表达式，提取重复出现的项，使用中间变量替换，计算的时候，先计算中间变量，再计算各项基础回归矩阵的元素，可以大大减少计算量。
具体而言，如图3所示，所述步骤S3包括如下步骤：
S3.1、将所述步骤S2.5得到的12500组代入式(10)所示的机器人各关节角度、角速度和角加速度的约束条件中：
可以得到6×12500×12500组不等式约束，式中，qmin,qmax,分别是各关节角度、角速度、角加速度的最小值和最大值，均为6×1的矢量；
S3.2、将所述步骤S3.16×12500×6组不等式约束进行合并简化成6×6组不等式约束：
式中，min(·)为求最小值函数；max(·)为求最大值函数。
具体而言，如图4所示，所述步骤S4包括如下步骤：
S4.1、在式(4)的约束范围内，随机生成(2×6+1)组代入式(9)所示的有限傅里叶级数公式，得到一个关于[(2×6+1)×6]个有限傅里叶级数系数的线性方程组，可以解得一组n个关节的有限傅里叶级数系数
S4.2、重复步骤S4.110000次，则可以得到10000组有限傅里叶级数系数那么有限傅里叶级数系数的约束范围为：
将式(12)添加入非线性优化的约束条件中。
具体而言，如图5所示，所述步骤S5包括如下步骤：
S5.1、重新定义有限傅里叶级数机器人动力学参数辨识激励轨迹非线性优化的表达式：
S5.2、使用内部点算法计算式(13)所示的非线性优化问题。
上述实施例为本方面较佳的实施方式，但本方明的实施方式并不受上述实施例的限制，其他的任何背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。
- 还没有人留言评论。精彩留言会获得点赞！