[发明专利]一种超冗余机械臂分段式动态避障方法

权利要求书

1.一种超冗余机械臂分段式动态避障方法，其特征在于，包括：

步骤1：针对场景中的机械臂状态信息与观测到的障碍物信息，建立以机械臂为中心的动态障碍预警区域；

步骤2：当障碍物进入该区域监测范围内，基于速度障碍策略构建碰撞风险评估模型，实时评估机械臂与障碍物间的碰撞可能性；

步骤3：若存在碰撞风险，依据障碍物信息将机械臂划分为若干避障区域，构建局部避障约束、关节限位约束以及连杆长度约束，求解机械臂各关节安全速度集合；

步骤4：通过前后向迭代逆运动学求解算法实时更新机械臂位姿，进而实现动态障碍场景下的超冗余机械臂分段式避障。

2.根据权利要求1所述的一种超冗余机械臂分段式动态避障方法，其特征在于：

在步骤1中，具体包含以下步骤：

步骤1.1：从三维捕捉系统与机械臂关节内部陀螺仪获取机械臂的状态信息：几何构型、机械臂臂型姿态、第i节机械臂连杆速度v_Li，与障碍物信息：几何构型、障碍物当前速度v_o，其中，i＝1,2,…,n，n表示超冗余机械臂连杆数量；

步骤1.2：利用封闭式椭球体将机械臂各连杆进行包覆，记包覆第i节机械臂连杆的封闭式椭球体为根据障碍物几何构型利用封闭式凸几何体对障碍物进行包覆，记包覆障碍物的封闭式凸几何体为

步骤1.3：将连杆进行简化，以障碍物质心为中心构建各连杆与障碍物间的封闭式闵可夫斯基和，记为：

式中，用表示闵可夫斯基和运算，表示取区域内任意一点x，表示取区域内任意一点y；

步骤1.4：计算封闭式闵可夫斯基和到对应连杆中心的最小欧式距离d_i，记机械臂连杆长度为L_a，以该连杆为中心设定动态障碍预警距离为d_warn＝n*L_a，当d_id_warn时，视为该障碍物存在潜在风险。

3.根据权利要求1所述的一种超冗余机械臂分段式动态避障方法，其特征在于：

在步骤2中，具体包含以下步骤：

步骤2.1：将封闭式闵可夫斯基和沿基座标系XOY与XOZ平面进行投影为与记第i节连杆空间坐标为P_Li，第i节连杆与障碍物的相对速度为v_rel＝v_Li-v_o，将P_Li与v_rel分别沿基座标系XOY与XOZ平面进行投影为^xoyP_Li、^xozP_Li与^xoyv_rel、^xozv_rel；

步骤2.2：在XOY平面内，以^xoyP_Li为观测中心，分别向两侧作切线，构成障碍锥区域，记为^xoyD，在XOZ平面内，以^xozP_Li为观测中心，分别向两侧作切线，构成障碍锥区域，记为^xozD，则速度障碍区域与表示为：

式中，τ为时间窗口量；

步骤2.3：当相对速度矢量^xoyv_rel落在区域内部或^xozv_rel落在区域内部时，视为与在[0,τ]时间范围内存在碰撞风险，进而构建碰撞风险评估模型为：

式中，∩表示取交集，∪表示取并集。