FPGA/DSP > DSP系统 > 详情

基于TMS320F206DSP的冗余度TT-VGT机器人的运动学求解

发布时间:2020-07-01 发布时间:
|


20世纪90年代以来,数字信号处理器(DSP)在自动控制中得到越来越广泛的应用。这主要是因为它具有以下优点:(1)并行体系结构和专用的硬件乘法器使得DSP运算能力极强;(2)高速特性使得DSP能实现实时处理和实时控制。

    据调查,目前将DSP应用于机器人控制系统的方案,通常是将机器人位置控制中运动学计算任务交给PC机完成,PC机将计算结果(机器人各关节的转角)下载到以DSP芯片为核心的电机控制器,实现机器人控制[2]。本文提出将机器人运动学计算任务直接交给DSP的控制方案,利用DSP的并行性计算特点,提高了计算速度,缩小了控制系统的体积。仿真结果表明,该方案计算精度和实时性都较好。

1 TMS320F206 DSP结构特点

TMS320F206DSP基本结构特点包括:①哈佛结构;②流水线操作;③专用的硬件乘法器;④特殊的DSP指令;⑤快速的指令周期(25ns);⑥芯片内部集成了4.5KRAM和32K FLASH RAM,大多数程序及数据可存放在DSP芯片内。这些特点使得该芯片可以实现快速的DSP计算,并能使大部分运算能够在一个指令周期内完成。TMS320F206的并行性表现在以下两方面:

    (1)哈佛结构是不同于传统的冯诺曼结构的并行体系结构,其主要特点是将程序和数据存储在不同的存储空间,因此取指令和执行能完全重叠运行。

(2)DSP芯片广泛采用流水线以减少指令执行时间。指令流水线由一系列总线操作组成。TMS320F206流水线具有4个独立的操作阶段:取指令、译码、取操作数和执行,如图1所示。由于4个操作阶段是独立的,因此,这些操作可交叠地进行。

2 TT-VGT机器人的位姿逆解

TT-VGT(Tetrahedron-Tetrahedron-Variable Geometry Truss)机器人是由多个四面体组成的变几何桁架机器人,平面ABC为机器人的基础平台,基本单元中各杆之间由球校连接,通过可伸缩构件li(i=1,2,…n)的长度,来改变机构的构形,如图2所示。

设冗余度TT-VGT机器人操作手由N个伸缩关节组成,图3所示为两个单元的TT-VGT构成。设变量qi(i=1,2,…N)为平面ACB和平面BCD的夹角,其相应的速度和加速度分别为qi,qi(i=1,2,…N)。

它们与li,li,li(i=1,2,…N)的关系如下[1]:

式中,d表示TT-VGT中不可伸缩构件的长度

li,l'i,l''I分别表示机器人可缩构件的长度、速度和加速度

相邻两个四面体单元的坐标系的建立如图3所示。坐标系XiYiZi相对于坐标系Xi-1Yi-1的变换矩阵可表示为:

对于机构自由度为N、任务自由度为L的冗余度TT-VGT机器人,其余四面体单元的结构与坐标系的建立与图3所示的相似。由文献[1]可知,其末端位姿X是中间变量qi(i=1,2,…,N)的函数,有:

X=f(q)     (3)

对式(3)求导,可得如下的运动学方程式:

X=Jq     (4)

式中,X=(x1,x2,…,xL) T∈R L

q=(q1,q2,…,qN)T∈R N

L

J为机器人的雅可比矩阵,

由式(4)可得:

q=J + X (6)

式中,J+为雅可比矩阵的伪逆,

J+=J T(JJT)-1 (7)

将式(6)离散化,可得机器人运动轨迹上第k点各关节中间变量的dqk及位置qk:

dqk=J+dXk (8)

q k=q (k-1)+dq k (9)

将q k代入式(1),可求得TT-VGT机器人各伸缩构件的长度li。

3 TMS320F206 DSP运动学程序设计

对于TT-VGT机器人的位姿逆解,采用DSP汇编语言设计的程序流程进行求解,如图4所示。

为了保证该程序的执行速度和计算精度,采取了以下算法:

(1)由于汇编语言指令系统中没有三角函数等数学函数指令,这些函数的计算只有通过级数展开算法实现,但计算量太大。考虑到三角函数的周期性,建立了一人1024点的正弦函数和余弦函数表,其分辨率完全能满足精度要求。

(2)由于TMS320F206 DSP芯片为定点运算器件,因此需要将浮点运算转换为定点运算。为保证计算精度,将数据定标设定为可动态调节,数据表达能力为从Q13(-4~+3.9998779)到Q0(-32768~+32767)。

(3)采用并行指令,充分利用TMS320F206四级流水线操作,来提高程序运行速度。

如:

MAC ;乘并累加

APAC ;累加

SACH +,3,AR2 ;将计算结果左移3位后,存于当前辅助寄存器(AR)所指的存储器单元中,并将AR内容加1,最后,将AR2设定为当有AR。

(4)对运算过程进行优化,既要减少计算量,又要防止计算溢出;在混合运算中采用“先除后加”、“加减交叉”的方法。

(5)尽量采用移位运算代替乘除运算,以提高运行速度和计算精度。

通过以上方法,实现了机器人运行学计算的实时性和准确性。

4 仿真计算

以四重四面体为例,建立如图5所示的基础坐标系XYZ,末端参考点H位于末端平台EFG的中点。设参考点H在基础坐标系中从点(0.522689,-0.818450,0.472752)直线运动到点(0.771439,-0.965700,0.721502),只实现空间的位置运动,不实现姿态。动态的整个时间T设为5秒,运动轨迹分为等时间间隔的100个区间。设各定长构件长度为1m。

中间变量q曲线和中间变量q误差曲线如图6和图7所示。从误差曲线可看出,采用TMS329F206DSP芯片进行的运动学计算精度较高。经过实测,该计算程序运行时间为34ms。(TMS320F206芯片指令周期为25ns),可见其实时性较强。

本文提出的采用TMS320F206 DSP的芯片进行冗余度TT-VGT机器人运动学计算方案,充分利用了DSP并行特性进行机器人位姿逆解计算,在程序设计中采用了多种技巧优化计算。仿真结果表明,该方案计算误差较小,实时性强。因此,可将其应用于机器人控制系统,实现机器人计算和控制任务一体化,从而大大缩小机器人体积、降低成本、增强灵活性、具有较强的先进性和实用性。


『本文转载自网络,版权归原作者所有,如有侵权请联系删除』

热门文章 更多
基于NI Scope实时数据采集系统设计