[发明专利]一种冗余自由度机械臂视觉伺服避障系统

权利要求书

1.一种冗余自由度机械臂视觉伺服避障系统，其特征在于，包括视觉伺服模块、信息处理模块、机械臂控制模块、驱动模块，以及三维实时仿真模块；

其中，所述驱动模块用于实现所述机械臂控制模块与待控制的冗余机械臂之间的通信，根据所述机械臂控制模块输出的控制指令驱动所述冗余机械臂运作，同时将所述冗余机械臂各关节的状态信息实时传输给所述信息处理模块；

所述视觉伺服模块用于采集冗余机械臂工作空间的环境信息，识别出其中的目标体和障碍物信息，并将目标体和障碍物信息传输给所述信息处理模块；

所述信息处理模块用于根据目标体和障碍物信息，构建障碍物的三维实体模型，同时获取目标体的位姿信息；并将障碍物的三维实体模型和冗余机械臂各关节的状态信息传输给所述三维实时仿真模块显示；同时将障碍物的三维实体模型和目标体的位姿信息传输给所述机械臂控制模块；

所述机械臂控制模块用于根据来自所述三维实时仿真模块的用户的操作指令，结合障碍物的三维实体模型和目标体的位姿信息，构建成本函数，将工作空间的目标转换为关节空间的控制指令，发送给所述驱动模块；

其中，所述机械臂控制模块包括避障成本计算单元、成本函数构造单元、规划与执行同步单元；所述避障成本计算单元用于采用人工势场的方法，基于所述信息处理模块构建的障碍物的三维实体模型信息，建立斥力场，得到所述冗余机械臂的成本函数中的避障成本函数因子；所述避障成本函数因子随着机械臂与障碍物距离的增加而减弱；

所述成本函数构造单元用于对避障成本函数因子、约束成本函数因子、扭矩成本函数因子赋予预设的权重，得到所述冗余机械臂的成本函数，包括：

假设机械臂末端要经过的任意一点为P_i，所构建的障碍物的三维模型的点O_j，j＝1,2,…n，n为障碍物的三维模型的点，机械臂末端在位置i的人工势场U_art(p)表示为目标势场U_G(p)和障碍物势场U_O(p)的和，得出所述冗余机械臂的避障成本函数为：

其中，目标势场U_G(p)和障碍物势场函数U_O(p)如下：

其中：k,η＞0为常数；ρ是机械臂到障碍物的最短距离，ρ₀为一阈值；

将避障成本函数q_o，约束成本函数q_c和扭矩成本函数q_t组成：

q(θ)＝∑q_o(θ_t)+q_c(θ_t)+q_t(θ_t)

其中，θ_t为关节状态，所述关节状态包括关节的位置、关节的速度以及关节的加速度，约束成本函数为：

其中，C表示所有的约束集合，v_c用来计算约束被违背所带来的成本的大小；扭矩成本为：

其中，τ_t为时刻t的扭矩；

所述规划与执行同步单元用于在收到目标体的期望位姿之后，基于所述成本函数构造单元构造的成本函数q(θ)，进行逆运动学求解，规划出存在的、可达到的最优路径，并将规划空间转换到关节空间，将关节空间的位置、速度、加速度值做平滑处理，分步向目标体运动靠近；

所述视觉伺服模块包括单目相机和双目相机；其中，

所述单目相机的安装位置相对于所述冗余机械臂的基座位置固定；所述双目相机安装在所述冗余机械臂的末端，随着所述冗余机械臂运作，所述双目相机的位置相对于所述冗余机械臂的基座位置时刻变化；

所述单目相机用于对所述冗余机械臂的整个工作环境进行拍摄得到所述冗余机械臂的工作空间的全景信息；所述双目相机用于采集视野内的图像信息，获取目标体和障碍物的深度信息及RGB图像信息；

所述视觉伺服模块还包括图像处理单元和图像显示单元；其中，

所述图像处理单元用于将所述单目相机拍摄到的工作空间的全景信息传输给所述图像显示单元显示；同时对所述双目相机采集到的深度信息和RGB图像信息进行降噪和去锯齿化操作，得到处理后的深度信息及RGB图像信息，并将处理后的深度信息及RGB图像信息传输给所述信息处理模块；

所述信息处理模块包括深度信息处理单元、目标体位姿信息获取单元、坐标转换单元和三维模型构建单元；其中，

所述深度信息处理单元用于将所述图像处理单元输出的深度信息转换为点云，并将转换的点云传输给所述三维模型构建单元；

所述三维模型构建单元用于根据所述深度信息处理单元输出的点云，采用贪婪三角投影算法构建出障碍物的三维实体模型信息，并将障碍物的三维实体模型信息传输到所述三维实时仿真模块和所述机械臂控制模块；

所述目标体位姿信息获取单元用于根据所述深度信息处理单元输出的点云，获取目标体以所述双目相机为基坐标的三维位置信息；根据所述图像处理单元输出的RGB图像信息，结合预设的目标体的正面图片，根据当前的目标体姿态推断出目标体以所述双目相机为基坐标的三维姿态；

所述坐标转换单元用于计算出障碍物的中心，将其作为障碍物原点，结合所述冗余机械臂的DH参数，根据目标体、双目相机、障碍物的位姿信息，完成相互以及相对于世界坐标系间的位姿转换关系，获取目标体和障碍物相对于世界坐标系的位姿信息，并将其输入到机械臂控制模块和三维实时仿真模块。