[发明专利]航天器机械臂柔性随动控制重力补偿方法

权利要求书

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），重力补偿完成。

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

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

7.如权利要求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）控制机械臂跟随人手运动。