system: [SYSTEM]

user: 5. 仿真实验 本文采用MATLAB 进行实验仿真，主要使用MATLAB Robotics Toolbox工具箱和优化工具箱进行机械臂避障仿真实验。首先通过MATLAB Robotics Toolbox工具箱建立两杆六自由度机械臂模型，设置两连杆的长度分别为 L 1 = L 2 = 5   cm 。连杆的最大径向半径为 R L = 0.5   cm ，设置机械臂初始位置坐标是 α 0 = [ 0 ， 0 ， 5 ] ，终止位置坐标为 α 1 = [ 0 ， 5 ， 0 ] ，通过机械臂逆运动学可将笛卡尔坐标转化为关节角度表示的形式，此外，设step = 50，即设置50个插值点对机械臂进行轨迹规划。 对MATLAB优化工具箱进行参数配置，求解程序选择ga-Geneice Algoriathm；适应度函数(Fitness function)根据式(14)编写；研究对象机械臂是六自由度，优化对象 C = [ C 1 ， C 2 ， C 3 ， C 4 ， C 5 ， C 6 ] ，故设置变量个数(number of variable)为6；种群大小(population size)设置为200；选择函数(selection function)设置为Roulette(轮盘赌算法)；杂交、突变、迁移设置采用默认值；终止条件(stopping criteria)设置迟滞代数为20代，精度为10 −6 。 5.1. 球形障碍物避障实验 5.1.1. 单个球形障碍物 设置障碍物球心坐标为 O = [ 1 ， 3 ， 2.5 ] ，障碍物半径为 R 1 = 2   cm ，即机械臂各连杆中轴线到球形障碍物中心点之间的最小距离 d c s 大于安全距离 S = R L + R 1 = 2.5   cm 时，机械臂不会发生碰撞。遗传算法迭代过程中最优适应值和平均适应值的迭代过程及最优参数C值如图10所示，经过遗传迭代42次，得出最优参数 C = [ 19.1339 ， − 43.6603 ， 32.5593 ， 30.3290 ， − 2.0237 ， − 15.4773 ] 。最优适应值 f C = − 0.0383 ，连杆L 1 运动轨迹的长度 f L 1 = 7.1709   cm ，末端执行器运动轨迹的长度 f L 2 = 9.3944   cm ，各个关节转动总量 f Q = 9.5296   rad 。机械臂运动轨迹如下图11所示，其中曲线s1、s2分别表示连杆L 2 和连杆L 1 运动轨迹。 机械臂两连杆运动过程中距障碍物球心之间的距离变化过程如图12所示，其中曲线d1、d2分别表示连杆L 1 、L 2 中心线距球形障碍物中心点的距离变化过程。连杆1距球形障碍物中心点的最小距离 d c s 1 = 3 .431   cm > 2.5   cm ，连杆2距球形障碍物中心点的最小距离 d c s 2 = 2.504   cm > 2.5   cm ，故机械臂运动过程中与障碍物之间的距离均大于安全距离，机械臂不会发生碰撞。 机械臂轨迹规划过程中角度、角速度、角加速度变化过程分别如图13(a)~(c)所示，由图可知，机械臂各个关节的角速度、角加速度均连续、平滑；避免了机械臂自身的冲击和震动。因此，规划出一条无碰撞运动轨迹的同时对连杆运动轨迹长度以及关节转动角进行协同优化的理性运动轨迹。 图10. 遗传算法迭代过程 图11. 机械臂运动轨迹图 图12. 两连杆距球形障碍物中心点距离变化过程 图13. 机械臂关节空间轨迹规划图 5.1.2. 多个球形障碍物 改变机械臂始末位置，其他条件不变，设机械臂起始位置 α 0 = [ 0 ， 5 ， 0 ] ，终止位置 α 1 = [ − 3 ， 4 ， 6 ] ，两球形障碍物的球心坐标为 O 1 = [ 0.5 ， 3 ， 3 ] ; O 2 = [ 2 ， − 3 ， 2 ] ；两障碍物半径大小均为 R O 1 = R O 2 = 2   cm ，故机械臂各连杆中轴线到障碍物中心点的最小距离 d c s 大于安全距离 S = R L + R O 1 = 2.5   cm 时，机械臂不会发生碰撞。遗传算法迭代过程中最优适应值和平均适应值的迭代过程以及最优参数C值如图14所示，经过遗传迭代21次，得出最优参数 C = [ 7.400 ， 24.5883 ， − 14.8077 ， 31.1375 ， 7.4239 ， − 5.9574 ] 。最优适应值 f C = − 0.0 203 ，连杆L 1 运动轨迹的长度 f L 1 = 14 .3200   cm ，末端执行器运动轨迹的长度 f L 2 = 18 .3640   cm ，各个关节转动总量 f Q = 16 .5967   rad 。机械臂运动轨迹如下图15所示，其中曲线s1、s2分别表示连杆L 2 和连杆L 1 运动轨迹。 图14. 遗传算法迭代过程 图15. 机械臂运动轨迹 机械臂两连杆运动过程中距两障碍物球心之间的距离变化过程如图16所示，其中曲线d11、d12分别表示障碍物1球心距连杆L 1 、L 2 中轴线的距离变化过程，曲线d21、d22分别表示障碍物2球心距连杆L 1 、L 2 中轴线的距离变化过程。可知，连杆L 1 、L 2 距离球心障碍物1中心点的最小距离分别为 d c s 11 = 2.999   cm 、 d c s 12 = 2.514   cm ；连杆L 1 、L 2 距离球形障碍物2中心点的最小距离分别为 d c s 21 = 2.834   cm 、 d c s 22 = 3.472   cm ，故机械臂在运动过程中，连杆到障碍物的距离均大于安全距离S，机械臂不会与障碍物发生碰撞。 图16. 两连杆与球形障碍物之间距离变化过程 机械臂轨迹规划过程中角度、角速度、角加速度变化过程分别如图17(a)~(c)所示，同样机械臂各个关节的角速度、角加速度均连续、平滑。 图17. 机械臂关节空间轨迹规划图 5.2. 圆柱形障碍物避障实验 保持其他条件不变，设障碍物两端点坐标分别为 T = [ 2.5 ， 2.5 ， 2.5 ] ； H = [ − 2.5 ， 2.5 ， 2.5 ] 。圆柱体障碍物半径 R C = 0.7 。故机械臂连杆中轴线到障碍物圆柱体中轴线之间的安全距离为 S = R L + R C = 1.2   cm ，即连杆与障碍物圆柱体间最小距离 d c c > S 时，机械臂不会发生碰撞。遗传算法迭代过程中最优适应值和平均适应值的迭代过程以及最优参数C值如图18所示，经过遗传迭代107次，得出最优参数 C = [ 19.1895 ， − 27.2539 ， 49.5119 ， − 11.76086.6817 ， 15.3590 ] 。最优适应值 f C = − 0.0 3979 ，连杆L 1 运动轨迹的长度 f L 1 = 7 .1935   cm ，末端执行器运动轨迹的长度 f L 2 = 8 .0961   cm ，各关节转动总量 f Q = 9 .8402   rad 。机械臂动轨迹如图19所示，其中曲线s1、s2分别表示连杆L 2 和连杆L 1 运动轨迹。 图18. 遗传算法迭代过程 图19. 机械臂运动轨迹 机械臂两连杆中轴线距圆柱体障碍物中轴线间的距离变化如图20所示，其中曲线d1、d2分别表示连杆L 1 、L 2 中心线距圆柱体障碍物中轴线的距离变化过程，连杆L 1 距障碍物中轴线距离的最小值 d c c 1 = 2.662   cm > 1 .2   cm ，连杆L 2 距障碍物中轴线距离的最小值 d c c 2 = 1.806   cm > 1.2   cm ，故机械臂运动过程中与障碍物间距离均大于安全距离，机械臂不会发生碰撞，验证得到算法在存在圆柱体障碍物时，进行避障轨迹规划同样有效。 图20. 两连杆距圆柱体障碍物中轴线的距离变化过程 机械臂轨迹规划过程中角度、角速度、角加速度变化过程分别如图21(a)~(c)所示，同样机械臂各个关节的角速度、角加速度均连续、平滑。 图21. 机械臂关节空间轨迹规划 5.3. 不同障碍物同时存在的避障实验 保持机械臂条件不变，验证球形障碍物和圆柱体障碍物同时存在时算法的有效性，设球形障碍物中心点 O 0 = [ 2.5 ， 3 ， − 3 ] ，半径 R 0 = 1   cm ；圆柱体障碍物两端坐标分别为 T = [ 2.5 ， 2.5 ， 2.5 ] 、 H = [ − 2.5 ， 2.5 ， − 2.5 ] ，半径 R 1 = 0.7   cm 。故机械臂与球形障碍物的安全距离 S 1 = 1.5   cm ；与圆柱体障碍物的安全距离 S 2 = 1.2   cm 。遗传算法迭代过程中最优适应值和平均适应值的迭代过程以及最优参数C值如图22所示，经过遗传迭代33次，得出最优参数 C = [ 17.8894 ， − 26.8963 ， 49.7783 ， − 10.1727 ， 3.3489 ， − 14.1221 ] 。最优适应值 f C = − 0.0 315 ，连杆L 1 运动轨迹的长度 f L 1 = 10 .3164   cm ，末端执行器运动轨迹的长度 f L 2 = 12 .1008   cm ，各个关节转动总量 f Q = 9 .3382   rad 。机械臂运动轨迹如下图23所示，其中曲线s1、s2分别表示连杆L 2 和连杆L 1 运动轨迹。 机械臂两连杆距障碍物中心的距离变化过程如图24所示，其中曲线dc1、dc2分别表示连杆L 1 、L 2 距圆柱体障碍物中轴线的距离变化过程；曲线ds1、ds2分别表示连杆L 1 、L 2 距球形障碍物中心线的距离变化图。可知，连杆L 1 、L 2 距圆柱体障碍物的最小距离分别为 d c c 1 = 1.205   cm 、 d c c 2 = 1.354   cm ，两者均大于安全距离S 2 ；连杆L 1 、L 2 距球形障碍物的最小距离分别为 d c s 1 = 1.545   cm 、 d c s 2 = 1.579   cm ，两者均大于安全距离S 1 。故机械臂与障碍物之间不会发生碰撞。 图22. 遗传算法迭代过程 图23. 机械臂运动轨迹 机械臂轨迹规划过程中角度、角速度、角加速度变化过程分别如图25(a)~(c)所示，同样机械臂各个关节的角速度、角加速度均连续、平滑。 综上，经过MATLAB中的多组仿真实验，验证了该算法在多个障碍物和多种障碍物的环境中进行避障轨迹规划的有效性，并且在完成避障的同时，对机械臂各个关节角度及连杆轨迹长度进行优化，规划出机械臂各个关节角速度、角加速度均连续的轨迹，故概算法满足了实际作业的中的高效、稳定、节能的要求。 图24. 两连杆距障碍物距离变化过程 图25. 机械臂关节空间轨迹规划