CN110125936B - 一种空间机器人的地面实验验证*** - Google Patents

Abstract

本发明提供一种空间机器人的柔顺控制方法及地面实验验证***，方法包括如下步骤：S1：获取所述空间机器人的机械臂末端工具与目标航天器之间的接触力；S2：根据所述接触力的大小确定柔顺度矩阵；S3：根据所述接触力和所述柔顺度矩阵确定所述机械臂末端工具的参考速度；S4：由所述机械臂末端工具的参考速度得到机械臂各关节的参考角速度，根据参考角速度控制所述机械臂各关节内部电机的运动。空间机器人的柔顺控制方法是根据外部接触力改变柔顺度系数的方法，并最终将外部接触力转化成空间机器人末端工具的速度，实现空间机器人的末端工具快速稳定的顺从外部接触力而运动，对接触力起到缓冲的作用。

Description

一种空间机器人的地面实验验证***

技术领域

本发明涉及空间机器人的技术领域，尤其涉及一种空间机器人的柔顺控制方法及地面实验验证***。

背景技术

随着科学技术的不断发展，人们对外太空的探索不断深入，空间中航天器的数量也越来越多。对太空垃圾的清理，故障航天器的维修，航天部件的更换等在轨服务任务成为人们探索太空的基本保障。由于太空环境复杂，仅依靠宇航员来进行在轨服务的成本代价过高，所以目前在轨服务技术越来越依赖于空间机器人的自主作业，而空间机器人柔顺控制是未来在轨服务工程化中的关键技术之一。

空间机器人是指在外太空环境中运行的机器人，通常由基座飞行器和安装在基座上的机械臂组成。捕获技术是空间机器人自主在轨服务的基础，它是指两个航天器(目标航天器和跟踪航天器)同时在预定的轨道位置相遇并最终在结构上形成一个整体。根据捕获目标是否具有专门的捕获装置，可分为合作目标和非合作目标。合作目标自主交会对接技术发展相对较好，已广泛应用于空间站的维护和补给任务中。然而，轨道上存在着许多非合作目标，如在轨卫星、报废卫星、轨道碎片和一些军事任务目标。因此，非合作目标捕获技术是未来自主轨道服务发展的重要方向。空间机器人在轨捕获技术的意义和价值体现在两个方面：

1、可用于空间垃圾处理和脱轨作业，提高轨道资源利用率；

2、对于轨道装配、维护和供给具有明显的经济效益。

空间机器人的非合作目标捕获可分为捕获前运动规划、捕获过程中的碰撞检测与力柔顺控制、捕获后稳定控制三个阶段。

在空间机器人对非合作目标进行抓捕的过程中，虽然有前期的视觉伺服跟踪和运动规划，但在捕获的瞬间空间机器人手爪和非合作目标之间仍有相对运动，如果此时空间机器人直接进行刚性抓捕的话，轻则造成非合作目标的逃离，重则损毁空间机器人，因此，在此过程中，空间机器人需具有一定的柔顺性来保证与非合作目标之间的接触力较小，安全完成捕获任务。而空间机器人基座的稳定性已被许多研究者广泛研究，可以通过喷气或飞轮等方式保证空间机器人在运动过程中基座的稳定。因此，本专利主要在空间机器人基座稳定的前提下对捕获过程的力柔顺控制进行研究。

现有技术中，采用机械的方式使刚性机械臂手腕具有一定柔性，从而延长控制捕获过程中接触的接触时间，降低了控制难度，种方法虽然简单有效，但通用性较差，功能比较单一；还有，采用关节力矩传感器来感知捕捉过程中的接触力，并通过相应的控制算法控制机械臂的各关节运动，从而使机械臂具有一定的柔顺性，并在地面2维平面中进行了实验验证；此方法中的机械臂每个关节都装有力矩传感器，成本过高；柔顺控制方法采用各关节的位置控制，响应速度低，而且只在地面2维空间进行了验证，具有一定的局限性；还有将关节电流近似等于关节力矩，建立机械臂的动力学模型，并在二连杆上做相应的实验验证，采用电流信号来近似代替负载力矩，虽然减少力矩传感器降低了成本，但其精度得不到保证，尤其在关节运动时两者差异加大，相关理论的验证只是在简单的二关节模型上得到验证，相关计算方案缺乏严格的理论推导，并且没有拓展到空间多自由度机器人，缺乏实验验证，需要对机械臂进行复杂的动力学建模以及模型参数的辨识，无法排除或减小一些非线性因素的影响。

所以现有技术中缺乏一种精度高、通用性好、成本适宜的空间机器热的柔顺控制方法，同时也缺乏一种对应的可靠的地面实验验证***。

发明内容

本发明为了解决现有技术中缺乏一种精度高、通用性好、成本适宜的空间机器热的柔顺控制方法，同时也缺乏一种对应的可靠的地面实验验证***的问题，提供一种空间机器人的柔顺控制方法及地面实验验证***。

为了解决上述问题，本发明采用的技术方案如下所述：

一种空间机器人的柔顺控制方法，包括如下步骤：S1：获取所述空间机器人的机械臂末端工具与目标航天器之间的接触力；S2：根据所述接触力的大小确定柔顺度矩阵；S3：:根据所述接触力和所述柔顺度矩阵确定所述机械臂末端工具的参考速度；S4：由所述机械臂末端工具的参考速度得到机械臂各关节的参考角速度，根据参考角速度控制所述机械臂各关节的内部电机的运动。

优选地，步骤S2确定柔顺度矩阵K_r的方法为：K_r＝c₀|F_e|，其中，c₀为常数，F_e为所述接触力。

优选地，c₀取值为0.002；通过PD控制算法控制所述机械臂各关节内部电机的运动。

优选地，步骤S3包括：所述接触力F_e通过基于速度环的阻抗控制转换成所述机械臂末端工具的参考速度V：V＝K_rF_e。

优选地，步骤S4包括：通过所述空间机器人的广义雅克比矩阵的逆J^*-1和所述机械臂末端工具的参考速度V得到所述机械臂各关节的参考角速度

即：

本发明还提供一种地面实验验证***，包括：机器人、机械臂末端工具、多维力传感器和处理器；所述多维力传感器分别与所述机器人和所述机械臂末端工具连接，并采集所述机械臂末端工具与环境之间的接触力在多维力传感器坐标系下X轴、Y轴、Z轴和绕X轴、绕Y轴、绕Z轴分量上的值；处理器，与所述机器人和所述多维力传感器通讯连接，用于接收所述机器人和所述多维力传感器的实时信息并实时发送运动控制指令给所述机器人和所述多维力传感器；对所述多维力传感器采集的所述接触力进行滑动均值滤波并补偿所述多维力传感器的零点漂移；对所述机械臂末端工具的重力进行补偿。

优选地，所述滑动均值滤波包括如下：获得所述多维力传感器连续采集的n个数据a[n]＝{x₁,x₂…x_n}，将这n个数据视为一组；此后，每采集一个数据x，便有如下操作：a[i-1]＝a[i]，a[n-1]＝x，其中，i＝1,2…n-1，n为自然数；对上述数据进行均值操作，便可得到滑动均值滤波后的所述接触力的值为：

优选地，所述补偿所述多维力传感器的零点漂移包括如下步骤：T1：在所述接触力为零的情况下，将所述机器人移动到所述机械臂末端工具的坐标系中Z轴竖直向上时的位姿，得到所述多维力传感器的读数F₁；T2：在所述接触力为零的情况下，将所述机器人移动到所述机械臂末端工具的坐标系中Z轴竖直向下时的位姿，得到所述多维力传感器的读数F₂；T3：得到所述多维力传感器的零点漂移F₀：

得到所述机械臂末端工具的重力F_g：

则，所述机械臂末端工具的所述接触力为：F_e＝F-F_g-F₀；其中，F是所述多维力传感器的实时测量值。

优选地，所述对所述机械臂末端工具的重力进行补偿包括：所述机器人的末端坐标系、所述多维力传感器的坐标系和所述机械臂末端工具的坐标系的关系表示如下：

其中，

是所述多维力传感器的坐标系{S}相对于所述机器人的末端坐标系的旋转矩阵，

是所述多维力传感器的坐标系{S}相对于所述机器人的末端坐标系的位置，

是所述机械臂末端工具的坐标系{T}相对于所述多维力传感器的坐标系{S}的旋转矩阵，

是所述机械臂末端工具的坐标系{T}相对于所述多维力传感器的坐标系{S}的位置，

表示所述多维力传感器的坐标系{S}相对于所述机器人的末端坐标系的变换矩阵；

表示所述机械臂末端工具的坐标系{T}相对于所述多维力传感器的坐标系{S}的变换矩阵；l₀,l₁表示Z轴上的增量；

所述机器人的末端坐标系相对于机器人基座的变化矩阵表示为：

其中，

和

分别为所述机器人的末端坐标系相对于所述机器人基座的旋转矩阵和位置；

获取所述机器人的末端姿态信息为：

其中，

为所述机器人的末端坐标系相对于所述机器人基座的姿态旋转矢量；

则，旋转角度为：

其中，Rx,Ry,Rz表示随着姿态旋转矢量变化的变量；

根据罗德里格斯旋转方程，可以获得旋转矩阵为：

其中，cθ＝cosθ，sθ＝sinθ，vθ＝1-cosθ，k_x＝Rx/θ，k_y＝Ry/θ，k_z＝Rz/θ；

所述机械臂末端工具的重力在所述机器人基座的坐标系中表示为：

⁰G＝[0 0 -mg]^T

其中，m和g分别为所述机械臂末端工具的质量和重力加速度；

得到所述机械臂末端工具的重力相对所述多维力传感器的坐标系的值为：

其中，

得到所述机械臂末端工具在所述多维力传感器的坐标系下沿X轴，Y轴和Z轴上所需补偿的重力值为：

其中，F_x，F_y和F_z分别表示所述机械臂末端工具在所述多维力传感器的坐标系下沿X轴，Y轴和Z轴上的重力补偿值；

在所述多维力传感器的坐标系下的对每个轴的补偿力矩为：

所述机械臂末端工具在不同姿态下的重力补偿值为：

本发明还提供一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时实现如上任一所述方法的步骤。

本发明的有益效果为：提供一种空间机器人的柔顺控制方法及地面实验验证***，空间机器人的柔顺控制方法是根据外部接触力改变柔顺度系数的方法，并最终将外部接触力转化成空间机器人末端工具的速度，实现空间机器人的末端工具快速稳定的顺从外部接触力而运动，对接触力起到缓冲的作用；地面实验验证***采用了滑动均值滤波的方法对多维力传感器信号进行滤波，提出了一种多维力传感器零点补偿和末端工具重力补偿的方法，本重力补偿的目的是模拟外太空的微重力环境。

附图说明

图1是本发明实施例中空间机器人的柔顺控制方法示意图。

图2是本发明实施例中基于机械臂末端速度的力柔顺控制方法示意图。

图3是本发明实施例中地面实验验证***的示意图。

图4是本发明实施例中一种机器人末端姿态的示意图。

图5是本发明实施例中另一种机器人末端姿态的示意图。

图6是本发明实施例中地面实验验证***中各坐标系的关系示意图。

具体实施方式

为了使本发明实施例所要解决的技术问题、技术方案及有益效果更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

需要说明的是，当元件被称为“固定于”或“设置于”另一个元件，它可以直接在另一个元件上或者间接在该另一个元件上。当一个元件被称为是“连接于”另一个元件，它可以是直接连接到另一个元件或间接连接至该另一个元件上。另外，连接即可以是用于固定作用也可以是用于电路连通作用。

需要理解的是，术语“长度”、“宽度”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明实施例和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。

此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括一个或者更多该特征。在本发明实施例的描述中，“多个”的含义是两个或两个以上，除非另有明确具体的限定。

实施例1

在空间机器人的机械臂末端工具捕获非合作目标过程中，机械臂末端工具和非合作目标之间的接触力往往属于碰撞产生的力，因此需要机械臂末端工具柔顺控制有较高响应速度，而且还需要机械臂末端工具可以根据碰撞力的大小和方向平稳运行。

如图1所示，本发明提供一种空间机器人的柔顺控制方法，包括如下步骤：

S1：获取所述空间机器人的机械臂末端工具与目标航天器之间的接触力；

S2：:根据所述接触力的大小确定柔顺度矩阵；

S3：根据所述接触力和所述柔顺度矩阵确定所述机械臂末端工具的参考速度；

S4：由所述机械臂末端工具的参考速度得到机械臂各关节的参考角速度，根据参考角速度控制所述机械臂各关节的内部电机的运动。

如图2所示，V表示根据机械臂末端工具和目标航天器之间的接触力F_e得到的参考速度；K_r为常数对角矩阵，即为柔顺度矩阵，

和

分别表示机械臂末端工具各关节的参考角速度和实际角速度。机械臂末端工具在捕获目标航天器时所产生的接触力通过基于速度环的阻抗控制转换成机械臂末端工具的参考速度为：

V＝K_rF_e (1)

通过空间机器人的广义雅克比矩阵的逆J^*-1，得到机械臂末端工具的各个关节所对应的参考角速度

即

关节控制器采用PD控制算法来控制关节内部电机的运动，从而使关节达到期望的运动速度，从而实现机械臂末端工具顺从碰撞力运动。

从公式(1)中可以看出K_r的值越大械臂末端工具柔顺度越高，但随之而来时问题是械臂末端工具的运动也越不稳定。在捕获过程中，空间机器人通常需要快速响应大的碰撞力，而当碰撞力较小时，它们需要更好的稳定性。因此，本文采用根据接触力大小改变柔顺度矩阵的方法来控制机械臂末端工具，并可设计为：

K_r＝c₀|F_e| (3)

其中，c₀为常数，c₀取值可根据需求人为设定，在一种实施例中可以取0.002。在本发明的一种实施例中，当机械臂末端工具和目标航天器之间产生较大的碰撞力时，柔顺算法中的柔顺度矩阵K_r也会比较大，机械臂末端工具更柔顺，可以快速的顺从碰撞力运动，从而起到缓冲作用；当二者之间的接触力较小时，柔顺度矩阵K_r也比较小，机械臂末端工具可以缓缓的顺从接触力运动，避免产生运动过激的情况，提高了稳定性。

实施例2

如图3所示，一种地面实验验证***，包括：机器人、机械臂末端工具、多维力传感器和处理器；

所述多维力传感器分别与所述机器人和所述机械臂末端工具连接，并采集所述机械臂末端工具与环境之间的接触力在多维力传感器坐标系下X轴、Y轴、Z轴和绕X轴、绕Y轴、绕Z轴分量上的值；

处理器，与所述机器人和所述多维力传感器通讯连接，用于接收所述机器人和所述多维力传感器的实时信息并实时发送运动控制指令给所述机器人和所述多维力传感器；对所述多维力传感器采集的所述接触力进行滑动均值滤波并补偿所述多维力传感器的零点漂移；对所述机械臂末端工具的重力进行补偿。

在本发明的一种实施例中，机器人是丹麦机器人公司生产的UR5机器人，多维力传感器是美国ATI公司生产的Mini45多维力传感器。

在机械臂末端工具与外界环境交互的过程中，安全性主要体现在接触力控制和位置控制两方面，一般来说***刚度越高对位置控制的精度也就越高，但是增大刚度会导致力控制的难度加大，具有柔性的末端更容易实现力控制，在实验初期也可一定程度上提高安全性。

处理器可以是中央处理单元(Central Processing Unit，CPU)，还可以是其他通用处理器、数字信号处理器(Digital Signal Processor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现成可编程门阵列(Field-Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等，所述处理器可以独立存在，也可以安装在桌上型计算机、笔记本、掌上电脑及云端服务器等计算设备。

在本发明的一种实施例中，在VS上采用C++语言编写了人机界面，***中的主控制程序采用多线程的方式通过TCP/IP协议。

为了提高实验中多维力传感器所采集到的接触力的可靠性，本发明对多维力传感器所采集的接触力的信息进行了滑动均值滤波，从而来降低在实验中噪声干扰，而且对多维力传感器的零点漂移也进行了补偿。

在实验中采用滑动均值滤波的方法来降低噪声干扰，滑动均值滤波是连续采集n个数据a[n]＝{x₁,x₂…x_n}，将这n个数据视为一组；此后，每采集一个数据x，便有如下操作：

a[i-1]＝a[i] (4)

a[n-1]＝x (5)

其中，i＝1,2…n-1，n为自然数。对上述数据进行均值操作，便可得到滑动均值滤波后的所述接触力的值为：

这种滤波的优点之一是不管n的值有多大，每采集到一个值就可以在本次程序循环中得到滤波后的结果，实时性较好。

在地面实验验证***中，多维力传感器的测量值F可表示为：

F＝F_e+F_g+F₀ (7)

其中：

F_e表示机械臂末端工具受到的外部接触力；

F_g表示机械臂末端工具的重力；

F₀为多维力传感器自身的零点漂移。

为了准确的获得机械臂末端工具与环境之间的接触力，提高传感器的测量可靠性，本实验需要对零点漂移量进行补偿。具体方法如下：

首先，在零外部接触力的情况下，将机器人移动到机械臂末端工具坐标系Z轴竖直向上时的位姿，如图4所示，并记录多维力传感器的读数F₁；

然后，在零外部接触力的情况下，将机器人移动到机械臂末端工具坐标系Z轴竖直向下时位姿，如图5所示，记录多维力传感器的读数F₂。

最后，可以获得以下等式：

根据公式(8)和(9)，可以得到本实验验证***中多维力传感器各个分量的零点漂移量F₀和机械臂末端工具的重量F_g。因此，机械臂末端工具所受的外部接触力可表示为：

F_e＝F-F_g-F₀ (10)

如图6所示，根据实验***的结构以得到机器人的末端坐标系{UR}、多维力传感器坐标系{S}和机械臂末端工具坐标系{T}的关系。机器人的末端坐标系，多维力传感器坐标系和机械臂末端工具坐标系具有相同的姿态，只是Z轴上有一定的增量，它们之间的关系可以表示为：

其中，

是所述多维力传感器的坐标系{S}相对于所述机器人的末端坐标系的旋转矩阵，

是所述多维力传感器的坐标系{S}相对于所述机器人的末端坐标系的位置，

是所述机械臂末端工具的坐标系{T}相对于所述多维力传感器的坐标系{S}的旋转矩阵，

是所述机械臂末端工具的坐标系{T}相对于所述多维力传感器的坐标系{S}的位置，

表示多维力传感器坐标系{S}相对于机器人的末端坐标系的变换矩阵；

表示机械臂末端工具坐标系{T}相对于多维力传感器坐标系{S}的变换矩阵；l₀,l₁表示Z轴上的增量。

机器人的末端坐标系相对于机器人基座的变换矩阵可表示为：

其中，

和

分别表示机器人的末端坐标系相对于其基座的旋转矩阵和位置。可以通过两种方式得到它们的值，一种是通过关节角度和DH参数，另一种是通过读取机器人数据包。在实验中，采用通过读取机器人数据包的方法来获取机器人末端姿态信息为：

其中，

表示机器人的末端坐标系相对于机器人基座的姿态旋转矢量。由旋转矢量可以得到旋转角度为：

其中，Rx,Ry,Rz表示随着姿态旋转矢量变化的变量；

根据罗德里格斯旋转方程，获得旋转矩阵为：

其中，cθ＝cosθ，sθ＝sinθ，vθ＝1-cosθ，k_x＝Rx/θ，k_y＝Ry/θ，k_z＝Rz/θ。

在地面实验验证***中，需要对机械臂末端工具的重力进行补偿来模拟空间中的微重力环境。末端工具的重力在机器人基座坐标系(惯性系)中可表示为：

⁰G＝[0 0 -mg]^T (16)

其中，m和g分别表示机械臂末端工具的质量和重力加速度。

因此，可以通过图6各坐标系之间的关系转换来获得工具重力相对于多维力传感器坐标系的值为：

其中，

因此，根据公式(15)，(16)和(17)可以得到机械臂末端工具在多维力传感器坐标系下沿X轴，Y轴和Z轴上所需补偿的重力值为：

其中，F_x，F_y和F_z分别表示末端工具在多维力传感器坐标系下沿X轴，Y轴和Z轴上的重力补偿值。而在本实验验证***中，由于机械臂末端工具坐标系和多维力传感器坐标系具有相同的姿势，仅仅在Z轴上有增量l₁。因此，在多维力传感器坐标系下的对每个轴的补偿力矩应为：

结合公式(18)(19)便可实时计算出机器人在不同姿态下机械臂末端工具的重力补偿值为：

该地面实验验证***对机械臂末端工具进行重力补偿之后便可以有效的模拟空间中的微重力环境，可以对本专利中所提的力柔顺控制方法进行实验验证。

空间机器人捕获目标的过程中，机械臂末端工具对外部接触力的柔顺度和响应速度与所设计的柔顺控制算法式(1)中的柔顺度系数K_r有关；为了空间机器人捕获非合作目标过程中的稳定性和及时性，本专发明提出了柔顺度系数K_r随外部接触力F_e的变化而变化(见公式(3))，为了验证这种力柔顺控制方法，取公式(3)中c₀值为0.002，并在地面实验验证***上进行了实验验证。本发明首先采用了重力补偿算法来对机械臂末端工具的重力进行补偿模拟空间中的失重环境；此外，也补偿了多维力传感器的零点漂移，提高外部接触力的感知精度和可读性。然后，采用本发明所设计的柔顺控制算法根据外部接触力计算出机械臂末端工具的参考速度，使机械臂末端工具在一定程度上顺从外部碰撞力运动，起到缓冲的作用。最后通过实验验证了机械臂末端工具能快速且稳定的顺从外部接触力运动，具有很好的柔顺效果，可以有效的缓冲空间机器人在捕获过程中的接触力，安全地完成捕获任务。

本发明至少具备如下优点：

1.本发明采用末端安装有多维力传感器的空间机器人可直接采集捕获过程中的末端接触力，相比采用电流信号来近似代替关节力矩的方式解决了的精确度不准问题，而且相比于采用关节力矩传感器方式降低了成本。

2.本发明所采用的力柔顺控制方法是直接将末端多维力传感器的测量值转换成相应的末端工具速度，不需要建立空间机器人的动力学模型，计算简单。

3.本发明提出了一种空间机器人力柔顺控制方法，该力柔顺控制方法采用了根据外部接触力变柔顺度系数的方法，不仅使机器人有较高的柔顺性，而且也提高了机器人运动过程中的稳定性，解决了稳定性和柔顺度不能兼得的问题。

4.为了验证本发明中所提的力柔顺控制方法，本发明设计了机器人力柔顺控制的地面实验验证***，硬件部分包括机器人、多维力传感器、机械臂末端工具和处理器，提高了地面实验验证***的实时性和准确性。

5.针对该地面实验验证***提出了机械臂末端工具重力补偿的方法，本重力补偿的目的是模拟外太空的微重力环境。

6.本发明在地面实验验证***中采用滑动均值滤波的方法对多维力传感器的测量值进行了滤波，并且针对该地面实验验证***提出了一种多维力传感器零点补偿方法，提高了空间机器人末端工具感知外部接触力精度，降低了噪声的影响。

实施例3

所述地面实验验证***如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明实现上述实施例方法中的全部或部分流程，也可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一计算机可读存储介质中，该计算机程序在被处理器执行时，可实现上述各个方法实施例的步骤。其中，所述计算机程序包括计算机程序代码，所述计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。所述计算机可读介质可以包括：能够携带所述计算机程序代码的任何实体或装置、记录介质、U盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、电载波信号、电信信号以及软件分发介质等。需要说明的是，所述计算机可读介质包含的内容可以根据司法管辖区内立法和专利实践的要求进行适当的增减，例如在某些司法管辖区，根据立法和专利实践，计算机可读介质不包括电载波信号和电信信号。

以上内容是结合具体的优选实施方式对本发明所作的进一步详细说明，不能认定本发明的具体实施只局限于这些说明。对于本发明所属技术领域的技术人员来说，在不脱离本发明构思的前提下，还可以做出若干等同替代或明显变型，而且性能或用途相同，都应当视为属于本发明的保护范围。

Claims (8)

1.一种地面实验验证***，其特征在于，包括：机器人、机械臂末端工具、多维力传感器和处理器；

所述多维力传感器分别与所述机器人和所述机械臂末端工具连接，并采集所述机械臂末端工具与环境之间的接触力在多维力传感器坐标系下X轴、Y轴、Z轴和绕X轴、绕Y轴、绕Z轴分量上的值；

处理器，与所述机器人和所述多维力传感器通讯连接，用于接收所述机器人和所述多维力传感器的实时信息并实时发送运动控制指令给所述机器人和所述多维力传感器；对所述多维力传感器采集的所述接触力进行滑动均值滤波并补偿所述多维力传感器的零点漂移；对所述机械臂末端工具的重力进行补偿；

所述滑动均值滤波包括如下：获得所述多维力传感器连续采集的n个数据a[n]＝{x₁,x₂…x_n}，将这n个数据视为一组；此后，每采集一个数据x，便有如下操作：

a[i-1]＝a[i]

a[n-1]＝x

其中，i＝1,2…n-1，n为自然数；

对上述数据进行均值操作，便可得到滑动均值滤波后的所述接触力的值为：

2.如权利要求1所述的地面实验验证***，其特征在于，所述补偿所述多维力传感器的零点漂移包括如下步骤：

T1：在所述接触力为零的情况下，将所述机器人移动到所述机械臂末端工具的坐标系中Z轴竖直向上时的位姿，得到所述多维力传感器的读数F₁；

T2：在所述接触力为零的情况下，将所述机器人移动到所述机械臂末端工具的坐标系中Z轴竖直向下时的位姿，得到所述多维力传感器的读数F₂；

T3：得到所述多维力传感器的零点漂移F₀：

得到所述机械臂末端工具的重力F_g：

则，所述机械臂末端工具的所述接触力为：F_e＝F-F_g-F₀；

其中，F是所述多维力传感器的实时测量值。

3.如权利要求1所述的地面实验验证***，其特征在于，所述对所述机械臂末端工具的重力进行补偿包括：

所述机器人的末端坐标系、所述多维力传感器的坐标系和所述机械臂末端工具的坐标系的关系表示如下：

其中，

是所述多维力传感器的坐标系{S}相对于所述机器人的末端坐标系的旋转矩阵，

是所述多维力传感器的坐标系{S}相对于所述机器人的末端坐标系的位置，

是所述机械臂末端工具的坐标系{T}相对于所述多维力传感器的坐标系{S}的旋转矩阵，

是所述机械臂末端工具的坐标系{T}相对于所述多维力传感器的坐标系{S}的位置，

表示所述多维力传感器的坐标系{S}相对于所述机器人的末端坐标系的变换矩阵；

表示所述机械臂末端工具的坐标系{T}相对于所述多维力传感器的坐标系{S}的变换矩阵；l₀,l₁表示Z轴上的增量；

所述机器人的末端坐标系相对于机器人基座的变化矩阵表示为：

其中，

和

分别为所述机器人的末端坐标系相对于所述机器人基座的旋转矩阵和位置；

获取所述机器人的末端姿态信息为：

其中，

为所述机器人的末端坐标系相对于所述机器人基座的姿态旋转矢量；

则，旋转角度为：

其中，Rx,Ry,Rz表示随着姿态旋转矢量变化的变量；

根据罗德里格斯旋转方程，可以获得旋转矩阵为：

其中，cθ＝cosθ，sθ＝sinθ，vθ＝1-cosθ，k_x＝Rx/θ，k_y＝Ry/θ，k_z＝Rz/θ；

所述机械臂末端工具的重力在所述机器人基座的坐标系中表示为：

⁰G＝[0 0 -mg]^T

其中，m和g分别为所述机械臂末端工具的质量和重力加速度；

得到所述机械臂末端工具的重力相对所述多维力传感器的坐标系的值为：

其中，

得到所述机械臂末端工具在所述多维力传感器的坐标系下沿X轴，Y轴和Z轴上所需补偿的重力值为：

其中，F_x，F_y和F_z分别表示所述机械臂末端工具在所述多维力传感器的坐标系下沿X轴，Y轴和Z轴上的重力补偿值；

在所述多维力传感器的坐标系下的对每个轴的补偿力矩为：

所述机械臂末端工具在不同姿态下的重力补偿值为：

4.如权利要求1所述的地面实验验证***，其特征在于，所述处理器接收所述机器人和所述多维力传感器的实时信息并实时发送运动控制指令给所述机器人和所述多维力传感器包括：采用柔顺控制方法控制所述机器人，包括如下步骤：

S1：获取所述机器人的机械臂末端工具与目标航天器之间的接触力；

S2：根据所述接触力的大小确定柔顺度矩阵；

S3：根据所述接触力和所述柔顺度矩阵确定所述机械臂末端工具的参考速度；

S4：由所述机械臂末端工具的参考速度得到机械臂各关节的参考角速度，根据所述参考角速度控制所述机械臂各关节的内部电机的运动。

5.如权利要求4所述的地面实验验证***，其特征在于，步骤S2确定柔顺度矩阵K_r的方法为：

K_r＝c₀|F_e|

其中，c₀为常数，F_e为所述接触力。

6.如权利要求5所述的地面实验验证***，其特征在于，c₀取值为0.002；通过PD控制算法控制所述机械臂各关节内部电机的运动。

7.如权利要求5所述的地面实验验证***，其特征在于，步骤S3包括：所述接触力F_e通过基于速度环的阻抗控制转换成所述机械臂末端工具的参考速度V：

V＝K_rF_e。

8.如权利要求5所述的地面实验验证***，其特征在于，步骤S4包括：通过所述机器人的广义雅克比矩阵的逆J^*-1和所述机械臂末端工具的参考速度V得到所述机械臂各关节的参考角速度

即：

Images