CN106651949A - 一种基于仿真的空间机械臂抓捕目标遥操作方法及*** - Google Patents

Abstract

本发明涉及一种基于仿真的空间机械臂抓捕目标遥操作***，包括数据源采集模块、中间件模块、目标追踪与定位模块、操作规划模块、运动模拟模块等，通过插件式的构建策略实现了***对多种不同类型数据源的实时采集，实现了数据采集和数据处理速度不一致的调节，采用快速跟踪算法保证摄像头对目标追踪的实时性，并通过位姿测量算法保证真实目标与模拟目标位姿的同步性。本发明可为空间机械臂对动态目标捕获过程中的视觉定位算法提供验证平台，同时也能够通过三维可视化的方式将抓捕过程进行演示，辅助地面操作人员完成对抓捕过程的控制。

Description

一种基于仿真的空间机械臂抓捕目标遥操作方法及***

技术领域

本发明涉及机械臂遥操作领域，具体涉及一种基于仿真的空间机械臂抓捕目标遥操作方法及***。

背景技术

空间机械臂是空间站建设和维护必不可少的关键设备，能够在轨完成航天器的组装和拆分、维护与修理、燃料输送、卫星释放与回收以及空间站内进行科学探测实验等各种任务，可以减少航天员的舱外活动，避免生命危险且节省出舱费用。我国将于2022年左右，建造完成载人空间站工程***，突破和掌握近地空间长期载人飞行技术，开展近地空间科技实验，增强综合开发利用太空资源能力。随着航天事业的发展，空间机械臂将发挥越来越重要的作用。

地面摇操作是空间机械臂操控的三大模式之一，是空间机械臂执行各种复杂空间任务必须依赖的重要操控方法。在空间机械臂执行飞行器抓捕、组件模块更换等任务时，机械臂携带的视觉***拍摄***作对象的状态。地面操控人员根据下传图像观察空间中的操作过程，然而由于空间航天器与地面遥操作中心的传输网络带宽有限且传输过程存在一定的时延，使得操作人员无法实时观察到操作过程，也无法精确预知机械臂与***作对象的相对位置关系。当机械臂与***作对象距离较近时，则容易发生非预知性碰撞，损坏机械臂、飞行器和组件模块。

发明内容

本发明所要解决的技术问题是提供利用空间机械臂携带的视觉***拍摄的图像测量机械臂末端相对于被抓捕目标位姿，进而预测机械臂的运动轨迹，辅助地面遥操作人员完成操控决策的仿真方法，以解决遥操作人员因无法预估空间机械臂与目标的相对状态而难以有效决策的问题。。

本发明解决上述技术问题的技术方案如下：一种基于仿真的空间机械臂抓捕目标遥操作方法，其特征在于，包括以下步骤：

步骤1、数据源采集模块通过多种类型的插件从对应类型的数据源采集目标的图像数据并统一转换成相同格式后发送至中间件模块；

步骤2、中间件模块接收并存储经过转换的图像数据并发送至目标追踪与定位模块进行处理，并保持数据源采集模块采集图像数据的速度和目标追踪与定位模块处理图像数据的速度之间的差值保持在预设范围内；

步骤3、目标追踪与定位模块对经过转换的图像数据进行处理，得到目标的位姿；

步骤4、操作规划模块根据目标的位姿和获取的机械臂末端位姿，规划空间机械臂各关节的运动；

步骤5、运动模拟模块建立空间机械臂三维模型和目标三维模型，并根据操作规划模块规划的机械臂各关节的运动，实时改变空间机械臂三维模型的位姿和目标三维模型的位姿；

步骤6、重复以上步骤，直到在运动模拟模块中完成机械臂对目标的捕获。

在上述技术方案的基础上，本发明还可以做如下改进。

进一步，所述步骤2具体包括以下步骤：

步骤2.1、插件管理单元对多种类型的插件进行管理与调配，接收经过转换的图像数据并存储在自适应缓冲区单元中；

步骤2.2、所述自适应缓冲区单元使所述数据源采集模块的数据采集速度和所述目标追踪与定位模块的处理速度之间的差值保持在预设范围内；

步骤2.3、数据输出接口单元将所述自适应缓冲区中的图像数据发送至目标跟踪与定位模块。

进一步，所述步骤2.2具体包括以下步骤：

设定缓冲区允许的最大帧差为F_BMAX，缓冲区允许的最大帧差的上限为F_MAX，丢弃帧标记数为F_Tf，图像采集和图像处理的帧差为F_P-C；

步骤2.2.1，如果F_P-C＞F_BMAX，执行F_Tf＝F_Tf+1，则跳转到步骤2.2.2；

如果F_P-C<F_BMAX/2，则跳转到步骤2.2.3；

如果F_P-C≥F_BMAX/2，并且F_P-C≤F_BMAX，则跳转到步骤2.2.4；

步骤2.2.2，如果F_Tf≥F_BMAX/2，执行F_BMAX＝2*F_BMAX，跳转到步骤2.2.6；如果F_Tf<F_BMAX/2，直接跳转到步骤2.2.6；

步骤2.2.3，如果F_P-C≤1，则令F_Tf＝0，否则执行F_Tf＝(F_Tf+1)/2，直到2*F_Tf≤F_P-C，执行完上述计算后跳转到步骤2.2.5；

步骤2.2.4，如果F_Tf>0，则执行F_Tf＝F_Tf-1，直到2*F_Tf≤F_P-C；

步骤2.2.5，如果F_BMAX>F_MAX，则令F_BMAX＝F_BMAX/2；

步骤2.2.6，如果F_Tf>0，丢弃缓冲区中存储的2*F_Tf-1帧图像数据，然后将第2*F_Tf帧图像数据推送给目标追踪与定位模块，即F_P-C＝F_P-C-2*F_Tf。

进一步，所述步骤3具体包括以下步骤：

步骤3.1、接收经过转换的图像数据并进行预处理，得到当前的图像帧中目标的各条边缘；

步骤3.2、通过边缘外框计算得到当前图像帧中的目标区域，其中，所述目标区域的几何中心为目标的位置，所述目标区域的尺寸为目标的尺寸；

步骤3.3、对所述当前图像帧中的目标区域通过目标跟踪算法进行处理后得到下一图像帧中的目标区域；

步骤3.4、对所述下一图像帧中的目标区域中的像素点应用灰度加权几何中心坐标计算方法得到目标跟踪位置；

步骤3.5、计算目标投影位置并通过所述目标投影位置和所述目标跟踪位置之间的偏移误差最小化的测量模型得到每帧图像中的目标的位姿。

进一步，所述步骤3.1具体包括以下步骤：

步骤3.1.1、对图像进行高斯滤波，选定某一阈值T₁，令图像中灰度大于T₁的像素取值为255，灰度小于T₁的像素取值为0，对高斯滤波后的图像进行二值化，得到二值化图像；

步骤3.1.2、对二值化图像采用腐蚀膨胀算法处理，消除二值化图像中的孤立亮点，使得二值化图像中仅有目标区域灰度值为255，其他位置灰度值均为0；

步骤3.1.3、对腐蚀膨胀后的二值化图像进行边缘提取。

进一步，所述步骤3.3中的目标跟踪算法为Camshift和卡尔曼滤波相融合的跟踪算法。

进一步，所述步骤3.4具体为，设定某一阈值T₀，对目标区域内所有灰度值大于等于T₀的像素点按照其在经过高斯滤波处理后的图像中的位置进行灰度加权平均运算，求得灰度加权几何中心坐标，其计算公式为：

其中x_c表示灰度加权几何中心的x坐标，y_c表示灰度加权几何中心的y坐标，x表示图像中像素点的x坐标，y表示图像中像素点的y坐标，I(x,y)表示坐标(x,y)处的灰度值，R表示目标区域。

进一步，所述步骤3.5具体包括以下步骤：

步骤3.5.1、估计目标在世界坐标系中的三维坐标位置；

步骤3.5.2、根据所述三维坐标位置，利用透视投影模型计算目标在图像中的投影坐标，得到目标投影位置；

步骤3.5.3、将所述目标投影位置与目标跟踪位置进行比较，计算偏移误差；

步骤3.5.4、根据所述偏移误差，利用最小二乘法修正目标在世界坐标系中的位置；

步骤3.5.5、反复多次直至收敛，即偏移误差小于预先给定的阈值。

进一步，所述步骤4具体包括以下步骤：

步骤4.1、机械臂末端运动轨迹规划单元根据由机械臂构型计算得到的机械臂末端的位姿和目标追踪与定位模块得到的目标的位姿，规划机械臂末端的移动路径；

步骤4.2、机械臂各关节协同运动规划单元根据规划得到的机械臂末端的移动路径的离散轨迹点，通过逆运动学算法求解每个离散轨迹点对应的机械臂各关节的运动角度和角速度。

一种基于仿真的空间机械臂抓捕目标遥操作***，包括：

数据源采集模块，用于通过多种类型的插件从对应类型的数据源采集目标的图像数据并统一转换成相同格式后发送至中间件模块；

中间件模块，用于接收并存储经过转换的图像数据并发送至目标追踪与定位模块进行处理，并保持数据源采集模块采集图像数据的速度和目标追踪与定位模块处理图像数据的速度之间的差值保持在预设范围内；

目标追踪与定位模块，用于对经过转换的图像数据进行处理，得到目标的位姿；

操作规划模块，用于根据目标的位姿和获取的机械臂末端位姿，规划空间机械臂各关节的运动；

运动模拟模块，用于建立空间机械臂三维模型和目标三维模型，并根据操作规划模块规划的机械臂各关节的运动，实时改变空间机械臂三维模型的位姿和目标三维模型的位姿。

进一步，所述中间件模块包括插件管理单元、自适应缓冲区单元和统一化数据输出接口单元；

所述插件管理单元，用于对多种类型的插件进行加载、管理与调配，接收经过转换的图像数据并存储在所述自适应缓冲区单元中；

所述自适应缓冲区单元，用于使所述数据源采集模块的数据采集速度和所述目标追踪与定位模块的处理速度之间的差值保持在预设范围内；

所述统一化数据输出接口单元，用于将所述自适应缓冲区中的图像数据发送至目标跟踪与定位模块；

进一步，所述目标追踪与定位模块包括图像预处理单元、目标检测单元、目标跟踪单元、位置结算单元和位姿解算单元；

所述图像预处理单元，用于接收经过转换的图像数据并进行预处理，得到当前的图像帧中目标的各条边缘；

所述目标检测单元，用于通过边缘外框计算得到当前图像帧中的目标区域，其中，所述目标区域的几何中心为目标的位置，所述目标区域的尺寸为目标的尺寸；

所述目标跟踪单元，用于对所述当前图像帧中的目标区域通过目标跟踪算法进行处理后得到下一图像帧中的目标区域；

所述位置解算单元，用于对所述下一图像帧中的目标区域中的像素点应用灰度加权几何中心坐标计算方法得到目标跟踪位置；

所述位姿解算单元，用于通过目标投影位置和所述目标跟踪位置之间的偏移误差最小化的测量模型得到每帧图像中的目标的位姿。

进一步，所述操作规划模块包括机械臂末端运动轨迹规划单元和机械臂各关节协同运动规划单元；

所述机械臂末端运动轨迹规划单元，用于根据机械臂末端的位姿和目标追踪与定位模块得到的目标的位姿，规划机械臂末端的移动路径；

所述机械臂各关节协同运动规划单元，用于根据规划得到的机械臂末端的移动路径的离散轨迹点，通过逆运动学算法求解每个离散轨迹点对应的机械臂各关节的运动角度和角速度。

本发明的有益效果是：与现有的遥操作仿真***相比，本发明设计的遥操作仿真***能够根据机械臂末端视觉***获取图像跟踪目标的相对变化并测量与估计被抓捕目标的位置和姿态，通过仿真***形象化展示给地面操作人员，辅助地面人员决策。这与现有的靠操作人员浏览图像判断机械臂和被抓捕目标相对位姿的遥操作方法相比，很大程度上减轻了操作人员对操作状态分析的负担，使得操作人员能够把主要精力放在对空间机械臂的操作与控制方面，大大提高遥操作的实施效率。。

附图说明

图1是空间机械臂抓捕目标遥操作***组成模块及各模块数据流向关系图；

图2是空间机械臂抓捕目标遥操作方法流程图；

图3是Camshift和卡尔曼滤波相融合的方法实现空间目标跟踪的算法流程图；

图4是空间机械臂三维模型和空间目标三维模型的空间关系及其运动控制步骤。

具体实施方式

以下结合附图对本发明的原理和特征进行描述，所举实例只用于解释本发明，并非用于限定本发明的范围。

如图1所示，本发明提供的一种基于仿真的空间机械臂抓捕目标的遥操作***由数据源采集模块、中间件模块、目标追踪与定位模块、操作规划模块和运动模拟模块组成，其实现方法如图2所示，包括以下步骤：

S1、数据源采集模块通过多种类型的插件从对应类型的数据源采集目标的图像数据并统一转换成相同格式后发送至中间件模块；

S2、中间件模块接收并存储经过转换的图像数据并发送至目标追踪与定位模块进行处理，并保持数据源采集模块采集图像数据的速度和目标追踪与定位模块处理图像数据的速度之间的差值保持在预设范围内；

S3、目标追踪与定位模块对经过转换的图像数据进行处理，得到目标的位姿；

S4、操作规划模块根据目标的位姿和机械臂末端位姿，规划空间机械臂各关节的运动，进行目标的捕获；

S5、运动模拟模块建立空间机械臂和目标的三维模型，并根据操作规划模块规划的机械臂各关节的运动，实时改变空间机械臂三维模型的位姿和目标三维模型的位姿；

重复以上步骤，直到在运动模拟模块中完成机械臂对目标的捕获。

以下对***的各个模块的实施方式作具体说明：

1、数据源采集模块的实施说明

如图1所示，数据源采集模块，包括摄像机图像流采集、磁盘视频流采集和网络数据流采集等多种类型的数据源采集插件，对每类插件进一步解释如下。

每类插件主要用于完成某一类型的数据源的读取，如摄像机图像流采集插件只能采集摄像机设备的图像流数据，磁盘视频流采集插件只能采集磁盘中存储的视频文件(如avi格式视频文件)，网络数据流采集插件只能读取网口收到的图像数据流。

每类插件的功能是：按照预先约定的数据格式将某一类型数据源中采集到的数据解析为单帧格式的图像数据，然后重新打包为统一格式图像数据帧，输出存放到中间件模块的自适应缓冲区单元中，例如统一格式图像数据帧可选用位图格式。

每类插件对中间件模块提供统一的数据和信息输出格式和功能接口，使得所有插件能够被中间件模块统一管理。各插件的功能接口主要包括数据源选择、数据采集控制、数据输出缓冲区设定等类型的接口。

2、中间件模块的实施说明

中间件模块，是数据源采集模块与目标追踪与定位模块交互的中介，包括插件管理单元、自适应缓冲区单元和数据输出接口单元。中间件模块利用插件管理单元，完成对不同类型插件的管理与调配。数据输出接口单元对目标追踪与定位模块提供通用化数据和功能接口，使得目标追踪与定位模块能够正确读取不同类型数据源的图像数据。中间件模块设置了自适应缓冲区，能够通过计算缓冲区中存储图像数据的数量，自动丢弃部分图像数据帧，使得数据采集端和追踪与定位的处理端尽可能同步。三个单元分别具体介绍如下。

插件管理单元能够读取数据源描述文件，并根据数据源描述文件中的信息加载和调用插件实现对各类数据源的读取。数据源描述文件主要记录数据源类型、数据源路径、数据源输出数据格式等信息。其中数据源类型用于辅助选择所加载和调用插件的类型，数据源路径和数据源输出数据格式等信息用于传送给插件，辅助插件完成正确的数据源读取和数据格式解析。

自适应缓冲区单元能够根据数据采集速度和目标追踪与定位模块的处理速度的差异，自动进行数据存储和部分数据帧的丢弃，使得目标追踪与定位模块当前处理的图像帧与数据源采集模块输入的最新图像帧之差在几帧范围内，保证数据源采集端和追踪与定位处理端的同步性。其具体实现办法是通过计算缓冲区中存储图像数据的数量，自动选择逐帧读取图像或者跳帧读取图像的模式，跳帧读取图像时，对跳过的图像帧进行丢弃。具体实现过程如下：

设定缓冲区允许的最大帧差为F_BMAX，缓冲区允许的最大帧差的上限为F_MAX，丢弃帧标记数为F_Tf，图像采集和图像处理的帧差为F_P-C；

步骤1，如果F_P-C＞F_BMAX，执行F_Tf＝F_Tf+1，则跳转到步骤2.2.2；

如果F_P-C<F_BMAX/2，则跳转到步骤2.2.3；

如果F_P-C≥F_BMAX/2，并且F_P-C≤F_BMAX，则跳转到步骤2.2.4；

步骤2，如果F_Tf≥F_BMAX/2，执行F_BMAX＝2*F_BMAX，跳转到步骤2.2.6；如果F_Tf<F_BMAX/2，直接跳转到步骤2.2.6；

步骤3，如果F_P-C≤1，则令F_Tf＝0，否则执行F_Tf＝(F_Tf+1)/2，直到2*F_Tf≤F_P-C，执行完上述计算后跳转到步骤2.2.5；

步骤4，如果F_Tf>0，则执行F_Tf＝F_Tf-1，直到2*F_Tf≤F_P-C；

步骤5，如果F_BMAX>F_MAX，则令F_BMAX＝F_BMAX/2；

步骤6，如果F_Tf>0，丢弃缓冲区中存储的2*F_Tf-1帧图像数据，然后将第2*F_Tf帧图像数据推送给目标追踪与定位模块，即F_P-C＝F_P-C-2*F_Tf。

数据输出接口单元是中间件模块和目标追踪与定位模块的接口，负责从自适应缓冲区单元中读取一帧图像，传送到目标追踪与定位模块，供目标追踪与定位模块处理使用。数据输出接口单元和目标追踪与定位模块的接口主要指图像帧数据的读取和传送接口，包括图像帧格式的描述和图像帧数据等的读取与传送。

3、目标追踪与定位模块的实施说明

目标追踪与定位模块，主要通过对数据源采集模块采集的序列图像的处理和分析，完成空间目标的实时跟踪与定位。主要包括图像预处理、空间目标的检测与跟踪和空间目标的位姿解算等功能。

图像预处理主要包括高斯滤波、二值化、腐蚀膨胀和边缘提取等预处理功能，其实现步骤为，首先对图像进行高斯滤波，选定某一阈值T1，令图像中灰度大于T1的像素取值为255，灰度小于T1的像素取值为0，对高斯滤波后的图像进行二值化，得到二值化图像；对二值化图像采用腐蚀膨胀算法处理，消除二值化图像中的孤立亮点，使得二值化图像中仅有目标区域灰度值为255，其他位置灰度值均为0；对腐蚀膨胀后的二值化图像进行边缘提取，根据各条边缘计算得到各个目标位置。

空间目标的检测，指目标在图像中位置和尺寸的计算。根据图像与处理中得到各条边缘，通过边缘外框计算可得到目标区域，对该区域的几何中心极为目标在图像中的位置，目标区域的尺寸即是目标在图像中的尺寸。

空间目标的跟踪，通过Camshift和卡尔曼滤波相融合的方法实现，如图2所示，其实现步骤为：

步骤1，卡尔曼预测：利用卡尔曼方法预测空间目标在图像中的位置，空间目标成像的状态变换方程表达为其中X_k为当前帧图像中空间目标的位置，表示预测得到的下一帧图像中空间目标的位置，F表示状态转移矩阵。预测误差的协方差由其在k时刻的协方差值获得，即其中Q_k为由于状态转移的不确定性而产生的随机误差的协方差。将空间目标的位置作为目标中心，当前帧图像中目标区域的大小作为目标尺寸，计算下一帧图像中的目标区域S_k+1；

步骤2，Camshift跟踪：主要包括3个步骤：1)利用目标模板的直方图计算卡尔曼预测的目标区域的反向投影图；3)根据目标区域的反向投影图，通过一阶矩和零阶矩运算计算目标区域反向投影图的重心(u_x,u_y)，即

其中，Ω表示目标区域的坐标集合，m₁₀和m₀₁表示目标区域反向投影图的一阶矩，m₀₀表示目标区域反向投影图的零阶矩；3)如果重心(u_x,u_y)相对于目标区域中心偏移小于某一阈值，则结束跟踪，否则以重心位置为中心更新目标区域R_k+1，重复1)到3)步骤。最终得到准确的目标区域和目标位置Z_k+1。

步骤3，卡尔曼滤波更新：利用第二步得到目标位置，通过卡尔曼滤波方法修正目标位置和目标误差的协方差矩阵。设卡尔曼滤波器的更新率K_k表示给定的目标新位置信息相对于已知目标位置信息的修正权重，H_k表示目标的观测矩阵，R_k为观测随机噪声产生的协方差矩阵，则可得：

从而可得目标的位置信息X_k和协方差矩阵P_k的后验估计值为：

基于上述目标跟踪算法，即可得到目标区域，一般根据这个目标区域的中心点得到目标跟踪位置，但是，在图像中的光照闪烁等情况下，计算出的目标跟踪位置不稳定，误差较大，因此，这里对目标区域中的像素点应用灰度加权几何中心坐标计算方法，可以求得更加稳定、准确的目标跟踪位置，其求取方式介绍如下：

设定某一阈值T₀，对目标区域内所有灰度值大于等于T₀的像素点按照其在高斯滤波图像中的位置进行灰度加权平均运算，求得灰度加权中心位置，其计算公式为：

其中x_c灰度加权几何中心的x坐标，y_c灰度加权几何中心的y坐标，x表示图像中像素点的x坐标，y表示图像中像素点的y坐标，I(x,y)表示坐标(x,y)处的灰度值，R表示目标区域。

空间目标的位姿解算，是指通过目标投影位置与目标跟踪位置之间的偏移误差最小化的测量模型求解每帧图像对应的目标在空间中的位姿。其中目标投影位置是指目标和相机光心在世界坐标系中坐标已知的条件下，可通过透视投影变换的方法计算目标发光灯在相机图像平面中的投影坐标。目标投影位置与目标跟踪位置之间的偏移误差最小化的测量模型是指首先粗略估计目标在世界坐标系中的三维坐标位置，然后根据该三维位置坐标利用透视投影模型计算目标在图像中的投影坐标，得到目标投影位置，将该目标投影位置与跟踪得到的目标跟踪位置进行比较，计算偏移误差，再根据偏移误差利用最小二乘法修正目标在世界坐标系中的位置；反复多次直至收敛，即偏移误差小于预先给定的阈值。公式化过程描述如下：

假设和θ分别表示目标在三维空间中的位置和姿态，N为目标发光灯的个数，表示目标投影位置，u_i表示目标跟踪位置，则目标投影位置与目标跟踪位置之间的偏移误差最小化可表达为：

其中表示目标投影位置是目标在三维空间中的位置和姿态的函数，即透视投影变换函数，该函数为非线性函数，可通过泰勒级数线性化展开如下：

其中表示的雅克比矩阵，即令则根据线性最小二乘法可求解得：

通过和θ＝θ+Δθ迭代更新和θ，直到小于指定阈值。

4、操作规划模块的实施说明

操作规划模块，包括空间机械臂末端运动轨迹规划单元和机械臂各关节协同运动规划单元，能够根据空间目标的位置和捕获任务的要求规划空间机械臂各关节的运动，使得机械臂完成空间目标的渐近式捕获。

空间机械臂末端运动轨迹规划单元，负责根据目标追踪与定位模块求得的目标位姿和由机械臂构型计算得到的机械臂末端的位姿，规划机械臂末端抓捕目标的移动路径和轨迹；空间机械臂各关节协同运动规划单元，负责根据规划得到的末端移动路径离散轨迹点，通过逆运动学算法求解每个离散轨迹点对应的机械臂各关节的运动角度和角速度。

由机械臂构型计算机械臂末端位姿的方法表述如下：设机械臂各关节角的角度Θ＝[θ₀θ₁θ₂…θ_n]，在机械臂臂杆长度d_i、轴线距离a_i、关节轴线夹角α_i等结构参数一定，则第i个关节与第i+1个关节之间的位姿变换关系表示为M_i＝R(θ_i)T(a_i,d_i)R(α_i)，则机械臂末端位姿可计算为：

其中，R(θ₁,θ₂,…,θ_n)∈R^3×3，T(θ₁…θ_n,α₁…α_n,d₁…d_n,a₁…a_n)∈R^3×1，分别表示机械臂末端到基座质心之间的旋转和平移变换关系。

由于空间机械臂末端的运动轨迹，不仅与空间机械臂各关节的协同运动关系有关，还与空间机械臂基座载体(空间站)的姿态密切相关。在实际应用中通常将空间机械臂末端运动轨迹规划功能简化为计算机械臂末端到目标的最短路径，即直线路径；而在对机械臂各关节角协同运动关系进行求解时，再根据对基座载体(空间站)姿态扰动最小的约束，调整空间机械臂的末端轨迹。对空间机械臂各关节角协同运动关系进行规划的过程，即通过逆运动学算法求解每个离散轨迹点对应的空间机械臂各关节的运动角度和角速度的过程，描述如下：

假设空间机械臂***有n个自由度，其各关节的微分运动学方程为：

其中，v_e,ω_e∈R³分别为空间机械臂末端的线速度和角速度，v₀,ω₀∈R³分别为空间站基座质心处的线速度和角速度，Θ∈Rⁿ为关节角(n为机械臂自由度数)，J_s为与空间站基座运动相关的雅可比矩阵，而J_m为与机械臂运动相关的雅可比矩阵。

空间机械臂处于自由漂浮模式，基座位置、姿态均不受控，空间站和机械臂组成的整体***满足线动量和角动量守恒，即满足：

其中，ω_i为B_i的角速度。将上式化简为矩阵形式可得：

利用上式求解ω₀可得：

将(4-4)带入(4-1)可得自由漂浮空间机械臂运动学方程如下：

其中，J_g为空间机械臂的广义雅可比矩阵，它是空间站基座姿态Ψ₀、机械臂关节角Θ、各关节和空间站质量m_i和惯量I_i的函数，J_gv和J_gv分别是速度和加速度的广义雅可比矩阵。

空间姿态角通常采用欧拉角E(α,β,γ)描述，笛卡尔姿态角速度与欧拉角速度之间的关系为：

若矩阵J_E为非奇异矩阵，根据式(4-4)和(4-6)可以得到空间站姿态角变化与机械臂关节角之间的关系：

对空间站姿态影响最小的路径规划可以通过以下优化问题求解：

其中，Ψ_l＝[α_l,β_l,γ_l]^T，Ψ_u＝[α_u,β_u,γ_u]^T，Ψ＝[α,β,γ]^T表示机械臂从初始状态到目标状态的任一状态下空间站的姿态角，必须限定在空间站正常工作的姿态范围内。将公式(4-7)按照等间隔采样的方法进行离散化，再对离散化的优化问题进行求解，即可求得各个时刻对应的空间机械臂各关节的运动角度Θ_j和角速度

5、运动模拟模块的实施说明

运动模拟模块，是指基于空间机械臂和空间目标数字化三维模型的虚拟仿真软件，包括空间机械臂三维模型及其操作行为、空间目标的三维模型及其相对于机械臂末端或者视觉相机的运动关系。

空间机械臂三维模型和空间目标三维模型在虚拟仿真软件中的外形，与空间机械臂实体和空间目标实体的外形完全或近似一致，两类三维模型的尺寸采用与两类实体1:1的比例关系设定。对空间机械臂三维模型和空间目标三维模型的空间关系及其运动控制步骤如图3所示。

空间机械臂三维模型的操作行为主要指空间机械臂各关节运动以及基座载体(空间站)与空间机械臂的协同运动，其实现方式包括两个步骤：

第一步，空间机械臂各关节运动，根据空间机械臂操作规划模块规划得到的各关节角运动角度和角速度序列进行设定；

第二步，基座载体(空间站)与空间机械臂的协同运动，根据基座载体与空间机械臂组合体重心位置不改变的特性，来计算基座载***置和姿态的对应变化关系，根据该变化关系设定空间机械臂的每一步运动对应的基座载***姿。

空间目标三维模型相对于机械臂末端或者视觉相机的运动，是指空间目标相对于机械臂末端或者视觉相机的位姿关系的改变，需要考虑空间目标相对于空间站(基座载体)的运动和空间机械臂自身的运动。空间站(空间站与空间机械臂组合整体)与空间目标在空间中飞行，空间站逐步靠近空间目标，假设空间站的飞行速度大于空间目标飞行速度，存在速度差△v，使得空间目标相对于空间机械臂末端和视觉相机的位置发生△vt的变化；同时由于空间机械臂各关节运动引起空间站、空间机械臂末端和视觉相机位置和姿态发生变化，引起空间目标相对于空间机械臂末端和视觉相机的位姿发生变化。对两种位置和姿态的变化进行叠加，计算得到空间目标相对于空间机械臂末端和视觉相机的绝对位姿变化关系，根据绝对位姿变化关系控制空间目标三维模型的位姿的改变。

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

Claims (13)

1.一种基于仿真的空间机械臂抓捕目标遥操作方法，其特征在于，包括以下步骤：

步骤1、数据源采集模块通过多种类型的插件从对应类型的数据源采集目标的图像数据并统一转换成相同格式后发送至中间件模块；

步骤2、中间件模块接收并存储经过转换的图像数据并发送至目标追踪与定位模块进行处理，并保持数据源采集模块采集图像数据的速度和目标追踪与定位模块处理图像数据的速度之间的差值保持在预设范围内；

步骤3、目标追踪与定位模块对经过转换的图像数据进行处理，得到目标的位姿；

步骤4、操作规划模块根据目标的位姿和获取的机械臂末端位姿，规划空间机械臂各关节的运动；

步骤5、运动模拟模块建立空间机械臂三维模型和目标三维模型，并根据操作规划模块规划的机械臂各关节的运动，实时改变空间机械臂三维模型的位姿和目标三维模型的位姿；

步骤6、重复以上步骤，直到在运动模拟模块中完成机械臂对目标的捕获。

2.根据权利要求1所述一种基于仿真的空间机械臂抓捕目标遥操作方法，其特征在于，所述步骤2具体包括以下步骤：

步骤2.1、插件管理单元对多种类型的插件进行管理与调配，接收经过转换的图像数据并存储在自适应缓冲区单元中；

步骤2.2、所述自适应缓冲区单元使所述数据源采集模块的数据采集速度和所述目标追踪与定位模块的处理速度之间的差值保持在预设范围内；

步骤2.3、数据输出接口单元将所述自适应缓冲区中的图像数据发送至目标跟踪与定位模块。

3.根据权利要求2所述一种基于仿真的空间机械臂抓捕目标遥操作方法，其特征在于，所述步骤2.2具体包括以下步骤：

设定缓冲区允许的最大帧差为F_BMAX，缓冲区允许的最大帧差的上限为F_MAX，丢弃帧标记数为F_Tf，图像采集和图像处理的帧差为F_P-C；

步骤2.2.1，如果F_P-C＞F_BMAX，执行F_Tf＝F_Tf+1，则跳转到步骤2.2.2；

如果F_P-C<F_BMAX/2，则跳转到步骤2.2.3；

如果F_P-C≥F_BMAX/2，并且F_P-C≤F_BMAX，则跳转到步骤2.2.4；

步骤2.2.2，如果F_Tf≥F_BMAX/2，执行F_BMAX＝2*F_BMAX，跳转到步骤2.2.6；如果F_Tf<F_BMAX/2，直接跳转到步骤2.2.6；

步骤2.2.3，如果F_P-C≤1，则令F_Tf＝0，否则执行F_Tf＝(F_Tf+1)/2，直到2*F_Tf≤F_P-C，执行完上述计算后跳转到步骤2.2.5；

步骤2.2.4，如果F_Tf>0，则执行F_Tf＝F_Tf-1，直到2*F_Tf≤F_P-C；

步骤2.2.5，如果F_BMAX>F_MAX，则令F_BMAX＝F_BMAX/2；

步骤2.2.6，如果F_Tf>0，丢弃缓冲区中存储的2*F_Tf-1帧图像数据，然后将第2*F_Tf帧图像数据推送给目标追踪与定位模块，即F_P-C＝F_P-C-2*F_Tf。

4.根据权利要求1所述一种基于仿真的空间机械臂抓捕目标遥操作方法，其特征在于，所述步骤3具体包括以下步骤：

步骤3.1、接收经过转换的图像数据并进行预处理，得到当前的图像帧中目标的各条边缘；

步骤3.2、通过边缘外框计算得到当前图像帧中的目标区域，其中，所述目标区域的几何中心为目标的位置，所述目标区域的尺寸为目标的尺寸；

步骤3.3、对所述当前图像帧中的目标区域通过目标跟踪算法进行处理后得到下一图像帧中的目标区域；

步骤3.4、对所述下一图像帧中的目标区域中的像素点应用灰度加权几何中心坐标计算方法得到目标跟踪位置；

步骤3.5、计算目标投影位置并通过所述目标投影位置和所述目标跟踪位置之间的偏移误差最小化的测量模型得到每帧图像中的目标的位姿。

5.根据权利要求4所述一种基于仿真的空间机械臂抓捕目标遥操作方法，其特征在于，所述步骤3.1具体包括以下步骤：

步骤3.1.1、对图像进行高斯滤波，选定某一阈值T₁，令图像中灰度大于T₁的像素取值为255，灰度小于T₁的像素取值为0，对高斯滤波后的图像进行二值化，得到二值化图像；

步骤3.1.2、对二值化图像采用腐蚀膨胀算法处理，消除二值化图像中的孤立亮点，使得二值化图像中仅有目标区域灰度值为255，其他位置灰度值均为0；

步骤3.1.3、对腐蚀膨胀后的二值化图像进行边缘提取。

6.根据权利要求5所述一种基于仿真的空间机械臂抓捕目标遥操作方法，其特征在于，所述步骤3.3中的目标跟踪算法为Camshift和卡尔曼滤波相融合的跟踪算法。

7.根据权利要求4所述一种基于仿真的空间机械臂抓捕目标遥操作方法，其特征在于，所述步骤3.4具体为，设定某一阈值T₀，对目标区域内所有灰度值大于等于T₀的像素点按照其在经过高斯滤波处理后的图像中的位置进行灰度加权平均运算，求得灰度加权几何中心坐标，其计算公式为：

$X_c=\underset{(x,y)\&Element;R}{\&Sigma;}xI(x,y)/\underset{(x,y)\&Element;R}{\&Sigma;}I(x,y),y_c=\underset{(x,y)\&Element;R}{\&Sigma;}yI(x,y)/\underset{(x,y)\&Element;R}{\&Sigma;}I(x,y),$

其中x_c表示灰度加权几何中心的x坐标，y_c表示灰度加权几何中心的y坐标，x表示图像中像素点的x坐标，y表示图像中像素点的y坐标，I(x,y)表示坐标(x,y)处的灰度值，R表示目标区域。

8.根据权利要求4所述一种基于仿真的空间机械臂抓捕目标遥操作方法，其特征在于，所述步骤3.5具体包括以下步骤：

步骤3.5.1、估计目标在世界坐标系中的三维坐标位置；

步骤3.5.2、根据所述三维坐标位置，利用透视投影模型计算目标在图像中的投影坐标，得到目标投影位置；

步骤3.5.3、将所述目标投影位置与目标跟踪位置进行比较，计算偏移误差；

步骤3.5.4、根据所述偏移误差，利用最小二乘法修正目标在世界坐标系中的位置；

步骤3.5.5、反复多次直至收敛，即偏移误差小于预先给定的阈值。

9.根据权利要求1所述一种基于仿真的空间机械臂抓捕目标遥操作方法，其特征在于，所述步骤4具体包括以下步骤：

步骤4.1、机械臂末端运动轨迹规划单元根据由机械臂构型计算得到的机械臂末端的位姿和目标追踪与定位模块得到的目标的位姿，规划机械臂末端的移动路径；

步骤4.2、机械臂各关节协同运动规划单元根据规划得到的机械臂末端的移动路径的离散轨迹点，通过逆运动学算法求解每个离散轨迹点对应的机械臂各关节的运动角度和角速度。

10.一种基于仿真的空间机械臂抓捕目标遥操作***，其特征在于，包括：

数据源采集模块，用于通过多种类型的插件从对应类型的数据源采集目标的图像数据并统一转换成相同格式后发送至中间件模块；

中间件模块，用于接收并存储经过转换的图像数据并发送至目标追踪与定位模块进行处理，并保持数据源采集模块采集图像数据的速度和目标追踪与定位模块处理图像数据的速度之间的差值保持在预设范围内；

目标追踪与定位模块，用于对经过转换的图像数据进行处理，得到目标的位姿；

操作规划模块，用于根据目标的位姿和获取的机械臂末端位姿，规划空间机械臂各关节的运动；

运动模拟模块，用于建立空间机械臂三维模型和目标三维模型，并根据操作规划模块规划的机械臂各关节的运动，实时改变空间机械臂三维模型的位姿和目标三维模型的位姿。

11.根据权利要求10所述一种基于仿真的空间机械臂抓捕目标遥操作***，其特征在于，所述中间件模块包括插件管理单元、自适应缓冲区单元和统一化数据输出接口单元；

所述插件管理单元，用于对多种类型的插件进行加载、管理与调配，接收经过转换的图像数据并存储在所述自适应缓冲区单元中；

所述自适应缓冲区单元，用于使所述数据源采集模块的数据采集速度和所述目标追踪与定位模块的处理速度之间的差值保持在预设范围内；

所述统一化数据输出接口单元，用于将所述自适应缓冲区中的图像数据发送至目标跟踪与定位模块；

12.根据权利要求10所述一种基于仿真的空间机械臂抓捕目标遥操作***，其特征在于，所述目标追踪与定位模块包括图像预处理单元、目标检测单元、目标跟踪单元、位置结算单元和位姿解算单元；

所述图像预处理单元，用于接收经过转换的图像数据并进行预处理，得到当前的图像帧中目标的各条边缘；

所述目标检测单元，用于通过边缘外框计算得到当前图像帧中的目标区域，其中，所述目标区域的几何中心为目标的位置，所述目标区域的尺寸为目标的尺寸；

所述目标跟踪单元，用于对所述当前图像帧中的目标区域通过目标跟踪算法进行处理后得到下一图像帧中的目标区域；

所述位置解算单元，用于对所述下一图像帧中的目标区域中的像素点应用灰度加权几何中心坐标计算方法得到目标跟踪位置；

所述位姿解算单元，用于通过目标投影位置和所述目标跟踪位置之间的偏移误差最小化的测量模型得到每帧图像中的目标的位姿。

13.根据权利要求10所述一种基于仿真的空间机械臂抓捕目标遥操作***，其特征在于，所述操作规划模块包括机械臂末端运动轨迹规划单元和机械臂各关节协同运动规划单元；

所述机械臂末端运动轨迹规划单元，用于根据机械臂末端的位姿和目标追踪与定位模块得到的目标的位姿，规划机械臂末端的移动路径；

所述机械臂各关节协同运动规划单元，用于根据规划得到的机械臂末端的移动路径的离散轨迹点，通过逆运动学算法求解每个离散轨迹点对应的机械臂各关节的运动角度和角速度。