CN103926845B - 空间机器人视觉伺服捕获运动目标的地面模拟系统及模拟方法 - Google Patents

Abstract

空间机器人视觉伺服捕获运动目标的地面模拟系统及模拟方法，涉及空间机器人视觉伺服捕获空间运动目标的地面验证技术。它为了解决现有地面试验系统无法模拟三维空间里空间机器人视觉伺服捕获运动目标卫星的情况的问题。本发明根据捕获手爪与捕获手柄的相对位姿关系得到关节运动信息，根据该信息获得各关节控制力矩，然后计算计算得到基座位姿和关节角信息，对上述信息计算得到等效的工业机器人关节角指令，然后对一号工业机器人进行控制。本发明能够模拟在三维空间里空间机器人视觉伺服捕获运动目标的过程和基座扰动情况，验证空间机器人视觉伺服的路径规划算法的可靠性。本发明适用于空间机器人视觉伺服捕获运动目标的地面验证。

Description

空间机器人视觉伺服捕获运动目标的地面模拟系统及模拟方法

技术领域

本发明涉及一种空间机器人视觉伺服捕获运动目标的地面验证技术，属于空间机器人技术领域。

背景技术

为了确保在轨任务能够成功，空间机器人在发射前必须进行充分的地面实验来验证和评估空间机器人目标捕获的路径规划和视觉伺服跟踪控制算法等。因此空间机器人目标捕获的地面试验是整个空间机器人系统试验任务中的重要组成内容。目前空间机器人地面实验系统主要有五大类：气浮平台实验系统、水浮式实验系统、吊丝配重实验系统、基于抛物线或自由落体运动的实验系统以及混合仿真实验系统。

而水浮法实验系统、吊丝配重实验系统、基于抛物线或自由落体运动的地面实验系统的局限性比较大，而常规的气浮台试验只能验证空间机器人在平面内的捕获过程，对于在太空中零重力环境下的真实三维运动状况和捕获能力无法进行充分验证。因此选择建立一套基于软硬件混合的半物理仿真地面实验系统来满足空间机器人在三维空间运动的目标捕获的研究要求。基于运动学等效与动力学模拟的空间机器人混合仿真地面实验系统可以用来实时仿真空间机器人系统的运动状态，可实现空间机械臂对在三维空间目标的捕获过程进行验证，可以充分地验证捕获过程中手眼视觉测量、空间机械手规划与控制方法的正确性和完成目标抓捕任务的能力。

目前空间机器人系统已完成在气浮平台的二维平面上的目标捕获实验，但是由于空间机器人结构限制和地球的重力环境的影响，空间机器人在地面上不具备三维运动的能力，无法验证在空间三维运动下的状况，因此需要建立一套地面仿真试验仿真系统来验证在三维空间里手眼相机视觉测量算法和视觉伺服跟踪算法的准确性和可靠性。

发明内容

本发明的目的是为了解决目前空间机器人目标捕获的地面试验系统因无法模拟三维空间里空间机器人视觉伺服捕获运动目标卫星的情况的问题，提供一种空间机器人视觉伺服捕获运动目标的地面模拟系统及模拟方法。

本发明所述的空间机器人视觉伺服捕获运动目标的地面模拟系统包括一号工业机器人3、二号工业机器人4、运动控制和空间机器人末端运动学等效计算机5、运动控制和目标卫星手柄运动学等效计算机6、动力学计算机7、空间机器人关节电模拟器8和中央控制器9；

所述一号工业机器人3用于模拟具有漂浮基座的服务卫星1的空间机械臂末端的运动；二号工业机器人4用于模拟目标卫星2的运动，二号工业机器人4上安装有目标卫星模拟器4-3，所述目标卫星模拟器4-3位于二号工业机器人4的机械臂与视觉靶标4-2之间；

手眼相机3-1的相对位姿关系信号端通过CAN总线连接中央控制器9的相对位姿关系信号端，中央控制器9的关节角信号端通过CAN总线连接空间机器人关节电模拟器8的关节角信号端，空间机器人关节电模拟器8的控制力矩信号端通过CAN总线连接动力学计算机7的控制力矩信号端，动力学计算机7的关节角和基座本体位姿信息信号端同时连接运动控制和空间机器人末端运动学等效计算机5的关节角和基座本体位姿信息信号端以及运动控制和目标卫星手柄运动学等效计算机6的关节角和基座本体位姿信息信号端，运动控制和空间机器人末端运动学等效计算机5的控制信号端通过PCI总线连接一号工业机器人3的控制信号端，运动控制和目标卫星手柄运动学等效计算机6的控制信号端通过PCI总线连接二号工业机器人4的控制信号端。

上述空间机器人视觉伺服捕获运动目标的地面模拟系统的地面验证方法包括以下步骤：

步骤一、手眼相机3-1将测量得到的捕获手爪3-2与捕获手柄4-1的相对位姿关系信息通过CAN总线传输给中央控制器9；

步骤二、中央控制器9根据步骤一中的相对位姿关系信息规划出空间机械臂的期望关节角和期望关节角速度，然后通过CAN总线将所述期望关节角和期望关节角速度传输给空间机器人关节电模拟器8；

步骤三、动力学计算机7仿真得到理论关节角和理论关节角速度，并将该理论关节角和理论关节角速度发送给空间机器人关节电模拟器8；

步骤四、空间机器人关节电模拟器8计算关节的控制力矩，并将所述控制力矩发送给动力学计算机7，

所述控制力矩τ_i通过下述公式计算：

${\mathrm{\τ}}_i=K_{\mathrm{pi}}(q_{\mathrm{ri}}-q_{\mathrm{ei}})+K_{\mathrm{di}}({\stackrel{\·}{q}}_{\mathrm{ri}}-{\stackrel{\·}{q}}_{\mathrm{ei}}),$

其中，i代表第i关节，p代表比例项，d代表微分项，e代表期望，r代表实际，τ_i代表空间机器人的关节控制力矩，q_ei代表空间机器人的期望关节角，代表空间机器人的期望关节角速度，q_ri代表空间机器人的理论关节角，代表空间机器人的理论关节角速度，K_pi代表比例控制系数，K_di代表微分控制系数；

步骤五、动力学计算机7以接收到的控制力矩作为输入量进行实时仿真，得到空间机器人当前时刻的关节角和基座本体位姿信息，并通过TCP/IP网络以组播的形式将关节角和基座本体位姿信息发送到运动控制和空间机器人末端运动学等效计算机5以及运动控制和目标卫星手柄运动学等效计算机6；

步骤六、运动控制和空间机器人末端运动学等效计算机5接收到的关节角和基座本体位姿信息后，首先通过运动学正解得到空间机器人末端的位姿信息xs，然后解算并生成一号工业机器人3运动等效所需要的关节角指令，并根据该关节角指令控制一号工业机器人3进行运动，

所述空间机器人末端的位姿信息x_s=FK(q_is)，其中q_is代表空间机器人的关节角，FK为正运动学函数，

所述关节角指令q_iA=IK(x_A)，其中x_A代表工业机器人末端位姿，IK为逆运动学函数。

本发明的中央控制器9的内部程序与所述的中控模块的内部程序相同，本实施方式的手眼相机3-1与服务卫星1的手眼相机相同，一号工业机器人3用于模拟服务卫星1，二号工业机器人4用于模拟目标卫星2。一号工业机器人3直接固定在地面，由于空间机器人与工业机器人在构型上的差异，中央控制器9规划产生的关节角指令无法直接作用于一号工业机器人3上。手眼相机3-1内部的视觉测量功能模块根据手眼相机3-1采集的图像信息来获得一号工业机器人3末端的捕获手爪3-2的中心点与二号工业机器人4末端的捕获手柄4-1的中心的相对位姿关系；该相对位姿关系通过CAN通信传递给中央控制器9，中央控制器9根据这一关系进行运动规划；中央控制器9运动规划得到的空间机器人的关节运动信息传递给空间机器人关节电模拟器8；空间机器人关节电模拟器8根据期望关节角、期望关节角速度、实际关节角和实际关节角速度信息获得空间机器人各关节的控制力矩，并将该控制力矩发送给动力学计算机7；动力学计算机7根据空间机器人的控制力矩计算得到空间机器人基座位姿和关节角信息，并将该基座位姿和关节角信息通过TCP/IP协议发送给运动控制和空间机器人末端运动学等效计算机5；运动控制和空间机器人末端运动学等效计算机5接收到当前的空间机器人基座位姿和关节角信息后，首先通过运动学正解得到一号工业机器人3末端的位姿信息，然后通过对一号工业机器人3的运动学逆解，得到等效的工业机器人关节角指令，然后对一号工业机器人3的关节进行控制。目标卫星模拟器4-3固定于二号工业机器人4上，通过运动控制和空间机器人末端运动学等效计算机5控制二号工业机器人4的末端来模拟目标卫星2的实际运动。当给定目标卫星2在空间的运动情况时，可以通过运动控制和目标卫星手柄运动学等效计算机6计算出二号工业机器人4的关节的运动情况，然后控制二号工业机器人4的运动。本发明系统硬件实现简单方便，能够模拟在三维空间里空间机器人视觉伺服捕获运动目标的过程和基座扰动情况，能够验证空间机器人视觉伺服的路径规划算法的可靠性，并测量空间机器人视觉伺服捕获的精度和捕获时间，此外，系统还具有很好的扩展性，可以扩展用于目标捕获的接触状况验证。

附图说明

图1是空间机器人视觉伺服捕获运动目标卫星的系统组成框图；

图2是空间机器人视觉伺服的地面验证系统硬件组成；

图3是空间机器人视觉伺服的地面验证系统实现原理框图；

图4是空间机器人视觉伺服的地面验证系统通信结构框图；

图5是一号工业机器人的控制流程图；

图6是二号工业机器人的控制流程图；

图7是空间机器人视觉伺服捕获目标时，手眼相机测量的捕获手爪相对于目标卫星捕获手柄的位置曲线；

图8是空间机器人视觉伺服捕获目标的基座姿态扰动曲线。

具体实施方式

具体实施方式一：结合图1至图4说明本实施方式，本实施方式所述的空间机器人视觉伺服捕获运动目标的地面模拟系统包括一号工业机器人3、二号工业机器人4、运动控制和空间机器人末端运动学等效计算机5、运动控制和目标卫星手柄运动学等效计算机6、动力学计算机7、空间机器人关节电模拟器8和中央控制器9；

所述一号工业机器人3用于模拟具有漂浮基座的服务卫星1的空间机械臂末端的运动；二号工业机器人4用于模拟目标卫星2的运动，二号工业机器人4上安装有目标卫星模拟器4-3，所述目标卫星模拟器4-3位于二号工业机器人4的机械臂与视觉靶标4-2之间；

手眼相机3-1的相对位姿关系信号端通过CAN总线连接中央控制器9的相对位姿关系信号端，中央控制器9的关节角信号端通过CAN总线连接空间机器人关节电模拟器8的关节角信号端，空间机器人关节电模拟器8的控制力矩信号端通过CAN总线连接动力学计算机7的控制力矩信号端，动力学计算机7的关节角和基座本体位姿信息信号端同时连接运动控制和空间机器人末端运动学等效计算机5的关节角和基座本体位姿信息信号端以及运动控制和目标卫星手柄运动学等效计算机6的关节角和基座本体位姿信息信号端，运动控制和空间机器人末端运动学等效计算机5的控制信号端通过PCI总线连接一号工业机器人3的控制信号端，运动控制和目标卫星手柄运动学等效计算机6的控制信号端通过PCI总线连接二号工业机器人4的控制信号端。

如图1所示，空间机器人系统主要包括服务卫星1和目标卫星2两部分，其中服务卫星1由漂浮卫星基座、空间机械臂、手眼相机和捕获手爪组成；目标卫星2由捕获手柄、视觉靶标和卫星本体组成。手眼相机的内部嵌入有视觉测量功能模块，中控模块根据视频测量功能模块发来的相对位姿信息进行规划产生关节角指令，并根据该指令控制服务卫星1运动。本实施方式的中央控制器9的内部程序与所述的中控模块的内部程序相同，本实施方式的手眼相机3-1与服务卫星1的手眼相机相同，一号工业机器人3用于模拟服务卫星1，二号工业机器人4用于模拟目标卫星2。

图3所示为本实施方式所述的空间机器人视觉伺服捕获运动目标的地面模拟系统的工作原理，一号工业机器人3直接固定在地面，由于空间机器人与工业机器人在构型上的差异，中央控制器9规划产生的关节角指令无法直接作用于一号工业机器人3上。手眼相机3-1内部的视觉测量功能模块根据手眼相机3-1采集的图像信息来获得一号工业机器人3末端的捕获手爪3-2的中心点与二号工业机器人4末端的捕获手柄4-1的中心的相对位姿关系；该相对位姿关系通过CAN通信传递给中央控制器9，中央控制器9根据这一关系进行运动规划；中央控制器9运动规划得到的空间机器人的关节运动信息传递给空间机器人关节电模拟器8；空间机器人关节电模拟器8根据期望关节角、期望关节角速度、实际关节角和实际关节角速度信息获得空间机器人各关节的控制力矩，并将该控制力矩发送给动力学计算机7；动力学计算机7根据空间机器人的控制力矩计算得到空间机器人基座位姿和关节角信息，并将该基座位姿和关节角信息通过TCP/IP协议发送给运动控制和空间机器人末端运动学等效计算机5；运动控制和空间机器人末端运动学等效计算机5接收到当前的空间机器人基座位姿和关节角信息后，首先通过运动学正解得到一号工业机器人3末端的位姿信息，然后通过对一号工业机器人3的运动学逆解，得到等效的工业机器人关节角指令，然后对一号工业机器人3的关节进行控制。目标卫星模拟器4-3固定于二号工业机器人4上，通过运动控制和空间机器人末端运动学等效计算机5控制二号工业机器人4的末端来模拟目标卫星2的实际运动。当给定目标卫星2在空间的运动情况时，可以通过运动控制和目标卫星手柄运动学等效计算机6计算出二号工业机器人4的关节的运动情况，然后控制二号工业机器人4的运动。还可在本实施方式所述的地面模拟系统的基础上增加三维运动模拟显示计算机，三维运动模拟显示计算机根据当前一号工业机器人3和二号工业机器人4的信息，实时显示一号工业机器人3捕获二号工业机器人4的情况。

图4为空间机器人视觉伺服捕获运动目标的地面模拟系统的通信接口图，在该地面模拟系统中，空间硬件部分（即空间机器人关节电模拟器8，中央控制器9和手眼相机3-1都是采用CAN总线进行通信，与空间使用时的通信方式完全一致。

本实施方式所述的空间机器人视觉伺服捕获运动目标的地面模拟系统具有以下优点：

（1）系统硬件实现简单方便；

（2）能够模拟在三维空间里空间机器人视觉伺服捕获运动目标的过程；

（3）能够模拟空间机器人运动过程中的基座扰动情况；

（4）能够验证空间机器人视觉伺服的路径规划算法的可靠性；

（5）能够测量空间机器人视觉伺服捕获的精度和捕获时间；

（6）具有很好的扩展性，可以扩展用于目标捕获的接触状况验证。

具体实施方式二：结合图2至图4说明本实施方式，本实施方式是对实施方式一所述的空间机器人视觉伺服捕获运动目标的地面模拟系统的进一步限定，本实施方式中，所述的动力学计算机7通过TCP/IP网络通信方式与运动控制和空间机器人末端运动学等效计算机5以及运动控制和目标卫星手柄运动学等效计算机6之间进行数据传输。

本实施方式中，地面工业机器人模拟部分采用TCP/IP网络通信方式，以保证系统的实时性。

具体实施方式三：结合图3说明本实施方式，本实施方式是对实施方式一所述的空间机器人视觉伺服捕获运动目标的地面模拟系统的进一步限定，本实施方式中，所述的运动控制和空间机器人末端运动学等效计算机5嵌入有由软件实现的空间机器人末端等效转换模块和一号工业机器人控制器。

本实施方式中，动力学计算机7根据空间机器人的控制力矩计算得到空间机器人基座位姿和关节角信息，并将基座位姿和关节角信息通过TCP/IP协议发送给运动控制和空间机器人末端运动学等效计算机5内的空间机器人末端等效转换模块，所述空间机器人末端等效转换模块首先通过运动学正解得到一号工业机器人3末端的位姿信息，然后通过对一号工业机器人3的运动学逆解，得到等效的工业机器人关节角指令，然后通过一号工业机器人控制器对一号工业机器人3的关节进行控制。

具体实施方式四：结合图3说明本实施方式，本实施方式是对实施方式一所述的空间机器人视觉伺服捕获运动目标的地面模拟系统的进一步限定，本实施方式中，所述的运动控制和目标卫星手柄运动学等效计算机6嵌入有由软件实现的空间机器人末端运动学等效运动模块和二号工业机器人控制器。

本实施方式中，当给定目标卫星2在空间的运动情况时，可以通过运动控制和目标卫星手柄运动学等效计算机6中的运动学等效运动模块计算出二号工业机器人4的关节的运动情况，然后通过二号工业机器人控制器来控制二号工业机器人4的运动。

具体实施方式五：结合图1至图8说明本实施方式，本实施方式所述的空间机器人视觉伺服捕获运动目标的地面模拟方法包括以下步骤：

步骤一、手眼相机3-1将测量得到的捕获手爪3-2与捕获手柄4-1的相对位姿关系信息通过CAN总线传输给中央控制器9；

步骤二、中央控制器9根据步骤一中的相对位姿关系信息规划出空间机械臂的期望关节角和期望关节角速度，然后通过CAN总线将所述期望关节角和期望关节角速度传输给空间机器人关节电模拟器8；

步骤三、动力学计算机7仿真得到理论关节角和理论关节角速度，并将该理论关节角和理论关节角速度发送给空间机器人关节电模拟器8；

步骤四、空间机器人关节电模拟器8计算关节的控制力矩，并将所述控制力矩发送给动力学计算机7，

所述控制力矩τ_i通过下述公式计算：

${\mathrm{\τ}}_i=K_{\mathrm{pi}}(q_{\mathrm{ri}}-q_{\mathrm{ei}})+K_{\mathrm{di}}({\stackrel{\·}{q}}_{\mathrm{ri}}-{\stackrel{\·}{q}}_{\mathrm{ei}}),$

其中，i代表第i关节，p代表比例项，d代表微分项，e代表期望，r代表实际，τ_i代表空间机器人的关节控制力矩，q_ei代表空间机器人的期望关节角，代表空间机器人的期望关节角速度，q_ri代表空间机器人的理论关节角，代表空间机器人的理论关节角速度，K_pi代表比例控制系数，K_di代表微分控制系数；

步骤五、动力学计算机7以接收到的控制力矩作为输入量进行实时仿真，得到空间机器人当前时刻的关节角和基座本体位姿信息，并通过TCP/IP网络以组播的形式将关节角和基座本体位姿信息发送到运动控制和空间机器人末端运动学等效计算机5以及运动控制和目标卫星手柄运动学等效计算机6；

步骤六、运动控制和空间机器人末端运动学等效计算机5接收到的关节角和基座本体位姿信息后，首先通过运动学正解得到空间机器人末端的位姿信息x_s，然后解算并生成一号工业机器人3运动等效所需要的关节角指令，并根据该关节角指令控制一号工业机器人3进行运动，

所述空间机器人末端的位姿信息x_s=FK(q_is)，其中q_is代表空间机器人的关节角，FK为正运动学函数，

所述关节角指令q_iA=IK(x_A)，其中x_A代表工业机器人末端位姿，IK为逆运动学函数。

本实施方式中，空间机器人系统的动力学计算公式为

$\left[\begin{array}{c}{\stackrel{\·\·}{x}}_b\\ {\stackrel{\·\·}{q}}_{\mathrm{is}}\end{array}\right]={\left[\begin{array}{cc}{H}_b& H_{\mathrm{bm}}\\ H_{\mathrm{bm}}^T& H_m\end{array}\right]}^{-1}\left(\begin{array}{c}\left[\begin{array}{c}{F}_b\\ {\mathrm{\τ}}_{\mathrm{is}}\end{array}\right]\end{array}-\left[\begin{array}{c}{c}_b\\ c_m\end{array}\right]\right),$

式中，H_b为卫星基座的惯量矩阵；H_m为空间机械臂的惯性矩阵；H_bm为卫星基座和机械臂的耦合惯量矩阵；为基座的加速度；为空间机器人的关节角加速度；c_b、c_m分别为与基座运动和机械臂运动相关的非线性力，包括向心力和哥氏力，c_b、c_m∈R⁶；F_b=[f_bx,f_by,f_by,τ_bx,τ_bx,τ_bx]^T∈R⁶，其中f_bx、f_by和f_by,分别为作用于基座的x、y和z方向的力，τ_bx、τ_bx和τ_bx分别为作用于基座x、y和z方向的力矩，τ_is为机械臂关节的驱动力矩，τ_is∈R⁶。

如图5所示，一号工业机器人3的控制过程如下：

实验开始前，先对手眼相机3-1进行标定，一号工业机器人3的各个关节和视觉测量系统22要进行复位和自检，捕获手爪3-2的位姿用坐标变换全部统一到机器人基座直角坐标系下，当有某个部分自检没通过时，报警并直接结束，否则，继续执行程序；

程序开始后，首先检查目标卫星模拟器4-3是否在手眼相机3-1的视场里；

当目标卫星模拟器4-3进入手眼相机3-1的视场范围时，运动控制和空间机器人末端运动学等效计算机5、一号工业机器人3、手眼相机3-1和中央控制器9便组成一个闭环控制回路；

手眼相机3-1的视觉测量系统22实时将采集到的相对位姿信息送到中央控制器9；

中央控制器9利用当前位姿信息进行实时规划，输出控制指令以驱动空间机器人关节电模拟器8进行仿真得到各关节的控制力矩；

控制力矩与当前关节角信息被输入至动力学计算机7进行仿真后，计算得到自由漂浮空间机器人的基座位姿及机械臂关节角；

运动控制和空间机器人末端运动学等效计算机5根据基座位姿与关节角信息计算得到一号工业机器人3的关节角指令，并通过上位机软件将指令发送至一号工业机器人3对其进行控制；

中央控制器9判断手眼相机3-1测得的相对位姿关系是否在捕获阈值范围之内，如果判断结果为是，则驱动捕获手爪3-2闭合，实现捕获；否则，重新判断目标卫星模拟器4-3是否在手眼相机3-1的视场里。

如图6所示，二号工业机器人4的控制过程如下：

运动控制和目标卫星手柄运动学等效计算机6通过运动学计算得到二号工业机器人4当前的位姿信息。

运动控制和目标卫星手柄运动学等效计算机6通过手柄运动学等效得到二号工业机器人4末端位姿信息；

中央控制器9根据该位姿信息对二号工业机器人4进行运动学逆解，得到二号工业机器人4的关节角指令信息；

最后通过运动控制和目标卫星手柄运动学等效计算机6的软件对二号工业机器人4进行控制。

采用本实施方式所述的空间机器人视觉伺服捕获运动目标的地面模拟方法验证空间机械臂在三维空间下、手眼视觉伺服在目标星不同运动状态下的跟随、接近和抓捕能力。

实验的初始条件设定为空间机械臂与目标星处于分离抓捕准备位置，该位置关系是机械臂末端与目标星手柄的相对位置为空间机器人在抓捕前关闭卫星姿控系统后手眼相机3-1测量到的相对位置信息，所述相对位置信息为[20mm,26mm,315mm]。在该位置关系下，空间机械臂开始根据手眼相机3-1提供的其与目标卫星模拟器4-3相对位姿信息接近目标卫星2运动，最终到达可以捕获的位置。空间机械臂接近目标卫星2运动的初始臂型对应的关节角为[0°,-121°,76°,0°,226°,0°]。

在地面上，两工业机器人根据空间机器人末端与目标星手柄中心的相对位姿关系，调整好它们的初始臂型，一号工业机器人3的初始关节角为[0°,45°,-15°,0°,-30°,0°]，二号工业机器人4的初始关节角为[0°,26°,-0.3°,0°,-26°,0°]。实验中，为了防止工业机器人运动速度过快，设定工业机器人末端速度上限为40mm/s，角速度上限为20°/s。

实验采用基于位置的视觉伺服自主路径规划方法进行运动目标捕获的实验。整个过程中，一号工业机器人3的基座处于自由漂浮状态。

1）首先启动一号工业机器人3和二号工业机器人4的上位机，即运动控制和空间机器人末端运动学等效计算机5和运动控制和目标卫星手柄运动学等效计算机6，初始化一号工业机器人3和二号工业机器人4的臂型，使一号工业机器人3和二号工业机器人4的末端的位置到达指定的初始化位置，该位置模拟空间机械臂末端和目标星手柄在空间中的分离抓捕准备位置；

2）启动空间机械臂的中央控制器9，连接中央控制器9的主服务器和控制计算机的网络通信；

3）启动空间机器人关节电模拟器8和一号工业机器人3的手眼相机3-1，并与中央控制器9建立通信；

4）启动动力学计算机7，并连接其与中央控制器9的通信；

5）开启一号工业机器人3和二号工业机器人4的网络通信；

6）设置二号工业机器人4的运动状态，开启动力学计算机7的网络通信，所述动力学计算机7为一号工业机器人3和二号工业机器人4的上位机软件和OSG发送控制指令。

8）在开启动力学计算机7后立即开启手眼相机3-1，所述手眼相机3-1的内部嵌入有视觉测量功能模块，该模块向中央控制器9发送视觉数据，然后中央控制器9向空间机器人关节电模拟器8发送期望关节角，空间机器人关节电模拟器8向动力学计算机7提供关节控制力矩，动力学计算机7通过关节控制力矩计算出机械臂的关节角和基座位姿，将机械臂的关节角和基座的位姿变化通过运动学等效得到机械臂末端的位姿。

实验结果如图7和图8所示，图7为机械臂末端的手眼相机3-1相对于目标卫星模拟器4-3的视觉伺服误差曲线，该曲线为手眼相机3-1测量的捕获手爪3-2相对于目标卫星捕获手柄4-1的位置曲线，通过处理后，发送到中央控制器9，然后通过计算机对该组信息实时保存。图8是空间机器人视觉伺服捕获目标的基座姿态扰动曲线，基座姿态扰动是由于机械臂的运动导致的，它是由动力学计算机7根据正动力学计算得到的，然后在实验过程中实时计算并保存数据。从图中可见相对位位置误差为[1.99mm，-0.66mm，1.97mm]，均在阈值范围内(实验中设定的阈值为相对位置偏差10mm)，图8为整个运动过程中漂浮基座相对惯性系的姿态和质心位置的变化曲线，漂浮基座姿态从[0,0,0]变为[-0.398°,-2.748°,0.800°]。

本实施方式所述的空间机器人视觉伺服捕获运动目标的地面模拟方法具有以下优点：

（1）能够模拟在三维空间里空间机器人视觉伺服捕获运动目标的过程；

（2）能够模拟空间机器人运动过程中的基座扰动情况；

（3）能够验证空间机器人视觉伺服的路径规划算法的可靠性；

（4）能够测量空间机器人视觉伺服捕获的精度和捕获时间；

（5）具有很好的扩展性，可以扩展用于目标捕获的接触状况验证。

具体实施方式六：本实施方式是对实施方式五所述的空间机器人视觉伺服捕获运动目标的地面模拟方法的进一步限定，本实施方式中，运动控制和空间机器人末端运动学等效计算机5的空间机器人末端运动学等效方法为：等效的一号工业机器人3的末端在其基座标系下的位姿T_G(t)为

$T_G\left(t\right)=T_{\mathrm{GB}}^{\mathrm{GE}}\·T_{\mathrm{SI}}^{\mathrm{ST}}\·T_S\left(t\right),$

其中，为初始臂型下，一号工业机器人3的基座标系到末端坐标系的齐次转换阵；为服务卫星1惯性坐标系到机械手末端坐标系的齐次转换阵；T_S(t)为t时刻服务卫星1的空间机械臂在服务卫星1的惯性坐标系下的位姿矩阵，且该位姿矩阵为齐次阵。

空间机器人视觉伺服捕获运动目标的地面模拟方法的软件部分包括两方面关键技术：即空间机器人的动力学实时计算和末端运动学等效算法。空间机器人的动力学实时计算可参考刚体动力学计算相关文献。

空间机器人末端等效：

利用一号工业机器人3的末端对空间机器人的末端进行等效，其主要目的是以工业机器人末端的运动来模拟空间机器人末端在服务星惯性坐标系下的运动，实时跟踪空间机器人末端在惯性空间下位姿的变化。

空间机器人末端运动学等效算法的实现方法如下：服务卫星1初始状态与目标卫星2处于分离抓捕位置，假设此时服务卫星1的惯性坐标系为∑_SI，服务卫星1的机械臂末端坐标系为∑_ST，则服务卫星1惯性坐标系到服务卫星1的机械臂末端坐标系的齐次转换阵为

假设工业机器人的基座标系为∑_GB。在初始臂型 ${\mathrm{\Θ}}_{\mathrm{GI}}=\left[\begin{array}{cccccc}{\mathrm{\θ}}_1^I& {\mathrm{\θ}}_2^I& {\mathrm{\θ}}_3^I& {\mathrm{\θ}}_4^I& {\mathrm{\θ}}_5^I& {\mathrm{\θ}}_6^I\end{array}\right]$ 下，工业机器人末端坐标系为∑_GE，其基座标系到末端坐标系的齐次转换阵为

为了实现一号工业机器人3对服务卫星1的空间机械臂的末端进行等效，需要使∑_GE=∑_ST，那么能够得到服务卫星1惯性坐标系到工业机器人基座标系的等效关系为：

$T\·_{\mathrm{GB}}^{\mathrm{GE}}{\left(T_{\mathrm{BH}}^{\mathrm{SI}}\right)}^{-1}{\mathrm{\Σ}}_{\mathrm{SI}}={\mathrm{\Σ}}_{\mathrm{GB}}$

假设在任一时刻t，空间机械臂在其惯性坐标系下的位姿为齐次阵T_S(t)，则等效的工业机器人末端在其基座标系下的位姿为

$T_G\left(t\right)=T_{\mathrm{GB}}^{\mathrm{GE}}{\left(T_{\mathrm{ST}}^{\mathrm{SI}}\right)}^{-1}{T}_S\left(t\right)=T\·T_{\mathrm{SI}}^{\mathrm{ST}}_{\mathrm{GB}}^{\mathrm{GE}}\·T_S\left(t\right)$

得到等效的机器人末端位姿后，对工业机器人反解得到工业机器人的关节角指令，然后对工业机器人进行规划和驱动。

具体实施方式七：本实施方式是对实施方式五所述的空间机器人视觉伺服捕获运动目标的地面模拟方法的进一步限定，本实施方式中，运动控制和目标卫星手柄运动学等效计算机6的目标卫星2手柄的运动学等效方法为：二号工业机器人4末端在其基座标系下的位姿T'_G(t)为：

${T^{\′}}_G\left(t\right)=T^{\′}_{\mathrm{GB}}^{\mathrm{GE}}{\left(T_{\mathrm{BH}}^{\mathrm{SI}}\right)}^{-1}{T}_H\left(t\right),$

其中，为二号工业机器人4在初始臂型时基座到末端的齐次转换阵，为二号工业机器人4的手柄坐标系到一号工业机器人3的惯性坐标系的转换阵，T_H(t)为t时刻二号工业机器人4的手柄在惯性坐标系下的位姿。

目标卫星2手柄运动等效模块的设计与机械手的原理基本一致，同样是将目标星手柄在惯性系中的位姿等效到二号工业机器人4的基座标系中。假设抓捕初始时，目标卫星2手柄坐标系到服务卫星1惯性坐标系的转换阵为二号工业机器人4初始臂型时基座到末端的齐次转换阵为

假设任一时刻t，目标星手柄在惯性坐标系下的位姿为T_H(t)，则等效的二号工业机器人4末端在其基座标系下的位姿为：

${T^{\′}}_G\left(t\right)=T^{\′}_{\mathrm{GB}}^{\mathrm{GE}}{\left(T_{\mathrm{BH}}^{\mathrm{SI}}\right)}^{-1}{T}_H\left(t\right)=T^{\′}_{\mathrm{GB}}^{\mathrm{GE}}\·T_{\mathrm{SI}}^{\mathrm{BH}}\·T_H\left(t\right)$

具体实施方式八：结合图2至图4说明本实施方式，本实施方式是对实施方式五所述的空间机器人视觉伺服捕获运动目标的地面模拟方法的进一步限定，本实施方式中，所述的动力学计算机7通过TCP/IP网络通信方式与运动控制和空间机器人末端运动学等效计算机5以及运动控制和目标卫星手柄运动学等效计算机6之间进行数据传输。

本实施方式中，地面工业机器人模拟部分采用TCP/IP网络通信方式，以保证系统的实时性。

具体实施方式九：结合图3说明本实施方式，本实施方式是对实施方式五所述的空间机器人视觉伺服捕获运动目标的地面模拟方法的进一步限定，本实施方式中，所述的运动控制和空间机器人末端运动学等效计算机5嵌入有由软件实现的空间机器人末端等效转换模块和一号工业机器人控制器。

本实施方式中，动力学计算机7根据空间机器人的控制力矩计算得到空间机器人基座位姿和关节角信息，并将基座位姿和关节角信息通过TCP/IP协议发送给运动控制和空间机器人末端运动学等效计算机5内的空间机器人末端等效转换模块，所述空间机器人末端等效转换模块首先通过运动学正解得到一号工业机器人3末端的位姿信息，然后通过对一号工业机器人3的运动学逆解，得到等效的工业机器人关节角指令，然后通过一号工业机器人控制器对一号工业机器人3的关节进行控制。

具体实施方式十：结合图3说明本实施方式，本实施方式是对实施方式五所述的空间机器人视觉伺服捕获运动目标的地面模拟方法的进一步限定，本实施方式中，所述的运动控制和目标卫星手柄运动学等效计算机6嵌入有由软件实现的空间机器人末端运动学等效运动模块和二号工业机器人控制器。

本实施方式中，当给定目标卫星2在空间的运动情况时，可以通过运动控制和目标卫星手柄运动学等效计算机6中的运动学等效运动模块计算出二号工业机器人4的关节的运动情况，然后通过二号工业机器人控制器来控制二号工业机器人4的运动。

Claims (7)

1.空间机器人视觉伺服捕获运动目标的地面模拟方法，其特征在于：该方法是基于下述地面模拟系统实现的，所述地面模拟系统包括一号工业机器人(3)、二号工业机器人(4)、运动控制和空间机器人末端运动学等效计算机(5)、运动控制和目标卫星手柄运动学等效计算机(6)、动力学计算机(7)、空间机器人关节电模拟器(8)和中央控制器(9)；

所述一号工业机器人(3)用于模拟具有漂浮基座的服务卫星(1)的空间机械臂末端的运动；二号工业机器人(4)用于模拟目标卫星(2)的运动，二号工业机器人(4)上安装有目标卫星模拟器(4-3)，所述目标卫星模拟器(4-3)位于二号工业机器人(4)的机械臂与视觉靶标(4-2)之间；

手眼相机(3-1)的相对位姿关系信号端通过CAN总线连接中央控制器(9)的相对位姿关系信号端，中央控制器(9)的关节角信号端通过CAN总线连接空间机器人关节电模拟器(8)的关节角信号端，空间机器人关节电模拟器(8)的控制力矩信号端通过CAN总线连接动力学计算机(7)的控制力矩信号端，动力学计算机(7)的关节角和基座本体位姿信息信号端同时连接运动控制和空间机器人末端运动学等效计算机(5)的关节角和基座本体位姿信息信号端以及运动控制和目标卫星手柄运动学等效计算机(6)的关节角和基座本体位姿信息信号端，运动控制和空间机器人末端运动学等效计算机(5)的控制信号端通过PCI总线连接一号工业机器人(3)的控制信号端，运动控制和目标卫星手柄运动学等效计算机(6)的控制信号端通过PCI总线连接二号工业机器人(4)的控制信号端；

该地面模拟方法包括以下步骤：

步骤一、手眼相机(3-1)将测量得到的捕获手爪(3-2)与捕获手柄(4-1)的相对位姿关系信息通过CAN总线传输给中央控制器(9)；

步骤二、中央控制器(9)根据步骤一中的相对位姿关系信息规划出空间机械臂的期望关节角和期望关节角速度，然后通过CAN总线将所述期望关节角和期望关节角速度传输给空间机器人关节电模拟器(8)；

步骤三、动力学计算机(7)仿真得到理论关节角和理论关节角速度，并将该理论关节角和理论关节角速度发送给空间机器人关节电模拟器(8)；

步骤四、空间机器人关节电模拟器(8)计算关节的控制力矩，并将所述控制力矩发送给动力学计算机(7)，

所述控制力矩τ_i通过下述公式计算：

${\mathrm{\τ}}_i=K_{pi}(q_{ri}-q_{ei})+K_{di}({\stackrel{\·}{q}}_{ri}-{\stackrel{\·}{q}}_{ei}),$

其中，i代表第i关节，p代表比例项，d代表微分项，e代表期望，r代表实际，τ_i代表空间机器人的关节控制力矩，q_ei代表空间机器人的期望关节角，代表空间机器人的期望关节角速度，q_ri代表空间机器人的理论关节角，代表空间机器人的理论关节角速度，K_pi代表比例控制系数，K_di代表微分控制系数；

步骤五、动力学计算机(7)以接收到的控制力矩作为输入量进行实时仿真，得到空间机器人当前时刻的关节角和基座本体位姿信息，并通过TCP/IP网络以组播的形式将关节角和基座本体位姿信息发送到运动控制和空间机器人末端运动学等效计算机(5)以及运动控制和目标卫星手柄运动学等效计算机(6)；

步骤六、运动控制和空间机器人末端运动学等效计算机(5)接收到的关节角和基座本体位姿信息后，首先通过运动学正解得到空间机器人末端的位姿信息x_s，然后解算并生成一号工业机器人(3)运动等效所需要的关节角指令，并根据该关节角指令控制一号工业机器人(3)进行运动，

所述空间机器人末端的位姿信息x_s＝FK(q_is)，其中q_is代表空间机器人的关节角，FK为正运动学函数，

所述关节角指令q_iA＝IK(x_A)，其中x_A代表工业机器人末端位姿，IK为逆运动学函数；

运动控制和空间机器人末端运动学等效计算机(5)的空间机器人末端运动学等效方法为：等效的一号工业机器人(3)的末端在其基座标系下的位姿T_G(t)为

$T_G\left(t\right)=T_{GB}^{GE}\·T_{SI}^{ST}\·T_S\left(t\right),$

其中，为初始臂型下，一号工业机器人(3)的基座标系到末端坐标系的齐次转换阵；为服务卫星(1)惯性坐标系到机械手末端坐标系的齐次转换阵；T_S(t)为t时刻服务卫星(1)的空间机械臂在服务卫星(1)的惯性坐标系下的位姿矩阵，且该位姿矩阵为齐次阵；

运动控制和目标卫星手柄运动学等效计算机(6)的目标卫星(2)手柄的运动学等效方法为：二号工业机器人(4)末端在其基座标系下的位姿T'_G(t)为：

${T^{\′}}_G\left(t\right)={T_{GB}^{GE}}^{\′}{\left(T_{BH}^{SI}\right)}^{-1}{T}_H\left(t\right),$

其中，为二号工业机器人(4)在初始臂型时基座到末端的齐次转换阵，为目标卫星(2)手柄坐标系到服务卫星(1)惯性坐标系的转换阵，T_H(t)为t时刻二号工业机器人(4)的手柄在惯性坐标系下的位姿。

2.根据权利要求1所述的空间机器人视觉伺服捕获运动目标的地面模拟方法，其特征在于：所述的动力学计算机(7)通过TCP/IP网络通信方式与运动控制和空间机器人末端运动学等效计算机(5)以及运动控制和目标卫星手柄运动学等效计算机(6)之间进行数据传输。

3.根据权利要求1所述的空间机器人视觉伺服捕获运动目标的地面模拟方法，其特征在于：所述的运动控制和空间机器人末端运动学等效计算机(5)嵌入有由软件实现的空间机器人末端等效转换模块和一号工业机器人控制器。

4.根据权利要求1所述的空间机器人视觉伺服捕获运动目标的地面模拟方法，其特征在于：所述的运动控制和目标卫星手柄运动学等效计算机(6)嵌入有由软件实现的空间机器人末端运动学等效运动模块和二号工业机器人控制器。

5.根据权利要求1所述的空间机器人视觉伺服捕获运动目标的地面模拟方法，其特征在于：动力学计算机(7)通过TCP/IP网络通信方式与运动控制和空间机器人末端运动学等效计算机(5)以及运动控制和目标卫星手柄运动学等效计算机(6)之间进行数据传输。

6.根据权利要求1所述的空间机器人视觉伺服捕获运动目标的地面模拟方法，其特征在于：所述的运动控制和空间机器人末端运动学等效计算机(5)嵌入有由软件实现的空间机器人末端等效转换模块和一号工业机器人控制器。

7.根据权利要求1所述的空间机器人视觉伺服捕获运动目标的地面模拟方法，其特征在于：所述的运动控制和目标卫星手柄运动学等效计算机(6)嵌入有由软件实现的空间机器人末端运动学等效运动模块和二号工业机器人控制器。