[发明专利]一种基于虚拟力的主从跟随与避碰方法

权利要求书

1.一种基于虚拟力的主从跟随与避碰方法，其特征在于，包括以下步骤：

步骤1、构建双机械臂系统运动模型，得到机械臂末端相对于基座的位置和姿态；

步骤2、根据机械臂末端状态，构建虚拟力场函数，计算双机械臂末端的虚拟引力与虚拟斥力；

步骤3、通过计算出虚拟引力和斥力，采用主从控制模式确定双机械臂跟随的约束关系；

步骤4、对机械臂进行虚拟力优化，求虚拟的最终目标点，使双机械臂系统跳出局值状态；

步骤5、利用求解的虚拟目标点、障碍物、双机械臂系统以及目标点之间的虚拟力场来对双机械臂系统进行避碰规划，判断是否发生碰撞，实现避碰；

步骤6、从机械臂根据主从机械臂约束关系确定末端姿态，并将主机械臂加入到从机械臂的障碍物中，再对从机械臂进行轨迹规划，实现从机械臂跟随主机械臂。

2.根据权利要求1所述的基于虚拟力的主从跟随与避碰方法，其特征在于，步骤1中构建双机械臂系统运动模型，得到机械臂末端相对于基座的位置和姿态，具体包括以下步骤：

步骤1-1、采用DH参数法，建立连杆i的固连坐标系{O_i}，坐标系{O_i-1}设立在i关节处，与i-1连杆固连；坐标系{O_i}的三个坐标轴为x_i轴、y_i轴、z_i轴，采集关节扭角α_i、连杆长度a_i、关节偏移d_i以及旋转角θ_i；

其中θ_i表示关于z_i-1轴，x_i轴在x_i-1轴下逆时针转过的角度；d_i表示坐标系{O_i-1}沿z_i-1轴平移到z_i-1轴与x_i轴的交点所经过的距离；a_i表示沿x_i-1轴方向z_i-1轴到z_i轴之间的距离；α_i表示z_i-1轴绕x_i轴到z_i轴所转过的角度；

步骤1-2、根据连杆间的一组DH参数，求出连杆间齐次变换矩阵

其中Trans(0,0,d_i)为沿着z_i-1轴移动d_i的齐次变换矩阵的简写，Trans(a_i,0,0)为沿着x_i-1轴移动a_i的齐次变换矩阵的简写，Rot(z_i-1,θ_i)为沿着z_i-1旋转θ_i的齐次变换矩阵的简写，Rot(x_i-1,α_i)为沿着x_i-1旋转α_i的齐次变换矩阵的简写；

步骤1-3、由齐次变换矩阵求解到六自由度机械臂末端相对于基座的位置和姿态，得到机械臂末端相对于基坐标系的齐次变换矩阵T,得到得到该坐标系相对于基坐标系的姿态向量

其中，n_x、o_x、a_x向量为x方向上的矩阵相乘所得积的缩写，n_y、o_y、a_y向量为y方向上的矩阵相乘所得积的缩写，n_z、o_z、a_z向量为z方向上的矩阵相乘所得积的缩写，p_x、p_y、p_z向量代表了机械臂末端的位置参数。