ObstacleAvoidanceAlgoBasedOnWebots

基于Webots平台的简易智能机器人避障算法的实现（华南理工大学2021年智能机器人期末课程作业）

实验目的：

熟悉机器人仿真软件（例如Simbad、Webots、TeamBots、Player/Stage/Gazebo、MotionPlaner等）的使用方法；掌握若干机器人路径规划算法。能够更清楚了解智能机器人软硬件组成、工作原理等基本知识；熟练掌握机器人程序的设计与编写；能够综合运用所学基础理论和专业知识进行创新设计的能力。

实验要求：

在机器人软件平台上建立一个包含若干个静止障碍物和运动障碍物的仿真环境，设定机器人的起始点和终点后，机器人能够规划出一条从起始点到目标点的安全路径。查阅相关路径规划算法，实现一种以上算法并相互比较。要求给出源代码、试验结果并且进行演示。

实验环境和平台：

Win10 + Webots

代码语言：

python

实验过程：

1.了解软件结构和各功能分区

打开界面，左侧栏是node界面，用于添加/删除/修改世界中的各节点。右侧是控制器代码的文本编辑器页面，Webots允许使用C/C++/Java/python/matlab等多种编程语言，可以选择自己熟悉的语言进行编写。页面上方是控制模拟的各种按钮，包括开始/暂停/重载等。页面最下方是控制台，可以看到程序运行过程中输出的信息。页面正中央是视图区，可以设计世界布局及调整视角。

2.新建一个世界

通过添加node，可以在不同的位置摆放不同的障碍物。Webots提供了各类内置的basic node。展开node，可以设置node的具体属性，包括transition，rotation，color，size等。

除了webots内置节点外，还可以通过修改basic node中的children属性，添加子节点，来实现自主设计。在本次实验中，我使用了内置的e-puck节点进行避障实验，同时还自主搭建了一个同时配备GPS，compass，两个发动机和8个距离传感器的robot来进行路径规划实验。

3.用e-puck机器人实现自动避障（包括静态障碍和动态障碍）

E-puck内置了8个距离传感器和左右两个发动机。其运动原理是通过读取位于其转塔周围的8个红外距离传感器的返回值，根据传感器读数判断周围环境并驱动两个车轮。距离传感器返回的值在0到4096之间缩放。数值越大表示障碍物越近。

避障代码实现在test_controller.py文件中

实现原理：

使用 getDevice() 方法，初始化各传感器。使用 enable() 和 setPosition() ， setVelocity() 等方法，初始化传感器和发动机的各项参数。

在while主循环中，通过 getValue() 方法读取传感器数值，并根据返回的传感器数值判断是否发生了碰撞。如果发生碰撞，则改变左右两个发动机的速度来实现转向。

4.实现BFS路径规划控制器

bfs_target.py:
路径规划算法，按照给定的地图设计合理路径，封装成BFS类

实现了 getTarget() 方法。该方法会返回一个列表类型，在规划路径后记录路径中所经点的坐标，并传递给主控制器。

bfs.py:
控制器代码，启动各部件并控制机器人按照规划好的路径进行移动

根据world的障碍物布局，手动输入map列表，实例化bfs类得到target列表（存放目标路径的经过点坐标）

计算robot当前位置到目标位置的距离和方向夹角，调整发动机速度。

5.实现DFS路径规划控制器

dfs_target.py
dfs.py

通过调整robots的controller参数，即可试验不同控制器的避障效果

6.实验结果和改进方向

在我个人设计的世界地图下，e-puck和连接了bfs控制器的robot被放置在同一场景下测试。

e-puck机器人在碰见静态障碍物（如地图中的木箱）和动态障碍物如（运动中的robot）都能实现自动转向和避障。
Robot测试过程中，设置左上角为起始点，右下角为终点。测试发现robot能够按照给定的地图进行合理的路径规划，并在控制台输出路径step数。修改robot的控制器属性为dfs可以进行相同的测试。

实验过程中也遇到了一些难点：

1、在实现BFS时，多次用到了list类型和dict类型，并组合使用。在构建队列时忽视了 浅拷贝和深拷贝 的区别，导致路径规划错误，花费了大量的时间进行修正。
2、除了BFS和DFS，还尝试了实现人工势场法，但由于对软件熟悉程度不够和时间不足，导致代码实现部分仍然存在问题，故未实现
3、在构建路径前， 需要手动输入 map列表。没有找到直接获取整个布局状态的传感器或对应的传感器参数，导致浪费大量时间且存在误差。
4、障碍物设置不当，自动避障小车在与球类障碍物相撞时，有时会引起 球类障碍物位置的偏移 ，进而影响路径规划的准确度。因此在最终实验中，用油桶或木块等其他障碍物进行了替代。需要找到克服球类障碍物随意移动特性的方法