一种超冗余机械臂分段式动态避障方法.pdf

(19)国家知识产权局(12)发明专利申请(10)申请公布号CN115990874A(43)申请公布日2023.04.21(21)申请号202211381036.X(22)申请日2022.11.05(71)申请人北京化工大学地址100029北京市朝阳区北三环东路15号(72)发明人曹政才居仁杰张东(74)专利代理机构北京思海天达知识产权代理有限公司11203专利代理师刘萍(51)Int.Cl.B25J9/16(2006.01)权利要求书3页说明书6页附图5页(54)发明名称一种超冗余机械臂分段式动态避障方法(57)摘要本发明公开了一种超冗余机械臂分段式动态避障方法。首先，针对场景中的机械臂状态信息与观测到的障碍物信息，建立以机械臂为中心的动态障碍预警区域。其次，当障碍物进入该区域监测范围内，基于速度障碍策略构建碰撞风险评估模型，实时评估机械臂与障碍物间的碰撞可能性。再次，若存在碰撞风险，依据障碍物信息将机械臂划分为若干避障区域，构建局部避障速度约束、关节限位约束以及连杆长度约束，求解机械臂各关节安全速度集合。最后，通过前后向迭代逆运动学求解算法实时更新机械臂位姿，进而实现动态障碍场景下的超冗余机械臂分段式避障。本发明充分发挥超冗余机械臂的灵活特性，使其在动态障碍场景下具有更好的适应能力。CN115990874ACN115990874A权利要求书1/3页1.一种超冗余机械臂分段式动态避障方法，其特征在于，包括：步骤1：针对场景中的机械臂状态信息与观测到的障碍物信息，建立以机械臂为中心的动态障碍预警区域；步骤2：当障碍物进入该区域监测范围内，基于速度障碍策略构建碰撞风险评估模型，实时评估机械臂与障碍物间的碰撞可能性；步骤3：若存在碰撞风险，依据障碍物信息将机械臂划分为若干避障区域，构建局部避障约束、关节限位约束以及连杆长度约束，求解机械臂各关节安全速度集合；步骤4：通过前后向迭代逆运动学求解算法实时更新机械臂位姿，进而实现动态障碍场景下的超冗余机械臂分段式避障。2.根据权利要求1所述的一种超冗余机械臂分段式动态避障方法，其特征在于：在步骤1中，具体包含以下步骤：步骤1.1：从三维捕捉系统与机械臂关节内部陀螺仪获取机械臂的状态信息：几何构型、机械臂臂型姿态、第i节机械臂连杆速度vLi，与障碍物信息：几何构型、障碍物当前速度vo，其中，i＝1,2,…,n，n表示超冗余机械臂连杆数量；步骤1.2：利用封闭式椭球体将机械臂各连杆进行包覆，记包覆第i节机械臂连杆的封闭式椭球体为根据障碍物几何构型利用封闭式凸几何体对障碍物进行包覆，记包覆障碍物的封闭式凸几何体为步骤1.3：将连杆进行简化，以障碍物质心为中心构建各连杆与障碍物间的封闭式闵可夫斯基和，记为：式中，用表示闵可夫斯基和运算，表示取区域内任意一点x，表示取区域内任意一点y；步骤1.4：计算封闭式闵可夫斯基和到对应连杆中心的最小欧式距离di，记机械臂连杆长度为La，以该连杆为中心设定动态障碍预警距离为dwarn＝n*La，当di<dwarn时，视为该障碍物存在潜在风险。3.根据权利要求1所述的一种超冗余机械臂分段式动态避障方法，其特征在于：在步骤2中，具体包含以下步骤：步骤2.1：将封闭式闵可夫斯基和沿基座标系XOY与XOZ平面进行投影为与记第i节连杆空间坐标为PLi，第i节连杆与障碍物的相对速度为vrel＝vLi‑vo，将PLi与xoyxozxoyxozvrel分别沿基座标系XOY与XOZ平面进行投影为PLi、PLi与vrel、vrel；xoy步骤2.2：在XOY平面内，以PLi为观测中心，分别向两侧作切线，构成障碍锥区xoyxoz域，记为D，在XOZ平面内，以PLi为观测中心，分别向两侧作切线，构成障碍锥区域，记为xozD，则速度障碍区域与表示为：式中，τ为时间窗口量；2CN115990874A权利要求书2/3页xoyxoz步骤2.3：当相对速度矢量vrel落在区域内部或vrel落在区域内部时，视为与在[0,τ]时间范围内存在碰撞风险，进而构建碰撞风险评估模型为：式中，∩表示取交集，∪表示取并集。4.根据权利要求1所述的一种超冗余机械臂分段式动态避障方法，其特征在于：在步骤3中，具体包含以下步骤：步骤3.1，当存在碰撞风险时，依据到对应连杆中心的最小距离di为超冗余机械臂动态分配避碰关节组；步骤3.2，记超冗余机械臂第i关节位置为PJi，构建连杆长度约束为：||PJi‑PJi‑1||2＝La；式中，||…||2表示向量的二范数计算；步骤3.3，记超冗余机械臂关节限位角为θmax，则以Pji为顶点，La为母线，矢量PJi‑PJi‑1为轴线，2θmax为锥角构建球锥区域Djoint，构建关节限位约束为：PJi+1∈Djoint；步骤3.4，记ui为vrel到速度障碍边界最小距离的矢量，记