CN103991074A - 一种基于粒子滤波算法的空间机械臂控制方法及系统 - Google Patents

Abstract

本发明实施例提供了一种基于粒子滤波算法的空间机械臂控制方法，包括：任务的第一控制周期结束后，使用粒子滤波算法获得空间机械臂末端的空间位置误差概率分布；依据所述空间位置误差概率分布，获得空间机械臂的使用可靠度；依据所述使用可靠度和预设的可靠度阈值，获得调整量；依据预设的第一目标空间位置信息和所述调整量，获得第二目标空间位置信息；所述第二目标空间位置信息为在第二控制周期结束后所述空间机械臂末端的空间位置信息；本发明实施例还提供一种基于粒子滤波算法的空间机械臂控制系统。根据本发明实施例提供的技术方案，可以保证在空间机械臂末端空间位置误差观测噪声为任意分布的条件下提高空间机械臂使用可靠性。

Description

一种基于粒子滤波算法的空间机械臂控制方法及系统

【技术领域】

本发明涉及自动化控制技术，尤其涉及一种基于粒子滤波算法的空间机械臂控制方法及系统。

【背景技术】

目前，空间站设备的组装、回收、维修及舱外活动都需要依赖空间机械臂来完成，由于空间机械臂处于微重力、高低温交变、高辐射等恶劣的太空环境中，维修和维护十分困难，因此空间机械臂的可靠性成为世界航天技术的主要难点。空间机械臂的可靠性包括固有可靠性和使用可靠性，其中，固有可靠性在空间机械臂的设计和生产过程中就已经确定，因此提高空间机械臂的使用可靠性是一个经济、有效地提高空间机械臂的任务完成质量的方法。

空间机械臂都是按照预先设置好的任务规划，执行任务，需要在预设的时间到达预设的空间位置，然而，由于空间机械臂关节上的齿轮间隙或者传感器噪声等原因，空间机械臂在执行任务后，实际到达的空间位置与预期的空间位置之间存在一定的误差，如果空间机械臂继续执行任务，任务的实际执行结果与预期的执行结果之间的误差将越来越大，因此空间机械臂的使用可靠性较低。

由于实际的空间机械臂系统可能存在各种内部结构或环境上的影响，空间机械臂的位置误差的实际分布情况可能不会为简单的高斯分布，而可能是各种难以进行解析描述的情况，这时若使用经典的贝叶斯理论来处理时就会出现如难以进行近似计算以及计算结果与实际有很大偏差等问题。

【发明内容】

有鉴于此，本发明实施例提供了一种基于粒子滤波算法的空间机械臂控制方法及系统，以实现提高空间机械臂的使用可靠性。

本发明实施例提供了一种基于粒子滤波算法的空间机械臂控制方法，包括：

任务的第一控制周期结束后，使用粒子滤波算法，获得空间机械臂末端的空间位置误差概率分布；

依据所述空间位置误差概率分布，获得空间机械臂的使用可靠度；

依据所述使用可靠度和预设的可靠度阈值，获得调整量；

依据预设的第一目标空间位置信息和所述调整量，获得第二目标空间位置信息；所述第二目标空间位置信息为在第二控制周期结束后所述空间机械臂末端的目标空间位置信息。

上述方法中，获得空间机械臂末端的空间位置误差概率分布的方法为：根据粒子滤波算法，获得表示空间机械臂末端空间位置误差可能取值的粒子，并根据空间机械臂末端空间位置误差的检测值得到对应粒子的权值，然后通过粒子和其对应的权值来描述空间机械臂末端的空间位置误差概率分布；

使用上述方法获得的空间机械臂末端的空间位置误差概率分布函数为：

$f\left({\mathrm{\θ}}_i\right|y_i)\≈\underset{j=1}{\overset{N_s}{\mathrm{\Σ}}}{\mathrm{\ω}}_i^j\mathrm{\δ}\left({{\mathrm{\θ}}_i-z}_i^j\right)$

其中，θ_i为第i个控制周期结束后空间机械臂末端的空间位置的实际误差；i为大于或者等于1的整数；y_i为第i个控制周期结束后空间机械臂末端的空间位置的检测误差；θ_i|y_i为表示空间机械臂末端的空间位置的检测误差为y_i时，空间机械臂末端的空间位置的实际误差；f(θ_i|y_i)表示空间机械臂末端的空间位置的检测误差为y_i时，空间机械臂末端的空间位置的实际误差θ_i的概率密度函数；N_s为粒子总数，是整数值；为第i控制周期的样本集即粒子集；为粒子所对应的权值；

其中，利用如下公式获得δ(x)：

$\mathrm{\δ}\left(x\right)=\left\{\begin{array}{cc}1,& x\≠0\\ 0,& x=0\end{array}\right.$

这里，x表示δ(x)函数中的变量；

其中，利用如下方法获得

上式中，i为大于或者等于1的整数；为第i-1控制周期的粒子；x_i-1为第i-1控制周期空间机械臂末端空间位置误差的调整量；e_i为一随机数，满足在0和之间的均匀分布；

其中，利用如下方法获得

其中，N_s为粒子总数；y_i为第i控制周期空间机械臂末端空间位置误差的观测值；为对应的粒子；h(x)为预设的权值函数，其中x表示变量；y_i为第i个控制周期结束后空间机械臂末端的空间位置的检测误差；表示自变量等于检测误差y_i与粒子的差值时，函数h(x)的取值。

上述方法中，所述依据所述空间位置误差概率分布，获得空间机械臂的使用可靠度，包括：

依据所述粒子分布所描述的空间机械臂末端的空间位置误差概率密度函数f(θ_i|y_i)，并利用如下公式，获得空间机械臂的使用可靠度：

其中，P{θ_i1≤θ_i≤θ_i2}为空间机械臂的使用可靠度；θ_i1为预设的第一阈值；θ_i2为预设的第二阈值，且θ_i2≥θ_i1。

上述方法中，所述依据所述使用可靠度和预设的可靠度阈值，获得调整量，包括：

比较所述使用可靠度与所述可靠度阈值的大小；

若所述使用可靠度小于所述可靠度阈值，依据所述空间机械臂末端的空间位置误差概率分布的期望，获得所述空间机械臂末端的调整量。

上述方法中，所述依据所述空间机械臂的空间位置误差概率分布的期望，获得所述空间机械臂末端的调整量，包括：

利用如下公式获得第i个控制周期结束后空间机械臂末端的空间位置误差概率分布的期望μ_i：

${\mathrm{\μ}}_i=\underset{j=1}{\overset{N_s}{\mathrm{\Σ}}}{\mathrm{\ω}}_i^j{z}_i^j$

其中， $\{z_i^j,j=1,...,N_s\}$ 为第i控制周期的粒子， $\{{\mathrm{\ω}}_i^j,j=1,...,N_s\}$ 为粒子所对应的权值，N_s为粒子总数；

依据所述空间机械臂的空间位置误差概率分布的期望，并利用如下公式，获得所述空间机械臂末端的调整量：

$x_i\left({\mathrm{\μ}}_i\right)=\left\{\begin{array}{cc}{k}_i{\mathrm{\μ}}_i,& L_i\left({\mathrm{\μ}}_i\right)\≥c+L_i\left((1+k_i){\mathrm{\μ}}_i\right)\\ 0,& L_i\left({\mathrm{\μ}}_i\right)\≤c+L_i\left((1+k_i){\mathrm{\μ}}_i\right)\end{array}\right.$

其中，x_i(μ_i)为空间机械臂末端的调整量；k_i为调整系数；L_i(μ_i)为在第i个控制周期的空间机械臂末端空间位置误差的估计值为μ_i时，不对空间机械臂末端的目标空间位置信息进行调整的情况下，第i+1个控制周期的期望损失；L_i((1+k_i)μ_i)为在第i个控制周期的空间机械臂末端空间位置误差的估计值为(1+k_i)μ_i时，不对空间机械臂末端的目标空间位置信息进行调整的情况下，第i+1个控制周期的期望损失；c为归一化的调整成本；

其中，调整系数k_i由以下公式得出：

$k_i=-1-\frac{{{\mathrm{\μ}}_i}^T\mathrm{\α}+{\mathrm{\α}}^T{\mathrm{\μ}}_i}{{\left|\right|{\mathrm{\μ}}_i\left|\right|}^2}$

其中，μ_i为第i个控制周期结束后空间机械臂末端的空间位置误差概率分布的期望；α为已知量。

上述方法中，所述第一目标空间位置信息包括以下信息中至少一个：空间机械臂末端在空间坐标系的坐标值、所述空间机械臂末端与空间坐标系中坐标轴的夹角；

所述依据预设的第一目标空间位置信息和所述调整量，获得第二目标空间位值信息，包括：

依据所述第一目标空间位置信息与所述调整量的和值，获得所述第二目标空间位置信息。

本发明实施例还提供了一种基于粒子滤波算法的空间机械臂控制系统，包括：

处理单元，用于在任务的第一控制周期结束后，使用粒子滤波算法，获得空间机械臂末端的空间位置误差概率分布；

评估单元，用于依据所述空间位置误差概率分布，获得空间机械臂的使用可靠度；

生成单元，用于依据所述使用可靠度和预设的可靠度阈值，获得调整量；

控制单元，用于依据预设的第一目标空间位置信息和所述调整量，获得第二目标空间位置信息；所述第二目标空间位置信息为在第二控制周期结束后所述空间机械臂末端的目标空间位置信息。

由以上技术方案可以看出，本发明实施例具有以下有益效果：

本发明实施例的技术方案中，依据空间位置误差概率分布，使用粒子滤波算法，获得空间机械臂的使用可靠度，进而依据使用可靠度获得调整量，依据调整量对下一个控制周期的目标空间位置进行调整，从而可以控制空间机械臂在下一个控制周期的任务执行，因此能够实现在空间机械臂的任务执行结果出现误差的情况下，及时地对空间机械臂的目标空间位置进行调整，以避免空间机械臂的任务执行结果与预期结果之间的误差越来越大，可以在空间机械臂末端空间位置误差观测噪声为任意分布的条件下提高空间机械臂的使用可靠性，提高任务执行成功率，保证空间机械臂的任务执行成功率高。

【附图说明】

为了更清楚地说明本发明实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其它的附图。

图1是本发明实施例所提供的基于粒子滤波算法的空间机械臂控制方法的流程示意图；

图2是本发明实施例提供的目标空间位置观测噪声分布曲线；

图3是利用本发明实施例提供的方法对目标空间位置信息进行调整的过程示意图；

图4是利用本发明实施例所提供的方法对目标空间位置信息进行调整的过程中使用可靠度的曲线图；

图5是基于现有技术的空间机械臂的控制过程的误差示意图；

图6是本发明实施例的执行结果的精度分布示意图；

图7是本发明实施例所提供的基于粒子滤波算法的空间机械臂控制系统的结构示意图。

【具体实施方式】

为了更好的理解本发明的技术方案，下面结合附图对本发明实施例进行详细描述。

应当明确，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。

空间机械臂的控制主要包括三个部分，即任务规划、路径规划和运动控制。其中，任务规划指的是输入空间机械臂需要执行的任务的任务规划约束，如任务目标(如任务的起始位置、空间机械臂末端的空间位置)、空间机械臂的约束(如空间机械臂中每个臂杆的长度、每个关节的角度)、环境障碍(如障碍的尺寸和位置)等；然后，依据任务规划约束，对一个完整的任务进行任务拆分，从而获得至少两个子任务；然后，依据任务规划约束，确定每个子任务的中间点；每个子任务的中间点可以是一个或多个。依据中间点和预设的路径规划约束，为空间机械臂生成每两个中间点之间的执行路径，所述执行路径为空间机械臂在两个中间点之间的移动路径，依据该执行路径，空间机械臂就可以按照规划的执行路径进行移动，以完成指定的任务。

使用可靠性是指空间机械臂执行规定的任务时，采用规定的控制方法成功完成任务的能力。使用可靠性通过使用可靠度进行量化，使用可靠度是采用规定的控制方法执行规定的任务，成功完成任务的概率。任务成功的标准由预设的任务执行的精度决定，只要在任务执行的精度范围内，就认为任务成功。执行相同的任务，控制方法不同，所表现出的使用可靠性不同。

本发明实施例中，预先设置至少两个控制周期，在控制空间机械臂执行上述任务过程中，在每个控制周期结束后，都获得空间机械臂的使用可靠度，然后依据使用可靠度获得调整量，依据调整量对下一个控制周期结束后空间机械臂末端的空间位置进行调整，以减小空间机械臂末端的空间位置的误差。为了方便说明，本发明实施例以第一控制周期和第二控制周期为例进行说明，即任务的第一控制周期结束后，可以获得空间机械臂的使用可靠度，依据使用可靠度，获得调整量，然后依据调整量对第二控制周期的目标空间位置信息进行调整。

本发明实施例给出一种基于粒子滤波算法的空间机械臂控制方法，请参考图1，其为本发明实施例所提供的基于粒子滤波算法的空间机械臂控制方法的流程示意图，如图1所示，该方法包括以下步骤：

步骤101，任务的第一控制周期结束后，使用粒子滤波算法，获得空间机械臂末端的空间位置误差概率分布。

具体的，首先，针对空间机械臂闭环控制系统，建立以下状态空间方程模型，用以描述空间机械臂的控制过程：

$\left\{\begin{array}{c}{\mathrm{\θ}}_i={\mathrm{\θ}}_{i-1}+x_{i-1}\\ y_i={\mathrm{\θ}}_i+{\mathrm{\υ}}_i\end{array}\right.$

其中，θ_i为第i个控制周期结束后空间机械臂末端的空间位置的实际误差，即空间机械臂末端的空间位置的真实误差；i为大于或者等于1的整数，y_i为第i个控制周期结束后空间机械臂末端的空间位置的检测误差，即检测到的带有噪声的空间位置误差；x_i-1为第i-1个控制周期结束后空间机械臂的调整量；υ_i为噪声，υ_i服从分布φ，其期望为α，其方差为β，概率密度函数曲线如图2所示。

由于空间机械臂的控制系统中各控制环节存在扰动和噪声，若空间机械臂在执行任务的过程中，空间机械臂末端的实际空间位置处于预设的精度范围内，过度地控制调整将会带来更多的扰动，将由此带来的精度损失定义为调整成本。依据空间机械臂末端的空间位置的实际误差和调整成本，构建如下绩效函数，用以描述空间机械臂执行任务的质量：

$L=E[\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{y_i}^T{y}_i+\mathrm{c\δ}\left(x_{i-1}\right)]$

其中，L表示空间机械臂执行任务的质量，E[·]表示数学期望运算，y_i为第i个控制周期结束后空间机械臂末端的空间位置的检测误差，c为归一化的调整成本；x_i-1为第i-1个控制周期结束后获得的调整量，T为矩阵y_i的转置。

其中，利用如下公式获得δ(x)：

$\mathrm{\δ}\left(x\right)=\left\{\begin{array}{cc}1,& x\≠0\\ 0,& x=0\end{array}\right.$

这里，x表示δ(x)函数中的变量。

因此，在同时考虑空间机械臂末端的空间位置的检测误差和调整成本的情况下，空间机械臂的最优控制策略为：使空间机械臂按照序列{x_i}进行调整，i为大于或者等于1的整数，x_i为第i个控制周期结束后获得的调整量。

依据每个控制周期结束后采集的空间机械臂末端的空间位置的检测误差y_i，通过粒子滤波算法对控制过程中的空间机械臂末端的空间位置的实际误差θ_i的分布估计。

其中，采用和两个集合来对θ_i的分布进行描述。其中是用来表示θ_i可能取值的一组样本数据，称为粒子，N_s为粒子数量；是粒子对应的权值，表示粒子与θ_i的相关程度，且规定

采用如下的正态分布模型作为空间机械臂末端的空间位置的实际误差θ_i的先验概率分布：

${\mathrm{\θ}}_0~N({\mathrm{\μ}}_0,{\mathrm{\Λ}}_0)$

其中，θ₀为空间机械臂末端的空间位置的实际误差的初始值，μ₀为预设的空间机械臂末端的空间位置的实际误差初始值θ₀的先验概率分布的期望，Λ₀为预设的空间机械臂末端的空间位置的实际误差初始值θ₀的先验概率分布的方差。

根据系统状态方程和先验分布实现粒子滤波，其主要步骤包括：

1)初始化，对于第一控制周期，根据θ₀的先验概率分布产生数量为N_s的随机量，将这些随机量集合作为初始粒子集并使其对应权值对于第二控制周期及以后的控制周期，根据上一控制周期的粒子 $\{z_{i-1}^j,j=1,...,N_s\}$ 和其权值 $\{{\mathrm{\ω}}_{i-1}^j,j=1,...,N_s\},$ 利用如下公式获得初始粒子集：

$\left\{\begin{array}{c}{z}_i^j=z_{i-1}^j+x_{i-1},z_{i-1}^j\≥e_i\\ {\mathrm{\ω}}_i^j=\frac{1}{N_s}\end{array}\right.$

其中，e_i为一随机数，满足在0和之间的均匀分布。

2)根据重要性函数更新权值，重要性函数与噪声分布φ有关，设φ的概率密度函数为h(ρ)，其中ρ为变量，则重要性函数为即 ${\mathrm{\ω}}_i^j=h(y_i-z_i^j);$

3)权值归一化，即令

根据以上方法得到空间机械臂末端的空间位置的实际误差的概率分布函数为：

$f\left({\mathrm{\θ}}_i\right|y_i)\≈\underset{j=1}{\overset{N_s}{\mathrm{\Σ}}}{\mathrm{\ω}}_i^j\mathrm{\δ}\left({{\mathrm{\θ}}_i-z}_i^j\right)$

其中θ_i为第i个控制周期结束后空间机械臂末端的空间位置的实际误差，θ_i|y_i为空间机械臂末端的空间位置误差概率分布，f(θ_i|y_i)表示空间机械臂末端的空间位置的检测误差为y_i时，空间机械臂末端的空间位置的实际误差θ_i的概率密度函数，为第i控制周期的样本集即粒子集，为粒子所对应的权值权值，且N_s为粒子总数；

其中，利用如下公式获得δ(x)：

$\mathrm{\δ}\left(x\right)=\left\{\begin{array}{cc}1,& x\≠0\\ 0,& x=0\end{array}\right.$

这里，x表示δ(x)函数中的变量。

步骤102，依据所述空间位置误差概率分布，获得空间机械臂的使用可靠度。

具体的，使用可靠度可以依据第i个控制周期结束后空间机械臂末端的空间位置的实际误差θ_i的后验概率进行评估，使用可靠度是空间机械臂的使用可靠性的量化，使用可靠度指的是第i个控制周期结束后械臂末端的空间位置的实际误差θ_i落在预设的精度范围内的概率。

首先，利用如下公式，获得所述空间机械臂末端的空间位置误差概率密度函数f(θ_i|y_i)：

$f\left({\mathrm{\θ}}_i\right|y_i)\≈\underset{j=1}{\overset{N_s}{\mathrm{\Σ}}}{\mathrm{\ω}}_i^j\mathrm{\δ}\left({{\mathrm{\θ}}_i-z}_i^j\right)$

其中θ_i为第i个控制周期结束后空间机械臂末端的空间位置的实际误差，θ_i|y_i为空间机械臂末端的空间位置误差概率分布，f(θ_i|y_i)表示空间机械臂末端的空间位置的检测误差为y_i时，空间机械臂末端的空间位置的实际误差θ_i的概率密度函数，为第i控制周期的样本集即粒子集，为粒子所对应的权值权值，且N_s为粒子总数。

然后，依据所述空间机械臂末端的空间位置误差概率密度函数f(θ_i|y_i)，并利用如下公式，获得空间机械臂的使用可靠度：

其中，θ_i1≤θ_i≤θ_i2为预设的精度范围，P{θ_i1≤θ_i≤θ_i2}为第i个控制周期结束后空间机械臂的使用可靠度，θ_i1为精度范围的第一阈值，即精度范围的下限值，θ_i2为精度范围的第二阈值，即精度范围的上限值，且θ_i2≥θ_i1。

步骤103，依据所述使用可靠度和预设的可靠度阈值，获得调整量。

具体的，比较获得的使用可靠度与预设的可靠度阈值的大小；若所述使用可靠度大于或者等于预设的可靠度阈值，则不进行目标空间位置信息的调整，因此，不需要计算调整量，保持空间机械臂当前运行状态继续运行；若所述使用可靠度小于预设的可靠度阈值，则需要依据空间机械臂末端的空间位置误差概率分布的期望，获得所述空间机械臂末端的调整量。

本发明实施例中，依据空间机械臂末端的空间位置误差概率分布的期望，获得所述空间机械臂末端的调整量的方法包括：

1)将第i个控制周期结束后空间机械臂末端的空间位置误差概率分布的期望μ_i作为第i个控制周期结束后空间机械臂末端的空间位置的估计值

即：

${\widehat{\mathrm{\θ}}}_i={\mathrm{\μ}}_i=\underset{j=1}{\overset{N_s}{\mathrm{\Σ}}}{\mathrm{\ω}}_i^j{z}_i^j$

其中， $\{z_i^j,j=1,...,N_s\}$ 为第i控制周期的粒子， $\{{\mathrm{\ω}}_i^j,j=1,...,N_s\}$ 为粒子所对应的权值，N_s为粒子总数。

2)确定调整量取值：

若为在第i个控制周期结束后空间机械臂末端的空间位置的误差估计值为μ_i时，对空间机械臂末端的目标空间位置信息进行调整的情况下，第i+1个控制周期最小期望损失。L_i(μ_i)为在第i个控制周期结束后空间机械臂末端空间位置误差的估计值为μ_i时，不对空间机械臂末端的目标空间位置信息进行调整的情况下，第i+1个控制周期最小期望损失。为简化计算，设调整量x_i＝k_iμ_i，k_i为一维量。

根据定义有：

$L_i^{*}\left({\mathrm{\μ}}_i\right)=\underset{x_i}{\min }\left\{E\right[y_{i+1}^T{y}_{i+1}+\mathrm{c\δ}\left(x_i\right)\left]\right\}$

根据状态方程

$\left\{\begin{array}{c}{\mathrm{\θ}}_i={\mathrm{\θ}}_{i-1}+x_{i-1}\\ y_i={\mathrm{\θ}}_i+{\mathrm{\υ}}_i\end{array}\right.$

得：

$\begin{array}{c}{L}_i^{*}\left({\mathrm{\μ}}_i\right)=\underset{x_i}{\min }\left\{E\right[{({\mathrm{\θ}}_{i+1}+{\mathrm{\υ}}_{i+1})}^T({\mathrm{\θ}}_{i+1}+{\mathrm{\υ}}_{i+1})+\mathrm{c\δ}\left(x_i\right)\left]\right\}\\ =\underset{k_i}{\min }\{{(1+k_i)}^2{\left|\right|{\mathrm{\μ}}_i\left|\right|}^2+(1+k_i)({{\mathrm{\μ}}_i}^T\mathrm{\α}+{\mathrm{\α}}^T{\mathrm{\μ}}_i)+\mathrm{tr}(\mathrm{\β}+{\mathrm{\Λ}}_i)+{\left|\right|\mathrm{\α}\left|\right|}^2\\ +\mathrm{c\δ}\left(k_i\right)\}\end{array}$

即：

$\begin{array}{c}{L}_i^{*}\left({\mathrm{\μ}}_i\right)=\mathrm{tr}\left(\mathrm{\β}{+\mathrm{\Λ}}_i\right)+{\left|\right|\mathrm{\α}\left|\right|}^2\\ +\underset{k_i}{\min }\{{(1+k_i)}^2{\left|\right|{\mathrm{\μ}}_i\left|\right|}^2+(1+k_i)({{\mathrm{\μ}}_i}^T\mathrm{\α}+{\mathrm{\α}}^T{\mathrm{\μ}}_i)+\mathrm{c\δ}\left(k_i\right)\}\end{array}$

令 $R\left({\mathrm{\μ}}_i\right)=\mathrm{mi}{n}_{k_i}\{{(1+k_i)}^2{\left|\right|{\mathrm{\μ}}_i\left|\right|}^2+(1+k_i)({{\mathrm{\μ}}_i}^T\mathrm{\α}+{\mathrm{\α}}^T{\mathrm{\μ}}_i)+\mathrm{c\δ}\left(k_i\right)\}$

可知，当k_i＝0时，有：

R(μ_i)＝||μ_i||²+μ_i ^Tα+α^Tμ_i

此时有： $L_i^{*}\left({\mathrm{\μ}}_i\right)=L_i\left({\mathrm{\μ}}_i\right).$

当k_i≠0时，有：

$R\left({\mathrm{\μ}}_i\right)=\underset{k_i}{\min }\{{(1+k_i)}^2{\left|\right|{\mathrm{\μ}}_i\left|\right|}^2+(1+k_i)({{\mathrm{\μ}}_i}^T\mathrm{\α}+{\mathrm{\α}}^T{\mathrm{\μ}}_i)+c\}$

相当于求关于k_i的二次函数的最小值。

由于||μ_i||²>0，所以最小值在对称轴处，即：

$k_i=-1-\frac{{{\mathrm{\μ}}_i}^T\mathrm{\α}+{\mathrm{\α}}^T{\mathrm{\μ}}_i}{{\left|\right|{\mathrm{\μ}}_i\left|\right|}^2}$

此时有， $L_i^{*}\left({\mathrm{\μ}}_i\right)=L_i\left((1+k_i){\mathrm{\μ}}_i\right)+c.$

即：

$L_i^{*}\left({\mathrm{\μ}}_i\right)=\min \{L_i\left({\mathrm{\μ}}_i\right),c+L_i\left((1+k_i){\mathrm{\μ}}_i\right)\}$

所以：

$\min \{L_i\left({\mathrm{\μ}}_i\right),L_i^{*}\left({\mathrm{\μ}}_i\right)\}=\min \{L_i\left({\mathrm{\μ}}_i\right),c+L_i\left((1+k_i){\mathrm{\μ}}_i\right)\}$

依据所述空间机械臂的空间位置误差概率分布的期望，并利用如下公式，获得所述空间机械臂末端的调整量：

$x_i\left({\mathrm{\μ}}_i\right)=\left\{\begin{array}{cc}{k}_i{\mathrm{\μ}}_i,& L_i\left({\mathrm{\μ}}_i\right)\≥c+L_i\left((1+k_i){\mathrm{\μ}}_i\right)\\ 0,& L_i\left({\mathrm{\μ}}_i\right)\≤c+L_i\left((1+k_i){\mathrm{\μ}}_i\right)\end{array}\right.$

其中，x_i(μ_i)为空间机械臂末端的调整量；k_i为调整系数；L_i(μ_i)为在第i个控制周期的空间机械臂末端空间位置误差的估计值为μ_i时，不对空间机械臂末端的目标空间位置信息进行调整的情况下，第i+1个控制周期的期望损失；L_i((1+k_i)μ_i)为在第i个控制周期的空间机械臂末端空间位置误差的估计值为(1+k_i)μ_i时，不对空间机械臂末端的目标空间位置信息进行调整的情况下，第i+1个控制周期的期望损失；c为归一化的调整成本。

其中，调整系数k_i由以下公式得出：

$k_i=-1-\frac{{{\mathrm{\μ}}_i}^T\mathrm{\α}+{\mathrm{\α}}^T{\mathrm{\μ}}_i}{{\left|\right|{\mathrm{\μ}}_i\left|\right|}^2}$

其中，μ_i为第i个控制周期结束后空间机械臂末端的空间位置误差概率分布的期望；α为观测噪声的期望值，为已知量。

步骤104，依据预设的第一目标空间位置信息和所述调整量，获得第二目标空间位置信息；所述第二目标空间位置信息为在第二控制周期结束后所述空间机械臂末端的目的空间位置信息。

具体的，预设的第一目标空间位置信息包括以下信息中至少一个：空间机械臂末端在空间坐标系的坐标值、所述空间机械臂末端与空间坐标系中坐标轴的夹角。预设的第一目标空间位置信息指的是为第i个控制周期空间机械臂执行任务预先规划好的目标空间位置信息，表示在第i个控制周期结束后，空间机械臂应该到达该目标空间位置信息所指示的位置。

本发明实施例中，依据预设的第一目标空间位置信息和所述调整量，获得第二目标空间位值信息的方法可以是：依据所述第一目标空间位置信息与所述调整量的和值，获得所述第二目标空间位置信息。

例如，第一目标空间位置信息包括6个维度，其中3个维度表示空间机械臂末端在空间坐标系的坐标值，另3个维度表示空间机械臂末端与空间坐标系中坐标轴的夹角，若第一目标空间位置信息包括空间机械臂末端在空间坐标系的坐标值g₁，则可以依据所述空间机械臂末端在空间坐标系的坐标值g₁与所述调整量x_i(μ_i)的和值，获得所述第二目标空间位置信息g′₁，即：

g′₁＝g₁+x_i(μ_i)

其中，第二目标空间位置信息g′₁中空间机械臂末端与空间坐标系中坐标轴的夹角，等于第一目标空间位置信息g₁中空间机械臂末端与空间坐标系中坐标轴的夹角，即仅对空间机械臂末端在空间坐标系的坐标值g₁进行调整，不调整空间机械臂末端与空间坐标系中坐标轴的夹角。

再例如，若第一目标空间位置信息包括空间机械臂末端与空间坐标系中坐标轴的夹角g₂，则可以依据所述空间机械臂末端与空间坐标系中坐标轴的夹角g₂与所述调整量x_i(μ_i)的和值，获得所述第二目标空间位置信息g′₂，即：

g′₂＝g₂+x_i(μ_i)

其中，第二目标空间位置信息g′₂中空间机械臂末端在空间坐标系的坐标值，等于第一目标空间位置信息g₂中空间机械臂末端在空间坐标系的坐标值，即仅对空间机械臂末端与空间坐标系中坐标轴的夹角g₂进行调整，不调整空间机械臂末端在空间坐标系的坐标值。

再例如，若第一目标空间位置信息中包括空间机械臂末端在空间坐标系的坐标值g₁和空间机械臂末端与空间坐标系中坐标轴的夹角g₂，则可以依据所述第一目标空间位置与调整量x_i(μ_i)的和值，获得第二目标空间位置信息g′₃，即：

g′₃＝g₃+x_i(μ_i)

依据本发明实施例提供的上述方法，对空间机械臂的控制进行了仿真，为一个具有9个控制周期的空间机械臂的空载转移任务，规定任务精度范围是±1mm，精度范围的第一阈值θ_i1等于-1mm，精度范围的第二阈值θ_i2等于+1mm，可靠度阈值可以为0.9。设置空间机械臂末端的空间位置的实际误差为7mm，空间机械臂末端的空间位置的实际误差的先验概率服从先验分布θ₀～N(0，1)。在每个控制周期结束后获得空间机械臂末端的空间位置的检测误差，引入噪声υ₀，噪声υ₀服从分布φ，其概率密度函数曲线如图2所示。分别使用本发明实施例的技术方案和不使用本发明实施例的技术方案对上述任务进行单次仿真，仿真结果如图3～图5所示。其中图3和图4中的横坐标轴表示控制周期。

请参考图3，其为利用本发明实施例提供的方法对目标空间位置信息进行调整的过程示意图。如图3所示，实线11表示空间机械臂末端的空间位置的实际误差，虚线12表示空间机械臂末端的空间位置的误差的估计值，虚线13表示空间机械臂末端的空间位置的检测误差，空间机械臂末端的空间位置的误差的估计值逐渐向空间机械臂末端的空间位置的实际误差收敛；整个控制过程分别在第1控制周期、第2控制周期和第7控制周期对相应的目的空间位置信息进行了调整；空间机械臂末端的空间位置的实际误差逐渐向0收敛，收敛过程平缓且没有波动。

请参考图4，其为利用本发明实施例所提供的方法对目标空间位置信息进行调整的过程中使用可靠度的曲线图，如图4所示，可以控制方法中依据使用可靠度进行控制的过程中使用可靠度呈现增加趋势，且收敛于1。

请参考图5，其为基于现有技术的空间机械臂的控制过程的误差示意图，如图5中的实线，空间机械臂末端的空间位置的检测误差中存在噪声，即使在每个控制周期都对目的空间位置信息进行调整，如图5中的虚线，空间机械臂末端的空间位置的实际误差仍然持续波动，且不能收敛。

请参考图6，其为本发明实施例的执行结果的精度分布示意图，如图6所示，在重复完成1000次任务后，获得执行结果精度分布，图6中的横坐标轴是精度值，纵坐标轴是执行结果的精度落在横坐标轴对应精度的次数，规定执行结果的精度位于±1.5mm范围内。如图6中实线所示，统计得到使用本发明实施例的技术方案的执行任务的成功概率为73.6％，即利用粒子滤波法对空间机械臂进行控制后，空间机械臂的使用可靠度达到73.6％；如图6中虚线所示，使用贝叶斯法对空间机械臂进行控制，空间机械臂的使用可靠度为35.5％。由图6的两条曲线看出，未使用本发明所述控制方法执行任务会结果产生明显的偏差，使用本发明所述控制方法执行任务的精度更高，有效的提高了空间机械臂的使用可靠性。

本发明实施例进一步给出实现上述方法实施例中各步骤及方法的装置实施例。

请参考图7，其为本发明实施例所提供的基于粒子滤波算法的空间机械臂控制系统的结构示意图。如图所示，该系统包括：

处理单元701，用于在任务的第一控制周期结束后，使用粒子滤波算法，获得空间机械臂末端的空间位置误差概率分布；

评估单元702，用于依据所述空间位置误差概率分布，获得空间机械臂的使用可靠度；

生成单元703，用于依据所述使用可靠度和预设的可靠度阈值，获得调整量；

控制单元704，用于依据预设的第一目标空间位置信息和所述调整量，获得第二目标空间位置信息；所述第二目标空间位置信息为在第二控制周期结束后所述空间机械臂末端的目标空间位置信息。

上述系统中，所述处理单元701具体用于：

使用如下方法获得空间机械臂末端的空间位置误差的概率分布：根据粒子滤波算法，获得表示空间机械臂末端空间位置误差可能取值的粒子，并根据空间机械臂末端空间位置误差的检测值得到对应粒子的权值，然后通过粒子和其对应的权值来描述空间机械臂末端的空间位置误差概率分布。

使用上述方法获得的空间机械臂末端的空间位置误差概率分布函数为：

$f\left({\mathrm{\θ}}_i\right|y_i)\≈\underset{j=1}{\overset{N_s}{\mathrm{\Σ}}}{\mathrm{\ω}}_i^j\mathrm{\δ}\left({{\mathrm{\θ}}_i-z}_i^j\right)$

其中，θ_i为第i个控制周期结束后空间机械臂末端的空间位置的实际误差；i为大于或者等于1的整数；y_i为第i个控制周期结束后空间机械臂末端的空间位置的检测误差；θ_i|y_i为表示空间机械臂末端的空间位置的检测误差为y_i时，空间机械臂末端的空间位置的实际误差；f(θ_i|y_i)表示空间机械臂末端的空间位置的检测误差为y_i时，空间机械臂末端的空间位置的实际误差θ_i的概率密度函数；N_s为粒子总数，是整数值；为第i控制周期的样本集即粒子集；为粒子所对应的权值；

其中，利用如下公式获得δ(x)：

$\mathrm{\δ}\left(x\right)=\left\{\begin{array}{cc}1,& x\≠0\\ 0,& x=0\end{array}\right.$

这里，x表示δ(x)函数中的变量；

其中，利用如下方法获得

上式中，i为大于或者等于1的整数；为第i-1控制周期的粒子；x_i-1为第i-1控制周期空间机械臂末端空间位置误差的调整量；e_i为一随机数，满足在0和之间的均匀分布；

其中，利用如下方法获得

其中，N_s为粒子总数；y_i为第i控制周期空间机械臂末端空间位置误差的观测值；为对应的粒子；h(x)为预设的权值函数，其中x表示变量；y_i为第i个控制周期结束后空间机械臂末端的空间位置的检测误差；表示自变量等于检测误差y_i与粒子的差值时，函数h(x)的取值。

上述系统中，所述评估单元702具体用于：

利用如下公式，获得所述空间机械臂末端的空间位置误差概率密度函数f(θ_i|y_i)：

$f\left({\mathrm{\θ}}_i\right|y_i)\≈\underset{j=1}{\overset{N_s}{\mathrm{\Σ}}}{\mathrm{\ω}}_i^j\mathrm{\δ}\left({{\mathrm{\θ}}_i-z}_i^j\right)$

其中θ_i为第i个控制周期结束后空间机械臂末端的空间位置的实际误差，θ_i|y_i为空间机械臂末端的空间位置误差概率分布，f(θ_i|y_i)表示空间机械臂末端的空间位置的检测误差为y_i时，空间机械臂末端的空间位置的实际误差θ_i的概率密度函数，为第i控制周期的样本集即粒子集，为粒子所对应的权值权值，且为粒子总数；

依据所述空间机械臂末端的空间位置误差概率密度函数f(θ_i|y_i)，并利用如下公式，获得空间机械臂的使用可靠度：

其中，P{θ_i1≤θ_i≤θ_i2}为空间机械臂的使用可靠度；θ_i1为预设的第一阈值；θ_i2为预设的第二阈值，且θ_i2≥θ_i1。

上述系统中，所述生成单元703具体用于：

比较所述使用可靠度与所述可靠度阈值的大小；

若所述使用可靠度小于所述可靠度阈值，依据所述空间机械臂末端的空间位置误差概率分布的期望，获得所述空间机械臂末端的调整量。

其中，所述生成单元703具体用于：

依据所述空间机械臂的空间位置误差概率分布获得第i个控制周期结束后空间机械臂末端的空间位置误差概率分布的期望μ_i：

${\mathrm{\μ}}_i=\underset{j=1}{\overset{N_s}{\mathrm{\Σ}}}{\mathrm{\ω}}_i^j{z}_i^j$

然后依据所述空间机械臂的空间位置误差概率分布的期望，并利用如下公式，获得所述空间机械臂末端的调整量：

$x_i\left({\mathrm{\μ}}_i\right)=\left\{\begin{array}{cc}{k}_i{\mathrm{\μ}}_i,& L_i\left({\mathrm{\μ}}_i\right)\≥c+L_i\left((1+k_i){\mathrm{\μ}}_i\right)\\ 0,& L_i\left({\mathrm{\μ}}_i\right)\≤c+L_i\left((1+k_i){\mathrm{\μ}}_i\right)\end{array}\right.$

其中，x_i(μ_i)为空间机械臂末端的调整量；k_i为调整系数；L_i(μ_i)为在第i个控制周期的空间机械臂末端空间位置误差的估计值为μ_i时，不对空间机械臂末端的目标空间位置信息进行调整的情况下，第i+1个控制周期的期望损失；L_i((1+k_i)μ_i)为在第i个控制周期的空间机械臂末端空间位置误差的估计值为(1+k_i)μ_i时，不对空间机械臂末端的目标空间位置信息进行调整的情况下，第i+1个控制周期的期望损失；c为归一化的调整成本；

其中，调整系数k_i由以下公式得出：

$k_i=-1-\frac{{{\mathrm{\μ}}_i}^T\mathrm{\α}+{\mathrm{\α}}^T{\mathrm{\μ}}_i}{{\left|\right|{\mathrm{\μ}}_i\left|\right|}^2}$

其中，μ_i为第i个控制周期结束后空间机械臂末端的空间位置误差概率分布的期望；α为观测噪声的期望值，为已知量。

所述生成单元具体703用于：

上述系统中，所述第一目标空间位置信息包括以下信息中至少一个：空间机械臂末端在空间坐标系的坐标值、所述空间机械臂末端与空间坐标系中坐标轴的夹角；

所述控制单元704具体用于：依据所述第一目标空间位置信息与所述调整量的和值，获得所述第二目标空间位置信息。

本发明实施例的技术方案具有以下有益效果：

本发明实施例的技术方案中，依据空间位置误差概率分布，获得空间机械臂的使用可靠度，进而依据使用可靠度获得调整量，依据调整量对下一个控制周期的目标空间位置进行调整，从而可以控制空间机械臂在下一个控制周期的任务执行，因此能够实现在空间机械臂的任务执行结果出现误差的情况下，及时地对空间机械臂的目标空间位置进行调整，以避免空间机械臂的任务执行结果与预期结果之间的误差越来越大，因此可以提高空间机械臂的使用可靠性，提高任务执行成功率，保证空间机械臂的任务执行成功率高。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明保护的范围之内。

Claims (7)

1.一种基于粒子滤波算法的空间机械臂控制方法，其特征在于，所述方法包括：

任务的第一控制周期结束后，使用粒子滤波算法，获得空间机械臂末端的空间位置误差概率分布；

依据所述空间位置误差概率分布，获得空间机械臂的使用可靠度；

依据所述使用可靠度和预设的可靠度阈值，获得调整量；

依据预设的第一目标空间位置信息和所述调整量，获得第二目标空间位置信息；所述第二目标空间位置信息为在第二控制周期结束后所述空间机械臂末端的目标空间位置信息。

2.根据权利要求1所述的方法，其特征在于，任务的第一控制周期结束后，使用粒子滤波算法，获得空间机械臂末端的空间位置误差概率分布，包括：根据粒子滤波算法，获得表示空间机械臂末端空间位置误差可能取值的粒子，并根据空间机械臂末端空间位置误差的检测值得到对应粒子的权值，然后通过粒子和其对应的权值来描述空间机械臂末端的空间位置误差概率分布；

使用上述方法获得的空间机械臂末端的空间位置误差概率密度函数为：

$f\left({\mathrm{\θ}}_i\right|y_i)\≈\underset{j=1}{\overset{N_s}{\mathrm{\Σ}}}{\mathrm{\ω}}_i^j\mathrm{\δ}\left({{\mathrm{\θ}}_i-z}_i^j\right)$

其中，θ_i为第i个控制周期结束后空间机械臂末端的空间位置的实际误差；i为大于或者等于1的整数；y_i为第i个控制周期结束后空间机械臂末端的空间位置的检测误差；θ_i|y_i为表示空间机械臂末端的空间位置的检测误差为y_i时，空间机械臂末端的空间位置的实际误差；f(θ_i|y_i)表示空间机械臂末端的空间位置的检测误差为y_i时，空间机械臂末端的空间位置的实际误差θ_i的概率密度函数；N_s为粒子总数，是整数值；为第i控制周期的样本集即粒子集；为粒子所对应的权值；

其中，利用如下公式获得δ(x)：

$\mathrm{\δ}\left(x\right)=\left\{\begin{array}{cc}1,& x\≠0\\ 0,& x=0\end{array}\right.$

这里，x表示δ(x)函数中的变量；

其中，利用如下方法获得

上式中，i为大于或者等于1的整数；为第i-1控制周期的粒子；x_i-1为第i-1控制周期空间机械臂末端空间位置误差的调整量；e_i为一随机数，满足在0和之间的均匀分布；

其中，利用如下方法获得

其中，N_s为粒子总数；y_i为第i控制周期空间机械臂末端空间位置误差的观测值；为对应的粒子；h(x)为预设的权值函数，其中x表示变量；y_i为第i个控制周期结束后空间机械臂末端的空间位置的检测误差；表示自变量等于检测误差y_i与粒子的差值时，函数h(x)的取值。

3.根据权利要求1所述的方法，其特征在于，所述依据所述空间位置误差概率分布，获得空间机械臂的使用可靠度，包括：

依据所述空间机械臂末端的空间位置误差概率密度函数f(θ_i|y_i)，并利用如下公式，获得空间机械臂的使用可靠度：

其中，P{θ_i1≤θ_i≤θ_i2}为空间机械臂的使用可靠度；θ_i1为预设的第一阈值；θ_i2为预设的第二阈值，且θ_i2≥θ_i1。

4.根据权利要求1至3中任一项所述的方法，其特征在于，所述依据所述使用可靠度和预设的可靠度阈值，获得调整量，包括：

比较所述使用可靠度与所述可靠度阈值的大小；

若所述使用可靠度小于所述可靠度阈值，依据所述空间机械臂末端的空间位置误差概率分布的期望，获得所述空间机械臂末端的调整量。

5.根据权利要求4所述的方法，其特征在于，首先依据所述空间机械臂的空间位置误差概率分布获得第i个控制周期结束后空间机械臂末端的空间位置误差概率分布的期望μ_i：

${\mathrm{\μ}}_i=\underset{j=1}{\overset{N_s}{\mathrm{\Σ}}}{\mathrm{\ω}}_i^j{z}_i^j$

然后依据所述空间机械臂的空间位置误差概率分布的期望，并利用如下公式，获得所述空间机械臂末端的调整量：

$x_i\left({\mathrm{\μ}}_i\right)=\left\{\begin{array}{cc}{k}_i{\mathrm{\μ}}_i,& L_i\left({\mathrm{\μ}}_i\right)\≥c+L_i\left((1+k_i){\mathrm{\μ}}_i\right)\\ 0,& L_i\left({\mathrm{\μ}}_i\right)\≤c+L_i\left((1+k_i){\mathrm{\μ}}_i\right)\end{array}\right.$

其中，x_i(μ_i)为空间机械臂末端的调整量；k_i为调整系数；L_i(μ_i)为在第i个控制周期的空间机械臂末端空间位置误差的估计值为μ_i时，不对空间机械臂末端的目标空间位置信息进行调整的情况下，第i+1个控制周期的期望损失；L_i((1+k_i)μ_i)为在第i个控制周期的空间机械臂末端空间位置误差的估计值为(1+k_i)μ_i时，不对空间机械臂末端的目标空间位置信息进行调整的情况下，第i+1个控制周期的期望损失；c为归一化的调整成本；

其中，调整系数k_i由以下公式得出：

$k_i=-1-\frac{{{\mathrm{\μ}}_i}^T\mathrm{\α}+{\mathrm{\α}}^T{\mathrm{\μ}}_i}{{\left|\right|{\mathrm{\μ}}_i\left|\right|}^2}$

其中，μ_i为第i个控制周期结束后空间机械臂末端的空间位置误差概率分布的期望；α为已知量。

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

所述第一目标空间位置信息包括以下信息中至少一个：空间机械臂末端在空间坐标系的坐标值、所述空间机械臂末端与空间坐标系中坐标轴的夹角；

所述依据预设的第一目标空间位置信息和所述调整量，获得第二目标空间位值信息，包括：

依据所述第一目标空间位置信息与所述调整量的和值，获得所述第二目标空间位置信息。

7.一种基于粒子滤波算法的空间机械臂控制系统，其特征在于，所述系统包括：

处理单元，用于在任务的第一控制周期结束后，使用粒子滤波算法，获得空间机械臂末端的空间位置误差概率分布；

评估单元，用于依据所述空间位置误差概率分布，获得空间机械臂的使用可靠度；

生成单元，用于依据所述使用可靠度和预设的可靠度阈值，获得调整量；

控制单元，用于依据预设的第一目标空间位置信息和所述调整量，获得第二目标空间位置信息；所述第二目标空间位置信息为在第二控制周期结束后所述空间机械臂末端的目标空间位置信息。