我们在路径规划中使用的是RRT算法，RRT在地图空间内随机取点延伸可行路径，最后将可以连接起点和终点的路径连接。我们对RRT产生的诸多路径进行了收敛和优化，将众多段短线收敛为几条总路径更短的长线段。相比A*的对地图搜索的完备性，RRT的优势在于检索速度快，代码量小，更适合用于实时规划。
我们在动态避障过程中使用了DWA算法，通过转化为速度空间中搜索，将障碍物、目标点、速度最大化等原则转化为打分的分数，搜索找到最好的速度、角速度。
程序设计结构：
├── course_agv_nav
│ ├── CMakeLists.txt
│ ├── launch
│ │ ├── nav.rviz
│ │ └── nav.launch
│ ├── package.xml
│ └── scripts
│ ├── rrt.py
│ ├── global_planner.py
│ └── dwa.py
│ └── local_planner.py我们主要进行了rrt.py,dwa.py部分,并且对于助教给的global_planner.py,local_planner.py进行了自己的修改，下面的部分主要详细说明rrt.py,dwa.py，global_planner.py,local_planner的改进会在第3个部分说明
def __init__(self, ox, oy, robot_radius, avoid_buffer,minx,miny,maxx,maxy):这个函数初始化了一些基本参数，从global_planner中接收到障碍物信息ox,oy，机器人大小和膨胀半径（避障距离），场地在坐标系中的边界值；接着自己在class中新定义了扫描次数、每次的距离增量等参数。
def plan(self, start_x, start_y, goal_x, goal_y):本函数为rrt算法的主要函数，接收到起始点和目的地的坐标信息，输出为path_x[]与path_y[]的规划路径点列。首先调用KDTree库用障碍物信息构建obstree，building函数生成rrt的路径搜索树，返回一系列点集，接下来通过backpath函数将搜索树中的起点和终点连接起来，返回连接起点和终点的一系列路径点，最后用shrink函数将大量固定步长的较小路径收缩成更少量大步长路径，返回最终的路径规划点。
# Obstacle KD Tree
obstree = KDTree(np.vstack((self.obstacle_x, self.obstacle_y)).T)
# Building tree
pt_x, pt_y,pt_f,endnum = self.building(start_x, start_y, goal_x, goal_y, obstree)
# Search Path
path_x, path_y = self.backpath(pt_x,pt_y,pt_f,endnum)
path_x,path_y=self.shrink(path_x,path_y,obstree)
path_x.reverse()
path_y.reverse()def building(self, start_x, start_y, goal_x, goal_y, obstree):building函数体现rrt的核心思想，在空间内随机取点采样，检测无碰后加入搜索树，直到采样点靠近目标点为止。
def check_obs(self, ix, iy, nx, ny, obstree):check_obs采用增量法判断是否碰到障碍物，从起始点不断伸长小段步长检测碰撞，直到到达目标点仍然无碰返回False。
动态窗口法也是采用了面向对象的方式，主要是定义了DWAPlanner类，并且定义了不同的成员函数。
def __init__(self):首先初始化定义了速度空间中的最大最小速度、速度的步长、预测时间、预测时间的步长、机器人半径，以及三个分数的系数
def dwa_search(self, X, u, goal, obstacles):这是主要的成员函数，在这个成员函数中，用两个for循环对速度和角速度空间进行了搜索，对每一个v,w预测的结果进行打分，并乘上系数，最后找到一个最小的分数对应的v,w，以此作为最好的结果
def calculate_vw_range(self, X):这个函数就是求出在dwa_search函数中搜索的速度和角速度的范围，主要依据的规则就是下面的规则，取交集。
def predict_trajectory(self, x_init, u):这个函数就是对预测时间内的运动情况进行预测，主要通过离散的时间步长进行预测，得到的运动轨迹会返回输入到打分的函数中
def heading_obj_func(self, trajectory, goal):
def clearance_obj_func(self, trajectory, ob):
def velocity_obj_func(self, trajectory):这三个函数就是根据输入的轨迹和目标点、障碍物点去计算打分。
heading_obj_func计算到目标点的距离，以欧式距离为判断标准，距离越近，打分越低，也就越好。
clearance_obj_func计算到所有障碍物的最近距离，由于距离越近越不利，所以我们将最小距离取了倒数，这样的话不利的情况这个打分就会很大。
velocity_obj_func计算速度和最大速度的差别，为了让时间更短，我们希望速度尽可能要大一点，这样有利，所以速度越接近于最大速度，打分会越低。
把他们三个的score整合，乘以对应的系数，总体分数越低那么越倾向于选择这样的速度和角速度
其实路径规划的过程中是不考虑机器人的执行能力的，而在AGV小车运动过程中，可以发现，如果路径中包含转角过大的折线，那么对于AGV小车的运动有很大的挑战，也会在最后的测试中浪费很多不必要的时间，所以我们在RRT规划出相关路径之后针对这种情况，进行了相关的改进，使得降低路径对于机器人运动能力的要求,使得机器人避免急转这种情况。
shrink函数是对原始路径点集的优化，将每个节点尝试与后续节点连接，可以连接则放弃中间节点，最后返回的是优化后的路径点集。
def shrink(self,path_x,path_y,obstree):
length=len(path_x)
i=length-1
while i > 1:
if not self.check_obs(path_x[i],path_y[i],path_x[i-2],path_y[i-2],obstree):
path_x.remove(path_x[i-1])
path_y.remove(path_y[i-1])
i=i-1
return path_x,path_yshrink前：
shrink后:
可以看出，在进行了路径优化之后，路径更加合理，对机器人的运动的连续和平滑性有很大的提高
# Obstacle KD Tree
obstree = KDTree(np.vstack((self.obstacle_x, self.obstacle_y)).T)
# Building tree
pt_x, pt_y,pt_f,endnum = self.building(start_x, start_y, goal_x, goal_y, obstree)虽然本次考核对于路径规划的要求只有两次，似乎时间要求并不高，但是我们还是对其中的一些数据结构进行了优化，其中就使用了KDTree这种数据结构。通过调用python的scipy库，我们在最近临节点的搜索过程加快了速度，对于以后有明确路径规划时间的限制的任务帮助会很大，对于本次仅有两次规划的任务来讲并不是特别的明显。
在dwa运行过程中我们发现，虽然机器人通过三种打分一直有朝着目标走和远离障碍物的倾向，但是总会摆过一个很大的角度，也就是说这个朝向并不在我们的判断范围之内。如果机器人朝向不对的话，那么走路径虽然可能会达到目标，但到达目标的过程中机器人一直在转动，并不稳定，这对于AGV小车来说是不行的。
由此我们想到，是否可以找到一个有关于朝向的第四个指标加入，我们将机器人预测时间内的最后一个点的朝向，与机器人和目标点之间的角度相减并取绝对值，以此来作为打分的第四个标准。
def orientation_obj_func(self, trajectory, goal):
angle1 = math.atan2(goal[1]-trajectory[-1,1], goal[0]-trajectory[-1,0])
angle2 = trajectory[-1,2]
delta = abs(angle1-angle2)
if abs(angle1-angle2) > math.pi:
delta = 2*math.pi - delta
return delta我们发现，通过这样增加新的打分指标，我们得到了更好的机器人运动轨迹，使得机器人在每一次发出的速度都尽可能接近目标点，机器人的朝向也更接近朝向目标点的角度。这样的改进减少了机器人的一些不必要的转向操作，很大程度上节约了时间。
这一部分代码主要的功能是在机器人坐标系下对于读到的激光信息进行处理，利用DWA动态窗口法进行最佳速度和角速度的寻找。但是在我们处理的过程中，由于我们未完整了解助教给出的代码，自己根据自己的想法进行了修改，遇到了很多错误，这一路遇挫的过程我觉得也是十分值得记录的，所以放在了这里。
def u2t(self, u):
w = u[2]
x = u[0]
y = u[1]
return np.array([[math.cos(w),-math.sin(w),x],[math.sin(w),math.cos(w),y],[0,0,1]])
def updateObstacle(self):
self.laser_lock.acquire()
self.plan_ob = []
self.plan_ob = np.array(self.ob)
trans = self.u2t([self.x, self.y, self.yaw])
temp = np.ones([len(self.plan_ob),1])
self.plan_ob = np.concatenate((self.plan_ob,temp),axis=1)
self.plan_ob = trans.dot(self.plan_ob.T)
self.plan_ob = self.plan_ob.T
self.plan_ob = self.plan_ob[:,0:2]
self.laser_lock.release()在这部分的代码中，我们定义了u2t函数，完成了齐次变换矩阵的构建，然后利用读到的激光的x,y坐标，再加上1构成广义坐标，左乘以刚刚计算出的矩阵，通过numpy库进行一些转置、切片等的操作，将机器人坐标系下的激光读到的障碍物坐标变换到全局坐标系下，然后在DWA中就输入全局的机器人坐标、全局的障碍物信息、全局的机器人目标点坐标，进行速度空间的搜索。起初我们并没有完全理解助教所给的这部分代码，于是进行了许多修改，终于完成了自己的版本，和助教的代码殊途同归，而且完成这一部分也为我们进行一定程度的动态避障准备了条件。
刚开始调试实车的时候可以发现，最开始我们的小车会原地打转，经过在不同语句输出相关信息后我们发现，得到的激光信息里有非常小的数值，我们估计激光扫到了自己，所以根据这种情况，我们不仅给激光信息设置了上限，防止读到太多信息影响运算速度；还设置了激光信息的下限，防止扫到自己影响判断。这样的小优化看似不起眼，但实际解决了我们的大问题，同时这个发现也让我们知道实际的一些传感器应用的过程中并不是我们想象的那么理想化，面对一些现实问题，要学会调试与解决。
（注：由于实车实验时多在紧张的调试和测试，并没有太多机会去获取信息，所以这里的讨论主要以仿真环境为主）
我们认为评价路径规划合理性的关键点主要在于减少冗余路径、降低小车执行难度、尽可能避开障碍物这三点。所以我们就从这三点出发去考虑问题：
我们需要提出一个量化指标来评定路径的长短，我们总取一个目标点，大约为（2，-7），将目标点和起始点的距离与路径总长度作差，观察其差值的大小，以此来作为量化指标：
| 规划次数 | 1 | 2 | 3 | 4 | 5 | 10次平均 |
|---|---|---|---|---|---|---|
| 起始点-目标点距离（m） | 7.884 | 8.052 | 7.423 | 8.261 | 7.012 | 7.508 |
| 路径长度（m） | 19.907 | 20.165 | 17.596 | 24.572 | 22.753 | 21.474 |
无论是从图中还是从数据分析角度都可以看出，路径还是有一些冗余的，我们认为这和RRT本身的算法有关系，本来RRT就是只负责找到路径，并不能保证路径一定是最优的，所以从这部分来讲，规划出的路径并不是最好的，有一些绕路。
在第三部分优化也谈到过，其实对于这个AGV小车来讲，直线运动并不耗时，耗时且执行难度大的是有很大转角的部分，所以我们规划出的路径不应该有很大的折线转角，所以我们可以用转角的绝对值总和作为路径规划的一种评价指标，在此分析如下：
读出每次路径节点的坐标信息，用arctan函数进行计算，得到了每次转角的绝对值总和：
| 规划次数 | 1 | 2 | 3 | 4 | 5 | 6 |
|---|---|---|---|---|---|---|
| 转角绝对值总和（rad) | 6.172 | 5.389 | 7.028 | 6.347 | 7.152 | 5.614 |
我们在第三部分已经针对这种情况进行了优化，得到的结果已经比较不错，基本在一周左右，但仍然有不小的方差，我们认为这是RRT的随机性导致的。从这个角度来看，路径对于小车执行的难度要求已经降到了不错的程度。
在我们路径规划的时候，所选择的路径节点就距离障碍物大于几乎两倍的机器人半径，并且在进行路径的简化时也考虑到了简化后不能与障碍物发生碰撞，从上图可以看出，在我们路径规划时，与障碍物始终保持合理的距离。
在下表中，我们通过计算每一次规划中所有节点和障碍物的最短距离，来充分说明我们呢路径规划的避障安全性：
| 规划次数 | 1 | 2 | 3 | 4 | 5 | 十次平均 |
|---|---|---|---|---|---|---|
| 距离障碍物的最短距离（m） | 0.762 | 0.847 | 0.693 | 0.561 | 0.712 | 0.705 |
综上，我们认为路径规划从路径长度上有一些冗长，避障特性比较好，对于机器人的执行能力的要求和执行时间的优化也还算不错。
详见仿真视频.mp4
（需要向老师说明的是，这里由于我们的电脑ubuntu中没有安装显卡驱动，所以运动起来会一卡一卡的，在ROS中的时间和实际的也是不一样的，相当于极慢速播放，但是这并不是我们代码的问题，实际的运动并不是这样的，是连续的，从我下面的分析也可以看出）
我们利用python中的xlrd,xlwt，在local_planner.py中每次DWA在速度空间中搜索到的线速度和角速度，存入excel文件中，连接相邻点，画出线速度和角速度变化曲线，由此来判断速度的连续性。
我们选择了三个路径节点的路径进行计算，共读到了17325个发布的速度和角速度指令，通过将其可视化，得到了这样的结果：
在上图的速度变化中，会发现会清晰的看到三个节点的存在，线速度和角速度在三个节点会达到一个峰值或者谷值，通过上面的速度变化曲线还可以看出，速度变化是比较连续且平滑的（中间出现一些断点是因为我取点是有间隔的），而在速度变化过程中加速度会有比较大的突变，也就是说可能存在刚性的力的冲击，加速度不连续。这是因为DWA规划的时候发布的速度是离散的，不能保证加速度是连续的，我们在程序中只能尽量保证加速度突变的程度不大，对AGV小车的冲击尽可能小。
从仿真视频中可以看出，我们也可以看出，轨迹规划的平滑性是比较不错的，AGV小车在执行的过程中不是严格按照原来的直线路径执行，而是路径自动平滑化了，运动还是比较流畅的，没有出现车突然翻起的情况（突然翻起是因为加速度过高），总体来讲效果还是不错的。
详见附件实车运行（1）.mp4，实车运行（2）.mp4
从视频中可以看出，小车运行速度很不错，基本在60s左右一个来回的水平；在运行过程中，没有明显的卡顿，比较平滑；车轮等执行机构没有发出一些磨损的声音，说明我们发出的指令没有超过执行能力的极限，是比较合理的。
我们在实际调试的过程中，测试了7次时间，分别从左下角到右上角并回到左下角（事实上，这并不是最复杂的路径点选取，考试时的点选取的更加刁钻，所以时间更长）：
| 规划次数 | 1 | 2 | 3 | 4 | 5 | 6 | 7 |
|---|---|---|---|---|---|---|---|
| 往返时间（s） | 54 | 68 | 56 | 70 | 64 | 61 | 53 |
所以无论从视频直观来看，还是从数据来看，运行效果都是不错的，能比较快速且无碰的到达目标点，整个运行过程中轨迹也非常光滑连续。
详见附件动态避障（1）.mp4，动态避障（2）.mp4
在实验中，我们进行了尝试，如果我们组内的一位同学突然出现在正在沿着路径行走的小车的路线上，小车会发现出现的动态障碍物，然后进行绕行动作。我们认为能完成这样的避障是因为我们的DWA写的比较好，而且DWA中的各种经验参数比如说障碍物的score，经过通过不断尝试，设置的比较合理，就比如下面的这些参数就通过不断的调试，读取不同的打分值并进行权重的比较，最终取得了一个不错的结果。
self.alpha = 4 #goal
self.beta = 1 #velocity
self.gamma = 2 #obstacle
self.avoid_size = 0.05当然，这样的避障效果也是有限的，比如说避障速度比较慢，避障动作有冗余的情况，在较大块障碍物突然挡住路径的情况下（比如说几块泡沫板组成的大立方体），动态避障就几乎没有作用了，而这也有可能是由于DWA本身算法的不足造成的。