专利名称：基于fpga的数字化关节片上控制系统的制作方法
技术领域：
本发明涉及机器人领域，具体涉及一种基于FPGA的数字化关节片上控制系统。
背景技术：
机器人的发展已从过去的工业机器人领域延伸到服务机器人和空间机器人领域， 新一代机器人的设计目标是质量轻、输出力矩大、具有多种感知和自学习能力，这样的特点 不可避免地增强了控制任务的复杂性，对关节伺服控制系统的设计提出了更加苛刻的要 求。电机是关节伺服控制系统的执行部件，随着微电子技术和电机控制的快速发展，电机的 控制经历了模拟控制阶段、基于微处理器的控制阶段、基于DSP+FPGA的控制阶段以及基于 FPGA控制的四个阶段。模拟控制方案尽管采用基于微处理器的数字控制策略，然而脉宽调制PWM信号的 产生、电流环的调节，仍由模拟电路实现，该方案的优点是响应快，但存在着抗干扰能力低、 占用的空间较大、电路复杂、功能单一等缺点；基于DSP的控制方案具有电路简单、软件控 制、灵活性高的优点，可适应各种场合，然而脉宽调制PWM信号的产生、电流环的实现需要 更高的采样率来获得更大的带宽；基于DSP和FPGA的混合控制方式，具有灵活性更高，功能 更强的优点，然而整个系统复杂，开发缓慢；内嵌微处理器的FPGA控制方案实现了真正的 片上系统，提供了可靠性更高、性能更强、开发快捷的控制方案。目前机器人关节一般采用基于DSP和FPGA的混合控制方式实现关节信息采集和 控制，但其系统复杂，集成度低，且开发周期长。
发明内容
为了解决现有的机器人关节控制系统集成度低的问题，本发明提供了一种基于 FPGA的数字化关节片上控制系统。本发明的基于FPGA的数字化关节片上控制系统，它包括NIOSII软核处理器、Avalon总线模块、CAN总线软核控制器、关节电机驱动电路、关节电机电流矢量控制电路和 关节传感器信息采集SPI接口电路，CAN总线软核控制器包括第一 CAN总线软核控制器，关 节电机驱动电路包括正弦波SPWM生成电路，关节电机电流矢量控制电路包括码盘接口电 路、电机位置坐标变换电路、电机相电流采集接口电路和电机电流环调节电路；NIOSII软 核处理器的通讯端通过Avalon总线模块分别连接到第一 CAN总线软核控制器的通讯端、 关节电机驱动电路的通讯端、关节电机电流矢量控制电路的通讯端和关节传感器信息采集 SPI接口电路的通讯端，码盘接口电路的信号输出端连接电机位置坐标变化电路的信号输 入端；NIOSII软核处理器，用于上位机发出的关节动作命令的解算，实现与上位机的CAN总 线通讯协议，还用于执行解算的命令，调整控制参数，实现关节空间的轨迹规划、关节空间 控制参数的管理以及关节空间的位置、力矩和阻抗控制；CAN总线软核控制器，用于将关节 空间信息通过CAN总线接口电路传送给上位机，还用于将上位机的控制命令和数据信息通 过Avalon总线模块发送给NIOSII软核处理器；关节电机驱动电路，用于输出驱动关节电机工作的正弦波信号或方波信号；码盘接口电路，用于传输被测量的电机位置信息和转速信 息；电机位置坐标变化电路，用于对测量获得的电机位置信息进行坐标变换；电机相电流 采集接口电路，用于传输被采集的电机相电流；关节传感器信息采集SPI接口电路，用于传 输包括位置和力矩的关节空间信息。本发明的有益效果本发明提供了一种集成度高和开发周期短的基于FPGA的数 字化关节片上控制系统；本发明采用一片FPGA芯片实现了关节空间信息的采集、关节电机 驱动、关节控制和CAN总线通讯等内容；本发明在FPGA中嵌入了 NIOSII软核处理器，所述 NIOSII软核处理器通过Avalon总线模块实现了对各接口电路的访问，同时在所述NIOSII 软核处理器中实现了关节空间的轨迹规划、位置控制、力矩控制等运算处理功能；本发明的 各接口电路实现了电机的矢量控制、传感器采集等功能，实现了基于FPGA的数字化关节片 上系统。
图1是应用本发明的基于FPGA的数字化关节片上控制系统的关节伺服控制系统 结构示意图，其中，8为关节、8-1为电机、8-2为减速器、8-3为连杆、9为H桥、10为码盘信 号电路、11为电机相电流采集电路、12为传感器采集电路、13为看门狗电路、14为CAN总线 接口电路和15为霍尔信号电路，图2是具体实施方式
四中的电机位置坐标变换电路5-2的 原理示意图，图3是具体实施方式
四中的电机电流环调节电路5-4的原理示意图，图4是具 体实施方式四中的正弦波SPWM生成电路原理示意图。
具体实施例方式具体实施方式
一根据说明书附图1具体说明本实施方式，本实施方式所述的基 于FPGA的数字化关节片上控制系统，它包括NIOSII软核处理器l、Avalon总线模块2、CAN 总线软核控制器3、关节电机驱动电路4、关节电机电流矢量控制电路5和关节传感器信息 采集SPI接口电路6，CAN总线软核控制器3包括第一 CAN总线软核控制器3_1，关节电机 驱动电路4包括正弦波SPWM生成电路4-1，关节电机电流矢量控制电路5包括码盘接口电 路5-1、电机位置坐标变换电路5-2、电机相电流采集接口电路5-3和电机电流环调节电路 5-4 ；NIOSII软核处理器1的通讯端通过Avalon总线模块2分别连接到第一 CAN总线软核 控制器3-1的通讯端、关节电机驱动电路4的通讯端、关节电机电流矢量控制电路5的通讯 端和关节传感器信息采集SPI接口电路6的通讯端，码盘接口电路5-1的信号输出端连接 电机位置坐标变化电路5-2的信号输入端；NIOSII软核处理器1，用于上位机U发出的关 节动作命令的解算，实现与上位机U的CAN总线通讯，还用于执行解算的命令，调整控制参 数，实现关节空间的轨迹规划、关节空间控制参数的管理以及关节空间的位置、力矩和阻抗 控制；CAN总线软核控制器3，用于将关节空间信息通过CAN总线接口电路14传送给上位机 U,还用于将上位机U的控制命令和数据信息通过Avalon总线模块2发送给NIOSII软核处 理器1 ；关节电机驱动电路4，用于输出驱动关节电机工作的正弦波信号或方波信号；码盘 接口电路5-1，用于传输被测量的电机位置信息和转速信息；电机位置坐标变化电路5-2， 用于对测量获得的电机位置信息进行坐标变换；电机相电流采集接口电路5-3，用于传输 被采集的电机相电流；关节传感器信息采集SPI接口电路6，用于传输包括位置和力矩的关节空间信息。本具体实施方式
中，在一片FPGA上实现了关节内部传感器信息采集、电机矢量控 制算法、M/T测速算法、电机正弦波SPWM驱动控制、关节空间的轨迹规划、位置控制和力矩 控制，并在FPGA内部嵌入了 CAN总线软核控制器3，实现了与主控计算机(上位机U)的CAN 总线通讯接口，使关节控制器参数可以在线调整。本实施方式的控制系统集成度高，在FPGA 内部嵌入了 32位的MOSII软核处理器1和CAN总线软核控制器3，有效的减少了电路板的 面积，并且可以定制本具体实施方式
的控制系统需要的模块，裁减不必要的模块，提高了系 统的集成度。
具体实施方式
二 本具体实施方式
与具体实施方式
一的不同之处在于，CAN总线 软核控制器3还包括第二 CAN总线软核控制器3-2，所述第二 CAN总线软核控制器3_2的通 讯端通过Avalon总线模块2连接到NIOSII软核处理器1的通讯端。本具体实施方式
中，在FPGA中增加了第二 CAN总线软核控制器3_2，，实现了冗余 的CAN总线通讯，提高了通讯系统的可靠性。
具体实施方式
三本具体实施方式
与具体实施方式
一或二的不同之处在于本具体 实施所述的基于FPGA的数字化关节片上系统还包括看门狗接口电路7，所述看门狗接口电 路7的通讯端通过Avalon总线模块2连接NIOSII软核处理器1的通讯端，所述看门狗接 口电路7用于传输喂狗信号或狗咬信号。本具体实施方式
，在FPGA中增加了看门狗接口电路7，配合外接看门狗电路13可 以监视FPGA内NIOSII软核处理器1的运行，防止程序跑飞，提高了系统的可靠性。
具体实施方式
四本具体实施方式
与具体实施方式
一、二或三的不同之处在于关 节电机驱动电路4还包括霍尔接口电路4-2、方波RPWM生成电路4-3和多路选择开关电路 4-4，正弦波SPWM生成电路4-1的信号输出端连接多路选择开关电路4-4的一个信号输入 端，霍尔接口电路4-2的信号输出端连接方波RPWM生成电路4-3的信号输入端，所述方波 RPWM生成电路4-3的信号输出端连接多路选择开关电路4-4的另一个信号输入端。本具体实施方式
中，正弦波SPWM生成电路4-1是利用电机电流环调节电路5-4中 电流环计算的电压值与20KHz的三角波计数器进行比较实现电机驱动，当电压值比计数值 大时输出高电平，而比计数值小时输出低电平；
方波RPWM生成电路4-3直接接到霍尔接口电路4-2的信号输出端进行逻辑换相实现 电机驱动，其中脉宽调制PWM信号的输入由NIOSII软核处理器1经过计算得到，霍尔接口 电路4-2的信号输入端与霍尔信号电路15连接；两种驱动方式直接输出脉宽调制PWM信号 到多路选择开关电路4-4，所述多路选择开关电路4-4受NIOSII软核处理器1的控制，默认 条件下为将正弦波生成电路4-1的正弦波信号输出；当正弦驱动出现故障时，NIOSII软核 处理器1将多路选择开关电路4-4修改为将方波RPWM生成电路4-3的方波信号输出。本具体实施方式
在FPGA中采用方波驱动电路进行备份，提高了系统的可靠性。本具体实施方式
中，CAN总线软核控制器3是独立的IP核，被嵌入到FPGA中，实现了 CAN总线2. OB的总线协议，所述CAN总线软核控制器3将关节传感器的信息通过CAN 总线传送给上位机U,并将所述上位机U的控制命令和数据信息发送给NIOSII软核处理器 1，为提高系统的可靠性，CAN总线软核控制器3采用两个，即第一 CAN总线软核控制器3-1 和第二 CAN总线软核控制器3-2。
本实施方式中，电机相电流采集接口电路5-3是IR公司的IR2175电流采集芯片， 期望电流信息由NIOSII软核处理器1提供，所述NIOSII软核处理器1为32位的定点处理 器，电机电流环调节电路5-4采用PI调节器调节电流环。针对本实施方式，进行实例分析
在实际的控制系统中正弦波SPWM信号频率为20KHz，IR2175电流采集芯片载波信号频 率为130KHZ，在每次产生正弦波信号或方波信号时时，生成一个同步信号控制电流的采集， 在同步信号之后，当IR2175电流采集芯片输出的第二个和第三个脉冲的上升沿来时，进行 电流信号采集，将两次采集的结果平均作为实际电流值，正弦波SPWM控制信号采用规则采 样II法生成，正弦波SPWM生成电路4-1如图4所示。本实施方式中的电机位置坐标变换电路5-2如图2所示，坐标变换包括由三相 静止坐标系油C到旋转坐标系匈的正变换和匀到油C的反变换，其中正变换如下式(1)所 示
,万⑴ L = -L -sin0H--￡os0(L +2ih)
, V 22
在式(ι)中存在下面的关系<formula>formula see original document page 6</formula>
因此坐标变换电路的实现，根号下的运算采用乘法、加法和移位操作实现，而正、余弦
JT
角度计算，需利用存储在FPGA的存储器中的
正弦表的信息实现。电机电流环调节电路5-4如图3所示，电流环采用PI调节器，为防止积分饱和，采 用遇限削弱积分方法。本实施方式中，码盘接口电路5-1主要由信号滤波电路、4倍频电路以及M/T法测 速单元组成。码盘采用增量式码盘，为克服信号线上的干扰，需进行数字滤波；然后将滤波 后的信号送入四倍频电路进行倍频处理，并得到电机位置信息，用于坐标变换及电机位置 测量；同时利用四倍频的信号和M/T法测得电机转速信息。本实施方式中，关节片上控制系统与上位机U采用CAN总线通讯，通讯周期为 250ms，而关节的控制周期为2ms。因此关节片上控制系统需要将上位机U提供的关节期望 位置信息进行细分，为保证加速度的连续性，采用四阶的Paul规划实现；为实现关节空间 的位置控制、力矩控制以及阻抗控制，通过关节传感器信息采集SPI接口电路6，可获得关 节空间的位置、力矩等传感器信息，由于电机采用电流环控制，使电机等效为力矩源，因此 关节空间设计了四阶状态反馈控制器，如(2)式所示<formula>formula see original document page 7</formula>
式(2)中Pkp、Pkd分别为位置控制器调节参数；
Tkp、τΜ分别为力矩控制器调节参数；
K分别为位置误差和速度误差；
τ、τ分别为力矩和力矩微分信息。
通过各控制器参数的调节，可获得不同的控制性能，如实现关节空间的位置控制 时，将力矩控制参数％、Tfai调节为零；实现力矩控制时，则需要将位置控制参数戶#、
PM调节为零；而实现阻抗控制时，则需要综合调节位置和力矩控制参数。
权利要求
基于FPGA的数字化关节片上控制系统，其特征在于它包括NIOS II软核处理器(1)、Avalon总线模块(2)、CAN总线软核控制器(3)、关节电机驱动电路(4)、关节电机电流矢量控制电路(5)和关节传感器信息采集SPI接口电路(6)，CAN总线软核控制器(3)包括第一CAN总线软核控制器(3-1)，关节电机驱动电路(4)包括正弦波SPWM生成电路(4-1)，关节电机电流矢量控制电路(5)包括码盘接口电路(5-1)、电机位置坐标变换电路(5-2)、电机相电流采集接口电路(5-3)和电机电流环调节电路(5-4)；NIOS II软核处理器(1)的通讯端通过Avalon总线模块(2)分别连接到第一CAN总线软核控制器(3-1)的通讯端、关节电机驱动电路(4)的通讯端、关节电机电流矢量控制电路(5)的通讯端和关节传感器信息采集SPI接口电路(6)的通讯端，码盘接口电路(5-1)的信号输出端连接电机位置坐标变化电路(5-2)的信号输入端；NIOS II软核处理器(1)，用于上位机(U)发出的关节动作命令的解算，实现与上位机(U)的CAN总线通讯协议，还用于执行解算的命令，调整控制参数，实现关节空间的轨迹规划、关节空间控制参数的管理以及关节空间的位置、力矩和阻抗控制；CAN总线软核控制器(3)，用于将关节空间信息通过CAN总线接口电路(14)传送给上位机(U)，还用于将上位机(U)的控制命令和数据信息通过Avalon总线模块(2)发送给NIOS II软核处理器(1)；关节电机驱动电路(4)，用于输出驱动关节电机工作的正弦波信号或方波信号；码盘接口电路(5-1)，用于传输被测量的电机位置信息和转速信息；电机位置坐标变化电路(5-2)，用于对测量获得的电机位置信息进行坐标变换；电机相电流采集接口电路(5-3)，用于传输被采集的电机相电流；关节传感器信息采集SPI接口电路(6)，用于传输包括位置和力矩的关节空间信息。
2.根据权利要求1所述的基于FPGA的数字化关节片上控制系统，其特征在于CAN总 线软核控制器(3)还包括第二 CAN总线软核控制器(3-2)，所述第二 CAN总线软核控制器 (3-2)的通讯端通过Avalon总线模块(2)连接到NIOSII软核处理器(1)的通讯端。
3.根据权利要求1或2所述的基于FPGA的数字化关节片上控制系统，其特征在于它还 包括看门狗接口电路(7)，所述看门狗接口电路(7)的通讯端通过Avalon总线模块(2)连 接NIOSII软核处理器(1)的通讯端，所述看门狗接口电路(7)用于传输喂狗信号或狗咬信 号。
4.根据权利要求3所述的基于FPGA的数字化关节片上控制系统，其特征在于关节电 机驱动电路(4)还包括霍尔接口电路(4-2)、方波RPWM生成电路(4-3)和多路选择开关电 路(4-4)，正弦波SPWM生成电路(4-1)的信号输出端连接多路选择开关电路(4-4)的一个 信号输入端，霍尔接口电路(4-2)的信号输出端连接方波RPWM生成电路(4-3)的信号输入 端，所述方波RPWM生成电路(4-3)的信号输出端连接多路选择开关电路(4-4)的另一个信 号输入端。
全文摘要
基于FPGA的数字化关节片上控制系统，它涉及机器人领域，它解决了现有的机器人关节控制系统集成度低的问题。本发明包括NIOSII软核处理器、Avalon总线模块、CAN总线软核控制器、关节电机驱动电路、关节电机电流矢量控制电路和关节传感器信息采集SPI接口电路，NIOSII软核处理器的通讯端通过Avalon总线模块分别连接到CAN总线软核控制器的通讯端、关节电机驱动电路的通讯端、关节电机电流矢量控制电路的通讯端和关节传感器信息采集SPI接口电路的通讯端，本发明适用于机械臂关节控制器。
文档编号H02P21/00GK101819427SQ20101013437
公开日2010年9月1日 申请日期2010年3月29日 优先权日2010年3月29日
发明者倪风雷, 刘宇, 刘宏, 朱映远, 金明河 申请人:哈尔滨工业大学