CN104020668A - 一种不确定时延条件下机械臂运动状态的预测方法及装置 - Google Patents

Abstract

本发明公开一种不确定时延条件下机械臂运动状态的预测方法，包括：对于机械臂的各关节，建立预测模型及对应的离散模型；计算下行时延的估计值，并利用各关节的预测模型，计算各关节的运动状态的预测数据；根据所述运动状态的预测数据，和接收自位于远端的机械臂的实测数据，对所述机械臂各关节的预测模型对应的离散模型进行修正；并利用修正后的离散模型，得到当前时刻修正后的机械臂各关节运动状态的预测值。本发明实施例还公开一种不确定时延条件下机械臂运动状态的预测装置。采用本发明实施例，能够提高机械臂运动状态的精确度，解决在不确定时延条件系统的稳定性问题。

Description

一种不确定时延条件下机械臂运动状态的预测方法及装置

技术领域

本发明涉及机器人领域，特别涉及一种不确定时延条件下机械臂运动状态的预测方法及装置。 

背景技术

人类通过远程控制机械臂执行危险任务或恶劣环境下的任务是当前机器人控制发展的重要方向，远程操作与现场操作的一个重要区别就在于时间延迟问题。 

按时延值的变化与否可将时延分为定时延和变时延。定时延一般为信息由于物理原因(如空间跨度、传输/处理介质、速度差)等不可抗因素在传输、处理过程中引起的时间延迟；变时延一般为由于传输、交互策略等因素引起的时间延迟。理论上变时延有优化的可能，在实际中对变时延的优化受到技术水平的限制。 

一方面，在远程控制机械臂中，由于时延的存在，操作者对远端环境的感知是滞后于当前时刻T₀的。在滞后的时间段内，远程工作环境可能已经发生了变化，而操作者仍然基于T₀时刻的反馈信息做出决策，由此使得该决策可能有误，造成远程操作指令失效，严重的情况下甚至导致不可逆损失。 

另一方面，操作者基于T₀时刻的反馈信息发出的控制命令传送到远端的过程中，也可能出现延时，而此时机械臂和远端的环境状态也可能发生了新的变化。 

由此可见，时延的存在不仅影响了操作者对远端环境的正确感知，还有可能导致闭环系统的不稳定，从而严重地降低了系统的操作性。 

现有技术中，可以采用引入预测模型的方式来消除时延对机械臂控制的影响。其中，该预测模型用于对机械臂的运动状态进行预测。 

但是，由于机械臂自身和外部环影响等因素的存在，现有的预测模型对机械臂的运动状态的预测总是会存在一定的偏差。对于无时延条件下，该偏差可以被忽略。但是，对于大时延条件下，该偏差会随着时间不断累积，对系统的稳定性造成严重影响。 

因此，如何增强机械臂的运动状态预测模型的准确度，以解决时延造成的系统不稳定问题，是本领域技术人员急需解决的。 

发明内容

本发明要解决的技术问题就是克服现有技术的缺陷，提出一种不确定时延条件下机械臂运动状态的预测方法及装置，以解决无时标情况下时延修正的问题。 

第一方面，公开一种不确定时延条件下机械臂运动状态的预测方法，包括： 

步骤1，对于机械臂的各关节，分别建立预测模型以及与各关节的预测模型对应的离散模型； 

步骤2，计算机械臂控制回路的下行时延的估计值；其中，所述下行时延是指位于远端的机械臂的运动状态数据传递至控制端的过程中发生的时延； 

步骤3，控制端向位于远端的机械臂发送控制指令，并接收所述机械臂返回的响应该控制指令的各关节的运动状态的实测数据； 

步骤4，根据所述控制回路的下行时延的估计值，结合各关节的预测模型，计算得到该机械臂的各关节的运动状态的预测数据； 

步骤5，根据所述机械臂各关节的运动状态的实测数据和预测数据，使用最小二乘法对所述机械臂各关节的预测模型对应的离散模型进行修正。 

步骤6，利用所述修正后的离散模型，对机械臂各关节的运动状态进行预测，获取当前时刻修正后的机械臂各关节运动状态的预测值。 

在第一方面的第一种可能的实现方式中， 

所述预测模型为： $G{\left(s\right)}_i=\frac{1}{T_1^i{s}^2+T_2^i{s}^2+1}$

其中，表示关节i对应的预测模型的参数T₁，表示关节i对应的预测模型的参数T₂； 

所述离散模型为： $y^i\left(k\right)=p_{11}^i\left(k\right)y^i(k-1)+p_{12}^i\left(k\right)y^i(k-2)+p_{13}^i\left(k\right)u^i(k-1)$

其中，分别为关节i对应的预测模型离散化后的三个参数；yⁱ(k)为关节i的运动状态的当前k时刻预测值，yⁱ(k-1)为关节i的运动状态的(k-1)时刻预测值，yⁱ(k-2)为关节i的运动状态的(k-2)时刻预测值，uⁱ(k-1)为(k-1)时刻的指令输入值。 

结合第一方面的第一种可能的实现方式，在第一方面的第二种可能的实现方式中，步骤5中，所述使用最小二乘法对所述机械臂各关节的预测模型对应的离散模型进行修正，包括： 

在无时延环境下，控制端对机械臂的运行状态进行修正测试，获取和的变动范围； 

利用所述和的变动范围，对和的取值范围进行修正： 

$\left\{\begin{array}{c}{p}_{11}^i=2-\mathrm{\Δt}\frac{T_2^i}{T_1^i}\\ p_{12}^i=\mathrm{\Δt}\frac{T_2^i}{T_1^i}-1-\frac{{\mathrm{\Δt}}^2}{T_1^i}\end{array}\right.$

其中，为大于0的实数。 

根据和的取值范围，当经过最小二乘法修正的分别落在对应的和的取值范围内时，修正结果成立，当未落在对应的取值范围内时，取边界值为修正结果。例如的取值范围经过计算为  分别为的边值，当经过最小二乘法修正的时，采用最小二乘法修正的结果，当时，如果则如果 $p_{11}^i\left(k\right)<a_{11}^i$ 则 $p_{11}^i\left(k\right)=a_{11}^i.$ 同理类推

在第一方面的第三种可能的实现方式中，所述步骤2包括： 

步骤20，在位于远端的机械臂位于静止状态下，控制端发送第一组控制指令序列至机械臂，该控制指令序列用于控制位于远端的机械臂运动； 

步骤21，控制端预测所述机械臂的第一组理想运动状态，其中，所述第 一组理想运动状态是指在控制器无时延环境下针对所述第一组控制指令序列预测得到的机械臂的运动状态； 

步骤22，控制端实时获取所述机械臂的第一组实际运动状态； 

步骤23，对所述机械臂的第一组理想运动状态和第一组实际运动状态进行波形匹配，获取控制回路第一组总时延；其中，所述控制回路第一组总时延为从所述控制端发送第一组控制指令序列至所述控制端检测到所述机械臂响应该第一组控制指令序列所经历的时间； 

步骤24，在位于远端的机械臂响应完所述控制端的第一组控制指令序列并重新恢复静止状态下，控制端发出第二组控制指令序列至机械臂；其中，所述第二组控制指令序列与第一组控制指令序列相比，所述第二组控制指令序列的顺序执行时间为所述控制回路第一组总时延的2倍； 

步骤25，当控制端的第二组指令序列发送完毕后，所述控制端向位于远端的机械臂发出停止指令；所述停止指令用于要求所述机械臂以当前状态停止动作； 

步骤26，控制端再次预测所述机械臂的第二组理想运动状态，其中，该第二组理想运动状态是指控制器在无时延环境下针对所述第二组控制指令序列预测得到的机械臂的运动状态； 

步骤27，控制端实时获取当前所述机械臂的第二组实际运动状态； 

步骤28，再次对所述机械臂的第二组理想运动状态和第二组实际运动状态进行波形匹配，获取控制回路第二组总时延和停止时间差； 

其中，所述控制回路第二组总时延为从所述控制端发送第二组控制指令序列至所述控制端检测到所述机械臂响应该第二组控制指令序列所经历的时间；所述停止时间差等于所述机械臂的理想停止时间和实际停止时间的差值； 

步骤29，所述机械臂控制回路的下行时延估计值等于所述控制回路第二组总时延减去所述上行时延估计值的差值；所述上行时延估计值等于所述停止时间差。 

在第一方面的第四种可能的实现方式中，所述步骤4包括： 

步骤41，利用所述机械臂各关节的预测模型计算在无时延环境下，该机械臂的各关节的运动状态理想值； 

步骤42，结合所述下行时延的估计值，将所述各关节的运动状态理想值延迟该下行时延的估计值，得到在该下行时延环境下，该机械臂各关节的运动状态的预测数据。 

第二方面，公开一种不确定时延条件下的机械臂运动状态的预测装置，包括： 

模型建立单元，用于对于机械臂的各关节，分别建立预测模型以及与各关节的预测模型对应的离散模型； 

第一计算单元，用于计算机械臂控制回路的下行时延的估计值；其中，所述下行时延是指位于远端的机械臂的运动状态数据传递至控制端的过程中发生的时延； 

接收发送单元，用于控制端向位于远端的机械臂发送控制指令，并接收所述机械臂返回的响应该控制指令的各关节的运动状态的实测数据； 

第二计算单元，用于根据所述控制回路的下行时延的估计值，结合各关节的预测模型，计算得到该机械臂的各关节的运动状态的预测数据； 

模型修正单元，用于根据所述机械臂各关节的运动状态的实测数据和预测数据，使用最小二乘法对所述机械臂各关节的预测模型对应的离散模型进行修正。 

状态预测单元，用于利用所述修正后的离散模型，对机械臂各关节的运动状态进行预测，获取当前时刻修正后的机械臂各关节运动状态的预测值。 

在第二方面的第一种可能的实现方式中， 

所述预测模型为： $G{\left(s\right)}_i=\frac{1}{T_1^i{s}^2+T_2^i{s}^2+1}$

其中，表示关节i对应的预测模型的参数T₁，表示关节i对应的预测模型的参数T₂； 

所述离散模型为： $y^i\left(k\right)=p_{11}^i\left(k\right)y^i(k-1)+p_{12}^i\left(k\right)y^i(k-2)+p_{13}^i\left(k\right)u^i(k-1)$

其中，分别为关节i对应的预测模型离散化后的三个参数；yⁱ(k)为关节i的运动状态的当前k时刻预测值，yⁱ(k-1)为关节i的运动 状态的(k-1)时刻预测值，yⁱ(k-2)为关节i的运动状态的(k-2)时刻预测值，uⁱ(k-1)为(k-1)时刻的指令输入值。 

结合第二方面的第一种可能的实现方式，在第二方面的第二种可能的实现方式中，所述模型修正单元包括： 

测试子单元，用于在无时延环境下，控制端对机械臂的运行状态进行修正测试，获取和的变动范围； 

修正子单元，用于利用所述和的变动范围，对和的取值范围进行修正： 

$\left\{\begin{array}{c}{p}_{11}^i=2-\mathrm{\&Delta;t}\frac{T_2^i}{T_1^i}\\ p_{12}^i=\mathrm{\&Delta;t}\frac{T_2^i}{T_1^i}-1-\frac{{\mathrm{\&Delta;t}}^2}{T_1^i}\end{array}\right.$

其中，为大于0的实数。 

根据和的取值范围，当经过最小二乘法修正的分别落在对应的和的取值范围内时，修正结果成立，当未落在对应的取值范围内时，取边界值为修正结果。例如的取值范围经过计算为  分别为的边值，当经过最小二乘法修正的时，采用最小二乘法修正的结果，当时，如果则如果 $p_{11}^i\left(k\right)<a_{11}^i$ 则 $p_{11}^i\left(k\right)=a_{11}^i.$ 同理类推

在第二方面的第三种可能的实现方式中，所述第一计算单元包括： 

第一发送子单元，用于在位于远端的机械臂位于静止状态下，控制端发送第一组控制指令序列至机械臂，该控制指令序列用于控制位于远端的机械臂运动； 

第一预测子单元，用于控制端预测所述机械臂的第一组理想运动状态，其中，所述第一组理想运动状态是指在控制器无时延环境下针对所述第一组控制指令序列预测得到的机械臂的运动状态； 

第一获取子单元，用于控制端实时获取所述机械臂的第一组实际运动状态； 

第一匹配子单元，用于对所述机械臂的第一组理想运动状态和第一组实际运动状态进行波形匹配，获取控制回路第一组总时延；其中，所述控制回路第一组总时延为从所述控制端发送第一组控制指令序列至所述控制端检测到所述机械臂响应该第一组控制指令序列所经历的时间； 

第二发送子单元，用于在位于远端的机械臂响应完所述控制端的第一组控制指令序列并重新恢复静止状态下，控制端发出第二组控制指令序列至机械臂；其中，所述第二组控制指令序列与第一组控制指令序列相比，所述第二组控制指令序列的顺序执行时间为所述控制回路第一组总时延的2倍； 

第三发送子单元，用于当控制端的第二组指令序列发送完毕后，所述控制端向位于远端的机械臂发出停止指令；所述停止指令用于要求所述机械臂以当前状态停止动作； 

第二预测子单元，用于控制端再次预测所述机械臂的第二组理想运动状态，其中，该第二组理想运动状态是指控制器在无时延环境下针对所述第二组控制指令序列预测得到的机械臂的运动状态； 

第二获取子单元，用于控制端实时获取当前所述机械臂的第二组实际运动状态； 

第二匹配子单元，用于再次对所述机械臂的第二组理想运动状态和第二组实际运动状态进行波形匹配，获取控制回路第二组总时延和停止时间差；其中，所述控制回路第二组总时延为从所述控制端发送第二组控制指令序列至所述控制端检测到所述机械臂响应该第二组控制指令序列所经历的时间；所述停止时间差等于所述机械臂的理想停止时间和实际停止时间的差值； 

第一计算子单元，用于计算所述机械臂控制回路的下行时延估计值，所述下行时延估计值等于所述控制回路第二组总时延减去所述上行时延估计值的差值；所述上行时延估计值等于所述停止时间差。 

在第二方面的第四种可能的实现方式中，所述第二计算单元包括： 

第二计算子单元，用于利用所述机械臂各关节的预测模型计算在无时延环境下，该机械臂的各关节的运动状态理想值； 

延迟子单元，用于结合所述下行时延的估计值，将所述各关节的运动状 态理想值延迟该下行时延的估计值，得到在该下行时延环境下，该机械臂各关节的运动状态的预测数据。 

本发明具有以下积极的效果： 

本发明实施例所述方法，用于对机械臂各关节的预测模型对应的离散模型进行修正，利用修正后的离散模型预测各关节的运动状态，从而提高机械臂运动状态的精确度，解决在不确定时延条件系统的稳定性问题。 

采用本发明实施例，对于未知的时延情况，通过对下行时延值进行估计，并且在修正方法中加入限定规则，从而实现在无时标条件下，面对不确定时延环境对远方机械臂的运动状态进行有效预测，从而增强了机械臂的运动状态预测模型的准确度，实现稳定连续的遥操作。 

附图说明

图1为典型的机械臂的关节电机的闭环控制模型图； 

图2为本发明实施例的不确定时延条件下机械臂运动状态的预测方法的流程图； 

图3为机械臂的某一关节的预测模型的修正示意图； 

图4为本发明实施例的不确定时延条件下的机械臂运动状态的预测装置的结构图。 

具体实施方式

下文中将结合附图对本发明的实施例进行详细说明。需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互任意组合。 

对于机械臂，一般包括若干个关节。在本发明中，每个关节对应一个预测模型，该预测模型用于预测对应的关节的运动状态。本发明实施例所述的不确定时延条件下的机械臂运动状态的修正方法，用于对机械臂各关节的运动状态分别进行修正，以增强各关节分别对应的预测模型预测得到的该关节的运动状态的精度，从而解决在不确定时延条件系统的稳定性问题。 

参照图1，为典型的机械臂的关节电机的闭环控制模型图。如图1所示，对于该关节电机驱动的机械臂关节，在以下6个假设条件下，该闭环模型的 传递函数如式(1)所示。 

其中，6个假设条件分别为： 

(1)关节电机的磁饱和效应忽略不计； 

(2)忽略关节电机的铁心磁滞、涡流的影响； 

(3)关节电机的各相均匀分布，气隙均匀； 

(4)关节电机的相绕组电压的通断是瞬间完成的，即其控制电压为阶跃式，为恒定电压驱动电源； 

(5)关节电机的负载是有一定惯量的干摩擦型； 

(6)关节电机的各项绕阻的电阻、电感分别相等且为常数。 

如图1中，G_c(s)为控制器的传递函数，k_b为反馈系数。 

当不采用测速发电机时，k_b＝k_e；当采用测速发电机时，k_b＝k_e+k_sk_ω；其中，k_e为旋转电压系数，k_s为测速发电机的传递函数，k_ω为速度反馈信号放大器的增益。η为传动比，η＝N_m/N_L，N_m和N_L分别为传动轴和负载轴上的齿轮齿数。 

对图1所示，当采用PID(Proportion-Integration-Differentiation，比例-积分-微分)控制器时，该闭环模型的关节角输出的传递函数为： 

$\begin{array}{c}{\mathrm{\&Theta;}}_y\left(s\right)=\frac{k_T\mathrm{\&eta;}(k_D{s}^2+k_{P}s+k_I)}{{\mathrm{LJs}}^4+(\mathrm{LB}+\mathrm{RJ})s^3+(\mathrm{RB}+k_D{k}_T\mathrm{\&eta;}+k_b{k}_T)s^2+k_P{k}_T\mathrm{\&eta;s}+k_I{k}_T\mathrm{\&eta;}}{\mathrm{\&Theta;}}_r\left(s\right)\\ +\frac{\mathrm{\&eta;}({\mathrm{Ls}}^2+\mathrm{Rs})}{{\mathrm{LJs}}^4+(\mathrm{LB}+\mathrm{RJ})s^3+(\mathrm{RB}+k_D{k}_T\mathrm{\&eta;}+k_b{k}_T)s^2+k_P{k}_T\mathrm{\&eta;s}+k_I{k}_T\mathrm{\&eta;}}{T}_d\left(s\right)\end{array}---\left(1\right)$

其中，PID控制器的传递函数为： 

$G_c\left(s\right)=k_P+\frac{k_I}{s}+k_{D}s---\left(2\right)$

对于上述模型，对于机械臂的关节i，其对应的预测模型为二阶模型，如式(3)所示： 

$G{\left(s\right)}_i=\frac{1}{T_1^i{s}^2+T_2^i{s}^2+1}---\left(3\right)$

其中，表示关节i对应的预测模型的参数T₁，表示关节i对应的预测模型的参数T₂，对式(3)进过离散化后，关节i对应的预测模型的递推式为： 

$y^i\left(k\right)=p_{11}^i\left(k\right)y^i(k-1)+p_{12}^i\left(k\right)y^i(k-2)+p_{13}^i\left(k\right)u^i(k-1)---\left(4\right)$

其中，分别为关节i对应的预测模型离散化后的三个参数。 

结合式(4)，yⁱ(k)为关节i的运动状态的当前k时刻预测值，yⁱ(k-1)为关节i的运动状态的(k-1)时刻预测值，yⁱ(k-2)为关节i的运动状态的(k-2)时刻预测值，uⁱ(k-1)为(k-1)时刻的指令输入值。 

根据离散化理论，在当前k时刻，与对应的修正模型 中的和之间的对应关系如式(5)。 

$\left\{\begin{array}{c}{p}_{11}^i=2-\mathrm{\&Delta;t}\frac{T_2^i}{T_1^i}\\ p_{12}^i=\mathrm{\&Delta;t}\frac{T_2^i}{T_1^i}-1-\frac{{\mathrm{\&Delta;t}}^2}{T_1^i}\end{array}\right.\&DoubleRightArrow;\left\{\begin{array}{c}{T}_1^i=\frac{{\mathrm{\&Delta;t}}^2}{(1-p_{11}^i+p_{12}^i)}\\ T_2^i=\frac{(2-p_{11}^i)\mathrm{\&Delta;t}}{(1-p_{11}^i+p_{12}^i)}\end{array}\right.---\left(5\right)$

式中，Δt为递推过程中的采样间隔。由式(5)可知，由于时标错位，得到的测量值序列不能反映或错误反映远端现场的响应过程。由于最小二乘方法是从数学上保证在接收数据前后小段值的相似性，有可能导致最小二乘方法拟合的瞬时参数所对应的瞬时预测模型参数和为反号，或负值。在稳定性理论中，瞬时预测模型参数和出现这种情况时，对应的预测模型是一个不稳定的模型，以该瞬时模型预测10s～20s以后的状态，其结果明显会与现实天差地别。除此之外，如果得出的瞬时接近于零或很大，其分别对应的是高震荡系统和高阻尼系统，这种情况对预测的误差也会造成较大影响。 

需要注意的是，上述三个情况对于一个良好受控的对象是不应当出现的，特别是对于航天任务，在控制器的设计和载荷大幅变化条件下的适应性是会有相当完备的优化。因此对于现实条件，瞬时的和的符号以及范围应当满足某些区域约束。这个约束条件的使用，对无时标下的在线修正至关重要。 

最小二乘法修正方法是可用于无时延或者固定时延条件下的，基于邮戳准则的方法，以最小二乘法为基础，可以用于有时标条件下的不确定时延处理，但是只能处理下行不确定时延。上述方法均不能实现无时标条件下的修 正。 

针对上述问题，本发明实施例提供一种不确定时延条件下的机械臂运动状态修正方法。 

参照图2，为本发明实施例的不确定时延条件下机械臂运动状态的预测方法的流程图。如图2所示，所述方法可以包括如下步骤： 

步骤101，对于机械臂的各关节，分别建立预测模型以及与各关节的预测模型对应的离散模型。 

结合本发明实施例前述内容可知，所述各关节的预测模型为： 

$G{\left(s\right)}_i=\frac{1}{T_1^i{s}^2+T_2^i{s}^2+1}---\left(3\right)$

所述各关节的预测模型对应的离散模型为： 

$y^i\left(k\right)=p_{11}^i\left(k\right)y^i(k-1)+p_{12}^i\left(k\right)y^i(k-2)+p_{13}^i\left(k\right)u^i(k-1)---\left(4\right)$

步骤102，计算机械臂控制回路的下行时延的估计值和上行时延的估计值；其中，所述下行时延是指位于远端的机械臂的运动状态数据传递至控制端的过程中发生的时延；所述上行时延是指控制端发送控制指令到机械臂接收该控制指令的过程中发生的时延。 

具体地，步骤102可以包括如下步骤： 

步骤102a，在位于远端的机械臂位于静止状态下，控制端发送第一组控制指令序列至机械臂，该控制指令序列用于控制位于远端的机械臂运动。 

步骤102b，控制端预测所述机械臂的第一组理想运动状态，其中，所述第一组理想运动状态是指在控制器无时延环境下针对所述第一组控制指令序列预测得到的机械臂的运动状态。 

步骤102c：控制端实时获取所述机械臂的第一组实际运动状态。其中，所述实际运动状态可以通过设置在机械臂上的传感器来获取。 

步骤102d：对所述机械臂的第一组理想运动状态和第一组实际运动状态进行波形匹配，获取控制回路第一组总时延t1；其中，所述控制回路第一组总时延t1为从所述控制端发送第一组控制指令序列至所述控制端检测到所述机械臂响应该第一组控制指令序列所经历的时间。 

步骤102e：在位于远端的机械臂响应完所述控制端的第一组控制指令序 列，重新恢复静止状态下，控制端发出第二组控制指令序列至机械臂，要求远端机械臂从所述第二组指令发出的起始时刻起，延迟t10时刻开始执行；其中，所述第二组控制指令序列与第一组控制指令序列相比，所述第二组控制指令序列的顺序执行时间为t1的2倍，且所述要求的开始执行的延迟t10也为t1的2倍。 

步骤102f：当控制端的第二组指令序列发送完毕后，在所述第二组指令发出的起始时刻延时t10后，所述控制端向位于远端的机械臂发出停止指令；所述停止指令用于要求所述机械臂在接收该指令后立即以当前状态停止动作。 

步骤102g：控制端再次预测所述机械臂的第二组理想运动状态，其中，该第二组理想运动状态是指控制器在无时延环境下针对所述第二组控制指令序列预测得到的机械臂的运动状态。 

步骤102h：控制端实时获取当前所述机械臂的第二组实际运动状态。 

步骤102i：再次对所述机械臂的第二组理想运动状态和第二组实际运动状态进行波形匹配，获取控制回路第二组总时延t2和停止时间差t3。 

其中，所述控制回路第二组总时延t2为从所述控制端发送第二组控制指令序列至所述控制端检测到所述机械臂响应该第二组控制指令序列所经历的时间。 

所述停止时间差t3等于所述机械臂的理想停止时间和实际停止时间的差值。其中，所述理想停止时间是指在无时延环境下预测得到的所述机械臂响应步骤102f中所述的停止指令的时间；所述实际停止时间是指所述机械臂实际响应该停止指令的时间。 

步骤102j：所述机械臂控制回路的下行时延估计值等于所述控制回路第二组总时延t2减去上行时延的差值；其中，所述上行时延等于步骤102i中得到的所述停止时间差t3。 

需要说明的是，本发明实施例所述方法中，可以多次重复上述步骤102a至102j，从而计算得到多组下行时延和上行时延的值，然后通过分别对所述多组下行时延和多组上行时延的值求平均，从而提高计算得到的下行时延的 估计值和上行时延的估计值的精确度。 

步骤103，控制端向位于远端的机械臂发送控制指令，并接收所述机械臂返回的响应该控制指令的各关节的运动状态的实测数据。其中，所述控制指令中包含控制端指定所述机械臂响应该控制指令的时刻的信息，所述时刻具体为所述控制端发送该控制指令的时间加上所述上行时延的时间的n倍(n为大于1的正数)。 

步骤104，根据所述控制回路的下行时延的估计值，结合各关节的预测模型，计算得到该机械臂的各关节的运动状态的预测数据。 

具体地，在步骤104中，首先利用所述预测模型计算在无时延环境下，该机械臂的各关节的的运动状态理想值，然结合所述下行时延的估计值，将所述各关节的运动状态的理想值延迟该下行时延的估计值，得到在该下行时延环境下，该机械臂各关节的运动状态的预测数据。 

步骤105，根据所述机械臂各关节的运动状态的实测数据和预测数据，使用最小二乘法对所述机械臂各关节的预测模型对应的离散模型进行修正。 

需要说明的是，本发明实施例中，采用最小二乘法对所述机械臂各关节的预测模型对应的离散模型进行修正时，首先需要限定公式(4)中的和的修正值范围。 

其中，所述公式(4)中的和的修正值范围可以通过以下步骤获得： 

步骤105a，在无时延环境下，控制端对机械臂的运行状态进行修正测试，获取和的变动范围。 

步骤105b，利用所述和的变动范围，对和的取值范围进行修正具体为： 

$\left\{\begin{array}{c}{p}_{11}^i=2-\mathrm{\&Delta;t}\frac{T_2^i}{T_1^i}\\ p_{12}^i=\mathrm{\&Delta;t}\frac{T_2^i}{T_1^i}-1-\frac{{\mathrm{\&Delta;t}}^2}{T_1^i}\end{array}\right.---\left(5\right)$

为大于0的实数。 

根据和的取值范围，当经过最小二乘法修正的 分别落在对应的和的取值范围内时，修正结果成立，当未落在对应的取值范围内时，取边界值为修正结果。例如的取值范围经过计算为  分别为的边值，当经过最小二乘法修正的时，采用最小二乘法修正的结果，当时，如果则如果 $p_{11}^i\left(k\right)<a_{11}^i$ 则 $p_{11}^i\left(k\right)=a_{11}^i.$ 同理类推

步骤106，根据所述修正后的和所构成的离散预测模型，使用延时后的数据，对机械臂各关节的运动状态进行预测，获取当前时刻修正后的机械臂各关节运动状态的预测值，并以此作为操作端的状态反馈至控制人员。 

本发明实施例所述方法，对于未知的时延情况，通过对下行时延值进行估计，并且在修正方法中加入限定规则，从而实现在无时标条件下，面对不确定时延环境对远方机械臂的运动状态进行有效预测，从而增强了机械臂的运动状态预测模型的准确度，实现稳定连续的遥操作。 

本发明实施例所述方法的应用特点是，对于已部署在太空、深海或者恶劣环境(如放射性区域)等现场维护成本极高的机械臂，而部署时因设计理论不完善而未能提供时标信息，采用本方法可以仅在操作端进行升级而无需机械臂端维护，即可提高整个系统的相应性能。 

由上述内容可见，本发明实施例所述方法，通过对回路时延和上/下行时延的分别估计，并且引入了物理约束限定了修正计算中的数学陷阱，通过这两个手段，可以有效的在在无时标情况下进行快速而正确的修正，从而达到准确预报和实施连续操作的目的。 

本发明所述方法在步骤104中，采用在线预测修正，如图3所示，为机械臂的某一关节的预测模型的修正示意图。离散系统的输入输出模型可以用标准的差分方程描述。其中，图3中： 

U为系统输入，U＝[u₁,u₂,u₃,…,u_m]^Τ(m为输入维数)， 

Y为真实对象的输出，Y＝[y₁,y₂,y₃,…,y_n]^Τ(n为输出维数)， 

为预测模型的预测输出，(n为输出维数)， 

ξ为白噪声， 

F为真实对象， 

为预测模型。 

对应于本发明实施例提供的不确定时延条件下的机械臂运动状态的预测方法，本发明实施例还提供一种不确定时延条件下的机械臂运动状态的预测装置。 

参照图4，为本发明实施例所述的不确定时延条件下的机械臂运动状态的预测装置的结构图。如图4所示，所述装置可以包括： 

所述模型建立单元401，用于对于机械臂的各关节，分别建立预测模型以及与各关节的预测模型对应的离散模型； 

所述第一计算单元402，用于计算机械臂控制回路的下行时延的估计值；其中，所述下行时延是指位于远端的机械臂的运动状态数据传递至控制端的过程中发生的时延； 

所述接收发送单元403，用于控制端向位于远端的机械臂发送控制指令，并接收所述机械臂返回的响应该控制指令的各关节的运动状态的实测数据； 

所述第二计算单元404，用于根据所述控制回路的下行时延的估计值，结合各关节的预测模型，计算得到该机械臂的各关节的运动状态的预测数据； 

所述模型修正单元405，用于根据所述机械臂各关节的运动状态的实测数据和预测数据，使用最小二乘法对所述机械臂各关节的预测模型对应的离散模型进行修正。 

所述状态预测单元406，用于利用所述修正后的离散模型，对机械臂各关节的运动状态进行预测，获取当前时刻修正后的机械臂各关节运动状态的预测值。 

本发明实施例，用于对机械臂各关节的预测模型对应的离散模型进行修正，利用修正后的离散模型预测各关节的运动状态，从而提高机械臂运动状态的精确度，解决在不确定时延条件系统的稳定性问题。 

采用本发明实施例，对于未知的时延情况，通过对下行时延值进行估计，并且在修正方法中加入限定规则，从而实现在无时标条件下，面对不确定时延环境对远方机械臂的运动状态进行有效预测，从而增强了机械臂的运动状态预测模型的准确度，实现稳定连续的遥操作。 

优选的，所述预测模型为： $G{\left(s\right)}_i=\frac{1}{T_1^i{s}^2+T_2^i{s}^2+1}.$

其中，表示关节i对应的预测模型的参数T₁，表示关节i对应的预测模型的参数T₂； 

所述离散模型为： $y^i\left(k\right)=p_{11}^i\left(k\right)y^i(k-1)+p_{12}^i\left(k\right)y^i(k-2)+p_{13}^i\left(k\right)u^i(k-1).$

其中，分别为关节i对应的预测模型离散化后的三个参数；yⁱ(k)为关节i的运动状态的当前k时刻预测值，yⁱ(k-1)为关节i的运动状态的(k-1)时刻预测值，yⁱ(k-2)为关节i的运动状态的(k-2)时刻预测值，uⁱ(k-1)为(k-1)时刻的指令输入值。 

优选的，所述模型修正单元405可以包括：测试子单元和修正子单元。 

所述测试子单元，用于在无时延环境下，控制端对机械臂的运行状态进行修正测试，获取和的变动范围； 

所述修正子单元，用于利用所述和的变动范围，对和的取值范围进行修正： 

$\left\{\begin{array}{c}{p}_{11}^i=2-\mathrm{\&Delta;t}\frac{T_2^i}{T_1^i}\\ p_{12}^i=\mathrm{\&Delta;t}\frac{T_2^i}{T_1^i}-1-\frac{{\mathrm{\&Delta;t}}^2}{T_1^i}\end{array}\right.$

其中，为大于0的实数。 

根据和的取值范围，当经过最小二乘法修正的分别落在对应的和的取值范围内时，修正结果成立，当未落在对应的取值范围内时，取边界值为修正结果。例如的取值范围经过计算为  分别为的边值，当经过最小二乘法修正的时，采用最小二乘法修正的结果，当时，如果则如果 $p_{11}^i\left(k\right)<a_{11}^i$ 则 $p_{11}^i\left(k\right)=a_{11}^i.$ 同理类推

优选的，所述第一计算单元402可以包括：第一发送子单元、第一预测子单元、第一获取子单元、第一匹配子单元、第二发送子单元、第三发送子单元、第二预测子单元、第二获取子单元、第二匹配子单元以及第一计算子单元。 

所述第一发送子单元，用于在位于远端的机械臂位于静止状态下，控制端发送第一组控制指令序列至机械臂，该控制指令序列用于控制位于远端的机械臂运动； 

所述第一预测子单元，用于控制端预测所述机械臂的第一组理想运动状态，其中，所述第一组理想运动状态是指在控制器无时延环境下针对所述第一组控制指令序列预测得到的机械臂的运动状态； 

所述第一获取子单元，用于控制端实时获取所述机械臂的第一组实际运动状态； 

所述第一匹配子单元，用于对所述机械臂的第一组理想运动状态和第一组实际运动状态进行波形匹配，获取控制回路第一组总时延；其中，所述控制回路第一组总时延为从所述控制端发送第一组控制指令序列至所述控制端检测到所述机械臂响应该第一组控制指令序列所经历的时间； 

所述第二发送子单元，用于在位于远端的机械臂响应完所述控制端的第一组控制指令序列并重新恢复静止状态下，控制端发出第二组控制指令序列至机械臂；其中，所述第二组控制指令序列与第一组控制指令序列相比，所述第二组控制指令序列的顺序执行时间为所述控制回路第一组总时延的2倍； 

所述第三发送子单元，用于当控制端的第二组指令序列发送完毕后，所述控制端向位于远端的机械臂发出停止指令；所述停止指令用于要求所述机械臂以当前状态停止动作； 

所述第二预测子单元，用于控制端再次预测所述机械臂的第二组理想运动状态，其中，该第二组理想运动状态是指控制器在无时延环境下针对所述第二组控制指令序列预测得到的机械臂的运动状态； 

所述第二获取子单元，用于控制端实时获取当前所述机械臂的第二组实 际运动状态； 

所述第二匹配子单元，用于再次对所述机械臂的第二组理想运动状态和第二组实际运动状态进行波形匹配，获取控制回路第二组总时延和停止时间差；其中，所述控制回路第二组总时延为从所述控制端发送第二组控制指令序列至所述控制端检测到所述机械臂响应该第二组控制指令序列所经历的时间；所述停止时间差等于所述机械臂的理想停止时间和实际停止时间的差值； 

所述第一计算子单元，用于计算所述机械臂控制回路的下行时延估计值，所述下行时延估计值等于所述控制回路第二组总时延减去所述上行时延估计值的差值；所述上行时延估计值等于所述停止时间差。 

优选的，所述第二计算单元404可以包括：第二计算子单元和延迟子单元。 

所述第二计算子单元，用于利用所述机械臂各关节的预测模型计算在无时延环境下，该机械臂的各关节的运动状态理想值； 

所述延迟子单元，用于结合所述下行时延的估计值，将所述各关节的运动状态理想值延迟该下行时延的估计值，得到在该下行时延环境下，该机械臂各关节的运动状态的预测数据。 

以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。 

Claims (10)

1.一种不确定时延条件下机械臂运动状态的预测方法，其特征在于，所述方法包括： 

步骤1，对于机械臂的各关节，分别建立预测模型以及与各关节的预测模型对应的离散模型； 

步骤2，计算机械臂控制回路的下行时延的估计值；其中，所述下行时延是指位于远端的机械臂的运动状态数据传递至控制端的过程中发生的时延； 

步骤3，控制端向位于远端的机械臂发送控制指令，并接收所述机械臂返回的响应该控制指令的各关节的运动状态的实测数据； 

步骤4，根据所述控制回路的下行时延的估计值，结合各关节的预测模型，计算得到该机械臂的各关节的运动状态的预测数据； 

步骤5，根据所述机械臂各关节的运动状态的实测数据和预测数据，使用最小二乘法对所述机械臂各关节的预测模型对应的离散模型进行修正。 

步骤6，利用所述修正后的离散模型，对机械臂各关节的运动状态进行预测，获取当前时刻修正后的机械臂各关节运动状态的预测值。 

2.根据权利要求1所述的方法，其特征在于， 

所述预测模型为：

其中，表示关节i对应的预测模型的参数T₁，表示关节i对应的预测模型的参数T₂； 

所述离散模型为：

其中，分别为关节i对应的预测模型离散化后的三个参数；yⁱ(k)为关节i的运动状态的当前k时刻预测值，yⁱ(k-1)为关节i的运动状态的(k-1)时刻预测值，yⁱ(k-2)为关节i的运动状态的(k-2)时刻预测值，uⁱ(k-1)为(k-1)时刻的指令输入值。 

3.根据权利要求2所述的方法，其特征在于，步骤5中，所述使用最 小二乘法对所述机械臂各关节的预测模型对应的离散模型进行修正，包括： 

在无时延环境下，控制端对机械臂的运行状态进行修正测试，获取和的变动范围； 

利用所述和的变动范围，对和的取值范围进行修正： 

其中，为大于0的实数。 

根据和的取值范围，当经过最小二乘法修正的分别落在对应的和的取值范围内时，修正结果成立，当未落在对应的取值范围内时，取边界值为修正结果。 

4.根据权利要求1所述的方法，其特征在于，所述步骤2包括： 

步骤20，在位于远端的机械臂位于静止状态下，控制端发送第一组控制指令序列至机械臂，该控制指令序列用于控制位于远端的机械臂运动； 

步骤21，控制端预测所述机械臂的第一组理想运动状态，其中，所述第一组理想运动状态是指在控制器无时延环境下针对所述第一组控制指令序列预测得到的机械臂的运动状态； 

步骤22，控制端实时获取所述机械臂的第一组实际运动状态； 

步骤23，对所述机械臂的第一组理想运动状态和第一组实际运动状态进行波形匹配，获取控制回路第一组总时延；其中，所述控制回路第一组总时延为从所述控制端发送第一组控制指令序列至所述控制端检测到所述机械臂响应该第一组控制指令序列所经历的时间； 

步骤24，在位于远端的机械臂响应完所述控制端的第一组控制指令序列并重新恢复静止状态下，控制端发出第二组控制指令序列至机械臂；其中，所述第二组控制指令序列与第一组控制指令序列相比，所述第二组控制指令序列的顺序执行时间为所述控制回路第一组总时延的2倍； 

步骤25，当控制端的第二组指令序列发送完毕后，所述控制端向位于远 端的机械臂发出停止指令；所述停止指令用于要求所述机械臂以当前状态停止动作； 

步骤26，控制端再次预测所述机械臂的第二组理想运动状态，其中，该第二组理想运动状态是指控制器在无时延环境下针对所述第二组控制指令序列预测得到的机械臂的运动状态； 

步骤27，控制端实时获取当前所述机械臂的第二组实际运动状态； 

步骤28，再次对所述机械臂的第二组理想运动状态和第二组实际运动状态进行波形匹配，获取控制回路第二组总时延和停止时间差； 

其中，所述控制回路第二组总时延为从所述控制端发送第二组控制指令序列至所述控制端检测到所述机械臂响应该第二组控制指令序列所经历的时间；所述停止时间差等于所述机械臂的理想停止时间和实际停止时间的差值； 

步骤29，所述机械臂控制回路的下行时延估计值等于所述控制回路第二组总时延减去所述上行时延估计值的差值；所述上行时延估计值等于所述停止时间差。 

5.根据权利要求1所述的方法，其特征在于，所述步骤4包括： 

步骤41，利用所述机械臂各关节的预测模型计算在无时延环境下，该机械臂的各关节的运动状态理想值； 

步骤42，结合所述下行时延的估计值，将所述各关节的运动状态理想值延迟该下行时延的估计值，得到在该下行时延环境下，该机械臂各关节的运动状态的预测数据。 

6.一种不确定时延条件下的机械臂运动状态的预测装置，其特征在于，所述装置包括： 

模型建立单元，用于对于机械臂的各关节，分别建立预测模型以及与各关节的预测模型对应的离散模型； 

第一计算单元，用于计算机械臂控制回路的下行时延的估计值；其中，所述下行时延是指位于远端的机械臂的运动状态数据传递至控制端的过程中发生的时延； 

接收发送单元，用于控制端向位于远端的机械臂发送控制指令，并接收所述机械臂返回的响应该控制指令的各关节的运动状态的实测数据； 

第二计算单元，用于根据所述控制回路的下行时延的估计值，结合各关节的预测模型，计算得到该机械臂的各关节的运动状态的预测数据； 

模型修正单元，用于根据所述机械臂各关节的运动状态的实测数据和预测数据，使用最小二乘法对所述机械臂各关节的预测模型对应的离散模型进行修正。 

状态预测单元，用于利用所述修正后的离散模型，对机械臂各关节的运动状态进行预测，获取当前时刻修正后的机械臂各关节运动状态的预测值。 

7.根据权利要求6所述的不确定时延条件下的机械臂运动状态的预测装置，其特征在于， 

所述预测模型为：

其中，表示关节i对应的预测模型的参数T₁，表示关节i对应的预测模型的参数T₂； 

所述离散模型为：

其中，分别为关节i对应的预测模型离散化后的三个参数；yⁱ(k)为关节i的运动状态的当前k时刻预测值，yⁱ(k-1)为关节i的运动状态的(k-1)时刻预测值，yⁱ(k-2)为关节i的运动状态的(k-2)时刻预测值，uⁱ(k-1)为(k-1)时刻的指令输入值。 

8.根据权利要求6所述的不确定时延条件下的机械臂运动状态的预测装置，其特征在于，所述模型修正单元包括： 

测试子单元，用于在无时延环境下，控制端对机械臂的运行状态进行修正测试，获取和的变动范围； 

修正子单元，用于利用所述和的变动范围，对和的取值范围进行修正： 

其中，为大于0的实数 

根据和的取值范围，当经过最小二乘法修正的分别落在对应的和的取值范围内时，修正结果成立，当未落在对应的取值范围内时，取边界值为修正结果。 

9.根据权利要求6所述的不确定时延条件下的机械臂运动状态的预测装置，其特征在于，所述第一计算单元包括： 

第一发送子单元，用于在位于远端的机械臂位于静止状态下，控制端发送第一组控制指令序列至机械臂，该控制指令序列用于控制位于远端的机械臂运动； 

第一预测子单元，用于控制端预测所述机械臂的第一组理想运动状态，其中，所述第一组理想运动状态是指在控制器无时延环境下针对所述第一组控制指令序列预测得到的机械臂的运动状态； 

第一获取子单元，用于控制端实时获取所述机械臂的第一组实际运动状态； 

第一匹配子单元，用于对所述机械臂的第一组理想运动状态和第一组实际运动状态进行波形匹配，获取控制回路第一组总时延；其中，所述控制回路第一组总时延为从所述控制端发送第一组控制指令序列至所述控制端检测到所述机械臂响应该第一组控制指令序列所经历的时间； 

第二发送子单元，用于在位于远端的机械臂响应完所述控制端的第一组控制指令序列并重新恢复静止状态下，控制端发出第二组控制指令序列至机械臂；其中，所述第二组控制指令序列与第一组控制指令序列相比，所述第二组控制指令序列的顺序执行时间为所述控制回路第一组总时延的2倍； 

第三发送子单元，用于当控制端的第二组指令序列发送完毕后，所述控 制端向位于远端的机械臂发出停止指令；所述停止指令用于要求所述机械臂以当前状态停止动作； 

第二预测子单元，用于控制端再次预测所述机械臂的第二组理想运动状态，其中，该第二组理想运动状态是指控制器在无时延环境下针对所述第二组控制指令序列预测得到的机械臂的运动状态； 

第二获取子单元，用于控制端实时获取当前所述机械臂的第二组实际运动状态； 

第二匹配子单元，用于再次对所述机械臂的第二组理想运动状态和第二组实际运动状态进行波形匹配，获取控制回路第二组总时延和停止时间差；其中，所述控制回路第二组总时延为从所述控制端发送第二组控制指令序列至所述控制端检测到所述机械臂响应该第二组控制指令序列所经历的时间；所述停止时间差等于所述机械臂的理想停止时间和实际停止时间的差值； 

第一计算子单元，用于计算所述机械臂控制回路的下行时延估计值，所述下行时延估计值等于所述控制回路第二组总时延减去所述上行时延估计值的差值；所述上行时延估计值等于所述停止时间差。 

10.根据权利要求6所述的不确定时延条件下的机械臂运动状态的预测装置，其特征在于，所述第二计算单元包括： 

第二计算子单元，用于利用所述机械臂各关节的预测模型计算在无时延环境下，该机械臂的各关节的运动状态理想值； 

延迟子单元，用于结合所述下行时延的估计值，将所述各关节的运动状态理想值延迟该下行时延的估计值，得到在该下行时延环境下，该机械臂各关节的运动状态的预测数据。