《Lattice Planner规划算法》的内容分享(一)

大家好,我是来自百度智能驾驶事业群的许珂诚。今天很高兴能给大家分享Apollo 3.0新发布的Lattice规划算法。

Lattice算法隶属于规划模块。规划模块以预测模块、routing模块、高精地图和定位的结果作为输入,通过算法,输出一条平稳、舒适、安全的轨迹,交给控制模块去执行。我们可以看到,规划模块在Apollo中是一个承上启下的重要模块。

这是Apollo中规划模块的工作流程。首先是依据routing和定位,通过平滑算法,生成一条平滑的参考线(平滑的道路中心线)。再通过规划算法,生成一条符合交规,安全舒适的规划轨迹。那么Lattice算法就是Apollo开源平台中,其中的一种规划算法。

一个合格规划算法,必须满足几个条件。首先,必须能够使自动驾驶汽车到达目的地;其次,必须符合交规;第三,能够避免碰撞;最后,也需要能保证一定的舒适性。

在Apollo中,规划算法的输出是一系列轨迹点连成的轨迹。每一个轨迹点包含位置,速度,加速的等信息。

下面我来介绍一下Lattice规划算法的工作流程。我们以右图中的场景为例。其中红车是我们的自动驾驶汽车,蓝车是其他障碍车,前面蓝色带尖头的曲线是蓝车的预测轨迹。那么这是一个前方即将有车辆并入的场景。

面对这样的场景,有些司机会按照右图中浅红色的轨迹,选择绕开蓝色的障碍车。另外有一些司机开车相对保守,会沿着右图中深红色较短的轨迹做一个减速,给蓝色障碍车让路。

既然对于同一个场景,人类司机会有多种处理方法,那么Lattice规划算法的第一步就是采样足够多的轨迹,提供尽可能多的选择。

Lattice规划算法的第二步是计算每一条轨迹计算的cost。这个cost考虑了轨迹的可行性、安全性等因素。我会在后面为大家详细介绍。

那么有了轨迹的cost以后,第三步就是一个循环检测的过程。在这个过程中,我们每次会先挑选出cost最低的轨迹,对其进行物理限制检测和碰撞检测。如果挑出来的轨迹不能同时通过这两个检测,就将其筛除,考察下一条cost最低的轨迹。

以右图为例,假设我们首先挑选出cost最低的是深红色较短的轨迹。但我们发现即便猛踩刹车也无法执行这条轨迹。也就是说,这条轨迹超出了汽车的减速度上限。那么它就无法通过物理限制检测,我们会将其筛除。

假设我们下一条选出来cost最低的轨迹是右图中深红色较长的轨迹。我们会发现若沿着这条轨迹前进,红车会和蓝色障碍车发生碰撞。也就是说,这条轨迹轨迹无法通过碰撞检测。于是只能放弃这条轨迹,考虑下一条cost最低的。

这样的过程循环继续下去,假设我们现在挑选出右图中靠左边的深红色轨迹,它既符合汽车的物理性状,也不会有碰撞风险。

我们最终就将这条轨迹作为规划轨迹输出。

无人驾驶汽车系统入门(五)——运动学自行车模型和动力学自行车模型[百度无人驾驶,无人驾驶论坛]

在简要了解了PID控制以后,我们就要接触一些现代的控制算法。在了解高级的车辆控制算法之前,掌握车辆运动模型是非常有必要的。车辆运动模型就是一类能够描述我们的车辆的运动规律的模型。显然,越复杂的模型就越接近现实中的车辆运动规律,本节我们一起了解一下两个广泛使用的车辆模型——运动学自行车模型(Kinematic Bicycle Model) 和 动力学自行车模型(Dynamic Bicycle Model)

原创不易,转载请注明出处:http://blog.csdn.net/adamshan/article/details/78696874

无人驾驶系统往往分成感知,决策和控制三个模块,其中无人车的路径规划和底层控制是工作在不同的层的,路径规划层往往会基于更加高层的(感知层,定位层)的信息和底层的(控制层)的实时信息指定行驶的路径,那么从路径规划层传来的就是车辆的参考路径,控制系统需要做的就是严格按照这个参考路径(以及速度等控制输入量)去驾驶我们的车辆,一般来说,我们会用多项式的行驶来描述这个路径曲线,如下所示的三次多项式就可以描述绝大多数的路径了:

其中 θ是其在 Yaw 方向的偏转角度,它是相对于 x 轴的逆时针方向的角度,v 是 θ 方向的速度,L 是车辆的轴距(前后轮胎的距离), (x,y) 是车辆的坐标。

下图是该车辆的自行车模型:

 

无人驾驶汽车系统入门(四)——反馈控制入门,PID控制[百度无人驾驶,无人驾驶论坛]

前面几篇博客介绍了卡尔曼滤波的一些基本算法,其实目标追踪,定位,传感器融合还有很多问题要处理,这些我们在以后的系列博客中在进一步细讲,现在我想给大家介绍一下无人驾驶汽车系统开发中需要的控制相关的理论和技术,还是和第一篇说的那样,我想到哪就写到哪,追踪和定位等更高级的算法我在后面会继续写。所以感兴趣的同学可以关注我的博客,无人驾驶汽车系统入门系列博客会一直更新下去。

这一篇主要讲控制的入门,为什么需要控制理论,以及最经典的PID控制,更高级的应用在实际系统中的控制算法我会在后面的文章中详述。编写不易,转载请注明出处:http://blog.csdn.net/adamshan/article/details/78458325

为什么需要控制理论

试想有如下场景,当你驾驶一辆汽车通过这个弯道的时候,假设你已经知道你要开的路线,那么你会怎么去操作控制你的车呢?

显然,如果你不是专业的选手的话,你无法做到一步到位的控制,你需要一边观察车辆相对于你想要开的路线的相对偏差,一边调整你的方向盘的角度和油门踏板的力度,这种基于环境反馈的控制我们称为 反馈控制 。反馈控制是现代控制理论的基础,这是反馈控制的一般思路:

我们希望我们控制的对象(无人车)能够按照我们希望(规划好)的路径行驶,我们会将环境当前给我们的反馈(我们当前的位置)和参考线进行比较,得到我们当前偏离参考线的距离(误差),基于这个误差,我们设计一定的算法来产生输出信号,使得这个误差不断的变小,这样的过程就是反馈控制的一般过程。那么我们如何基于这个误差来产生控制指令呢?我们最直观的感觉就是要让误差在我们的控制下逐渐变小直到为0:

0误差就意味着车一直在你想让它开的路径上开。如何减少误差就是我们这几篇博客要向大家介绍的内容。

为了了解反馈控制,我先向大家介绍 PID控制,PID控制是目前利用最为广泛的控制理论,我们以它为出发点讨论控制理论。

比例,积分和导数

PID就是指 比例(proportion)积分(integral)导数(derivative),这三项表示我们如何使用我们的误差来产生控制指令,整个流程如下:

首先是根据反馈和参考值求出误差,这里的误差根据具体的情况可以是各种度量,比如说控制车辆按照指定的路径形式,那么就是车辆当前位置和参考线的距离,控制车辆的速度在设定的值,那么就是当前速度和设定速度的差值,求出误差以后,再根据误差求比例,积分和微分三项,其中 KpKi , 和 Kd 是三项的系数,它们决定着这三项对最后输出的影响的比重。将 P,I,D 三项求和作为最后的输出信号。我们分别讨论这三项的意义。

P控制

考虑一个简单的情况,假设我们希望无人车按照图中绿线行驶,但是我们的车在如图所示的位置:

那么我们要转多少度角呢?如果都按照固定的角度转(如下图),那么车的轨迹将如图中所示:

那么显然坐这样的车是不舒服的。一个直观的解决方法就是使用比例控制。如图所示,当偏差大的时候,我们偏转更多的角度,当偏差小的时候,则偏转小一点。

那么这就是P control(比例控制)这里我们使用 CTE(Cross Track Error) 作为偏差度量 ,CTE就是我们到参考线的距离。那么这个时候转角就变成了:

其中的 e(t) 就是在t时刻的CTE,在P控制中系数 Kp会直接影响到实际的控制效果,在合理的数值范围内 Kp 越大控制的效果越好(越快速的回到参考线附近),但是,当本身位置和参考线相距很远且 Kp系数较大的时候,就会出现车辆失去控制的情况:

所以说,如果 Kp 参数设计合理的话,P控制要比固定控制要更好,但是还是不能控制的很好,因为P控制的车辆容易0值的影响,如图所示:

此时车辆虽然在参考线上,但是并不是我们希望的状态(它在下一刻就会偏离),但是对于P控制而言,这是理想状态,此时控制转角为0,因此,P控制会一次又一次的超过参考线(overshot),为了矫正这种overshot,我们需要考虑一个额外的误差项——CTE变化率

PD控制

CTE的变化率描述了我们的无人车向着参考线方向移动的有多快,如果我们的无人车一直都完美的在参考线上运动的话,那么我们的CTE变化率就为0。那么这一项(描述误差的变化率)就可以用导数来表示,那么,现在我们的控制输出就变成了比例项和导数项求和的形式:

其中的 Kd就是导数项的系数,它的大小决定了CTE变化率对于反馈控制的影响。此时我们的控制叫做PD控制,在PD控制中,我们有两个系数需要调整,直观上来看,增大 P 系数将会增大无人车向着参考线方向运动的倾向;增大 D 系数将会增大无人车快速向参考线方向的运动的“抵抗力”从而使得向参考线方向的运动变得更加平滑。使用过大的 PP 系数,过小的 D 系数的系统我们称之为 欠阻尼的(underdamped),这种情况的无人车将沿着参考线震荡前进,反之,如果P系数过小,D系数过大,那么我们称之为 过阻尼的(overdamped),这将使得无人车要较长的时间才能纠正其误差。合适地选择 PD参数可以使无人车能快速回到参考线上的同时很好的维持在参考线上运动。

PD控制似乎已经能够胜任良好的反馈控制了,但其实还不够,PD控制器可以保证正常的控制的需求,但是当环境存在扰动的时候,比如说下面这种情况:

车在受力发生轻微偏移以后,由于PD控制中下 P 项倾向于向参考线方向运动,而 D 项则尝试抵消这种倾向,造成无人车始终都无法沿着参考线运动,这个问题叫做 steady state error 为了解决这个问题,我们再引入一项—— 积分项

PID控制

我们将积分项也就如到我们的控制输出函数中,这个时候,无人车的转角就可以表示为:

其中 Ki 就是积分项系数,积分项在我们这个例子中其实很好理解,本质就是车的实际路线到参考线的图形的面积,加入积分项以后,控制函数会尽可能使车辆路线的积分尽可能小(也就是使车辆路线和实际运动参考线之间形成的形状的面积尽可能小),那么也就避免了steady state这种情况了。

同样的,这里的积分项系数的大小也会影响我们整个控制系统的稳定性,过大的 Ki 会使控制系统“震荡”地运行,过小的 Ki 又会使控制的车辆在遇到扰动以后(处于steady state)要很久才能回到参考线上,这在某些情况下势必会使车辆处于一个危险的境况。

PID控制就是由这三项共同决定的,还有其他应用于无人驾驶汽车的高级控制算法,但是他们都和我们介绍的PID控制的原理相似。

我们发现其实PID实现确实不难,但是三个系数的选择却很难,那么如何选择PID系数呢?我们可以在我们的控制循环中通过一定的算法不断尝试,下面我提供给大家一种寻找参数的算法:

 

无人驾驶汽车系统入门(三)——无损卡尔曼滤波,目标追踪,C++[百度无人驾驶,无人驾驶论坛]

前面两篇文章我们了解了卡尔曼滤波以及扩展卡尔曼滤波在目标追踪的应用,我们在上一篇文章中还具体用Python实现了EKF,但是细心的同学会发现,EKF的效率确实很低,计算雅可比矩阵确实是一个很费时的操作,当问题(非线性的)一旦变得复杂,其计算量就变得十分不可控制。在此再向大家接受一种滤波——无损卡尔曼滤波(Unscented Kalman Filter, UKF)

转载来源:http://blog.csdn.net/adamshan/article/details/78359048

通过上一篇文章,我们已经知道KF不适用于非线性系统,为了处理非线性系统,我们通过一阶泰勒展式来近似(用线性函数近似),这个方案直接的结果就是,我们对于具体的问题都要求解对应的一阶偏导(雅可比矩阵),求解雅可比矩阵本身就是费时的计算,而且我们上一篇还使用了Python而非C++,而且我们图省事还用了一个叫做numdifftools的库来实现雅可比矩阵的求解而不是自行推导,这一切都造成了我们上一次博客的代码执行效率奇低!显然现实应用里面我们的无人车是不能接受这种延迟的,我相信很多同学也和我一样讨厌求解雅可比矩阵,那么,就让我们来学习一种相对简单的状态估计算法——UKF。

UKF使用的是统计线性化技术,我们把这种线性化的方法叫做无损变换(unscented transformation)这一技术主要通过n个在先验分布中采集的点(我们把它们叫sigma points)的线性回归来线性化随机变量的非线性函数,由于我们考虑的是随机变量的扩展,所以这种线性化要比泰勒级数线性化(EKF所使用的策略)更准确。和EKF一样,UKF也主要分为预测和更新

 

 

 

 

 

无人驾驶汽车系统入门(二)——高级运动模型和扩展卡尔曼滤波[百度无人驾驶,无人驾驶论坛]

转载来源:http://blog.csdn.net/adamshan/article/details/78265754

前言:上一篇文章的最后我们提到卡尔曼滤波存在着一个非常大的局限性——它仅能对线性的处理模型和测量模型进行精确的估计,在非线性的场景中并不能达到最优的估计效果。所以之前为了保证我们的处理模型是线性的,我们上一节中使用了恒定速度模型,然后将估计目标的加减速用处理噪声来表示,这一模型用来估算行人的状态其实已经足够了,但是在现实的驾驶环境中,我们不仅要估计行人,我们除了估计行人状态以外,我们还需要估计其他车辆,自行车等等状态,他们的状态估计显然不能使用简单的线性系统来描述,这里,我们介绍非线性系统情况下的一种广泛使用的滤波算法——扩展卡尔曼滤波(Extended Kalman Filter, EKF)。

本节主要讲解非线性系统中广泛使用的扩展卡尔曼滤波算法,我们通常将该算法应用于实际的车辆状态估计(或者说车辆追踪)中。另外,实际的车辆追踪运动模型显然不能使用简单的恒定速度模型来建模,在本节中会介绍几种应用于车辆追踪的高级运动模型。并且已经其中的CTRV模型来构造我们的扩展卡尔曼滤波。最后,在代码实例中,我会介绍如何使用EKF做多传感器融合。

应用于车辆追踪的高级运动模型

首先要明确的一点是,不管是什么运动模型,本质上都是为了帮助我们简化问题,所以我们可以根据运动模型的复杂程度(次数)来给我们常用的运动模型分一下类。

一次运动模型(也别称为线性运动模型):

恒定速度模型(Constant Velocity, CV)

恒定加速度模型(Constant Acceleration, CA)

这些线性运动模型假定目标是直线运动的,并不考虑物体的转弯。

二次运动模型:

恒定转率和速度模型(Constant Turn Rate and Velocity,CTRV)

恒定转率和加速度模型(Constant Turn Rate and Acceleration,CTRA)

CTRV目前多用于机载追踪系统(飞机),这些二次运动模型大多假定速度 vv 和 偏航角速度(yaw rate) ωω 没有关系,因此,在这类运动模型中,由于偏航角速度测量的扰动(不稳定),即使车辆没有移动,我们的运动模型下的角速度也会发生细微的变化。

为了解决这个问题,速度 v和 偏航角速度 ω 的关联可以通过设定转向角 Φ 恒定来建立,这样就引出了 恒定转向角和速度模型(Constant Steering Angle and Velocity,CSAV), 另外,速度可以别假定为线性变化的,进而引出了常曲率和加速度模型(Constant Curvature and Acceleration,CCA)。

这些运动模型的关系如图:

运动模型的状态转移公式

由于除CCA以外,以上的运动模型都非常著名,故本文不提供详细的推导过程。本文提供CV和CTRV模型的状态转移公式。

状态转移公式:就是我们的处理模型由上一状态的估计计算下一个状态的先验分布的计算公式,可以理解为我们基于一定的先验知识总结出来的运动公式。

CV模型: 

CV模型的状态空间可以表示为:

 

那么转移函数为:

 

CTRV模型:

在CTRV中,目标的状态量为:

 

其中,θ为偏航角,是追踪的目标车辆在当前车辆坐标系下与x轴的夹角,逆时针方向为正,取值范围是[0, 2π), ω是偏航角速度。CTRV的状态转移函数为:

 

本文下面的内容将以CTRV模型作为我们的运动模型。使用CTRV还存在一个问题,那就是 ω=0 的情况,此时我们的状态转移函数公式中的 (x,y) 将变成无穷大。为了解决这个问题,我们考察一下ω=0 的情况,此时我们追踪的车辆实际上是直线行驶的,所以我们的 (x,y)的计算公式就变成了:

 

那么现在问题来了,我们知道,卡尔曼滤波仅仅用于处理线性问题,那么很显然我们现在的处理模型是非线性的,这个时候我们就不能简单使用卡尔曼滤波进行预测和更新了,此时预测的第一步变成了如下非线性函数:

 

其中,g()表示CTRV运动模型的状态转移函数,u 表示处理噪声。为了解决非线性系统下的问题,我们引入扩展卡尔曼滤波(Extended Kalman Filter,EKF)

扩展卡尔曼滤波

雅可比矩阵

扩展卡尔曼滤波使用线性变换来近似非线性线性变换,具体来说,EKF使用一阶泰勒展式来进行线性化:

数学中,泰勒公式是一个用函数在某点的信息描述其附近取值的公式。如果函数足够平滑的话,在已知函数在某一点的各阶导数值的情况之下,泰勒公式可以用这些导数值做系数构建一个多项式来近似函数在这一点的邻域中的值。泰勒公式还给出了这个多项式和实际的函数值之间的偏差。

回到我们的处理模型中,我们的状态转移函数为:

那么,对于这个多元函数,我们需要使用多元泰勒级数:

 

其中,Df(a)叫雅可比矩阵,它是多元函数中各个因变量关于各个自变量的一阶偏导数构成的矩阵。

 

在向量微积分中,雅可比矩阵是一阶偏导数以一定方式排列成的矩阵,其行列式称为雅可比行列式。雅可比矩阵的重要性在于它体现了一个可微方程与给出点的最优线性逼近。因此,雅可比矩阵类似于多元函数的导数。

在扩展卡尔曼滤波中,由于(x−u) 本身数值很小,那么 (x−u)就更小了,所以更高阶的级数在此问题中忽略不计,我们只考虑到利用雅可比矩阵进行线性化。

那么接下来就是求解雅可比矩阵,在CTRV模型中,对各个元素求偏导数可以得到雅可比矩阵(ω≠0):

 

当 ω=0时,雅可比矩阵为:

 

在我们后面的Python实现中,我们将使用numdifftools包直接计算雅可比矩阵,而不需要我们使用代码写这个雅可比矩阵。在得到我们CTRV模型的雅可比矩阵以后,我们的处理模型就可以写成:

处理噪声

 

处理噪声模拟了运动模型中的扰动,我们引入运动模型的出发点就是要简化我们要处理的问题,这个简化是建立在多个假设的基础上(在CTRV中,这些假设就是恒定偏航角速度和速度),但是在现实问题中这些假设就会带来一定的误差,处理噪声实际上描述了当我们的系统在运行一个指定时间段以后可能面临多大的这样的误差。在CTRV模型中噪声的引入主要来源于两处:直线加速度和偏航角加速度噪声,我们假定直线加速度和偏航角加速度满足均值为 0,方差分别为 的高斯分布,由于均值为 0, 我们在状态转移公式中的 u就可以不予考虑,我们来看噪声带来的不确定性 Q,直线加速度和偏航角加速度将影响我们的状态量(x,y,v,θ,ω),这两个加速度量对我们的状态的影响的表达式如下:

其中˙ 为直线上和转角上的加速度(在这个模型中,我们把把它们看作处理噪声),我们分解这个矩阵:

 

我们知道 QQ 就是处理噪声的协方差矩阵,其表达式为:

 

其中:

 

所以,我们在CTRV模型中的处理噪声的协方差矩阵 Q 的计算公式就是:

测量

假设我们有激光雷达和雷达两个传感器,它们分别以一定的频率来测量如下数据:

激光雷达:测量目标车辆的坐标 (x,y)。这里的x,y是相对于我们的车辆的坐标系的,即我们的车辆为坐标系的原点,我们的车头为x轴,车的左侧为y轴。

雷达:测量目标车辆在我们车辆坐标系下与本车的距离 ρ,目标车辆与x轴的夹角 ψ,以及目标车辆与我们自己的相对距离变化率 ρ˙(本质上就是目标车辆的实际速度在我们和目标车辆的连线上的分量)

前面的卡尔曼滤波器中,我们使用一个测量矩阵 H 将预测的结果映射到测量空间,那是因为这个映射本身就是线性的,现在,我们使用雷达和激光雷达来测量目标车辆(我们把这个过程称为传感器融合),这个时候会有两种情况,即:

 

激光雷达的测量模型仍然是线性的,其测量矩阵为:

 

将预测映射到激光雷达测量空间:

空间是非线性的,其表达式为:

此时我们使用 h(x) 来表示这样一个非线性映射,那么在求解卡尔曼增益时我们也要将该非线性过程使用泰勒公式将其线性化,参照预测过程,我们也只要求解 h(x)的雅可比矩阵:

虽然这个雅可比矩阵看似非常复杂,但是我们待会编程的时候并不需要完整的推导出这个雅可比矩阵的表达式,在本篇中,我们采用numdifftools这个公式来直接求解雅可比矩阵。

综上,EKF的整个过程为:

 

 

Python实现

和之前一样,为了实现交互式的代码,实例的代码为了便于理解,我们仍然使用大家熟悉的Python来实现,当然,实际无人车项目肯定是需要用C++来实现的,要将下面的示例代码使用C++来改写是非常简单快速的。

首先引入相关的库:

from __future__ import print_function

import numpy as np

import matplotlib.dates as mdates

import matplotlib.pyplot as plt

from scipy.stats import norm

from sympy import Symbol, symbols, Matrix, sin, cos, sqrt, atan2

from sympy import init_printing

init_printing(use_latex=True)

import numdifftools as nd

import math

 

首先我们读取我们的数据集,该数据集包含了追踪目标的LIDAR和RADAR的测量值,以及测量的时间点,同时为了验证我们追踪目标的精度,该数据还包含了追踪目标的真实坐标。(数据下载链接见文章末尾)

 

 

其中第一列表示测量数据来自LIDAR还是RADAR,LIDAR的2,3列表示测量的目标 (x,y),第4列表示测量的时间点,第5,6,7,8表示真实的    , RADAR测量的(前三列)是(ρ,ψ,ρ˙),其余列的数据的意义和LIDAR一样。

首先我们读取整个数据:

dataset = []

 

# read the measurement data, use 0.0 to stand LIDAR data

# and 1.0 stand RADAR data

with open(‘data_synthetic.txt’, ‘rb’) as f:

lines = f.readlines()

for line in lines:

line = line.strip(‘\n’)

line = line.strip()

numbers = line.split()

result = []

for i, item in enumerate(numbers):

item.strip()

if i == 0:

if item == ‘L’:

result.append(0.0)

else:

result.append(1.0)

else:

result.append(float(item))

dataset.append(result)

f.close()

 

 

P = np.diag([1.0, 1.0, 1.0, 1.0, 1.0])

print(P, P.shape)

H_lidar = np.array([[ 1.,  0.,  0.,  0.,  0.],

[ 0.,  1.,  0.,  0.,  0.]])

print(H_lidar, H_lidar.shape)

 

R_lidar = np.array([[0.0225, 0.],[0., 0.0225]])

R_radar = np.array([[0.09, 0., 0.],[0., 0.0009, 0.], [0., 0., 0.09]])

print(R_lidar, R_lidar.shape)

print(R_radar, R_radar.shape)

 

# process noise standard deviation for a

std_noise_a = 2.0

# process noise standard deviation for yaw acceleration

std_noise_yaw_dd = 0.3

 

在整个预测和测量更新过程中,所有角度量的数值都应该控制在 [−π,π],我们知道角度加减 2π 不变,所以用如下函数表示函数来调整角度:

 

def control_psi(psi):

while (psi > np.pi or psi < -np.pi):

if psi > np.pi:

psi = psi – 2 * np.pi

if psi < -np.pi:

psi = psi + 2 * np.pi

return psi

 

使用第一个雷达(或者激光雷达)的测量数据初始化我们的状态,对于激光雷达数据,可以直接将测量到的目标的 (x,y) 坐标作为初始 (x,y),其余状态项初始化为0,对于雷达数据,可以使用如下公式由测量的 ρ,ψ,ρ˙得到目标的坐标 (x,y):

x=ρ×cos(ψ)

 

y=ρ×sin⁡(ψ)

具体状态初始化代码为:

 

state = np.zeros(5)

init_measurement = dataset[0]

current_time = 0.0

if init_measurement[0] == 0.0:

print(‘Initialize with LIDAR measurement!’)

current_time = init_measurement[3]

state[0] = init_measurement[1]

state[1] = init_measurement[2]

 

else:

print(‘Initialize with RADAR measurement!’)

current_time = init_measurement[4]

init_rho = init_measurement[1]

init_psi = init_measurement[2]

init_psi = control_psi(init_psi)

state[0] = init_rho * np.cos(init_psi)

state[1] = init_rho * np.sin(init_psi)

print(state, state.shape)

 

 

写一个辅助函数用于保存数值:

# Preallocation for Saving

px = []

py = []

vx = []

vy = []

 

gpx = []

gpy = []

gvx = []

gvy = []

 

mx = []

my = []

 

def savestates(ss, gx, gy, gv1, gv2, m1, m2):

px.append(ss[0])

py.append(ss[1])

vx.append(np.cos(ss[3]) * ss[2])

vy.append(np.sin(ss[3]) * ss[2])

 

gpx.append(gx)

gpy.append(gy)

gvx.append(gv1)

gvy.append(gv2)

mx.append(m1)

my.append(m2)

 

定义状态转移函数和测量函数,使用numdifftools库来计算其对应的雅可比矩阵,这里我们先设 Δt=0.05,只是为了占一个位置,当实际运行EKF时会计算出前后两次测量的时间差,一次来替换这里的 Δt。

measurement_step = len(dataset)

state = state.reshape([5, 1])

dt = 0.05

 

I = np.eye(5)

 

transition_function = lambda y: np.vstack((

y[0] + (y[2] / y[4]) * (np.sin(y[3] + y[4] * dt) – np.sin(y[3])),

y[1] + (y[2] / y[4]) * (-np.cos(y[3] + y[4] * dt) + np.cos(y[3])),

y[2],

y[3] + y[4] * dt,

y[4]))

 

# when omega is 0

transition_function_1 = lambda m: np.vstack((m[0] + m[2] * np.cos(m[3]) * dt,

                                                             m[1] + m[2] * np.sin(m[3]) * dt,

                                                                        m[2],

m[3] + m[4] * dt,

m[4]))

 

J_A = nd.Jacobian(transition_function)

J_A_1 = nd.Jacobian(transition_function_1)

# print(J_A([1., 2., 3., 4., 5.]))

 

measurement_function = lambda k: np.vstack((np.sqrt(k[0] * k[0] + k[1] * k[1]),

math.atan2(k[1], k[0]),

(k[0] * k[2] * np.cos(k[3]) + k[1] * k[2] * np.sin(k[3])) / np.sqrt(k[0] * k[0] + k[1] * k[1])))

J_H = nd.Jacobian(measurement_function)

# J_H([1., 2., 3., 4., 5.])

 

 

 

EKF的过程代码:

for step in range(1, measurement_step):

 

# Prediction

# ========================

t_measurement = dataset[step]

if t_measurement[0] == 0.0:

m_x = t_measurement[1]

m_y = t_measurement[2]

z = np.array([[m_x], [m_y]])

 

dt = (t_measurement[3] – current_time) / 1000000.0

current_time = t_measurement[3]

 

# true position

g_x = t_measurement[4]

g_y = t_measurement[5]

g_v_x = t_measurement[6]

g_v_y = t_measurement[7]

 

else:

m_rho = t_measurement[1]

m_psi = t_measurement[2]

m_dot_rho = t_measurement[3]

z = np.array([[m_rho], [m_psi], [m_dot_rho]])

 

dt = (t_measurement[4] – current_time) / 1000000.0

current_time = t_measurement[4]

 

# true position

g_x = t_measurement[5]

g_y = t_measurement[6]

g_v_x = t_measurement[7]

g_v_y = t_measurement[8]

 

if np.abs(state[4, 0]) < 0.0001:  # omega is 0, Driving straight

state = transition_function_1(state.ravel().tolist())

state[3, 0] = control_psi(state[3, 0])

JA = J_A_1(state.ravel().tolist())

else:  # otherwise

state = transition_function(state.ravel().tolist())

state[3, 0] = control_psi(state[3, 0])

JA = J_A(state.ravel().tolist())

 

 

G = np.zeros([5, 2])

G[0, 0] = 0.5 * dt * dt * np.cos(state[3, 0])

G[1, 0] = 0.5 * dt * dt * np.sin(state[3, 0])

G[2, 0] = dt

G[3, 1] = 0.5 * dt * dt

G[4, 1] = dt

 

Q_v = np.diag([std_noise_a*std_noise_a, std_noise_yaw_dd*std_noise_yaw_dd])

Q = np.dot(np.dot(G, Q_v), G.T)

 

# Project the error covariance ahead

P = np.dot(np.dot(JA, P), JA.T) + Q

 

# Measurement Update (Correction)

# ===============================

if t_measurement[0] == 0.0:

# Lidar

S = np.dot(np.dot(H_lidar, P), H_lidar.T) + R_lidar

K = np.dot(np.dot(P, H_lidar.T), np.linalg.inv(S))

 

y = z – np.dot(H_lidar, state)

y[1, 0] = control_psi(y[1, 0])

state = state + np.dot(K, y)

state[3, 0] = control_psi(state[3, 0])

# Update the error covariance

P = np.dot((I – np.dot(K, H_lidar)), P)

 

# Save states for Plotting

savestates(state.ravel().tolist(), g_x, g_y, g_v_x, g_v_y, m_x, m_y)

 

else:

# Radar

JH = J_H(state.ravel().tolist())

 

S = np.dot(np.dot(JH, P), JH.T) + R_radar

K = np.dot(np.dot(P, JH.T), np.linalg.inv(S))

map_pred = measurement_function(state.ravel().tolist())

if np.abs(map_pred[0, 0]) < 0.0001:

# if rho is 0

map_pred[2, 0] = 0

 

y = z – map_pred

y[1, 0] = control_psi(y[1, 0])

 

state = state + np.dot(K, y)

state[3, 0] = control_psi(state[3, 0])

# Update the error covariance

P = np.dot((I – np.dot(K, JH)), P)

 

savestates(state.ravel().tolist(), g_x, g_y, g_v_x, g_v_y, m_rho * np.cos(m_psi), m_rho * np.sin(m_psi))

 

这里有几点需要注意,首先,要考虑清楚有几个地方被除数有可能为 0,比如说ω=0,以及 ρ=0 的情况。

处理完以后我们输出估计的均方误差(RMSE),并且把各类数据保存以便我们可视化我们的EKF的效果:

 

def rmse(estimates, actual):

result = np.sqrt(np.mean((estimates-actual)**2))

return result

 

print(rmse(np.array(px), np.array(gpx)),

rmse(np.array(py), np.array(gpy)),

rmse(np.array(vx), np.array(gvx)),

rmse(np.array(vy), np.array(gvy)))

 

# write to the output file

stack = [px, py, vx, vy, mx, my, gpx, gpy, gvx, gvy]

stack = np.array(stack)

stack = stack.T

np.savetxt(‘output.csv’, stack, ‘%.6f’)

 

最后我们来看一下我们的EKF在追踪目标的时候的均方误差:

0.0736336090893 0.0804598933194 0.229165985264 0.309993887661

我们把我们的EKF的估计结果可视化:

我们放大一个局部看一下:

其中,蓝色的点为我们的EKF对目标位置的估计,橙色的点为来自LIDAR和RADAR的测量值,绿色的点是目标的真实位置,由此可知,我们的测量是不准确的,因此我们使用EKF在结合运动模型的先验知识以及融合两个传感器的测量的基础上做出了非常接近目标正是状态的估计。

EKF的魅力其实还不止在此!我们使用EKF,不仅能够对目标的状态(我们关心的)进行准确的估计,从这个例子我们会发现,EKF还能估计我们的传感器无法测量的量(比如本例中的v,ψ,ψ)

 

那么,扩展卡尔曼滤波就到此结束了,大家可能会问,怎么没看到可视化结果那一部分的代码?哈哈,为了吸引一波人气,请大家关注我的博客,我会在下一期更新中(无损卡尔曼滤波)提供这一部分代码。

 

最后,细心的同学可能会发现,我们这个EKF执行的效率太低了,实际上,EKF的一个最大的问题就是求解雅可比矩阵计算量比较大,而且相关的偏导数推导也是非常无聊的工作,因此引出了我们下一篇要讲的内容,无损卡尔曼滤波(Unscented Kalman Filter,UKF)

附数据下载链接:http://download.csdn.net/download/adamshan/10033533