基于Koopman算子的全方位移动机械臂数据驱动模型预测控制方法与流程

本发明涉及全方位移动机械臂控制技术领域，具体涉及基于koopman算子的全方位移动机械臂数据驱动模型预测控制方法。

背景技术：

全方位移动机械臂由全方位移动平台与固定在移动平台上的机械臂组成，兼具全方位移动平台的广阔操作空间和机械臂的操作灵活性，广泛应用在工业制造、太空探索及勘探资源等领域。与非完整移动平台相比，全方位移动平台能在不改变轮子方向的情况下向任意方向运动，具有更好的运动灵活性。轨迹跟踪能力是其完成指定任务的重要组成部分，由于全方位移动机械臂的动力学模型非线性强、耦合度高等特点，建立精确的动力学模型非常困难，机理法建模耗时且分析困难，得到的模型往往精确度不高，对最终的控制精度有很大的影响，不精确的模型甚至会导致全方位移动机械臂轨迹跟踪控制任务失败。

当前的控制方法均有各自的局限性，绝大部分的研究工作控制器设计均需动力学模型信息，但实际应用中精确模型难以获得，而且控制器结构复杂，计算量大，调参工作繁重。对系统本身产生的数据得不到有效利用，且目前的大多数算法都无法有效处理机器人系统的状态和控制量的约束，限制了实际场合中的进一步应用。

发明目的

本发明的目的是为克服现有技术的不足，而提供一种基于koopman算子的全方位移动机械臂数据驱动模型预测控制方法，是利用koopman算子能够将非线性系统辨识为高维线性模型的特性，得到全方位移动机械臂这一强非线性系统的高维线性表示，并与模型预测控制技术结合，实现对全方位移动机械臂的有效控制。

为实现本发明的目的所采取的技术方案如下：

基于koopman算子的全方位移动机械臂数据驱动模型预测控制方法，包括步骤：

步骤1：利用能够激发机器人动力学特性的控制输入进行输入输出数据的采集，利用产生的输出数据与控制输入数据建立数据集。

全方位移动机械臂动力学模型如下：

式中，表示世界坐标系下机器人的位姿，[·]^t表示矩阵的转置，如图1所示，x、y为机器人质心om在世界坐标系owxwywzw下的位置，为xw方向和xm方向夹角，连杆l1与水平面的夹角为θ1，连杆l2与zm方向的夹角为θ2，连杆l1和连杆l2分别由两个电机提供驱动力矩；m∈r^5×5表示一个惯性矩阵，∈表示集合间的“属于”关系，r^5×5表示5行5列的实数矩阵；c∈r^5×5表示离心力和哥氏力矩阵；g∈r^5×1表示模型的重力项；u∈r^5×1表示控制输入，b∈r^5×5为输入转换矩阵。

采集数据时，应根据自身系统特点，给定能够激发系统动力学特性的控制输入数据，然后采集系统的输入输出数据组成数据集。本发明中，将电压序列给到机器人后能够得到当前位置和下一时刻位置的位姿数据，分别存储为矩阵x和y，基于x,y,u数据集可进行高维线性模型的求解，步骤1中采集到的数据集如下：

其中：

步骤2：基于koopman算子理论，在上述数据集的基础上，利用扩展动力学模态分解方法求解机器人所对应的koopman高维线性模型。

基于步骤1数据集，利用扩展动力学模态分解算法求解得到移动机械臂的高维线性模型状态在高维空间的演化为线性的。

本发明中利用扩展动力学模态分解方法求解koopman高维模型的具体实施步骤如下：

利用koopman算子能够构建出原未知非线性系统的状态空间形式的高维线性模型，如下所示：

其中z(k)＝ψ(xk)为升维函数，为预测的系统输出。

记ψ(x)＝[x^t,ψn+1(x),...,ψn(x)]^t。

xlift＝[ψ(x1),k,ψ(xk)],ylift＝[ψ(x2),k,ψ(xk+1)]，高维模型表示式(3)中的a,b,c矩阵能够通过在升维空间构造的最小二乘问题求解得到：

前n维由系统自身状态组成，ψi是需人为选定的非线性函数。式(4)的解析解为：

由此，求解出全方位移动机械臂的koopman高维线性模型。

步骤3：给定参考轨迹，借助步骤2求得的高维线性模型设计模型预测控制器。

针对高维系统(3)设计模型预测控制器，首先可以写出对未来状态的预测方程如下：

其中np,nc分别为预测时域和控制时域长度，且np≥nc。

系统的输出为:

将系统的预测方程写成如下紧凑的形式：

y(k)＝fz(k)+φu(k)(8)

其中：

u(k)＝[u^t(k),u^t(k+1|k),...,u^t(k+nc-1|k)]^t。

紧凑的预测方程中矩阵为：

预测时域内的参考轨迹为：yr(k)＝[yr^t(k+1),yr^t(k+2),...,yr^t(k+np)]^t。

q和r为正定对角矩阵，对角线上为常数元素，则mpc的性能指标函数可写为：

j＝(y(k)-yr(k))^tq(y(k)-yr(k))+u^t(k)ru(k)(11)

将上式整理成二次规划问题求解：

j＝(fz(k)+φu(k)-yr(k))^tq(fz(k)+φu(k)-yr(k))+u^t(k)ru(k)

＝(fz(k)-yr(k))^tq(fz(k)-yr(k))+2u^t(k)φ^tq(fz(k)-yr(k))+u^t(k)(φ^tqφ+r)u(k)(12)

由于上式右边的第一项与u(k)无关，则得：

其中：h＝φ^tq(fz(k)-yr(k))，e＝φ^tqφ+r(14)

这就将mpc的求解转化为了线性二次规划问题，u(k)的第一个元素即为此时的控制量。

给定参考轨迹，借助高维线性模型设计mpc控制器，通过优化求解得到每步闭环控制的最优控制信号，并且求解出的控制量在执行器的约束范围内，将控制信号发送至移动机械臂，从而控制移动机械臂跟踪参考轨迹。

与现有技术相比，本发明所述的基于koopman算子的数据驱动模型预测控制只依赖系统的离线输入输出数据，控制器设计无需复杂的机理建模方法，省去了对模型的分析过程，能够从数据中挖掘系统特性，同时为非线性系统提供了一种线性控制方法，能够借助发展完善的线性控制理论来发挥本方法的最大优势。

目前绝大多数非线性模型预测控制都需求解一个凹优化的方程，但是凹优化难以获得全局最优解，只能得到局部最优的解，且求解时间过长，计算量大。而本发明的基于koopman算子的数据驱动模型预测控制方法的优化方程是凸优化的，且能够处理系统状态和控制量的约束，求解出的控制量在执行器的约束范围内，能够快速得到使性能指标函数最小的最优解，快速求解控制信号能够防止出现失控的状态，确保对于参考轨迹的跟踪性能，且无需系统精确的动力学模型信息，降低了对运算资源的依赖。

附图说明

图1为本发明中全方位移动机械臂的模型示意图；

图2为本发明的koopman算子与模型预测控制结合示意图；

图3为本发明的koopman高维模型对真实模型未来状态的预测效果图；

图4a是全方位移动机械臂平面正方形轨迹跟踪控制仿真平面轨迹曲线；

图4b是全方位移动机械臂平面正方形轨迹跟踪控制仿真各方向轨迹追踪曲线；

图4c是全方位移动机械臂平面正方形轨迹跟踪控制仿真各方向轨迹跟踪误差变化曲线；

图4d是全方位移动机械臂平面正方形轨迹跟踪控制仿真控制输入变化曲线；

图5a是全方位移动机械臂平面圆形轨迹跟踪控制仿真平面轨迹曲线；

图5b是全方位移动机械臂平面圆形轨迹跟踪控制仿真各方向轨迹追踪曲线

图5c是全方位移动机械臂平面圆形轨迹跟踪控制仿真各方向轨迹跟踪误差变化曲线；

图5d是全方位移动机械臂平面圆形轨迹跟踪控制仿真控制输入变化曲线。

具体实施方式

本发明的特征在于借助全方位移动机械臂的离线输入输出数据实现，基于koopman理论计算求得移动机械臂的高维线性模型，并利用该模型设计模型预测控制器，生成控制信号使得移动机械臂能够跟踪给定的参考轨迹。

模型预测控制是工业场合中应用广泛的一种控制方法，能够有效处理状态与控制量所受的约束。其本质是基于模型的闭环优化控制策略，算法核心是可预测未来状态的动态模型，当前控制动作是在每一个采样瞬间通过求解一个有限时域开环最优控制问题而获得的，解得的最优控制序列只实施第一个控制作用，具有控制效果好、鲁棒性强等特点。传统的非线性模型预测控制是非凸优化的过程，计算耗时，不利于实时控制。

而基于koopman算子的模型预测控制摆脱了上述限制，为研究全方位移动机械臂的模型预测控制以及koopman算子在机器人系统中的应用提供了重要依据。

基于koopman算子的全方位移动机械臂数据驱动模型预测控制方法，包括步骤：

步骤1：利用能够激发机器人动力学特性的控制输入进行输入输出数据的采集，利用产生的输出数据与控制输入数据建立数据集。

全方位移动机械臂动力学模型如下：

式中，表示世界坐标系下机器人的位姿，[·]^t表示矩阵的转置，如图1所示，x、y为机器人质心om在世界坐标系owxwywzw下的位置，为xw方向和xm方向夹角，连杆l1与水平面的夹角为θ1，连杆l2与zm方向的夹角为θ2，连杆l1和连杆l2分别由两个电机提供驱动力矩；m∈r^5×5表示一个惯性矩阵，∈表示集合间的属于关系，r^5×5表示5行5列的实数矩阵；c∈r^5×5表示离心力和哥氏力矩阵；g∈r^5×1表示模型的重力项；u∈r^5×1表示控制输入，b∈r^5×5为输入转换矩阵,连杆l3与连杆l2，连杆l4的一端连接，连杆l3的一端连接末端执行器，连杆l2，连杆l4平行布置，并通过连杆l1在另一端相连接，所述全方位机械臂具有移动平台，移动平台具有全向轮，移动平台上安装有支架，通过支架安装由四个连杆构成的全方位机械臂。

采集数据时，应根据自身系统特点，给定能够激发系统动力学特性的控制输入数据，然后采集系统的输入输出数据组成数据集。

本发明中，将电压序列给到机器人后能够得到当前位置和下一时刻位置，分别存储为矩阵x和y，基于x,y,u数据集可进行高维线性模型的求解。

步骤1中采集到的数据集为：

其中：

步骤2：基于koopman算子理论，在上述的数据集的基础上，利用扩展动力学模态分解方法求解机器人所对应的koopman高维线性模型。

基于步骤1数据集，利用扩展动力学模态分解算法求解得到移动机械臂的高维线性模型状态在高维空间的演化为线性的。

koopman算子最大的特点是能够从数据中学习到未知系统的动力学模型，在复杂的动力学系统中利用数据学习是非常重要的。在控制理论领域中，模型的不确定性或扰动通常使用鲁棒控制或自适应的方法来进行补偿，鲁棒控制器常用来处理模型的不确定性，自适应方法能够在产生扰动时对控制器进行自调整。然而，对系统模型的依赖限制了上述方法在更为一般的情况下的应用。近年来，控制领域中数据驱动的控制方法发展迅速，koopman算子能够从数据中挖掘系统的信息，在数据驱动控制中占据着重要的位置。

koopman算子是一个无限维线性的数学算符，利用koopman算子能够精确捕捉非线性系统的动力学行为。通过定义一个观测函数，该观测函数可以将低维非线性的系统状态映射到另一个函数空间，在该空间下观测函数的演化由koopman算子控制。

总的来说，通过对原系统状态的转换，利用koopman算子能够实现原非线性系统的全局线性化。koopman算子能够将非线性系统表示成传统的状态空间方程，这就具有了与发展较为完善的线性控制理论结合的优势，mpc、lqr、h无穷等算法均可以与koopman线性模型结合，koopman算子是将线性控制方法应用到非线性系统控制的有效工具。

利用koopman算子能从输入-输出数据中构建非线性受控系统的线性模型，并使用已有的线性控制方法进行控制。理论上koopman算子是无限维线性算子，实际中计算这样一个算子是不可行的，常用dynamicmodedecompositiondmd、extendeddynamicmodedecompositionedmd算法求解一个高维常数矩阵作为koopman算子的逼近，求解高维矩阵的方法只需系统的数据，不需动力学系统的任何先验信息。

由上可知，基于koopman算子理论，原非线性系统的线性化动力学方程能够完全从数据中计算出来，并能应用到控制器的设计中。

其中，利用扩展动力学模态分解方法求解koopman高维模型的具体实施步骤如下：

利用koopman算子能够构建出原未知非线性系统的状态空间形式的高维线性模型，如下所示：

其中z(k)＝ψ(xk)为升维函数，为预测的系统输出。

记ψ(x)＝[x^t,ψn+1(x),...,ψn(x)]^t。

xlift＝[ψ(x1),k,ψ(xk)],ylift＝[ψ(x2),k,ψ(xk+1)]，高维模型表示式(3)中的a,b,c矩阵能够通过在升维空间构造的最小二乘问题求解得到：

前n维由系统自身状态组成，ψi是需人为选定的非线性函数。式(4)的解析解为：

由此，求解出全方位移动机械臂的koopman高维线性模型。

步骤3：给定参考轨迹，借助步骤2求得的高维线性模型设计模型预测控制器。

针对高维线性模型(3)设计模型预测控制器，首先写出对未来状态的预测方程如下：

其中np,nc分别为预测时域和控制时域长度。

系统的输出为:

将系统的预测方程写成如下紧凑的形式：

y(k)＝fz(k)+φu(k)(8)

其中：

u(k)＝[u^t(k),u^t(k+1|k),...,u^t(k+nc-1|k)]^t。

紧凑的预测方程中矩阵为：

预测时域内的参考轨迹为：yr(k)＝[yr^t(k+1),yr^t(k+2),...,yr^t(k+np)]^t。

q和r为正定矩阵，模型预测控制mpc的性能指标函数可写为：

j＝(y(k)-yr(k))^tq(y(k)-yr(k))+u^t(k)ru(k)(11)

将上式整理成二次规划问题求解：

j＝(fz(k)+φu(k)-yr(k))^tq(fz(k)+φu(k)-yr(k))+u^t(k)ru(k)

＝(fz(k)-yr(k))^tq(fz(k)-yr(k))+2u^t(k)φ^tq(fz(k)-yr(k))+u^t(k)(φ^tqφ+r)u(k)(12)

由于上式右边的第一项与u(k)无关，则得：

其中：h＝φ^tq(fz(k)-yr(k))，e＝φ^tqφ+r(14)

这样，就将模型预测控制(mpc)的求解转化为了线性二次规划问题，u(k)的第一个元素即为此时的控制量。

给定参考轨迹，借助高维线性模型设计mpc控制器，通过优化求解得到每步闭环控制的最优控制信号，并且求解出的控制量在执行器的约束范围内，将控制信号发送至移动机械臂，从而控制移动机械臂跟踪参考轨迹。

为验证本发明所设计的控制算法的有效性，以matlab作为仿真平台，以全方位移动机械臂(图1所示)为控制对象进行了全方位移动机械臂轨迹跟踪控制仿真实验的验证。下面结合仿真实验和附图，对本发明提出的基于koopman算子的全方位移动机械臂数据驱动轨迹跟踪控制方法做出详细说明。图2是koopman算子与模型预测控制结合的附图说明。

进行控制之前，需先建立系统的输入输出数据集。本次验证中数据集包含两万个数据点，升维函数ψ(x)＝[x^t,ψn+1(x),...,ψn(x)]^t，其中n＝60，ψi＝||x-xc||²log(||x-xc||)，xc为元素均在-1到1之间的常向量。

在得到koopman高维线性模型后首先需测试其对真实模型的拟合程度，如图3所示，koopman高维线性模型与真实模型均给定图3a所示的方波电压，初始状态为x0＝[-0.5；0.7；-0.1；0；0]，将预测状态与真实状态进行对比，图3b中可以看到在相同输入情况下高维模型的状态输出与真实模型状态输出很好地吻合。然后设计mpc控制器跟踪给定轨迹。因高维模型在mpc控制过程中无法确定输入电压的位置，所以需将世界坐标系下的参考轨迹转换到移动坐标系下升维然后进行控制。如图4a-4d，图5a-5d所示，仿真中令全方位移动机械臂跟踪两种轨迹任务，移动平台按照给定的圆形轨迹或正方形轨迹在平面上运动，机械臂的两个关节角跟踪正弦轨迹。

由于本发明的控制器设计中不包含任何模型信息，故这里没有给出具体的模型参数。

模型预测控制器的参数如下：

预测时域长度np＝3，优化目标中终端误差项增益qnp＝200，其余时刻误差增益q＝0.2，电压优化项增益r＝10^-5，电压的输出约束在执行器的接受范围内，本发明中执行器输出范围为[-24v，+24v]。圆形轨迹仿真时间为30秒，正方形轨迹仿真时间为40秒，采样频率均为100hz。

移动平台的正方形参考轨迹与机械臂的正弦参考轨迹均为关于时间t的函数，t的单位为秒。如下：

θ1d[rad]＝0.7sin(π/10×t)0s≤t≤40s

θ2d[rad]＝0.6sin(π/10×t)0s≤t≤40s

移动平台的圆形参考轨迹与机械臂的正弦参考轨迹均为关于时间t的函数，t的单位为秒。如下：

两种轨迹的跟踪仿真结果如图4a-4d，图5a-5d所示。从图4a、b和图5a、5b中能够看到各个通道轨迹跟踪的控制效果，能够较好地跟踪上设定的轨迹；从图4c、图5c中看到，系统的跟踪误差很小，满足控制算法的跟踪要求；图4d、图5d显示了控制电压随时间的变化曲线，且控制电压均能约束在-24v到24v之间。

经过上述分析，验证了本发明算法的有效性。