[发明专利]基于Kinect深度摄像头的工业机械臂实时避障规划抓取系统

权利要求书

1.基于Kinect深度摄像头的工业机械臂实时避障规划抓取系统，其特征在于，包括：

运动学模块，用于机械臂的正逆运动学求解，用户通过输入末端执行器的期望抓取位姿，通过该模块计算得到机械臂各个关节的角度值，用户也能够通过给定机械臂各个关节的角度值来计算末端执行器的位姿；

路径规划模块，用于通过运动学模块求解得到的抓取位姿，采用基于采样的LazyPRM算法，在机械臂状态空间中采样来进行路径规划；

物体抓取模块，用于对空间中物体的抓取，用户通过5DT数据手套来控制五指灵巧手的物体抓取作业，所述5DT数据手套能够通过传感器来采集用户手指关节的弯曲角度，通过关节映射来驱动五指灵巧手的电机使得五指灵巧手运动到抓取姿态；

动态环境检测模块，用于对空间中动态环境进行跟踪检测，能够对空间中的运动障碍物进行检测和追踪，进一步反馈到路径规划模块中，为后续的路径规划提供动态障碍物的空间位置；

仿真模块，用于对机械臂进行实时仿真，用户能够通过人机交互界面对机械臂进行运动控制；

其中，在所述运动学模块中，机械臂正逆运动学采用D-H矩阵进行描述，其逆运动学求解同时采用了几何方法和代数方法：

第一个关节角通过从基坐标原点到第四个关节坐标原点的投影向量来求解：

θ₁＝arctan(p_y/p_x)

或θ₁＝arctan(p_y/p_x)+π

其中p_x和p_y分别为投影向量在基坐标系的x,y轴上的分量；

第二个关节角度通过前五个关节的几何关系求出：

或

其中β₁为从第二个关节坐标原点到第五个关节坐标原点形成的向量与第二个关节坐标系的y轴形成的夹角，β₂为第二个关节坐标原点与第三个关节坐标原点、第五个关节坐标原点的连线形成的夹角；

第三个关节角度通过前五个关节的几何关系求出：

θ₃＝π-φ-α或θ₃＝π+φ-α

其中α为第三连杆与第三关节坐标原点到第五关节坐标原点的连线形成的夹角，φ为第三个关节坐标原点与第二个关节坐标原点、第五个关节坐标原点的连线形成的夹角；

第四个关节角通过代数方法求出：

θ₄＝a tan2(c₁c₂-s₁a_x-c₁c₂₃a_x,-s₁c₂₃a_y-s₂₃a_z)

其中c₁、c₂、c₂₃分别代表cosθ₁、cosθ₂、cos(θ₂+θ₃)；s₁、s₂₃分别代表sinθ₁、sin(θ₂+θ₃)；a_x、a_y、a_z分别代表正运动学基坐标到末端执行器坐标原点变换矩阵的第三列分量；

第五个关节角通过向量点积法求出：

θ₅＝arccos(N₆N₃)

其中N₆、N₃代表第六、第三关节轴的方向向量；

第六个关节角同样通过代数法求解：

θ₆＝a tan2(c₁s₂₃o_x+s₁s₂₃o_y+c₂₃o_z,-c₁s₂₃n_x-s₁s₂₃n_y-c₂₃n_z)

其中o和n分别代表基坐标到末端执行器坐标原点变换矩阵的第一、第二列列向量，其下标x,y,z表示列向量的分量。