[Brushless] LAN1902 - 线性卡尔曼滤波器

注:本来打算一篇里写完,后来发现不知不觉写的太长,还是选择分P了。扩展卡尔曼和无迹卡尔曼的部分下一篇再写,至于实际应用先鸽着,,,

0. 前言

近期开始搞一些BLDC应用相关的东西,硬件和软件都有所涉猎。随着了解的越来越多,我发现这是一个无穷无尽的坑,工程上常用的手段只能满足比较一般的需求(而我所在的实习公司对这些手段的表现已经足够满意了),但对于一些特殊的应用场合,例如超高速,超低速,负载快速变化的场合,往往需要更加复杂的算法辅助。

BLDC也就是“直流无刷电机”,和PMSM(永磁同步电机)并列是目前应用最广泛的电机类型,在高速,高可靠性,小体积,高效率,少维护等要求下是首选。和有刷电机依靠机械结构进行电-磁控制不同,它要求控制器去开关MOSFET来产生特定的磁场,以维持马达转动。但是如果在不合适的时间打开关闭MOSFET,会导致马达产生相反方向的驱动力或者内部各绕组所产生的磁场的作用效果相互抵消,大大降低效率,乃至让马达停转。因此如何控制这个导通的时间点就成为了关键,为了知道应该在什么时候控制开关动作,需要知道转子当前到达了什么位置。

传统的使用特定传感器的方法如下:

  • 电磁传感器:通过电机内磁场变化导致的感应电场(类似变压器的原理)或者转子引起谐振频率的改变测量位置。缺点是分辨率差,成本高
  • 磁敏传感器:通过霍尔效应测量磁场,目前使用最广泛
  • 光电传感器:转子上安装遮光板,通过测量光线变化得出转子位置,缺点是对灰尘和震动很敏感

图1. 三相BLDC的基于霍尔传感器的驱动波形图[1]

传感器必然会产生额外的成本,额外的体积占用,更糟糕的是在一些特别恶劣的环境下,例如马达本体需要放置在极高温或极低温环境下,此时不可能把传感器和马达放在一起。无传感器位置检测法可以满足这样的需要:通过测量反电动势,相电流,甚至电感量来推出转子的位置。其中反电动势法是目前使用最为广泛的手段,其原理为测量在当前位置上未被使用的其中一相线圈上产生的电动势而推测转子位置。

图2. 三相BLDC的基于反电动势探测的驱动波形图[1]

图中我们感兴趣的点是每一相在没有被驱动时(即没有大幅度噪声的间隙)因为电磁感应而产生的电压。最简单的方法为使用单片机内部的比较器外设,对该电压的过零点进行探测;因为现代单片机通常内置了速度很快的ADC,也可以对其采样,不需要任何外部芯片(考虑到ESD问题可能需要一些无源保护器件)的前提下就可以实现很精细的控制。

在很小转速时,由法拉第定律我们知道该信号的幅度会快速下降,直到某个时刻噪声的信噪比无法满足需要,就成了限制整个系统的最低转速;而在高速旋转时,电动势和对应位置间会出现相移,当这种相移大到一定程度,会出现在最佳换相时刻之后才探测到事件的情况。对于简单的方波驱动BLDC这种偏差不会造成太大的影响,但如果需要采用正弦驱动的话这样的误差会导致效率严重下降,在低速时尤其明显。

为了拓展工作区间,人们提出了一些改良措施,例如三次谐波法(三次谐波通常比较稳定,不会随转速和负载变化而产生显著的相移),以及近期兴起的高频注入法(在极低速场合非常有效)。卡尔曼滤波也是这样一种手段(事实上,PLL仅仅是卡尔曼滤波器的一种特例),它通过结合相电流和反电动势的测量结果预估下一时刻转子应该处在的位置,并且不断根据新的测量结果做出新的预测。

在此基础之上,我们还希望能够对马达的速度做出准确的测量,以霍尔传感器为例,我们可以通过霍尔传感器产生上升沿的频率来推测速度。但是霍尔传感器每个周期只能给出三个读数,对于高速而言问题不大,当转速下降时该读数会有很大的波动。为此有些应用场合会额外加一个速度传感器,例如光电计数器来作为实际测量速度的传感器。光电传感器的栅格盘是固定的间隔,超低速场合同样只能获得非常少的采样点。如果我们只关心平均速度,这样的测量是没问题的,如果我们关心瞬时速度或者需要对速度积分求距离,在低速时误差会非常大。而卡尔曼滤波器可以被用作插值的工具,根据“趋势”补充出中间点,可以取得比其他插值方式更加准确的结果。

此前没有接触过这方面的理论,本文总结了我学习卡尔曼滤波器以及相关理论的一些心得,文末还有附有一些Matlab例子,至于具体应用在电机上不知道要留到什么时候才有空做。

1. 卡尔曼滤波器(Kalman Filter)能做什么

关于卡尔曼的粗略介绍,此处我就直接摘抄Wikipedia上的内容了:

卡尔曼滤波的一个典型实例是从一组有限的,包含噪声的,对物体位置的观察序列(可能有偏差)预测出物体的位置的坐标及速度。在很多工程应用(如雷达、计算机视觉)中都可以找到它的身影。同时,卡尔曼滤波也是控制理论以及控制系统工程中的一个重要课题。例如,对于雷达来说,人们感兴趣的是其能够跟踪目标。但目标的位置、速度、加速度的测量值往往在任何时候都有噪声。卡尔曼滤波利用目标的动态信息,设法去掉噪声的影响,得到一个关于目标位置的好的估计。这个估计可以是对当前目标位置的估计(滤波),也可以是对于将来位置的估计(预测),也可以是对过去位置的估计(插值或平滑)。

...

简单来说,卡尔曼滤波器是一个“optimal recursive data processing algorithm(最优化自回归数据处理算法)”。对于解决很大部分的问题,他是最优,效率最高甚至是最有用的。他的广泛应用已经超过30年,包括机器人导航,控制,传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等。近年来更被应用于计算机图像处理,例如头脸识别,图像分割,图像边缘检测等等。

通过“卡尔曼滤波器”这个名称我们首先可以知道这个东西实质上是一种滤波器,滤波器最主要的作用就是将信号中的某一类成分分离出来,卡尔曼滤波器针对的就是系统中存在的随机噪声。对于我们的电机例子,理想的无刷直流电机应当是十分接近线性的,即输入和输出之间的关系非常容易建模,但实际上种种噪音的影响会使这种简单的模型失效。

噪音淹没了有用的信号,这个看似不可解决的问题,其实还有一个条件没有应用,就是这些信号的统计特性,我们是否可以构造一种有记忆性的滤波器,在已知各个成分的统计特性的前提下,得出更加准确的值?卡尔曼滤波器所做的事情就是这样,而且这种滤波器可以工作在超前状态,也就是可以对未来时间点的取值进行预测,此时我们将其称作“状态观测器”(State Observer)以示区别。

实际上关于卡尔曼滤波器的发明有个趣闻:Weiner滤波器早在1940年代就被发明出来,在此之后人们不断对这个需要无限次过去状态的滤波器进行修改,使它越来越接近卡尔曼滤波器,完整的卡尔曼滤波器的系统方程在1959年由Swerling首先推出并应用在工程中(还涉及了非线性系统的情形),但他在论文中仅将其当成一种工程手段写出,完整的理论被卡尔曼抢先发表。Swerling与这个荣誉失之交臂,实在可惜。

卡尔曼滤波相比此前其他类似理论的巨大优势是只用存储一个时间点的几个变量就可以得到足够好的结果,这使得在单片机或其他存储空间受限的场合实现这样的算法非常容易,而工业界第一次公开地实现这样的算法就是在阿波罗登月计划使用的阿波罗导航计算机(Apollo Guidance Computer, AGC)上(首先由卡尔曼本人在1960年在IBM704大型机上完成了仿真,之后才被移植到AGC上,在1961年美国工业界已经知道了这样算法的存在,但出于各种限制,要等到到60年代中期年才开始在航空工业界应用)。这台电脑仅仅是个4200个NOR门组成的超级精简RISC,其指令执行速度仅有kHz级别。正是这样一个巧妙的,适合极低资源下实现的算法,将阿波罗飞行过程中各个传感器的数据综合起来得到预测值,大大减少了需要手动校准飞船参数的次数。[6][7]

2. 最小均方估计

卡尔曼滤波器建立在隐马尔科夫模型(HMM)的基础之上,马尔科夫链是概率论中一种非常重要的随机过程,将其想象成一个在各个状态间随机转换的状态机(当然亦可是连续过程,此处仅讨论离散),其特性是“过程中各个状态st的概率分布仅与前一个状态有关”

换言之,如果我们需要分析一个随机切换的状态机,我们可以完全不管之前发生了什么,而从其中一个状态向下递推,这样仍然可以得到正确的统计规律。而所谓“隐马尔科夫模型”则指的是状态机的情况到其输出之间还存在一个统计联系。

图3. 一个有三个状态(x[3:1])四个输出(y[4:1])的隐马尔科夫模型

隐马尔科夫模型为以下三个问题指明了方向:1. 给定一个系统模型,产生一个观测序列的概率;2. 给定一个系统模型和观测序列,推测最有可能产生这个输出的状态机;3. 给定观测序列,推测系统的某些参数,使得产生该观测序列的可能性最大。

虽然卡尔曼滤波器不会非常强调和该模型的联系,隐马尔卡夫模型是卡尔曼滤波器能够产生正确观测结果的理论基础。使用一些基础的概率理论,卡尔曼得出了这样的结论:[8]

假设存在一个随机信号 和噪声 ,仅 可以被观测到,我们感兴趣的是t1时刻的输出值,对于“预测”问题,t1>t。

定义一个预测值X(t1)和实际x(t1)之间的损失函数,使得该函数满足:1. ,2.

假设这个函数在0时为0,单调,而且是偶函数,并且假设系统的条件概率分布

满足:1. 关于平均值对称,2. 对于任意小于平均值的 ,这是个突函数(

那么能够使得平均损失最小的x取值就是条件期望

现实中大量随机变量相叠加时其统计特性趋近于一个高斯随机过程(中心极限定理),而高斯函数是满足上述两个要求的,因此条件期望值就是最佳预测值。

更有用的结论是,如果我们将损失函数定义为 ,那么条件概率分布即便不满足上述两个条件,该最佳预测值仍然成立。换句话说,线性最小均方估计(Linear Minimum Mean Square  Estimation, LMMSE)总是能得到最优估计值,前提仅是系统函数可微(更严谨的证明由J. L. Doob 给出)。

卡尔曼滤波器则是线性最小均方估计的一种递归实现,最基本的线性卡尔曼滤波算法要求系统方程是线性的,噪声是高斯分布的。在实际工程中这样的例子可能不是很常见,而扩展卡尔曼滤波器(Extended Kalman Filter, EKF)、无迹卡尔曼滤波器(Unscented Kalman Filter, UKF)以及一些新提出的算法例如粒子滤波(Partical Filtering, PF),使用近似手段消除了卡尔曼滤波器的一些限制,拓展了适用范围。

3. 卡尔曼滤波器的原理

在开始理解卡尔曼滤波器前,首先需要明确一个概念(因为我看到网上很多例子中都没有讲得很清楚):“估计”和“预测”这两个词的定义。“估计”在英文中对应Estimation,“预测”在英文中对应Prediction,虽然是近义词,但估计指的更多是根据现有结果,进行判断的过程,而预测指的是在不知道的情况下猜出未来的状态。

现在假设我们有一个最简单的线性的系统,其方程是y=ax,我们并不知道a是多少,在没有噪声的情况下可以任意取两个点得到k,如果有噪声的话就需要一个被称为“观测器”(Observer)的算法来推测出合理的k值:

图4. 滤波结果示意图

这个观测器可以根据历史数据推测出一个可能的k,然后根据预测结果和实际测量结果的偏差,修正猜测的k值。假设这个修正的过程对每一个采样点都在进行:

图5. 每个采样点预测-测量-估计的过程

根据之前推测出的k产生一个预测值x,当到达下一时刻,得到新测量值z时,综合测量和预测结果,通过某种手段(仅仅举例的话:取平均值)得到新的估计值,称作 (x-hat),并更新k,如此往复。

依照这种思想设计的滤波器统称“ 滤波器”(只含线性部分的称作“ 滤波器”,亦称为“g-h滤波器”或“f-g滤波器”,增加二次项后称为“ 滤波器”,亦称作“ghk滤波器”)[3][13] 这类滤波器的范围相当广,区别就在于如何根据预测和测量结果得到新估计值,它们适用的情形各不相同,卡尔曼滤波器只是其中最为简单且有效的实现之一,因为其运算量小,在工程领域才使用的如此广泛。

图6. 卡尔曼滤波器选择新估计点的方法

卡尔曼滤波器选择新估计值的思想很简单:两个高斯函数相乘结果是一个新的高斯函数,如果这两个高斯函数重叠部分不多,那么新的高斯函数会具有非常小的方差。

提取出因子K,可以将其他方程写成更简单的形式

这里注意到下一次的结果可以从上一次的结果推出,将其中的均值替换成前文提到的各个变量,将方差写作P,测量不确定性写作R,k点处方程的形式变为:

这三个方程是卡尔曼滤波器状态更新方程,其中出现了 这个预测值,根据前文的思路,将预测方程写出来,即可得到一维线性卡尔曼滤波器的总共五个方程

这两个方程是卡尔曼滤波器预测方程,其中引入了几个新的量: 是按一定权重增加了一个“驱动力”,作用是修正外界的某种已知的影响,q是一个预先设置的常数,描述的是每次预测行为会让预测值的高斯分布扩散多少。

可以这样去理解卡尔曼滤波器:它将来自预测方程的值和传感器的读数视作同样的数据,通过反馈动态调节预测与传感器读数的比重,从而达到最佳预测效果。我们可以看出这种高斯函数相乘的过程可以乘上更多的高斯函数来进一步缩小方差,换做现实中就是增加更多的传感器,即便这些传感器都非常不准,卡尔曼滤波器也可以将多个传感器的读数融合成一个最优值。

对于更多维度下的情形,可以使用矩阵来表示,使方程的形式保持不变。需要注意的是多维高斯函数相乘的过程会出现 这个东西,因为在高纬度下还会出现协方差,即各个维度间的相互影响,各个维度完全无关时

图7a. 高纬下预测过程由矩阵变换表示

图7b. 通过高纬高斯函数相乘得到新预测值

图7c. 将多个精度和准确度均很差的传感器二维读数融合

4. MATLAB例

A. 一维线性卡尔曼滤波算法

图8a. 对叠加噪声的一次函数应用卡尔曼滤波器

图8b. 对叠加噪声的一次函数应用卡尔曼滤波器,噪声绝对值比较

可以看出,线性卡尔曼滤波器对这种线性运动的滤波效果是非常出色的。需要注意的是,线性卡尔曼滤波器对几个初始参数的设置比较敏感,例如在Q设置不合理的情况下会出现滞后或滤波效果下降,对x的估计若不考虑系统模型(在本例中去掉x估计过程中加上的A0),会产生静差

图9a. 对叠加噪声的正弦函数应用卡尔曼滤波器

图9b. 对叠加噪声的正弦函数应用卡尔曼滤波器,噪声绝对值比较

对程序A中的实际函数f进行修改,将其改成正弦波,可以产生如图所示的结果。简单的线性卡尔曼滤波器仍然有滤波效果,只是没有那么好了。通过改变q可以取得更好的结果。

B. 二维运动路径预测例

接下来举一个稍微复杂一些的例子,假设我们设计了一个雷达,它可以追踪锁定目标,并给出带有xy坐标信息。现在需要用这个雷达去跟踪一架飞行器的运动路径。我们应当如何建立模型呢?如果是仅仅考虑xy位置的话模型的精度很差,意味着预测很不准,我们假设这个模型有两层,一层是坐标,一层是某方向的速度。因为前文提到的隐马尔科夫链的性质,即便我们只能观测到坐标,坐标和速度间的联系是随机分布,我们仍然能够通过坐标信息推测速度。

当然可以继续增加模型的层次,例如把 加速度 也包含到模型中,不过就本例而言我测试了含有加速度的模型,发现拟合效果还不如更简单的位置-速度模型,有些时候使用太过复杂的模型可能反而导致误差增大。

设置感兴趣的向量是:

简写为

那么问题来了,这个模型有几个维度呢?理想情况下,假如运动完全符合牛顿运动定律,这就是个二维问题,因为已知与x或y相关的任何一项都能推出另外一项。但是当能够被观测到的系统是模糊的(即实际采样出的系统模型内部的变量间对应关系是随机分布,与理想模型有偏差),那么这个问题就变成了一个四维问题:每一个变量是一个维度。对应的状态更新矩阵以及理想系统模型(前文中出现的F和A)就应当是4x4的向量

图10a. 理想无噪声条件下,每一T内速度和加速度的对应关系

图10b. 当模型模糊时,更高的速度有很大可能对应更大的位置变化量,这种可能性有多强由协方差矩阵Q描述

在我们的例子中,使用牛顿运动定律的矩阵形式来描述状态间的变换:

xy合并起来写成:

此时预测方程就可以以矩阵形式写出来了:

这里的P,Q均为4x4的矩阵,我们需要自行给出Q的预测值,Q中的每一项越大两个变量对应关系越不确定,等于0时意味着两个变量完全相关(当然,还要看系统模型里对这两个变量的描述)。当Q所有项都等于0时,相当于我们选择完全相信系统模型的描述。在实际工程中这个矩阵并不能直接得到,而是要靠估算或者多次测试以最终决定,现在也有一些算法可以自动修正该矩阵。(类似自适应滤波器的手法)

这里估算该矩阵的方法是这样:假设系统模型准确,误差全部来自“加速度a”,位置和速度均可以由加速度表示出来,根据方差的特性可以由加速度的方差来计算位置、速度的协方差

将其用Matlab实现,可以看到如图所示的执行结果

图11a. 空间图像

图11b. 每个点的误差(直线距离)

计算平均值,我们将测量结果的噪声减小了15dB(20)

源代码:

 

C. 多个传感器数据合并

假设有一架敌机入侵我方领空,正在匀速盘旋,现在需要对其进行运动跟踪以准备防空火力。我方有四台雷达可供使用,其中有三台是误差较大的位置探测雷达,还有一台是误差较小的速度探测雷达,这四个雷达总共可以在每一时刻给出xy方向的八个读数,应当使用什么方法将这四个雷达的数据合并呢?

如果三个位置测量雷达对于敌机在任意位置的探测精度都是不变的,那么我们直接三角定位(并将速度探测雷达当作某种辅助手段)就可以了,但现实中我们知道目标距离越远,读数的噪声就越大,此时就需要对这四台雷达的读数做加权平均,至于权重究竟应该怎样选,就可以靠卡尔曼滤波来自动进行。

本次相较于上一个例子选用简单的位置-速度模型,将系统模型建立为:

进行这种传感器数据融合非常容易,只需要将以上卡尔曼滤波器方程中的H增加几项就可以了。H中每一行中的1对应该传感器的读数是如何在几个被追踪的内部变量间分散的,如果这个传感器测量的就是某一方向的速度,那么

 

 

5. 后记

关于卡尔曼滤波器,之前早有耳闻,尤其是关于阿波罗AGC和汉密尔顿姐姐的各种文章里基本都会提及。但是我一直没有深入了解,更没有实践过。随着实习即将结束,在公司我手头的硬件其实并不能实现这个功能,题图里那个 X-NUCLEO-IHM07M1扩展板支持反向电动势过零检测和平均相电流测量,应该有点搞头。不知道实际着手去用会拖到什么时候…毕竟这和我的专业还是距离比较远,搞不好我这一生再也不会这么近地BLDC底层开发了?

现在厂商能够提供的库都已经相当强大,其中TI的FAST算法是业界顶尖水准,在超低速运转,满负荷启动,快速收敛等领域几乎无人能敌。现在包括ST在内的很多公司也已经把自己的库开源。从头开始造轮子,除非有什么很特殊的应用场合,否则还是节约时间吧。国内有为数不多的大神可以达到和TI同等水平(例如知乎上的某些文章里展示的)

事实上,本人在这方面绝大多数的知识都来自一个看似毫不相关的学科:通信原理,在课程的期末演讲上我们组选择的题目是“信道均衡器”,即通过滤波器的手段抵消信号传输中产生的畸变。期间我们浅浅地谈到了自适应滤波器乃至非线性自适应滤波器。有了这些知识,卡尔曼滤波器名副其实,无论怎样理解它,归根结底都是一种“滤波器”罢了。

而随着对滤波器的理解更加深入,我发现现代(指194x年之后?)对滤波器的理解都远不是《信号与系统》里讨论的那么局限了,它本质上已经成为了一种数学函数映射问题。(例如非常新颖的分数阶傅里叶变换频分复用(FRFT-ODFM),其中分数阶傅里叶变换也是为了在非线性空间里可以使用线性滤波器而采用的)关于卡尔曼滤波器,从函数映射的角度看就变得容易理解了许多。

有趣的是,诸如这样的非线性滤波算法实际上发展到最后就变成了人工神经网络中的全连接层。每个全连接层所做的事情也是从一个超高维度空间到低维度空间的非线性映射,在低维度空间就可以使用线性的手段来解决这些问题了。加上“反向传播”的理论基础,通过训练手段可以自动调整这样的映射关系,我们就实现了最为基础的人工神经网络结构。

所谓知识体系中某些深层的思想应当是能够融会贯通的,目前感觉已经能够微微尝到一点这样的甜头了。

6. 参考文献

[1]. Microchip. AN1083 使用反电动势滤波进行无传感器BLDC 控制

[2]. 谭建成, 邵晓强. 永磁无刷直流电机技术

[3]. Kalmanfilter.net

[4]. Bzarg. How a Kalman filter works, in pictures

[5]. Paul Kettle, Aengus Murray, Finbarr Moynihan. Sensorless Control of a Brushless DC Motor Using an Extended Kalman Estimator. PCIM'98 Intelligent Motion, 1998

[6]. aerith7’s blog. apollo11号のソースコードを読みつつ. 2016

[7]. Leonard A. McGee, Stanley F. Schmidt. Discovery of the Kalman Filter as a Practical Tool for Aerospace and Industry. NASA Technical Memorandum 86847, NASA Archive. Nov. 1985

[8]. R. E. KALMAN. A New Approach to Linear Filtering and Prediction Problems. Trans. ASME–Journal of Basic Engineering, 82 (Series D): 35-45. 1960

[9]. 半闲居士. SLAM中的EKF,UKF,PF原理简介

[10]. fishmarch. 概率机器人——扩展卡尔曼滤波、无迹卡尔曼滤波. 北京航空航天大学 机器人研究所

[11]. Thrun S. Probabilistic robotics[M]. MIT Press, 2006.

[12]. 虞波, 周翟和, 赵庆涛, 胡佳佳. 一种带遗忘因子的自适应卡尔曼滤波器及其在移动机器人中的应用. 电气与自动化. Aug 2016.

[13]. Wikipedia. Alpha beta filter

One thought on “[Brushless] LAN1902 - 线性卡尔曼滤波器”

  1. 草生,这个LaTex插件如果开了MathJax在一些浏览器里干脆就一片空白加载不出来,于是现在我把全站的公式显示都改成图片模式了,不知道会不会给host带来比较大的负担……
    此外Crayon对MATLAB的着色处理的实在是有点问题,该考虑找别的代码着色插件了,

Leave a Reply

Your email address will not be published. Required fields are marked *

This site uses Akismet to reduce spam. Learn how your comment data is processed.