一种基于虚拟力的主从跟随与避碰方法技术

本发明专利技术公开了一种基于虚拟力的主从跟随与避碰方法，该方法为：构建双机械臂系统运动模型，得到机械臂末端相对于基座的位置和姿态；根据机械臂末端状态，构建虚拟力场函数，计算双机械臂末端的虚拟引力与虚拟斥力；采用主从控制模式确定双机械臂跟随的约束关系；对机械臂进行虚拟力优化，求虚拟的最终目标点，使双机械臂系统跳出局值状态；利用求解的虚拟目标点、障碍物、双机械臂系统以及目标点之间的虚拟力场来对双机械臂系统进行避碰规划；从机械臂根据主从机械臂约束关系确定末端姿态，并将主机械臂加入到从机械臂的障碍物中，再对从机械臂进行轨迹规划，实现从机械臂跟随主机械臂。本发明专利技术实现了狭窄作业环境下良好的避碰与主从跟随。主从跟随。主从跟随。

【技术实现步骤摘要】
一种基于虚拟力的主从跟随与避碰方法


[0001]本专利技术属于机器人双臂协同和运动规划
，具体为一种基于虚拟力的主从跟随与避碰方法。

技术介绍

[0002]近年来我国配电网进入飞速发展阶段，与此同时配电线路所需的运维工作量也大幅增加。狭小空间作业限制了带电机器人的开发。在带电作业领域，与日益增长的带电维护作业量相比，我国目前广泛采用的人工绝缘杆和绝缘手套作业法明显效率不足。需要提高配电线路维护机器人的自主性、安全性和灵活性，而双臂协作技术正是提高机器人自主性与灵活性的关键技术。采用机器人代替人工完成带电作业，不仅可以避免人员伤亡事故的发生，提高作业效率；同时还可以提高电网运行质量，进一步减少供电系统维护人工成本，带来巨大的经济效益和社会效益。总之，双臂协同与避障技术迫在眉睫。
[0003]在双机械臂协作完成带电作业任务过程中，同时包含各机械臂自身位姿约束以及协作机械臂间的相对运动约束，这些约束是作业任务本身带来的要求；另一方面，在特定作业任务下，由于机械臂与工件接触产生的接触力，以及环境对系统施加的外力干扰，都会增大机械臂间的碰撞，当机械臂夹持工件过程中受力超出安全范围，容易造成设备的损坏。

技术实现思路

[0004]本专利技术的目的在于提供一种基于虚拟力的主从跟随与避碰方法，实现狭窄作业环境下良好的避碰与主从跟随。
[0005]实现本专利技术目的的技术方案为：一种基于虚拟力的主从跟随与避碰方法，包括以下步骤：
[0006]步骤1、构建双机械臂系统运动模型，得到机械臂末端相对于基座的位置和姿态；
[0007]步骤2、根据机械臂末端状态，构建虚拟力场函数，计算双机械臂末端的虚拟引力与虚拟斥力；
[0008]步骤3、通过计算出虚拟引力和斥力，采用主从控制模式确定双机械臂跟随的约束关系；
[0009]步骤4、对机械臂进行虚拟力优化，求虚拟的最终目标点，使双机械臂系统跳出局值状态；
[0010]步骤5、利用求解的虚拟目标点、障碍物、双机械臂系统以及目标点之间的虚拟力场来对双机械臂系统进行避碰规划，判断是否发生碰撞，实现避碰；
[0011]步骤6、从机械臂根据主从机械臂约束关系确定末端姿态，并将主机械臂加入到从机械臂的障碍物中，再对从机械臂进行轨迹规划，实现从机械臂跟随主机械臂。
[0012]本专利技术与现有技术相比，其显著优点为：(1)突破了狭窄作业环境下，双臂易出现碰撞、难以主从跟随的问题；(2)引入了双机械臂约束关系，能够实现双机械臂主从跟随；(3)运用了优化的虚拟力，引入虚拟目标点，保证机械臂系统不会陷入局部极值状态，实现
避碰。
[0013]下面结合附图对本专利技术作进一步详细描述。
附图说明
[0014]图1为本专利技术基于虚拟力的主从跟随与避碰方法的流程图。
[0015]图2为本专利技术中连杆坐标系的关节图。
具体实施方式
[0016]本专利技术一种基于虚拟力的主从跟随与避碰方法，包括以下步骤：
[0017]步骤1、构建双机械臂系统运动模型，得到机械臂末端相对于基座的位置和姿态；
[0018]步骤2、根据机械臂末端状态，构建虚拟力场函数，计算双机械臂末端的虚拟引力与虚拟斥力；
[0019]步骤3、通过计算出虚拟引力和斥力，采用主从控制模式确定双机械臂跟随的约束关系；
[0020]步骤4、对机械臂进行虚拟力优化，求虚拟的最终目标点，使双机械臂系统跳出局值状态；
[0021]步骤5、利用求解的虚拟目标点、障碍物、双机械臂系统以及目标点之间的虚拟力场来对双机械臂系统进行避碰规划，判断是否发生碰撞，实现避碰；
[0022]步骤6、从机械臂根据主从机械臂约束关系确定末端姿态，并将主机械臂加入到从机械臂的障碍物中，再对从机械臂进行轨迹规划，实现从机械臂跟随主机械臂。
[0023]进一步地，步骤1中构建双机械臂系统运动模型，得到机械臂末端相对于基座的位置和姿态，具体包括以下步骤：
[0024]步骤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
；
[0025]其中θ
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
轴所转过的角度；
[0026]步骤1
‑
2、根据连杆间的一组DH参数，求出连杆间齐次变换矩阵2、根据连杆间的一组DH参数，求出连杆间齐次变换矩阵
[0027]其中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
的齐次变换矩阵的简写；
[0028]步骤1
‑
3、由齐次变换矩阵求解到六自由度机械臂末端相对于基座的位置和
姿态，得到机械臂末端相对于基坐标系的齐次变换矩阵T,得到得到该坐标系相对于基坐标系的姿态向量
[0029]其中，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
向量代表了机械臂末端的位置参数。
[0030]进一步地本文档来自技高网...

【技术保护点】

【技术特征摘要】
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参数，求出连杆间齐次变换矩阵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
向量代表了机械臂末端的位置参数。3.根据权利要求1所述的基于虚拟力的主从跟随与避碰方法，其特征在于，步骤2中根
据机械臂末端状态，构建虚拟力场函数，具体包括以下步骤：步骤2
‑
1、采集机械臂末端位姿x，目标点位置坐标x
g
，机械臂与障碍物距离d，障碍场半径范围为d0；步骤2
‑
2、计算六个关节角对应到目标点的角度θ
gi
，求解虚拟引力函数U
aat
，其中，k
a
为引力常数，θ
i
是机械臂相应的第i关节的角度；步骤2
‑
3、根据机械臂各个关节与障碍物之间的最短距离d
i
，计算在斥力场中各连杆i的虚拟斥力函数U
rep
(i)，计算连杆合力计算连杆合力其中，k
r
为斥力常数；由机械臂通过关节连接相邻的连杆，六自由度机械臂连杆数为5；步骤2
‑
4、计算总虚拟力函数U
tl
＝U
aat
+U
rep*
。4.根据权利要求1所述的基于虚拟力的主从跟随与避碰方法，其特征在于，步骤3中通过计算出虚拟引力和斥力，采用主从控制模式确定双机械臂跟随的约束关系，具...

【专利技术属性】
技术研发人员：王越，郑先杰，罗威，陈欢，樊卫华，郭健，
申请(专利权)人：南京理工大学，
类型：发明
国别省市：