基于Nios II的机器人视觉伺服控制器的研究与设计

Altera公司的Nios II处理器是可编程逻辑器件的软核处理器。NiosII软核处理器和存储器、I／O接口等外设可嵌入到FPGA中，组成一个可编程单芯片系统(SOPC)，大大降低了系统的成本、体积和功耗。适合网络、电信、数据通信、嵌入式和消费市场等各种嵌入式应用场合。  

本文提出一个基于Nios II处理器结构的系统用于实现机器人实时运动检测跟踪，使用线性卡尔曼滤波器算法来快速完成运动估计及进一步分析和校正，算法中的乘除利用MATLAB／DSP Builder生成的模块作为Nios II处理器的自定义指令的设计方法。  

机器人视觉伺服控制器的研究与设计  

机器人视觉伺服控制就是用各种成像系统代替视觉器官作为输入的敏感手段，并由高速处理器替代大脑完成相应的处理和解释，其最终研究目标就是使机器人视觉伺服控制器能像人那样通过视觉观察和理解世界，具有自主适应环境的能力，可依据视觉敏感和反馈，以某种程度的智能完成一定的任务。  

系统硬件实现  

基于SOPC的机器人视觉伺服控制器，主要由FPGA、存储器和外设三个部分。  

摄像头位置固定，它所能采集图像的范围称为视觉区域，调整摄像机使视觉区域覆盖机器人的工作空间，即机器人要跟踪的曲线在该视觉区域内。利用摄像机采集图像，而后系统对采集的图像进行处理，分析、提取出离散的采样点序列，最后再根据采样点序列规划机器人的运动路径。其中，FPGA部分核心是Nios II处理器Core。在一般的嵌入式系统开发中，当需要新的外设模块时往往需要在PCB上加入相应的外设芯片或者换用更高档的CPU，而SOPC设计可以同一个FPGA芯片内加入相应的外设模块核，并通过在片上的Avalon总线与NiosⅡ处理器Core相连，因而不需要在PCB这个层面上作更多的修改。成像采集装置从目标对象场景中采集图像序列，保存在SOPC的片外存储器中，然后利用NiosⅡ处理器和定制的乘法、除法等DSP运算指令来实现线形卡尔曼滤波器的算法，从而实现运动目标的识别与跟踪。  

系统软件的实现  

离散线性卡尔曼滤波算法  

线性卡尔曼滤波是美国工程师Kalman在线性最小方差估计的基础上，提出的数学结构上比较简单的最优线性递推滤波方法，具有计算量、存储量低，实时性高的优点。特别是经历了初始滤波的过渡状态后，滤波效果非常好。  

线性卡尔曼滤波基本算法如下：设一随机动态系统，其数学模型为：  



公式1中x(k)为系统状态矢量，w(k)为系统噪音矢量,φ(k),Г(k)为系统矩阵,公式2中Z(k)为系统观测矢量，H(k)为系统观测矩阵，V(k)为系统观测噪音矩阵。  

关于系统的随机性，本文假定，系统噪音和观测噪音是不相关的零均值高斯白噪声。随机系统的状态估计问题，就是根据选定的估计准则和获取的测量信息对系统状态进行估计，卡尔曼滤波的估计准则是：  



其中，，即估计是x(k)无偏和最小方差，根据这两个准则可推导出对系统的完整的滤波算法，即：  



预测误差方程为：  



增益矩阵方程为：  



滤波误差方差阵为：  



上述公式中I是单位矩阵，Q为w(k)自协方差方差阵，R为V(k)自协方差方差阵。卡尔曼滤波采用递推算法，计算最优滤波值时，K(k+1)由P(k+1|k)来确定，P(k+1|k)由P(k)来确定，P(k+1)由P(k+1|k)和K(k+1)来确定，如此反复递推运算。  

系统程序流程  

本系统的主要功能是完成运动目标的锁定，并控制运动平台对目标进行实时跟踪。使用高性能NiosII处理器作为控制器控制着整个系统的实现流程和高效的线性卡尔曼滤波算法对目标运动参数的估计，有效地提高了图像处理速度，实现了运动目标的快速跟踪。以下是系统流程：

1．系统初始化：由SOPC通过控制总线设置USB接口微型摄像头工作模式，并初始化其主控程序变量。  
2．确定运动区域：由Nios II处理器根据图像序列锁定运动物体，根据被跟踪物体确定运动跟踪区间，接下来的跟踪操作都是在这个跟踪窗口中进行。  
3．预测计算：利用线形卡尔曼滤波器方程进行计算。  
4．滤波：预测和滤波是相互作用的，即由滤波得到预测而由预测又可得到滤波。  
5．输出：SOPC发出控制信号给随动平台。  

该系统采用集成了Nios软核处理器的Stratix高密度FPGA，控制器通过摄像头记录每一时刻运动目标的位置和速度作为观测值。然后按照公式(3)、(4)、(5)、(6)进行最佳状态估计，得到每一时刻运动目标的位置和速度的预测值。由于各种干扰因素的存在，经过七八个时间段后预测位置与观测位置相当接近，即可实现准确的状态预测。