CN108724187B - 一种机器人抓取飞行物体的运动规划方法及系统 - Google Patents

Abstract

本发明公开了一种机器人抓取飞行物体的运动规划方法及系统，其基于运动捕获系统，能够对飞行物体进行精确的实时定位，保证了后续预测飞行物体的运动轨迹以及机器人抓取飞行物体的运动轨迹规划的精度；同时，采用了基于支持向量回归的算法对飞行物体的运动轨迹进行建模，降低了对飞行物体运动轨迹建模的复杂程度，并且提高了建模的精度；此外，采用了基于高斯混合回归的算法对机器人抓取飞行物体时的运动轨迹进行建模，在飞行物体位置在变动的情况下，仍然能够生成到达目标位置的轨迹，弥补了预测飞行物体轨迹存在误差的缺陷。

Description

一种机器人抓取飞行物体的运动规划方法及系统

技术领域

本发明涉及机器人系统的物体操作技术领域，尤其涉及一种机器人抓取飞行物体的运动规划方法及系统。

背景技术

机器人对静态物体以及传送带上物体这种静止或者低速状态下的物体的抓取操作技术相对比较成熟，但是对于抛出的高速运动的飞行物体的抓取还存在着诸多的挑战，在日常的生产生活中，对飞行物体的抓取还是一项必要的任务，因此，机器人抓取飞行物体的运动规划也就成为了一项关键技术。

机器人抓取飞行物体，整个任务包括：

(1)对环境中的高速运动的飞行物体进行实时识别和定位；

(2)能够预测飞行物体的运动轨迹；

(3)针对预测飞行物体轨迹误差较大的情况，需要能够不断的预测飞行物体的运动轨迹，因此机器人抓取飞行物体的运动轨迹要能够随着飞行物体轨迹的变化而变化，以完成抓取任务。

目前可以使用运动捕获系统来实现对飞行物体的飞行物体进行实时定位，然而实时预测飞行物体的运动轨迹还是一件比较困难的事情，并且由于预测时会产生误差，所以需要不断的预测运动轨迹，以减少误差，对机器人的抓取运动规划来说，抓取位置是一直在变动的，这又给抓取飞行物体带来了更大的挑战。

发明内容

本发明的目的是提供一种机器人抓取飞行物体的运动规划方法及系统，能实现使机器人成功抓取高速运动的飞行物体的任务。

本发明的目的是通过以下技术方案实现的：

一种机器人抓取飞行物体的运动规划方法，包括：

步骤S1、根据多组运动捕获系统记录的飞行物体的飞行轨迹，利用支持向量回归算法构建飞行物体飞行时的运动轨迹模型；并将当前飞行物体的位置和速度输入运动轨迹模型，得到下一时刻飞行物体的位置和速度，如此往复循环，得到一整条飞行物体飞行时的运动轨迹，并通过预设抓取平面，确定抓取飞行物体的抓取位置；

步骤S2、通过所述运动捕获系统，记录多组人工示教拖动机器人抓取飞行物体时的机器人末端执行器的运动轨迹，利用高斯混合回归算法构建所述机器人抓取飞行物体时的运动轨迹模型，将抓取位置输入到所述运动轨迹模型中，得到所述机器人相应的速度，根据间隔时间得到所述机器人下一时刻的位置，如此循环往复，得到所述机器人抓取飞行物体时的运动轨迹，从而实现抓取飞行物体。

由上述本发明提供的技术方案可以看出，1)基于运动捕获系统，能够对飞行物体进行精确的实时定位，保证了后续预测飞行物体的运动轨迹以及机器人抓取飞行物体的运动轨迹规划的精度。2)采用了基于支持向量回归的算法对飞行物体的运动轨迹进行建模，降低了对飞行物体运动轨迹建模的复杂程度，并且提高了建模的精度。3)采用了基于高斯混合回归的算法对机器人抓取飞行物体时的运动轨迹进行建模，在飞行物体位置在变动的情况下，仍然能够生成到达目标位置的轨迹，弥补了预测飞行物体轨迹存在误差的缺陷。

附图说明

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

图1为本发明实施例提供的一种机器人抓取飞行物体的运动规划方法的流程图；

图2为本发明实施例提供的预测飞行物体轨迹的误差示意图；

图3为本发明实施例提供的在飞行物体发生变动时，机器人的运动轨迹示意图；

图4为本发明实施例提供的机器人抓取飞行物体时，机器人末端执行器运动轨迹以及飞行物体运动轨迹示意图；

图5为本发明实施例提供的一种机器人抓取飞行物体的运动规划系统的示意图。

具体实施方式

下面结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明的保护范围。

本发明实施例提供一种机器人抓取飞行物体的运动规划方法，为了保证机器人抓取飞行物体的成功，需要考虑到如下三个方面：1、需要建立一个能够预测飞行物体飞行时运动轨迹的模型；2、需要根据飞行物体的运动轨迹，确定机器人的抓取位置和抓取时间；3、需要对机器人末端执行器进行运动规划，以保证机器人能够在相应的时间内到达抓取位置，完成对飞行物体的抓取。

本发明实施例提供的上述运动规划方法应用于由机器人、运动捕获系统及飞行物体所组成的作业环境中；所述运动捕获系统由六台运动捕获相机组成，所述运动捕获系统能对运动捕获相机视野内的飞行物体进行实时定位；所述机器人安装在运动捕获相机视野内；所述飞行物体上贴有能够被所述运动捕获系统识别的标记点。

本领域技术人员可以理解，运动捕获系统主要实现对飞行物体进行精确的实时定位，如背景技术中记载可知，该运动捕获系统可以通过常规技术来实现。

如图1所示，为本发明实施例提供的一种机器人抓取飞行物体的运动规划方法的流程图，其主要包括：

步骤S1、根据多组运动捕获系统记录的飞行物体的飞行轨迹，利用支持向量回归算法构建飞行物体飞行时的运动轨迹模型；并将当前飞行物体的位置和速度输入运动轨迹模型，得到下一时刻飞行物体的位置和速度，如此往复循环，得到一整条飞行物体飞行时的运动轨迹，并通过预设抓取平面，确定抓取飞行物体的抓取位置。

本步骤主要包括：

步骤S11、利用所述运动捕获系统记录下每一次飞行物体的运动轨迹，将这些飞行物体的运动轨迹组成数据集

其中，^oξ∈R^D,

分别表示的是飞行物体的位置、速度与加速度；D表示飞行物体的位置的维度，N表示轨迹的条数，T表示一条轨迹上的轨迹点的个数。

通常情况下，可以通过抛出贴有标记点的飞行物体多次，利用运动捕获系统记录下每一次的飞行物体的运动轨迹。

步骤S12、利用支持向量回归算法构建飞行物体飞行时的运动轨迹模型，表示为：

K(ζ,^dζ_m)＝exp(-γ||ζ-ζ_m||²)

式中，

是由飞行物体的位置和速度所组成的模型的状态变量；M表示的是支持向量的个数；ζ_m表示的是第m个支持向量d表示第d个支持向量回归模型；K(ζ,^dζ_m)表示支持向量回归模型中的核函数；α_m表示的是核函数的系数；^db表示第d个支持向量回归模型的偏置；γ表示一个常数因子。

步骤S13、使用所述运动捕获系统得到飞行物体在初始时刻t₁及t₂＝t₁+Δt时刻的位置，分别记为p₁、p₂，从而得到飞行物体的运动速度v₂为：

v₂＝(p₂-p₁)/Δt。

其中，Δt为两个相邻时刻的时间间隔；

步骤S14、将飞行物体的位置p₂和速度v₂输入所述运动轨迹模型，得到飞行物体的加速度a₂，然后得到t₃时刻飞行物体的速度和位置为：

v₃＝v₂+a₂·Δt

p₃＝v₂·Δt+1/2·a₂·Δt²。

步骤S15、执行上述步骤S14若干次后，得到一整条飞行物体飞行时的运动轨迹，将机器人所处的平面S作为抓取平面，平面S与运动轨迹的交点即为抓取位置，并得到初始时刻t₁的抓取位置A₁距离抓取位置的时间为T₁。

步骤S16、从初始时刻t₁开始，取t_i时刻对应的轨迹观测点，t_i+1＝t_i+Δt，i＝1...I；即取t_i时刻飞行物体的位置和速度，作为飞行物体的初始状态变量，重复步骤S13～步骤S15，得到抓取位置A_i,i＝1...n，并得到每个初始点距离抓取位置的时间为T_i,i＝1...n。

步骤S2、通过所述运动捕获系统，记录多组人工示教拖动机器人抓取飞行物体时的机器人末端执行器的运动轨迹，利用高斯混合回归算法构建所述机器人抓取飞行物体时的运动轨迹模型，将抓取位置输入到所述运动轨迹模型中，得到所述机器人相应的速度，根据间隔时间得到所述机器人下一时刻的位置，如此循环往复，得到所述机器人抓取飞行物体时的运动轨迹，从而实现抓取飞行物体。

本步骤主要包括：

步骤S21、通过所述运动捕获系统，记录多组人工示教拖动机器人抓取飞行物体时的机器人末端执行器的运动轨迹，将这些运动轨迹组成数据集

其中，

分别表示的是t_i时刻对应的轨迹观测点中机器人末端执行器的位置和速度，N′表示运动轨迹的条数，T′表示一条运动轨迹上的轨迹点的个数。

通常情况下，可以在机器人末端执行器上贴有几个标记点，便于运动捕获系统记录末端执行器位置，抛出飞行物体多次，人为拖动机器人去触碰飞行物体，利用运动捕获系统记录每一次机器人末端执行器的运动轨迹。

步骤S22、利用高斯混合模型算法表示所述机器人抓取飞行物体时的运动轨迹的概率密度函数，表示为：

式中，K表示高斯混合模型中所用的高斯函数的层数，θ表示模型参数，即μ^k、∑^k以及p(k)；p(k)为每一层高斯函数所占的权重，k为层数的序号，μ^k和∑^k分别为均值和协方差矩阵；d为机器人末端执行器位置的维度；通常来说，机器人末端执行器位置的维度为三维，即d＝3。

步骤S23、使用最大似然估计估计模型参数，即μ^k、∑^k以及p(k)；并且使用k-means算法对最大似然估计初始化。

步骤S24、使用高斯混合回归获得

和^rξ_i的函数关系；根据输入的机器人末端执行器的位置^rξ_i，由高斯混合模型计算出联合概率密度，取期望值乘以一个速度因子λ作为机器人末端执行器的速度

机器人的运动轨迹模型写为：

上式中，A^k、b^k、h^k(^rξ_i)没有具体含义，主要为了简化模型表达式，避免公式太长；

对应的表示

的均值，

为^rξ_i的协方差矩阵；p(^rξ_i|k)、p(^rξ_i|j)对应的表示输入为^rξ_i时，第k层、第j层高斯模型的概率密度函数；

步骤S25、将步骤S16得到的t_i时刻的抓取位置A_i与机器人末端执行器的初始位置之差输入机器人的运动轨迹模型，从而估计机器人此刻的速度，通过积分即可得到机器人末端执行器下一时刻，即t_i+1时刻的位置p_i+1：

步骤S26、将位置p_i+1与抓取位置A_i之差输入机器人的运动轨迹模型，估计出机器人此刻的速度，通过积分得到机器人末端执行器下一时刻的位置，如此循环往复，直到机器人末端执行器位置与抓取位置之差为0，记此时耗费的时间记为

利用时间控制器计算t_i+1时刻的速度因子λ_ti+1：

式中，t_i表示的是在机器人运动轨迹规划中的第i个时刻，t_i+1＝t_i+Δt，Δt为两个相邻时刻的间隔，初始时刻t₁＝0；

为t_i时刻的速度因子，速度因子在初始时刻的值为

k_p和k_d分别为比例因子和微分因子，T_i为步骤S16中得到的飞行物体在t_i时刻位置距离抓取位置的时间，

为t_i-1时刻估计到达抓取位置A_i-1的耗时。

步骤S27、由步骤S26能够计算出t_i+1时刻速度因子λ_ti+1，从而得到机器人末端执行器速度

由步骤S25中公式即可计算得到t_i+2时刻机器人末端执行器的位置，并发送给机器人执行，由步骤S16中得到t_i+1时刻对应的抓取位置和距离抓取位置的时间T_i+1，然后重复步骤S25～步骤S27，如此往复循环，直到机器人到达最终的抓取位置，完成对飞行物体的抓取。

本发明实施例上述方案中，建立了飞行物体运动轨迹预测模型，用以预测飞行物体的运动轨迹，并根据预设的抓取平面计算抓取位置和抓取时间，再构建了机器人末端执行器运动轨迹模型，根据计算出来的抓取位置和抓取时间，生成机器人末端执行器运动轨迹，发送给机器人执行，以实现对飞行物体的抓取。

如图2所示，为预测飞行物体轨迹的误差示意图。可以发现，随着预测的初始时刻越来越接近终止时刻，预测的误差在一步步减小，最终在距离终止点0.2s时，误差减小1cm，可以很好的满足抓取要求。如图3所示，为在飞行物体发生变动时，机器人的运动轨迹示意图，可以发现，随着目标点的变动，机器人能够实时规划出到达目标点的轨迹。如图4所示，为机器人抓取飞行物体时，机器人末端执行器运动轨迹以及飞行物体运动轨迹示意图，其中实线表示的机器人末端执行器的运动轨迹，虚线表示的是飞行物体的飞行轨迹；从图中可以看出，机器人能够抓取飞行物体，本文提出的抓取飞行物体的运动规划算法是有效的。

本发明实施例上述方案主要具有如下优点：

1)基于运动捕获系统，能够对飞行物体进行精确的实时定位，保证了后续预测飞行物体的运动轨迹以及机器人抓取飞行物体的运动轨迹规划的精度。

2)采用了基于支持向量回归的算法对飞行物体的运动轨迹进行建模，降低了对飞行物体运动轨迹建模的复杂程度，并且提高了建模的精度。

3)采用了基于高斯混合回归的算法对机器人抓取飞行物体时的运动轨迹进行建模，在飞行物体位置在变动的情况下，仍然能够生成到达目标位置的轨迹，弥补了预测飞行物体轨迹存在误差的缺陷。

4)采用了时间控制器来加速机器人抓取飞行物体的运动规划，保证了机器人能够在期望时间内到达目标位置，从而实现快速、精确的飞行物体抓取。

本发明另一实施例还提供一种机器人抓取飞行物体的运动规划系统，该系统用于实现上述运动规划方法，如图5所示，其主要包括：

飞行物体运动轨迹预测模块，用于根据多组运动捕获系统记录的飞行物体的飞行轨迹，利用支持向量回归算法构建飞行物体飞行时的运动轨迹模型；并将当前飞行物体的位置和速度输入运动轨迹模型，得到下一时刻飞行物体的位置和速度，如此往复循环，得到一整条飞行物体飞行时的运动轨迹，并通过预设抓取平面，确定抓取飞行物体的抓取位置；

机器人运动轨迹规划模块，用于通过所述运动捕获系统，记录多组人工示教拖动机器人抓取飞行物体时的机器人末端执行器的运动轨迹，利用高斯混合回归算法构建所述机器人抓取飞行物体时的运动轨迹模型，将抓取位置输入到所述运动轨迹模型中，得到所述机器人相应的速度，根据间隔时间得到所述机器人下一时刻的位置，如此循环往复，得到所述机器人抓取飞行物体时的运动轨迹，从而实现抓取飞行物体。

本发明实施例中，所述飞行物体运动轨迹预测模块执行如下步骤：

步骤S11、利用所述运动捕获系统记录下每一次飞行物体的运动轨迹，将这些飞行物体的运动轨迹组成数据集

其中，^oξ∈R^D,

分别表示的是飞行物体的位置、速度与加速度；D表示飞行物体的位置的维度，N表示轨迹的条数，T表示一条轨迹上的轨迹点的个数；

步骤S12、利用支持向量回归算法构建飞行物体飞行时的运动轨迹模型，表示为：

K(ζ,^dζ_m)＝exp(-γ||ζ-ζ_m||²)

式中，

是由飞行物体的位置和速度所组成的模型的状态变量；M表示的是支持向量的个数；ζ_m表示的是第m个支持向量；d表示第d个支持向量回归模型；K(ζ,^dζ_m)表示支持向量回归模型中的核函数；α_m表示的是核函数的系数；^db表示第d个支持向量回归模型的偏置；γ表示一个常数因子。

步骤S13、使用所述运动捕获系统得到飞行物体在初始时刻t₁及t₂＝t₁+Δt时刻的位置，分别记为p₁、p₂，从而得到飞行物体的运动速度v₂为：

v₂＝(p₂-p₁)/Δt

其中，Δt为两个相邻时刻的间隔；

步骤S14、将飞行物体的位置p₂和速度v₂输入所述运动轨迹模型，得到飞行物体的加速度a₂，然后得到t₃时刻飞行物体的速度和位置为：

v₃＝v₂+a₂·Δt

p₃＝v₂·Δt+1/2·a₂·Δt²

步骤S15、执行上述步骤S14若干次后，得到一整条飞行物体飞行时的运动轨迹，将机器人所处的平面S作为抓取平面，平面S与运动轨迹的交点即为抓取位置，并得到初始时刻t₁的抓取位置A₁距离抓取位置的时间为T₁；

步骤S16、从初始时刻t₁开始，取t_i时刻对应的轨迹观测点，t_i+1＝t_i+Δt，i＝1...I，即取t_i时刻飞行物体的位置和速度，作为飞行物体的初始状态变量，重复步骤S13～步骤S15，得到抓取位置A_i,i＝1...n，并得到每个初始点距离抓取位置的时间为T_i,i＝1...n。

本发明实施例中，所述机器人运动轨迹规划模块执行如下步骤：

步骤S21、通过所述运动捕获系统，记录多组人工示教拖动机器人抓取飞行物体时的机器人末端执行器的运动轨迹，将这些运动轨迹组成数据集

其中，

分别表示的是t_i时刻对应的轨迹观测点中机器人末端执行器的位置和速度，N′表示运动轨迹的条数，T′表示一条运动轨迹上的轨迹点的个数；

步骤S22、利用高斯混合模型算法表示所述机器人抓取飞行物体时的运动轨迹的概率分布函数，表示为：

式中，K表示高斯混合模型中所用的高斯函数的层数，θ表示模型参数，即μ^k、∑^k以及p(k)；p(k)为每一层高斯函数所占的权重，k为层数的序号，μ^k和∑^k分别为均值和协方差矩阵；d为机器人末端执行器位置的维度；

步骤S23、使用最大似然估计估计模型参数，即μ^k、∑^k以及p(k)；并且使用k-means算法对最大似然估计初始化；

步骤S24、使用高斯混合回归获得^rξ_i和^rξ_i的函数关系；根据输入的机器人末端执行器的位置^rξ_i，由高斯混合模型计算出联合概率密度，取期望值乘以一个速度因子λ作为机器人末端执行器的速度

机器人的运动轨迹模型写为：

上式中，A^k、b^k、h^k(^rξ_i)没有具体含义，主要为了简化模型表达式，避免公式太长；

对应的表示

的均值，

为^rξ_i的协方差矩阵；p(^rξ_i|k)、p(^rξ_i|j)对应的表示输入为^rξ_i时，第k层、第j层高斯模型的概率密度函数；

步骤S25、将步骤S16得到的t_i时刻的抓取位置A_i与机器人末端执行器的初始位置之差输入机器人的运动轨迹模型，从而估计机器人此刻的速度，通过积分即可得到机器人末端执行器下一时刻，即t_i+1时刻的位置p_i+1：

步骤S26、将位置p_i+1与抓取位置A_i之差输入机器人的运动轨迹模型，估计出机器人此刻的速度，通过积分得到机器人末端执行器下一时刻的位置，如此循环往复，直到机器人末端执行器位置与抓取位置之差为0，记此时耗费的时间记为

利用时间控制器计算t_i+1时刻的速度因子

式中，t_i表示的是在机器人运动轨迹规划中的第i个时刻，t_i+1＝t_i+Δt，Δt为两个相邻时刻的间隔，初始时刻t₁＝0；

为t_i时刻的速度因子，速度因子在初始时刻的值为

k_p和k_d分别为比例因子和微分因子，T_i为步骤S16中得到的飞行物体在t_i时刻位置距离抓取位置的时间，

为t_i-1时刻估计到达抓取位置A_i-1的耗时。

步骤S27、由步骤S26能够计算出t_i+1时刻速度因子

从而得到机器人末端执行器速度

由S25中公式即可计算得到t_i+2时刻机器人末端执行器的位置，并发送给机器人执行，由步骤S16中得到t_i+1时刻对应的抓取位置和距离抓取位置的时间T_i+1，然后重复步骤S25～步骤S27，如此往复循环，直到机器人到达最终的抓取位置，完成对飞行物体的抓取。

本发明实施例中，所述运动规划系统应用于由机器人、运动捕获系统及‘’飞行物体所组成的作业环境中；所述运动捕获系统由六台运动捕获相机组成，所述运动捕获系统能对运动捕获相机视野内的飞行物体进行实时定位；所述机器人安装在运动捕获相机视野内；所述飞行物体上贴有能够被所述运动捕获系统识别的标记点。

需要说明的是，上述系统中包含的各个功能模块所实现的功能的具体实现方式在前面的实施例中已经有详细描述，故在这里不再赘述。

所属领域的技术人员可以清楚地了解到，为描述的方便和简洁，仅以上述各功能模块的划分进行举例说明，实际应用中，可以根据需要而将上述功能分配由不同的功能模块完成，即将系统的内部结构划分成不同的功能模块，以完成以上描述的全部或者部分功能。

通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到上述实施例可以通过软件实现，也可以借助软件加必要的通用硬件平台的方式来实现。基于这样的理解，上述实施例的技术方案可以以软件产品的形式体现出来，该软件产品可以存储在一个非易失性存储介质(可以是CD-ROM，U盘，移动硬盘等)中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例所述的方法。

以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明披露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应该以权利要求书的保护范围为准。

Claims (4)

1.一种机器人抓取飞行物体的运动规划方法，其特征在于，包括：

步骤S1、根据多组运动捕获系统记录的飞行物体的飞行轨迹，利用支持向量回归算法构建飞行物体飞行时的运动轨迹模型；并将当前飞行物体的位置和速度输入运动轨迹模型，得到下一时刻飞行物体的位置和速度，如此往复循环，得到一整条飞行物体飞行时的运动轨迹，并通过预设抓取平面，确定抓取飞行物体的抓取位置；

步骤S2、通过所述运动捕获系统，记录多组人工示教拖动机器人抓取飞行物体时的机器人末端执行器的运动轨迹，利用高斯混合回归算法构建所述机器人抓取飞行物体时的运动轨迹模型，将抓取位置输入到所述运动轨迹模型中，得到所述机器人相应的速度，根据间隔时间得到所述机器人下一时刻的位置，如此循环往复，得到所述机器人抓取飞行物体时的运动轨迹，从而实现抓取飞行物体；

所述步骤S1包括：

步骤S11、利用所述运动捕获系统记录下飞行物体每一次的运动轨迹，将飞行物体的这些运动轨迹组成数据集

其中，^oξ∈R^D,

分别表示的是飞行物体的位置、速度与加速度；D表示飞行物体的位置的维度，N表示轨迹的条数，T表示一条轨迹上的轨迹点的个数；

步骤S12、利用支持向量回归算法构建飞行物体飞行时的运动轨迹模型，表示为：

K(ζ,^dζ_m)＝exp(-γ||ζ-ζ_m||²)

式中，

是由飞行物体的位置和速度所组成的模型的状态变量；M表示的是支持向量的个数；ζ_m表示的是第m个支持向量；d表示第d个支持向量回归模型；α_m表示的是核函数的系数；K(ζ,^dζ_m)表示支持向量回归模型中的核函数；^db表示第d个支持向量回归模型的偏置；γ表示一个常数因子；

步骤S13、使用所述运动捕获系统得到飞行物体在初始时刻t₁及t₂＝t₁+Δt时刻的位置，分别记为p₁、p₂，从而得到飞行物体的运动速度v₂为：

v₂＝(p₂-p₁)/Δt

步骤S14、将飞行物体的位置p₂和速度v₂输入所述机器人抓取飞行物体时的运动轨迹模型，得到飞行物体的加速度a₂，然后得到t₃时刻飞行物体的速度和位置为：

v₃＝v₂+a₂·Δt

p₃＝v₂·Δt+1/2·a₂·Δt²

步骤S15、执行上述步骤S14若干次后，得到一整条飞行物体飞行时的运动轨迹，将机器人所处的平面S作为抓取平面，平面S与运动轨迹的交点即为抓取位置，并得到初始时刻t₁的抓取位置A₁距离抓取位置的时间为T₁；

步骤S16、从初始时刻t₁开始，取t_i时刻对应的轨迹观测点，t_i+1＝t_i+Δt，i＝1...I，即取t_i时刻飞行物体的位置和速度，作为飞行物体的初始状态变量，重复步骤S13～步骤S15，得到抓取位置A_i,i＝1...n，并得到每个初始点距离抓取位置的时间为T_i,i＝1...n；

所述步骤S2包括：

步骤S21、通过所述运动捕获系统，记录多组人工示教拖动机器人抓取飞行物体时的机器人末端执行器的运动轨迹，将这些运动轨迹组成数据集

其中，^rξ_i,

分别表示的是t_i时刻对应的轨迹观测点中机器人末端执行器的位置和速度，N′表示运动轨迹的条数，T′表示一条运动轨迹上的轨迹点的个数；

步骤S22、利用高斯混合模型算法表示所述机器人抓取飞行物体时的运动轨迹的概率分布函数，表示为：

式中，K表示高斯混合模型中所用的高斯函数的层数，θ表示模型参数，即μ^k、∑^k以及p(k)；p(k)为每一层高斯函数所占的权重，k为层数的序号，μ^k和∑^k分别为均值和协方差矩阵；d为机器人末端执行器位置的维度；

步骤S23、使用最大似然估计估计模型参数，即μ^k、∑^k以及p(k)；并且使用k-means算法对最大似然估计初始化；

步骤S24、使用高斯混合回归算法获得

和^rξ_i的函数关系；根据输入的机器人末端执行器的位置^rξ_i，由高斯混合模型计算出联合概率密度，取期望值乘以一个速度因子λ作为机器人末端执行器的速度

机器人的运动轨迹模型写为：

上式中，

分别表示^rξ_i、

的均值，

为^rξ_i的协方差矩阵；p(^rξ_i|k)、p(^rξ_i|j)分别表示输入为^rξ_i时，第k层、第j层高斯模型的概率密度函数；

步骤S25、将步骤S16得到的t_i时刻的抓取位置A_i与机器人末端执行器的初始位置之差输入机器人的运动轨迹模型，从而估计机器人此刻的速度，通过积分即可得到机器人末端执行器下一时刻，即t_i+1时刻的位置p_i+1：

步骤S26、将位置p_i+1与抓取位置A_i之差输入机器人的运动轨迹模型，估计出机器人此刻的速度，通过积分得到机器人末端执行器下一时刻的位置，如此循环往复，直到机器人末端执行器位置与抓取位置之差为0，记此时耗费的时间记为

利用时间控制器计算t_i+1时刻的速度因子

式中，t_i表示的是在机器人运动轨迹规划中的第i个时刻，t_i+1＝t_i+Δt，Δt为两个相邻时刻的间隔，初始时刻t₁＝0；

为t_i时刻的速度因子，速度因子在初始时刻的值为

k_p和k_d分别为比例因子和微分因子，T_i为步骤S16中得到的飞行物体在t_i时刻位置距离抓取位置的时间，

为t_i-1时刻估计到达抓取位置A_i-1的耗时；

步骤S27、由步骤S26能够计算出t_i+1时刻速度因子

从而得到机器人末端执行器速度

由步骤S25中公式即可计算得到t_i+2时刻机器人末端执行器的位置，并发送给机器人执行，由步骤S16中得到t_i+1时刻对应的抓取位置和距离抓取位置的时间T_i+1，然后重复步骤S25～步骤S27，如此往复循环，直到机器人到达最终的抓取位置，完成对飞行物体的抓取。

2.根据权利要求1所述的一种机器人抓取飞行物体的运动规划方法，其特征在于，所述运动规划方法应用于由机器人、运动捕获系统及飞行物体所组成的作业环境中；所述运动捕获系统由六台运动捕获相机组成，所述运动捕获系统能对运动捕获相机视野内的飞行物体进行实时定位；所述机器人安装在运动捕获相机视野内；所述飞行物体上贴有能够被所述运动捕获系统识别的标记点。

3.一种机器人抓取飞行物体的运动规划系统，其特征在于，包括：

飞行物体运动轨迹预测模块，用于根据多组运动捕获系统记录的飞行物体的飞行轨迹，利用支持向量回归算法构建飞行物体飞行时的运动轨迹模型；并将当前飞行物体的位置和速度输入运动轨迹模型，得到下一时刻飞行物体的位置和速度，如此往复循环，得到一整条飞行物体飞行时的运动轨迹，并通过预设抓取平面，确定抓取飞行物体的抓取位置；

机器人运动轨迹规划模块，用于通过所述运动捕获系统，记录多组人工示教拖动机器人抓取飞行物体时的机器人末端执行器的运动轨迹，利用高斯混合回归算法构建所述机器人抓取飞行物体时的运动轨迹模型，将抓取位置输入到所述运动轨迹模型中，得到所述机器人相应的速度，根据间隔时间得到所述机器人下一时刻的位置，如此循环往复，得到所述机器人抓取飞行物体时的运动轨迹，从而实现抓取飞行物体；

所述飞行物体运动轨迹预测模块执行如下步骤：

步骤S11、利用所述运动捕获系统记录下飞行物体每一次的运动轨迹，将飞行物体的这些运动轨迹组成数据集

其中，^oξ∈R^D,

分别表示的是飞行物体的位置、速度与加速度；D表示飞行物体的位置的维度，N表示轨迹的条数，T表示一条轨迹上的轨迹点的个数；

步骤S12、利用支持向量回归算法构建飞行物体飞行时的运动轨迹模型，表示为：

K(ζ,^dζ_m)＝exp(-γ||ζ-ζ_m||²)

式中，

是由飞行物体的位置和速度所组成的模型的状态变量；M表示的是支持向量的个数；ζ_m表示的是第m个支持向量；d表示第d个支持向量回归模型；α_m表示的是核函数的系数；K(ζ,^dζ_m)表示支持向量回归模型中的核函数；^db表示第d个支持向量回归模型的偏置；γ表示一个常数因子；

步骤S13、使用所述运动捕获系统得到飞行物体在初始时刻t₁及t₂＝t₁+Δt时刻的位置，分别记为p₁、p₂，从而得到飞行物体的运动速度v₂为：

v₂＝(p₂-p₁)/Δt

步骤S14、将飞行物体的位置p₂和速度v₂输入所述机器人抓取飞行物体时的运动轨迹模型，得到飞行物体的加速度a₂，然后得到t₃时刻飞行物体的速度和位置为：

v₃＝v₂+a₂·Δt

p₃＝v₂·Δt+1/2·a₂·Δt²

步骤S15、执行上述步骤S14若干次后，得到一整条飞行物体飞行时的运动轨迹，将机器人所处的平面S作为抓取平面，平面S与运动轨迹的交点即为抓取位置，并得到初始时刻t₁的抓取位置A₁距离抓取位置的时间为T₁；

步骤S16、从初始时刻t₁开始，取t_i时刻对应的轨迹观测点，t_i+1＝t_i+Δt，i＝1...I，即取t_i时刻飞行物体的位置和速度，作为飞行物体的初始状态变量，重复步骤S13～步骤S15，得到抓取位置A_i,i＝1...n，并得到每个初始点距离抓取位置的时间为T_i,i＝1...n；

所述机器人运动轨迹规划模块执行如下步骤：

步骤S21、通过所述运动捕获系统，记录多组人工示教拖动机器人抓取飞行物体时的机器人末端执行器的运动轨迹，将这些运动轨迹组成数据集

其中，^rξ_i,

分别表示的是t_i时刻对应的轨迹观测点中机器人末端执行器的位置和速度，N′表示运动轨迹的条数，T′表示一条运动轨迹上的轨迹点的个数；

步骤S22、利用高斯混合模型算法表示所述机器人抓取飞行物体时的运动轨迹的概率分布函数，表示为：

式中，K表示高斯混合模型中所用的高斯函数的层数，θ表示模型参数，即μ^k、∑^k以及p(k)；p(k)为每一层高斯函数所占的权重，k为层数的序号，μ^k和∑^k分别为均值和协方差矩阵；d为机器人末端执行器位置的维度；

步骤S23、使用最大似然估计估计模型参数，即μ^k、∑^k以及p(k)；并且使用k-means算法对最大似然估计初始化；

步骤S24、使用高斯混合回归算法获得

和^rξ_i的函数关系；根据输入的机器人末端执行器的位置^rξ_i，由高斯混合模型计算出联合概率密度，取期望值乘以一个速度因子λ作为机器人末端执行器的速度

机器人的运动轨迹模型写为：

上式中，

分别表示^rξ_i、

的均值，

为^rξ_i的协方差矩阵；p(^rξ_i|k)、p(^rξ_i|j)分别表示输入为^rξ_i时，第k层、第j层高斯模型的概率密度函数；

步骤S25、将步骤S16得到的t_i时刻的抓取位置A_i与机器人末端执行器的初始位置之差输入机器人的运动轨迹模型，从而估计机器人此刻的速度，通过积分即可得到机器人末端执行器下一时刻，即t_i+1时刻的位置p_i+1：

步骤S26、将位置p_i+1与抓取位置A_i之差输入机器人的运动轨迹模型，估计出机器人此刻的速度，通过积分得到机器人末端执行器下一时刻的位置，如此循环往复，直到机器人末端执行器位置与抓取位置之差为0，记此时耗费的时间记为

利用时间控制器计算t_i+1时刻的速度因子

式中，t_i表示的是在机器人运动轨迹规划中的第i个时刻，t_i+1＝t_i+Δt，Δt为两个相邻时刻的间隔，初始时刻t₁＝0；

为t_i时刻的速度因子，速度因子在初始时刻的值为

k_p和k_d分别为比例因子和微分因子，T_i为步骤S16中得到的飞行物体在t_i时刻位置距离抓取位置的时间，

为t_i-1时刻估计到达抓取位置A_i-1的耗时；

步骤S27、由步骤S26能够计算出t_i+1时刻速度因子

从而得到机器人末端执行器速度

由步骤S25中公式即可计算得到t_i+2时刻机器人末端执行器的位置，并发送给机器人执行，由步骤S16中得到t_i+1时刻对应的抓取位置和距离抓取位置的时间T_i+1，然后重复步骤S25～步骤S27，如此往复循环，直到机器人到达最终的抓取位置，完成对飞行物体的抓取。

4.根据权利要求3所述的一种机器人抓取飞行物体的运动规划系统，其特征在于，所述运动规划系统应用于由机器人、运动捕获系统及飞行物体所组成的作业环境中；所述运动捕获系统由六台运动捕获相机组成，所述运动捕获系统能对运动捕获相机视野内的飞行物体进行实时定位；所述机器人安装在运动捕获相机视野内；所述飞行物体上贴有能够被所述运动捕获系统识别的标记点。

Images