在本课中，我们将探讨自动驾驶汽车如何规划穿越世界的轨迹。车辆基于高清地图、感知和预测模块的数据制定此计划。首先，我们介绍如何使用a星规划到达目的地的地图级路线。然后，我们专注于建立无碰撞的轨迹，引导我们一步一步地到达目的地。我们学习了如何使用过去的速度解耦规划或网格规划来构建这些轨迹。比较这两种方法将培养你的规划直觉以及如何使用成本函数选择轨迹。规划是开发自动驾驶汽车最困难的部分之一。

在规划路线的时，需要机器人路线附近的障碍物距离，机器人控制系统需要知道当前机器人与障碍物最短的距离。本文主要是分析如何计算机器人与障碍物的距离，如果将机器人和障碍物分别考虑成质点，机器人与障碍物的距离就很容易求解了，但是事实上，障碍物与机器人在实际工程中不可能是质点。因此，本文需要解决的是：机器人形状分别圆形、线性、多边形，障碍物也分别是圆形、线性、多边形时，二者的最小距离求解。

根据两个车轮的轮速去估计当前的车的定位。

x + a2 * x^2 + a3 * x^3 ，那么现在有个需求，希望根据上述x和y的关系，去估计规划预瞄轨迹yawRate。首先CD = x，AC = y ,BD = AB = R,CD与BD相互垂直 ，AC与CD垂直。角BDA = 角BAD =90 – angle，故有角B = 2 * angle。又由于angle = arcsin(y/(x^ 2+y^ 2)^(0.5))

提到卡尔曼，不得不说一个故事：片绿油油的草地上有一条曲折的小径,通向一棵大树.一个要求被提出:从起点沿着小径走到树下.“很简单.” A说,于是他丝毫不差地沿着小径走到了树下.现在，难度被增加了：蒙上眼。“也不难，我当过特种兵。” B说，于是他歪歪扭扭地走到了树旁。“唉，好久不练，生疏了。” （只凭自己的预测能力）“看我的，我有 DIY 的 GPS！” C说，于是他像个醉汉似地歪歪扭扭的走到了树旁。

对于每个障碍物（点/占用的成本图单元、线、多边形），定位计划轨迹的最接近姿态（见图）。此外，更大的障碍需要更多的连接姿势，以避免不可错过的捷径。显然，优化涉及到找到指定成本函数（目标函数）的最小成本解（轨迹）。简单地说：如果一个计划的（未来）姿势违反了与障碍物的期望分离，那么成本函数的成本必须增加。惩罚项确实是非零的，但优化器会被卡住（达到这个局部最小值），因为将相应的姿势横向移动到其中一个障碍物

机器人控制方面有两种模型，一是自行车模型，二是差速模型，今天主要推导了差速模型，实验室的履带车都是差速模型。参考了https://blog.csdn.net/iProphet/article/details/83661753博文。假设电机输出的角速度为和，轮子驱动半径为r，有如下式子：c点的瞬时速度为。左右轮间的间距为l，...

对于每个障碍物（点/占用的成本图单元、线、多边形），定位计划轨迹的最接近姿态（见图）。此外，更大的障碍需要更多的连接姿势，以避免不可错过的捷径。显然，优化涉及到找到指定成本函数（目标函数）的最小成本解（轨迹）。简单地说：如果一个计划的（未来）姿势违反了与障碍物的期望分离，那么成本函数的成本必须增加。惩罚项确实是非零的，但优化器会被卡住（达到这个局部最小值），因为将相应的姿势横向移动到其中一个障碍物

本文主要介绍针对于两轮差速模型的逆运动学数学推导。因为在机器人控制领域，决策规划控制层给执行器输出的控制指令。(角速度)，因此，我们比较关心，当底层两个驱动电机接收到此信息，如何将。我们根据上述公式，给出两轮差速模型的逆运动学数学推导，将。至此，我们给出了两轮差速模型的逆运动学数学推导，即如何将。需要指出的是，此公式也包含了曲率信息，曲率。，此曲率值，可以用于大曲率路径的前馈补偿。如上图，是两轮差

经验表明,估计和控制应用领域的工程师更重视在计算机上简单地实现卡尔曼滤波算法,而没有考虑过卡尔曼滤波算法的计算误差案例。——Gerald Bierman和 Catherine Thornton[ Bie77a]本文着眼于卡尔曼滤波方程的一些其他形式,有一些卡尔曼滤波方程在数学上是等价的。在查阅不同的文献或书籍时,人们可能会发现完全不同的卡尔曼滤波方程的表示形式,这些差异可能是印刷错误,也可能它们在