基于FPGA的串联机器人控制系统设计
2015-05-11张道德宋成龙
张道德, 尹 洋, 宋成龙
(湖北工业大学机械工程学院, 湖北 武汉 430068)
基于FPGA的串联机器人控制系统设计
张道德, 尹 洋, 宋成龙
(湖北工业大学机械工程学院, 湖北 武汉 430068)
通过Pro/Enginee构建5自由度机械手硬件机械结构模型,设计基于FPGA嵌入式处理器的NiosII的SOPC架构系统的软件平台,以及FPGA控制器与步进电机驱动模块,并分析机械臂运动学的正解、逆解问题,通过VC、MATLAB等工具实现机械臂的最优解的选取、轨迹规划的仿真,以及运用Robotics Toolbox工具箱进行正逆运动学仿真,验证算法可行性和求解的正确性。
现场可编程逻辑门阵列; 机器人控制; 机械臂
机械臂的控制方式主要有3种方案:第一种是以PC机作为硬件平台,利用VC软件实现机械臂的算法控制,PC的硬件配置要求较高;第二种是PC机和运动控制卡的结合方案,控制卡安装在PC机中,处理器完成部分控制算法,主要任务由运动控制卡完成,控制系统比较稳定;第三种是机器人控制模块与PC机的结合,模块化具有高效、可移植性好的特点,但价格昂贵。本文提出的控制方式是通过FPGA作为硬件平台,在FPGA 里完成算法控制,将控制结果输出到驱动模块,实现机械臂的实时控制。由于FPGA具有高效、并行执行等特点,对机器臂的控制具有很好的效果。
1 硬件结构模型的构建
1.1 机械臂模型建立
5自由度机械臂的机械结构系统组成包括:机身、大臂、小臂、手腕和末端操作器(手抓)等5部分。机械臂机械结构模型见图1。
图1 机械臂机械结构模型
机械臂由若干关节和连杆串联组成,采用D-H参数法来确定每个关节上的坐标姿态和相邻两个坐标系之间的相对平移距离和旋转角度[1]。机器人各关节坐标关系及机构简图如图2所示。D-H参数由连杆长度a、连杆扭角αi、连杆距离di和连杆转角θi构成。D-H参数如表1所示。
图2 机器人坐标关系
表1 机械臂D-H参数
连杆di/cma/cmαi/(°)θi/(°)i=100-900~135i=20100-90~90i=309.50-90~90i=403.2-90-90~90i=514.500-90~90
1.2 串联机械臂工作空间的分析和仿真
机械臂的工作空间是机械臂的手腕中心或末端操作器所能达到空间点位置和合集,它是用于衡量串联机械臂工作能力的重要指标。其计算十分复杂多样,主要有解析法、图解法、数值法和仿真法,其中较常用的方法是数值法中的蒙特卡洛法,采用MATLAB中SimMechanics工具箱进行工作空间的分析计算,仿真实现起来更快捷、方便。利用机器人坐标关系图和D-H参数建立SimMechanics模型,设置各关节转动驱动,分别是角度、角速度、角加速度[2]。SimMechanics模型见图3,仿真结果见图4。
图3 SimMechanics模型
图4 机械臂工作空间三维图
2 系统方案的设计
2.1 系统工作流程
5自由度串联机械臂控制系统方案比较复杂。数据采集模块采集到起始位姿和终止位姿数据后,通过串口发送到FPGA 内部,在FPGA内部实现运动学的逆解和解的最优化,然后进行轨迹规划,在将指令发送到脉冲驱动模块驱动机械臂(图5)。
图5 系统工作流程图
由于在FPGA内部采用Verilog HDL实现运动学逆解及其最优化,矩阵运算等运算非常复杂,因此采用FPGA 内部的Nios II软核处理器C语言处理复杂运算,脉冲驱动模块等简单运算可以运用Verilog HDL硬件语言实现。两种方式相互结合会使方案实现起来可行性更高。
2.2 SOPC系统的构建
SOPC(System On a Programmable Chip)能把整个可编程系统集成到一块硅片上,是一种特殊的嵌入式系统。NIOS II 是一个用户可配置的通用32位RISC嵌入式软核处理器,它是SOPC片上可编程系统的核心。运用Quartus软件开发工具的SOPC Builder可以很方便地定义一个我们需要的系统。SOPC系统的构建首先需要定义软核时钟100 MH,接着需要建立Nios IIProcessor,主要包括CPU模块、EPCS控制器、SYSTEM ID 、JTAG UART,这是SOPC系统最基本组成部分。依据系统需要,还要构建PIO口用于脉冲输出,RS232口用于数据接收,还需构建一个PLL锁相环对时钟倍频。系统构建的SOPC系统如图6所示。系统构建的Nios II的RTL视图如图7所示。
图6 SOPC系统
2.3 自定义Avalon-MM总线
Avalon总线由ALTERA公司提出,用于在基于FPGA的片上系统中连接片内处理器和片内外设的总线结构。 连接到Avalon总线的设备分为主从设备,并各有其工作模式。Avalon总线的读/写时序是指外设之间产生的一次读/写数据总线接口各个信号之间的时序关系。在clk的第一个上升沿,Avalon总线传递address,byteenable_n和read_n信号到目标外设Avalon总线模块内部对address进行译码,产生片选并驱动从端口的chipselect信号,从而输出readdata[3]。从组件的基本读时序如图8所示。
图7 系统RTL视图
图8 从组件的基本读时序
由于脉冲输出模块没有复杂的运算,因此运用Avalon-MM总线自定义一个外设模块具有更高的运行效率。构建的脉冲输出模块的RTL视图如图9所示,并且通过Modelsim仿真软件验证了其正确性(图10)。
图9 脉冲输出模块的RTL视图
2.4 机械臂运动学逆解及优化
机器人运动学的逆向求解问题是指已知机器人末端操作器(手部)要到达的目标位姿的情况下,通过坐标逆变换来求出所需的各个关节变量平移和旋转值,以驱动各关节的伺服电机或步进电机旋转,使手部的位姿得到满足.这就是机器人的反向运动学问题。
图10 脉冲输出模块仿真波形
依据表1 机械臂D-H参数,求解机械臂的整体坐标变换矩阵公式如下
T50=T1·T2·T3·T4·T5=
(1)
求解机器人的运动学方程:
(2)
其中,-3.2、0和34分别代表机械臂的末端操作器相对于固定坐标系的x轴、y轴和z轴三坐标轴的平移量。
通过解析法可以求得机械臂的逆向运动学方程,最终求得每个转角的值。已知运动学方程的位姿矩阵
(3)
且T50=T1T2T3T4T5,通过T1、T2、T3、T4和T5可以求出T1-1、T2-1、T3-1、T4-1和T5-1分别为
(4)
(5)
(6)
(7)
(8)
通过算运,可以分别求出机械臂每个电机的转角,但是由于有反三角函数存在,因此运算存在多解情况。
5自由度机械臂的逆向运动学共有8组解(图11),但是由于机械臂实际存在约束条件,有些解不在末端操作器工作域内,所以5自由度机械臂的最后运动情况要根据真实情况和最优解的选择来决定[1]。
图11 5自由度机械臂逆解
需要让每个步进电机耗能最小,则选用距离最优的算法。该方法使机械臂的每个关节运动量最少。建立的最优解目标函数F1和F2,F1是优化关节1、关节2和关节3的目标函数,F2是优化关节4和关节5的目标函数,如
其中,ai(i=1,2,3)是D-H参数中的连杆长度,θi(i=1,2,3)和θi(i=4,5)是D-H参数中的关节角。目标函数F1体现了一种加权思想,而目标函数F2直接取关节4和关节5的两个关节角的平均值[1]。在实际应用中,机械臂运动学逆解最优化方法很多,一般有时间最优、功率最优、距离最优、避障和受力等。应该根据实际情况选择,并建立最优化函数。
图12 逆解最优化流程图
将运动学逆解和最优化程序整合后,在VC中仿真的效果如图13所示。首先逆解出8个角后排出不合理的角度,然后通过距离最优得到5个最优解。由于NIOSII编辑环境是C语言,可以将VC的仿真程序直接移植到NIOSII编辑环境中。
图13 距离最优化结果
Mtalab下的RoboticsToolbox可以对其进行仿真和验证,Ikine()函数作用是求解逆向运动学,函数调用格式如下
(9)
式中:ROBOT为一个机器人对象,T为要进行反解的变换矩阵,Q是初始猜测关节角(默认全是0),M是忽略某个关节自由度。MATLAB程序及结果见图14。
其结果可见,MATLAB仿真得到的结果与VC程序得到的5个输出角度值一致。
2.5 轨迹规划的仿真
轨迹规划是工业机器人的控制基础,其控制目标是平稳、快速和精确实现所规划的轨迹运动,否则将对机械臂的机械结构部件造成一定的磨损,并可能导致机械臂运动时的震动和冲击,最终影响机械臂的运动精度和定位精度。
图14 5R机械臂逆解程序及结果
本文主要讨论连续路径的无障碍轨迹规划方法[6]。主要方法有三次多项式插值法、五次多项式法和用抛物线过渡的线性插值法。而用抛物线过渡的线性插值法更加常用。时间t在不同的时间段中求出的关节位置函数
保证ta有解,则加速度a的值要选择的足够大,要满足
(10)
其中:q0、qa、qh、qb-a和qb分别是对应t0、ta、th、tb-a和tb时间点的关节位置。
运用MATLAB进行仿真,通过抛物线过渡的线性插值法,得到结果见图15。
图15 机械臂关节角位置、角速度和角加速度曲线
3 系统PCB板设计
3.1 机械臂FPGA控制系统PCB板设计
一般FPGA最小系统主要包括FPGA芯片、外部时钟、下载电路、复位电路和电源[4]。如果需要使用NIOSII软嵌入式处理器,还需要包括SDRAM和Flash这些组件。本系统选用的FPGA芯片为比较成熟的CycloneII芯片EPM2C8Q208C8。除了基本模块外,还添加了串口通信模块来完成硬件与PC机的通信,进行人机交互。系统原理图见图16。
图16 机械臂Fpga控制系统原理图
3.2 步进电机驱动板设计
由于机械臂由步进电机带动,因此选择稳定、高效的步进电机驱动方案十分重要。步进电机总转动角度由输入脉冲数决定,转速由脉冲信号频率决定,具有控制简单、惯量低、定位精度高等特点。TB6560芯片是东芝推出的低功耗、高集成两相混合式步进电机驱动芯片[8],集成双全桥MOSFET驱动,单相输出最大电流3.5A(峰值),有多种细分方式,具有自动断开输出、过流保护等功能。
电机以TB6560为核心芯片驱动电路,附加的电路有光电隔离电路外部接口电路、自动半流电路,以及细分电路等组成。其中隔离电路采用两片4N25高速光耦和一片PC817普通光耦。电机控制信号隔离电路如图17所示。PCB图如图18所示。
图17 电机控制信号隔离电路
图18 电机驱动PCB
4 结束语
本文构建了机械臂三维模型以仿真机械臂的工作空间,并且提出了更加方便、高效的机械臂控制方案,构建了SOPC控制系统,分析并仿真了机械臂运动学的逆解、优化和轨迹规划等问题,最后设计出了FPGA的控制版和步进电机驱动板。本文较为完整地设计了串联机器人的控制方案,并验证了其可行性。
[1] 王 强.基于FPGA的5自由度工业机械臂控制系统研究[D].武汉:湖北工业大学,2012.
[2] 许卫斌,平雪良.6R型串联机器人控制系统的软件实现[J].中国制造业信息化,2012,41(17):60-63.
[3] 刘福奇.VerilogHDL设计与实战[M]北京:北京航空航天大学出版社,2012.
[4] 华清远见嵌入式培训中心.FPGA应用开发入门与典型实例[M].北京:人民邮电出版社,2008.
[5] 吴厚航.爱上FPGA开发——特权和你一起学NIOSII[M].北京:北京航空航天大学出版社,2011.
[6] 韩建海.工业机器人[M]. 第二版.武汉:华中科技大学出版社,2014.
[7] 周国义,谢明红.6 自由度解耦机器人运动学逆解优化的研究[J].机电产品开发与创新,2009(05):21-23.
[8] 卢 贶,宋 霞.基于TB6560 步进电机驱动系统的设计[J].武汉船舶职业技术学院学报,2013(04):36-41.
[责任编校: 张 众]
Design of Robot Control System Based on FPGA
ZHANG Daode,YIN Yang,SONG Chenglong
(SchoolofMechanicalEngin.,HubeiUniv.ofTech.,Wuhan430068,China)
Through PROE robot hardware, the study first constructed the five degrees of freedom mechanical structure model, and designed the software platform based on FPGA embedded processor NiosII of SOPC framework, and FPGA controller and stepper motor driver module. It then analysed the positive and inverse problem solutions of manipulators’ kinematics. It also completed the selection of the optimal solution of manipulators by VC, MATLAB and other tools and trajectory planning simulation. It finally used the Robotics Toolbox Toolbox in positive and inverse kinematics simulation to verify the feasibility of the algorithm and the correctness of the solutions.
FPGA;NIOS II;MATLAB; manipulators
2014-09-10
张道德(1973-), 男, 湖北黄梅人,工学博士,湖北工业大学教授,研究方向为智能控制,图像识别
1003-4684(2015)01-0059-06
TP391
A