CN110119085A - 一种Manutec R3型工业机器人动态优化系统 - Google Patents

Abstract

本发明涉及一种Manutec R3工业机器人动态优化系统。本发明由控制室工程师指定机器人每个关节变量的初始状态、末端状态等参数，优化计算系统通过计算时间网格精细化的优化算法，得出使机器人完成作业消耗时间最短的每个关节轴上的扭矩电压，将其转换为控制指令信号，传输给驱动单元，驱动单元根据传感器反馈回来的信号，支配机器人每个关节的执行单元去完成规定的运动和功能，位置传感器返回机器人手臂位置，从而实时在线优化。本发明能够根据Manutec R3机器人系统状况计算出当前实现最优运行的路径规划，最大限度地缩短机器人的工作时间，提高工作效率，该系统非常适用于复杂的机器人动态优化问题的在线优化。

Description

一种Manutec R3型工业机器人动态优化系统

技术领域

本发明属于动态优化控制技术领域，具体涉及一种Manutec R3型工业机器人动态优化系统。

背景技术

机器人的路径规划问题、时间最优问题、移动机器人的区域搜索控制问题等诸多复杂问题均为最优控制问题，一般也称为动态优化问题。这类问题一般含有微分和代数方程，以及众多的轨线等式和不等式约束。对于复杂最优控制问题，传统上采用间接解法，求解的一般步骤为：(1)首先将原系统扩展成Hamilton系统；(2)推导出一阶最优性必要条件；(3)获得数学上的两点边值问题，进而采用相应的方法进行求解得到精确的解析解。但是对于一些大规模的并且含有约束条件的最优控制问题时，在求解过程中，则需要引入更多的乘子函数和互补条件，这也是间接法不适合求解大规模、复杂度较高的动态优化问题的原因。

迭代动态规划算法是基于动态规划计算量大，求解效率不高等缺点提出的，改进的地方主要包括两大部分：网格离散和区域缩减。虽然迭代动态规划方法是一种具有全局收敛性的优化方法，并且不需要计算梯度等信息，但是它也有它明显的不足之处：由于离散后的时间段很细才会得到满意的最优解致使计算成本大幅度提高。

随着计算机和计算技术的发展，解决复杂最优控制问题的方法往往是采用直接法。直接法，顾名思义，与间接法相反，不需要求解最优性条件，而是直奔动态优化问题本身，直接对性能指标进行寻优。直接法的原理是将动态问题的整个时间域中的控制变量以及状态变量进行离散化，这样便能够将原动态优化问题转化成一个大规模的非线性规划问题。离散方法中的离散策略对计算精度和实时性影响很大，时间网格划分得是否恰当决定了求解效率和最优控制轨迹的逼近精度。划分地疏，对最优控制轨迹的逼近程度不高；划分地密，虽然确保了离散精度满足要求，但大大增加了非线性规划问题的维数和计算成本。为此，本发明给出一种精细化时间网格的Manutec R3型工业机器人动态优化系统，内部嵌入改进的动态优化求解算法，使得Manutec R3型工业机器人在线控制精度和效率大大提高。

发明内容

本发明的目的在于提供一种Manutec R3型工业机器人动态优化系统，

本发明用于复杂的机器人的在线优化控制。包括优化计算模块、控制器、数模转换器、模数转换器、驱动器、执行器、位置传感器、现场总线网络、人机交互模块、Manutec R3型工业机器人机械主体，系统运行过程具体包括以下步骤：

步骤A1：由控制室工程师通过人机交互模块指定Manutec R3机器人腰关节、肩关节、肘关节变量的初始状态以及末端状态，凭借工程经验设定机器人完成作业的大致时间t_f，指定扭矩电机的性能约束、起始时间网格数N个、控制变量的初始值，10≤N≤20，非线性规划问题求解模块的精度ε_l、精细化收敛性判断模块的精度ε_J，精细化迭代次数上限I_max、最小时间网格宽度w_min以及控制网格精细化模块的合并系数α和插入系数β_ii，ii＝1,2,…,nn；nn表示插入系数个数；

步骤A2：优化计算模块通过输入的参数，执行控制网格精细化模块的优化策略，计算出机器人最快完成相应动作所需的每个关节轴上的扭矩电压，通过现场总线网络发送给控制器的数模转换器；

步骤A3：控制器根据控制指令控制相应的驱动器，驱动器支配机器人每个关节的执行单元去完成规定的运动和功能；

步骤A4：位置传感器实时采集机器人手臂的位置信息，经过模数转换后用现场总线网络送给优化计算模块，可以实时更新控制策略，实现在线优化。

所述的优化计算模块，包括输入模块、初始化模块、控制网格精细化模块、非线性规划问题求解模块、精细化收敛性判断模块、输出模块；所述的优化计算模块执行的步骤如下：

步骤B1：输入模块接收工程师通过人机交互系统输入的腰关节、肩关节、肘关节变量的初始状态，末端状态，最终时间t_f，扭矩电机的性能约束，时间网格数N，控制变量的初始值以及ε_l、ε_J、I_max、w_min、α、β_ii参数信息；

步骤B2：执行初始化模块，根据最终时间t_f，起始时间网格数N，采用均匀离散化的方式，计算出起始时间网格分布；初始化控制变量的初始值，初始化非线性规划问题求解模块的精度ε_l、迭代次数l＝0，设置精细化收敛性判断模块的精度ε_J、精细化迭代次数I＝0、精细化迭代次数上限I_max、最小时间网格宽度w_min、合并系数α、插入系数β_ii；

步骤B3：执行最优控制形式转换模块；

步骤B4：通过非线性规划问题求解模块求解非线性规划问题，得到当前时间网格下的最优的控制参数和目标函数值，当l＝0时，跳过步骤B5，直接执行步骤B6；

步骤B5：运行精细化收敛性判断模块，若满足收敛条件，则算法终止，并执行输出模块；否则，执行下一步；

步骤B6：运行控制网格精细化模块，得到新的控制参数、新的时间网格分布和新的目标函数值J，精细化迭代次数I＝I+1，转入步骤B4。

所述的最优控制形式转换模块，执行步骤如下：

步骤C1：判断机器人最优控制问题的目标函数、等式约束、不等式约束是否都为Mayer形式，是的话跳出最优控制形式转换模块，执行步骤B4；否则，如果机器人最优控制问题的目标函数、等式约束、不等式约束中含有Lagrange形式的函数，则执行步骤C2；如果机器人最优控制问题的目标函数、等式约束、不等式约束中含有Bolza形式的函数，则执行步骤C3；

步骤C2：将机器人最优控制问题转换为不含积分项的Mayer形式的函数，引入新的状态变量x_n+1(t)，并使其满足式(1)：

其中，u(t)是(n×1)维的控制变量，x(t)是(m×1)维的状态变量，L₀(t,x(t),u(t))为与机器人目标函数中积分项有关的函数，t为时间，t₀为起始时间，表示x_n+1(t)的导数；

则有式(2)：

经过上式的转换，目标函数J和不等式约束以及等式约束中的积分项均可由新的状态变量在终端时刻值确定，如式(3):

其中，X(t)为加入新的状态变量x_n+1(t)后的状态变量，f[t,x(t),u(t)]表示机器人动态特性的函数，L_r[t,X(t),u(t)]＝0为m₁个等式约束，L_r[t,X(t),u(t)]≥0为m₂个不等式约束；X(t₀)表示起始时间t₀时状态变量，和u分别表示控制变量的上下界约束；

步骤C3：将机器人最优控制问题转换为不含积分项的Mayer形式的函数，同样引入新的状态变量x_n+1(t)，并使其满足式(4)：

将目标函数J转换为式(5)：

其中，Φ₀[x(t_f),t_f]为终值项；通过Mayer形式转换过后，最优控制问题的Mayer形式表示如式(6)：

其中，X(t)为加入新的状态变量x_n+1(t)后的状态变量，Φ_r[x(t_f)]表示与x(t_f)相关的函数项，Φ_r[x(t_f)]+L_r[t,X(t),u(t)]＝0为m₁个等式约束，Φ_r[x(t_f)]+L_r[t,X(t),u(t)]≥0为m₂个不等式约束。

所述的控制网格精细化模块，包括控制网格合并模块、控制网格插入模块、时间切换点定位模块，执行步骤如下：

步骤D1：根据式(7)、式(8)计算当前的控制参数 对应的左斜率右斜率

其中，分别是时间区间[t_i-2,t_i-1]，[t_i-1,t_i]和[t_i,t_i+1]的控制参数，t_i(i＝0,1,…,N)为离散化后的时间网格节点，j表示控制变量u(t)第j个分量；

步骤D2：消除一些没有必要的时间节点来降低非线性规划问题维数；如果控制参数的左斜率与下一时间网格的控制参数的右斜率满足时间网格节点的合并准则，见式(9)：

其中，α为合并系数，那么就可消除t_i这个时间节点，合并时间子区间[t_i-1,t_i]和[t_i,t_i+1]，合并后的子区间[t_i-1,t_i+1]的控制参数的取值为控制参数和控制参数的平均值；

步骤D3：插入一些时间网格来提高逼近精度，定义时间网格节点细化规则如式(10)：

式中，Δk为插入时间节点的个数，β_ii(ii＝1,2,…,nn)为用于表征控制参数变化快慢的插入系数，其值随着ii值的增大而增大，nn表示插入系数个数，一般为1-6之间的整数；对于控制参数的左右斜率和如果其斜率平均值在[β₁,β₂)内，则在控制参数所属的时间网格[t_i,t_i+1]内的0.5(t_i+1-t_i)+t_i时刻插入一个时间节点；对于控制参数的左右斜率和如果斜率平均值在[β_nn-1,β_nn)，则在这个控制参数所属的时间网格[t_i,t_i+1]内均匀插入nn-1个时间节点；假设第b-1与第b个时间网格区间被细化分为Δk个小区间，对于i＝b，那么细化后的时间节点将被重新标记如式(11)：

对于i＝b+1,b+2,…N，那么细化后的时间节点将被重新标记为式(12)：

式中为重新标记后时间节点；

步骤D4：对重要时间切换点进行定位；为了更好地描述相邻两个控制参数的变化趋势，对于控制参数和下一个时间网格控制参数的中点连线定义一个斜率，称为中间斜率si，i＝1,2,…,N-1，如式(13)：

对重要时间切换点的判断准则如式(14)：

s_i·s_i+1≤0 (14)；

则说明相邻的控制参数和呈现一个“转折”现象，这个转折点被认为是重要的时间切换点；若

|s_i|≤|s_i+1| (15)；

则将t_i+1作为待优化的时间节点与控制变量同时优化，若

|s_i|≥|s_i+1| (16)；

则将t_i作为待优化的时间节点与控制变量同时优化。

所述的精细化收敛性判断模块，执行步骤如下：

步骤E1：计算当前精细化迭代的目标函数值与前一次精细化迭代的目标函数值之差的绝对值是否满足设定的误差容限系数ε_J，如果满足式(17)，则跳到步骤E4；否则，执行步骤E2；

|J^(I+1)-J^(I)|＜ε_J (17)；

其中J^(I+1)和J^(I)分别代表第I+1次与第I次精细化迭代所得的目标函数值；

步骤E2：计算时间网格的宽度w_i＝t_t-t_i-1(i＝1,2,...,N)是否满足式(18)，如果满足下式，则跳到步骤E4；否则，执行步骤E3；

min{w_i}≤w_min i＝1,2,...,N (18)；

步骤E3：判断是否大于设定的迭代次数上限I_max，如果满足式(19)，则跳到步骤E4；否则，跳到步骤E5；

I＞I_max (19)；

步骤E4：满足收敛条件，则算法终止，并执行输出模块；

步骤E5：不满足收敛条件，执行步骤B6。

所述的非线性规划问题求解模块运行步骤，执行步骤如下：

步骤F1：通过ODE求解模块解微分方程初值问题，获取本次迭代的目标函数值和约束函数值；

步骤F2：NLP收敛性判断，如果l＝0，则认为不满足NLP收敛条件，直接执行步骤F3；如果l>0且步骤F1中本次迭代的目标函数值J^(l)与上一个迭代的目标函数值J^(l-1)的绝对值之差小于设定的精度ε_l，则认为满足NLP收敛，返回本次迭代得到的3个关节轴上的扭矩电压，执行步骤B5；否则执行步骤F3；

步骤F3：计算搜索方向矢量，将式(20)所示的非线性优化命题泰勒展开为如式(21)所示的QP子问题：

其中f(x)为一维连续可微目标函数，c(x)表示mu维连续可微约束方程；x表示nu维与机器人有关的变量，包括控制变量和状态变量，x^U和x^L分别表示变量的上下界约束；

x在迭代点x_l处对上式进行泰勒展开，并忽略高次项和目标函数中的常数项，则原命题转化为以下QP子问题：

式中d_l为搜索方向矢量，g_l ^T和A_l ^T分别表示在x_l处目标函数f(x_l)的导数g_l的转置和约束方程c(x_l)的雅克比矩阵，c_l表示x_l处约束方程c(x_l)的值，W为拉格朗日函数的Hessian阵；通过求解式(21)，可获得搜索方向矢量的值d_l；

步骤F4：步长计算，令x_l+1＝x_l+γd_l，这样就得到了下一个迭代点；γ∈(0,1]，通过一维搜索方法求取，使得x_l+1对应的目标函数值比x_l对应的目标函数值更优，该求取方法为成熟技术，执行步骤F1，l＝l+1；

所述的ODE求解模块采用四级五阶龙格库塔方法解微分方程初值问题，其计算公式为式(22)：

其中，t_i表示该方法选择的积分时刻；x^(k)(t_i)为机器人在第k次迭代中t_i时刻的状态信息，u^(k)为机器人在第k次迭代中t_i时刻的控制变量值，F为相应的微分方程，K₁、K₂、K₃、K₄分别为该方法在积分过程中的4个节点的函数值；h为积分步长，计算公式如下：

本发明能够根据Manutec R3机器人系统状况计算出当前实现最优运行的路径规划，最大限度地缩短机器人的工作时间，提高工作效率，该系统非常适用于复杂的机器人动态优化问题的在线优化。

附图说明

图1为Manutec R3型工业机器人机械本体的示意图；

图2为本发明的工作原理图；

图3为优化计算模块的执行流程图。

具体实施方式

实施例1，如图1所示，Manutec R3型工业机器人的运动过程的模型可以描述为式(24)：

对角度的约束如式(25)：

对角速度的约束如式(26)：

其中，关节变量q(t)＝[q₁(t),q₂(t),q₃(t)]^T是腰关节、肩关节、肘关节的相对夹角，单位为rad；v(t)＝[v₁(t),v₂(t),v₃(t)]^T为关节运动的速度，单位为rad/s；关节轴上的扭矩电压u(t)＝[u₁(t),u₂(t),u₃(t)]^T由对角矩阵D＝diag(-126.0,252.0,72.0)控制，单位为伏特。另外，P(q(t))是包含转动惯量的3×3正定对称质量矩阵，如式(27)：

其中c_i(i＝1,2,…,12)是常数，如表1所示。

表1 Manutec R3型工业机器人最优控制问题参数

表示由科里奥利和离心力引起的力矩，对于i＝1,2,3满足式(28)：

其中Γ_i,j,k(q)由式(29)表示：

χ^g(q(t))表示由引力造成的力矩，见式(30)：

其中b_i(i＝1,2)是常数，如式(31)：

b₁＝(12.1806+0.98mm)g b₂＝(41.7325+0.50mm)g (31)；

其中g＝9.81表示重力常数，mm＝15kg为运输负荷。

所述的系统如图2所示，执行步骤为：

步骤A1：由控制室工程师通过人机交互模块指定Manutec R3机器人腰关节、肩关节、肘关节变量的初始状态以及末端状态，凭借工程经验设定机器人完成作业的大致时间t_f为0.6s，指定扭矩电机的性能约束||u(t)||_∞≤7.5、|q₁(t)|≤2.87(rad)、|q₂(t)|≤2.01(rad)、|q₃(t)|≤2.86(rad)、|v₁(t)|≤3.0(rad/sec)、|v₂(t)|≤1.5(rad/sec)、|v₃(t)|≤5.2(rad/sec)。指定起始时间网格数N＝10、控制变量的初始值均为0、ε_l＝10^-6、ε_J＝10^-6、I_max＝6、w_min＝0.01、α＝0.1、β_ii(ii＝1,2,3,4)分别为0.05、0.1、1、5；

步骤A2：优化计算模块通过输入的参数，执行控制网格精细化方法的优化策略，计算出机器人最快完成相应动作所需的每个关节轴上的扭矩电压，通过现场总线网络发送给控制器的数模转换器；

步骤A3：控制器根据控制指令控制相应的驱动器，驱动器支配机器人每个关节的执行单元去完成规定的运动和功能；

步骤A4：位置传感器实时采集机器人手臂的位置信息，经过模数转换后用现场总线网络送给优化计算模块，可以实时更新控制策略，实现在线优化。

所述的优化计算模块，包括输入模块、初始化模块、最优控制形式转换模块、控制网格精细化模块、非线性规划问题求解模块、ODE求解模块、精细化收敛性判断模块、输出模块。其运行过程如图3所示，执行步骤为：

步骤B1：输入模块接收工程师通过人机交互系统输入的腰关节、肩关节、肘关节变量的初始状态，末端状态，最终时间t_f，扭矩电机的性能约束，时间网格数N，控制变量的初始值以及ε_l、ε_J、I_max、w_min、α、β_ii(ii＝1,2,…,nn)等参数信息；

步骤B2：执行初始化模块，根据最终时间t_f＝0.6s，起始时间网格数N＝10，采用均匀离散化的方式，计算出起始时间网格分布[0s、0.06s、0.12s、0.18s、0.24s、0.3s、0.36s、0.42s、0.48s、0.54s、0.6s]。初始化控制变量的初始值，初始化非线性规划问题求解模块的精度ε_l、迭代次数l＝0，设置精细化收敛性判断模块的精度ε_J、精细化迭代次数I＝0、精细化迭代次数上限I_max、最小时间网格宽度w_min、合并系数α、插入系数β_ii。

步骤B3：执行最优控制形式转换模块；

步骤B4：通过非线性规划问题求解模块求解非线性规划(NLP)问题，得到当前时间网格下的最优的控制参数和目标函数值，当l＝0时，跳过步骤B5，直接执行步骤B6；

步骤B5：运行精细化收敛性判断模块，若满足收敛条件，则算法终止，并执行输出模块；否则，执行下一步。

步骤B6：运行控制网格精细化模块，得到新的控制参数、新的时间网格分布和新的目标函数值J，精细化迭代次数I＝I+1，转入步骤B4。

所述的最优控制形式转换模块，执行步骤如下：

步骤C1：判断机器人最优控制问题的目标函数、等式约束、不等式约束是否都为Mayer形式，是的话跳出最优控制形式转换模块，执行步骤B4；否则，如果机器人最优控制问题的目标函数、等式约束、不等式约束中含有Lagrange形式的函数，则执行步骤C2；如果机器人最优控制问题的目标函数、等式约束、不等式约束中含有Bolza形式的函数，则执行步骤C3；

步骤C2：如果机器人最优控制问题的目标函数、等式约束、不等式约束中含有Lagrange形式的函数，则将其转换为不含积分项的Mayer形式的函数，引入新的状态变量x_n+1，并使其满足式(1)：

其中，u(t)是(n×1)维的控制变量，x(t)是(m×1)维的状态变量，L₀(t,x(t),u(t))为与机器人目标函数中积分项有关的函数，t为时间，t₀为起始时间，表示x_n+1(t)的导数。

则有式(2)：

经过上式的转换，目标函J和不等式约束以及等式约束中的积分项均可由新的状态变量在终端时刻值确定，如式(3)：

其中，X(t)为加入新的状态变量x_n+1(t)后的状态变量，f[t,x(t),u(t)]表示机器人动态特性的函数，L_r[t,X(t),u(t)]＝0为m₁个等式约束，L_r[t,X(t),u(t)]≥0为m₂个不等式约束。X(t₀)表示起始时间t₀时状态变量，和u分别表示控制变量的上下界约束。

步骤C3：如果机器人最优控制问题的目标函数、等式约束、不等式约束中含有Bolza形式的函数，则将其转换为不含积分项的Mayer形式的函数，引入新的状态变量x_n+1，并使其满足式(4)：

将目标函数J转换为式(5)：

其中，Φ₀[x(t_f),t_f]为终值项。通过Mayer形式转换过后，最优控制问题的Mayer形式表示如式(6)：

其中，X(t)为加入新的状态变量x_n+1后的状态变量，Φ_r[x(t_f)]表示与x(t_f)相关的函数项，Φ_r[x(t_f)]+L_r[t,X(t),u(t)]＝0为m₁个等式约束，Φ_r[x(t_f)]+L_r[t,X(t),u(t)]≥0为m₂个不等式约束。

所述的控制网格精细化模块，分别包括控制网格合并子模块、控制网格插入子模块、时间切换点定位子模块，执行步骤如下：

步骤D1：根据式(7)、式(8)计算当前的控制参数 对应的左斜率右斜率

其中，分别是时间区间[t_i-2,t_i-1]，[t_i-1,t_i]和[t_i,t_i+1]的控制参数，t_i(i＝0,1,…,N)为离散化后的时间网格节点，j表示控制变量u(t)第j个分量。

步骤D2：消除一些没有必要的时间节点来降低非线性规划问题维数。如果控制参数的左斜率与下一时间网格的控制参数的右斜率满足时间网格节点的合并准则，见式(9)：

其中，α为合并系数，那么就可消除t_i这个时间节点，合并时间子区间[t_i-1,t_i]和[t_i,t_i+1]，合并后的子区间[t_i-1,t_i+1]的控制参数的取值为控制参数和控制参数的平均值。

步骤D3：插入一些时间网格来提高逼近精度，定义时间网格节点细化规则如式(10)：

式中，βii(ii＝1,2,…,nn)为插入系数，Δk为插入时间节点的个数,nn表示插入系数个数。对于控制参数的左右斜率和如果其斜率平均值在[β₁,β₂)内，则在控制参数所属的时间网格[t_i,t_i+1]内的0.5(t_i+1-t_i)+t_i时刻插入一个时间节点；对于控制参数的左右斜率和如果斜率平均值在[β_nn-1,β_nn)，则在这个控制参数所属的时间网格[t_i,t_i+1]内均匀插入nn-1个时间节点。假设第b-1与第b个时间网格区间被细化分为Δk个小区间，对于i＝b，那么细化后的时间节点将被重新标记如式(11)：

对于i＝b+1,b+2,…N，那么细化后的时间节点将被重新标记为式(12)：

式中为重新标记后时间节点。

步骤D4：对重要时间切换点进行定位，为了更好地描述相邻两个控制参数的变化趋势，对于控制参数和下一个时间网格控制参数的中点连线定义一个斜率，称为中间斜率s_i(i＝1,2,…,N-1)，如式(13)：

对重要时间切换点的判断准则如式(14)：

s_i·s_i+1≤0 (14)；

则说明相邻的控制参数和呈现一个“转折”现象，这个转折点被认为是重要的时间切换点。若

|s_i|≤|s_i+1| (15)；

则将t_i+1作为待优化的时间节点与控制变量同时优化，若

|s_i|≥|s_i+1| (16)；

则将t_i作为待优化的时间节点与控制变量同时优化。

所述的精细化收敛性判断模块，执行步骤如下：

步骤E1：计算当前精细化迭代的目标函数值与前一次精细化迭代的目标函数值之差的绝对值是否满足设定的误差容限系数ε_J，如果满足式(17)，则跳到步骤E4；否则，执行步骤E2；

|J^(I+1)-J^(I)|＜ε_J (17)；

其中J^(I+1)和J^(I)分别代表第I+1次与第I次精细化迭代所得的目标函数值。

步骤E2：计算时间网格的宽度w_i＝t_t-t_i-1(i＝1,2,...,N)是否满足式(18)，如果满足下式，则跳到步骤E4；否则，执行步骤E3；

min{w_i}≤w_min i＝1,2,...,N (18)；

步骤E3：判断是否大于设定的迭代次数上限I_max，如果满足式(19)，则跳到步骤E4；否则，跳到步骤E5；

I＞I_max (19)；

步骤E4：满足收敛条件，则算法终止，并执行输出模块；

步骤E5：不满足收敛条件，执行步骤B6；

所述的非线性规划问题求解模块运行步骤，执行步骤如下：

步骤F1：通过ODE求解模块解微分方程初值问题，获取本次迭代的目标函数值和约束函数值；

步骤F2：NLP收敛性判断，如果l＝0，则认为不满足NLP收敛条件，直接执行步骤F3；如果l>0且步骤F1中本次迭代的目标函数值J^(l)与上一个迭代的目标函数值J^(l-1)的绝对值之差小于设定的精度ε_l，则认为满足NLP收敛，返回本次迭代得到的3个关节轴上的扭矩电压，执行步骤B5；否则执行步骤F3；

步骤F3：计算搜索方向矢量，将式(20)所示的非线性优化命题泰勒展开为如式(21)所示的QP子问题：

其中f(x)为一维连续可微目标函数，c(x)表示mu维连续可微约束方程。x表示nu维与机器人有关的变量，包括控制变量和状态变量，x^U和x^L分别表示变量的上下界约束。

x在迭代点x_l处对上式进行泰勒展开，并忽略高次项和目标函数中的常数项，则原命题转为求解以下QP子问题：

式中d_l为搜索方向矢量，g_l ^T和A_l ^T分别表示在x_l处目标函数f(x_l)的导数g_l的转置和约束方程c(x_l)的雅克比矩阵，c_l表示x_l处约束方程c(x_l)的值，W为拉格朗日函数的Hessian阵。通过求解式(21)，可获得搜索方向矢量的值d_l。

步骤F4：步长计算，令x_l+1＝x_l+γd_l，这样就得到了下一个迭代点。γ∈(0,1]，通过一维搜索方法求取，使得x_l+1对应的目标函数值比x_l对应的目标函数值更优，该求取方法为成熟技术，执行步骤F1，l＝l+1。

所述的ODE求解模块运行步骤，采用四级五阶龙格库塔方法，计算公式为式(22)：

其中，t_i表示该方法选择的积分时刻。x^(k)(t_i)为机器人在第k此迭代中第t_i时刻的状态信息，u^(k)为机器人在第k次迭代中t_i时刻的控制变量值，F为相应的微分方程，K₁、K₂、K₃、K₄分别为该方法在积分过程中的4个节点的函数值。h为积分步长，计算公式如下：

以上内容是结合具体的优选实施方式对本发明所作的进一步详细说明，不能认定本发明的具体实施只限于这些说明。对于本发明所属技术领域的普通技术人员来说，在不脱离发明构思的前提下，还可以做出若干简单推演或替换，都应当视为属于本发明的保护范围。

Claims (6)

1.一种Manutec R3型工业机器人动态优化系统，包括优化计算模块、控制器、数模转换器、模数转换器、驱动器、执行器、位置传感器、现场总线网络、人机交互模块、Manutec R3型工业机器人机械主体，其特征在于：系统运行过程具体包括以下步骤：

步骤A1：由控制室工程师通过人机交互模块指定Manutec R3机器人腰关节、肩关节、肘关节变量的初始状态以及末端状态，凭借工程经验设定机器人完成作业的大致时间t_f，指定扭矩电机的性能约束、起始时间网格数N个、控制变量的初始值，10≤N≤20，非线性规划问题求解模块的精度ε_l、精细化收敛性判断模块的精度ε_J，精细化迭代次数上限I_max、最小时间网格宽度w_min以及控制网格精细化模块的合并系数α和插入系数β_ii，ii＝1,2,…,nn；nn表示插入系数个数；

步骤A2：优化计算模块通过输入的参数，执行控制网格精细化模块的优化策略，计算出机器人最快完成相应动作所需的每个关节轴上的扭矩电压，通过现场总线网络发送给控制器的数模转换器；

步骤A3：控制器根据控制指令控制相应的驱动器，驱动器支配机器人每个关节的执行单元去完成规定的运动和功能；

步骤A4：位置传感器实时采集机器人手臂的位置信息，经过模数转换后用现场总线网络送给优化计算模块，可以实时更新控制策略，实现在线优化。

2.如权利要求1所述的一种Manutec R3型工业机器人动态优化系统，其特征在于：所述的优化计算模块，包括输入模块、初始化模块、控制网格精细化模块、非线性规划问题求解模块、精细化收敛性判断模块、输出模块；所述的优化计算模块执行的步骤如下：

步骤B1：输入模块接收工程师通过人机交互系统输入的腰关节、肩关节、肘关节变量的初始状态，末端状态，最终时间t_f，扭矩电机的性能约束，时间网格数N，控制变量的初始值以及ε_l、ε_J、I_max、w_min、α、β_ii参数信息；

步骤B2：执行初始化模块，根据最终时间t_f，起始时间网格数N，采用均匀离散化的方式，计算出起始时间网格分布；初始化控制变量的初始值，初始化非线性规划问题求解模块的精度ε_l、迭代次数l＝0，设置精细化收敛性判断模块的精度ε_J、精细化迭代次数I＝0、精细化迭代次数上限I_max、最小时间网格宽度w_min、合并系数α、插入系数β_ii；

步骤B3：执行最优控制形式转换模块；

步骤B4：通过非线性规划问题求解模块求解非线性规划问题，得到当前时间网格下的最优的控制参数和目标函数值，当l＝0时，跳过步骤B5，直接执行步骤B6；

步骤B5：运行精细化收敛性判断模块，若满足收敛条件，则算法终止，并执行输出模块；否则，执行下一步；

步骤B6：运行控制网格精细化模块，得到新的控制参数、新的时间网格分布和新的目标函数值J，精细化迭代次数I＝I+1，转入步骤B4。

3.如权利要求2所述的一种Manutec R3型工业机器人动态优化系统，其特征在于：所述的最优控制形式转换模块，执行步骤如下：

步骤C1：判断机器人最优控制问题的目标函数、等式约束、不等式约束是否都为Mayer形式，是的话跳出最优控制形式转换模块，执行步骤B4；否则，如果机器人最优控制问题的目标函数、等式约束、不等式约束中含有Lagrange形式的函数，则执行步骤C2；如果机器人最优控制问题的目标函数、等式约束、不等式约束中含有Bolza形式的函数，则执行步骤C3；

步骤C2：将机器人最优控制问题转换为不含积分项的Mayer形式的函数，引入新的状态变量x_n+1(t)，并使其满足式(1)：

其中，u(t)是(n×1)维的控制变量，x(t)是(m×1)维的状态变量，L₀(t,x(t),u(t))为与机器人目标函数中积分项有关的函数，t为时间，t₀为起始时间，表示x_n+1(t)的导数；

则有式(2)：

经过上式的转换，目标函数J和不等式约束以及等式约束中的积分项均可由新的状态变量在终端时刻值确定，如式(3):

其中，X(t)为加入新的状态变量x_n+1(t)后的状态变量，f[t,x(t),u(t)]表示机器人动态特性的函数，L_r[t,X(t),u(t)]＝0为m₁个等式约束，L_r[t,X(t),u(t)]≥0为m₂个不等式约束；X(t₀)表示起始时间t₀时状态变量，和u分别表示控制变量的上下界约束；

步骤C3：将机器人最优控制问题转换为不含积分项的Mayer形式的函数，同样引入新的状态变量x_n+1(t)，并使其满足式(4)：

将目标函数J转换为式(5)：

其中，Φ₀[x(t_f),t_f]为终值项；通过Mayer形式转换过后，最优控制问题的Mayer形式表示如式(6)：

其中，X(t)为加入新的状态变量x_n+1(t)后的状态变量，Φ_r[x(t_f)]表示与x(t_f)相关的函数项，Φ_r[x(t_f)]+L_r[t,X(t),u(t)]＝0为m₁个等式约束，Φ_r[x(t_f)]+L_r[t,X(t),u(t)]≥0为m₂个不等式约束。

4.如权利要求2所述的一种Manutec R3型工业机器人动态优化系统，其特征在于：所述的控制网格精细化模块，包括控制网格合并模块、控制网格插入模块、时间切换点定位模块，执行步骤如下：

步骤D1：根据式(7)、式(8)计算当前的控制参数 对应的左斜率右斜率

其中，分别是时间区间[t_i-2,t_i-1]，[t_i-1,t_i]和[t_i,t_i+1]的控制参数，t_i(i＝0,1,…,N)为离散化后的时间网格节点，j表示控制变量u(t)第j个分量；

步骤D2：消除一些没有必要的时间节点来降低非线性规划问题维数；如果控制参数的左斜率与下一时间网格的控制参数的右斜率满足时间网格节点的合并准则，见式(9)：

其中，α为合并系数，那么就可消除t_i这个时间节点，合并时间子区间[t_i-1,t_i]和[t_i,t_i+1]，合并后的子区间[t_i-1,t_i+1]的控制参数的取值为控制参数和控制参数的平均值；

步骤D3：插入一些时间网格来提高逼近精度，定义时间网格节点细化规则如式(10)：

式中，Δk为插入时间节点的个数，β_ii(ii＝1,2,…,nn)为用于表征控制参数变化快慢的插入系数，其值随着ii值的增大而增大，nn表示插入系数个数，一般为1-6之间的整数；对于控制参数的左右斜率和如果其斜率平均值在[β₁,β₂)内，则在控制参数所属的时间网格[t_i,t_i+1]内的0.5(t_i+1-t_i)+t_i时刻插入一个时间节点；对于控制参数的左右斜率和如果斜率平均值在[β_nn-1,β_nn)，则在这个控制参数所属的时间网格[t_i,t_i+1]内均匀插入nn-1个时间节点；假设第b-1与第b个时间网格区间被细化分为Δk个小区间，对于i＝b，那么细化后的时间节点将被重新标记如式(11)：

对于i＝b+1,b+2,…N，那么细化后的时间节点将被重新标记为式(12)：

式中为重新标记后时间节点；

步骤D4：对重要时间切换点进行定位；为了更好地描述相邻两个控制参数的变化趋势，对于控制参数和下一个时间网格控制参数的中点连线定义一个斜率，称为中间斜率s_i，i＝1,2,…,N-1，如式(13)：

对重要时间切换点的判断准则如式(14)：

s_i·s_i+1≤0 (14)；

则说明相邻的控制参数和呈现一个“转折”现象，这个转折点被认为是重要的时间切换点；若

|s_i|≤|s_i+1| (15)；

则将t_i+1作为待优化的时间节点与控制变量同时优化，若

|s_i|≥|s_i+1| (16)；

则将t_i作为待优化的时间节点与控制变量同时优化。

5.如权利要求2所述的一种Manutec R3型工业机器人动态优化系统，其特征在于：所述的精细化收敛性判断模块，执行步骤如下：

步骤E1：计算当前精细化迭代的目标函数值与前一次精细化迭代的目标函数值之差的绝对值是否满足设定的误差容限系数ε_J，如果满足式(17)，则跳到步骤E4；否则，执行步骤E2；

|J^(I+1)-J^(I)|＜ε_J (17)；

其中J^(I+1)和J^(I)分别代表第I+1次与第I次精细化迭代所得的目标函数值；

步骤E2：计算时间网格的宽度w_i＝t_t-t_i-1(i＝1,2,...,N)是否满足式(18)，如果满足下式，则跳到步骤E4；否则，执行步骤E3；

min{w_i}≤w_min i＝1,2,...,N (18)；

步骤E3：判断是否大于设定的迭代次数上限I_max，如果满足式(19)，则跳到步骤E4；否则，跳到步骤E5；

I＞I_max (19)；

步骤E4：满足收敛条件，则算法终止，并执行输出模块；

步骤E5：不满足收敛条件，执行步骤B6。

6.如权利要求2所述的一种Manutec R3型工业机器人动态优化系统，其特征在于：所述的非线性规划问题求解模块运行步骤，执行步骤如下：

步骤F1：通过ODE求解模块解微分方程初值问题，获取本次迭代的目标函数值和约束函数值；

步骤F2：NLP收敛性判断，如果l＝0，则认为不满足NLP收敛条件，直接执行步骤F3；如果l>0且步骤F1中本次迭代的目标函数值J^(l)与上一个迭代的目标函数值J^(l-1)的绝对值之差小于设定的精度ε_l，则认为满足NLP收敛，返回本次迭代得到的3个关节轴上的扭矩电压，执行步骤B5；否则执行步骤F3；

步骤F3：计算搜索方向矢量，将式(20)所示的非线性优化命题泰勒展开为如式(21)所示的QP子问题：

其中f(x)为一维连续可微目标函数，c(x)表示mu维连续可微约束方程；x表示nu维与机器人有关的变量，包括控制变量和状态变量，x^U和x^L分别表示变量的上下界约束；

x在迭代点x_l处对上式进行泰勒展开，并忽略高次项和目标函数中的常数项，则原命题转化为以下QP子问题：

式中d_l为搜索方向矢量，g_l ^T和A_l ^T分别表示在x_l处目标函数f(x_l)的导数g_l的转置和约束方程c(x_l)的雅克比矩阵，c_l表示x_l处约束方程c(x_l)的值，W为拉格朗日函数的Hessian阵；通过求解式(21)，可获得搜索方向矢量的值d_l；

步骤F4：步长计算，令x_l+1＝x_l+γd_l，这样就得到了下一个迭代点；γ∈(0,1]，通过一维搜索方法求取，使得x_l+1对应的目标函数值比x_l对应的目标函数值更优，该求取方法为成熟技术，执行步骤F1，l＝l+1；

所述的ODE求解模块采用四级五阶龙格库塔方法解微分方程初值问题，其计算公式为式(22)：

其中，t_i表示该方法选择的积分时刻；x^(k)(t_i)为机器人在第k次迭代中t_i时刻的状态信息，u^(k)为机器人在第k次迭代中t_i时刻的控制变量值，F为相应的微分方程，K₁、K₂、K₃、K₄分别为该方法在积分过程中的4个节点的函数值；h为积分步长，计算公式如下：