CN113183146B - 一种基于快速灵活全纯嵌入思想的机械臂运动规划方法 - Google Patents

Abstract

本发明公开了一种基于快速灵活全纯嵌入思想的机械臂运动规划方法，具体运用于冗余度机械臂容错型运动规划问题。本发明采用分段级数展开、离散化有理逼近、自适应调参、预测‑校正等先进的数值计算方法，跳出传统方法“碎步递增”的框架，利用分段式架构设计本发明中新方法，每个区间独立进行近似解构造，自动分段且进行预测‑校正，使计算精确度不像传统方法那样依赖于迭代步长，计算结果也不依赖于邻近节点的计算结果，计算速度与计算精度都得到大幅提升。本发明具有性能高效、稳定可靠、在高精确度需求下保持较快计算速度、简单、灵活、自动化等特点，可有效解决传统计算方法在高精确度需求下求解速度极低，计算时间过长等问题。

Description

一种基于快速灵活全纯嵌入思想的机械臂运动规划方法

技术领域

本发明涉及智能控制技术领域，尤其涉及一种具体运用于冗余度机械臂容错型运动规划问题的基于快速灵活全纯嵌入思想的机械臂运动规划方法。

背景技术

带参数的微分代数方程组的求解是一种经典的数值计算问题。在传统的数值计算方法中，有欧拉法，(三阶、四阶)龙格库塔法，通过邻近的一个或数个节点的迭代来进行求解；还有高斯消去法，牛顿-拉夫森法，分别用来求解静态的线性方程组和非线性方程组。在机械人设计与控制等工程应用领域，比如通过求解带参微分代数方程组来控制规划机械臂的运动轨迹，需要更快更准的求解方法提升工作效率和服务质量，对工业设计与应用有着重要意义。

对于传统求解方法，高斯消去法和牛顿-拉夫森法主要用来求解静态方程组，用于解决时变问题时无法考虑时间解的相关性，从而导致精度较低；而能用来求解时变问题的欧拉法和龙格库塔法，一般使用迭代来进行计算，对迭代步长的依赖较高，随着工程应用领域的深度发展，对于求解精确度的要求越来越高，例如，在冗余度机械臂容错型运动规划中，需要很精确地求解一个二次规划问题，在极高精确度的需求下，迭代步长需要极小，在给定的计算范围内，迭代步数就会暴增，计算能力成为瓶颈，常常出现求解速度极低，计算时间过长，不能满足机械臂运动对于实时性的要求，会导致工程应用领域生产力低下的问题，进而影响品牌良好形象，甚至会给品牌带来重大损失。。

发明内容

有鉴于现有技术的上述缺陷，本发明所要解决的技术问题是提供一种基于快速灵活全纯嵌入思想的机械臂运动规划方法，为工业应用领域提供快速、精确、可扩展的数值计算方法，解决目前工业应用领域对于求解带参数的微分、代数方程组的方法存在的问题，满足工业应用领域目前及未来对精准高效的数值计算方法的需求。

为实现上述目的，本发明提供了一种基于快速灵活全纯嵌入思想的机械臂运动规划方法，包括以下步骤：

S1、输入由时变问题转换的含参变量代数方程组或微分方程组和初值；

S2、将各未知量按参数进行幂级数展开，得到幂级数系数的方程组，得到未知量的解；

S3、采用有理函数对连续函数进行逼近，根据是否采用离散化，对整个区间作帕德函数近似，或者计算区间内少数几个离散点处解值；

S4、求得该区间终点的各未知量的值和误差值，判断最大误差值是否小于阈值；如果不是，进入步骤S5；如果是，进入步骤S6；

S5、将该区间长度等倍缩小；

S6、判断区间终点是否为方程组所求区间的终点；如果不是，进入步骤S7；如果是，进入步骤S8；

S7、记录帕德逼近函数或区间终点的数值解，以区间的终点为新区间的起点，将该点的数值作为初值代入幂级数系数方程组，并计算区间内少数几个离散点处解值；

S8、输出各未知量的在不同时刻的数值解。

进一步的，所述步骤S1和步骤S2具体为：得到在时间取初始值时所有变量的初始值；将所有变量作为关于时间参变量的幂级数展开式，并将这些展开式代入问题给出的微分代数方程组中，对于每次幂得到一个关于幂级数展开式系数的方程组，利用给定的变量初始值，即零次幂系数，从一次幂向高次幂求解每次幂下的关于系数的方程组，解得所有变量的幂级数展开式在当次幂下的系数，并保存到幂级数系数矩阵，最终得到所有变量关于时间参变量的幂级数展开式，在任一时间下可以通过这一形式计算出所有变量的值，即原问题给出的微分代数方程组在当前时间下的解，并构建出完整的幂级数系数矩阵。

进一步的，采用有理函数对连续函数进行逼近，具体为：设P(x)是n次代数多项式，Q(x)是m次代数多项式，称P(x)/Q(x)为对幂级数展开式函数的一个(n，m)阶有理函数逼近。

进一步的，所述步骤S4求得该区间终点的各未知量的值和误差值之前，还包括：给定误差阈值和最大区间长度，将初始值作为区间起点，最大区间长度作为区间初始长度。

本发明的有益效果是：

本发明采用分段级数展开、离散化有理逼近、自适应调参、预测-校正等先进的数值计算方法，跳出传统方法“碎步递增”的框架，利用分段式架构设计本发明中新方法，每个区间独立进行近似解构造，自动分段且进行预测-校正，使计算精确度不像传统方法那样依赖于迭代步长，计算结果也不依赖于邻近节点的计算结果，计算速度与计算精度都得到大幅提升。本发明具有性能高效、稳定可靠、在高精确度需求下保持较快计算速度、简单、灵活、自动化等特点，可有效解决传统计算方法在高精确度需求下求解速度极低，计算时间过长等问题。

以下将结合附图对本发明的构思、具体结构及产生的技术效果作进一步说明，以充分地了解本发明的目的、特征和效果。

附图说明

图1是本发明的方法流程图；

图2是本发明的采用FFHE高性能数值计算方法的冗余度机械臂容错型运动规划架构图；

图3是本发明的实施例的实验结果图。

具体实施方式

本发明采用分段级数展开、离散化有理逼近、自适应调参、预测-校正等先进的数值计算方法，跳出传统方法“碎步递增”的框架，利用分段式架构设计本发明中新方法，每个区间独立进行近似解构造，在高精度要求下保证了较快的计算速度。算法的整体流程如图3所示，由幂级数系数矩阵计算，有理逼近和区间自适应及校验这三大模块组成。

幂级数系数矩阵计算前，先得到初始时刻下的初始值，即在时间取初始值时所有变量的初始值；将所有变量作为关于参变量(时间)的幂级数展开式，并将这些展开式代入问题给出的微分代数方程组中，对于每次幂得到一个关于幂级数展开式系数的方程组，利用给定的变量初始值(即零次幂系数)，从一次幂向高次幂求解每次幂下的关于系数的方程组，解得所有变量的幂级数展开式在当次幂下的系数，并保存到幂级数系数矩阵，最终得到所有变量关于参变量(时间)的幂级数展开式，在任一时间下可以通过这一形式计算出所有变量的值，即原问题给出的微分代数方程组在当前时间下的解，并构建出完整的幂级数系数矩阵。

有理逼近是有理函数对连续函数的逼近，可以采用经典的帕德逼近算法，或是新型的LM算法，即离散化有理逼近算法。设P(x)是n次代数多项式，Q(x)是m次代数多项式，称P(x)/Q(x)为对幂级数展开式函数的一个(n，m)阶有理函数逼近。帕德逼近往往比截断的泰勒级数准确，而且当泰勒级数不收敛时，帕德逼近往往仍可行。

区间自适应之前，需要给定误差阈值和最大区间长度，将初始值作为区间起点，最大区间长度作为区间初始长度，当区间终点的变量误差值超过误差阈值时，缩短区间长度并继续判断区间终点的变量误差值，直到其在误差阈值以内，从而得到当前区间范围，并矫正当前区间终点，将它预测为下一区间起点(由于区间终点的误差值在误差阈值以内，经过校验后作为下一区间起点时可行的)，以新的起点值作为初值，重新构造幂级数系数矩阵并进行有理逼近，自适应划分新的区间，直到整个区间划分完毕。自适应区间和校验不仅可以将整个区间的误差几乎控制在误差阈值以内，而且对于给定时刻，只计算极少量的点的数值解就可以得到所求点的数值解(传统方法需要从初始值开始进行一步步迭代直到到达所求点，需要计算其间的每个点的数值解)，拥有更快的计算速度和更低的存储开销。

本发明可以应用于冗余度机械臂运动规划及控制领域，具体涉及一种冗余度机械臂的容错型运动规划方法。冗余度机械臂是一种自由度大于任务空间所需最少自由度的末端能动机械装置，其运动任务包括焊接、油漆、组装、挖掘和绘图等，广泛应用于装备制造、产品加工、机器作业等国民经济生产活动中。冗余度机械臂的逆运动学问题是指已知机械臂末端位姿，确定机械臂的关节角问题。当冗余度机械臂的某一关节因故障锁死时(相当于减少了机械臂的自由度)，就一般规划方案而言，该机械臂末端就无法根据预定的解析方案完成指定的任务；而容错型冗余度解析方案，就是对出现问题的关节进行容错处理(编号、记录锁定角度和进行加A型二次规划)，使得机械臂能够完成指定末端轨迹任务。

本发明在这一领域所要解决的技术问题是提供一种计算量小、用于冗余度机械臂发生关节故障时候的容错型运动规划方法，关键在于将基于二次规划的容错型冗余度解析方案运用数值方法进行求解。

冗余度机械臂容错型运动规划方法中，参数A表示机械臂关节的状态故障矩阵(用于在方案解析过程中固定故障关节)，根据发生故障出现锁死的关节编号，调整状态故障矩阵A中的相应元素为1(而A中该行其余元素均为0)。设计其性能指标为：最小化

，

受约束于Jθ^＆＝r^＆，Aθ^＆＝0，θ^-≤θ≤θ⁺，θ^＆-≤θ^＆≤θ^＆+，其中θ^＆表示关节速度向量，Q和p为合适维数的矩阵和向量，上标T表示矩阵和向量的转置，J表示机械臂的雅可比矩阵，θ表示关节角向量，θ^＆表示机械臂末端执行器速度向量，θ^-≤θ≤θ⁺，θ^＆-≤θ^＆≤θ^＆+分别表示关节角极限范围和关节速度极限范围，θ^±表示关节角上下限，θ^＆+表示关节速度上下限。

上述冗余度机械臂容错型运动规划可以转化为一个标准二次规划，得到所要解决的问题：最小化

，

受约束于C_k+1X_k+1＝d_k+1，t_k≤x_k+1≤t_k+1，其中 k_u用来调节和保证关节速度的足够大的可行域。

为了求解这个离散的时变二次规划问题，我们在一定范围内定义它的连续问题的拉格朗日函数为：

L(x(t)，l(t)，t)＝f(x(t)，t)+l^T(t)(A(t)x(t)-b(t)) (3)

，其中l^T(t)代表拉格朗日乘子向量。于是得到：

其中：

于是，t∈[t₀，t_f]的范围内，始终成立：

C(t)y^*(t)+d(t)＝0 (5)

这样得到一个代数方程组，用本发明介绍的算法来对其进行求解，算法关键步骤如下：

1、构建线性方程组A_qX_q＝B_q，并解得X_q，q为幂次，从而得到幂级数系数。

2、令(向下取整)，n＝q-m。

a)构建用于离散化有理逼近的L矩阵和M矩阵。

b)构建用于有理逼近的帕德近似函数。

3、根据当前点与区间起点的间隔Δs。

a)计算行列式L/M的值作为所求变量值。

b)计算有理近似函数的函数值作为所求变量值。

4、等倍缩小区间长度直到误差小于阈值，得到区间终点，然后返回步骤1，并以此为起点划分下一区间，直到划分完整个范围。

从而得到冗余度机械臂容错型运动规划的最优解。

下面给出一个例子进行具体说明：

C(t)y^*(t)+d(t)＝0，t∈[t₀，t_f] (6)

其中

此时方程组为：

即：

(2 sin t+3)x₁+cos t x₂+cos 2t l+sin 3t＝0 ①

cos 2t x₁+sin 2t x₂-sin t＝0 ③

带入幂级数展开式：

1、构建A_qX_q＝B_q：

2、令(向下取整)，n＝q-m。

a)构建L，M矩阵。

以系数列a对应的幂级数展开式为例，有：

b)构建帕德近似函数。

计算有理多项式除法的系数得到帕德近似函数。

3、代入Δs。

a)计算det(L)/det(M)，得到相应变量值。

b)计算帕德近似函数值，得到相应变量值。

4、等倍缩小区间长度直到误差小于阈值，得到区间终点，然后返回步骤1，并以此为起点划分下一区间，直到划分完整个范围。

该例子的实验结果如图3所示，图中0～10范围内共分14个区间，q取19，误差阈值为10^-5。

以上详细描述了本发明的较佳具体实施例。应当理解，本领域的普通技术人员无需创造性劳动就可以根据本发明的构思做出诸多修改和变化。因此，凡本技术领域中技术人员依本发明的构思在现有技术的基础上通过逻辑分析、推理或者有限的实验可以得到的技术方案，皆应在由权利要求书所确定的保护范围内。

Claims (3)

1.一种基于快速灵活全纯嵌入思想的机械臂运动规划方法，其特征在于，包括以下步骤：

S1、输入由冗余度机械臂容错型运动规划问题给出的含参变量微分代数方程组和初值；

S2、将各含参变量按参数进行幂级数展开，得到幂级数系数的方程组，得到含参变量的解；

给定误差阈值和最大区间长度，将初始值作为区间起点，最大区间长度作为区间初始长度，构建划分区间；

S3、采用有理函数对幂级数系数的方程组进行逼近，根据是否采用离散化，对划分区间作帕德函数近似，或者计算区间内少数几个离散点处解值；

S4、求得该区间终点的各含参变量的值和误差值，判断最大误差值是否小于阈值；如果不是，进入步骤S5；如果是，进入步骤S6；

S5、将该区间长度等倍缩小，然后返回步骤S4；

S6、判断区间终点是否为方程组所求区间的终点；如果不是，进入步骤S7；如果是，进入步骤S8；

S7、记录帕德逼近函数或区间终点的数值解，以区间的终点为新区间的起点，将该点的数值作为初值代入幂级数系数方程组，并计算区间内少数几个离散点处解值，然后返回步骤S4；

S8、输出各含参变量的在不同时刻的数值解。

2.如权利要求1所述的一种基于快速灵活全纯嵌入思想的机械臂运动规划方法，其特征在于，所述步骤S1和步骤S2具体为：得到在时间取初始值时所有含参变量的初始值；将所有含参变量作为关于时间参变量的幂级数展开式，并将这些展开式代入问题给出的微分代数方程组中，对于每次幂得到一个关于幂级数展开式系数的方程组，利用给定的变量初始值，即零次幂系数，从一次幂向高次幂求解每次幂下的关于系数的方程组，解得所有含参变量的幂级数展开式在当次幂下的系数，并保存到幂级数系数矩阵，最终得到所有含参变量关于时间参变量的幂级数展开式，在任一时间下可以通过这一形式计算出所有含参变量的值，即原问题给出的微分代数方程组在当前时间下的解，并构建出完整的幂级数系数矩阵。

3.如权利要求1所述的一种基于快速灵活全纯嵌入思想的机械臂运动规划方法，其特征在于，采用有理函数对连续函数进行逼近，具体为：设P(x)是n次代数多项式，Q(x)是m次代数多项式，称P(x)/Q(x)为对幂级数系数的方程组的一个(n,m)阶有理函数逼近。