[发明专利]一种超冗余机械臂的路径规划方法、装置及控制系统

权利要求书

1.一种超冗余机械臂的路径规划方法，所述超冗余机械臂包括多个臂段，多个所述臂段均包括多个小节，多个所述小节间依次通过关节连接，其特征在于，包括步骤：

S10、获取所述臂段的臂形描述参数并规划所述超冗余机械臂的期望路径，在所述期望路径上取多个期望节点，且分别设有一个所述期望节点处于所述期望路径的首端和末端，并以第一个所述期望节点为目标期望节点；

S20、根据所述臂形描述参数获得所述机械臂的当前等效臂形参数；

S30、获取所述目标期望节点的目标期望位置信息和目标期望指向信息，并根据所述当前等效臂形参数递推获得所述超冗余机械臂的等效臂形的末端当前位置信息、末端当前指向信息和雅可比矩阵；

S40、根据所述末端当前位置信息、所述末端当前指向信息、所述雅可比矩阵、所述目标期望位置信息和所述目标期望指向信息获得末端期望线速度和末端期望角速度；

S50、基于所述雅可比矩阵不断迭代所述末端期望线速度和所述末端期望角速度获得各个所述臂段的期望臂形参数；

S60、根据所述期望臂形参数获得关节驱动参数，并根据所述关节驱动参数将所述超冗余机械臂向所述目标期望节点驱动；

S70、以下一个所述期望节点为所述目标期望节点，重复所述步骤S20至所述步骤S60直至遍历完所有所述期望节点；

所述步骤S50之后所述步骤S60之前还包括步骤：

S510、根据所述期望臂形参数获得末端期望位置信息和末端期望指向信息；

S511、设定误差范围要求，当所述末端期望位置信息与所述目标期望位置信息的误差满足所述误差范围要求，且当所述末端期望指向信息与所述目标期望指向信息的误差满足所述误差范围要求时，执行所述步骤S60，否则返回执行所述步骤S20。

2.根据权利要求1所述的超冗余机械臂的路径规划方法，其特征在于，所述步骤S40具体包括子步骤：

S401、根据所述目标期望位置信息、所述目标期望指向信息、所述末端当前位置信息和所述末端当前指向信息获得位置信息差和指向信息差；

S402、根据所述位置信息差、所述指向信息差及所述雅可比矩阵计算得出所述末端期望线速度和所述末端期望角速度。

3.根据权利要求1或2任一项所述的超冗余机械臂的路径规划方法，其特征在于，分别对多个所述臂段内的多个所述关节按顺序进行标号，且将所述标号为奇数的所述关节设置为Pitch-Yaw型，所述标号为偶数的所述关节设置为Yaw-Pitch型。

4.一种超冗余机械臂的路径规划装置，所述超冗余机械臂包括多个臂段，多个所述臂段均包括多个小节，多个所述小节两两之间通过关节连接，多个所述臂段内的所述关节按顺序进行标号，且将所述标号为奇数的所述关节设置为Yaw-Pitch型，所述标号为偶数的所述关节设置为Pitch-Yaw型，其特征在于，包括：

臂形描述参数获取及期望路径规划模块，用于获取所述臂段的臂形描述参数并规划所述超冗余机械臂的期望路径，在所述期望路径上取多个期望节点，且分别设有一个所述期望节点处于所述期望路径的首端和末端，并以第一个所述期望节点为目标期望节点；

当前等效臂形参数获取模块，用于根据所述臂形描述参数获得所述机械臂的当前等效臂形参数；

参数信息计算获取模块，用于获取所述目标期望节点的目标期望位置信息和目标期望指向信息，并根据所述前等效臂形参数推获得所述超冗余机械臂的等效臂形的末端当前位置信息、末端当前指向信息和雅可比矩阵；

末端期望速度获取模块，用于根据所述末端当前位置信息、所述末端当前指向信息、所述雅可比矩阵、所述目标期望位置信息和所述目标期望指向信息获得末端期望线速度和末端期望角速度；

期望臂形参数获取模块，用于基于所述雅可比矩阵不断迭代所述末端期望线速度速度和所述末端期望角速度获得各个所述臂段的期望臂形参数；

驱动控制模块，根据所述期望臂形参数获得关节驱动参数，并根据所述关节驱动参数将所述超冗余机械臂向所述目标期望节点驱动；

期望节点遍历模块，用于以下一个所述期望节点为所述目标期望节点，重复所述当前等效臂形参数获取模块、所述参数信息计算获取模块、末端期望速度获取模块、所述期望臂形参数获取模块和所述驱动控制模块的工作直至遍历完所有所述期望节点。