一种未知参数下的双功能模型预测控制方法与流程

本发明属于工业自动控制领域，涉及一种机械臂控制方法。

背景技术：

工业革命以来，各行业尤其是工业界对更好技术的需求飞速增长。随着国际竞争力和经济效益方面的压力不断加剧，对产品质量的要求也不断增加，这些都对加工机器的可靠性和精确性提出了更高的要求，尤其是在高速生产过程中。

在这种背景下，工业机器人成为工业生产过程中不可或缺的自动化设备。其中，机械臂作为一种较早期出现的工业机器人，是工业机器人的重要组成之一。机械臂通过模仿人类手臂完成一些动作及功能，所以常被用于工业制造、军事、医疗、航空航天等领域。随着现代工业技术的飞速发展，对机械臂的速度和精度的要求也越来越高，这不断促使各国的学者将现代控制理论和机械臂控制系统相结合，从而更高效地解决机械臂中的控制问题。

机械臂由多个连杆组成，因此它有着多个自由度，具有高度耦合、多输入多输出以及强非线性的特点。与大多数的机械系统相同，已知机械臂的模型和系统参数时，可以直接用含有机械臂参数的数学模型来表示该机械臂的动态特性。但在实际情况中，常常因为机械臂自身参数的不确定以及外界环境中的干扰，无法得到机械臂准确的数学模型，所以也无法对机械臂进行精确控制。这样一来，机械臂模型的参数辨识问题成为机器人领域研究的热点之一。

基于机械臂系统自身多变量、强非线性及强耦合的特点，再加上参数的不确定和随机干扰影响，机械臂的控制面临极大的困难和挑战。传统的pid控制方法简单、灵活，但难以实现快速和高精度的控制。自适应控制可以根据估计值不断修正控制策略，从而满足实际闭环控制系统的要求，但由于没有考虑系统的动力学约束和不确定性，因此很难同时满足快速性和稳定性的要求。而模型预测控制是一种通过反馈控制来对被控对象进行前向滚动的最优控制方法，所以可以很好的克服模型自身的误差以及环境中的随机干扰。不难看出，模型预测控制对于机械臂这种复杂的控制系统来说，是一种实时有效的控制方法。

因此在实现机械臂控制的同时在线估计其动力学模型中的未知参数，将会是一种解决未知参数下的机械臂控制问题的有效途径。

文献“heirung,torakseln,ydstie,b.erik,foss,bjarne.towardsdualmpc.ifacproceedingsvolumes,2012,45(17):502-507.”提出了一种双功能模型预测控制的方法，该方法可以在实现控制的同时对模型中的未知参数进行估计。但该方法没有给出一个固定的代价函数形式，因此难以推广使用。

文献“morinelly,juane,ydstie,b.erik.dualmpcwithreinforcementlearning.ifacpapersonline,49(7):266-271.”提出了一种双功能模型预测控制与强化学习相结合的方法，将算法中的控制和估计部分利用强化学习中的“执行器-评判器”框架进行迭代学习，并在算法控制部分的最优决策中引入探索能力，对未来进行预测，并估计出系统的未知参数。但该算法只适用于系统量测方程上存在未知参数的情况，实际问题中经常会出现状态方程和量测方程同时存在未知参数的情况。因此需要提出一种新的算法，旨在针对当系统的状态方程和量测方程中同时存在未知参数时，在实现系统控制的同时估计出未知参数。将该算法和机械臂模型相结合，就可以在实现机械臂控制的同时，估计出动力学模型中的未知参数，从而实现机械臂的精准控制。

技术实现要素：

为了克服现有技术的不足，本发明提供一种未知参数下的双功能模型预测控制方法，能够在机械臂系统的动力学模型不确定时提高对于机械臂的控制精度。

本发明解决其技术问题所采用的技术方案包括以下步骤：

步骤1，对六自由度机械臂进行动力学建模；

步骤2，针对机械臂动力学模型，构建模型预测控制优化问题下的性能指标函数，得到一个目标函数；

步骤3，重构目标函数，求解该优化问题的最优控制输入序列，估计出机械臂动力学模型中的未知参数，该最优控制输入序列的第一个值就是机械臂在每一时刻对应的最优关节驱动力矩。

所述的机械臂动力学模型为表示第i个关节的位置向量，表示第i个关节的速度，表示第i个关节的加速度，表示第i个关节的驱动力矩向量，m(qi)为机器人的惯性矩阵，表示该机械臂受到的惯性力大小，是科里奥利矩阵，表示离心力和科氏力，为重力矩阵，表示该机器人受到重力的大小；将模型简化为离散时间下标准状态空间的形式：

xt+1＝(a+γi)xt+but

其中，xt表示机械臂在预测时域内第t个采样时刻的预测输出，该模型的控制变量为关节驱动力矩ut，vt表示高斯白噪声，a和b是已知的系数矩阵，i为单位矩阵。γ是状态方程中的未知参数，θ是量测方程中一个未知的系数矩阵。

所述的性能指标函数其中，m和n都是未知的参数，r＞0为折扣因子；得到最优控制输入序列和无限时域下的目标函数其中，n为一个有限时域，k是对称正定的终端惩罚矩阵，其中，为折扣因子。

所述的重构目标函数其中，c是阶段代价函数，π＝μ0,μ1,...，μk:属于一系列可靠的策略集合π。

本发明的有益效果是：通过引入未来的预期信息，沿着控制路径引入探索。在状态方程和量测方程同时存在未知参数时，可以估计出系统的未知参数；同时在强化学习的“执行器-评判器”框架下，能够实现系统边控制边估计的效果。由此可以实现机械臂动力学模型不确定时的准确控制。

附图说明

图1为方法步骤流程框图。

图2为matlab下机械臂建模仿真图。

图3为双功能模型预测控制算法流程图。

图4为matlab下利用双功能模型预测控制算法对未知参数θ和γ估计的仿真图。

图5为matlab下系统控制输出y的仿真图。

具体实施方式

下面结合附图和实施例对本发明进一步说明，本发明包括但不仅限于下述实施例。

本发明提供一种未知参数下的双功能模型预测控制方法，包括以下步骤：

步骤1：利用拉格朗日法对六自由度机械臂进行动力学建模，并对其进行简化和重构：

简化拉格朗日方程可以得到如下机械臂模型的动力学模型：

其中，表示第i个关节的位置向量；表示第i个关节的速度；表示第i个关节的加速度；表示第i个关节的驱动力矩向量，其与第i个关节的角位移量qi有关；m(qi)为机器人的惯性矩阵，表示该机械臂受到的惯性力大小。是科里奥利矩阵，表示离心力和科氏力；为重力矩阵，表示该机器人受到重力的大小。

将该模型线性化后再利用泰勒展开对其进行离散化，取采样时间为t。考虑到实际情况，在系统模型中引入高斯白噪声vt，并为了便于后续算法的推导，将模型简化为离散时间下标准状态空间的形式：

其中，xt表示机械臂在预测时域内第t个采样时刻的预测输出，该模型的控制变量为关节驱动力矩ut，噪声a和b是系统已知的系数矩阵，i为单位矩阵。γ是状态方程中的未知参数，θ是量测方程中一个未知的系数矩阵。

步骤2：针对化简后的机械臂模型，构建模型预测控制优化问题下的二次性能指标函数。求解该优化问题所得到的最优控制输入序列u^*(xt)，就是机械臂在每一时刻对应的最优关节驱动力矩。

将式(2)的状态方程代入量测方程中得：

将式(3)分解为下列两个子式：

为了方便后续的推导，设和由此构建性能指标函数：

其中，m和n都是未知的，r＞0为折扣因子。显然，j(xt)是一个关于u(xt)的函数。通过最小化该函数，即可得到最优控制输入序列u^*(xt)：

设计(n,∞)时间下的终端代价函数：

其中，n为一个有限时域，k是对称正定的终端惩罚矩阵，通过求解该系统的代数黎卡提方程获得。则该系统无限时域下的目标函数为：

其中，为折扣因子。

步骤3：双功能模型预测控制算法控制部分的设计，利用模型预测控制算法实现对机械臂的控制。

基于强化学习中的“执行器-评判器”框架，算法中控制部分和估计部分是一种相互迭代的关系，直至系统收敛。控制部分相当于执行器，通过求解优化问题，求出最优控制策略。估计部分相当于评判器，通过对未知参数进行估计，构造可解的目标函数。

在强化学习的框架下，重构目标函数：

其中，c是阶段代价函数，π＝μ0,μ1,...，μk:属于一系列可靠的策略集合π。

根据贝尔曼方程，式(9)可拓展为如下n步代价函数：

则最优控制策略为：

步骤4：双功能模型预测控制算法估计部分的设计，利用强化学习和参数辨识中的相关知识，估计出机械臂动力学模型中的未知参数。

利用系统通过模型预测控制预测得到的未来k步的控制输入，引入预期信息，构造信息矩阵对未来的不确定信息进行估计：

其中，为先前的信息矩阵，ut和yt分别为系统的控制输入和控制输出。pt是未知参数的误差矩阵，

利用状态和参数联合估计的方法：第一步，给定未知参数初始值，利用其对状态变量进行估计；第二步，利用上一步估计得到的状态变量，对未知参数进行估计。以此不断迭代，交替进行估计，直至参数收敛。

利用估计得到的参数和状态变量，构造近似阶段代价函数：

其中，为了方便表述，定义z1,t+k:＝p1,t+k|txt+k|t，z2,t+k:＝p2,t+k|tut+k。

重构目标函数：

其中，终端惩罚矩阵

由此设计出的双功能模型预测控制算法可以很好的解决机械臂动力学模型中含有未知参数时的控制问题。通过求解模型预测控制中的优化问题得到当前时刻的最优控制力矩，输入该力矩后在线估计动力学模型中的未知参数，以此不断进行迭代，直至机械臂到达指定目标点。

本发明的实施例包括以下步骤：

步骤1：利用拉格朗日法对六自由度机械臂进行动力学建模，并对其进行简化和重构；

如图2所示，选取六自由度的机械臂模型作为研究对象，该机械臂包含六个刚性连杆和六个关节。简化拉格朗日方程可以推导出机械臂的动力学方程：

其中，表示第i个关节的位置向量；表示第i个关节的速度；表示第i个关节的加速度；表示第i个关节的驱动力矩向量，其与第i个关节的角位移量qi有关；m(qi)为机器人的惯性矩阵，表示该机械臂受到的惯性力大小。是科里奥利矩阵，表示离心力和科氏力；为重力矩阵，表示该机器人受到重力的大小。

为了简化模型方便计算，定义机械臂的状态变量为先将式(1)表示为t时刻的连续空间状态方程：

其中，p(x1)＝m(q)^-1。

设采样周期为t，将状态变量中的各个分量利用泰勒展开式进行离散化，由此可以得出系统离散化的状态空间模型：

其中，c＝[in0n×n]，n为预测时域，x(k)表示机械臂在第k个采样时刻的预测输出，关节力矩τ(k)为该模型的控制变量。

考虑到在实际情况下，系统环境中总是存在有不确定的干扰，因此在输入信号中引入高斯白噪声vt，为了便于后续算法的推导，改写式(3)为以下离散状态空间：

其中，xt表示机械臂在预测时域内第t个采样时刻的预测输出，该模型的控制变量为关节驱动力矩ut，γ是一个未知参数，θ是一个未知的矩阵向量，噪声a和b是系统已知的系数矩阵，i为单位矩阵。γ是状态方程中的未知参数，θ是量测方程中一个未知的系数矩阵。

步骤2：针对化简后的机械臂模型，构建模型预测控制优化问题下的二次性能指标函数。求解该优化问题所得到的最优控制输入序列u^*(xt)，就是机械臂在每一时刻对应的最优关节驱动力矩。

根据式(4)不难得到：

为了方便后续推导，假定则有：

将式(6)分解为下列两个子式：

由此构建出如下性能指标函数：

其中，m和n都是未知的，r＞0为折扣因子。显然，j(xt)是一个关于u(xt)的函数。通过最小化该函数，即可得到最优控制输入序列u^*(xt)：

根据预测控制的基本原理，优化得到的控制输入序列的第一个元素会作用于被控系统。在下一采样时刻，系统获得了新的状态值，以该状态值为预测系统未来动态的初始条件，即用该状态值刷新优化问题，再次求解优化问题并得到一个新的控制序列。因此，预测控制利用“滚动优化-重复进行”的机制实现在线优化。

设计局部线性状态反馈控制律u＝lx驱使系统渐近地回到原点，即此时a'+bl是稳定的。则(n,∞)时间下的终端代价函数为：

其中，n为一个有限时域，k是对称正定的终端惩罚矩阵。

则该系统无限时域下的目标函数为：

其中，为折扣因子。

一般地，终端惩罚矩阵k为下面这个代数黎卡提方程的解：

再根据预测的“最优性”，使用线性最优控制方法求解最优反馈控制增益矩阵：

其中，k^*是最优控制下的终端惩罚矩阵。

显然，目标函数j(xt)中存在未知参数m和n，该优化问题无法直接用传统的模型预测控制方法对其进行求解。由此，需要先估计出未知参数m和n，才可以求解该目标函数。但在对未知参数进行估计时，又需要利用系统的控制输入u，该控制输入通过求解目标函数j(xt)获得。因此，在该问题下，估计和控制两者之间是一种相互影响、相互耦合的关系。

步骤3：双功能模型预测控制算法控制部分的设计，利用模型预测控制算法实现对机械臂的控制。

这里，运用强化学习中的“执行器-评判器”框架。将估计看作学习中的“评判器”，通过收集环境中的控制输出y，估计出未知参数γ和θ，使得目标函数可解，从而可以用来评估该步控制输入的价值；将控制看作学习中的“执行器”，将动作也就是控制策略u^*作用于环境。两者不断进行策略迭代，直至系统收敛，即可实现一种边估计边控制的效果。在该框架下，强化学习是一种通过观察环境对非最优控制策略的实时响应来学习最优行为的方法。

在强化学习的框架下，将目标函数(11)重构为：

其中，c是阶段代价函数，π＝μ0,μ1,...，μk:属于一系列可靠的策略集合π。也就是说，如果给出的策略π是准确性的，那么该策略在每个状态x下的动作也是确定的。α是折扣因子，且0＜α≤1。如果增广矩阵{a',b}可控，则当jπ(xt)取到最小值时的策略即为最优策略，u^*＝μ^*(x)。

根据贝尔曼方程，式(10)可拓展写为如下n步代价函数：

则最优控制策略可由下式求出：

经过不断的策略迭代，最终当k收敛时，即kl＝kl-1时，此时k^*所对应的策略就是所求的最优策略。

引入预期信息，将先前时刻和将来时刻的控制输入以及测量值用下式表示:

其中，这里利用模型预测控制计算得到的未来k步的控制输入，构成信息矩阵。引入探索，预测未来的未知信息，同时估计了未知参数m和n。

不难得出，

其中，k＝0,1,...，输出测量值是确定的，状态变量中由于包含未知参数γ，因此是未知的，系统控制输入序列ut,ut+1,...,ut+k-1是已知的。

t时刻有关未知参数的误差协方差矩阵定义如下：

因此近似代价函数可以写为(k≥0):

为了简化计算，定义z1,t+k:＝p1,t+k|txt+k|t，z2,t+k:＝p2,t+k|tut+k。因此式(22)可以表示为：

重构目标函数：

其中，

显然，该目标函数中的各个参数都是已知的，所以此时可以用传统的模型预测控制算法求解该目标函数，得到此时的最优控制输入u^*。

进一步，为了更清晰的展示目标函数中每一部分的作用，将式(26)可以写为：

步骤4：双功能模型预测控制算法估计部分的设计，利用强化学习和参数辨识中的相关知识，估计出机械臂动力学模型中的未知参数。

由于在状态方程和量测方程上同时存在未知参数γ和θ，这两个未知参数是一种相互耦合的关系，因此无法直接利用递推最小二乘法对其进行估计。所以，这里采用了状态和参数联合估计的方法。

联合估计方法的计算步骤：第一步，假定参数值已知，来估计状态变量；第二步，将上一步估计得到的状态变量代入系统方程中，对参数进行估计。

以此不断进行交替估计。

p2,t+1＝(in-g2,t+1ut)p2,t(33)

再联立两个式子解出γ和θ的估计值：

估计出参数后再计算状态量：

式中，ρi为收敛因子，并且满足：

由此设计出的双功能模型预测控制算法可以很好的解决机械臂动力学模型中含有未知参数时的控制问题。通过求解模型预测控制中的优化问题得到当前时刻的最优控制力矩，输入该力矩后在线估计动力学模型中的未知参数，以此不断进行迭代，直至机械臂到达指定目标点。