[发明专利]一种基于运动基元库的带电作业机械臂自主避障路径规划方法

摘要

本发明提出一种基于运动基元库的带电作业机械臂自主避障路径搜索方法。建立机械臂完整运动学模型、机械臂简化运动学模型并构建机械臂工作环境；分类设计运动基元库，为各类库设计相应运动基元作为库成员；分类建立机械臂与目标点、障碍物的距离计算函数，设置危险区域阈值和到达目标点精度阈值；创建笛卡尔空间中的启发函数h_d(x)和关节空间中的启发函数h_c(x)以及启发规则，实现启发搜索；计算机械臂当前位姿与期望位姿及障碍物的笛卡尔空间距离，根据设定的判断条件进行路径规划。本发明能快速、精确构建机械臂作业场景信息，保证避障路径搜索结果具有次优性和完备性，满足动力学约束前提下能快速完成路径规划，保证机械臂运动过程的平稳性。

主权项

1.一种基于运动基元库的带电作业机械臂自主避障路径规划方法，其特征在于，步骤如下：步骤一：建立机械臂完整运动学模型、机械臂简化运动学模型并构建机械臂工作环境；步骤二：分类设计运动基元库，为各类库设计相应运动基元作为库成员；运动基元库包括单关节空间轨迹规划库SP_LIB、自适应逆运动学轨迹规划库AP_LIB以及笛卡尔空间轨迹规划库CP_LIB；单关节空间轨迹规划库SP_LIB以单关节空间轨迹规划结果作为库的运动基元，用于构建降维稀疏的C空间；当机械臂接近目标位姿时，使用自适应逆运动学规划库AP_LIB自适应地建立精确到达指定位姿的轨迹；笛卡尔空间轨迹规划库CP_LIB用于快速脱离障碍物区域；步骤三：初始化机械臂工作环境参数；分类建立机械臂与目标点、障碍物的距离计算函数，设置危险区域阈值和到达目标点精度阈值；创建笛卡尔空间中的启发函数h_d(x)和关节空间中的启发函数h_c(x)以及启发规则，实现启发搜索；初始化机械臂工作环境参数时，首先初始化世界坐标系W₀，建立机械臂末端与世界坐标系的变换矩阵T_W初始化机械臂当前关节空间状态S₀；然后根据三维点云功能模块和图像处理模块的计算结果，选择步骤一中建立的相匹配的机器人作业场景模型进行匹配操作，匹配成功后利用双目相机重复采样模型与环境匹配特征点的位置坐标，利用最小二乘法拟合变换矩阵，获取各障碍物基准点相对w₀位姿坐标O_W，若匹配失败则利用点云相机和双目相机数据进行实时三维场景重建；距离计算函数包括建立机械臂末端与障碍物的笛卡尔空间距离检测函数D_O(q)、机械臂末端与目标点笛卡尔空间距离检测函数D1_F(q)和关节空间距离检测函数D2_F(q)；危险区域阈值包括笛卡尔空间距离末端区域触发阈值K1_F、关节空间距离末端区域触发阈值K2_F、笛卡尔空间距离障碍物区域触发阈值K_O以及笛卡尔空间距离紧急避障区域阈值K_M；目标点精度范围阈值包括笛卡尔空间阈值A_F1和关节空间阈值A_F2；步骤四：计算机械臂当前位姿与期望位姿及障碍物的笛卡尔空间距离，根据设定的判断条件选择执行下一步；调用机械臂末端与目标点笛卡尔空间距离检测函数D1_F(q)、关节空间距离检测函数D2_F(q)和机械臂末端与障碍物的笛卡尔空间距离检测函数D_O(q)函数，将函数计算结果与区域触发阈值K1_F、K2_F及K_O进行比较，根据判断条件选择执行以下步骤：条件1：若D1_F(q)＞K1_F或D2_F(q)＞K2_F，且D_O(q)＞K_O，则顺序执行步骤五；条件2：若D1_F(q)＞K1_F或D2_F(q)＞K2_F，且D_O(q)＜K_O，则跳转至步骤六；条件3：若D1_F(q)＜A_F1或D2_F(q)＜A_F2，则判断机械臂末端到达期望位姿态，获得搜索结果跳转至步骤八；条件4：其他情况跳转至步骤七；步骤五：在降维稀疏的C空间中使用单关节空间轨迹规划库SP_LIB执行启发式搜索，获得当前库中最优运动基元作为规划结果；步骤六：在非降维稀疏C空间中使用单关节空间轨迹规划库SP_LIB库或笛卡尔空间轨迹规划库CP_LIB执行启发式搜索，获得当前库中最优运动基元作为规划结果；步骤七：规划精确到达目标点轨迹，获得各个关节精确达到目标关节角的单元运动规划结果；步骤八：跳出计算过程，保存路径搜索结果，清理缓存和状态参数，等待指令执行下一次路径规划任务。