[发明专利]一种七自由度空间机械臂自主避障规划方法

权利要求书

1.一种七自由度空间机械臂自主避障规划方法，其特征在于包括如下步骤：

(1)在任务空间对七自由度空间机械臂的末端进行路径规划，得到末端的运动路径点数据；

(2)依次选取末端的运动路径点数据，固定任一冗余关节角度进行逆运动学计算，得到当前末端的运动路径点数据对应的其它关节的角度；

(3)对得到的当前末端的运动路径点数据对应的所有关节的角度进行碰撞检测，通过对固定的关节角度进行自适应调整，使得当前末端的运动路径点数据对应的七自由度空间机械臂与障碍物不发生碰撞；

(4)重复上述过程，直至得到所有末端的运动路径点数据对应的七自由度空间机械臂的自主避障规划。

2.根据权利要求1所述的一种七自由度空间机械臂自主避障规划方法，其特征在于：所述的在任务空间对七自由度空间机械臂的末端进行路径规划，得到末端的运动路径点数据的方法为：

(11)根据任务需求确定七自由度空间机械臂末端需要到达的目标位置、姿态；

(12)根据目标位置、姿态在任务空间对七自由度空间机械臂末端的运动进行路径规划，得到七自由度空间机械臂末端的N个运动路径点数据Ei，i＝1,2，……，N，其中，N为正整数。

3.根据权利要求2所述的一种七自由度空间机械臂自主避障规划方法，其特征在于：所述的依次选取末端的运动路径点数据，固定任一冗余关节角度进行逆运动学计算，得到当前末端的运动路径点数据对应的其它关节的角度的方法为：

(21)选取七自由度空间机械臂的7个关节中的任一冗余关节k，固定该关节的角度为θ_k，得到等效六自由度空间机械臂，其中，1≤k≤7，且k为整数，θ_k的初值为0；

(22)选取第i个末端运动路径点数据Ei进行逆运动学计算，得到与Ei对应的等效六自由度空间机械臂的关节角度，进而得到Ei对应的七自由度空间机械臂的关节角度，其中i为正整数。

4.根据权利要求3所述的一种七自由度空间机械臂自主避障规划方法，其特征在于：所述的对得到的当前末端的运动路径点数据对应的所有关节的角度进行碰撞检测，通过对固定的关节角度进行自适应调整，使得当前末端的运动路径点数据对应的七自由度空间机械臂与障碍物不发生碰撞的方法为：

(31)对得到的当前末端的运动路径点数据对应的七自由度空间机械臂的关节角度，采用碰撞检测的方法判断是否与障碍物发生碰撞，如果不发生碰撞，则转至步骤(4)，否则转至步骤(32)；

(32)令关节角度增量Δ的初始值Δ₀＝0.1°，将当前固定关节的关节角度设置为θ_k+关节角度增量Δ，重新计算当前末端的运动路径点数据对应的七自由度空间机械臂的关节角度；

(33)采用碰撞检测的方法判断是否与障碍物发生碰撞，如果不发生碰撞，则转至步骤(4)，否则转至步骤(34)；

(34)将当前固定关节的关节角度设置为θ_k-关节角度增量Δ，重新计算当前末端的运动路径点数据对应的七自由度空间机械臂的关节角度；

(35)采用碰撞检测的方法判断是否与障碍物发生碰撞，如果不发生碰撞，则转至步骤(4)，否则将当前关节角度增量Δ设为原来的两倍，转入至步骤(32)，直至对得到的当前末端的运动路径点数据对应的所有关节的角度进行碰撞检测，通过对固定的关节角度进行自适应调整，使得当前末端的运动路径点数据对应的七自由度空间机械臂与障碍物不发生碰撞。

5.根据权利要求4所述的一种七自由度空间机械臂自主避障规划方法，其特征在于：所述的重复上述过程，直至得到所有末端的运动路径点数据对应的七自由度空间机械臂的自主避障规划的方法为：

(41)判断i是否等于七自由度空间机械臂末端的运动路径点数据个数N，如果相等，则结束当前自主避障规划；否则转至步骤(42)；

(42)设置i＝i+1，重新转回至步骤(2)。

6.一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1-5任意之一所述方法的步骤。