粒子滤波源代码（滤波器 代码）

本篇文章给大家谈谈粒子滤波源代码，以及滤波器 代码对应的知识点，希望对各位有所帮助，不要忘了收藏本站喔。

本文目录一览：

• 1、粒子滤波的粒子滤波（PF：Particle Filter）
• 2、救命啊！！关于改进粒子滤波算法问题
• 3、DSP和ARM的区别是什么？
• 4、浅谈各种定位技术对比
• 5、自动驾驶技术基本知识介绍
• 6、opencv怎么计算物体移动速度 求具体解决方法,最好能有源代码参考一下,

粒子滤波的粒子滤波（PF：Particle Filter）

与卡尔曼滤波（Kalman Filter）相比较

粒子滤波(PF: Particle Filter)的思想基于蒙特卡洛方法(Monte Carlo methods)，它是利用粒子集来表示概率，可以用在任何形式的状态空间模型上。其核心思想是通过从后验概率中抽取的随机状态粒子来表达其分布，是一种顺序重要性采样法(Sequential Importance Sampling)。简单来说，粒子滤波法是指通过寻找一组在状态空间传播的随机样本对概率密度函数进行近似，以样本均值代替积分运算，从而获得状态最小方差分布的过程。这里的样本即指粒子,当样本数量N→∝时可以逼近任何形式的概率密度分布。

尽管算法中的概率分布只是真实分布的一种近似，但由于非参数化的特点，它摆脱了解决非线性滤波问题时随机量必须满足高斯分布的制约，能表达比高斯模型更广泛的分布，也对变量参数的非线性特性有更强的建模能力。因此，粒子滤波能够比较精确地表达基于观测量和控制量的后验概率分布，可以用于解决SLAM问题。 在现代目标跟踪领域，由于实际问题的复杂性，所面对的更多的是非线性非高斯问题，Hue等把PF推广到多目标跟踪和数据关联 ，Gordon等对杂波中的目标跟踪问题提出混合粒子滤波器弼 ，Mcginnity等提出机动目标跟踪的多模型粒子滤波器 ，Doucet等对跳跃Markov系统状态估计提出了更有效的PF算法 j，Guo把PF用于传感器网络下的协同跟踪 J，Freitas等用PF训练神经网络 ，Srivastava等把PF用于自动目标识别 ，Fox等把PF用于移动机器人定位 ，Ward等提出语音源定位的PF算法，Orton等对来自多个传感器的无序量测提出基于PF的多目标跟踪和信息融合方法 ，Penny等使用PF实现多传感器资源最优管理和部署 ，Hernandez等结合PF、数据融合和优化算法实现多传感器资源管理 ．研究表明PF是解决此类非线性问题的有力工具之一．PF在计算机视觉、可视化跟踪领域被称为凝聚算法(CONDENsATION)，该领域是PF的一个非常活跃的应用领域，Bruno提出图像序列中目标跟踪的PF算法 ，Maskell等提出基于图像传感器多目标跟踪的PF算法_4 ．在听觉视觉联合目标定位和跟踪方面，Vermaak等利用PF提出声音和视觉融合的集成跟踪 ，Zotkin等使用PF将来自多个摄像机和麦克风组的视觉听觉信息融合跟踪移动目标。

在粒子滤波算法下一些传统的难点问题如目标检测、遮挡、交叉、失跟等得到更好的结果．在无线通讯中PF被广泛用于信道盲均衡、盲检测、多用户检测等方面．其它的应用领域还有机器人视觉跟踪 、导航 、图象处理 、生物信息 引、故障诊断和过程控制 、金融数据处理 等．研究表明在有关非高斯非线性系统的数据处理和分析领域PF都具有潜在的应用价值．值得一提的是国内学者在PF的研究上也取得许多成果，莫等利用PF算法提出一种混合系统状态监测与诊断的新方法 ，Chen等利用PF预测非线性系统状态分布，获得故障预测概率 ，Li等提出基于PF的可视化轮廓跟踪方法 J，Shan等提出基于PF的手形跟踪识别方法 ，Hu等提出闪烁噪声下的PF跟踪算法 等，这些工作推动PF在国内的研究． 粒子滤波器采用一组随机粒子逼近状态的后验概率分布，有可能用粒子逼近平滑分布，由于重采样使得粒子丧失多样性，直接由滤波分布边缘化得到的平滑分布效果很差，Doucet等应用MCMC方法增加样本多样性用于固定延迟平滑取得好的效果，Fong等把RBPF推广到粒子平滑器，并用于语音信号处理 1．

在PF的性能优化方面，目前大多优化某个局部的性能指标，如重要性权的方差等，Doucet等使用随机逼近对PF关于某个全局性能指标进行在线优化 ，Chan等人进一步利用SPSA随机优化方法优化PF ，避免1r梯度的计算．为了减少计算量使得PF能用于实时数据处理，Foxt提出了粒子个数可变的自适应粒子滤波器 ，Kwok等把粒子划分为小的集合，每个小样本集可以进行实时处理，采用加权和的方法逼近状态后验分布 ，Brun等提出PF的并行结构算法以获得在线实时应用 ．

最近几年，粒子方法出现了又一些新的发展，一领域用传统的分析方法解决不了的问题，现在可以借助基于粒子仿真的方法来解决．在动态系统的模型选择，故障检测、诊断方面，出现了基于粒子的假设检验、粒子多模型、粒子似然度比检测等方法．在参数估计方面，通常把静止的参数作为扩展的状态向量的一部分，但是由于参数是静态的，粒子会很快退化成一个样本，为避免退化，常用的方法有给静参数人为增加动态噪声_9 以及Kernel平滑方法 ，而Doucet等提出的点估计方法避免对参数直接采样，在粒子框架下使用最大似然估计(ML)以及期望值最大(EM)算法直接估计未知参数 ．在随机优化方面，出现了基于粒子方法的梯度估计算法，使得粒子方法也用于最优控制等领域．Andrieu，Doucet等在文献[70]中详细回顾了粒子方法在变化检测、系统辨识和控制中的应用及理论上的一些最新进展，许多仅仅在几年前不能解决的问题现在可以求助于这种基于仿真的粒子方法．

总结与展望(Summarization and prospect)

目前粒子滤波器的研究已取得许多可喜的进展，应用范围也由滤波估计扩展到新的领域，作为一种新方法，粒子方法还处于发展之中，还存在许多有待解决的问题，例如随机采样带来Monte Carlo误差的积累甚至导致滤波器发散、为避免退化和提高精度而需要大量的粒子使得计算量急剧增加、粒子方法是否是解决非线性非高斯问题的万能方法还值得探讨．此外粒子滤波器还只是停留在仿真阶段，全面考虑实际中的各种因素也是深化PF研究不可缺少的一个环节．尽管如此，在一些精度要求高而经典的分析方法又解决不了的场合，这种基于仿真的逼近方法发挥了巨大潜力，而现代计算机和并行计算技术的迅速发展又为粒子方法的发展和应用提供了有力支持，相信粒子滤波器的研究将朝着更深，更广的方向发展．

救命啊！！关于改进粒子滤波算法问题

粒子滤波算法的核心思想是：为了求解数学或物理等方面的问题，首先建立一个概率模型或者随机过程，使它的参数等于问题的解；然后通过对模型或过程的观察或采样试验计算所求参数的统计特征；最后给出所求解的近似值。

下面详细介绍粒子滤波的基本思想。

粒子滤波就是用完全描述后验概率密度分布

这里，x0：k={xj，j=0，…，k}，z0：k={zj，j=0，…，k}，分别表示各个时刻的系统状态和观测状态，表示j时刻所对应的粒子的归一化权值，即

直接从后验概率p(x0：k|z1：k)中进行取样是比较困难的。假设存在π(x)，有并且可以很方便地从π(x)中进行取样，这样的π(x)称作重要性密度。根据贝叶斯理论有：

这样，就能很容易对系统状态进行估计，权值的递推方程可以写成：

在k时刻的后验概率密度可以近似地写成：

1.2 粒子滤波算法改进策略

传统的粒子滤波算法需要使用状态转移后的所有粒子进行系统观测和重采样。这使得计算量很大，而且增加了错误信息，甚至会导致跟踪目标漂移。鉴于此，笔者对传统的粒子滤波进行了一系列的改进。在对系统观测过程进行改进时，只选取局部最优粒子（即权值较大的粒子）进行状态转移；在重采样环节，也使用了这种局部最优原理，只选取部分大权值粒子。改进的粒子滤波算法,能够在很大程度上解决上述问题。

图1描述的是状态转移环节，当前时刻所有粒子对应的位置服从均匀分布。图中“○”表示跟踪目标在该时刻的真实位置，黑点表示该时刻的粒子，黑点的大小代表粒子权值的大小。

由图发现，离“○”越近的粒子权值越大，超出1/2粒子传播半径的粒子，权值可忽略。根据这种先验知识，基于局部最优化原理选取部分粒子。当利用粒子进行系统观测时，将粒子按权值大小依次排序，只需选取半数大权值粒子，就能很准确地计算出最优估计的位置。这种改进策略可使该环节的计算量减少40%。由于忽略了远处的粒子，剔除了部分错误信息，因此跟踪精度也得到了提高。

粒子重采样环节的改进是分裂粒子时只选取上述的半数大权值粒子。改进的重采样具体实现过程如下：将系统观测过程中使用的粒子，按大小排序均分为两组。权值大的一组，每个粒子分裂为三个新的粒子；权值较小的一组，每个粒子只生成一个新粒子。这样做不但简化了计算，提高了运行速度，而且增强了重采样粒子的有效性。

该改进算法的优点：(1)使得后验分布样本更加接近真实分布；(2)大大减少了计算量。多次实验表明，改进算法在人体运动跟踪时较传统算法效果更好，鲁棒性更高。

2 基于改进粒子滤波算法的人体运动跟踪

视频中的特定人体有其独特的运动特征，且其关节运动都是非刚性的。为使运动模型在运动预测中有更好的适用性和更高的准确性，依据统计分析的成果，采用一阶自回归过程ARP（Auto-Regressive Process）动力学模型作为运动模型，并通过训练序列计算出该模型的参数。在模板区域相似性计算的基础上，通过粒子滤波实现人体运动的跟踪。基于这一基本思路，本文提出基于粒子滤波的人体运动跟踪的流程图，如图2所示。

2.1 初始化

跟踪初始化，就是按运动模型的要求，在初始帧形成表示各自分布模型的粒子集。具体过程如下：在被跟踪序列的初始帧中手动提取目标模板，将它作为起始帧的状态向量。提取目标的初始运动参数在状态向量的各分量上加正态随机噪声，构造N个状态向量，即粒子数为N，每个粒子代表一个可能的运动状态。若初始的权值ωi为1，则具有N个运动状态参数这里Pi选择Pinit附近的点。

2.2 采样

采样就是从表示先验模型的状态向量集中按概率选取其中的若干向量。一次采样选取一个状态向量，进行N次这样的采样，得到N个新的状态向量。显然，权值太小的状态向量(即粒子)在采样过程中被选中的概率就小，而且在逐步迭代中可能被丢掉；权值大的粒子被选中的概率就比较大，也可能被多次选中。这样，采样得到的新的粒子集能更好地估计概率分布。

2.3 状态转移

系统状态转移，即粒子的传播过程。从采样得到的新的粒子集出发，经运动模型计算得到预测模型。粒子传播是一种随机运动过程，服从一阶ARP方程。

式中，xt为目标在t时刻的状态，wt-1是归一化噪声量，A和B是常数。这里，t时刻系统的状态转移过程与当前时刻的观测量无关。

粒子Ni的运动状态参量为：

式中，A1、A2、B1、B2为常数，一般A取1，B为粒子传播半径（系统状态转移过程中，粒子所能够传播的范围），W是[-1，1]内的随机数。系统状态转移的实质就是在坐标上迭加一个扰动量。

这一步是先验概率的传播过程，即“假设”目标状态将以何种方式传播。粒子传播是否合理需要通过下一阶段即系统观测进行验证。

2.4 系统观测

系统观测过程就是将当前预测模型中的每一个状态向量所对应的外观模型与由其确定的当前帧图像中的对应区域进行相似性计算，并给每一个状态向量赋一个权值。

每个粒子状态转移后，用对应新坐标计算一个MADi。

定义概率密度函数为：

它作为当前帧的跟踪结果输出。

2.5 重采样

按上述采样、转移和观测过程完成当前帧的跟踪计算后，对当前帧的后验模型进行同样的采样，得到下一帧的先验模型。进而可以对下一帧进行新的预测和观测，继续后续序列的跟踪计算，直至最后一帧，完成跟踪过程。

3 实验仿真和分析

3.1 仿真结果

DSP和ARM的区别是什么？

数据处理方面用DSP，因为他可以实现比较复杂的运算，我指的是硬件完成运算，比如除法，一般的arm没有除法器，而是把除法变成加法等运算，所以要很多步实现比较慢，这点你看看c编译成的汇编就可以看出。当然不只是除法其它微分卷积等等。

arm优点在于外设，包括ad，da等等，所以一般用在控制类的电子产品上。

当然高级点的arm中有的也会集成dsp处理器，起到相互补充的作用。 

举例:比如数码相机用的是dsp处理的图片，实现粒子滤波算法等等。

而汽车中有的电子仪表，就是有个小液晶屏上面显示各个门，油量等等那个用的是arm，因为不需要大量处理数据，只需采集显示。

浅谈各种定位技术对比

蜂窝移动网络定位技术

随着第二代、第三代到第四代移动网络通信长期演进(Long Term Evolution,LTE)定位技术的发展，基于基站的蜂窝移动网络定位技术的精度得到了较大提高;第五代移动网络通信技术协议投入商用对室内定位领域是一个巨 大的契机，其密集组网技术也使得基站定位具备广阔的应用前景和发展空间。蜂窝定位技术可以便捷使用搭建的基础设施，依靠移动通信系统的体系结构和传输信息实现用户的位置坐标推算。利用室内可直按测得的无线电通信信号，与WiFi、蓝牙、UWB技术相同，既可基于信号强度使用传统的位置指纹匹配方法，也可以进行TOA、TDOA、AOA等测距方式测量。蜂窝移动网络定位技术依赖通信基站，与基站密度密切相关:虽然室内信号受基站输出功率的动态调整和非视距传播效应的影响，定位精度不高，但在室内外无缝定位需求下，可作为普适化的室内外坐标- -体化的定位方案。

其他室内定位技术

1）伪卫星定位技术。 伪卫星是指安装在地面附近的能够发射类似于GNSS信号的装置，其本质是-一个GNSS信号模拟器，可以作为室内环境中对GNSS信号的补充。伪卫星技术定位的规模化难度比较低，同时定位精度为亚米级，能够满足大多数时候的定位需求，但是较高的基站部署成本使该技术停留在专业领域，尚未投入市场使用。目前国内上海交通大学、中国电子 科技 集团公司第54研究所也对伪卫星技术进行了深入的研究，对伪卫星的组网配置方案进行了详细的研究和分析，共同探讨伪p星独立组网配置方案的可行方案。

2)基于天然信源的室内定位技术。 基于天然信源的室内定位技术是指利用传感器将某此与位置相关的天然信源转换为可用于定位的信号以实现定位，例如，惯性导航技术利用惯性传感器感知载体的运动状态:地磁导航技术利用地磁传感器获取当前位置的磁场特征:气压计测高技术利用气压计测量当前位置的气压等。

3)惯性定位。 惯性导航技术是基于惯性测量单元(Inertial MeasuremenUnis, IMU)对状态进行预测，具体是利用加速度计、陀螺仪和磁力计等传感器对前一时刻的位置信息进行处理，得到当前时刻的相对位置。随着传感器的消失型集成化与低成本，近些年来IMU被广泛应用于室内定位导航。惯性导航系统基于航位推算方法实现终端的定位，具备较强的自主性，短时间内的定位精度连续性非常高;但定位导航精度极大地受限于器件成本，且不可避免地随着时间的推移产生累积误差，需要借助外界定位信息源不断地对位置推算进行校准准。零速校正是惯性导航技术中的一种误差补偿技术， 可以有效控制长时间的累积积分误差，提高系统精度。

4)地磁定位。 地球的磁场特性最先被广泛用于航海和军事等室外定位。地磁定位同样可以采用指纹匹配的方法，通过事先采集并构建精确的地磁指纹数据库，利用传感器获取人员当前位置的磁场数据，将实时数据与地磁指纹库基准备数据精确匹配获得最佳估测值，从而实现人员在指定区域中的定位。由于地球磁场分布方向的原因，室内采集到的地磁3轴数据本质上只具备2个维度的指纹信息，大型建筑物的室内地磁特征差异不明显，在传统的室内区域栅格化指纹路匹配方法中表现不佳，因此室内地磁信息多用于室内定位的多源信息融合，与惯性导航系统组合使用，起到辅助和误差纠正的作用。

5)多源融合定位技术。 上文介绍的各种定位技术各具优势和局限性，例如如，WiFi、 蓝牙和UWB信号属于射频信号，易受多径效应的影响;惯性导航虽不依赖外置信源，但定位误差会随时间累积。目前，国内主流的室内定位方法是根据场景需求及各类室内定位技术的特点，选择2种及以上的定位技术进行融合以获得当前位置的最优估计。融合方法有松耦合和紧耦合两种方法，两者的区别在于:松耦合需要各类传感器提供定位结果，而紧耦合需要各类传感器直接提供观测信息;松耦合易于实现，但要求各类传感器均输出定位结果，紧耦合与松耦合相比实现难度大，但各类传感器只需提供观测信息即可。信息融合理地实现依赖滤波算法，如卡尔曼滤波、无迹滤波和粒子滤波，目前工程应用多采用卡尔曼滤波器。融合定位的信息源可以是多种多样的，如GNSS信号、加速度计/陀螺仪、基站信号、WiFi、 蓝牙、气压计、地磁、视觉、室内地图等;但融合定位模型和方案同样需要考虑室内定位结果的精度和可靠性:多种信息的协同融合可以带来精度的提升，同样可能会导致灾难性的定位失准。获得传感器数据后需要对更多来源信息进行预处理以剔除原生和融合噪声，从数据中提取特征后要根据不同应用情景、设备条件和具体需求进行特征级融合，赋予不同的权重，结合地图信息和各种状态估计滤波算法后进行决策级融合。随着室内定位技术的不断进步，定位精度也逐渐提高到米级甚至亚米级，开始迈入消费级市场的水平。下表所示为不同室内定位技术的对比情况

自动驾驶技术基本知识介绍

自动驾驶车，是一种无须人工干预而能够感知其周边环境和导航的车辆。它利用了包括雷达、激光、超声波、GPS、里程计、计算机视觉等多种技术来感知其周边环境，通过先进的计算和控制系统，来识别障碍物和各种标识牌，规划合适的路径来控制车辆行驶。

美国汽车工程师协会（SAE，Society of Automotive Engineers），则将自动驾驶划分为 0~5 共六级。

Level 0：无自动化（No Automation）

没有任何自动驾驶功能或技术，人类驾驶员对汽车所有功能拥有绝对控制权。驾驶员需要负责转向、加速、制动和观察道路状况。任何驾驶辅助技术，例如现有的前向碰撞预警、车道偏离预警，以及自动雨刷和自动前灯控制等，虽然有一定的智能化，但是仍需要人来控制车辆，所以都仍属于 Level 0。

Level 1：驾驶辅助（Driver Assistance）

驾驶员仍然对行车安全负责，不过可以授权部分控制权给系统管理，某些功能可以自动进行，比如常见的自适应巡航（Adaptive Cruise Control，ACC）、应急刹车辅助（Emergency Brake Assist，EBA）和车道保持（Lane-Keep Support，LKS）。Level 1 的特点是只有单一功能，驾驶员无法做到手和脚同时不操控。

Level 2：部分自动化（Partial Automation）

人类驾驶员和汽车来分享控制权，驾驶员在某些预设环境下可以不操作汽车，即手脚同时离开控制，但驾驶员仍需要随时待命，对驾驶安全负责，并随时准备在短时间内接管汽车驾驶权。比如结合了 ACC 和 LKS 形成的跟车功能。Level 2 的核心不在于要有两个以上的功能，而在于驾驶员可以不再作为主要操作者。

Level 3：有条件自动化（Conditional Automation）

在有限情况下实现自动控制，比如在预设的路段（如高速和人流较少的城市路段），汽车自动驾驶可以完全负责整个车辆的操控，但是当遇到紧急情况，驾驶员仍需要在某些时候接管汽车，但有足够的预警时间，如即将进入修路的路段（Road work ahead）。Level 3 将解放驾驶员，即对行车安全不再负责，不必监视道路状况。

Level 4：高度自动化（High Automation）

自动驾驶在特定的道路条件下可以高度自动化，比如封闭的园区、高速公路、城市道路或固定的行车线路等，这这些受限的条件下，人类驾驶员可以全程不用干预。

Level 5：完全自动化（Full Automation）

对行车环境不加限制，可以自动地应对各种复杂的交通状况和道路环境等，在无须人协助的情况下由出发地驶向目的地，仅需起点和终点信息，汽车将全程负责行车安全，并完全不依赖驾驶员干涉，且不受特定道路的限制。

注释：DDT(Dynamic driving task):动态驾驶任务，指汽车在道路上行驶所需的所有实时操作和策略上的功能，不包括行程安排、目的地和途径地的选择等战略上的功能。

无人驾驶系统的核心可以概述为三个部分：感知（Perception），规划（Planning）和控制（Control），这些部分的交互以及其与车辆硬件、其他车辆的交互可以用下图表示：

感知是指无人驾驶系统从环境中收集信息并从中提取相关知识的能力。其中，环境感知（Environmental Perception）特指对于环境的场景理解能力，例如障碍物的位置，道路标志/标记的检测，行人车辆的检测等数据的语义分类。 一般来说，定位（Localization）也是感知的一部分，定位是无人车确定其相对于环境的位置的能力。

为了确保无人车对环境的理解和把握，无人驾驶系统的环境感知部分通常需要获取周围环境的大量信息，具体来说包括：障碍物的位置，速度以及可能的行为，可行驶的区域，交通规则等等。无人车通常是通过融合激光雷达（Lidar），相机（Camera），毫米波雷达（Millimeter Wave Radar）等多种传感器的数据来获取这些信息。

车载雷达传感器功能及优缺点各有不同，相关比较如下表所示：

激光雷达 是一类使用激光进行探测和测距的设备，它能够每秒钟向环境发送数百万光脉冲，它的内部是一种旋转的结构，这使得激光雷达能够实时的建立起周围环境的3维地图。

通常来说，激光雷达以10Hz左右的速度对周围环境进行旋转扫描，其扫描一次的结果为密集的点构成的3维图，每个点具备（x,y,z）信息，这个图被称为点云图（Point Cloud Graph），如下图所示，是使用Velodyne VLP-32c激光雷达建立的一个点云地图：

激光雷达因其可靠性目前仍是无人驾驶系统中最重要的传感器，然而，在现实使用中，激光雷达并不是完美的，往往存在点云过于稀疏，甚至丢失部分点的问题，对于不规则的物体表面，使用激光雷达很难辨别其模式，另一个比较大的挑战是一个比较大的挑战是激光雷达感知范围比较近，感知范围平均在 150m 左右，这取决于环境和障碍物的不同。激光雷达在角分辨度上也远远不及照相机。激光雷达对环境的敏感度也是比较大的，例如雨天中，车辆行驶中溅起来的水花，在激光雷达上都是有噪点的。

毫米波雷达 通过发射电磁波并通过检测回波来探测目标的有无、距离、速度和方位。由于毫米波雷达技术相对成熟，成本较低，并且在不良天气下表现良好，因此成为感知设备中重要的一环。但由于其分辨率较低，因此不能作为激光雷达的替代品，而是激光雷达的重要补充设备。

摄像机 根据镜头和布置方式的不同主要有以下四种：单目摄像机、双目摄像机、三目摄像机和环视摄像机。

单目摄像机 模组只包含一个摄像机和一个镜头。由于很多图像算法的研究都是基于单目摄像机开发的，因此相对于其他类别的摄像机，单目摄像机的算法成熟度更高。但是单目有着两个先天的缺陷。一是它的视野完全取决于镜头。焦距短的镜头，视野广，但缺失远处的信息。反之亦然。因此单目摄像机一般选用适中焦距的镜头。二是单目测距的精度较低。摄像机的成像图是透视图，即越远的物体成像越小。近处的物体，需要用几百甚至上千个像素点描述；而处于远处的同一物体，可能只需要几个像素点即可描述出来。这种特性会导致，越远的地方，一个像素点代表的距离越大，因此对单目来说物体越远，测距的精度越低。

双目摄像机 由于单目测距存在缺陷，双目摄像机应运而生。相近的两个摄像机拍摄物体时，会得到同一物体在摄像机的成像平面的像素偏移量。有了像素偏移量、相机焦距和两个摄像机的实际距离这些信息，根据数学换算即可得到物体的距离。虽然双目能得到较高精度的测距结果和提供图像分割的能力，但是它与单目一样，镜头的视野完全依赖于镜头。而且双目测距原理对两个镜头的安装位置和距离要求较多，这就会给相机的标定带来麻烦。

三目摄像机 由于单目和双目都存在某些缺陷，因此广泛应用于无人驾驶的摄像机方案为三目摄像机。三目摄像机其实就是三个不同焦距单目摄像机的组合。根据焦距不同，每个摄像机所感知的范围也不尽相同。对摄像机来说，感知的范围要么损失视野，要么损失距离。三目摄像机能较好地弥补感知范围的问题。因此在业界被广泛应用。正是由于三目摄像机每个相机的视野不同，因此近处的测距交给宽视野摄像头，中距离的测距交给主视野摄像头，更远的测距交给窄视野摄像头。这样一来每个摄像机都能发挥其最大优势。三目的缺点是需要同时标定三个摄像机，因而工作量更大。其次软件部分需要关联三个摄像机的数据，对算法要求也很高。

环视摄像机， 之前提到的三款摄像机它们所用的镜头都是非鱼眼的，环视摄像机的镜头是鱼眼镜头，而且安装位置是朝向地面的。某些高配车型上会有“360°全景显示”功能，所用到的就是环视摄像机。安装于车辆前方、车辆左右后视镜下和车辆后方的四个鱼眼镜头采集图像，鱼眼摄像机为了获取足够大的视野，代价是图像的畸变严重。环视摄像机的感知范围并不大，主要用于车身5~10米内的障碍物检测、自主泊车时的库位线识别等。

为了理解点云信息，通常来说，我们对点云数据进行两步操作：分割（Segmentation）和分类（Classification）。其中，分割是为了将点云图中离散的点聚类成若干个整体，而分类则是区分出这些整体属于哪一个类别（比如说行人，车辆以及障碍物）。分割算法可以被分类如下几类：

在完成了点云的目标分割以后，分割出来的目标需要被正确的分类，在这个环节，一般使用机器学习中的分类算法，如支持向量机（Support Vector Machine，SVM）对聚类的特征进行分类，最近几年由于深度学习的发展，业界开始使用特别设计的卷积神经网络（Convolutional Neural Network，CNN）对三维的点云聚类进行分类。

实践中不论是提取特征-SVM的方法还是原始点云-CNN的方法，由于激光雷达点云本身解析度低的原因，对于反射点稀疏的目标（比如说行人），基于点云的分类并不可靠，所以在实践中，我们往往融合雷达和相机传感器，利用相机的高分辨率来对目标进行分类，利用Lidar的可靠性对障碍物检测和测距，融合两者的优点完成环境感知。

无人驾驶系统中，我们通常使用图像视觉来完成道路的检测和道路上目标的检测。道路的检测包含对道路线的检测（Lane Detection），可行驶区域的检测（Drivable Area Detection）；道路上路标的检测包含对其他车辆的检测（Vehicle Detection），行人检测（Pedestrian Detection），交通标志和信号的检测（Traffic Sign Detection）等所有交通参与者的检测和分类。

车道线的检测涉及两个方面： 第一是识别出车道线，对于弯曲的车道线，能够计算出其曲率，第二是确定车辆自身相对于车道线的偏移（即无人车自身在车道线的哪个位置） 。一种方法是抽取一些车道的特征，包括边缘特征（通常是求梯度，如索贝尔算子），车道线的颜色特征等，使用多项式拟合我们认为可能是车道线的像素，然后基于多项式以及当前相机在车上挂载的位置确定前方车道线的曲率和车辆相对于车道的偏离。

可行驶区域的检测目前的一种做法是采用深度神经网络直接对场景进行分割，即通过训练一个逐像素分类的深度神经网络，完成对图像中可行驶区域的切割。

交通参与者的检测和分类目前主要依赖于深度学习模型，常用的模型包括两类：

传感器层将数据以一帧帧、固定频率发送给下游，但下游是无法拿每帧的数据去进行决策或者融合的。因为传感器的状态不是100%有效的，如果仅根据某一帧的信号去判定前方是否有障碍物(有可能是传感器误检了)，对下游决策来说是极不负责任的。因此上游需要对信息做预处理，以保证车辆前方的障得物在时间维度上是一直存在的， 而不是一闪而过。

这里就会使用到智能驾驶领域经常使用到的一个算法 卡尔曼滤波。

卡尔曼滤波（Kalman filter） 是一种高效率的递归滤波器（自回归滤波器），它能够从一系列的不完全及包含噪声的测量中，估计动态系统的状态。卡尔曼滤波会根据各测量量在不同时间下的值，考虑各时间下的联合分布，再产生对未知变数的估计，因此会比只以单一测量量为基础的估计方式要准。

卡尔曼滤波在技术领域有许多的应用。常见的有飞机及太空船的导引、导航及控制。卡尔曼滤波也广为使用在时间序列的分析中，例如信号处理及计量经济学中。卡尔曼滤波也是机器人运动规划及控制的重要主题之一，有时也包括在轨迹最佳化。卡尔曼滤波也用在中轴神经系统运动控制的建模中。因为从给与运动命令到收到感觉神经的回授之间有时间差，使用卡尔曼滤波有助于建立符合实际的系统，估计运动系统的目前状态，并且更新命令。

信息融合是指把相同属性的信息进行多合一操作。

比如摄像机检测到了车辆正前方有一个障碍物，毫米波也检测到车辆前方有一个障碍物，激光雷达也检测到前方有一个障碍物，而实际上前方只有一个障碍物，所以我们要做的是把多传感器下这辆车的信息进行一次融合，以此告诉下游，前面有辆车，而不是三辆车。

坐标转换在自动驾驶领域十分重要。

传感器是安装在不同地方的比如超声波雷达(假如当车辆右方有一个障碍物，距离这个超声波雷达有3米，那么我们就认为这个障碍物距离车有3米吗?并不一定，因为决策控制层做车辆运动规划时，是在车体坐标系下做的(车体坐标系-般以后轴中心为O点)所以最终所有传感器的信息，都是需要转移到自车坐标系下的。因此感知层拿到3m的障碍物位置信息后，必须将该章碍物的位置信息转移到自车坐标系下，才能供规划决策使用。 同理，摄像机一般安装在挡风玻璃下面，拿到的数据也是基于摄像机坐标系的，给下游的数据，同样需要转换到自车坐标系下。

在无人车感知层面，定位的重要性不言而喻，无人车需要知道自己相对于环境的一个确切位置，这里的定位不能存在超过10cm的误差，试想一下，如果我们的无人车定位误差在30厘米，那么这将是一辆非常危险的无人车（无论是对行人还是乘客而言），因为无人驾驶的规划和执行层并不知道它存在30厘米的误差，它们仍然按照定位精准的前提来做出决策和控制，那么对某些情况作出的决策就是错的，从而造成事故。由此可见，无人车需要高精度的定位。

目前使用最广泛的无人车定位方法当属融合 全球定位系统（Global Positioning System，GPS）和惯性导航系统（Inertial Navigation System）定位方法 ，其中，GPS的定位精度在数十米到厘米级别之间，高精度的GPS传感器价格也就相对昂贵。融合GPS/IMU的定位方法在GPS信号缺失，微弱的情况下无法做到高精度定位，如地下停车场，周围均为高楼的市区等，因此只能适用于部分场景的无人驾驶任务。

地图辅助类定位算法是另一类广泛使用的无人车定位算法， 同步定位与地图构建（Simultaneous Localization And Mapping，SLAM） 是这类算法的代表，SLAM的目标即构建地图的同时使用该地图进行定位，SLAM通过利用已经观测到的环境特征确定当前车辆的位置以及当前观测特征的位置。这是一个利用以往的先验和当前的观测来估计当前位置的过程，实践上我们通常使用贝叶斯滤波器（Bayesian filter）来完成，具体来说包括卡尔曼滤波（Kalman Filter），扩展卡尔曼滤波（Extended Kalman Filter）以及粒子滤波（Particle Filter）。SLAM虽然是机器人定位领域的研究热点，但是在实际无人车开发过程中使用SLAM定位却存在问题，不同于机器人，无人车的运动是长距离的，大开放环境的。在长距离的运动中，随着距离的增大，SLAM定位的偏差也会逐渐增大，从而造成定位失败。

在实践中，一种有效的无人车定位方法是改变原来SLAM中的扫描匹配类算法，具体来说，我们不再在定位的同时制图，而是事先使用传感器如激光雷达对区域构建点云地图，通过程序和人工的处理将一部分“语义”添加到地图中（例如车道线的具体标注，路网，红绿灯的位置，当前路段的交通规则等等），这个包含了语义的地图就是我们无人驾驶车的 高精度地图（HD Map） 。实际定位的时候，使用当前激光雷达的扫描和事先构建的高精度地图进行点云匹配，确定我们的无人车在地图中的具体位置，这类方法被统称为扫描匹配方法（Scan Matching），扫描匹配方法最常见的是迭代最近点法（Iterative Closest Point ，ICP），该方法基于当前扫描和目标扫描的距离度量来完成点云配准。

除此以外， 正态分布变换（Normal Distributions Transform，NDT） 也是进行点云配准的常用方法，它基于点云特征直方图来实现配准。基于点云配准的定位方法也能实现10厘米以内的定位精度。虽然点云配准能够给出无人车相对于地图的全局定位，但是这类方法过于依赖事先构建的高精度地图，并且在开放的路段下仍然需要配合GPS定位使用，在场景相对单一的路段（如高速公路），使用GPS加点云匹配的方法相对来说成本过高。

拓展阅读： L4 自动驾驶中感知系统遇到的挑战及解决方案

浅析自动驾驶的重要一环：感知系统发展现状与方向

无人车的规划模块分为三层设计：任务规划，行为规划和动作规划，其中，任务规划通常也被称为路径规划或者路由规划（Route Planning），其负责相对顶层的路径规划，例如起点到终点的路径选择。 我们可以把我们当前的道路系统处理成有向网络图（Directed Graph Network），这个有向网络图能够表示道路和道路之间的连接情况，通行规则，道路的路宽等各种信息，其本质上就是我们前面的定位小节中提到的高精度地图的“语义”部分，这个有向网络图被称为路网图（Route Network Graph），如下图所示：

这样的路网图中的每一个有向边都是带权重的，那么，无人车的路径规划问题，就变成了在路网图中，为了让车辆达到某个目标（通常来说是从A地到B地），基于某种方法选取最优（即损失最小）的路径的过程，那么问题就变成了一个有向图搜索问题，传统的算法如迪科斯彻算法（Dijkstra’s Algorithm）和A 算法（A Algorithm）主要用于计算离散图的最优路径搜索，被用于搜索路网图中损失最小的路径。

行为规划有时也被称为决策制定（Decision Maker），主要的任务是按照任务规划的目标和当前的局部情况（其他的车辆和行人的位置和行为，当前的交通规则等），作出下一步无人车应该执行的决策，可以把这一层理解为车辆的副驾驶，他依据目标和当前的交通情况指挥驾驶员是跟车还是超车，是停车等行人通过还是绕过行人等等。

行为规划的一种方法是使用包含大量动作短语的复杂有限状态机（Finite State Machine，FSM）来实现，有限状态机从一个基础状态出发，将根据不同的驾驶场景跳转到不同的动作状态，将动作短语传递给下层的动作规划层，下图是一个简单的有限状态机：

如上图所示，每个状态都是对车辆动作的决策，状态和状态之间存在一定的跳转条件，某些状态可以自循环（比如上图中的循迹状态和等待状态）。虽然是目前无人车上采用的主流行为决策方法，有限状态机仍然存在着很大的局限性：首先，要实现复杂的行为决策，需要人工设计大量的状态；车辆有可能陷入有限状态机没有考虑过的状态；如果有限状态机没有设计死锁保护，车辆甚至可能陷入某种死锁。

通过规划一系列的动作以达到某种目的（比如说规避障碍物）的处理过程被称为动作规划。通常来说，考量动作规划算法的性能通常使用两个指标：计算效率（Computational Efficiency）和完整性（Completeness），所谓计算效率，即完成一次动作规划的处理效率，动作规划算法的计算效率在很大程度上取决于配置空间（Configuration Space），如果一个动作规划算法能够在问题有解的情况下在有限时间内返回一个解，并且能够在无解的情况下返回无解，那么我们称该动作规划算法是完整的。

配置空间：一个定义了机器人所有可能配置的集合，它定义了机器人所能够运动的维度，最简单的二维离散问题，那么配置空间就是[x, y]，无人车的配置空间可以非常复杂，这取决于所使用的运动规划算法。

在引入了配置空间的概念以后，那么无人车的动作规划就变成了：在给定一个初始配置（Start Configuration），一个目标配置（Goal Configuration）以及若干的约束条件（Constraint）的情况下，在配置空间中找出一系列的动作到达目标配置，这些动作的执行结果就是将无人车从初始配置转移至目标配置，同时满足约束条件。在无人车这个应用场景中，初始配置通常是无人车的当前状态（当前的位置，速度和角速度等），目标配置则来源于动作规划的上一层——行为规划层，而约束条件则是车辆的运动限制（最大转角幅度，最大加速度等）。显然，在高维度的配置空间来动作规划的计算量是非常巨大的，为了确保规划算法的完整性，我们不得不搜索几乎所有的可能路径，这就形成了连续动作规划中的“维度灾难”问题。目前动作规划中解决该问题的核心理念是将连续空间模型转换成离散模型，具体的方法可以归纳为两类：组合规划方法（Combinatorial Planning）和基于采样的规划方法（Sampling-Based Planning）。

运动规划的组合方法通过连续的配置空间找到路径，而无需借助近似值。由于这个属性，它们可以被称为精确算法。组合方法通过对规划问题建立离散表示来找到完整的解，如在Darpa城市挑战赛（Darpa Urban Challenge）中，CMU的无人车BOSS所使用的动作规划算法，他们首先使用路径规划器生成备选的路径和目标点（这些路径和目标点事融合动力学可达的），然后通过优化算法选择最优的路径。另一种离散化的方法是网格分解方法（Grid Decomposition Approaches），在将配置空间网格化以后我们通常能够使用离散图搜索算法（如A*）找到一条优化路径。

基于采样的方法由于其概率完整性而被广泛使用，最常见的算法如PRM（Probabilistic Roadmaps），RRT（Rapidly-Exploring Random Tree），FMT（Fast-Marching Trees），在无人车的应用中，状态采样方法需要考虑两个状态的控制约束，同时还需要一个能够有效地查询采样状态和父状态是否可达的方法。

自动驾驶汽车的车辆控制技术旨在环境感知技术的基础之上，根据决策规划出目标轨迹，通过纵向和横向控制系统的配合使汽车能够按照跟踪目标轨迹准确稳定行驶，同时使汽车在行驶过程中能够实现车速调节、车距保持、换道、超车等基本操作。

互联网科技公司主要做软件，以工程机上层为主；而车厂其实以下层的组装为主，也就是OEM，也不是那么懂车。像制动、油门和转向等这些领域，话语权依然集中在博世、大陆这样的Tier 1身上。

自动驾驶控制的核心技术是车辆的纵向控制和横向控制技术。纵向控制，即车辆的驱动与制动控制；横向控制，即方向盘角度的调整以及轮胎力的控制。实现了纵向和横向自动控制，就可以按给定目标和约束自动控制车运行。所以，从车本身来说，自动驾驶就是综合纵向和横向控制。

车辆纵向控制是在行车速度方向上的控制，即车速以及本车与前后车或障碍物距离的自动控制。巡航控制和紧急制动控制都是典型的自动驾驶纵向控制案例。这类控制问题可归结为对电机驱动、发动机、传动和制动系统的控制。各种电机-发动机-传动模型、汽车运行模型和刹车过程模型与不同的控制器算法结合，构成了各种各样的纵向控制模式，典型结构如图所示。

此外，针对轮胎作用力的 滑移率控制 是纵向稳定控制中的关键部分。滑移率控制系统通过控制车轮滑移率调节车辆的纵向动力学特性来防止车辆发生过度驱动滑移或者制动抱死，从而提高车辆的稳定性和操纵性能。制动防抱死系统（antilock brake system）简称 ABS，在汽车制动时，自动控制制动器制动力的大小，使车轮不被抱死，处于边滚边滑（滑移率在 20%左右）的状态，以保证地面能够给车轮提供最大的制动作用力值。一些智能滑移率控制策略利用充足的环境感知信息设计了随道路环境变化的车轮最有滑移率调节器，从而提升轮胎力作用效果。

智能控制策略，如模糊控制、神经网络控制、滚动时域优化控制等，在纵向控制中也得到广泛研究和应用，并取得了较好的效果，被认为是最有效的方法。

而传统控制的方法， 如PID控制和前馈开环控制 ，一般是建立发动机和汽车运动过程的近似线形模型，在此基础上设计控制器，这种方法实现的控制，由于对模型依赖性大及模型误差较大，所以精度差、适应性差。从目前的论文和研究的项目看，寻求简单而准确的电机-发动机-传动、刹车过程和汽车运动模型，以及对随机扰动有鲁棒性和对汽车本身性能变化有适应性的控制器仍是研究的主要内容。

车辆横向控制指垂直于运动方向上的控制，对于汽车也就是转向控制。目标是控制汽车自动保持期望的行车路线，并在不同的车速、载荷、风阻、路况下有很好的乘坐舒适性和稳定性。

车辆横向控制主要有两种基本设计方法，一种是基于驾驶员模拟的方法；另一种是给予汽车横向运动力学模型的控制方法。基于驾驶员模拟的方法，一种策略是使用较简单的运动力学模型和驾驶员操纵规则设计控制器；另一策略是用驾驶员操纵过程的数据训练控制器获取控制算法。基于运动力学模型的方法要建立较精确的汽车横向运动模型。典型模型是所谓单轨模型，或称为自行车模型，也就是认为汽车左右两侧特性相同。横向控制系统基本结构如下图。控制目标一般是车中心与路中心线间的偏移量，同时受舒适性等指标约束。

opencv怎么计算物体移动速度 求具体解决方法,最好能有源代码参考一下,

你起码得识别出来这个物体啊!

然后通过跟踪算法 kalman 粒子滤波 跟踪这个物体的中心点 比如重心

根据移动的速度和时间来计算

你先做出来物体识别跟踪 速度自然就有了

百度搜 物体识别 opencv

kalman opencv

粒子滤波源代码的介绍就聊到这里吧，感谢你花时间阅读本站内容，更多关于滤波器 代码、粒子滤波源代码的信息别忘了在本站进行查找喔。