[发明专利]一种机械臂运动控制方法

权利要求书

1.一种机械臂运动控制方法，其特征在于，包括以下步骤：

步骤一：针对六自由度机械臂建立机械臂运动D-H模型；

步骤二：对六自由度机械臂建立机械臂四连杆简化模型；

步骤三：利用改进人工势场法对机械臂运动进行避障控制。

2.根据权利要求1所述的机械臂运动控制方法，其特征在于，步骤一中，所述机械臂运动D-H模型通过运动学正解的方法得到在当前关节角下所对应的末端位置，以机械臂关节1～关节5的关节角度作为控制目标，不考虑关节6角度的影响。

3.根据权利要求1所述的机械臂运动控制方法，其特征在于，步骤二中，所述机械臂四连杆简化模型将机械臂的末端定义为关节7；

将基座、关节1和关节2构成的一条线段定义为直线一；

将关节2、关节3和关节4构成的一条线段定义为直线二；

将关节4和关节5构成的一条线段定义为直线三；

将关节5、关节6和关节7构成的一条线段定义为直线四。

4.根据权利要求3所述的机械臂运动控制方法，其特征在于，根据所述机械臂四连杆简化模型，可由机械臂当前关节角组合S＝(θ_i1，θ_i2，…，θ_i6)求解出基座、关节2、关节4、关节5和关节7的空间位置，计算方法如下：

基座：

关节2：

关节4：

关节5：

关节7：

上式中，p_ix，p_iy，p_iz分别表示关节i在世界坐标系下x，y，z三个轴向上的分量，通过立体几何方法，由基座、关节2、关节4、关节5和关节7的空间位置求得直线一到直线四的空间解析方程，进一步求解得到空间障碍到各个连杆的距离。

5.根据权利要求1所述的机械臂运动控制方法，其特征在于，步骤三中，所述改进人工势场法以机械臂各关节角度作为控制和运算的对象。

6.根据权利要求5所述的机械臂运动控制方法，其特征在于，所述改进人工势场法采用机械臂关节空间作为搜索空间，以当前关节状态S＝(θ_i1，θ_i2，…，θ_i5)作为初始状态，遍历当前状态的相邻关节值，求取出各种关节组合所对应的合势能，选择合适的关节组合作为机械臂下一步的运动目标，设置搜索步长λ为0.5；

以机械臂六个关节角度最终位置为目标，求解得到机械臂所受的引力势能和斥力势能：

其中，所述引力势能：

所述斥力势能：

其中E_att、E_rep和E_i分别表示机械臂所受引力势能、斥力势能和各关节连杆所受的斥力势能，k_a和k_r分别表示所受引力和斥力的比例系数，d_i和d₀分别表示各关节到障碍物的距离和障碍物的斥力影响范围。

7.根据权利要求6所述的机械臂运动控制方法，其特征在于，所述改进人工势场法在局部极小点附近添加空间虚拟障碍点以避免机械臂运动过程中易于陷入局部极小值点。

8.根据权利要求7所述的机械臂运动控制方法，其特征在于，所述空间虚拟障碍点的设置满足以下条件：

所述空间虚拟障碍点与机械臂到实际障碍点最近位置点P的连线垂直于平面X；

平面X由当前角度下关节B、关节C所在位置点与目标角度下关节C所在的位置点确定；

关节B和关节C为从关节2、关节4、关节5和关节7中的其中两个关节组成的关节组合；

设定所述空间虚拟障碍点的斥力系数为实际障碍点的10～100倍、作用范围为实际障碍点的2～3倍；

设定所述空间虚拟障碍点到点P的距离为机械臂的安全距离d₀，设定斥力为实际障碍点的10倍，斥力范围为实际障碍点的2倍。