TMS320F206DSP的冗余度TT-VGT机器人
3 TMS320F206DSP运动学程序设计
对于TT-VGT机器人的位姿逆解,采用DSP汇编语言设计的程序流程进行求解,如图4所示。
为了保证该程序的执行速度和计算精度,采取了以下算法：
（1）由于汇编语言指令系统中没有三角函数等数学函数指令,这些函数的计算只有通过级数展开算法实现,但计算量太大。考虑到三角函数的周期性,建立了一人1024点的正弦函数和余弦函数表,其分辨率完全能满足精度要求。
（2）由于TMS320F206DSP芯片为定点运算器件,因此需要将浮点运算转换为定点运算。为保证计算精度,将数据定标设定为可动态调节,数据表达能力为从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并行特性进行机器人位姿逆解计算,在程序设计中采用了多种技巧优化计算。仿真结果表明,该方案计算误差较小,实时性强。因此,可将其应用于机器人控制系统,实现机器人计算和控制任务一体化,从而大大缩小机器人体积、降低成本、增强灵活性、具有较强的先进性和实用性。
评论