一种模块化机械臂协调操作最优控制方法及系统与流程

1.本发明涉及机器人控制技术领域，特别是涉及一种模块化机械臂协调操作最优控制方法及系统。


背景技术：

2.模块化机械臂是一类具有标准模块与接口，可根据不同的任务需求对自身构形进行重新组合与配置的机械臂，相比于传统的工业机械臂，模块化机械臂更适用于在轨维护、空间探测、行星表面采样返回等一系列任务，然而这些任务时刻都存在着协调操作。此外，模块化机械臂的一个重要的特性是机械臂模块可以添加、移除、代替不需要重新调整其他模块的增益以及参数，由于这种特性，研究模块化机械臂协调操作任务下的分散控制十分必要。
3.根据模块化机械臂的“模块化”与“可重构”的设计理念，模块化机械臂的每个关节模块均包含了通讯、传感、驱动、控制等单元，通过对这些模块进行重新配置可以使机械臂呈现多种装配构形来完成不同的工作任务，从而表现出传统机械臂所不具有的优势。在模块化机械臂的使用过程中，不仅要考虑其稳定性、精确性、鲁棒性等指标，还需要考虑其节能性，因此，选取一个合适的控制策略是十分必要的。
4.最优控制由于不仅考虑了系统的准确性还考虑了节能性，因此被广泛应用于非线性系统。近似动态规划理论作为一种有效解决最优控制问题的维数灾问题，在连续时间系统、离散时间系统、数据驱动系统等都有广泛的应用。对于模块化机械臂系统，要获取其最优控制策略则需求解耦合的哈密顿-雅克比方程，而该方程是一类偏微分方程，难以用解析方法求得最优解，无法保证整体能耗最优，因此如何能够保证最优控制方法可以使整体能耗最优是需要解决的问题。


技术实现要素：

5.本发明的目的是提供一种模块化机械臂协调操作最优控制方法及系统，可以解决现有技术中对于多个子系统不能保证整体能耗最优的协调操作问题。
6.为实现上述目的，本发明提供了如下方案：
7.一种模块化机械臂协调操作最优控制方法，包括：
8.构建模块化机械臂系统的各子系统的动力学模型和各所述子系统的性能指标函数；
9.对于任意一个子系统，根据所述子系统的性能指标函数得到所述子系统的最优性能指标函数和所述子系统的最优控制律表达式；
10.根据所述子系统的最优性能指标函数和所述子系统的最优控制律表达式得到所述子系统的耦合哈密顿-雅克比方程；
11.根据所述子系统的最优控制律表达式和所述子系统的耦合哈密顿-雅克比方程得到所述子系统的控制律表达式；
12.根据所述子系统的最优性能指标函数得到所述子系统的性能指标函数；
13.采用值迭代算法对所述子系统的性能指标函数进行值迭代求解得到最优性能指标函数对；
14.根据最优性能指标函数对和所述子系统的控制律表达式得到所述子系统的最优控制律表达式对；
15.根据所述子系统的动力学模型得到所述子系统的分散控制器表达式；
16.根据所述子系统的最优控制律表达式对和所述子系统的分散控制器表达式得到所述子系统的最优控制器表达式，根据各所述子系统的最优控制器表达式对各所述子系统进行控制。
17.可选的，所述根据所述子系统的性能指标函数得到所述子系统的最优性能指标函数和所述子系统的最优控制律表达式，具体包括：
18.根据所述子系统的性能指标函数确定所述子系统的哈密顿方程；
19.根据所述子系统的哈密顿方程确定所述子系统的最优性能指标函数；
20.根据所述子系统的最优性能指标函数确定所述子系统的最优控制律表达式。
21.可选的，所述根据所述子系统的动力学模型得到所述子系统的分散控制器表达式，具体包括：
22.根据所述子系统的动力学模型确定所述子系统的状态空间表达式；
23.根据所述子系统的状态空间表达式对所述子系统的动力学模型进行扩张得到所述子系统的扩张动力学模型；
24.根据所述子系统的扩张动力学模型得到所述子系统的分散动力学模型；
25.根据所述子系统的分散动力学模型确定所述子系统的分散控制器表达式。
26.可选的，所述根据所述子系统的最优控制律表达式对和所述子系统的分散控制器表达式得到所述子系统的最优控制器表达式，具体包括：
27.将所述子系统的最优控制律表达式对中最优的控制律表达式确定为最终最优控制律表达式；
28.将所述最终最优控制律表达式与所述子系统的分散控制器表达式相加确定为所述子系统的最优控制器表达式。
29.一种模块化机械臂协调操作最优控制系统，包括：
30.构建模块，用于构建模块化机械臂系统的各子系统的动力学模型和各所述子系统的性能指标函数；
31.最优表达式确定模块，用于对于任意一个子系统，根据所述子系统的性能指标函数得到所述子系统的最优性能指标函数和所述子系统的最优控制律表达式；
32.耦合哈密顿-雅克比方程确定模块，用于根据所述子系统的最优性能指标函数和所述子系统的最优控制律表达式得到所述子系统的耦合哈密顿-雅克比方程；
33.控制律表达式确定模块，用于根据所述子系统的最优控制律表达式和所述子系统的耦合哈密顿-雅克比方程得到所述子系统的控制律表达式；
34.性能指标函数确定模块，用于根据所述子系统的最优性能指标函数得到所述子系统的性能指标函数；
35.值迭代模块，用于采用值迭代算法对所述子系统的性能指标函数进行值迭代求解
得到最优性能指标函数对；
36.最优控制律表达式对确定模块，用于根据最优性能指标函数对和所述子系统的控制律表达式得到所述子系统的最优控制律表达式对；
37.分散控制器表达式确定模块，用于根据所述子系统的动力学模型得到所述子系统的分散控制器表达式；
38.最优控制器表达式确定模块，用于根据所述子系统的最优控制律表达式对和所述子系统的分散控制器表达式得到所述子系统的最优控制器表达式，根据各所述子系统的最优控制器表达式对各所述子系统进行控制。
39.可选的，所述最优表达式确定模块，具体包括：
40.哈密顿方程确定单元，用于根据所述子系统的性能指标函数确定所述子系统的哈密顿方程；
41.最优性能指标函数确定单元，用于根据所述子系统的哈密顿方程确定所述子系统的最优性能指标函数；
42.最优控制律表达式确定单元，用于根据所述子系统的最优性能指标函数确定所述子系统的最优控制律表达式。
43.可选的，所述分散控制器表达式确定模块，具体包括：
44.状态空间表达式确定单元，用于根据所述子系统的动力学模型确定所述子系统的状态空间表达式；
45.扩张单元，用于根据所述子系统的状态空间表达式对所述子系统的动力学模型进行扩张得到所述子系统的扩张动力学模型；
46.分散动力学模型确定单元，用于根据所述子系统的扩张动力学模型得到所述子系统的分散动力学模型；
47.分散控制器表达式确定单元，用于根据所述子系统的分散动力学模型确定所述子系统的分散控制器表达式。
48.可选的，所述最优控制器表达式确定模块，具体包括：
49.最终最优控制律表达式确定单元，用于将所述子系统的最优控制律表达式对中最优的控制律表达式确定为最终最优控制律表达式；
50.最优控制器表达式确定单元，用于将所述最终最优控制律表达式与所述子系统的分散控制器表达式相加确定为所述子系统的最优控制器表达式。
51.根据本发明提供的具体实施例，本发明公开了以下技术效果：本发明提供的模块化机械臂协调操作最优控制方法及系统，方法包括：构建模块化机械臂系统的各子系统的动力学模型和各所述子系统的性能指标函数；对于任意一个子系统，根据所述子系统的性能指标函数得到所述子系统的最优性能指标函数和所述子系统的最优控制律表达式；根据所述子系统的最优性能指标函数和所述子系统的最优控制律表达式得到所述子系统的耦合哈密顿-雅克比方程；根据所述子系统的最优控制律表达式和所述子系统的耦合哈密顿-雅克比方程得到所述子系统的控制律表达式；根据所述子系统的最优性能指标函数得到所述子系统的性能指标函数；采用值迭代算法对所述子系统的性能指标函数进行值迭代求解得到最优性能指标函数对；根据最优性能指标函数对和所述子系统的控制律表达式得到所述子系统的最优控制律表达式对；根据所述子系统的动力学模型得到所述子系统的分散控
制器表达式；根据所述子系统的最优控制律表达式对和所述子系统的分散控制器表达式得到所述子系统的最优控制器表达式，根据各所述子系统的最优控制器表达式对各所述子系统进行控制，采用值迭代算法得到最优性能指标函数对，根据最优性能指标函数对和所述子系统的分散控制器表达式得到所述子系统的最优控制器表达式，根据在近似动态规划系统中的值迭代算法，来近似性能指标函数并估计耦合的哈密顿-雅克比方程的解，然后设计分散控制器最终得到系统的最优控制律，可以解决现有技术中对于多个子系统不能保证整体能耗最优的协调操作问题。
附图说明
52.为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
53.图1为本发明实施例提供的一种模块化机械臂协调操作最优控制方法的流程图。
具体实施方式
54.下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
55.为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。
56.本发明实施例提供了一种模块化机械臂协调操作最优控制方法，具体是一种基于值迭代分散非零和博弈的模块化机械臂协调操作最优控制方法，如图1所示，所述协调操作最优控制方法包括：
57.步骤101：构建模块化机械臂系统的各子系统的动力学模型和各所述子系统的性能指标函数。
58.步骤102：对于任意一个子系统，根据所述子系统的性能指标函数得到所述子系统的最优性能指标函数和所述子系统的最优控制律表达式。
59.步骤103：根据所述子系统的最优性能指标函数和所述子系统的最优控制律表达式得到所述子系统的耦合哈密顿-雅克比方程。
60.步骤104：根据所述子系统的最优控制律表达式和所述子系统的耦合哈密顿-雅克比方程得到所述子系统的控制律表达式。
61.步骤105：根据所述子系统的最优性能指标函数得到所述子系统的性能指标函数。
62.步骤106：采用值迭代算法对所述子系统的性能指标函数进行值迭代求解得到最优性能指标函数对。
63.步骤107：根据最优性能指标函数对和所述子系统的控制律表达式得到所述子系统的最优控制律表达式对。
64.步骤108：根据所述子系统的动力学模型得到所述子系统的分散控制器表达式。
65.步骤109：根据所述子系统的最优控制律表达式对和所述子系统的分散控制器表达式得到所述子系统的最优控制器表达式，根据各所述子系统的最优控制器表达式对各所述子系统进行控制。
66.在实际应用中，步骤102具体包括：
67.根据所述子系统的性能指标函数确定所述子系统的哈密顿方程。
68.根据所述子系统的哈密顿方程确定所述子系统的最优性能指标函数。
69.根据所述子系统的最优性能指标函数确定所述子系统的最优控制律表达式。
70.在实际应用中，步骤108具体包括：
71.根据所述子系统的动力学模型确定所述子系统的状态空间表达式。
72.根据所述子系统的状态空间表达式对所述子系统的动力学模型进行扩张得到所述子系统的扩张动力学模型。
73.根据所述子系统的扩张动力学模型得到所述子系统的分散动力学模型。
74.根据所述子系统的分散动力学模型确定所述子系统的分散控制器表达式。
75.在实际应用中，步骤109具体包括：
76.将所述子系统的最优控制律表达式对中最优的控制律表达式确定为最终最优控制律表达式。
77.将所述最终最优控制律表达式与所述子系统的分散控制器表达式相加确定为所述子系统的最优控制器表达式。
78.本发明实施例还提供了一种与上述方法对应的模块化机械臂协调操作最优控制系统，包括：
79.构建模块，用于构建模块化机械臂系统的各子系统的动力学模型和各所述子系统的性能指标函数。
80.最优表达式确定模块，用于对于任意一个子系统，根据所述子系统的性能指标函数得到所述子系统的最优性能指标函数和所述子系统的最优控制律表达式。
81.耦合哈密顿-雅克比方程确定模块，用于根据所述子系统的最优性能指标函数和所述子系统的最优控制律表达式得到所述子系统的耦合哈密顿-雅克比方程。
82.控制律表达式确定模块，用于根据所述子系统的最优控制律表达式和所述子系统的耦合哈密顿-雅克比方程得到所述子系统的控制律表达式。
83.性能指标函数确定模块，用于根据所述子系统的最优性能指标函数得到所述子系统的性能指标函数。
84.值迭代模块，用于采用值迭代算法对所述子系统的性能指标函数进行值迭代求解得到最优性能指标函数对。
85.最优控制律表达式对确定模块，用于根据最优性能指标函数对和所述子系统的控制律表达式得到所述子系统的最优控制律表达式对。
86.分散控制器表达式确定模块，用于根据所述子系统的动力学模型得到所述子系统的分散控制器表达式。
87.最优控制器表达式确定模块，用于根据所述子系统的最优控制律表达式对和所述子系统的分散控制器表达式得到所述子系统的最优控制器表达式，根据各所述子系统的最优控制器表达式对各所述子系统进行控制。
88.可选的，所述最优表达式确定模块，具体包括：
89.哈密顿方程确定单元，用于根据所述子系统的性能指标函数确定所述子系统的哈密顿方程。
90.最优性能指标函数确定单元，用于根据所述子系统的哈密顿方程确定所述子系统的最优性能指标函数。
91.最优控制律表达式确定单元，用于根据所述子系统的最优性能指标函数确定所述子系统的最优控制律表达式。
92.可选的，所述分散控制器表达式确定模块，具体包括：
93.状态空间表达式确定单元，用于根据所述子系统的动力学模型确定所述子系统的状态空间表达式。
94.扩张单元，用于根据所述子系统的状态空间表达式对所述子系统的动力学模型进行扩张得到所述子系统的扩张动力学模型。
95.分散动力学模型确定单元，用于根据所述子系统的扩张动力学模型得到所述子系统的分散动力学模型。
96.分散控制器表达式确定单元，用于根据所述子系统的分散动力学模型确定所述子系统的分散控制器表达式。
97.可选的，所述最优控制器表达式确定模块，具体包括：
98.最终最优控制律表达式确定单元，用于将所述子系统的最优控制律表达式对中最优的控制律表达式确定为最终最优控制律表达式。
99.最优控制器表达式确定单元，用于将所述最终最优控制律表达式与所述子系统的分散控制器表达式相加确定为所述子系统的最优控制器表达式。
100.在实际应用中，模块化机械臂系统的第i个子系统的动力学模型为：
[0101][0102]
其中i
im
表示第i个子系统的转动惯量；γi代表第i个子系统的减速比，是第i个子系统的关节摩擦；代表第i个子系统的交联耦合项；代表第i个子系统的位置、速度、加速度向量；τ
is
代表第i个子系统力矩传感器测量得到的数据；τi代表第i个子系统的控制力矩；di(qi)代表第i个子系统受到的不确定性干扰；f代表机械臂与物体接触之间产生的内应力，代表雅克比矩阵。
[0103]
在实际应用中，根据所述子系统的动力学模型确定所述子系统的状态空间表达式，具体为：
[0104]
根据第i个子系统的动力学模型，定义第i个子系统的状态向量其中x
i1
为第i个子系统的位置状态向量，x
i2
为第i个子系统的速度状态向量，分别表示机器人第i个子系统的实际位置和速度，r为实数集。
[0105]
根据状态向量和第i个子系统的控制器ui＝τi∈r1×1可以得到第i个子系统的状态空间表达式为：
[0106][0107]
其中，表示第i个子系统的实际速度，表示第i个子系统的实际加速度，gi＝(i
im
γi)-1
∈r

是控制输入矩阵，为系统的漂移动力学，为系统的不确定扰动，x表示机械臂系统的全局向量。
[0108]
在实际应用中，第i个子系统的扩张动力学模型为：
[0109][0110]
其中x1＝[x
11
,...,x
i1
,...,x
n1
]
t
,x2＝[x
12
,...,x
i2
,...,x
n2
]
t
,
[0111]
分
[0112]
别为机械臂系统的位置、速度、漂移动力学、控制输入矩阵，um是第m个子系统的控制输入，x1为全局位置向量，x2为全局速度向量，x1和x2上面有点含义为全局位置向量与速度向量的导数：f(x)表示全局漂移动力学，gm表示全局控制输入矩阵。
[0113]
在实际应用中，第i个子系统的性能指标函数为：
[0114][0115]
其中位置误差定义为e＝[e1,e2,
…
,en]
t
＝x
1-xd，en表示第n个子系统的位置误差，上角标t表示转置，xd为机械臂系统期望的位置向量，速度误差定义为xd为机械臂系统期望的速度向量，qi,r
im
是正定的已知常数矩阵，代表第i个子系统的效用函数。
[0116]
在实际应用中，根据所述子系统的性能指标函数确定所述子系统的哈密顿方程，具体包括：
[0117]
对子系统的性能指标函数取极值，可以定义第i个子系统的哈密顿方程如下：
[0118][0119]
其中，是期望的加速度，为性能指标函数的梯度。
[0120]
在实际应用中，所述子系统的最优性能指标函数具体为：
[0121][0122]
在实际应用中，根据所述子系统的最优性能指标函数确定所述子系统的最优控制
律表达式具体为：
[0123]
基于非线性系统最优控制设计理论下，易知满足如下耦合的哈密顿-雅克比方程：
[0124][0125]
若存在并且连续可微分，则模块化机械臂系统的最优控制律为：
[0126][0127]
其中r
ii
是第i个子系统输入矩阵，gi为第i个子系统扩张的控制输入矩阵，表示最优性能指标函数的梯度。
[0128]
在实际应用中，根据所述子系统的最优性能指标函数和所述子系统的最优控制律表达式得到所述子系统的耦合哈密顿-雅克比方程，具体为：
[0129]
在实际应用中，将子系统的最优性能指标函数代入子系统的最优控制律表达式，可以得到改进的子系统的耦合哈密顿-雅克比方程为：
[0130][0131]
其中为第m个子系统的最优性能指标函数，r
mm
为第m个子系统的输入矩阵，r
im
为第i个和第m个子系统之间耦合的输入矩阵。
[0132]
在实际应用中，根据最优性能指标函数对和所述子系统的控制律表达式得到所述子系统的最优控制律表达式对，并根据子系统的最优控制律表达式对中最优的控制律表达式确定为最终最优控制律表达式，具体为：
[0133]
由于子系统的最优控制律表达式中的最优控制律难以直接获得，因此使用值迭代方法来得到控制律的表达形式。
[0134]
第i个子系统的控制律表达式为：
[0135][0136]
其中为第i个子系统在第k次设计的最优控制律，t为时间间隔，为第i个子系统在t t时刻第k次设计的性能指标函数；为第i个子系统在第k次设计的性能指标函数的梯度。
[0137]
第i个子系统的性能指标函数具体为：
[0138][0139]
其中为第i个子系统在第k 1次设计的性能指标函数。
[0140]
当k从0增长到无穷大的时刻，公式(10)和(11)之间进行迭代，控制策略对：以及对应的性能指标函数对：可以分别收敛到其相应的最优值，先对公式(11)进行迭代求解得到性能指标函数对，将每次迭代得到的性能指标函数对输入公式(10)得到对应的控制策略对，多次迭代选取最优的控制策略对，得到最终最优控制律表达式。
[0141]
在实际应用中，模块化机械臂的一个重要的特性是机械臂模块可以添加、移除、代替不需要重新调整其他模块的增益以及参数，由于这种特性，研究模块化机械臂协调操作任务下的分散控制十分必要，所以根据所述子系统的扩张动力学模型得到所述子系统的分散动力学模型，具体为：
[0142]
为了能够实施分散控制方法，将子系统的扩张动力学模型中的f(x)改写为如下的形式：
[0143]
f(x)＝fi(xi,x
md
) δfi(x,x
md
), (12)
[0144]
其中x
md
为第m个子系统期望的状态，fi(x,x
md
)表示部分已知的子系统漂移动力学，δfi(x,x
md
)为替代误差，满足如下的形式：
[0145][0146]
其中d
im
是一个未知的常数，em＝||x
m-x
md
||,为函数逼近误差，xm表示第m个子系统的位置。
[0147]
在实际应用中，根据所述子系统的分散动力学模型确定所述子系统的分散控制器表达式，具体为：根据(12)和(13)，可以得到第m个子系统的控制输入(分散控制器表达式)：
[0148][0149]
其中为第m个子系统期望的加速度，fm(xd)为第m个子系统期望的漂移动力学。
[0150]
子系统i的最优控制器表达式为：
[0151][0152]
为了验证上述实施例所提出的基于值迭代分散非零和博弈的模块化机械臂协调操作最优控制的有效性，本实施例建立一个六自由度模块化机械臂模型进行实验验证，其中包括增量式编码器、直流电机、谐波减速器、绝对式编码器以及关节力矩传感器。直流电机的型号是maxon公司的218014、额定电压为48v、额定力矩为190mnm、减速比为100:1。电机侧的位置变量是由maxon公司的500线增量式编码器测量的，连杆侧的位置变量是由netzer公司的19位编码器测量的。关节力矩传感器的额定力矩为20nm被嵌在关节模块末端。采用
quanser公司生产的线性功率放大器驱动关节模块的电动机和qpide数据采集卡采集各传感器的数据。
[0153]
通过实验结果表明，所提出的基于值迭代分散非零和博弈协调操作最优控制为模块化机械臂提供稳定性和精确性，以满足各种任务的要求。
[0154]
本发明有以下技术效果：
[0155]
在协调操作任务方面，本发明采用了基于值迭代的非零和博弈的方法，可以提高系统的跟踪精度以及响应速度。
[0156]
在控制力矩方面，本发明的控制力矩更加连续和平滑，不仅可以保持跟踪误差一致最终有界，而且可以降低每个关节的能量损耗。
[0157]
本发明解决了现有协调操作技术中存在控制精度低以及跟踪速度慢的问题，为模块化机械臂提供稳定性和精确性，并且可以满足各种任务的需求
[0158]
本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。
[0159]
本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。