一种基于FPGA的扩展卡尔曼滤波器电路结构设计方法与流程

本发明涉及一种扩展卡尔曼滤波器电路结构设计方法，涉及扩展卡尔曼滤波器电路设计领域。

背景技术：

常规pmsm电机采用速度、电流、位置三环控制，其中的位置环控制需要使用当前位置、角速度作为pi调节器的参数。由于普通传感器易受到磁场干扰，永磁直流电机周围磁场较强，所以观测到的数据噪声大，稳定性差。且传感器设备臃肿成本高等原因。工业上也会采取各种转子位置速度估算方法(即无传感器法)来估计当前时刻的速度位置，常规估算方法有直接计算，滑膜观测法，模型参考自适应算法，高频信号注入法，人工智能法和在扩展卡尔曼滤波方法，考虑到各种方法的优劣以及其在fpga上的可实现性，本发明以扩展卡尔曼滤波法进行设计。

以往的应用中,大多采用dsp(数字信号处理器)实现卡尔曼滤波,其优点是灵活、编程简单、可直接采用c语言矩阵运算，缺点是这种灵活性有时是以牺牲效率为代价的,当要求精度很高时,程序复杂度就会增大，因而很难保证实时的要求。针对通用处理器实现卡尔曼滤波算法执行速度较慢的问题，为了追求更好的电机控制效果，需要做到在短时间内完成更多次对速度位置的估算，所以应尽可能提高算法计算速度。用fpga实现此结构，主要由于fpga是靠控制每个时钟来驱动信号和寄存器传输，而且fpga是并行执行的，与通用处理器相比，fpga是通过较慢的时钟频率，同时并行执行数据流，而通用处理器虽然主频高，但是并行执行能力差，综合起来对于同样的代码，应用程序在fpga上的运行速度可能比在传统cpu上运行要快100倍。考虑到现场可编程门阵列(fieldprogrammablegatearray，fpga)具有的高度并行性、执行速度快的特点，近些年来对在fpga中设计卡尔曼滤波算法的硬件实现展开了初步的研究。但现有技术中,没有人提出在fpga上实现扩展卡尔曼滤波算法的低功耗。

技术实现要素：

本发明的目的是提供一种基于fpga的扩展卡尔曼滤波器(ekf)电路结构设计方法，主要针对永磁同步电机(pmsm)转动时速度与位置的估计，即为了实现无传感器时永磁同步直流电机(pmsm)转动时速度与位置的估计。

本发明解决上述技术问题所采用的技术手段为：

一种基于fpga的扩展卡尔曼滤波器电路结构设计方法，所述方法设计的基于fpga的扩展卡尔曼滤波器电路结构用于实现传统pmsm电机矢量控制系统中的位置和速度估算器模块，其输出量为角速度(ω)与角度(θ)的估计；所述方法为：

ekf滤波实现步骤：

(1)设置qd，r，p0初始值，将qd和r初始值设置为0，将p0设置为四阶单位矩阵；

(2)从pmsm系统中测量到iα(n)，iβ(n)，vα(n)，vβ(n)

(3)根据下列公式计算：

(4)计算pn，首先要计算pn|n-1，计算方法如下：

(5)计算卡尔曼增益kn：

其中，

(6)当前时刻电流、角度、位置估算方程：

式中：

qd代表了预测/更新模型的过程性噪声；

r代表了ekf中观测值的不确定度，也可以叫做观测误差，属于传感器属性，不依赖于时间；

pn|n-1代表了ekf中当前数据不确定度的预测值(平方误差)，此值并未使用当前时刻的数据更新；

pn代表了ekf中当前时刻不确定度的预测值的更新值，此值使用了当前时刻的数据更新，pn是一个4×4矩阵；p0表示第一次计算时的不确定度，可采用4阶单位矩阵；

iα(n)表示当前时刻的iα观测值，该值来自电流环的clark变换模块；

iβ(n)表示当前时刻的iβ观测值，该值来自电流环的clark变换模块；

vα(n)表示当前时刻的vα观测值，该值来自电流环的park逆变换模块；

vβ(n)表示当前时刻的vβ观测值，该值来自电流环的park逆变换模块；

表示当前时刻的角速度估计值；

表示当前时刻的角度估计值(即转子位置)；

iα(n-1)表示上一时刻的iα观测值；

iβ(n-1)表示上一时刻的iβ观测值；

vα(n-1)表示上一时刻的vα观测值；

vβ(n-1)表示上一时刻的vβ观测值；

表示上一时刻的角速度估计值；

表示上一时刻的角度估计值；

表示当前时刻的估计值，即完成一次ekf运算后的最终估计值

tc表示每次计算的周期(2.44us)；

rs表示电机电阻(0.63欧姆)；

ls表示电机电感(0.00277h)；

ekf电路模块设计，其过程为：

根据上述公式设计电路结构，基于fpga是一种并行电路结构，在考虑到资源速度方面因素、对电路结构反复优化设计确定最终电路，使用sin模块、cos模块实现公式(1)、(2)中的sin函数、cos函数，使用add模块、mul模块、div模块实现公式中的加法、乘法、除法，state_machine模块控制调用运算模块的顺序；由于fpga资源有限，设计中状态机的每一步至多只能执行一次加/乘运算，state_machine模块的0-7步，用于实现公式(1、2、3、4)，算得8-62步用于实现公式(5、6、7)，算得pn|n-1，71-111步用于实现公式(8-18)，算得kn、pn，112-120步用于实现公式(19-22)，算得为了储存公式(19-22)计算出的数据，电路使用了ram存储数据；

整个电路包括counter模块、state_machine模块、ram_controller模块、calucate_controller模块、sin模块、cos模块、div模块、add模块和mul模块；

(1)counter模块用于计数，每计数到5us为state_machine模块提供重置信号；其功能的实现过程为：

①在每个计数周期开始后的100ns为ram_controller模块提供读使能rden_ram信号，并且读使能持续20个时钟周期(400ns)；

②开始执行状态机，执行122个时钟周期(2.44us)，每一步执行20ns；

③ram_controller获得写使能信号，写使能wren_ram信号持续20个时钟周期(400ns)；

④每个周期留有1.7us余量，在modelsim仿真中可看到每个周期为5us；

(2)state_machine模块是一个122步状态机模块，用于协调各个模块的工作，其功能实现过程为：

①输入数据包括时钟信号clk、置位信号rst_n、三角函数标志位信号start_sign；

②输出为en_mul，en_add，en_div，en_sin，en_cos，根据上述公式的计算顺序使能mul模块、add模块、div模块、sin模块、cos模块；

③每完成一次122步状态机即为完成一次扩展卡尔曼滤波算法的计算；

(3)ram_controller模块用于调用、控制ram模块，其功能的实现过程为：

①ram模块由altera公司提供的ip核设计，ram深度为16，宽度为16；该ram模块使用mif文件进行初始化，mif文件中存放设定好的用于扩展卡尔曼算法的初始数据，包括p11，p12，p13，p14，p22，p23，p24，p33，p34，p44，ian-1，ibn-1，θn-1，ωn-1，其中p11，p12，p13，p14，p22，p23，p24，p33，p34，p44代表了扩展卡尔曼滤波算法中的斜方差矩阵，iα(n)、iβ(n)、ω(n)、θ(n)是上一时刻的计算数据，存储的每个数据都是16位有符号数；

②ram_controller模块用来控制ram在特定时刻按顺序写入或者读取数据(ram_controller模块结构图见图6)，写入的数据来自calucate_controller模块，读出的数据交给calucate_controller模块、sin模块和cos模块进行计算；

(4)calucate_controller模块

①接收来自状态机的三种使能信号，分别使能add模块，mul模块，div模块；

②接收ram中的存储数据，add、mul、div、sin、cos模块的输出数据；

③为add模块，mul模块，div模块提供需要运算的数据：rs、ls、iα(n)、iβ(n)、ω(n)、θ(n)；

④en_add信号位宽为7位，不同的en_add信号使能了不同的加法器输入，en_mul信号位宽也为7位，不同的en_mul信号使能了不同的乘法器输入；en_div信号位宽为2位，不同的en_div信号使能了不同的除法器输入；

(5)sin模块和cos模块基于lub方法设计，lub方法需要rom模块，rom模块由altera公司提供的ip核设计，宽度为15位，深度为1024位，rom模块通过mif文件初始化，该mif文件记录了由0°到90°的正弦(余弦)函数值；sin(cos)模块由状态机使能，模块的输入数据为角度，角度数据由ram_controller模块给出，输出结果传递到calucate_controller；

(6)add模块、mul模块和div模块三类模块分别用于计算带符号定点数的加法，乘法和除法，

①add模块均采用饱和加法器以适应加法可能会导致溢出，

②考虑到对于q15格式数，mul模块做乘法运算时需要将结果除以2^15，即右移15位，div模块除法运算时需要将结果乘以2^15，即左移15位；

③采用精简寄存器时序除法进行运算，此除法设计可以使移位和减法操作数在同一时钟周期内进行，而不是在不同时钟周期；此除法基于在除法运算序列开始执行时，被除数寄存器的内容就要向最高位移动，同时为商的位留出存储空间；

数字格式的设计：

所有数值全部采用q15格式有符号数，q15格式用于处理定点数的计算，可将小数化为正数，q15格式数的精度为1/32768。

ekf滤波实现中，每次计算的周期tc为2.44us，电阻rs为0.63欧姆，电感ls为0.00277h。

所述精简寄存器时序除法，是一种有效的除法器结构，它基于在除法运算序列开始执行时，被除数寄存器的内容就要向最高位移动，同时为商的位留出储存空间，因为它不再使用单独的寄存器保存商，所以这种结构在物理资源利用方面更为有效；寄存器包括了一个扩展位用来调整被除数和除数所需的初始移位，并用来保存从被除数中减去除数所形成的符号位，该寄存器需在右边扩展一位以保存求商所形成的第一位；设计中使用数据通路单元中的计数器来产生状态信号，并传送给控制单元。

本发明具有以下有益技术效果：

本发明是为了对永磁同步电机转动时转子的位置角度进行估计而设计，故结合fpga电路结构本身的优势和ekf算法占据资源少的特点，设计出此发明。本发明可以在fpga上实现ekf算法，每秒约计算20万次，功耗较传统dsp实现扩展卡尔曼滤波算法更低。此种设计主要针对无传感器时永磁同步直流电机(pmsm)转动时速度与位置的估计。可以在不使用传感器的情况下估计当前时刻速度和位置，用于pmsm控制系统。

本发明根据pmsm当前时刻电流、电压、转子位置和角速度，通过扩展卡尔曼滤波算法(ekf)预测下一时刻的pmsm转子位置和角速度。设计包括counter模块，state_machine模块，ram_controller模块，calucate_controller模块，sin模块，cos模块，div模块，add模块，mul模块。counter模块用于计数，可以按照固定周期数进行一次扩展卡尔曼算法，该周期根据pmsm控制系统中的adc采样周期(5us)设定，state_machine是整个设计的核心部分，用于按规定顺序顺序使能sin、cos、div、add、mul模块以用于完成ekf的计算，sin模块、cos模块用于计算角度的三角正弦、余弦数值，div、add、mul模块分别用于计算带符号定点数的除法、加法、乘法。ram_controller用于存储上一时刻扩展卡尔曼滤波算法产生的数据。

本发明所述的基于fpga的卡尔曼滤波算法的硬件实现方法完全适用于处理永磁同步电机的位置、速度估计。

附图说明

图1为本发明用于pmsm电机矢量控制系统的框图；

图2为本发明的所述系统的整体框图；图中，counter表示counter模块，为计数模块；state_machine表示state_machine模块表示状态机模块，ram_controller表示ram_controller模块，为ram控制模块，cos表示cos模块，为cos函数计算模块，sin表示sin模块，为sin函数计算模块，div模块为除法模块，add模块为加法模块，mul模块为乘法模块，calucate_controller为计算模块，用于控制div，add，mul模块。

图3为本发明电路结构时序图；

图4为本发明modelsim仿真图；

图5为本发明的状态机转移图；

图6为本发明的ram控制模块电路原理图，wren是来自counter模块的写使能信号，address是来自ram_counter模块的地址信号，ian，ibn，omega，theta和p矩阵是来自状态机模块完成一次计算后，得到的数据，其中p矩阵是一个4*4矩阵，且其中p12＝p21，p13＝p31，p14＝p41，p23＝p32，p24＝p42，p34＝p43。

图7为本发明calucate_controller模块电路原理图，其中en_div、en_add、en_mul信号来自状态机模块，o_add、o_mul、o_div来自add、mul、div模块，in1_add、in2_add、in1_mul、in2_mul、in1_div、in2_div分别作为add、mul、div模块的输入。

图8为本发明中的一种精简寄存器时序除法原理框图，其中load_words是需要加载的数据，shift_dividend用于调整被除数的初始状态，shift_divisor用于调整除数，使其与被除数对准，sub_and_shift用于减去除数所形成的符号位且移位操作，flush_divr是指除法对齐,xfer_rem是指寄存器中数据移位，state_is_shsub是指当前除法状态为移位并且做减法的状态，w1_is_0信号是指word1为0，w2_is_0信号是指word2为0，divr_is_1是指除数为1，msb_divr用于表示除数最高位，sign_bit表示符号位，max用于检测所允许的最大移位何时发生。

具体实施方式

结合图1至8对本发明的实现进行如下阐述：

1、pmsm电机矢量控制系统框架

如图1^【2】，这是一种成熟的pmsm电机控制系统，本专利用于实现框图中的位置和速度估算器模块。该模块输出量为速度(ω)与角度(θ)，输出的角度和速度用于pmsm控制系统。

2、ekf滤波实现步骤：

(1)设置qd，r，p0初始值，本发明将qd和r初始值设置为0，将p0设置为四阶单位矩阵。

(2)从pmsm系统中测量到iα(n)，iβ(n),vα(n),vβ(n)，modelsim软件仿真时需要设置的数值根据文献[2]中获得。

(3)根据下列公式计算：

其中，是上一时刻的ekf算法计算所得的估计值。tc为每次计算的周期(2.44us)，rs为电阻(0.63欧姆)，ls电感(0.00277h)。

(4)计算pn，首先要计算pn|n-1,计算方法如下：

(5)计算卡尔曼增益kn：

其中，

(6)当前时刻电流、角度、位置估算方程：

3、ekf电路模块设计

根据上述理论设计电路结构，由于fpga是一种并行电路结构，所以在考虑到资源速度等方面因素、对电路结构反复优化设计后，最终结构如图2，电路包括counter模块，state_machine模块，ram_controller模块，calucate_controller模块，sin模块，cos模块，div模块，add模块，mul模块。

(1)counter模块用于计数，每计数到5us为state_machine模块提供重置信号，如图3所示。

①在每个计数周期开始后的100ns为ram_controller模块提供读使能rden_ram信号，并且读使能持续20个时钟周期(400ns)。

②开始执行状态机，执行122个时钟周期(2.44us)，每一步执行20ns。

③ram_controller获得写使能信号，写使能wren_ram信号持续20个时钟周期(400ns)。

④每个周期留有1.7us余量，在modelsim仿真中可以看到每个周期为5us，如图4。

(2)state_machine模块是一个122步状态机模块，用于协调各个模块的工作。

①输入数据包括时钟信号clk，置位信号rst_n，三角函数标志位信号start_sign。

②输出为en_mul，en_add，en_div，en_sin，en_cos，可以按状态转换图的顺序使能mul模块、add模块、div模块、sin模块、cos模块。

③每完成一次122步状态机即为完成了一次扩展卡尔曼滤波算法的计算，具体状态转换图见图5。

(3)ram_controller模块用于调用、控制ram模块。

①ram模块由altera公司提供的ip核设计，ram深度为16，宽度为16。该ram模块使用mif文件进行初始化，mif文件中存放设定好的用于扩展卡尔曼算法的初始数据，包括p11，p12，p13，p14，p22，p23，p24，p33，p34，p44，ian-1，ibn-1，θn-1，n-1，其中p11，p12，p13，p14，p22，p23，p24，p33，p34，p44代表了扩展卡尔曼滤波算法中的斜方差矩阵，ian-1，ibn-1，θn-1，ωn-1是上一时刻的计算数据，存储的每个数据都是16位有符号数。

②ram_controller模块用来控制ram在特定时刻按顺序写入或者读取数据，写入的数据来自calucate_controller模块，读出的数据交给calucate_controller模块、sin模块和cos模块进行计算。ram_controller模块结构图见图6。

(4)calucate_controller模块

①接收来自状态机的三种使能信号，分别使能add模块，mul模块，div模块。

②接收ram中的存储数据，add，mul，div，sin，cos模块的输出数据。

③为三种运算模块提供运算数据。如图7。

④en_add信号位宽为7位，不同的en_add信号使能了不同的加法器输入。en_mul信号位宽也为7位，不同的en_mul信号使能了不同的乘法器输入。en_div信号位宽为2位，不同的en_div信号使能了不同的除法器输入。

(5)sin模块和cos模块基于lub方法设计，lub方法需要rom模块，rom模块由altera公司提供的ip核设计，宽度为15位，深度为1024位，rom模块通过mif文件初始化，该mif文件记录了由0°到90°的正弦(余弦)函数值。sin(cos)模块由状态机使能，模块的输入数据为角度，角度数据由ram_controller模块给出，输出结果传递到calucate_controller，如图2。

(6)add模块、mul模块和div模块三类模块分别用于计算带符号定点数的加法，乘法和除法。

①考虑到加法可能会导致溢出，本发明中的add模块均采用饱和加法器。

②考虑到对于q15格式数，mul模块做乘法运算时需要将结果除以2^15，即右移15位。div模块除法运算时需要将结果乘以2^15，即左移15位。

③考虑到多位数除法运算速度较慢，本发明采用了一种精简寄存器时序除法。如图8，此除法设计可以使移位和减法操作数在同一时钟周期内进行，而不是在不同时钟周期。此除法基于在除法运算序列开始执行时，被除数寄存器的内容就要向最高位移动，同时为商的位留出存储空间，这种结构不再使用单独的寄存器保存商，所以在物理资源利用方面更加节省资源。

4、数字格式

由于三角函数、电流、电压等数值可能会出现小数或负数，所以本发明中所有数值全部采用q15格式有符号数。q15格式常用于处理定点数的计算，可以将小数化为正数，q15格式数的精度为1/32768，足以满足本实验要求。由于本发明中会出现负数，所以计算模块要先将数据化作原码，再做处理。

本发明中援引的参考文献明细如下：

[1]inanr,barutm,karakayaf.fpgaimplementationofextendedkalmanfilterforspeed-sensorlesscontrolofinductionmotors[c]//ietinternationalconferenceonpowerelectronics.iet,2014.

[2]袁雷,胡冰新,魏克银,etal.现代永磁同步电机控制原理及matlab仿真[m].北京航空航天大学出版社,2016.

[3]quangnk,tungdd,haqp.fpga-basedsensorlesspmsmspeedcontrolusingadaptiveextendedkalmanfilter[c]//automationscienceandengineering(case),2015ieeeinternationalconferenceon.ieee,2015.

[4]bahri,maalouf,idkhajine,etal.fpga-basedimplementationofsensorlessacdrivecontrollersforembeddedelectricalsystems[c]//sensorlesscontrolforelectricaldrives.ieee,2011.

[5]inanr,barutm.speed-sensorlessdirectvectorcontrolofinductionmotorwiththeekfbasedstatorresistanceestimationonfpga[c]//acemp2015.ieee,2015.

[6]idkhajine,monmasson,maalouf.extendedkalmanfilterforacdrivesensorlessspeedcontroller-fpga-basedsolutionordsp-basedsolution[c]//ieeeinternationalsymposiumonindustrialelectronics.ieee,2010.

[7]wangjm,tianxh,jiangxl.veriloghdldigitalsystemdesign[m].harbin:harbininstituteoftechnologypress,2010:1-300.

王建民，田晓华，江晓林.veriloghdl数字系统设计[m].第一版.哈尔滨：哈尔滨工业大学出版社，2010：1-300.