[发明专利]一种超冗余联动机械臂的运动轨迹控制方法、装置及系统

权利要求书

1.一种超冗余联动机械臂的运动轨迹控制方法，所述超冗余联动机械臂的结构信息包括：具有第一臂段、第二臂段、第三臂段、第四臂段和第五臂段，每个所述臂段依次连接，每个所述臂段均包括数量相同的多个小节，同一个所述臂段中的所述多个小节具有相等的长度，所述多个小节依次通过关节连接，分别对每个所述臂段内的所述关节按顺序进行标号，且将所述标号为奇数的所述关节设置为Pitch-Yaw型，所述标号为偶数的所述关节设置为Yaw-Pitch型，其特征在于，包括：

根据所述结构信息建立空间双圆弧约束方程；

根据所述空间双圆弧约束方程建立机械臂第一约束方程，所述机械臂第一约束方程用于表示所述第四臂段和所述第五臂段构成的双圆弧曲线；

根据所述空间双圆弧约束方程建立机械臂第二约束方程，所述机械臂第二约束方程用于表示所述第一臂段和所述第二臂段构成的双圆弧曲线；

根据所述机械臂第一约束方程、所述机械臂第二约束方程建立机械臂全局约束方程，所述机械臂全局约束方程用于表示所述超冗余联动机械臂的圆弧曲线；

根据所述机械臂全局约束方程分别求解5个所述臂段的驱动关节角度，并根据各个所述驱动关节角度驱动所述超冗余联动机械臂向规划轨迹运动。

2.根据权利要求1所述的超冗余联动机械臂的运动轨迹控制方法，其特征在于，根据所述空间双圆弧约束方程建立机械臂第一约束方程具体包括：

规划第五臂段末端的第五臂段期望位置信息和第五臂段期望指向信息，并根据所述结构信息、所述第五臂段期望位置信息和所述第五臂段期望指向信息规划所述超冗余联动机械臂的第三臂段末端的第三臂段期望位置信息；

将所述第三臂段末端的期望指向信息进行参数化得到第三臂段期望指向信息表达式；

根据所述超冗余联动机械臂的第四臂段参数信息、所述超冗余联动机械臂的第五臂段参数信息、所述第五臂段期望位置信息、所述第五臂段期望指向信息、所述空间双圆弧约束方程和所述第三臂段期望指向信息表达式建立所述机械臂第一约束方程。

3.根据权利要求2所述的超冗余联动机械臂的运动轨迹控制方法，其特征在于，

所述第四臂段参数信息包括所述超冗余联动机械臂的第四臂段中单个所述小节对应的第四臂段单小节长度，以及所述第四臂段所对应的第四臂段圆心角；

所述第五臂段参数信息包括所述超冗余联动机械臂的第五臂段中单个所述小节对应的第五臂段单小节长度，以及所述第五臂段所对应的第五臂段圆心角。

4.根据权利要求2或3所述的超冗余联动机械臂的运动轨迹控制方法，其特征在于，根据所述空间双圆弧约束方程建立机械臂第二约束方程具体包括：

根据所述第三臂段期望位置信息和所述第三臂段期望指向信息表达式获取第二臂段末端的第二臂段期望位置信息和第二臂段期望指向信息；

根据所述超冗余联动机械臂的第一臂段参数信息、所述超冗余联动机械臂的第二臂段参数信息、所述第二臂段期望位置信息、所述第二臂段期望指向信息、所述空间双圆弧约束方程和所述第三臂段期望指向信息表达式建立所述机械臂第二约束方程。

5.根据权利要求4所述的超冗余联动机械臂的运动轨迹控制方法，其特征在于，所述第一臂段参数信息包括所述超冗余联动机械臂的第一臂段中单个所述小节对应的第一臂段单小节长度、所述第一臂段所对应的第一臂段圆心角、第一臂段起始位置信息、第一臂段起始指向信息；

所述第二臂段参数信息包括所述超冗余联动机械臂的第二臂段中单个所述小节对应的第二臂段单小节长度，以及所述第二臂段所对应的第二臂段圆心角。

6.根据权利要求1、2、3或5所述的超冗余联动机械臂的运动轨迹控制方法，其特征在于，根据所述机械臂全局约束方程分别求解5个所述臂段的驱动关节角度，并根据各个所述驱动关节角度驱动所述超冗余联动机械臂向规划轨迹运动具体包括：

求解所述机械臂全局约束方程获得所述超冗余联动机械臂的各个臂段末端的各个期望位置信息和各个期望指向信息；

基于各个所述期望位置信息和各个所述期望指向信息通过数学求解方法计算出各个所述臂段的各个所述驱动关节角度；

根据各个所述驱动关节角度驱动所述超冗余联动机械臂向规划轨迹运动。