[发明专利]一种多冗余度机械臂系统的协同控制方法与装置

权利要求书

1.一种多冗余度机械臂系统的协同控制方法，其特征在于，所述多冗余度机械臂系统包括两个以上的冗余度机械臂，所述两个以上的冗余度机械臂在通信拓扑图中连通；

所述控制方法包括：

确定所述两个以上的冗余度机械臂中至少一个冗余度机械臂为目标冗余度机械臂；

获取所述目标冗余度机械臂的相邻冗余度机械臂的位置信息，所述相邻冗余度机械臂与所述目标冗余度机械臂在通信拓扑图中相邻连接；

获取所述目标冗余度机械臂的参考点轨迹信息，所述参考点轨迹信息由预设的目标参考点和所述目标参考点的期望轨迹确定；

根据所述位置信息和所述参考点轨迹信息按照预设的规则构建所述目标冗余度机械臂对应的雅可比矩阵等式；

在所述雅可比矩阵等式、障碍物躲避不等式、关节角度极限和关节速度极限的约束下，根据二次型优化和标准二次规划方法确定所述目标冗余度机械臂的控制信号；

根据所述控制信号控制所述目标冗余度机械臂，使得所述目标冗余度机械臂的可操作度最大化。

2.根据权利要求1所述的协同控制方法，其特征在于，所述根据所述位置信息和所述参考点轨迹信息按照预设的规则构建所述目标冗余度机械臂对应的雅可比矩阵等式具体包括：

构建所述目标冗余度机械臂对应的权重矩阵，所述权重矩阵中的各个元素为所述目标冗余度机械臂与所述两个以上的冗余度机械臂中的各个机械臂之间分别对应的第一连通权值；

根据所述参考点轨迹信息的获取状态确定所述目标冗余度机械臂的第二连通权值；

根据所述权重矩阵、所述第二连通权值、所述位置信息和所述参考点轨迹信息构建所述目标冗余度机械臂对应的雅可比矩阵等式。

3.根据权利要求2所述的协同控制方法，其特征在于，所述第一连通权值通过以下步骤确定：

将所述目标冗余度机械臂与所述目标冗余度机械臂自身之间的第一连通权值设置为1；

将所述目标冗余度机械臂与相邻冗余度机械臂之间的第一连通权值设置为1；

将所述目标冗余度机械臂与所述两个以上的冗余度机械臂中的非自身且非相邻冗余度机械臂之间的第一连通权值设置为0；

所述根据所述参考点轨迹信息的获取状态确定所述目标冗余度机械臂的第二连通权值具体包括：

判断所述目标冗余度机械臂是否获取到所述参考点轨迹信息；

若所述目标冗余度机械臂获取到所述参考点轨迹信息，则将所述第二连通权值设置为1；

若所述目标冗余度机械臂未获取到所述参考点轨迹信息，则将所述第二连通权值设置为0；

所述雅可比矩阵等式的通用表达式为：

其中，J_i表示目标冗余度机械臂i的雅可比矩阵；表示目标冗余度机械臂i的关节速度向量；表示与目标冗余度机械臂i之间的第一连通权值为1的机械臂的编号；A_ij表示目标冗余度机械臂i的权重矩阵，所述权重矩阵中的各个元素为目标冗余度机械臂i与冗余度机械臂j之间的第一连通权值；ρ_i表示目标冗余度机械臂i的第二连通权值；δ_i＝r_i-r_irp为所述位置信息，r_i为目标冗余度机械臂i的末端执行器位置，r_irp为预设的目标冗余度机械臂i的末端执行器相对于所述目标参考点的距离向量；δ_j＝r_j-r_jrp；r_d为所述目标参考点的期望轨迹；c₀＞0为控制算法收敛速度的参数，其越大则代表收敛速度越快。

4.根据权利要求1至3中任一项所述的协同控制方法，其特征在于，所述在所述雅可比矩阵等式、障碍物躲避不等式、关节角度极限和关节速度极限的约束下，根据二次型优化和标准二次规划方法确定所述目标冗余度机械臂的控制信号具体包括：

设置性能指标为可操作度最大化，在所述雅可比矩阵等式、障碍物躲避不等式、关节角度极限和关节速度极限的约束下确定二次型优化结果；

将所述二次型优化结果转换为标准二次规划；

对所述标准二次规划进行求解，得到求解结果；

根据所述求解结果确定所述目标冗余度机械臂的控制信号。