论文部分内容阅读
【摘要】在导航系统中,惯性测量组件(IMU)的精度和速度严重影响到导航计算机的性能。为了提高IMU系统的精度和速度,提出了在硬件方面采用高速AD转换器和DSP,同时在程序中采用了基于LD分解的Kalman滤波技术,并采用线性汇编语言对滤波程序进行了优化。系统的可靠性和速度得到很大的提高。
【关键词】IMU DSP Kalman滤波 线性汇编语言
【中图分类号】TP314 【文献标识码】A 【文章编号】1009-8585(2011)01-00-03
1 引言
捷联惯性测量组件(IMU)在捷联导航系统中所起的作用是相当重要的,其误差参数标定的准确性以及速度直接影响着导航精度。载体的导航信息包括角速度和加速度,当这种导航信息发生变化时,陀螺仪和加速度计就会检测到这种变化量,并进行相关的转化,最后以脉冲数大小的形式来表示这种变化量的大小。数据采集卡就是用于在采样时间内统计脉冲的数目,并利用预先测定的标度因子进行解算处理,进而得出直观的导航信息。因而,这里对采集卡速度和精度就有较高的要求。然而,随着对IMU体积的要求,仅仅依靠硬件电路的设计来提高系统的速度和精度已经难以达到要求,这就需要改进算法程序。
2 系统硬件电路
本系统主要采用了三个陀螺和三个加速度计、高速AD器件和核心处理器DSP。由于需要采集六路模拟量,而系统空间有限以及节省硬件开销,故需要对输入的信号进行依次选通,最后当所有的数据都采集完毕以后,再对数据进行补偿处理,其基本框图如图1所示:
为了提高系统的速度,本系统采用了高速AD器件ADS5521和高性能核心处理器件TMS320C6713。ADS5521是一种高性能12位模数转换器,它的采样速度可以达到105MSPS,仅采用3.3V单电源供电,并且是内置参考电压。TMS320C6713芯片的关键性能特点是:最高可达225MHz的时钟频率;264K的在上数据存储器,存储资源足够大;凭借超常指令字结构和二级高速缓存,DSP的性能可达2400MIPS,最大1800MFLOPS,速度非常快;
该系统采用这两种芯片,在很大程度上可以提高系统的速度,但是系统的运行程序也会影响整个IMU的速度和精度,因此必须充分考虑程序的执行时间和效率。
3 系统软件设计以及优化
DSP程序是系统进行数据处理的核心模块。根据AD输入的陀螺和加速度计信号进行误差补偿,然后转化为RS422形式并发送给外部设备。整个系统根据功能要求主要分为两个模块:引导程序bootloader模块和用户程序模块。
软件结构如图2所示。
引导程序Boot模块:DSP的系统引导装载(Boot)是指在系统加电时,由DSP将一段存储在外部非易失存储器中的代码移植到高速存储器单元中执行。引导程序负责将中断向量表和用户代码段从片外FLASH移植到其它高速存储器中。
用户程序模块:首先数据采集程序采集陀螺和加速度计的数据,然后对陀螺和加速度计的数据进行误差补偿处理,最后将补偿后的数据通过RS422发送程序发送到系统的外部。参见图3。
程序中最消耗时间的是数据处理程序中的误差补偿部分,在补偿滤波程序中,为了防止发散,我们采用了LD分解的Kalman滤波技术,该技术的应用同时也提高的系统的精度。但是,该方法不仅增加了代码的长度,而且消耗了大量的时间,因此要对次部分程序做大量的优化工作。
3.1 利用编辑器选项优化
C6000的C优化器是TI 公司的一个创新,它对程序循环的流水处理使得C代码的效率得到很大的提高,,只要在编译时加入优化项即可。
使用-mt编译选项,使用此选项是函数在参数传递过程中,C编译器会假设存储器关联消除,从而能够更好的优化。该系统中采用最高级别的优化选项进行编译: cl6x-o3-pm file.c-z。
3.2 采用C程序和汇编程序混合编程
混合编程的一般方法有三种:一是对C程序编译后形成的汇编程序进行手工的修改与优化。二是直接在C语言程序中嵌入汇编语句。三是分别编写C程序和汇编程序,再独立编译成目标代码模块,再进行链接。第三种方式是混合编程最常用的方式之一,也是我们系统采用的方式。在这种方式下,C程序与汇编程序均可使用另一方定义的函数与变量。
变量定义的规则与数组、指针的使用变量定义是混合编程的基本问题。C程序与汇编程序定义的变量相互之间可以进行访问。在汇编程序中定义时,需要在变量前加下划线“_”,然后再用.global定义为全局变量,在C程序中则需要说明为extern变量。
以下程序是为了完成LD分解的Kalman滤波的线性汇编程序而编写的部分程序:
程序1:矩阵乘法
线性汇编程序
.global _mpyMatix
_ mpyMatrix:
.cproc a,b,c,n
.reg l_out,l_mid,l_in,i,j,sum,k,
v1,v2,v11,v21,v12,a_i,b_i,c_i
MV n,l_out
ZEROi
outloop: ZERO j
MV n,l_mid
midloop: ZEROsum
ZEROk
MV n,l_in
inloop:MPY i,n,v1
MPY k,n,v2
ADDv1,k,v11
ADDv2,j,v21
ADDv1,j,v12
LDW*+a[v11],a_i
LDW*+b[v21],b_i
MPYa_i,b_i,c_i
ADDsum,c_i,sum
ADDk,1,k
[l_in] SUBl_in,1,l_in
[l_in] B inloop
STWsum,*+c[v12]
ADDj,1,j
[l_mid]SUBl_mid,1,l_mid
[l_mid]B midloop
ADDi,1,i
[l_out]SUBl_out,1,l_out
[l_out]B outloop
.endproc
C程序中的声明和调用
double mpyMatix(double *a, double *b, double *c,int n);
mpyMatix (&a[0][0],&b[0][0],&c[0][0],n);
程序2:LD分解
.global _LDdis
_LDdis:
.eproc n,pmatrix,lmatrix,dmatrix
.reg i,j,k,m_loop1,m_loop2,m_loop3,
m_loop4, sum1,constant1,constant2, v1,v2,v3,
v, x,x1,y
ZERO i
MVn,m_loop1
Loop1:MPYn,i,x
ADDx,i,x
MVK1,constant1
STWconstant1,*+lmatrix[x]
ADDi,1,i
[m_loop1] SUBm_loop1,1,m_loop1
[m_loop1] B loop1
LDW*+pmatrix[0],x
STWx,*+dmatrix[0]
MVn,m_loop2
SUBm_loop2,1,m_loop2
MVconstant1,i
RCPDP x,constant2
Loop2:MPYn,i,x
LDW*+pmatrix[x],v1
MPYv1,constant2,v2
STWv2,*+lmatrix[x]
ADDi,1,i
[m_loop2] SUBm_loop2,1,m_loop2
[m_loop2] B loop2
MVn,m_loop3
SUBm_loop3,1,m_loop3
MVK 1,j
Loop3: MPYj,n,x1
MPYk,n,y
ZERO sum1
ZERO k
MVj,m_loop4
Loop4: ADDx1,k,x
ADDy,k,y
LDW*+lmatrix[x],v1
LDW*+dmatrix[y],v2
MPYv1,v2,v
MPYv,v1,v
ADDsum1,v,sum1
ADDk,1,k
[m_loop4]SUBm_loop4,1,m_loop4
[m_loop4]B m_loop4
ADDx1,j,x1
LDW*+pmatrix[x1],v3
SUBv3,sum1,v3
STWv3,*+pmatix[x1]
ADDj,1,j
[m_loop3]SUBm_loop3,1,m_loop3
[m_loop3]B loop3
…
…
由于空间有限,只列写了一部分该程序。
4 结论
本系统在硬件电路上采用了高速AD器件和DSP芯片,同时在程序设计上也采取了适当的优化措施,对于消耗时间比较长的误差处理补偿程序,采用了线性汇编语言编写,虽然增加了代码的长度,但是却大大提高了系统的时间。
参考文献
[1] 裴听国,李汉舟.全数字IMU方案[J].中国惯性技术学报.2003.12:69－83.
[2] 李璞,蔡同英.捷联测量装置的数据采集技术[J].应用科技.2004.4:48-50.
[3] 阳明晔, 张志勇. 基于TMS320C6000系列DSP的C代码优化方法研究[J].微处理机.2004.4:59-64.
[4] 张淑艳,刘伟宁.TIC6200系列DSP代码优化技术研究[J].计算机工程.2006.1:82-86.
[5] 李方慧,王飞等.TMS320C600系列DSPs原理与应用[M].北京:电子工业出版社.2003.197-314.
【关键词】IMU DSP Kalman滤波 线性汇编语言
【中图分类号】TP314 【文献标识码】A 【文章编号】1009-8585(2011)01-00-03
1 引言
捷联惯性测量组件(IMU)在捷联导航系统中所起的作用是相当重要的,其误差参数标定的准确性以及速度直接影响着导航精度。载体的导航信息包括角速度和加速度,当这种导航信息发生变化时,陀螺仪和加速度计就会检测到这种变化量,并进行相关的转化,最后以脉冲数大小的形式来表示这种变化量的大小。数据采集卡就是用于在采样时间内统计脉冲的数目,并利用预先测定的标度因子进行解算处理,进而得出直观的导航信息。因而,这里对采集卡速度和精度就有较高的要求。然而,随着对IMU体积的要求,仅仅依靠硬件电路的设计来提高系统的速度和精度已经难以达到要求,这就需要改进算法程序。
2 系统硬件电路
本系统主要采用了三个陀螺和三个加速度计、高速AD器件和核心处理器DSP。由于需要采集六路模拟量,而系统空间有限以及节省硬件开销,故需要对输入的信号进行依次选通,最后当所有的数据都采集完毕以后,再对数据进行补偿处理,其基本框图如图1所示:
为了提高系统的速度,本系统采用了高速AD器件ADS5521和高性能核心处理器件TMS320C6713。ADS5521是一种高性能12位模数转换器,它的采样速度可以达到105MSPS,仅采用3.3V单电源供电,并且是内置参考电压。TMS320C6713芯片的关键性能特点是:最高可达225MHz的时钟频率;264K的在上数据存储器,存储资源足够大;凭借超常指令字结构和二级高速缓存,DSP的性能可达2400MIPS,最大1800MFLOPS,速度非常快;
该系统采用这两种芯片,在很大程度上可以提高系统的速度,但是系统的运行程序也会影响整个IMU的速度和精度,因此必须充分考虑程序的执行时间和效率。
3 系统软件设计以及优化
DSP程序是系统进行数据处理的核心模块。根据AD输入的陀螺和加速度计信号进行误差补偿,然后转化为RS422形式并发送给外部设备。整个系统根据功能要求主要分为两个模块:引导程序bootloader模块和用户程序模块。
软件结构如图2所示。
引导程序Boot模块:DSP的系统引导装载(Boot)是指在系统加电时,由DSP将一段存储在外部非易失存储器中的代码移植到高速存储器单元中执行。引导程序负责将中断向量表和用户代码段从片外FLASH移植到其它高速存储器中。
用户程序模块:首先数据采集程序采集陀螺和加速度计的数据,然后对陀螺和加速度计的数据进行误差补偿处理,最后将补偿后的数据通过RS422发送程序发送到系统的外部。参见图3。
程序中最消耗时间的是数据处理程序中的误差补偿部分,在补偿滤波程序中,为了防止发散,我们采用了LD分解的Kalman滤波技术,该技术的应用同时也提高的系统的精度。但是,该方法不仅增加了代码的长度,而且消耗了大量的时间,因此要对次部分程序做大量的优化工作。
3.1 利用编辑器选项优化
C6000的C优化器是TI 公司的一个创新,它对程序循环的流水处理使得C代码的效率得到很大的提高,,只要在编译时加入优化项即可。
使用-mt编译选项,使用此选项是函数在参数传递过程中,C编译器会假设存储器关联消除,从而能够更好的优化。该系统中采用最高级别的优化选项进行编译: cl6x-o3-pm file.c-z。
3.2 采用C程序和汇编程序混合编程
混合编程的一般方法有三种:一是对C程序编译后形成的汇编程序进行手工的修改与优化。二是直接在C语言程序中嵌入汇编语句。三是分别编写C程序和汇编程序,再独立编译成目标代码模块,再进行链接。第三种方式是混合编程最常用的方式之一,也是我们系统采用的方式。在这种方式下,C程序与汇编程序均可使用另一方定义的函数与变量。
变量定义的规则与数组、指针的使用变量定义是混合编程的基本问题。C程序与汇编程序定义的变量相互之间可以进行访问。在汇编程序中定义时,需要在变量前加下划线“_”,然后再用.global定义为全局变量,在C程序中则需要说明为extern变量。
以下程序是为了完成LD分解的Kalman滤波的线性汇编程序而编写的部分程序:
程序1:矩阵乘法
线性汇编程序
.global _mpyMatix
_ mpyMatrix:
.cproc a,b,c,n
.reg l_out,l_mid,l_in,i,j,sum,k,
v1,v2,v11,v21,v12,a_i,b_i,c_i
MV n,l_out
ZEROi
outloop: ZERO j
MV n,l_mid
midloop: ZEROsum
ZEROk
MV n,l_in
inloop:MPY i,n,v1
MPY k,n,v2
ADDv1,k,v11
ADDv2,j,v21
ADDv1,j,v12
LDW*+a[v11],a_i
LDW*+b[v21],b_i
MPYa_i,b_i,c_i
ADDsum,c_i,sum
ADDk,1,k
[l_in] SUBl_in,1,l_in
[l_in] B inloop
STWsum,*+c[v12]
ADDj,1,j
[l_mid]SUBl_mid,1,l_mid
[l_mid]B midloop
ADDi,1,i
[l_out]SUBl_out,1,l_out
[l_out]B outloop
.endproc
C程序中的声明和调用
double mpyMatix(double *a, double *b, double *c,int n);
mpyMatix (&a[0][0],&b[0][0],&c[0][0],n);
程序2:LD分解
.global _LDdis
_LDdis:
.eproc n,pmatrix,lmatrix,dmatrix
.reg i,j,k,m_loop1,m_loop2,m_loop3,
m_loop4, sum1,constant1,constant2, v1,v2,v3,
v, x,x1,y
ZERO i
MVn,m_loop1
Loop1:MPYn,i,x
ADDx,i,x
MVK1,constant1
STWconstant1,*+lmatrix[x]
ADDi,1,i
[m_loop1] SUBm_loop1,1,m_loop1
[m_loop1] B loop1
LDW*+pmatrix[0],x
STWx,*+dmatrix[0]
MVn,m_loop2
SUBm_loop2,1,m_loop2
MVconstant1,i
RCPDP x,constant2
Loop2:MPYn,i,x
LDW*+pmatrix[x],v1
MPYv1,constant2,v2
STWv2,*+lmatrix[x]
ADDi,1,i
[m_loop2] SUBm_loop2,1,m_loop2
[m_loop2] B loop2
MVn,m_loop3
SUBm_loop3,1,m_loop3
MVK 1,j
Loop3: MPYj,n,x1
MPYk,n,y
ZERO sum1
ZERO k
MVj,m_loop4
Loop4: ADDx1,k,x
ADDy,k,y
LDW*+lmatrix[x],v1
LDW*+dmatrix[y],v2
MPYv1,v2,v
MPYv,v1,v
ADDsum1,v,sum1
ADDk,1,k
[m_loop4]SUBm_loop4,1,m_loop4
[m_loop4]B m_loop4
ADDx1,j,x1
LDW*+pmatrix[x1],v3
SUBv3,sum1,v3
STWv3,*+pmatix[x1]
ADDj,1,j
[m_loop3]SUBm_loop3,1,m_loop3
[m_loop3]B loop3
…
…
由于空间有限,只列写了一部分该程序。
4 结论
本系统在硬件电路上采用了高速AD器件和DSP芯片,同时在程序设计上也采取了适当的优化措施,对于消耗时间比较长的误差处理补偿程序,采用了线性汇编语言编写,虽然增加了代码的长度,但是却大大提高了系统的时间。
参考文献
[1] 裴听国,李汉舟.全数字IMU方案[J].中国惯性技术学报.2003.12:69－83.
[2] 李璞,蔡同英.捷联测量装置的数据采集技术[J].应用科技.2004.4:48-50.
[3] 阳明晔, 张志勇. 基于TMS320C6000系列DSP的C代码优化方法研究[J].微处理机.2004.4:59-64.
[4] 张淑艳,刘伟宁.TIC6200系列DSP代码优化技术研究[J].计算机工程.2006.1:82-86.
[5] 李方慧,王飞等.TMS320C600系列DSPs原理与应用[M].北京:电子工业出版社.2003.197-314.