CN103600354B - 航天器机械臂柔性随动控制重力补偿方法 - Google Patents

Abstract

本发明公开了一种航天器机械臂的柔性随动控制重力补偿方法，柔性随动控制中将六维力传感器设置在机械臂末端与负载之间，该方法通过控制机械臂使负载处于多个不同的空间姿态，在负载无外力作用的情况下，记录每个空间姿态下六维力传感器的测量数据并根据力与力矩的物理关系，由测量数据计算得到负载的重力大小G及负载重心的坐标；并根据负载的受力情况对重力的影响进行补偿。本发明的方法，用机械臂系统本身测量负载的重力和重心位置，设计的算法计算出六维力传感器测得6个分量的重力补偿值，解决了机械臂柔性随动控制中的重力补偿问题。

Description

航天器机械臂柔性随动控制重力补偿方法

技术领域

本发明属于机械臂的控制技术领域，具体来说，本发明涉及一种机械臂柔性随动控制中补偿负载重力的方法。

背景技术

航天器研制具有单件小批量的特点，目前的装配作业大量依赖人工操作，并辅以吊具、升降车、架梯等简易工具进行不同航天器的装配工作。这种装配方式存在一定的局限性，对于大尺寸、大重量零部件人工安装困难，对于狭小空间内零部件的安装人工难以操作，对于安装精度要求较高的零部件，人工安装难以控制安装精度。

为解决航天器复杂工况的装配难题，提高装配质量、装配效率和安全性，希望引入机械臂，采用柔性随动控制方法对航天器进行装配。所述的机械臂柔性随动控制方法为：人手直接作用于安装在机械臂末端的负载，在机械臂末端法兰与负载之间安装有六维力传感器，用于感知作用其上的力与力矩信息，控制系统进一步获得人手作用的力与力矩信息，以此作为输入通过一定的控制算法控制机械臂运动，使负载跟随人手运动。

在上述机械臂柔性随动控制方法中，六维力传感器安装在机械臂末端法兰与负载之间，其感知到的力与力矩信息是负载重力与人手施力的综合作用结果，需要进行重力补偿，即将负载重力的作用分量从六维力传感器得到的力与力矩参数中减去，进而获得人手作用产生的力与力矩信息，作为输入参数用于控制机械臂的运动。

机械臂在运动过程中，如果其末端姿态发生变化，则六维力传感器与负载的空间姿态也随之变化，从六维力传感器得到的力与力矩参数中负载重力的作用分量也发生了变化，要在机械臂末端姿态不断变化的情况下实现柔性随动控制，需要在机械臂运动过程中，实时获得负载在当前姿态下的重力作用分量，并进行重力补偿。

本发明即根据机械臂柔性随动控制的需要，提出了一种航天器机械臂柔性随动控制重力补偿方法。

发明内容

本发明的目的在于提供一种航天器机械臂柔性随动控制重力补偿方法，能够在机械臂运动过程中，实时得到负载在当前姿态下六维力传感器各分量的重力补偿值，通过重力补偿实时获得人手或其他外力作用产生的力与力矩信息，实现机械臂的柔性随动控制。

为了实现上述目的，本发明采用了如下的技术方案：

一种航天器机械臂柔性随动控制重力补偿方法，其中柔性随动控制中将六维力传感器设置在机械臂末端与负载之间，该方法包括如下步骤：

1）控制机械臂使负载处于多个不同的空间姿态，在负载无外力作用的情况下，记录每个空间姿态下六维力传感器的测量数据；

2）根据力与力矩的物理关系，由步骤1）的测量数据计算得到负载的重力大小G及负载重心在六维力传感器自身坐标系中的坐标（x，y，z）；

3）在负载有外力作用的情况下，实时根据重力在六维力传感器的自身坐标系中的方向，将重力在六维力传感器的自身坐标系的三个坐标轴上投影得到三个重力分量Gx、Gy、Gz，分别作为六维力传感器的自身坐标系中X、Y、Z轴力分量的重力补偿值；

4）由公式MGx=Gz×y－Gy×z，MGy=Gx×z－Gz×x，MGz=Gy×x－Gx×y，计算得到六维力传感器自身坐标系中X、Y、Z轴上由重力引起的三个力矩分量MGx、MGy、MGz，分别作为六维力传感器自身坐标系中X、Y、Z轴上力矩分量的补偿值；

5）在负载有外力作用的情况下，六维力传感器测得的三个力分量为Fx、Fy、Fz，三个力矩分量为Mx、My、Mz，补偿得到外力在六维力传感器自身坐标系中的作用力为（Fx－Gx，Fy－Gy，Fz－Gz），作用力矩为（Mx－MGx，My－MGy，Mz－MGz），重力补偿完成。

上述技术方案步骤1）中，所述六维力传感器每次测量得到的数据有六个分量，包括在六维力传感器自身坐标系中的三个坐标轴方向的力分量，以及三个坐标轴方向的力矩分量。所述六维力传感器自身坐标系是与六维力传感器自身固连的空间直角坐标系。

上述技术方案步骤1）中，所述负载无外力作用指的是负载连接在六维力传感器上，仅受自身重力及与六维力传感器的连接支持力，不受其它外部作用力。

上述技术方案步骤2）中，假设步骤1）对N个不同的负载姿态测量得到N组六维力传感器数据，第i组的3个力分量为Fxi、Fyi、Fzi，3个力矩分量为Mxi、Myi、Mzi，在负载无外力作用的情况下，测得的力与力矩分量都由负载重力引起，则负载重力大小为：G=(Fxi²+Fyi²+Fzi²)^1/2。设负载重心在六维力传感器坐标系中的坐标为（x，y，z），在该坐标系下根据力与力矩的关系有：

Mxi=Fzi×y－Fyi×z

Myi=Fxi×z－Fzi×x

Mzi=Fyi×x－Fxi×y

取多个不同的负载姿态测量得到的多组六维力传感器数据，根据上面的方程组，采用一定的数学方法，如最小二乘法，即可求得负载重心在六维力传感器坐标系中的坐标（x，y，z）。

上述技术方案步骤3）中，在柔性随动控制过程中，机械臂末端姿态会发生变化，六维力传感器坐标系相对于大地坐标系的姿态也会随之变化，但负载重力始终竖直向下，因此负载重力在六维力传感器坐标系中的方向也在随之变化。经过对机械臂安放姿态的标定，机械臂控制系统容易实时得到重力方向与六维力传感器坐标系X、Y、Z轴的夹角（α，β，γ），则可计算得到负载重力在六维力传感器坐标系3个坐标轴上的投影为：Gx=G×cosα，Gy=G×cosβ，Gz=G×cosγ，Gx、Gy、Gz即是六维力传感器测得的力分量数据Fx、Fy、Fz中由于负载重力产生的部分。

上述技术方案步骤4）中，计算得到的MGx、MGy、MGz是六维力传感器测得的力矩分量数据Mx、My、Mz中由于负载重力产生的部分。

上述技术方案步骤5）中，若在柔性随动控制人手对负载进行操作，（Fx－Gx，Fy－Gy，Fz－Gz）可被认为是人手作用产生的力的分量，（Mx－MGx，My－MGy，Mz－MGz）可被认为是人手作用产生的力距的分量，根据（Fx－Gx，Fy－Gy，Fz－Gz）与（Mx－MGx，My－MGy，Mz－MGz）可控制机械臂跟随人手运动。

本发明的航天器机械臂柔性随动控制重力补偿方法，用机械臂系统本身测量负载的重力和重心位置，设计的算法可计算出六维力传感器测得6个分量的重力补偿值，解决了机械臂柔性随动控制中的重力补偿问题。

附图说明

图1为航天器机械臂柔性随动控制重力补偿方法适用的机械臂柔性随动控制的系统示意图。

图2为本发明的重力补偿方法中六维力传感器坐标系中负载重力的作用示意图。

1－机械臂、2－六维力传感器、3－被操作件、4－操作者、5－手、6－夹具、7－机械臂末端法兰。

具体实施方式

以下介绍的是作为本发明所述内容的具体实施方式，下面通过具体实施方式对本发明的所述内容作进一步的阐明。当然，描述下列具体实施方式只为示例本发明的不同方面的内容，而不应理解为限制本发明范围。

实施方式1：

如图1所示，在本发明的重力补偿方法使用的机械臂柔性随动控制系统中，六维力传感器2安装在机械臂1的末端法兰7与夹具6之间，六维力传感器是一种业内人员熟知的成熟的传感器产品，容易购买获得（如ATI公司的六维力传感器），其被操作件3被夹具6夹持，被操作件3与夹具6共同构成了六维力传感器2的负载，负载重力为G，方向为竖直向下，操作者4用手5推动被操作件3，对被操作件施加的力为Fh。

六维力传感器2检测到的力与力矩信息是负载重力G与手5作用力Fh共同作用的结果，要实现对机械臂1的柔性随动控制，需要解算出手5作用力Fh对六维力传感器2造成的力分量与力矩分量。

如图2所示，六维力传感器2的坐标系有X、Y、Z三个坐标轴，负载重力为G，负载重心在六维力传感器2的坐标系中的坐标为（x，y，z），负载重力G在X、Y、Z轴方向的作用分力分别为Gx、Gy、Gz，负载重力G对X、Y、Z轴的作用力矩分别为MGx、MGy、MGz，根据力与力矩的关系，参照图2易得到：

MGx=Gz×y－Gy×z

MGy=Gx×z－Gz×x

MGz=Gy×x－Gx×y

即：

$\left[\begin{array}{c}{\mathrm{MG}}_x\\ {\mathrm{MG}}_y\\ {\mathrm{MG}}_z\end{array}\right]=\left[\begin{array}{ccc}0& -z& y\\ z& 0& -x\\ -y& x& 0\end{array}\right]\·\left[\begin{array}{c}{G}_x\\ G_y\\ G_z\end{array}\right]$

当负载不受手5作用力Fh及其它外力时，六维力传感器2测得的X、Y、Z轴方向的3个力分量以及3个力矩分量均由负载重力引起。当机械臂1的末端姿态变化后，六维力传感器2的空间姿态也随之变化，而负载重力G的方向始终竖直向下，因此负载重力G在六维力传感器2坐标系中的作用分量Gx、Gy、Gz以及MGx、MGy、MGz也会变化，但Gx、Gy、Gz以及MGx、MGy、MGz始终满足上面所列的方程组。

在负载无外力作用的情况下，控制机械臂1使负载处于3个不同的空间姿态，测得3组六维力传感器2的测量数据，3组力分量分别为（Fx1，Fy1，Fz1）、（Fx2，Fy2，Fz2）、（Fx3，Fy3，Fz3），3组力矩分量分别为（Mx1，My1，Mz1）、（Mx2，My2，Mz2）、（Mx3，My3，Mz3），由上面所列方程组易得出：

$\left[\begin{array}{ccc}{M}_{x1}& M_{x2}& M_{x2}\\ M_{y1}& M_{y2}& M_{y2}\\ M_{z1}& M_{z2}& M_{z3}\end{array}\right]=\left[\begin{array}{ccc}0& -z& y\\ z& 0& -x\\ -y& x& 0\end{array}\right]\·\left[\begin{array}{ccc}{F}_{x1}& F_{x2}& F_{x2}\\ F_{y1}& F_{y2}& F_{y2}\\ F_{z1}& F_{z2}& F_{z3}\end{array}\right]$

容易选择机械臂1末端3个姿态使矩阵 $\left[\begin{array}{ccc}{F}_{x1}& F_{x2}& F_{x2}\\ F_{y1}& F_{y2}& F_{y2}\\ F_{z1}& F_{z2}& F_{z3}\end{array}\right]$ 可逆，进而有：

$\left[\begin{array}{ccc}0& -z& y\\ z& 0& -x\\ -y& x& 0\end{array}\right]=\left[\begin{array}{ccc}{M}_{x1}& M_{x2}& M_{x2}\\ M_{y1}& M_{y2}& M_{y2}\\ M_{z1}& M_{z2}& M_{z3}\end{array}\right]\·{\left[\begin{array}{ccc}{F}_{x1}& F_{x2}& F_{x2}\\ F_{y1}& F_{y2}& F_{y2}\\ F_{z1}& F_{z2}& F_{z3}\end{array}\right]}^{-1}$

这样就得到负载的重心在六维力传感器2坐标系中的坐标（x，y，z）。负载重力G的大小可取任意一组数据由G=(Fxi²+Fyi²+Fzi²)^1/2计算得到。

在负载有手5作用力Fh作用的情况下，经过对机械臂1安放姿态的标定，通过控制系统（图中未画出）易得到负载重力G的方向与六维力传感器2坐标系X、Y、Z轴的夹角（α，β，γ），则可计算得到负载重力在六维力传感器2坐标系3个坐标轴上的投影为：Gx=G×cosα，Gy=G×cosβ，Gz=G×cosγ，再根据已得到的负载重心坐标（x，y，z），由MGx=Gz×y－Gy×z，MGy=Gx×z－Gz×x，MGz=Gy×x－Gx×y，可计算得到负载重力作用的3个力矩分量MGx、MGy、MGz。

在负载有手5作用力Fh作用的情况下，六维力传感器2测得的3个力分量为Fx、Fy、Fz，3个力矩分量为Mx、My、Mz，补偿得到手5作用力Fh在六维力传感器2坐标系中的作用力为（Fx－Gx，Fy－Gy，Fz－Gz），作用力矩为（Mx－MGx，My－MGy，Mz－MGz），重力补偿完成，以此作为输入控制机械臂跟随人手运动。

实施方式2：

本实施方式与实施方式1的不同在于负载重力G及重心坐标（x，y，z）的计算方法。

固定机械臂1的末端在任意姿态，在负载无外力作用的情况下，六维力传感器2测得的力分量为Fx0，Fy0，Fz0，力矩分量为Mx0、My0、Mz0，则：

Mx0=Fz0×y－Fy0×z

My0=Fx0×z－Fz0×x

Mz0=Fy0×x－Fx0×y

调整机械臂1的末端姿态，使六维力传感器2坐标系的Y轴指向重力方向，此时在负载无外力作用的情况下，六维力传感器2测得的力分量为Fx1，Fy1，Fz1，力矩分量为Mx1、My1、Mz1，同样满足：

Mx1=Fz1×y－Fy1×z

My1=Fx1×z－Fz1×x

Mz1=Fy1×x－Fx1×y

由于六维力传感器2坐标系的Y轴指向重力方向，此时Fx1=0，Fy1=G，Fz1=0，My1=0，利用上面列出的Fx1，Fy1，Fz1与Mx1、My1、Mz1的关系方程，易得到：

$x=\frac{M_{z1}}{F_{y1}}$

$z=-\frac{M_{x1}}{F_{y1}}$

再利用上面列出的Fx0，Fy0，Fz0与Mx0、My0、Mz0的关系方程，易得到：

$y=\frac{M_{x0}+F_{y0}\×z}{F_{z0}}$

以及：

$y=\frac{F_{y0}\×x-M_{z0}}{F_{x0}}$

均可以计算出y。

至此，负载重力G及重心坐标（x，y，z）已全部计算出来，其余与实施方式1相同。

根据六维力传感器2的测量数据计算负载重力G及重心坐标（x，y，z）的具体方式有很多，不再一一列举。

尽管上文对本发明的具体实施方式进行了详细的描述和说明，但应该指明的是，我们可以对上述实施方式进行各种改变和修改，但这些都不脱离本发明的精神和所附的权利要求所记载的范围。

Claims (6)

1.一种航天器机械臂的柔性随动控制重力补偿方法，其中柔性随动控制中将六维力传感器设置在机械臂末端与负载之间，该方法包括如下步骤：

1)控制机械臂使负载处于多个不同的空间姿态，在负载无外力作用的情况下，记录每个空间姿态下六维力传感器的测量数据；

2)根据力与力矩的物理关系，由步骤1)的测量数据计算得到负载的重力大小G及负载重心在六维力传感器自身坐标系中的坐标(x，y，z)；

3)在负载有外力作用的情况下，实时根据重力在六维力传感器自身坐标系中的方向，将重力在六维力传感器自身坐标系的三个坐标轴上投影得到三个重力分量Gx、Gy、Gz，分别作为六维力传感器自身坐标系中X、Y、Z轴力分量的重力补偿值；

4)由公式MGx＝Gz×y－Gy×z，MGy＝Gx×z－Gz×x，MGz＝Gy×x－Gx×y，计算得到六维力传感器自身坐标系中X、Y、Z轴上由重力引起的三个力矩分量MGx、MGy、MGz，分别作为六维力传感器自身坐标系中X、Y、Z轴上力矩分量的补偿值；

5)在负载有外力作用的情况下，六维力传感器测得的三个力分量为Fx、Fy、Fz，三个力矩分量为Mx、My、Mz，补偿得到外力在六维力传感器自身坐标系中的作用力为(Fx－Gx，Fy－Gy，Fz－Gz)，作用力矩为(Mx－MGx，My－MGy，Mz－MGz)，重力补偿完成；

其中，在柔性随动控制过程中，机械臂末端姿态会发生变化，六维力传感器自身坐标系相对于大地坐标系的姿态也会随之变化，但负载重力始终竖直向下，负载重力在六维力传感器自身坐标系中的方向也在随之变化，经过对机械臂安放姿态的标定，机械臂控制系统容易实时得到重力方向与六维力传感器自身坐标系X、Y、Z轴的夹角(α，β，γ)，则可计算得到负载重力在六维力传感器自身坐标系3个坐标轴上的投影为：Gx＝G×cosα，Gy＝G×cosβ，Gz＝G×cosγ，Gx、Gy、Gz即是六维力传感器测得的力分量数据Fx、Fy、Fz中由于负载重力产生的部分。

2.如权利要求1所述的柔性随动控制重力补偿方法，其中，所述六维力传感器每次测量得到的数据有六个分量，包括在六维力传感器自身坐标系中的三个坐标轴方向的力分量，以及三个坐标轴方向的力矩分量，所述六维力传感器自身坐标系是与六维力传感器自身固连的空间直角坐标系。

3.如权利要求1所述的柔性随动控制重力补偿方法，其中，所述负载无外力作用指的是负载连接在六维力传感器上，仅受自身重力及与六维力传感器的连接支持力，不受其它外部作用力。

4.如权利要求1所述的柔性随动控制重力补偿方法，其中，假设步骤1)对N个不同的负载姿态测量得到N组六维力传感器数据，第i组的3个力分量为Fxi、Fyi、Fzi，3个力矩分量为Mxi、Myi、Mzi，在负载无外力作用的情况下，测得的力与力矩分量都由负载重力引起，则负载重力大小为：G＝(Fxi²+Fyi²+Fzi²)^1/2；设负载重心在六维力传感器自身坐标系中的坐标为(x，y，z)，在该坐标系下根据力与力矩的关系有：

Mxi＝Fzi×y－Fyi×z

Myi＝Fxi×z－Fzi×x

Mzi＝Fyi×x－Fxi×y

取多个不同的负载姿态测量得到的多组六维力传感器数据，根据上面的方程组，采用最小二乘法，即可求得负载重心在六维力传感器自身坐标系中的坐标(x，y，z)。

5.如权利要求1所述的柔性随动控制重力补偿方法，步骤4)中，计算得到的MGx、MGy、MGz是六维力传感器测得的力矩分量数据Mx、My、Mz中由于负载重力产生的部分。

6.如权利要求1所述的柔性随动控制重力补偿方法，步骤5)中，若在柔性随动控制人手对负载进行操作，(Fx－Gx，Fy－Gy，Fz－Gz)认为是人手作用产生的力的分量，(Mx－MGx，My－MGy，Mz－MGz)认为是人手作用产生的力距的分量，根据(Fx－Gx，Fy－Gy，Fz－Gz)与(Mx－MGx，My－MGy，Mz－MGz)控制机械臂跟随人手运动。