基于Webots仿真平台，在未知的环境中，控制小车从起点运动到终点。假设已知起点为右上角，终点为左下角，控制器的编程语言为Python。
小车的感知部分主要使用了Webots中的三种设备， lidar, IMU 和gps。各设备作用如下：
Lidar： 测距传感器，发射光束，返回遇到障碍物的距离，根据小车自身的位置完成映射，从而构建障碍物
IMU：惯性测量单元 ，获得小车的朝向，以此控制小车的运动
GPS：定位小车，获得实时的坐标
Webots中的Lidar是一种测距传感器，激光探头会在一帧时间内，旋转360°，获取全向的距离信息。由IMU得到小车的朝向，从而可以计算障碍点的坐标。
用一个二维数组来存储地图的信息，0代表该点存在障碍物。注意GPS返回的是仿真场景中的坐标，和实际地图的坐标是有区别的，需要进行转换。
雷达在获取数据时，若雷达同时在运动，则一帧雷达数据中，雷达点会在不同位置被采集。比如，扫描频率5Hz的激光雷达，一帧数据的首尾时间差200ms，若机器人以0.5m/s的速度向x方向移动，扫描前面的墙面，那么200ms后尾部的测量距离和首部的测量距离在x方向上相差10cm，如下图所示：
这里使用插值的方法处理运动畸变，代码中已做了注释。处理前后对比如下：
路径规划使用RRT算法，基本思路是：以当前点为根节点，随机生长若干的新节点，生长时进行碰撞 检测，保证节点间的连线不会触碰小车感知到的墙体。每次生长后，选择新节点中距离终点代价最小的 节点继续生长，直到生长的节点可以直接连接终点为止，最后记录这条路径。
生长新节点时，我们生成一个随机的角度，然后进行碰撞检测，保证新的节点和节点间的连线都不会碰到墙体。在进行碰撞检测时，为了尽可能减少误差，在地图中判断对应的值时，除了目标点以外还取 了额外的四个点（一个正方形的四角）来判断是否为障碍物。
由于树的生长存在一定的随机性，为了避免生长时在一个墙角内多次重复搜索浪费时间，在碰撞检测 的基础上加入一个二次检查，如果新的节点与之前生成的某个节点距离过近，则判断为重复搜索，进行 舍弃。
RRT得到的路径，很有可能是一条曲线。为了得到一条平滑的路径，每次成功RRT后需要进行一次路 径合并：从终点出发，沿着路径往回搜索每个节点，一旦搜索到的节点和起点可以直接相连（没有碰 撞），把中间的节点全部剔除。代
运动控制部分比较简单，设定一个 move_flag 来标志小车能否运动。当完成一次成功的RRT后，便设置 move_flag = 1 ，然后让小车进行运动，运动到目标点后，再设置 move_flag = 0 ，让小车重新进行路径规划。
编程语言Python，引入第三方库OpenCV，仿真平台WebotsR2021a。
代码中包含使用OpenCV动态展示的随机树的生长以及路径的规划，由于耗时较长，做了注释处理，有需要可以取消注释。
