一种六自由度工业机械臂避障实验装置及其控制方法与流程

1.本发明涉及机器人避障实验设备领域，尤其涉及六自由度工业机械臂避障实验设备及其控制技术领域。


背景技术：

2.随着生产力的发展，工业机械臂在生产过程中的地位与使用比重越来越高，工业机械臂作为人工智能领域重要的研究板块，被越来越多的研发机构作为自己的项目领域进行研究与开发。目前当工业机械臂发生碰撞时的应急措施往往是紧急制动，而且在实际的生产过程中，工业机械臂的停止运作势必会对整条生产线的生产过程造成影响，降低生产效率并造成生产流程的混乱。目前国内外有很多机器人生产商都开发了工业机械臂避障算法，在实际的操作中需要一套完善的实验装置来测试开发的算法。就现阶段而言，市场上并没有一个固定的机械臂避障检测标准，尤其是六自由度工业机械臂的避障实验装置和避障控制算法仍然需要深入研究。
3.目前市面上存在的机械臂避障实验装置大多是针对六自由度以下的工业机械臂以及固定障碍物的避障测试，然而一般机械臂的实际工作环境较为复杂，不仅需要对静态物体进行避障，更需要对动态的物体进行避障，并且这些避障实验装置的障碍物设计过于单一，无法证明工业机械臂对于不同形状障碍物的碰撞检测能力与避障能力。因此迫切需要我们设计一套六自由度工业机械臂避障实验装置用来验证面对静态、动态障碍物时避障算法的避障性能，同时根据常规的碰撞检测算法中的包络法确定其障碍物的形状，使其对于避障算法的检测更加可靠，从而填补机械臂避障实验技术的相关空缺。


技术实现要素：

4.本发明所设计的一种六自由度工业机械臂避障实验装置，其包括测试平台(1)、平台支撑模块(2)和控制台(3)，测试平台(1)与平台支撑模块(2)通过测试平台底盘(1-10)的螺纹孔与平台支撑模块(2)的支撑杆(2-1)上的螺纹连接，测试平台(1)与控制台(3)通过控制总线连接；
5.进一步的，所述测试平台由传感器(1-1)、机械臂(1-2)、法兰a(1-3)、障碍物(1-4)、障碍物摆放台(1-5)、内齿轮(1-6)、驱动齿轮(1-7)、驱动电机(1-8)、驱动轴(1-9)、测试平台底盘(1-10)、安装槽(1-11)、端面轴承(1-12)、法兰b(1-13)组成；传感器(1-1)通过螺钉安装在机械臂(1-2)的关节部位，所测试的机械臂(1-2)为六自由度机械臂，每隔两个关节轴安装一个传感器(1-1)，左右两侧各三个传感器(1-1)；机械臂(1-2)通过法兰a(1-3)固定在驱动电机(1-8)顶部的支撑面上；驱动齿轮(1-7)设有键槽，通过驱动电机(1-8)上设有键的驱动轴(1-9)连接；障碍物摆放台(1-5)为圆形，障碍物摆放台(1-5)设有内齿轮(1-6)，与安装驱动轴(1-9)上的驱动齿轮(1-7)啮合，驱动电机(1-8)通过驱动轴(1-9)上的驱动齿轮(1-7)带动障碍物摆放台(1-5)转动；障碍物(1-4)的伸缩杆底端设有键，障碍物摆放台上设有二十四个带键槽的插孔，插孔呈圆形排列，分为外圈、中圈、内圈，每一圈插孔之间的距
离相等，外圈的最大半径应不超过所测试机械臂(1-2)运动半径，障碍物(1-4)通过插入插孔固定在障碍物摆放平台上；障碍物摆放平台(1-5)下面与测试平台底盘(1-10)的上面设有安装槽(1-11)，用来装配端面轴承(1-12)，障碍物摆放平台(1-5)与测试平台底盘(1-10)通过端面轴承(1-12)连接；驱动电机(1-7)通过法兰b(1-13)与底盘的底面固定；
6.进一步的，所述平台支撑模块由支撑杆(2-1)、支撑模块底盘(2-2)和万向轮(2-3)组成，支撑杆(2-1)两端设有螺纹，测试平台底盘(1-10)的下面设有四个螺纹孔，支撑模块底盘(2-1)的上下两面分别设有四个螺纹孔，支撑杆(2-1)通过螺纹分别与测试平台底盘(1-10)下面和支撑模块底盘(2-1)上面的螺纹孔连接；万向轮(2-3)的轴上设有螺纹，万向轮(2-3)通过支撑模块底盘(2-2)下面的螺纹孔固定；
7.进一步的，所述控制台由控制器(3-1)、显示器(3-2)、桌子(3-3)组成，控制器(3-1)与显示器(3-2)摆放在桌子(3-3)上，控制器(3-1)与显示器(3-2)通过数据线连接；
8.进一步的，所述障碍物分为长方体障碍物(1-4-1)、圆柱体障碍物(1-4-2)、球体障碍物(1-4-3)和障碍板a(1-4-4)、障碍板b(1-4-5)；其中长方体障碍物(1-4-1)，圆柱体障碍物(1-4-2)和球体(1-4-3)障碍物均为木质材质；长方体障碍物(1-4-1)、圆柱体障碍物(1-4-2)和球体障碍物(1-4-3)固定在伸缩杆(1-4-6)上，伸缩杆(1-4-6)为铝合金材质，共三节，伸缩杆(1-4-6)末端通过一个可旋转的轴承连接一个带键的轴，障碍物摆放台(1-5)上设有二十四个带键槽的插孔，障碍物通过插入插孔固定在障碍物摆放平台上；障碍板(1-4-4、1-4-5)分为木质材料(1-4-4)和玻璃材料(1-4-5)，障碍板(1-4-4、1-4-5)的底端接有一根铝合金材质的杆(1-4-7)，杆(1-4-7)的末端通过轴承连接一个同样设有键的轴，障碍板(1-4-4、1-4-5)通过插入插孔固定在障碍物摆放平台(1-5)上。
9.所述的一种六自由度工业机械臂避障实验控制方法如下所述：
10.其中，圆柱体包络法的轴心线可描述为：
[0011][0012]
其中(xi,yi,zi)和(x
i 1
,y
i 1
,z
i 1
)分别为圆柱体上底面、下底面的中心点位置坐标，t为[0,1]之间的常数，则圆柱体轴心线的方程为：
[0013]
li＝[xi(t),yi(t),zi(t)]＝[xi,yi,zi] t
×
[x
i 1-xi,y
i 1-yi,z
i 1-zi],t∈(0,1)(2)
[0014][0015]
所述一种六自由度工业机械臂避障实验控制方法，其中检测机械臂与圆柱体是否碰撞的检测方法为：机械臂各连杆通过圆柱体包络法包络，对应的圆柱体的半径依次记为ri(i＝1,2,3,4,5,6)，i为连杆序数，障碍物的包络半径记为rz，通过式(1)、(2)确定机械臂各连杆和障碍物的轴线方程，计算各连杆与障碍物的轴线间距diz(i＝1,2,3,4,5,6),当diz》rz ri(i＝1,2,3,4,5,6)时，不会发生碰撞；当diz≤rz ri(i＝1,2,3,4,5,6)时，会发生碰撞；
[0016]
长方体的数学模型可描述为：
[0017]
c＝{(x,y,z)|mx≤x≤mx,my≤y≤my,mz≤z≤mz}(3)
[0018]
式中的mx，my，mz分别代表长方体边长沿坐标轴x、y、z方向的最小坐标值，mx、my、mz分别代表长方体边长沿坐标轴x、y、z方向的最大坐标值；
[0019]
所述一种六自由度工业机械臂避障实验控制方法，其中检测机械臂与长方体是否
碰撞的方法为：机械臂各连杆通过圆柱体包络法包络，对应的圆柱体的半径依次记为ri(i＝1,2,3,4,5,6)，i为连杆序数，选择最大的ri作为最大的半径rmax，然后将最大半径rmax加入到长方体障碍物的长宽高中使其膨胀化，通过正运动学算法推导出每个连杆末端的坐标，然后确定机械臂各连杆的直线向量与长方体各平面的法向量是否垂直，如果垂直则直线与法向量不存在交点，即与对应长方体面无交点，如果不垂直判断交点是否在平面上，如果交点在平面上则相撞，如果不在则不相撞；
[0020]
球体包络法的数学模型可表示为：
[0021]
globe＝{(x,y,z)|(x-x0)2 (y-y0)2 (z-z0)2≤r}(4)
[0022]
其中r为球体的半径，(x0,y0,z0)为球体的中心点坐标；
[0023]
所述的一种六自由度工业机械臂避障实验控制方法，其中检测机械臂与球体是否碰撞的检测方法为：机械臂各连杆通过圆柱体包络法包络，对应的圆柱体的半径依次记为ri(i＝1,2,3,4,5,6)，选择最大的ri作为最大的半径rmax，通过机器人正运动学算法求出各由式(1)、(2)确定机械臂各连杆对应的圆柱轴线方程，计算球体中心点到圆柱轴线的距离di(i＝1,2,3,4,5,6)，如果d》rmax r，则机械臂与球体不会碰撞，如果d≤rmax r，则说明机械臂与球体会碰撞；
[0024]
进一步的，所述功能的实现流程如图6，首先将需要验证的避障算法写入控制台(3)；通过控制台(3)输入机械臂(1-2)的初始位姿和目标位姿，规划一条机械臂的初始路径穿过障碍物；通过控制台(3)控制驱动电机(1-8)启停，驱动障碍物摆放台(1-5)的旋转；安装于机械臂(1-2)关节的传感器(1-1)发射信号构建工作环境及障碍物信息；通过构建的工作环境判断障碍物(1-4)的位置，然后由控制台(3)进行避障算法的处理，验证避障算法的效果；针对实际的避障效果对避障算法进行更改与调整。
[0025]
本发明的有益效果是：
[0026]
1、根据常用的碰撞检测包络方法确定障碍物(1-4)形状，从而对避障算法的可行性进行准确地验证；
[0027]
2、本发明通过在障碍物摆放台(1-5)上不同位置设计了二十四个插孔，使用伸缩杆(1-4-6)用来支撑障碍物，并且在伸缩杆(1-4-6)末端增加了可旋转的结构，从而可以达到在机械臂(1-2)的运动空间内对不同位置、不同姿态的障碍物进行测试的功能；
[0028]
3、通过设计驱动电机(1-8)驱动障碍物摆放台(1-5)旋转的功能，可以测试三种不同情况下机械臂(1-2)的避障效果，即机械臂(1-2)运动而障碍物(1-4)静止、机械臂(1-2)静止而障碍物(1-4)运动、机械臂(1-2)与障碍物(1-4)同时运动的情况，可以模拟实际环境中不同的避障情景；
[0029]
4、通过控制台(3)控制机械臂(1-2)动作，规划指定路径，返回避障数据，可以更清楚地显示实验结果，从而使测试结果更加可靠。
附图说明
[0030]
下面结合附图及实施方式对本发明作进一步详细的说明：
[0031]
图1为六自由度工业机械臂避障实验装置总体示意图；
[0032]
图2为测试平台及其及内部结构示意图；
[0033]
图3为支撑模块示意图；
[0034]
图4为控制台示意图；
[0035]
图5为障碍物示意图；
[0036]
图6为六自由度工业机械臂的功能流程图。
具体实施方式
[0037]
实施例
[0038]
一种六自由度工业机械臂避障实验装置，其包括测试平台(1)、平台支撑模块(2)和控制台(3)，测试平台(1)与平台支撑模块(2)通过测试平台底盘(1-10)的螺纹孔与平台支撑模块(2)的支撑杆(2-1)上的螺纹连接，测试平台(1)与控制台(3)通过一根控制总线连接；
[0039]
所述测试平台由传感器(1-1)、机械臂(1-2)、法兰a(1-3)、障碍物(1-4)、障碍物摆放台(1-5)、内齿轮(1-6)、驱动齿轮(1-7)、驱动电机(1-8)、驱动轴(1-9)、测试平台底盘(1-10)、安装槽(1-11)、端面轴承(1-12)、法兰b(1-13)组成；传感器(1-1)通过螺钉安装在机械臂(1-2)的关节部位，所测试的机械臂(1-2)为六自由度机械臂，每隔两个关节轴安装一个传感器(1-1)，左右两侧各三个传感器(1-1)；机械臂(1-2)通过法兰a(1-3)固定在驱动电机(1-8)顶部的支撑面上；驱动齿轮(1-7)设有键槽，通过驱动电机(1-8)上设有键的驱动轴(1-9)连接；障碍物摆放台(1-5)为圆形，障碍物摆放台(1-5)设有内齿轮(1-6)，与安装驱动轴(1-9)上的驱动齿轮(1-7)啮合，驱动电机(1-8)通过驱动轴(1-9)上的驱动齿轮(1-7)带动障碍物摆放台(1-5)转动；障碍物(1-4)的伸缩杆底端设有键，障碍物摆放台上设有二十四个带键槽的插孔，插孔呈圆形排列，分为外圈、中圈、内圈，每一圈插孔之间的距离相等，外圈的最大半径应不超过所测试机械臂(1-2)运动半径，障碍物(1-4)通过插入插孔固定在障碍物摆放平台上；障碍物摆放平台(1-5)下面与测试平台底盘(1-10)的上面设有安装槽(1-11)，用来装配端面轴承(1-12)，障碍物摆放平台(1-5)与测试平台底盘(1-10)通过端面轴承(1-12)连接；驱动电机(1-7)通过法兰b(1-13)与底盘的底面固定；
[0040]
所述平台支撑模块由支撑杆(2-1)、支撑模块底盘(2-2)和万向轮(2-3)组成，支撑杆(2-1)两端设有螺纹，测试平台底盘(1-10)的下面设有四个螺纹孔，支撑模块底盘(2-1)的上下两面分别设有四个螺纹孔，支撑杆(2-1)通过螺纹分别与测试平台底盘(1-10)下面和支撑模块底盘(2-1)上面的螺纹孔连接；万向轮(2-3)的轴上设有螺纹，万向轮(2-3)通过支撑模块底盘(2-2)下面的螺纹孔固定；
[0041]
所述控制台由控制器(3-1)、显示器(3-2)、桌子(3-3)组成，控制器(3-1)与显示器(3-2)摆放在桌子(3-3)上，控制器(3-1)与显示器(3-2)通过数据线连接；
[0042]
所述障碍物分为长方体障碍物(1-4-1)、圆柱体障碍物(1-4-2)、球体障碍物(1-4-3)和障碍板a(1-4-4)、障碍板b(1-4-5)；其中长方体障碍物(1-4-1)，圆柱体障碍物(1-4-2)和球体(1-4-3)障碍物均为木质材质；长方体障碍物(1-4-1)、圆柱体障碍物(1-4-2)和球体障碍物(1-4-3)固定在伸缩杆(1-4-6)上，伸缩杆(1-4-6)为铝合金材质，共三节，伸缩杆(1-4-6)末端通过一个可旋转的轴承连接一个带键的轴，障碍物摆放台(1-5)上设有二十四个带键槽的插孔，障碍物通过插入插孔固定在障碍物摆放平台上；障碍板(1-4-4、1-4-5)分为木质材料(1-4-4)和玻璃材料(1-4-5)，障碍板(1-4-4、1-4-5)的底端接有一根铝合金材质的杆(1-4-7)，杆(1-4-7)的末端通过轴承连接一个同样设有键的轴，障碍板(1-4-4、1-4-5)
通过插入插孔固定在障碍物摆放平台(1-5)上。
[0043]
所述一种六自由度工业机械臂避障实验控制方法如下所述：
[0044]
其中，圆柱体包络法的轴心线可描述为：
[0045][0046]
其中(xi,yi,zi)和(x
i 1
,y
i 1
,z
i 1
)分别为圆柱体上底面、下底面的中心点位置坐标，t为[0,1]之间的常数，则圆柱体轴心线的方程为：
[0047]
li＝[xi(t),yi(t),zi(t)]＝[xi,yi,zi] t
×
[x
i 1-xi,y
i 1-yi,z
i 1-zi],t∈(0,1)(2)
[0048][0049]
所述一种六自由度工业机械臂避障实验控制方法，其中检测机械臂与圆柱体是否碰撞的检测方法为：机械臂各连杆通过圆柱体包络法包络，对应的圆柱体的半径依次记为ri(i＝1,2,3,4,5,6)，i为连杆序数，障碍物的包络半径记为rz，通过式(1)、(2)确定机械臂各连杆和障碍物的轴线方程，计算各连杆与障碍物的轴线间距diz(i＝1,2,3,4,5,6),当diz》rz ri(i＝1,2,3,4,5,6)时，不会发生碰撞；当diz≤rz ri(i＝1,2,3,4,5,6)时，会发生碰撞；
[0050]
长方体的数学模型可描述为：
[0051]
c＝{(x,y,z)|mx≤x≤mx,my≤y≤my,mz≤z≤mz}(3)
[0052]
式中的mx，my，mz分别代表长方体边长沿坐标轴x、y、z方向的最小坐标值，mx、my、mz分别代表长方体边长沿坐标轴x、y、z方向的最大坐标值；
[0053]
所述一种六自由度工业机械臂避障实验控制方法，其中检测机械臂与长方体是否碰撞的方法为：机械臂各连杆通过圆柱体包络法包络，对应的圆柱体的半径依次记为ri(i＝1,2,3,4,5,6)，i为连杆序数，选择最大的ri作为最大的半径rmax，然后将最大半径rmax加入到长方体障碍物的长宽高中使其膨胀化，通过正运动学算法推导出每个连杆末端的坐标，然后确定机械臂各连杆的直线向量与长方体各平面的法向量是否垂直，如果垂直则直线与法向量不存在交点，即与对应长方体面无交点，如果不垂直判断交点是否在平面上，如果交点在平面上则相撞，如果不在则不相撞；
[0054]
球体包络法的数学模型可表示为：
[0055]
globe＝{(x,y,z)|(x-x0)2 (y-y0)2 (z-z0)2≤r}(4)
[0056]
其中r为球体的半径，(x0,y0,z0)为球体的中心点坐标；
[0057]
所述一种六自由度工业机械臂避障实验控制方法，其中检测机械臂与球体是否碰撞的检测方法为：机械臂各连杆通过圆柱体包络法包络，对应的圆柱体的半径依次记为ri(i＝1,2,3,4,5,6)，选择最大的ri作为最大的半径rmax，通过机器人正运动学算法求出各由式(1)、(2)确定机械臂各连杆对应的圆柱轴线方程，计算球体中心点到圆柱轴线的距离di(i＝1,2,3,4,5,6)，如果d》rmax r，则机械臂与球体不会碰撞，如果d≤rmax r，则说明机械臂与球体会碰撞。
[0058]
所述功能的实现流程如图6，首先将需要验证的避障算法写入控制台(3)；通过控制台(3)输入机械臂(1-2)的初始位姿和目标位姿，规划一条机械臂的初始路径穿过障碍物；通过控制台(3)控制驱动电机(1-8)启停，驱动障碍物摆放台(1-5)的旋转；安装于机械臂(1-2)关节的传感器(1-1)发射信号构建工作环境及障碍物信息；通过构建的工作环境判
断障碍物(1-4)的位置，然后由控制台(3)进行避障算法的处理，验证避障算法的效果；针对实际的避障效果对避障算法进行更改与调整。
[0059]
本实施例根据常用的碰撞检测包络方法确定障碍物(1-4)形状，从而对避障算法的可行性进行准确地验证；通过在障碍物摆放台(1-5)上不同位置设计了二十四个插孔，使用伸缩杆(1-4-6)用来支撑障碍物，并且在伸缩杆(1-4-6)末端增加了可旋转的结构，从而可以达到在机械臂(1-2)的运动空间内对不同位置、不同姿态的障碍物进行测试的功能；通过设计驱动电机(1-8)驱动障碍物摆放平台(1-5)旋转的功能，可以测试三种不同情况下机械臂(1-2)的避障效果，即机械臂(1-2)运动而障碍物(1-4)静止、机械臂(1-2)静止而障碍物(1-4)运动、机械臂(1-2)与障碍物(1-4)同时运动的情况，可以模拟实际环境中不同的避障情景；通过控制台(3)控制机械臂动作，规划指定路径，返回避障数据，可以更清楚地显示实验结果，从而使测试结果更加可靠。
[0060]
本实施例通过在现有的技术基础上对机械臂避障实验装置的结构和控制方法进行了综合优化，具有较好的适用性和可操作性，弥补了机械臂避障测试领域的相关空缺，其具有较大的科研价值和实用价值。