[发明专利]一种旋翼飞行双机械臂目标定位抓取系统及方法

权利要求书

1.一种旋翼飞行双机械臂目标定位抓取方法，其特征在于，所述旋翼飞行双机械臂目标定位抓取方法包括：

通过视觉传感器实时获取环境视频流信息，并采用MobileNet-SSD模型训练识别抓取目标；

将视觉传感器和AprilTag视觉定位切换使用，获取识别抓取目标的三维坐标信息；

引入FCL碰撞，采用RRT_Connect路径规划算法进行机械臂的逆向运动学运算，进行空间无碰撞双臂规划与抓取；

所述旋翼飞行双机械臂目标定位抓取方法具体包括：

步骤1：通过视觉传感器实时获取环境视频流信息，采用MobileNet-SSD模型识别目标，锁定待抓取目标物；

步骤2：根据步骤1锁定抓取目标后，采用视觉传感器获取目标的点云3D数据和AprilTag视觉定位切换的分段方法,获取目标的三维坐标；

步骤3：根据步骤2中的三维信息，引入FCL碰撞检测机制，根据末端位姿信息进行机械臂逆向运动学运算，采用RRT_Connect路径规划算法得到双臂运动轨迹，实现协同规划抓取；

步骤2中获取目标3D坐标，先采用视觉传感器获取目标3D信息，当飞行器运动至接近目标物时切换至AprilTag视觉定位获取3D信息，其距离阈值为1米；

所采用视觉传感器和AprilTag视觉定位切换分段的具体步骤包括：

(1)视觉传感器实时获取视频流，通过步骤1方法确定目标并给定目标框；

(2)通过视觉传感器获取目标RGB图像，获取目标像素坐标(x,y)，得到深度图像，IR图像，得到目标深度值Z；

(3)基于棋盘格，采用张正友标定法对视觉传感器进行标定，获取内参矩阵C和外参矩阵T；

(4)基于相机参数，根据相机坐标系和图像坐标系之间的转换关系得到图像中目标相对于相机坐标系的三维坐标，坐标转换关系为：

式中，(x,y)表示图像像素坐标系；(X,Y,Z)表示相机坐标系；C表示相机内参矩阵；T表示相机外部参数矩阵；

(5)视觉传感器获取的三维信息传递给飞行器，旋翼飞行器不断飞行运动至接近目标，设置循环，判断距离是否满足Z≤1米，若否，则继续使用该视觉传感器获取3D坐标；若是，则转至第(6)步采用AprilTag视觉定位方法获取目标更精确3D坐标；

(6)通过视觉传感器，获取图像特征，计算像素梯度得到幅值图像，得到梯度方向；

(7)将相似的梯度方向和幅值集群到一个组件，即每个节点代表一个像素，使用加权最小二乘法，提取场景中的直线，将方形区域同态映射为正方形，与Tag库进行匹配，取0-15号Tag标签作为对象，大小都取为10cm和15cm两种，调用AprilTag视觉库得到目标精确3D坐标；

(8)将获取的3D坐标实时传送给飞行控制器和机载计算机。

2.如权利要求1所述的旋翼飞行双机械臂目标定位抓取方法，其特征在于，步骤1中MobileNet-SSD模型实现待抓取目标检测，步骤如下：

收集待抓取物体的数据集，并对数据集进行初始化处理，标注数据；

划分训练集和测试集，制作LMDB文件；

通过MobileNet-SSD模型，输出训练文件；

通过视觉传感器实时获取环境视频流信息，在机载计算机上运行MobileNet-SSD模型识别并锁定待抓取目标。

3.如权利要求1所述的旋翼飞行双机械臂目标定位抓取方法，其特征在于，步骤3中引入FCL碰撞检测机制，根据末端位姿信息进行机械臂逆向运动学运算，采用RRT_Connect路径规划算法得到双臂运动轨迹，实现协同规划抓取的实现步骤包括：

1)创建机械臂规划群，建立4自由度机械臂的部位连接关系，建立关节空间笛卡尔坐标系；

2)创建机器人目标位姿，将获取的3D坐标即机械臂末端位姿信息，作为机械臂系统的输入；

3)把末端位姿作为输入，采用RRT_Connect算法采样输出关节空间集合；

4)引入FCL碰撞检测机制，结合输出关节空间集合，得到一条不与环境中任何障碍碰撞的最优轨迹。

4.一种终端，其特征在于，所述终端至少搭载实现权利要求1～3任意一项所述旋翼飞行双机械臂目标定位抓取方法的控制器。