利用DSP实现工业机器人关节电机绝对位置测量

强艳辉，杨 侃

(海军装备研究院，北京 100161)

利用DSP实现工业机器人关节电机绝对位置测量

强艳辉，杨 侃

(海军装备研究院，北京 100161)

文章提出了一种机器人关节伺服电机绝对位置获取方法，适用于关节伺服系统由电机与相配套伺服驱动器组成的结构。采用以DSP芯片TMS320F1812为核心的运动控制卡实现并运用如下位置获取策略：系统初始化阶段，运动控制卡接收伺服驱动器传送的初始绝对位置串行数据；运行阶段，TMS320F1812内嵌QEP解码电路计数增量脉冲获取增量位置，并利用初始绝对位置，计算当前实时绝对位置。针对通过脉冲计数方向标志字判别电机转向存在的缺陷，提出新型转向判别方法及其适用条件，给出了以该判别方法为基础的绝对位置计算方法。文中对运动控制卡位置获取模块的软硬件设计进行了详细介绍，并通过实验证明该位置测量系统能够满足工业机器人的绝对位置的测量需求。

机器人；绝对位置；运动控制卡

0 引言

工业机器人根据关节绝对位置计算末端执行器的空间位置，并利用对位置的微分计算电机的速度、加速度以实施闭环控制。因此，实时准确获取关节电机绝对位置是工业机器人一个必要的、重要的技术。

运动控制卡是机器人获取关节电位位置的关键，诸多研究专注于此。相当一部分运动控制卡只能接收增量式编码器信号，获取电机相对位置[1-3]。这类提供相对位置数据的运动控制卡应用于工业机器人时，为保证作业精度，需在运行前实施机械回零到达基准位置。回零操作可能导致机器人与周围物体碰撞，极大的限制了其灵活性和安全性。因此，大多数现代工业机器人关节伺服系统由安装绝对式编码器的电机与配套伺服驱动器组成，由绝对编码器提供绝对位置数据。绝对位置获取的最佳方式是利用运动控制卡直接解码电机绝对编码器输出的位置信息[4-6]。然而，大多数电机的绝对编码器信息通过专用协议传输至伺服驱动器，再由伺服驱动器输出，这使得只能依赖于接收伺服驱动器的输出获取关节电机的绝对位置。针对这种形式的机器人，通常采用专用运动控制卡获取绝对位置。比如：利用PMAC运动控制卡加装专用附件接收伺服驱动器输出实现绝对位置的获取[7-9]。其缺点在于附件价格昂贵，成本较高，并且受限于专用运动控制卡(诸如PMAC卡、PCI-208[10]卡等)的软硬件限制，控制算法的灵活性较差。另外一种方式即采用自研运动控制卡实现机器人绝对位置的获取[11]，不足之处在于多种微处理器及逻辑芯片的使用增加了系统的复杂度。

鉴于上述学者的工作及相关分析，针对绝对式编码器电机与伺服驱动器构成的关节伺服系统，提出了一种以DSP芯片(TMS320F1812)为核心的单芯片运动控制卡，充分利用了DSP芯片的丰富外设，实现了对伺服驱动器位置输出信息的读取。所设计的运动控制卡结构简单，绝对位置计算、电机控制等核心算法均在DSP芯片中完成，且可以依据需要灵活调整。特别适用于工业机器人的关节电机的控制和绝对位置的获取。

运动控制卡计数增量脉冲信号计算绝对位置需判别电机转向，诸多学者[12-14]利用计数器状态标志判别转向。本文基于实验，分析并验证了此种方式的固有缺点，提出了无需计数器状态标志的电机转向判别方法，给出了其适用条件和绝对位置计算公式。

1 工业机器人基本控制结构及绝对位置获取策略

1.1 工业机器人基本控制结构

机器人关节驱动采用成熟伺服系统，本文以安川Σ-II系统伺服系统为例说明。其中机器人关节控制器包括运动控制卡(完成电机位置读取、关节插补算法、位置或速度闭环控制算法、控制信号的输出等)及伺服驱动器(Σ-II系统服驱动器)。

1.2 绝对位置获取策略

依据图1所示工业机器人控制器的特点，设计绝对位置获取策略。安川Σ-II系列伺服电机配装绝对编码器，绝对位置信息传输至伺服驱动器。由于传输协议的封闭性，运动控制卡无法直接从电机接收绝对位置，只能通过伺服驱动器的输出获取绝对位置信息。

图1 机器人控制器示意图机器人关节驱动采用

伺服驱动器全时以串行数据形式输出绝对位置(见图2、表1，PSO，其输出周期为约40ms)，运行过程中输出正交编码脉冲(QEP)增量信号，即位置增量信号(见图2、表1中PAO、PBO)。

图2 伺服驱动器位置输出示意图

表1 伺服驱动器各信号含义

通常，运动控制卡的伺服周期在几十微秒到几十毫秒之间，而伺服驱动器串行总线绝对位置发送周期至少为40ms。所以，系统运行阶段通过串口实时获取关节电机的绝对位置不现实、不可行，无法满足实时计算要求。

针对伺服驱动器位置输出的方式及特点，设计如下绝对位置获取策略：

(1) 机器人控制器初始化阶段，运动控制卡接收串口发送的关节电机绝对位置；

(2) 机器人控制器运行阶段，运动控制卡对伺服驱动器输出的正交编码脉冲计数，获取相对位置，并计算电机当前绝对位置。

2 运动控制卡绝对位置获取模块硬件设计

如1.2节所述绝对位置获取策略，需运动控制卡既具备串行数据接收能力，又具备对正交编码脉冲计数的能力。本节设计了以TMS320F2812为核心的运动控制卡，满足位置获取策略的硬件需求。以下给出位置获取模块的硬件设计。

2.1 TMS320F2812与绝对位置获取相关资源介绍

运动控制卡位置获取硬件应该包括两部分：串行接口(绝对位置)及QEP电路(增量脉冲信号)。如图3所示，TMS320F2812具有两路串行接口(SCIA、SCIB)，两路正交编码脉冲(QEP)解码电路和捕获单元共用输入接口，分别为CAP1/QEP1、CAP2/QEP2、CAP3/QEPI1(EVA)，CAP4/QEP4、CAP5/QEP5、CAP6/QEPI2(EVB)。通过设置相应捕获单元控制寄存器使能QEP解码电路而禁止其捕获功能，实现对正交编码脉冲信号的计数。TMS320F2812的硬件特点恰好可满足绝对位置获取的硬件需求。

图3 TMS320F2812与绝对位置获取相关资源介绍

TMS320F2812的QEP电路对伺服驱动器输出正交编码脉冲信号(PAO、PBO)进行计数。正交编码脉冲、定时器计数脉冲及计数方向时序逻辑如图4所示。两列正交输入脉冲两个边沿都被正交编码脉冲电路计数，因此，产生的时钟频率(CLK)是输入序列的四倍。正交编码脉冲的方向检测逻辑检测出两个脉冲哪一个是先导序列，产生方向信号(DIR)作为定时器的计数方向输入，使定器工作在增减模式下对输入脉冲计数。

图4 正交编码脉冲、计数脉冲及方向信号

2.2 位置获取电路硬件设计

本文侧重于绝对位置的测量，故只给出运动控制卡位置获取部分的硬件设计，其余电路未在本文展示。如图5所示，主要包括：数字信号处理器芯片TMS320F2812、电平转换电路(TTL电平转为5V电平)、差分接收电路及接插件(与伺服驱动器线缆连接)。

TMS320F2812作为系统核心，完成串行数据的接收、QEP信号的计数及实时绝对位置的计算。

伺服驱动器位置输出信号有四路，即PAO、PBO、PCO、PSO(具体含义见表1)，其中PAO、PBO、PCO三路信号经74LVTH245电平转换后由SN75175接收，送至DSP的 CAP4/QEP4、CAP5/QEP5引脚，由DSP的QEP电路完成对伺服驱动器正交脉冲信号的计数及方向处理；PSO信号经74LVTH245电平转换后由SN75175接收，送至DSP的串行接口(SCIRXB)，用于系统初始化时刻接收串行格式的绝对位置数据。

由图5可知，运动控制卡取模块充分利用了DSP芯片(TMS320F2812)的丰富外设，实现了串行数据的接收、QEP信号的计数。外围电路仅采用了电平转换电路及接插件等，整个系统电路结构较为简单。

图5 位置获取电路硬件结构设计原理图

3 软件设计

3.1 通过计数器方向标志位判别转向存在的问题

伺服驱动器输出正交编码脉冲(PAO、PBO)至TMS320F2812的CAP4/QEP4、CAP5/QEP5引脚。经DSP内部译码逻辑单元后产生4倍频脉冲信号CLK和转向信号DIR(见图4)。设置定时器GPTimer4工作于定向增/减模式，对CLK信号完成计数，A相超前，增计数，B相超前，减计数。

定时器控制寄存器标志位GPTCONB-T4STAT反映了定时器4的计数状态，很多文献[12-13]通过该标志判别电机转向，使用中发现：电机由图6示箭头方向运行后停止于位置P，在负载及伺服力作用下，电机会在位置P正负几个单位编码值间抖动。程序读取GPTimer4的计数器值P后，读取方向信号，若此时恰巧位置变化为P-，则计数方向即转向判别错误，导致位置计算错误。

图6 位置示意

3.2 问题的处理

计数器溢出后归零重新开始计数，周而复始。所以，计数器可理解为环形方式工作(如图7示)，假设逆时针为增计数方向。PA为计数器前一次计数值，PB为当前计数器值。在只有一次溢出的情况下，仅存在两种可能：逆时针增计数或者顺时针减计数到达PB位置，且必然一条路径小于等于另一条路径。两条路径距离相同的情形为电机行程恰为计数器值一半。假定位置读取周期内电机行程不超过计数器最大计数值一半，较短路径方向即为电机转向。

图7 位置示意

为保证判别方法的可行性，位置采样周期须满足以下条件：

TS=T4CNT/(2R)

(1)

式中，TS为位置读取周期；

R为电机的最大转速；

T4CNT为计数器最大值。

本文采用的伺服电机被设置为转动一圈绝对编码器输出4倍频值为65536。基于上述分析，令ΔP为编码器位置，其为正值时电机正转，负值时电机反转，给出如下位置计算方式：

(1) 依据设定条件，必然发生溢出，则：

若PB>PA，电机反转，ΔP=-65536+PB-PA；

否则，电机正转，ΔP=65536-PB+PA；

(2) 否则，没有溢出：

若PB>PA，电机正转，ΔP=PB-PA；

否则，电机反转，ΔP=PB-PA。

3.3 软件实现

本节给电机转向判别及绝对位置获取的程序实现。

为使 DSP 内部 QEP 电路工作于计数方式，主程序中设置相关寄存器。通用定时器 GPTimer4 设置为定向增/减计数模式且使用正交编码脉冲电路作时钟源；使能定时器操作；同时，设置波特率等寄存器，使能SCIB。初始化完成后，通过SCIB串口接收伺服驱动器发送的串行绝对位置数据。

位置测量程序模块采用定时中断方式调用，依据位置采样周期限定条件式(1)，设置合适的采样周期值。定时中断程序完成位置读取及计算等工作。

软件包括主程序与子程序，主程序完成初始化等工作，并打开定时中断。计算绝对位置的程序作为关节控制器程序的一个函数，用C语言编写，软件流程图见图8。

图8 主程序、定时中断中位置获取及计算程序流程图

4 实验

本文所述绝对位置测量模块已应用于弧焊机器人运动控制卡中(见图9、图10)，并完成了焊接试验。

图9 应用该运动控制卡的弧焊机器人

图10 弧焊实验

运动控制卡伺服周期设置为50μs。即每隔50μs进行关节电机绝对位置计算，并完成插补算法与关节电机的运动控制。

为验证所提出的绝对位置测量方法的正确性和控制有效性，进行了机器人关节运动控制实验，并验证了绝对位置读取的正确性。

(1)接通伺服系统电源，利用研发的运动控制卡获取当前绝对位置数据，并与伺服系统PC端配套监视软件的输出结果比较；

(2)机器人运行一段时间后，利用研发的运动控制卡获取当前绝对位置数据，并与伺服系统PC端配套监视软件的输出结果比较。

两种测试结果中运动控制卡对关节电机绝对位置的实时读取值均与伺服系统PC端配套监视软件结果一致，且避免了电机保持某位置时位置抖动情况下绝对位置计算的正确性，证明了本文所述方法可以准确获得机器人的绝对位置。

本文所提出的绝对位置读取方法，相较于外购及其它自研运动控制卡，得益于DSP芯片TMS320F2812的丰富外设和强大的计算能力，伺服周期更短(仅为50μs，且可以根据需要设置为更短)，仅采用单芯片即实现了关节电机绝对位置的读取及电机的运动控制，结构简单，功能可靠。

5 总结

本文以关节伺服驱动系统为绝对编码器式电机及伺服驱动器的工业机器人控制器为例，详细阐述了以DSP芯片TMS320F2812为核心的运动控制卡对电机绝对位置的获取方法，给出了软硬件设计方案。在位置计算模块中，提出了无需计数方向标志的转向判别方法和位置计算方法，并通过实验验证了此位置获取策略的有效性。所提出的运动控制器具有以下特点：

(1)充分利用了DSP芯片 TMS320F2812的外设功能，使用单芯片构建了运动控制器，结构简单可靠，降低了系统成本；

(2)通过初始绝对位置及增量位置的获取，实现了对关节绝对位置的实时计算和获取，为机器人控制系统更好的工作奠定了基础；

(3)提出了一种判别电机转向的方法，避免了使用DSP芯片 TMS320F2812的计数标志位判别电机转向导致的错误，该方法简单可靠，并通过了实验验证。

[1] 裴九芳，程亚石. 基于DSP控制器的精锻机器人控制系统设计[J]. 工业仪表与自动化装置,2009(4): 18-21.

[2] Claudio Urrea, Juan Cortes, Jose Pascal. Design, construction and control of a SCARA manipulator with 6 degrees of freedom[J].Journal of Applied Research and Technology,2016,14(6): 396-404.

[3] Hyunchal Oh, Ki Young Song, Kwan Yuhl Cho. Initial rotor position detecting algorithm of PM synchronous motor using incremental encoder[J]. Proceedings of ECCE Asia Downunder. Melbourne, VIC, 2013(6): 681-686.

[4] 邓建. 基于 DSP的绝对式光电编码器的电机转速测量[J]. 电机与控制应用,2010, 37(1): 50-55.

[5] Dominik Luczak, Krzysztof Nowopolski, Krzysztof Siembab, et al. Speed calculation methods in electrical drive with non-ideal position sensor[C]. 2014 19th International Conference On Methods and Models in Automation and Robotics. Miedzyzdroje, Poland. Sept. 2014: 726-731.

[6] 张立华, 吴红星, 郑继贵. 基于工业机器人用位置伺服磁编码器的设计[J]. 微电机,2013, 46(10): 56-60.

[7] 张连新, 高洪明, 吴 林, 等. 机器人绝对位置数据的获取与处理[J]. 制造业自动化,2005, 27(5): 38-41.

[8] 沈跃，刘慧. 开放式结构平台上的SIASU N-06B 机器人控制系统的研究[J]. 组合机床与自动化加工技术,2004(5): 101-103.

[9] 王殿君，彭文祥，高锦宏. 六自由度轻载搬运机器人控制系统设计[J]. 机床与液压,2017, 45(3): 14-18.

[10] 朱华炳，张卫荣，秦磊. 基于PCI-208卡的码垛机器人系统设计与研究[J]. 组合机床与自动化加工技术,2012(9): 66-69.

[11] 罗伟涛, 张铁. 基于ARM+DSP+FPGA的机器人运动控制器研究[J]. 机床与液压,2011, 39(15): 55-58.

[12] 郭李艳，何萍，李美莲. 一种应用TMS320F2812和编码器测量电机转速的方法[J]. 桂林航天工业高等专科学校学报,2007,12(3): 13-15.

[13] 李本红. 基于DSP的全数字直流伺服电动机位置随动系统的设计[J]. 电机技术, 2006(1): 28-30.

[14] 王玲, 陈远超. 基于Multisim 12的正交编码器与解码器的电路设计与仿真[J]. 现代电子技术,2015,38(8): 145-150.

(编辑李秀敏)

AbsolutePositionMeasuringofJointMotorforIndustrialRobotUsingDSPchip

QIANG Yan-hui, YANG Kan

(Navy Academy of Armament, Beijing 100161, China)

Absolute position measuring of servo motor for robot is proposed in this paper, it’s designed for joint servo system which make up of motor and servo drivers. The motion controller based on DSP (TMS320F1812) using following method. The motion controller receive serial data of absolute position in initial phase. It get incremental position through QEP module embedded in DSP chip(TMS320F1812) at steady running phase. Then, the absolute position is calculated in real time. A new motor rotation determination method is presented aiming at overcoming the shortcoming of pulse counting register direction flag bit. The hardware and software design scheme of motion controller are described in detail. An experiment has conducted to verify the absolute position measuring method for industrial robot.

robot; absolute position; motion controller

TH165;TG659

：A

1001-2265(2017)09-0103-04

10.13462/j.cnki.mmtamt.2017.09.026

2017-04-27；

：2017-06-06

强艳辉( 1983—) ，男，山西中阳人，海军装备研究院工程师，博士，研究方向为机器人运动控制，(E- mail) happyyanhui@ 163.com。