Odometry and Laser Scanner Fusion Based on a Discrete Extended Kalman Filter for Robotic Platooning Guidance

Espinosa, Felipe; Santos, Carlos; Marrón-Romera, Marta; Pizarro, Daniel; Valdés, Fernando; Dongil, Javier

doi:10.3390/s110908339

开放式访问第条

基于离散扩展卡尔曼滤波的机器人排导里程计和激光扫描仪融合

西班牙马德里阿尔卡拉大学高等技术学院电子系，邮编28805

^*

信件应寄给的作者。

传感器 2011,11(9), 8339-8357;https://doi.org/10.3390/s110908339

收到的意见：2011年7月18日/修订日期：2011年8月20日/接受日期：2011年8月21日/发布日期：2011年8月29日

（本文属于特刊协作传感器)

下载

浏览地物

版本注释

摘要

以下为：本文描述了一种用于在室内环境中实现机器人车队导航的相对定位系统。该定位系统融合了两个传感源：（a）里程表系统和（b）激光扫描仪以及位于装置顶部的人造地标。激光源允许补偿死锁固有的累积误差；而里程计源在短轨迹中提供较少的姿态不确定性。为了在实时约束下实现这一目标，使用了为此应用定制的离散扩展卡尔曼滤波器。显示了先锋P3-DX装置护卫队跟踪非线性轨迹的不同实验结果。本文表明，基于低成本激光测距系统和机器人内置里程表传感器的简单设置，能够对室内应用的护航单元的相对定位问题提供高度的鲁棒性和准确性。

关键词：

卡尔曼滤波器;传感器融合;智能机器人;数据处理;机器人控制;激光应用;航位推算;状态估计;多机器人系统;机器人传感系统

1.简介

过去，移动机器人协作在多种应用场景中得到了广泛的研究。与单机器人系统相比，多机器人系统在灵活性、适应性、可扩展性和经济性方面具有优势。然而，在协作机器人中，定位、通信和控制挑战更为重要。

从本地化的角度来看，可以考虑两种场景：第一种场景需要独立地对每个单元进行全局本地化（例如群应用程序[1]). 在第二种情况下，只需要机器人之间的相对定位（例如车队应用[2]，只有车队领队可能需要全球本地化）。

机器人定位所需的传感系统的选择是一项关键任务，在很大程度上取决于应用场景(即室内或室外环境）。户外场景中的定位可以通过GPS系统和相对定位传感器（例如，等相反，在室内环境中定位是一个具有挑战性的问题，在某些方面仍然没有解决。室内GPS系统使用多种传感器技术（例如，视觉、超声波、红外、，等）大多处于研究状态。考虑到室内环境中的非线性轨迹，研究了护航机器人协作制导的相对定位问题。

多传感器策略通常被用来解决相对定位问题，其中里程信息(即.，最初包含在大多数机器人中，容易增加累积误差），与其他传感器（如激光、超声波或视觉）结合。一般来说，这些技术的准确性在很大程度上取决于传感器设置。仅考虑机器人上的传感器，定位精度取决于几个因素，例如成本、传感器数量、每种技术的复杂性和局限性。本文建议在每个机器人的顶部安装一个传感器，该传感器能够给出机器人车队中下一个机器人单元的位置和方向。在这种情况下，激光测距仪的精度高于基于声纳（超声波）的测距仪[三,4]，使用机器人的自然形状或上面有人工标志。另一方面，计算视觉能够使用视觉标志轻松提高激光精度[5]成本相对较低。然而，当需要使系统抵抗各种照明条件（例如，主动红外地标）并以高帧速率运行时，设置成本及其复杂性会增加。

本文表明，基于低成本激光测距系统和内置机器人里程传感器的简单设置能够对室内环境中护航部队的相对定位问题提供高度鲁棒性和准确性。除了定位问题外，车队中每个单元的控制策略设计也面临着重要挑战。为了追随领导人的轨迹，仅仅保证全球稳定是不够的。至少每对连续单元之间还需要一个运动协调计划[6]. 这种协调涉及车队单位之间不断交换运动状态，如以下工程中所述[7–10]. 为了成功实现车队导航，必须有一个高度可靠和精确的定位系统，为车队领队提供其全球姿态，并为每个车队单位提供其相对于前一单位的相对定位。

在有关室内应用护航指南的已发表著作中，应提及以下内容：[11]为了保证室内施工现场的安全，设计了一种用于车队高层导航的传感系统。机器人中拟议的车载传感系统（超声波测距模块、红外测距设备、彩色摄像机、麦克风和扬声器）由无线传感器网络设备补充。在[12]视觉系统可以使用安装在引导装置上的标记来识别跟随机器人并对其进行相对定位。在[13]使用放置在环境中的摄像机和彩色标志牌解决了车队引导的室内定位问题。应用卡尔曼滤波和交互多模型方法，找到机器人的准确位置，并通过标识牌进行识别。在[14,15]描述了一名领队和四名跟随者的演示，其中护航部队之间的相对定位通过Sick LMS 200激光测距仪（LIDAR）解决，机器人本身（先锋2-DX）用作地标。提出的Sick LIDAR传感器具有0.25°的角分辨率、15 mm的深度分辨率和10 m的范围。虽然使用了精确的传感器，但上述工作不包括里程信息，因此仅依赖于激光测量。

根据之前的工作，本文的主要贡献是为室内运输场景中的机器人单元车队实现一个创新的、低成本的相对定位系统。本文提出的激光雷达传感器是北洋URG-04-LX[16,17]. 它的性能显著低于前面提到的Sick LIDAR（0.36°角分辨率、40 mm线性精度和4 m射程），但成本大约便宜六倍。为了补偿其精度，本文建议将激光测量与里程计相结合。通过这种方式，该算法能够在短距离运动中利用里程计的高分辨率（1.2毫米分辨率），同时使用激光测量在大轨迹中补偿非累积误差。此外，里程表传感器工作在高频（在本文所述的应用中为50毫秒），这允许在发生瞬时致盲时保持相对定位准确；这不可能仅使用激光、相机或声纳设备。总之，数据融合使机器人里程表系统的定位数据（具有低不确定性但累积误差）与随动装置中的车载激光扫描仪（具有非累积误差）相结合成为可能。提出了一种基于离散扩展卡尔曼滤波器（EKF）的解决方案。车队演示车中使用的单元基于MobileRobots的P3-DX机器人[18,19]，已针对不同电子设备的拟议场景要求进行了调整（参见图1和2); 其中一些是设计的特别的[20,21].

如中详细描述[2,6,7,21]在排班应用中，制导任务的成功与否在很大程度上取决于控制、通信和传感系统之间的关系。关于硬非线性轨迹中排队制导的控制和通信解决方案，作者在COVE项目背景下进行了不同的工作[22]. 全局控制体系结构[2]对于护卫队的每个跟随者，包括一个三级控制器，如所示图3.

低液位基于一组PID控制器，用于调节每个主动轮的速度。中间层包括一个伺服控制器，以确保机器人可靠的角速度和线速度（Vo）。机器人装置配有光学编码器，每转500个脉冲，与每个主动轮相连，因为其直径为19厘米，运动分辨率为1.2毫米。这样，里程表可以关闭中低控制回路。此外，还包括离散卡尔曼滤波器（KF估计器），以便对与编码器提供的测量相关的噪声进行滤波，并获得用于控制目的的滤波速度矢量（VE）。高层为中层生成输入（UHL），这样每个机器人都会跟随前一个机器人以保证安全距离，并以自己的方式接近领导者选择的离散姿势。完成高控制水平目标所需的跟随器姿态由离散EKF结合里程表系统和添加的激光扫描仪的数据进行估计。

本文分为五个部分：在这一介绍之后，第2节介绍了融合过程中涉及的感官来源，第3节描述了离散EKF在数据融合数据中的应用。第4节显示了使用真实机器人车队演示器获得的实验结果，第5节显示了结论。数学组件和算法描述包含在附录在论文的最后。

2.基于里程表和激光组合的跟随器单元姿态估计

护卫队由一组只配备相对定位传感器的机器人单元组成(即.、里程表和激光传感器）。其中一个单元被指定为领导单元，并假定它知道其在环境中的全局位置。此外，所有单元都是无线局域网（Wi-Fi链路）的节点[2].

如前所述，如果每个跟随器都知道其运动状态和至少一个先验单位，则可以实现全局稳定性。为了实现这一规范，作为第一近似值，可以使用连接到每个机器人上已经构建的主动轮上的编码器，通过无重编码过程估计排中每个机器人的角速度和线速度。该估计可以在每对机器人之间进行组合，并通过无线链路发送，以获得每个单元与前一个单元的相对距离和方向。然而，由于死锁过程固有的累积误差（主要是非线性轨迹），第一种方法在姿态估计中产生了重要的漂移。因此，需要一个互补的传感系统，以便更好地估计个人姿势，并保证车队执行的制导任务的可靠性和稳定性。

本文提出将里程信息与激光传感器姿态估计相结合。激光传感器提供每个机器人到前一个机器人的相对姿势（距离和方向）（参见图1和2)从而避免了此信息中的累积错误。然而，实验结果表明，由激光传感器计算出的与姿态信息相关的不确定度大于从里程表传感器获得的不确定性。然而，必须考虑到，与激光扫描仪相关的误差是有界的，而与里程表系统相关的误差则是累积的。因此，需要使用融合策略来补偿限制，并利用与制导应用中的两个感官系统中的每一个相关的积极特性。

激光有助于测量间隔距离d日_里排内各单位之间，并确定修正角度θ_工程安装每个单元需要接近下一个姿势标记P（P）_{立陶宛（LTk）}由领队派往其他车队[2]. 这些变量如所示图4作为激光扫描仪的补充，基本的人工地标被放置在机器人平台的顶部（参见图2). 地标系统包括两个小平面和它们之间的一个圆柱体，悬在基本平台的紧凑体积之上。气缸位于机器人的动力学参考点上。可以在中注意到图5作为人工地标包含的三个元素中的两个足以获得两个分离距离d日_里和相对角度θ_里机器人姿势之间P（P）_我和P（P）_i−1.

然而，与拟建地标合作具有重要优势：

包含两个平面元素可使计算角度时的误差最小化α，因为他们之间的距离足够大。角度α用于计算相对方向θ_里排中每两个连续的单位之间。
尽管距离d日_里可以通过措施间接获得[d日₁,θ₁]和[d日₂,θ₂]地标中间的圆柱简化了直接计算，提高了估计的准确性和计算时间。
由于它的三个组成部分，人工地标可以很容易地在机器人结构中识别，也可以从环境中的不同元素中识别，从而最大限度地减少故障检测。

一旦在跟随器单元顶部的激光扫描仪检测到人造地标，该单元与其前面的单元之间的相对距离就直接从激光测量到圆柱形结构d中获得_里从激光扫描仪在地标上检测到的两个最外部测量值（点e（电子）₁和e（电子）₂在里面图5)，角度α可以计算：

α = 阿坦 2 (\frac{{d日}_{1} 罪 (θ_{1}) - {d日}_{2} 罪 (θ_{2})}{{d日}_{2} 余弦 (θ_{2}) - {d日}_{1} 余弦 (θ_{1})})

(1)

哪里数据2是逆切线函数的四象限版本。

这样，相对于前一个单元的相对方向通过以下等式获得：

θ_{里} = \frac{π}{2} - α

(2)

为了更好地理解数据融合过程，应记住以下术语：X̃是基于里程计的预测姿势，Z轴是通过激光测量和X̂表示通过EKF算法修正的姿势。

由于单元之间的无线连接，修正后的姿势X̂_我_−1,_k个₋₁= [X̂_我_−1，_k个₋₁ ŷ_我_−1,_k个₋₁ θ̂_我_−1，_k个₋₁]^T型的F类_我₋₁单位位于第k-1次瞬间，单位已知F类_我在第k个瞬间。这样，使用相对激光测量，估计姿态Z轴_我_,_k个= [x̄_我_,_k个 ȳ_我_,_k个 θ̄_我_,_k个]^T型属于F类_我在第k个瞬间获得，如所示方程式（3–5)以下为：

{\bar{θ}}_{我, k个} = {\hat{θ}}_{我 - 1, k个 - 1} - θ_{里, k个}

(3)

{\bar{x个}}_{我, k个} = {\hat{x个}}_{我 - 1, k个 - 1} - {d日}_{里, k个} 余弦 ({\bar{θ}}_{我, k个} {+ θ}_{ci公司, k个})

(4)

{\bar{年}}_{我, k个} = {\hat{年}}_{我 - 1, k个 - 1} - {d日}_{里, k个} 罪 ({\bar{θ}}_{我, k个} {+ θ}_{ci公司, k个})

(5)

最后，EKF算法允许融合里程信息X̃_我_,_k个用激光枪Z轴_我_,_k个以获得正确的姿势X̂_我_,_k个的F类_我单位，请参见图6.

3.离散EKF在里程计和激光融合中的应用

车队中每个跟随者的最佳姿态估计是通过离散扩展卡尔曼滤波器实现的[23–25]，融合里程计和激光扫描仪信息。EKF可以突出两个感官系统的优势。因此，过滤器开发了如下所示的功能图6分两步进行：

机器人姿态预测X̃_k个.里程表信息作为输入向量包括在内U型_k个根据每个采样时间的主动轮速度。修正后的状态X̂_k个₋₁在前一个样本中，时间也是必需的。
姿态估计的修正X̂_k个。此步骤需要估计的姿势Z轴_k个一旦获得激光扫描仪信息。

在本文的最后附录从数学上详细说明了离散EKF对本工作中解决的问题的具体适应，总结如下图7.

实现的EKF具有此过滤器的标准结构，除了因子Θ_k个。该系数表示激光扫描仪测量的可用性：如果其测量在特定时间可用k个然后Θ_k个= 1; 否则Θ_k个=0，此时不会执行校正步骤k个.因子的使用Θ_k个允许预测和校正过程具有时间独立性[26]. 在这项工作中，预测步骤经常使用Ts=0.05 s的采样时间，同时时间校正将根据激光扫描仪测量的可用性而变化，如所述。

以下段落总结了滤波器在EKF预测步骤中开发的不同任务：

（p.1）状态预测X̃_{i、 k个}绝对定位参考系统中随动装置的（位置和方向）。基于里程表系统的死锁模型（中的f函数图7)，以及上一时间步的校正状态X̂_我,k个−1，以获得此预测状态。
（p.2）措施估算Z轴_我,k个从先例单元的修正姿势Xõ_{我−1,k个−1}以及基于激光扫描仪的测量模型(克中的函数图7).
（p.3）估计误差协方差矩阵的预测P̃_{i、 k个}，使用上一时间步长的该矩阵修正值P（P）_我,k个−1以及噪声协方差矩阵Σ_W公司里程表测量模型和两个雅可比公式J型_{f、 X（X）}和J型_{f、 W公司}（请参见附录).

另一方面，离散滤波器在校正步骤中开发的任务如下：

（c.1）更新卡尔曼增益K_{i、 k个}为了获得该增益，必须预先计算一些矩阵：矩阵P̃的估计_{i、 k个}，噪声协方差矩阵∑_V（V）激光扫描仪测量模型和两个雅可比J_{g、 X̄}和J_g.伏（请参见附录).
（c.2）姿势状态预测值X̂的修正_{i、 k个}，仅当激光扫描仪更新的测量值可用时_k个= 1. 正如在中可以注意到的那样图7，该校正是对利用激光扫描Z获得的位置信息之间的差进行加权而获得的_k个，及其预测X̃_k个通过里程计和卡尔曼增益。
（c.3）更新估计误差协方差矩阵P_{i、 k个}.

在本文的贡献中，应该考虑用于排导机器人姿态估计的标准离散EKF自适应。具体而言，作者开发了：

的特征（f）和克功能。这个（f）一种是与使用里程表系统获取位置信息所使用的无重编码模型有关。这个克功能涉及基于激光扫描仪的相关测量和先验装置的姿态的定位系统。
对与两个感觉系统相关的噪声建模的协方差矩阵的计算：与里程计系统相关的协方差矩阵Σ_W公司另一个是激光扫描仪Σ_V（V）为了找到这些值，在50个实验中记录了主动轮的角速度以及用激光扫描仪测量的距离和角度。相关噪声变量的标准偏差和完整协方差矩阵是从这些寄存器中计算出来的。
计算所需雅可比数。雅各宾派依靠里程测量：J型_{f、 X（X）}和J型_{f、 W公司}; 以及关于激光测量的内容：J型_{g、 Z轴}和J型_{g、 V（V）}.

4.实验结果

在为验证所述方案而开发的实验测试中，三个机器人单元改编自原始P3-DX平台（参见图1,2和5)已部署。它们最初都是同步的，目前通过符合IEEE802.11b/.11g标准的Wi-Fi LAN进行连接[20]. 研究小组在其他工作中解决了缓解数据包丢失影响的解决方案[27]并已在这些测试中实现。最困难的时间限制由以下因素决定：北洋设备的扫描时间（100 ms）[16,17]和机器人的速度（限制为1 m/s）[2,18].

本节包括两种类型的测试。第一种方法致力于展示所实现的融合技术的优势，比较研究中的两个感官系统各自独立获得的定位结果。第二类侧重于应用于机器人车队的控制和传感器融合集成的全局结果。

在第一组结果中，只使用了两个单位：引导器，用于跟踪轨迹，包括直线段和曲线段；和一个追随者。图8根据不同的感测源显示跟随路径，但没有融合应用：红色表示里程传感器系统记录的运动；蓝色的是激光扫描仪通过相对于领队运动的相对测量记录的。随动装置从点[x=−1，y=0]开始。所描绘的两个轨迹沿着其第一直线部分是接近的。然而，当轨迹呈现曲线路径时，两个传感系统给出的信息会发生偏离。图9显示了在第一次实验中测试的随动装置的里程表系统记录的线性和角速度。该图允许人们以其他方式演示所包含的滤波器去除里程计噪声的效果。事实上，可以注意到，这种噪声在角速度情况下更为相关，这证实了需要在全局控制解决方案中插入KF估计器（参见图3)用于非线性轨迹跟踪。

使用相同的机器人编队和相同的路径参考进行了新的实验。这一次，融合算法的输出应用于制导体系结构中的高级控制，如所示图3。当由于缺乏激光扫描仪测量而无法严格执行融合任务时，为了评估离散EKF功能，该传感器在某些时间间隔内被遮挡。在路径上的某些特定时刻，障碍物正好插入扫描仪前面。然后可以分析当障碍物消失后，导航应用程序如何从单独使用里程表产生的漂移中纠正机器人路径。图10显示了跟随器单元使用此处提出的全局融合算法进行的路径追踪（红线○）；如果可用，激光扫描仪的测量值也用蓝色绘制。在此路径中，沿线段“ab公司”, “光盘“和”ef（参考）“两个感官系统都产生有效的姿势测量，因此，融合过程得到了正确的发展。此外，沿着路段”公元前“和”判定元件“，激光扫描仪系统中没有位置信息，因此仅使用里程表信息进行EKF的预测步骤。由于缺少激光扫描仪的信息，当仅使用里程表时，机器人装置的移动会与预期路径产生相关偏移，主要是在曲线间隔内。在任何情况下，一旦激光扫描仪测量再次可用于融合算法，引导过程将快速调整到正确的路径。

第二种测试由三个单元组成的车队进行，如所示图2，在更复杂的场景中。可以在中看到显示整个实验的视频[22]. 基于三个控制层和本文描述的传感器融合算法的排制导策略在两个跟随单元中实现。因为它可以在图11，排从L03实验室开始（“一”, “b条“和”c（c）“分别是每个单元的初始定位）并通过走廊最终进入L02实验室（其中”一’ ”, “b条“”和“c（c）“”分别是每个单元的最终本地化）。在这个排导示例中，每个机器人所遵循的总路径用不同的颜色表示。机器人的位置是从每个“EKF传感器融合”块获得的（参见图3). 可以说，两个跟随器单元以可忽略的误差跟踪领导者描述的轨迹。

5.结论

这项工作详细说明了里程计和低成本激光扫描仪的适当组合如何为机器人护卫队的室内应用提供所需的精度和非累积误差。首先，已经证明，来自所提出的单个传感器的信息不足以完成协同制导中一个或多个单元的正确定位。在此背景下，本文提出的建议要求将里程数据（机器人的典型定位系统）与激光扫描仪（与基本地标结构一起添加到机器人平台）融合，以实现P3-DX机器人车队的引导任务。

实现的离散EKF的贡献是双重的。一方面，通过激光测量来校正因无重定位而产生的固有累积误差。另一方面，与所使用的低成本激光扫描仪相关的最高不确定性由P3-DX编码器中最低的一个进行补偿。

室内实验结果表明，感知融合过程对于保持跟随者之间的安全距离和跟踪领导者的轨迹至关重要。实施的解决方案可以实现这些目标，即使在缺乏部分感官信息的情况下也是如此。

总之，本文定量地详细介绍了如何通过适当的融合算法，利用独立传感源的最佳特性，并最大限度地减少其固有的局限性，来高度提高独立传感源的性能。

致谢

这项工作得到了西班牙科学与创新部通过VISNU（参考号TIN2009-08984）项目的支持。

工具书类

Haoyao，C.走向多机器人编队：基于视觉的定位系统研究。博士论文，。香港大学：中国香港，2009年4月。在线提供：http://hdl.handle.net/2031/5836（2011年7月1日加入）。[谷歌学者]
桑托斯，C；埃斯皮诺萨，F；皮萨罗，D；瓦尔德斯，F；桑提索，E；Díaz，I.机器人编队非线性轨迹制导的模糊分散控制。2010年9月13日至16日，西班牙毕尔巴鄂，第15届IEEE新兴技术和工厂自动化国际会议（ETFA'10）会议记录。
Aulinas，J；佩蒂略，Y；萨尔维，J；Lladó，X.SLAM问题：一项调查。加泰罗尼亚人工智能协会第十一届国际会议记录，西班牙圣马丁·德恩普里，2008年10月22日至24日。
Borenstein，J；人力资源部埃弗雷特；冯，L；Wehe，D.移动机器人定位：传感器和技术。J.罗布。系统 1997,14, 231–249. [谷歌学者]
费尔南德斯，我；马祖，M；拉扎罗，JL；皮萨罗，D；桑提索，E；马丁，P；Losada，C.使用位于环境中的一组静态摄像机引导移动机器人。自动机器人 2007,23, 305–324. [谷歌学者]
加藤，S；Tsugawa，S；德田，K；松井，T；Fujii，H.与自动化车辆和车间通信协作驾驶的车辆控制算法。IEEE传输。智力。运输。系统 2002,三，155–161。[谷歌学者]
斯坦科维奇，SS；Stanojevic，MJ；DD西尔雅克。一排车辆的分散重叠控制。IEEE传输。控制系统。Technol公司 2000,8, 816–832. [谷歌学者]
赫佩，X；德拉方丹，J；博瑞德，M；Michaud，F.适应不断变化的环境条件的车辆排的制导和控制。IEEE系统、人与控制论国际会议论文集，美国华盛顿特区，2003年10月5日至8日；4，第3091-3096页。
埃斯皮诺萨，F；阿沃德，AMH；小马祖；罗德里格斯，JM；博科斯，A；Manzano，M.通过分散重叠控制减少车辆排的横向和纵向振动。2007年12月12日至14日，美国洛杉矶新奥尔良，第46届IEEE决策与控制会议记录。
罗德里格斯，M；伊格莱西亚斯，R；埃斯皮诺萨，F；昆蒂亚，P；Regueiro，简历；Valdes，F.基于协作任务强化的学习建议：机器人车队编队。第四届欧洲移动机器人会议记录，2009年9月23日至25日，克罗地亚姆里尼/杜布罗夫尼克。
Cho，YK；Youn，J-H.室内施工现场安全用无线传感器驱动智能导航机器人。2006年10月3日至5日在日本东京举行的第23届建筑自动化和机器人国际研讨会论文集；第493-498页。
Sequeira，G.移动机器人基于视觉的领导-跟随编队控制。硕士论文，。密苏里罗拉大学：罗拉，密苏里州，美国。在线提供：http://scholarsmine.mst.edu/thesis/pdf/Sequeira_09007dcc804429d4.pdf（2011年7月1日查阅）。[谷歌学者]
桥本，M；奥巴，F；Tomiie，T.使用彩色招牌定位移动机器人。机电一体化J 1999,9，633–656页。[谷歌学者]
新墨西哥州法林顿；Nguyen，HG；Pezeshkian，N.在未知环境中运行的室内移动机器人车队的智能行为。程序SPIE 2004, 164–172. [谷歌学者]
Nguyen，HG公司；法林顿，N；北京，N。战术地面机器人通信链路的维护; A692624；AUVSI无人系统北美：美国加利福尼亚州阿纳海姆；八月；2004. [谷歌学者]
测距激光扫描仪URG-04LX。在线提供：http://www.acroname.com/robots/parts/R283-HOKUYO-LASER1.html（2011年7月1日查阅）。
瓦兹奎兹·马汀，R；努涅斯，P；班德拉，A；Sandoval，F.使用激光测距传感器进行机器人导航的基于曲率的环境描述。传感器 2009,9, 5894–5918. [谷歌学者]
移动机器人。P3-DX装置，在线提供：http://www.mobilerobots.com/researchrobots/researchrobots/pioneerp3dx.aspx（2011年7月1日查阅）。
皮萨罗，D；马祖，M；桑提索，E；马龙，M；Jimenez，D；科布雷斯，S；Losada，C.使用里程表和外部视觉传感器定位移动机器人。传感器 2010,10, 3655–3680. [谷歌学者]
埃斯皮诺萨，F；萨拉查，M；博科斯，A；瓦尔德斯，F；Iglesias，R.《基于玩家/舞台的P3-DX远程机器人操作通信体系结构的设计与实现》。2008年5月19日至23日，美国加利福尼亚州帕萨迪纳市，IEEE机器人与自动化国际会议论文集：远程机器人的新愿景和挑战；第65-70页。
埃斯皮诺萨，F；萨拉查，M；皮萨罗，D；Valdés，F.关于P3-DX装置远程机器人操作的电子提案。在远程和远程机器人; Mollet，N，编辑。；InTech：克罗地亚里耶卡，2010年。[谷歌学者]
COVE研究项目，在线提供：http://www.geintra-uah.org/idi/demostraciones/demostraciones（2011年7月1日查阅）。
佐治亚州特雷贾努。扩展卡尔曼滤波器教程; 技术报告；。布法罗大学计算机科学与工程系：布法罗，纽约，美国。在线提供：http://users.ices.utexas.edu/~terejanu/files/tutorialEKF.pdf（2011年7月1日查阅）。
萨西亚德克，JZ；Hartana，P.使用卡尔曼滤波器的传感器数据融合。第三届信息融合国际会议记录，2000年7月10日至13日，法国巴黎，Fusion 2000。
基里，E；M.布勒。移动机器人定位的三状态扩展卡尔曼滤波器; 技术报告；麦吉尔大学智能机器中心：加拿大魁北克省蒙特利尔；12; 4月；2002. [谷歌学者]
Smyth，A；Wu，M.动态系统监测中位移和加速度响应测量数据融合的多速率卡尔曼滤波。机械。系统。信号处理 2007,21，706–723页。[谷歌学者]
博科斯，A；埃斯皮诺萨，F；萨拉查，M；Valdés，F.基于机器人遥操作TVKF最优估计器的信道丢包补偿。2008年5月19日至23日在美国加利福尼亚州帕萨迪纳举行的机器人与自动化国际会议记录（ICRA 2008）；第16-20页。

附录

离散EKF算法实现的数学描述

在差动驱动机器人中，机器人姿态与主动轮速度数据之间的运动学关系是非线性的。因此，为了在状态空间中表示这种关系，转换方程和输出方程必须分别表示如下：

{X（X）}_{我, k个} = （f） ({X（X）}_{我, k个 - 1}, {U型}_{我, k个}, {W公司}_{k个})

（A.1）

{Z轴}_{我, k个} = 克 ({X（X）}_{我 - 1, k个 - 1}, {V（V）}_{k个})

（A.2）

哪里X（X）_{i、 k个}∈ 𝕉^三，是表示跟随器单元及其三个分量的绝对姿态的状态向量（x，y，θ）;X（X）_我−1,k个∈ 𝕉^三，是先例单元的状态向量；U型_{i、 k个}∈ 𝕉²，是输入向量，包含两个分量：角速度(ω_R（右）, ω_我)平台上的两个主动轮（右侧和左侧）；Z轴_{i、 k个}∈ 𝕉^三，是通过激光扫描仪测量的姿态估计值（距离和角度）；W公司_k个∈ 𝕉²，是状态噪声矢量，因此与里程表系统相关；和V（V）_k个∈ 𝕉⁶，是与激光扫描仪感知系统相关的测量噪声矢量。

如前几段所述，非线性和随机函数（f）和克分别与机器人固有的里程表系统和激光扫描仪传感系统有关。

如第3节所述图7，为了发展EKF感测融合，定期重复两个步骤。在预测步骤中，方程式（A.3）–（A.5）已确定。在此步骤中，假设所有噪声分量都为空值：

{\tilde{X（X）}}_{我, k个} = （f） ({\hat{X（X）}}_{我, k个 - 1}, {U型}_{我, k个}, {W公司}_{k个})

（A.3）

{Z轴}_{我, k个} = 克 ({\hat{X（X）}}_{我 - 1, k个 - 1}, 0)

（A.4）

{\tilde{P（P）}}_{我, k个} = {J型}_{（f）, X（X） 我} {P（P）}_{我, k个 - 1} {J型}_{（f）, X（X） 我}^{T型} + {J型}_{（f）, W公司} Σ_{w个} {J型}_{（f）, W公司}^{T型}

（A.5）

在研究中的案例中，功能（f）可以定义为分别分析Xõ_{i、 k个}，从而获得表达式（A.6）至（A.8）以替换（A.3）如下：

{\tilde{x个}}_{我, k个} = {\hat{x个}}_{我, k个 - 1} + \frac{(ω_{我, k个} + ω_{R（右）, k个}) 第页}{2} \cdot T型 \cdot 余弦 [{\hat{θ}}_{我, k个 - 1} + \frac{(ω_{R（右）, k个} - ω_{我, k个}) 第页}{D类} \cdot T型]

（A.6）

{\tilde{年}}_{我, k个} = {\hat{年}}_{我, k个 - 1} + \frac{(ω_{我, k个} + ω_{R（右）, k个}) 第页}{2} \cdot T型 \cdot 罪 [{\hat{θ}}_{我, k个 - 1} + \frac{(ω_{R（右）, k个} - ω_{我, k个}) 第页}{D类} \cdot T型]

（A.7）

{\tilde{θ}}_{我, k个} = {\hat{θ}}_{我, k个 - 1} + \frac{(ω_{R（右）, k个} - ω_{我, k个}) 第页}{D类} \cdot T型

（A.8）

哪里T型是离散融合过程的采样时间，第页是有效车轮半径D类是它们之间的距离。在此特定应用中T型=0.05秒，第页=0.09米和D类=0.33米。

为了获得P̃_{i、 k个}、组件J型_{f、 Xi（希）},J型_{f、 W公司}和Σ_W公司都是必需的。表达式（A.9）和（A.12）在感兴趣的上下文中定义了前两个雅可比：

{J型}_{（f）, Xi（希）} = [\begin{matrix} \frac{\partial {\tilde{x个}}_{我, k个}}{\partial {\hat{x个}}_{我, k个 - 1}} & \frac{\partial {\tilde{x个}}_{我, k个}}{\partial {\hat{年}}_{我, k个 - 1}} & \frac{\partial {\tilde{x个}}_{我, k个}}{\partial {\hat{θ}}_{我, k个 - 1}} \\ \frac{\partial {\tilde{年}}_{我, k个}}{\partial {\hat{x个}}_{我, k个 - 1}} & \frac{\partial {\tilde{年}}_{我, k个}}{\partial {\hat{年}}_{我, k个 - 1}} & \frac{\partial {\tilde{年}}_{我, k个}}{\partial {\hat{θ}}_{我, k个 - 1}} \\ \frac{\partial {\tilde{θ}}_{我, k个}}{\partial {\hat{x个}}_{我, k个 - 1}} & \frac{\partial {\tilde{θ}}_{我, k个}}{\partial {\hat{年}}_{我, k个 - 1}} & \frac{\partial {\tilde{θ}}_{我, k个}}{\partial {\hat{θ}}_{我, k个 - 1}} \end{matrix}] = [\begin{matrix} 1 & 0 & \frac{\partial {\tilde{x个}}_{我, k个}}{\partial {\hat{θ}}_{我, k个 - 1}} \\ 0 & 1 & \frac{\partial {\tilde{年}}_{我, k个}}{\partial {\hat{θ}}_{我, k个 - 1}} \\ 0 & 0 & 1 \end{matrix}]

（A.9）

哪里：

\frac{\partial {\tilde{x个}}_{我, k个}}{\partial {\hat{θ}}_{我, k个 - 1}} = - \frac{(ω_{我, k个} + ω_{R（右）, k个}) 第页}{2} \cdot T型 \cdot 罪 ({\hat{θ}}_{我, k个 - 1} + \frac{(ω_{我, k个} + ω_{R（右）, k个}) 第页}{D类} \cdot T型)

（A.10）

\frac{\partial {\tilde{年}}_{我, k个}}{\partial {\hat{θ}}_{我, k个 - 1}} = \frac{(ω_{我, k个} + ω_{R（右）, k个}) 第页}{2} \cdot T型 \cdot 余弦 ({\hat{θ}}_{我, k个 - 1} + \frac{(ω_{我, k个} + ω_{R（右）, k个}) 第页}{D类} \cdot T型)

（A.11）

和：

{J型}_{（f）, W公司} = [\begin{matrix} \frac{\partial {\tilde{x个}}_{我, k个}}{\partial ω_{R（右）, k个}} & \frac{\partial {\tilde{x个}}_{我, k个}}{\partial ω_{我, k个}} \\ \frac{\partial {\tilde{年}}_{我, k个}}{\partial ω_{R（右）, k个}} & \frac{\partial {\tilde{年}}_{我, k个}}{\partial ω_{我, k个}} \\ \frac{\partial {\tilde{θ}}_{我, k个}}{\partial ω_{R（右）, k个}} & \frac{\partial {\tilde{θ}}_{我, k个}}{\partial ω_{我, k个}} \end{matrix}]

（A.12）

雅各布的不同元素J型_{f、 W公司}应用（A.6）至（A.8）中所述的运动学关系可获得如下结果：

\begin{array}{l} \frac{\partial {\tilde{x个}}_{我, k个}}{\partial ω_{R（右）, k个}} = \frac{第页}{2} \cdot T型 \cdot 余弦 [{\hat{θ}}_{我, k个 - 1} + \frac{(ω_{R（右）, k个} - ω_{我, k个}) 第页}{D类} \cdot T型] - \\ \frac{(ω_{R（右）, k个} + ω_{我, k个}) {第页}^{2}}{2 D类} \cdot {T型}^{2} \cdot 罪 [{\hat{θ}}_{我, k个 - 1} + \frac{(ω_{R（右）, k个} - ω_{我, k个}) 第页}{D类} \cdot T型] \end{array}

（A.13）

\begin{array}{l} \frac{\partial {\tilde{x个}}_{我, k个}}{\partial ω_{我, k个}} = \frac{第页}{2} \cdot T型 \cdot 余弦 [{\hat{θ}}_{我, k个 - 1} + \frac{(ω_{R（右）, k个} - ω_{我, k个}) 第页}{D类} \cdot T型] + \\ \frac{(ω_{R（右）, k个} + ω_{我, k个}) {第页}^{2}}{2 D类} \cdot {T型}^{2} \cdot 罪 [{\hat{θ}}_{我, k个 - 1} + \frac{(ω_{R（右）, k个} - ω_{我, k个}) 第页}{D类} \cdot T型] \end{array}

（A.14）

\begin{array}{l} \frac{\partial {\tilde{年}}_{我, k个}}{\partial ω_{R（右）, k个}} = \frac{第页}{2} \cdot T型 \cdot 罪 [{\hat{θ}}_{我, k个 - 1} + \frac{(ω_{R（右）, k个} - ω_{我, k个}) 第页}{D类} \cdot T型] + \\ \frac{(ω_{R（右）, k个} + ω_{我, k个}) {第页}^{2}}{2 D类} \cdot {T型}^{2} \cdot 余弦 [{\hat{θ}}_{我, k个 - 1} + \frac{(ω_{R（右）, k个} - ω_{我, k个}) 第页}{D类} \cdot T型] \end{array}

（A.15）

\begin{array}{l} \frac{\partial {\tilde{年}}_{我, k个}}{\partial ω_{我, k个}} = \frac{第页}{2} \cdot T型 \cdot 罪 [{\hat{θ}}_{我, k个 - 1} + \frac{(ω_{R（右）, k个} - ω_{我, k个}) 第页}{D类} \cdot T型] - \\ \frac{(ω_{R（右）, k个} + ω_{我, k个}) {第页}^{2}}{2 D类} \cdot {T型}^{2} \cdot 余弦 [{\hat{θ}}_{我, k个 - 1} + \frac{(ω_{R（右）, k个} - ω_{我, k个}) 第页}{D类} \cdot T型] \end{array}

（A.16）

\frac{\partial {\tilde{θ}}_{我, k个}}{\partial ω_{R（右）, k个}} = \frac{第页}{D类} \cdot T型

（A.17）

\frac{\partial {\tilde{θ}}_{我, k个}}{\partial ω_{我, k个}} = - \frac{第页}{D类} \cdot T型

（A.18）

此外，与里程信息相关的噪声协方差矩阵Σ_W公司是根据经验和统计定义的（见第三节），因此应用如下：

Σ_{w个} = [\begin{matrix} Σ_{电子控制} & 0 \\ 0 & Σ_{电子控制} \end{matrix}], 存在 Σ_{电子控制} = 3.8125 106 {拉德}^{2} / 秒^{2}

（A.19）

一次方程式（A.3）已详细说明，我们将重点放在方程式（A.4）通过g函数。它包括先例单元的条款F类_i−1和激光扫描仪测量，如中所述方程式（3）到(5)，并已在中描述图5基于激光传感系统的估计姿态包括三个部分Z轴_{i、 k个}= [x̄_{i、 k个},ȳ_{i、 k个},θ̄_{i、 k个}]^T型将单独分析：

{\bar{x个}}_{我, k个} = {\hat{x个}}_{我 - 1, k个 - 1} - {d日}_{里, k个} 余弦 [θ_{ci公司, k个} + {\bar{θ}}_{我, k个 - 1} - \frac{π}{2} + 阿坦 2 (\frac{{d日}_{1, k个} 罪 (θ_{1, k个}) - {d日}_{2, k个} 罪 (θ_{2, k个})}{{d日}_{2, k个} 余弦 (θ_{2, k个}) - {d日}_{1, k个} 余弦 (θ_{1, k个})})]

（A.20）

{\bar{年}}_{我, k个} = {\hat{年}}_{我 - 1, k个 - 1} - {d日}_{里, k个} 罪 [θ_{ci公司, k个} + {\bar{θ}}_{我 - 1, k个 - 1} - \frac{π}{2} + 阿坦 2 (\frac{{d日}_{1, k个} 罪 (θ_{1, k个}) - {d日}_{2, k个} 罪 (θ_{2, k个})}{{d日}_{2, k个} 余弦 (θ_{2, k个}) - {d日}_{1, k个} 余弦 (θ_{1, k个})})]

（A.21）

{\bar{θ}}_{我, k个} = {\hat{θ}}_{我 - 1, k个 - 1} - \frac{π}{2} + 阿坦 2 (\frac{{d日}_{1, k个} 罪 (θ_{1, k个}) - {d日}_{2, k个} 罪 (θ_{2, k个})}{{d日}_{2, k个} 余弦 (θ_{2, k个}) - {d日}_{1 k个} 余弦 (θ_{1, k个})})

（A.22）

此时，纠正步骤已完成。卡尔曼增益K（K）_{i、 k个}，估计误差协方差矩阵P（P）_{i、 k个}和修正后的姿势X̂_{i、 k个}进行评估，如所示图7.卡尔曼增益是从雅可比矩阵中获得的J型_{g、 X̄}和J型_{g、 V（V）}，其值计算如下：

{J型}_{克, \bar{X（X）}} = [\begin{matrix} \frac{\partial {\bar{x个}}_{我, k个}}{\partial {\bar{x个}}_{我, k个}} & \frac{\partial {\bar{x个}}_{我, k个}}{\partial {\bar{年}}_{我, k个}} & \frac{\partial {\bar{x个}}_{我, k个}}{\partial {\bar{θ}}_{我, k个}} \\ \frac{\partial {\bar{年}}_{我, k个}}{\partial {\bar{x个}}_{我, k个}} & \frac{\partial {\bar{年}}_{我, k个}}{\partial {\bar{年}}_{我, k个}} & \frac{\partial {\bar{年}}_{我, k个}}{\partial {\bar{θ}}_{我, k个}} \\ \frac{\partial {\bar{θ}}_{我, k个}}{\partial {\bar{x个}}_{我, k个}} & \frac{\partial {\bar{θ}}_{我, k个}}{\partial {\bar{年}}_{我, k个}} & \frac{\partial {\bar{θ}}_{我, k个}}{\partial {\bar{θ}}_{我, k个}} \end{matrix}] = [\begin{matrix} 1 & 0 & \frac{\partial {\bar{x个}}_{我, k个}}{\partial {\bar{θ}}_{我, k个}} \\ 0 & 1 & \frac{\partial {\bar{年}}_{我, k个}}{\partial {\bar{θ}}_{我, k个}} \\ 0 & 0 & 1 \end{matrix}]

（A.23）

哪里：

\frac{\partial {\bar{x个}}_{我, k个}}{\partial {\bar{θ}}_{我, k个}} = {d日}_{里, k个} 罪 (θ_{ci公司, k个} + {\bar{θ}}_{我, k个})

（A.24）

\frac{\partial {\bar{年}}_{我, k个}}{\partial {\bar{θ}}_{我, k个}} = - {d日}_{里, k个} 余弦 (θ_{ci公司, k个} + {\bar{θ}}_{我, k个})

（A.25）

和：

{J型}_{克, V（V）} = [\begin{matrix} \frac{\partial {\bar{x个}}_{我, k个}}{\partial {d日}_{里, k个}} & \frac{\partial {\bar{x个}}_{我, k个}}{\partial θ_{ci公司, k个}} & \frac{\partial {\bar{x个}}_{我, k个}}{\partial {d日}_{1, k个}} & \frac{\partial {\bar{x个}}_{我, k个}}{\partial θ_{1, k个}} & \frac{\partial {\bar{x个}}_{我, k个}}{\partial {d日}_{2, k个}} & \frac{\partial {\bar{x个}}_{我, k个}}{\partial θ_{2, k个}} \\ \frac{\partial {\bar{年}}_{我, k个}}{\partial {d日}_{里, k个}} & \frac{\partial {\bar{年}}_{我, k个}}{\partial θ_{ci公司, k个}} & \frac{\partial {\bar{年}}_{我, k个}}{\partial {d日}_{1, k个}} & \frac{\partial {\bar{年}}_{我, k个}}{\partial θ_{1, k个}} & \frac{\partial {\bar{年}}_{我, k个}}{\partial {d日}_{2, k个}} & \frac{\partial {\bar{年}}_{我, k个}}{\partial θ_{2, k个}} \\ \frac{\partial {\bar{θ}}_{我, k个}}{\partial {d日}_{里, k个}} & \frac{\partial {\bar{θ}}_{我, k个}}{\partial θ_{ci公司, k个}} & \frac{\partial {\bar{θ}}_{我, k个}}{\partial {d日}_{1, k个}} & \frac{\partial {\bar{θ}}_{我, k个}}{\partial θ_{1, k个}} & \frac{\partial {\bar{θ}}_{我, k个}}{\partial {d日}_{2, k个}} & \frac{\partial {\bar{θ}}_{我, k个}}{\partial θ_{2, k个}} \end{matrix}]

（A.26）

从（A.29）到（A.46）的表达式允许实现J型_{g、 V（V）}，知道：

一 = {d日}_{1, k个} 罪 (θ_{1, k个}) - {d日}_{2, k个} 罪 (θ_{2, k个})

（A.27）

b条 = {d日}_{2, k个} 余弦 (θ_{2, k个}) - {d日}_{1, k个} 余弦 (θ_{1, k个})

（A.28）

然后：

\frac{\partial {\bar{x个}}_{我, k个}}{\partial {d日}_{里, k个}} = - 余弦 [θ_{ci公司, k个} + {\hat{θ}}_{我 - 1, k个 - 1} - \frac{π}{2} + 阿坦 2 (\frac{{d日}_{1, k个} 罪 (θ_{1, k个}) - {d日}_{2, k个} 罪 (θ_{2, k个})}{{d日}_{2, k个} 余弦 (θ_{2, k个}) - {d日}_{1, k个} 余弦 (θ_{1, k个})})]

（A.29）

\frac{\partial {\bar{x个}}_{我, k个}}{\partial θ_{ci公司, k个}} = {d日}_{里} 罪 [θ_{ci公司, k个} + {\hat{θ}}_{我 - 1, k个 - 1} - \frac{π}{2} + 阿坦 2 (\frac{{d日}_{1, k个} 罪 (θ_{1, k个}) - {d日}_{2, k个} 罪 (θ_{2, k个})}{{d日}_{2, k个} 余弦 (θ_{2, k个}) - {d日}_{1, k个} 余弦 (θ_{1, k个})})]

（A.30）

\frac{\partial {\bar{x个}}_{我, k个}}{\partial {d日}_{1, k个}} = {d日}_{里} 罪 [θ_{ci公司, k个} + {\hat{θ}}_{我 - 1, k个 - 1} - \frac{π}{2} + 阿坦 2 (\frac{一}{b条})] \frac{罪 (θ_{1, k个}) b条 + 余弦 (θ_{1, k个}) 一}{一^{2} + {b条}^{2}}

（A.31）

\frac{\partial {\bar{x个}}_{我, k个}}{\partial θ_{1, k个}} = {d日}_{里} 罪 [θ_{ci公司, k个} + {\hat{θ}}_{我 - 1, k个 - 1} - \frac{π}{2} + 阿坦 2 (\frac{一}{b条})] \frac{余弦 (θ_{1, k个}) b条 - 罪 (θ_{1, k个}) 一}{一^{2} + {b条}^{2}} {d日}_{1, k个}

（A.32）

\frac{\partial {\bar{x个}}_{我, k个}}{\partial {d日}_{2, k个}} = {d日}_{里} 罪 [θ_{ci公司, k个} + {\hat{θ}}_{我 - 1, k个 - 1} - \frac{π}{2} + 阿坦 2 (\frac{一}{b条})] \frac{- 罪 (θ_{2, k个}) b条 - 余弦 (θ_{2, k个}) 一}{一^{2} + {b条}^{2}}

（A.33）

\frac{\partial {\bar{x个}}_{我, k个}}{\partial θ_{2, k个}} = {d日}_{里} 罪 [θ_{ci公司, k个} + {\hat{θ}}_{我 - 1, k个 - 1} - \frac{π}{2} + 阿坦 2 (\frac{一}{b条})] \frac{罪 (θ_{2, k个}) 一 - 余弦 (θ_{2, k个}) b条}{一^{2} + {b条}^{2}} {d日}_{2, k个}

（A.34）

\frac{\partial {\bar{年}}_{我, k个}}{\partial {d日}_{里, k个}} = - 罪 [θ_{ci公司, k个} + {\hat{θ}}_{我 - 1, k个 - 1} - \frac{π}{2} + 阿坦 2 (\frac{{d日}_{1, k个} 罪 (θ_{1, k个}) - {d日}_{2, k个} 罪 (θ_{2, k个})}{{d日}_{2, k个} 余弦 (θ_{2, k个}) - {d日}_{1, k个} 余弦 (θ_{1, k个})})]

（A.35）

\frac{\partial {\bar{年}}_{我, k个}}{\partial θ_{ci公司, k个}} = - {d日}_{里} 余弦 [θ_{ci公司, k个} + {\hat{θ}}_{我 - 1, k个 - 1} - \frac{π}{2} + 自动变速箱 2 (\frac{{d日}_{1, k个} 罪 (θ_{1, k个}) - {d日}_{2, k个} 罪 (θ_{2, k个})}{{d日}_{2, k个} 余弦 (θ_{2, k个}) - {d日}_{1, k个} 余弦 (θ_{1, k个})})]

（A.36）

\frac{\partial {\bar{年}}_{我, k个}}{\partial {d日}_{1, k个}} = - {d日}_{里} 余弦 [θ_{ci公司, k个} + {\hat{θ}}_{我 - 1, k个 - 1} - \frac{π}{2} + 阿坦 2 (\frac{一}{b条})] \frac{罪 (θ_{1, k个}) b条 + 余弦 (θ_{1, k个}) 一}{一^{2} + {b条}^{2}}

（A.37）

\frac{\partial {\bar{年}}_{我, k个}}{\partial θ_{1, k个}} = - {d日}_{里} 余弦 [θ_{ci公司, k个} + {\hat{θ}}_{我 - 1, k个 - 1} - \frac{π}{2} + 自动变速箱 2 (\frac{一}{b条})] \frac{余弦 (θ_{1, k个}) b条 - 罪 (θ_{1, k个}) 一}{一^{2} + {b条}^{2}} {d日}_{1, k个}

（A.38）

\frac{\partial {\bar{年}}_{我, k个}}{\partial {d日}_{2, k个}} = - {d日}_{里} 余弦 [θ_{ci公司, k个} + {\hat{θ}}_{我 - 1, k个 - 1} - \frac{π}{2} + 阿坦 2 (\frac{一}{b条})] \frac{- 罪 (θ_{2, k个}) b条 - 余弦 (θ_{2, k个}) 一}{一^{2} + {b条}^{2}}

（公元39年）

\frac{\partial {\bar{年}}_{我, k个}}{\partial θ_{2, k个}} = - {d日}_{里} 余弦 [θ_{ci公司, k个} + {\hat{θ}}_{我 - 1, k个 - 1} - \frac{π}{2} + 自动变速箱 2 (\frac{一}{b条})] \frac{罪 (θ_{2, k个}) 一 - 余弦 (θ_{2, k个}) b条}{一^{2} + {b条}^{2}} {d日}_{2, k个}

（A.40）

\frac{\partial {\bar{θ}}_{我, k个}}{\partial {d日}_{c（c）, k个}} = 0

（A.41）

\frac{\partial {\bar{θ}}_{我, k个}}{\partial θ_{c（c）, k个}} = 0

（A.42）

\frac{\partial {\bar{θ}}_{我, k个}}{\partial {d日}_{1, k个}} = = \frac{罪 (θ_{1, k个}) b条 + 余弦 (θ_{1, k个}) 一}{一^{2} + {b条}^{2}}

（A.43）

\frac{\partial {\bar{θ}}_{我, k个}}{\partial θ_{1, k个}} = \frac{余弦 (θ_{1, k个}) b条 - 传感器 (θ_{1, k个}) 一}{一^{2} + {b条}^{2}} {d日}_{1, k个}

（A.44）

\frac{\partial {\bar{θ}}_{我, k个}}{\partial {d日}_{2, k个}} = \frac{- 罪 (θ_{2, k个}) b条 - 余弦 (θ_{2, k个}) 一}{一^{2} + {b条}^{2}}

（A.45）

\frac{\partial {\bar{θ}}_{我, k个}}{\partial θ_{2, k个}} = \frac{- 余弦 (θ_{2, k个}) b条 + 传感器 (θ_{2, k个}) 一}{一^{2} + {b条}^{2}} {d日}_{2, k个}

（公元46年）

关于激光扫描仪信息的噪声协方差矩阵Σ_V（V）通过第三节中所述的角度和距离参数进行统计和经验定义。在研究案例中，结果为：

Σ_{V（V）} = [\begin{matrix} Σ_{d日} & 0 & 0 & 0 & 0 & 0 \\ 0 & Σ_{θ} & 0 & 0 & 0 & 0 \\ 0 & 0 & Σ_{d日} & 0 & 0 & 0 \\ 0 & 0 & 0 & Σ_{θ} & 0 & 0 \\ 0 & 0 & 0 & 0 & Σ_{d日} & 0 \\ 0 & 0 & 0 & 0 & 0 & Σ_{θ} \end{matrix}]

（A.47）

哪里Σ_d日= 8.286 · 10⁻⁶米²、和Σ_θ= 7.615 · 10⁻⁵拉德².

如前几段所述生成的所有数据都需要更正F类_我姿势，通过表情（A.48）。此方程执行状态向量的校正X̂_{i、 k个}，根据其预测X̃_{i、 k个}基于里程信息和激光扫描仪测量得出的姿势Z轴_{i、 k个}以下为：

{\hat{X（X）}}_{我, k个} = {\tilde{X（X）}}_{我, k个} + Θ_{k个} {K（K）}_{我, k个} [{Z轴}_{我, k个} - {\tilde{X（X）}}_{我, k个})]

（A.48）

哪里Θ_k个基本功能见第3节。

校正步骤结束更新误差估计协方差矩阵，如下所示：

{P（P）}_{我, k个} = (我 - {K（K）}_{我, k个} {J型}_{克, \bar{X（X）}}) {\tilde{P（P）}}_{我, k个}

（A.49）

我是单位矩阵。

图1。P3-DX机器人平台之一，配备编码器和激光扫描仪，用于排引导演示。

图2。由三个机器人组成的车队，如图所示图1与激光扫描系统相关的地标位于机器人顶部。

图3。控制级的组织及其与每个跟随器单元中包含的传感系统的关系。

图4。排高级控制中涉及的变量：领队轨迹点、每个跟随者的姿势以及通过激光扫描仪获得的相对位置信息。

图5。变量之间的几何关系描述[d日₁,θ₁,d日₂,θ₂,d日_里,θ_ci公司]暗示在激光扫描仪相对定位系统中。

图5。变量之间的几何关系描述[d日₁,θ₁,d日₂,θ₂,d日_里,θ_ci公司]隐含在激光扫描仪的相对定位系统中。

图6。EKF中隐含的过程和变量，以获取Fi运动状态。X̃是预测姿势（里程表系统），Z轴是估计的姿势（激光测量），以及X̂表示修正后的姿势（EKF算法）。

图7。基于标准EKF实现的融合算法框图。

图8。由P3-DX机器人单元跟随领队进行移动。红色轨迹与机器人的里程信息一致。然而，激光扫描仪提供了更真实的蓝色路径。

图8。由P3-DX机器人单元跟随领导者开发的运动。红色轨迹与机器人的里程信息一致。然而，激光扫描仪提供了更真实的蓝色路径。

图9。随动装置在所示轨迹中的线速度和角速度图8。里程表系统记录的值以蓝色绘制，过滤的值（用于控制任务）以红色绘制。

图10。跟随器单元使用离散EKF融合方案进行路径跟踪，作为高级控制的一部分。蓝色图显示激光扫描仪记录的位置信息，红色图显示EKF估计的位置。激光扫描仪仅在间隔时间内可用”ab公司”, “光盘“和”ef（参考）”.

图10。跟随器单元使用离散EKF融合方案进行路径追踪，作为高级控制的一部分。蓝色图显示激光扫描仪记录的位置信息，红色图显示EKF估计的位置。激光扫描仪仅在间隔时间内可用”ab公司”, “光盘“和”ef（参考）”.

图11。排遵循的轨迹。引线描述的路径以红色绘制，第一个跟随者描述的路径显示为绿色，第二个跟随者所描述的路径绘制为黑色。先导的参考轨迹输入也显示为蓝色。

分享和引用

MDPI和ACS样式

埃斯皮诺萨，F。；桑托斯，C。；马里恩·罗梅拉，M。；Pizarro，D。；瓦尔德斯，F。；J·多吉尔。基于离散扩展卡尔曼滤波器的测速和激光扫描仪融合用于机器人排导。传感器 2011,11, 8339-8357.https://doi.org/10.3390/s110908339

AMA风格

Espinosa F、Santos C、Marrón-Romera M、Pizarro D、Valdés F、Dongil J。用于机器人排导的基于离散扩展卡尔曼滤波器的测距和激光扫描仪融合。传感器. 2011; 11(9):8339-8357.https://doi.org/10.3390/s110908339

芝加哥/图拉宾风格

埃斯皮诺萨、费利佩、卡洛斯·桑托斯、马尔塔·马尔隆·罗梅拉、丹尼尔·皮萨罗、费尔南多·巴尔德斯和哈维尔·多吉尔。2011.“基于离散扩展卡尔曼滤波器的机器人排班制导里程表和激光扫描仪融合”传感器11号，编号：8339-8357。https://doi.org/10.3390/s110908339

文章菜单

基于离散扩展卡尔曼滤波的机器人排导里程计和激光扫描仪融合

摘要

1.简介

2.基于里程表和激光组合的跟随器单元姿态估计

3.离散EKF在里程计和激光融合中的应用

4.实验结果

5.结论

致谢

工具书类

附录

离散EKF算法实现的数学描述

分享和引用

文章指标

文章访问统计

更多信息

指导方针

MDPI计划

遵循MDPI