[发明专利]一种冗余机械臂运动障碍物避障算法

权利要求书

1.一种冗余机械臂运动障碍物避障算法，其特征在于：该方法包括步骤如下：

步骤S1：将运动障碍物简化成一个质点，将冗余机械臂的杆件简化成线段，并求得线段上距离质点最近的点作为碰撞预测点；

步骤S2：根据碰撞预测点与障碍物的相对位置和速度关系，求得各杆件的躲避速度；

步骤S3：利用雅可比转置矩阵将杆件的躲避速度转换为机械臂相应的关节角速度变量，并引入梯度投影法中，最后得到各关节角速度，实现对运动障碍物的躲避。

2.按权利要求1所述的冗余机械臂运动障碍物避障算法，其特征在于：所述碰撞预测点求解的具体步骤如下：

步骤S11：求得沿杆件方向的单位向量；

步骤S12：利用垂直向量点积为0的性质求得碰撞预测点距杆件指定端点的距离，并用标量表示；

步骤S13：如果此距离值小于0，则碰撞预测点为该指定端点，如果距离值大于杆件的长度，则碰撞预测点为另外一个端点，如果距离值为0和杆件长度之间的任意值，碰撞预测点在杆件上，可以利用指定端点和距离值来确定其具体位置。

3.按权利要求1所述的冗余机械臂运动障碍物避障算法，其特征在于：所述的杆件躲避速度求解的具体步骡如下：

步骤S21：将运动障碍物的速度在障碍物与碰撞预测点之间的连线上进行投影，得到最短预测距离；

步骤S22：当杆件的最短预测距离大于安全阈值时，该杆件的躲避速度为0，当杆件的最短预测距离小于安全阈值时，杆件存在躲避速度，且最短预测距离越小，杆件躲避速度越大。

4.按权利要求1所述的冗余机械臂运动障碍物避障算法，其特征在于：所述的关节角速度求解的具体步骤如下：

步骤S31：利用机械臂的转置雅可比矩阵将杆件的躲避速度转化为相应的关节角速度变量；

步骤S32：将各关节的角速度变量引入梯度投影法中，求得关节的角速度。