[发明专利]一种气浮台抓捕试验机械臂控制方法

权利要求书

1.一种气浮台抓捕试验机械臂控制方法，所述机械臂为两个多自由度机械臂，其特征在于，步骤如下：

(1)根据相对测量敏感器输出的相对距离确定是否进行预抓捕，当追踪气浮台与目标气浮台之间的相对距离小于预抓捕设定距离时，两个机械臂进行预抓捕，将机械臂伸展到目标气浮台大包络范围；

(2)采集相对测量敏感器输出的相对距离和相对姿态信息，如果相对距离和相对姿态在机械臂抓捕有效作用范围内，则根据相对测量敏感器输出的相对距离和相对姿态信息计算每个机械臂目标特征抓捕部位坐标；根据每个机械臂目标特征抓捕部位坐标和运动学逆解算法解算每个机械臂各关节目标转角；根据各关节目标转角与当前关节转角确定各个关节期望运动轨迹；然后开始进行第一时间间隔的计时；

(3)对于每个机械臂，通过机械臂关节位置和速度闭环控制各关节沿期望运动轨迹运动；

(4)对于每个机械臂，根据安装在机械臂作用末端的触碰传感器输出的信号判断机械臂是否与目标气浮台接触，如果机械臂接触目标气浮台，则转入步骤(5)，否则判断是否到达设定的第一时间间隔，如果到达第一时间间隔则转入步骤(2)，如果没有到达第一时间间隔则转入步骤(3)；

(5)根据机械臂作用末端安装的压力传感器输出的信号进行力闭环控制，使得机械臂目标特征抓捕部位与目标气浮台的接触力恒定。

2.根据权利要求1所述的控制方法，其特征在于，所述多自由度机械臂为三自由度机械臂，该三自由度机械臂包括三个连杆和三个关节，其中第一连杆、第二连杆的长度为l₁，l₂；第三个连杆长度为0；所述步骤(2)中的运动学逆解算法如下：

其中，θ₁，θ₂，θ₃分别为第1关节、第2关节、和第3关节目标转角；(p_x，p_y)为机械臂目标特征抓捕部位坐标。

3.根据权利要求1所述的控制方法，其特征在于，所述步骤(3)的力闭环控制方法如下：

首先由设定的抓捕末端与目标气浮台期望压力与力传感器输出压力值做差，通过两次积分及运动学逆解转换得到期望的关节转角；

然后由期望的关节转角与码盘测角装置获得的实际关节转角做差，并通过第一PID控制器滤波，得到期望的关节角速度；

最后对码盘测角装置获得的实际关节转角进行差分获得实际关节角速度，对期望关节角速度与实际关节角速度做差，通过第二PID控制器调整得到机械臂各关节电机的驱动指令；最后通过各关节电机驱动机械臂运动。