├── .gitignore ├── README.md ├── RRT ├── RRT.md └── RRT.py ├── RRTStar ├── RRTStar.md └── RRT_Star.py ├── bezier ├── Bezier.md └── bezier_planning.py ├── cubic_spline ├── CubicSpline ├── __pycache__ │ ├── cubic_spline.cpython-35.pyc │ └── cubic_spline.cpython-38.pyc └── cubic_spline.py ├── dubins_path └── dubins_path_planning.py ├── dynamic_astar └── DynamicAstar.py ├── dynamic_window ├── DynamicWindow └── dynamic_window.py ├── eta3spline ├── Eta3Spline.md ├── Eta3SplineTra.md ├── __pycache__ │ └── eta3spline_path_planning.cpython-35.pyc ├── eta3spline_path_planning.py └── eta3spline_trajectory.py ├── frenet_optimal_trajectory └── frenet_optimal_trajectory.py ├── grid_astar ├── GridAstar └── grid_astar.py ├── grid_coverage ├── GridCoverage ├── __pycache__ │ └── grid_map_lib.cpython-35.pyc ├── grid_coverage.py └── grid_map_lib.py ├── grid_dijkstra ├── Grid_Dijkstra └── grid_dijkstra.py ├── hybrid_astar ├── HybridAstar.py └── hybrid_astar.md ├── model_predictive_path_follower └── model_predictive_path_follower.py ├── model_predictive_trajectory_generation ├── ModelPredictiveTrajectoryGeneration ├── __pycache__ │ ├── model.cpython-35.pyc │ └── model_predictive_trajectory.cpython-35.pyc ├── lookup_table.py ├── model.py ├── model_predictive_trajectory.py └── reference ├── potential_field ├── PotentialField └── potential_field.py ├── probabilistic_roadmap ├── ProbabilisticRoadMap └── probabilistic_roadmap.py ├── quintic_polynomial ├── QuinticPolynomial.md └── quintic_polynomial.py ├── smoothed_astar ├── __pycache__ │ ├── grid_astar.cpython-35.pyc │ └── map_loading.cpython-35.pyc ├── data │ ├── BIG_WHITE.png │ ├── SCURVE.png │ ├── SideParking.png │ ├── WHITE.png │ ├── diku_bare.png │ ├── diku_full.png │ ├── diku_lines.png │ ├── diku_point.png │ ├── highway.png │ ├── highway_start.png │ ├── highway_start_0cm.png │ ├── highway_start_10cm.png │ ├── highway_start_20cm.png │ ├── highway_start_30cm.png │ ├── highway_start_40cm.png │ ├── highwayw.png │ ├── k-turn_parking.png │ ├── k_turn_up40_down20.png │ ├── park.png │ ├── scurve_with_cones.png │ ├── sida_with_parking_spot.png │ ├── uturn_4.8_3.5_1.5.png │ ├── uturn_4.8_4._1.5.png │ ├── uturn_5.2_3.5_1.5.png │ ├── uturn_5.2_4._1.5.png │ ├── uturn_5._3.5_1.5.png │ ├── uturn_5._4._1.5.png │ ├── uturn_6.5_3.5_1.5.png │ ├── uturn_6.5_4._1.5.png │ ├── uturn_blocked.png │ └── 起点信息.txt ├── grid_astar.py ├── map_loading.py └── test.py ├── state_lattice ├── Boss2008Darpa ├── StateLattice ├── lookuptable.csv └── state_lattice.py └── voronoi_roadmap ├── VoronoiRoadMap └── voronoi_roadmap.py /.gitignore: -------------------------------------------------------------------------------- 1 | 2 | quintic_polynomial/\.vscode/ 3 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Python Robot Path Planning 2 | ## 参考资料python_robotic重新编写，加入中文注释和代码思路解读 3 | ## 使用环境为python3.6 4 | ## 依赖库为numpy，scipy，matplotlib，pandas和cvxpy 5 | 6 | 20种移动机器人, 无人驾驶基础规划算法代码开源,参考github上的python robotics项目,自己实现后添加中文注释和说明文件. 7 | -------------------------------------------------------------------------------- /RRT/RRT.md: -------------------------------------------------------------------------------- 1 | RRT是一种概率完备的路径搜索算法,与PRM相同,它同样是在地图中进行采样,利用采样点进行搜索.但是,不同的地方是RRT是树状结构,逐步进行采样,而不像PRM直接进行全图采样生成无向图.RRT过程如下:首先确定起点和终点的位置,给定采样随机点的概率,步长和最大迭代次数.从起点开始,在地图中以一定概率随机采样,一定概率采样终点.得到采样点后,计算当前生成树中离采样点最近的子节点.随后,子节点朝着采样点的方向延申步长距离,得到新的子节点.如果延伸路径没有发生碰撞,则将子节点加入到当前的生成树中.以上过程不断循环,直到子节点与终点的距离小于阈值或达到迭代上限. -------------------------------------------------------------------------------- /RRT/RRT.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/python3 2 | #! -*- coding:utf-8 -*- 3 | 4 | """ 5 | 6 | Rapid random tree 7 | author: flztiii 8 | 9 | """ 10 | 11 | import matplotlib.pyplot as plt 12 | import random 13 | import numpy as np 14 | 15 | # 全局变量 16 | PATH_RESOLUTION = 0.1 # 路径采样点距离 17 | THRESHOLD = 0.2 # 离终点距离阈值 18 | 19 | # 节点 20 | class Node: 21 | def __init__(self, x, y): 22 | self.x_ = x # 节点的x坐标 23 | self.y_ = y # 节点的y坐标 24 | self.path_x_ = [] # 从上一点到本节点的路径x坐标 25 | self.path_y_ = [] # 从上一点到本节点的路径y坐标 26 | self.parent_ = None # 上一个节点 27 | 28 | # RRT规划器 29 | class RRTPlanner: 30 | def __init__(self, start, goal, obstacle_list, rand_area, rand_rate, expand_dis, max_iter): 31 | self.start_ = start 32 | self.goal_ = goal 33 | self.obstacle_list_ = obstacle_list 34 | self.rand_area_ = rand_area 35 | self.rand_rate_ = rand_rate 36 | self.expand_dis_ = expand_dis 37 | self.max_iter_ = max_iter 38 | 39 | # 开始进行规划 40 | def planning(self): 41 | # 初始化起始节点 42 | self.start_node_ = Node(self.start_[0], self.start_[1]) 43 | # 初始化终止节点 44 | self.goal_node_ = Node(self.goal_[0],self.goal_[1]) 45 | # 初始化生成树 46 | self.tree_ = [self.start_node_] 47 | # 开始进行循环搜索 48 | i = 0 49 | while i < self.max_iter_: 50 | # 首先找到采样点 51 | random_node = self.getRandomNode() 52 | # 判断生成树中离采样点最近的点 53 | nearest_node_index = self.getNearestNodeIndexFromTree(random_node) 54 | nearest_node = self.tree_[nearest_node_index] 55 | # 从最近节点向采样点方法进行延伸,得到新的节点 56 | new_node = self.expandNewNode(nearest_node, random_node) 57 | # 进行可视化 58 | self.drawGraph(random_node) 59 | # 判断从最近节点到新节点是否发生障碍物碰撞 60 | if not self.isCollision(new_node): 61 | # 没有发生碰撞 62 | # 将节点加入生成树 63 | self.tree_.append(new_node) 64 | # 判断新节点是否离终点距离小于阈值 65 | if self.calcNodesDistance(new_node, self.goal_node_) < THRESHOLD: 66 | # 到达终点 67 | # 计算最终路径 68 | return self.getFinalPath(new_node) 69 | i += 1 70 | 71 | # 找到采样点 72 | def getRandomNode(self): 73 | rand = random.random() 74 | if rand > self.rand_rate_: 75 | # 取终点 76 | return Node(self.goal_[0],self.goal_[1]) 77 | else: 78 | # 取随机点 79 | return Node(random.uniform(self.rand_area_[0], self.rand_area_[1]), random.uniform(self.rand_area_[0], self.rand_area_[1])) 80 | 81 | # 获得新的节点 82 | def expandNewNode(self, init_node, end_node): 83 | # 计算初始节点到结束节点的距离和朝向 84 | distance = self.calcNodesDistance(init_node, end_node) 85 | theta = np.arctan2(end_node.y_ - init_node.y_, end_node.x_ - init_node.x_) 86 | # 计算从初始节点到结束节点的路径 87 | distance = min(distance, self.expand_dis_) 88 | x, y = init_node.x_, init_node.y_ 89 | path_x, path_y = [], [] 90 | for sample in np.arange(0.0, distance + PATH_RESOLUTION, PATH_RESOLUTION): 91 | x = sample * np.cos(theta) + init_node.x_ 92 | y = sample * np.sin(theta) + init_node.y_ 93 | path_x.append(x) 94 | path_y.append(y) 95 | # 构造新的节点 96 | new_node = Node(x, y) 97 | new_node.path_x_= path_x[:-1] 98 | new_node.path_y_ = path_y[:-1] 99 | new_node.parent_ = init_node 100 | return new_node 101 | 102 | # 判断节点是否发生碰撞 103 | def isCollision(self, node): 104 | # 计算节点路径上每一个点到障碍物距离是否小于阈值 105 | for ix, iy in zip(node.path_x_, node.path_y_): 106 | for obs in self.obstacle_list_: 107 | distance = np.sqrt((ix - obs[0]) ** 2 + (iy - obs[1]) ** 2) 108 | if distance <= obs[2]: 109 | return True 110 | return False 111 | 112 | # 得到最终路径 113 | def getFinalPath(self, final_node): 114 | path_x, path_y = [], [] 115 | node = final_node 116 | while node.parent_ is not None: 117 | path_x = node.path_x_ + path_x 118 | path_y = node.path_y_ + path_y 119 | node = node.parent_ 120 | return path_x, path_y 121 | 122 | # 计算生成树中离给出节点最近的节点下标 123 | def getNearestNodeIndexFromTree(self, node): 124 | distances = [self.calcNodesDistance(node, tree_node) for tree_node in self.tree_] 125 | min_index = distances.index(min(distances)) 126 | return min_index 127 | 128 | # 计算两点之间的距离 129 | def calcNodesDistance(self, node_1, node_2): 130 | return np.sqrt((node_1.x_ - node_2.x_) ** 2 + (node_1.y_ - node_2.y_) ** 2) 131 | 132 | # 可视化 133 | def drawGraph(self, rnd = None): 134 | # 清空之前可视化 135 | plt.clf() 136 | if rnd is not None: 137 | plt.plot(rnd.x_, rnd.y_, "^k") 138 | for node in self.tree_: 139 | if node.parent_: 140 | plt.plot(node.path_x_, node.path_y_, "-g") 141 | 142 | for (ox, oy, size) in self.obstacle_list_: 143 | plt.plot(ox, oy, "ok", ms=30 * size) 144 | 145 | plt.plot(self.start_node_.x_, self.start_node_.y_, "xr") 146 | plt.plot(self.goal_node_.x_, self.goal_node_.y_, "xr") 147 | plt.axis([self.rand_area_[0], self.rand_area_[1], self.rand_area_[0], self.rand_area_[1]]) 148 | plt.grid(True) 149 | plt.pause(0.01) 150 | 151 | # 主函数 152 | def main(): 153 | # 初始化起点,终点信息 154 | start = [0.0, 0.0] 155 | goal = [5.0, 10.0] 156 | # 初始化障碍物信息 157 | obstacle_list = [ 158 | (5, 5, 1), 159 | (3, 6, 2), 160 | (3, 8, 2), 161 | (3, 10, 2), 162 | (7, 5, 2), 163 | (9, 5, 2) 164 | ] # [x,y,size] 165 | # 初始化采样 166 | rand_area=[-2.0, 15.0] 167 | # 初始化步长 168 | expand_dis = 1.0 169 | # 初始化最大迭代次数 170 | max_iter = 1000 171 | # 初始化随机点采样概率 172 | rand_rate = 0.5 173 | 174 | # 开始规划 175 | rrt_planner = RRTPlanner(start, goal, obstacle_list, rand_area, rand_rate, expand_dis, max_iter) 176 | path_x, path_y = rrt_planner.planning() 177 | 178 | # 进行可视化 179 | rrt_planner.drawGraph() 180 | plt.plot(path_x, path_y) 181 | plt.grid(True) 182 | plt.show() 183 | 184 | 185 | if __name__ == "__main__": 186 | main() -------------------------------------------------------------------------------- /RRTStar/RRTStar.md: -------------------------------------------------------------------------------- 1 | RRT星与RRT算法并没有本质上的不同,是对RRT算法的一种优化.优化的过程主要包括以下部分:在得到采样节点后,得到离它最近的生成树节点,生成新节点.随后,还以新节点为中心,一定距离为半径,寻找半径内的全部邻居节点.新节点会尝试将每一个邻居节点作为父节点,判断是否发生碰撞,如果不发生碰撞,则选则其中代价最小的(代价以欧式距离进行衡量)作为父节点.随后,对于每一个邻居节点,尝试将其的父节点改为新节点,判断是否可以将代价降低,如果可以,则将父节点改为新节点,并传播更新它的全部子节点的代价.通过上述过程,完成对于RRT搜索路径的优化. -------------------------------------------------------------------------------- /RRTStar/RRT_Star.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/python3 2 | #! -*- coding: utf-8 -*- 3 | 4 | """ 5 | 6 | Rapid random tree star 7 | author: flztiii 8 | 9 | """ 10 | 11 | import matplotlib.pyplot as plt 12 | import random 13 | import numpy as np 14 | 15 | # 全局变量 16 | PATH_RESOLUTION = 0.1 # 路径采样点距离 17 | THRESHOLD = 0.2 # 离终点距离阈值 18 | 19 | # 节点 20 | class Node: 21 | def __init__(self, x, y): 22 | self.x_ = x # 节点的x坐标 23 | self.y_ = y # 节点的y坐标 24 | self.path_x_ = [] # 从上一点到本节点的路径x坐标 25 | self.path_y_ = [] # 从上一点到本节点的路径y坐标 26 | self.parent_ = None # 上一个节点 27 | self.cost_ = 0.0 # 代价 28 | 29 | # RRT规划器 30 | class RRTStarPlanner: 31 | def __init__(self, start, goal, obstacle_list, rand_area, rand_rate, expand_dis, connect_circle_dist, max_iter): 32 | self.start_ = start 33 | self.goal_ = goal 34 | self.obstacle_list_ = obstacle_list 35 | self.rand_area_ = rand_area 36 | self.rand_rate_ = rand_rate 37 | self.expand_dis_ = expand_dis 38 | self.max_iter_ = max_iter 39 | self.connect_circle_dist_ = connect_circle_dist 40 | 41 | # 开始进行规划 42 | def planning(self): 43 | # 初始化起始节点 44 | self.start_node_ = Node(self.start_[0], self.start_[1]) 45 | # 初始化终止节点 46 | self.goal_node_ = Node(self.goal_[0],self.goal_[1]) 47 | # 初始化生成树 48 | self.tree_ = [self.start_node_] 49 | # 开始进行循环搜索 50 | i = 0 51 | while i < self.max_iter_: 52 | print("Iter:", i, ", number of nodes:", len(self.tree_)) 53 | # 首先找到采样点 54 | random_node = self.getRandomNode() 55 | # 判断生成树中离采样点最近的点 56 | nearest_node_index = self.getNearestNodeIndexFromTree(random_node) 57 | nearest_node = self.tree_[nearest_node_index] 58 | # 从最近节点向采样点方法进行延伸,得到新的节点 59 | new_node = self.expandNewNode(nearest_node, random_node, self.expand_dis_) 60 | # 找到新节点的在生成树中的邻居节点 61 | neighbor_node_indexes = self.findNeighborNodeIndexes(new_node) 62 | # 为新节点更新父节点 63 | new_node = self.updateParentForNewNode(new_node, neighbor_node_indexes) 64 | # 进行可视化 65 | if i % 10 == 0: 66 | self.drawGraph(random_node) 67 | # 判断新节点是否有效 68 | if new_node is not None: 69 | # 将新节点加入生成树 70 | self.tree_.append(new_node) 71 | # 新节点有效,更新邻居节点的连接 72 | self.updateWire(new_node, neighbor_node_indexes) 73 | i += 1 74 | # 遍历完成,开始得到最终路径 75 | last_index = self.findFinalNode() 76 | if last_index is None: 77 | print('no path find') 78 | exit(0) 79 | else: 80 | return self.getFinalPath(self.tree_[last_index]) 81 | 82 | # 找到采样点 83 | def getRandomNode(self): 84 | rand = random.random() 85 | if rand > self.rand_rate_: 86 | # 取终点 87 | return Node(self.goal_[0],self.goal_[1]) 88 | else: 89 | # 取随机点 90 | return Node(random.uniform(self.rand_area_[0], self.rand_area_[1]), random.uniform(self.rand_area_[0], self.rand_area_[1])) 91 | 92 | # 获得新的节点 93 | def expandNewNode(self, init_node, end_node, max_distance=float('inf')): 94 | # 计算初始节点到结束节点的距离和朝向 95 | distance = self.calcNodesDistance(init_node, end_node) 96 | theta = np.arctan2(end_node.y_ - init_node.y_, end_node.x_ - init_node.x_) 97 | # 计算从初始节点到结束节点的路径 98 | distance = min(distance, max_distance) 99 | x, y = init_node.x_, init_node.y_ 100 | path_x, path_y = [], [] 101 | for sample in np.arange(0.0, distance + PATH_RESOLUTION, PATH_RESOLUTION): 102 | x = sample * np.cos(theta) + init_node.x_ 103 | y = sample * np.sin(theta) + init_node.y_ 104 | path_x.append(x) 105 | path_y.append(y) 106 | # 构造新的节点 107 | new_node = Node(x, y) 108 | new_node.path_x_= path_x[:-1] 109 | new_node.path_y_ = path_y[:-1] 110 | new_node.parent_ = init_node 111 | new_node.cost_ = self.calcCost(init_node, new_node) 112 | return new_node 113 | 114 | # 判断节点是否发生碰撞 115 | def isCollision(self, node): 116 | # 计算节点路径上每一个点到障碍物距离是否小于阈值 117 | for ix, iy in zip(node.path_x_, node.path_y_): 118 | for obs in self.obstacle_list_: 119 | distance = np.sqrt((ix - obs[0]) ** 2 + (iy - obs[1]) ** 2) 120 | if distance <= obs[2]: 121 | return True 122 | return False 123 | 124 | # 得到最终路径 125 | def getFinalPath(self, final_node): 126 | path_x, path_y = [], [] 127 | node = final_node 128 | while node.parent_ is not None: 129 | path_x = node.path_x_ + path_x 130 | path_y = node.path_y_ + path_y 131 | node = node.parent_ 132 | return path_x, path_y 133 | 134 | # 计算生成树中离给出节点最近的节点下标 135 | def getNearestNodeIndexFromTree(self, node): 136 | distances = [self.calcNodesDistance(node, tree_node) for tree_node in self.tree_] 137 | min_index = distances.index(min(distances)) 138 | return min_index 139 | 140 | # 计算两点之间的距离 141 | def calcNodesDistance(self, node_1, node_2): 142 | return np.sqrt((node_1.x_ - node_2.x_) ** 2 + (node_1.y_ - node_2.y_) ** 2) 143 | 144 | # 计算节点的代价 145 | def calcCost(self, init_node, end_node): 146 | return init_node.cost_ + self.calcNodesDistance(init_node, end_node) 147 | 148 | # 寻找邻居节点 149 | def findNeighborNodeIndexes(self, node): 150 | # 得到搜索半径 151 | radius = self.connect_circle_dist_ * np.sqrt(np.log(len(self.tree_) + 1) / (len(self.tree_) + 1)) 152 | indexes = [] 153 | for i, tree_node in enumerate(self.tree_): 154 | distance = self.calcNodesDistance(node, tree_node) 155 | if distance <= radius: 156 | indexes.append(i) 157 | return indexes 158 | 159 | # 为新节点更新父节点 160 | def updateParentForNewNode(self, node, neighbor_node_indexes): 161 | # 遍历邻居节点 162 | valid_temp_node = [] 163 | for neighbor_node_index in neighbor_node_indexes: 164 | neighbor_node = self.tree_[neighbor_node_index] 165 | # 构建临时节 166 | temp_node = self.expandNewNode(neighbor_node, node) 167 | # 判断临时节点是否发生碰撞 168 | if not self.isCollision(temp_node): 169 | # 如果没有发生碰撞,加入列表中 170 | valid_temp_node.append(temp_node) 171 | if len(valid_temp_node) == 0: 172 | # 没有有效节点 173 | return None 174 | else: 175 | # 返回代价最小的 176 | min_cost_node = min(valid_temp_node, key = lambda temp_node: temp_node.cost_) 177 | return min_cost_node 178 | 179 | # 更新新的连接关系 180 | def updateWire(self, node, neighbor_node_indexes): 181 | # 遍历邻居节点 182 | for neighbor_node_index in neighbor_node_indexes: 183 | neighbor_node = self.tree_[neighbor_node_index] 184 | # 构建临时节点 185 | temp_node = self.expandNewNode(node, neighbor_node) 186 | # 判断是否发生碰撞 187 | if self.isCollision(temp_node): 188 | # 如果发生碰撞,跳过 189 | continue 190 | # 如果不发生碰撞,判断代价 191 | if temp_node.cost_ < neighbor_node.cost_: 192 | # 如果新的节点代价更低,更新之前的邻居节点为新的 193 | self.tree_[neighbor_node_index] = temp_node 194 | # 更新该邻居的全部子节点 195 | self.propegateCost(neighbor_node) 196 | 197 | # 向子节点传播损失 198 | def propegateCost(self, node): 199 | # 遍历全部的生成树节点 200 | for i, tree_node in enumerate(self.tree_): 201 | if tree_node.parent_ == node: 202 | # 找到了子节点,进行损失更新 203 | self.tree_[i].cost_ = self.calcCost(node, self.tree_[i]) 204 | self.propegateCost(self.tree_[i]) 205 | 206 | # 寻找离终点最近的节点 207 | def findFinalNode(self): 208 | final_indexes = [] 209 | for i, tree_node in enumerate(self.tree_): 210 | distance = self.calcNodesDistance(tree_node, self.goal_node_) 211 | if distance < THRESHOLD: 212 | final_indexes.append(i) 213 | # 判断是否找到终点 214 | if len(final_indexes) == 0: 215 | # 没有找到终点 216 | return None 217 | final_index = min(final_indexes, key = lambda index: self.tree_[index].cost_) 218 | return final_index 219 | 220 | # 可视化 221 | def drawGraph(self, rnd = None): 222 | # 清空之前可视化 223 | plt.clf() 224 | if rnd is not None: 225 | plt.plot(rnd.x_, rnd.y_, "^k") 226 | for node in self.tree_: 227 | if node.parent_: 228 | plt.plot(node.path_x_, node.path_y_, "-g") 229 | 230 | for (ox, oy, size) in self.obstacle_list_: 231 | plt.plot(ox, oy, "ok", ms=30 * size) 232 | 233 | plt.plot(self.start_node_.x_, self.start_node_.y_, "xr") 234 | plt.plot(self.goal_node_.x_, self.goal_node_.y_, "xr") 235 | plt.axis([self.rand_area_[0], self.rand_area_[1], self.rand_area_[0], self.rand_area_[1]]) 236 | plt.grid(True) 237 | plt.pause(0.01) 238 | 239 | # 主函数 240 | def main(): 241 | # 初始化起点,终点信息 242 | start = [0.0, 0.0] 243 | goal = [5.0, 10.0] 244 | # 初始化障碍物信息 245 | obstacle_list = [ 246 | (5, 5, 1), 247 | (3, 6, 2), 248 | (3, 8, 2), 249 | (3, 10, 2), 250 | (7, 5, 2), 251 | (9, 5, 2) 252 | ] # [x,y,size] 253 | # 初始化采样 254 | rand_area=[-2.0, 15.0] 255 | # 初始化步长 256 | expand_dis = 1.0 257 | # 初始化最大迭代次数 258 | max_iter = 500 259 | # 初始化随机点采样概率 260 | rand_rate = 0.5 261 | # 初始邻居半径 262 | connect_circle_dist = 50.0 263 | 264 | # 开始规划 265 | rrt_star_planner = RRTStarPlanner(start, goal, obstacle_list, rand_area, rand_rate, expand_dis, connect_circle_dist, max_iter) 266 | path_x, path_y = rrt_star_planner.planning() 267 | 268 | # 进行可视化 269 | rrt_star_planner.drawGraph() 270 | plt.plot(path_x, path_y, 'r') 271 | plt.grid(True) 272 | plt.show() 273 | 274 | if __name__ == "__main__": 275 | main() -------------------------------------------------------------------------------- /bezier/Bezier.md: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flztiii/path_planning_basic_algorithm/d1827cbb355416da8a14831eb37a929c5c15e644/bezier/Bezier.md -------------------------------------------------------------------------------- /bezier/bezier_planning.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/python3 2 | # -*- coding: utf-8 -*- 3 | 4 | """ 5 | 6 | bezier planning (三阶贝塞尔曲线规划) 7 | author: flztiii 8 | 9 | """ 10 | 11 | import numpy as np 12 | import math 13 | import matplotlib.pyplot as plt 14 | import scipy.special 15 | 16 | # 贝赛尔曲线类(三阶以上) 17 | class Bezier: 18 | def __init__(self, control_points): 19 | self.control_points_ = control_points # 控制点 20 | self.n_ = len(control_points) - 1 # 贝赛尔曲线的阶数 21 | if self.n_ > 2: 22 | self.__calcd() # 计算贝赛尔曲线一阶导数的控制点 23 | self.__calcdd() # 计算贝赛尔曲线二阶导数的控制点 24 | 25 | # 获取贝赛尔曲线特定位置 26 | def calcPosition(self, t): 27 | position = np.zeros(2) 28 | for i in range(0, self.n_ + 1): 29 | position += self.__calcBezierCoefficient(self.n_, i, t) * self.control_points_[i] 30 | return position 31 | 32 | # 获取贝赛尔曲线特定导数 33 | def calcd(self, t): 34 | d = np.zeros(2) 35 | for i in range(0, self.n_): 36 | d += self.__calcBezierCoefficient(self.n_ - 1, i, t) * self.control_points_d_[i] 37 | return d 38 | 39 | # 获取贝赛尔曲线特定二阶导数 40 | def calcdd(self, t): 41 | dd = np.zeros(2) 42 | for i in range(0, self.n_ - 1): 43 | dd += self.__calcBezierCoefficient(self.n_ - 2, i, t) * self.control_points_dd_[i] 44 | return dd 45 | 46 | # 计算对应的bezier系数 47 | def __calcBezierCoefficient(self, n, i, t): 48 | return scipy.special.comb(n, i) * (1 - t) ** (n - i) * t ** i 49 | 50 | # 计算贝赛尔曲线一阶导数的控制点 51 | def __calcd(self): 52 | self.control_points_d_ = [] 53 | for i in range(0, self.n_): 54 | self.control_points_d_.append(self.n_ * (self.control_points_[i + 1] - self.control_points_[i])) 55 | self.control_points_d_ = np.array(self.control_points_d_) 56 | 57 | # 计算贝赛尔曲线二阶导数控制点 58 | def __calcdd(self): 59 | self.control_points_dd_ = [] 60 | for i in range(0, self.n_ - 1): 61 | self.control_points_dd_.append((self.n_ - 1) * (self.control_points_d_[i + 1] - self.control_points_d_[i])) 62 | self.control_points_dd_ = np.array(self.control_points_dd_) 63 | 64 | # 生成三阶贝赛尔曲线的四个控制点 65 | def generateControlPoints(start_x, start_y, start_yaw, end_x, end_y, end_yaw, offset): 66 | # 起点到终点的距离 67 | distance = np.sqrt((end_x - start_x) ** 2 + (end_y - start_y) ** 2) 68 | # 起点 69 | control_points = [[start_x, start_y]] 70 | # 起点切线控制点 71 | control_points.append([start_x + np.cos(start_yaw) * distance / offset, start_y + np.sin(start_yaw) * distance / offset]) 72 | # 终点切线控制点 73 | control_points.append([end_x - np.cos(end_yaw) * distance / offset, end_y - np.sin(end_yaw) * distance / offset]) 74 | # 终点 75 | control_points.append([end_x, end_y]) 76 | return np.array(control_points) 77 | 78 | # 生成三阶贝赛尔曲线 79 | def generateBezier(start_x, start_y, start_yaw, end_x, end_y, end_yaw, offset): 80 | # 第一步根据输入信息生成三阶贝赛尔曲线控制点，因此控制点的数量为四个 81 | control_points = generateControlPoints(start_x, start_y, start_yaw, end_x, end_y, end_yaw, offset) 82 | # 构造贝赛尔曲线 83 | bezier_curve = Bezier(control_points) 84 | 85 | return bezier_curve, control_points 86 | 87 | # 计算曲率 88 | def calcKappa(dx, dy, ddx, ddy): 89 | return (dx * ddy - dy * ddx) / math.pow(dx ** 2 + dy ** 2, 1.5) 90 | 91 | # 可视化朝向 92 | def plotArrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"): # pragma: no cover 93 | """Plot arrow.""" 94 | if not isinstance(x, float): 95 | for (ix, iy, iyaw) in zip(x, y, yaw): 96 | plot_arrow(ix, iy, iyaw) 97 | else: 98 | plt.arrow(x, y, length * np.cos(yaw), length * np.sin(yaw), 99 | fc=fc, ec=ec, head_width=width, head_length=width) 100 | plt.plot(x, y) 101 | 102 | # 主函数 103 | def main(): 104 | # 初始化起点 105 | start_x = 10.0 # [m] 106 | start_y = 1.0 # [m] 107 | start_yaw = np.radians(180.0) # [rad] 108 | 109 | end_x = -0.0 # [m] 110 | end_y = -3.0 # [m] 111 | end_yaw = np.radians(-45.0) # [rad] 112 | offset = 3.0 # 控制点的等分数 113 | bezier_curve, control_points = generateBezier(start_x, start_y, start_yaw, end_x, end_y, end_yaw, offset) 114 | 115 | # 得到贝赛尔曲线的采样 116 | sample_number = 100 117 | samples = np.linspace(0, 1, sample_number) 118 | curve_points = [] 119 | for sample in samples: 120 | curve_points.append(bezier_curve.calcPosition(sample)) 121 | curve_points = np.array(curve_points) 122 | # 贝赛尔曲线特定点信息 123 | special_sample = 0.86 124 | # 计算此点的位置 125 | point = bezier_curve.calcPosition(special_sample) 126 | # 计算此点的朝向 127 | point_d = bezier_curve.calcd(special_sample) 128 | point_d_norm = point_d / np.linalg.norm(point_d, 2) 129 | # 计算此点的切向单位向量 130 | targent = np.array([point, point + point_d_norm]) 131 | # 计算此点的法向单位向量 132 | norm = np.array([point, point + np.array([- point_d_norm[1], point_d_norm[0]])]) 133 | # 计算此点的曲率半径 134 | point_dd = bezier_curve.calcdd(special_sample) 135 | point_kappa = calcKappa(point_d[0], point_d[1], point_dd[0], point_dd[1]) 136 | radius = 1.0 / point_kappa 137 | # 计算圆心位置 138 | circular_center = point + radius * np.array([- point_d_norm[1], point_d_norm[0]]) 139 | circle = plt.Circle(tuple(circular_center), radius, color=(0, 0.8, 0.8), fill=False, linewidth=1) 140 | 141 | # 可视化 142 | fig, ax = plt.subplots() 143 | # 可视化控制点 144 | ax.plot(control_points.T[0], control_points.T[1], '--o', label = 'control points') 145 | # 可视化贝赛尔曲线 146 | ax.plot(curve_points.T[0], curve_points.T[1], label = 'bezier curve') 147 | # 可视化曲率圆 148 | ax.add_artist(circle) 149 | # 可视化特定点的切向单位向量 150 | ax.plot(targent.T[0], targent.T[1], label = 'targent') 151 | # 可视化特定点的法向单位向量 152 | ax.plot(norm.T[0], norm.T[1], label = 'norm') 153 | # 可视化起点和终点朝向 154 | plotArrow(start_x, start_y, start_yaw) 155 | plotArrow(end_x, end_y, end_yaw) 156 | ax.legend() 157 | ax.axis("equal") 158 | ax.grid(True) 159 | plt.show() 160 | 161 | if __name__ == "__main__": 162 | main() -------------------------------------------------------------------------------- /cubic_spline/CubicSpline: -------------------------------------------------------------------------------- 1 | cubic spline是通过三次样条差值的方法来进行路径生成，是一种路径生成方法而不是搜索算法。插值与拟合都是根据一系列散点来得到函数，但是拟合只需要函数在趋势上与散点相同即可，而插值需要函数经过所用散点。因此插值容易出现剧烈抖动现象。为了避免抖动，插值通常是通过分段进行的，即每两个散点之间进行一次差值，为了保证分段之间平滑过度，因此需要高次函数进行插值，这也就是三次样条差值的由来。 2 | 假设散点为[(x0, y0), (x1, y1),...,(xn, yn)]共有n+1个，则可以得到n段三次样条函数。其中第i段三次样条表达式为Si(x) = ai + bi * (x - xi) + ci * (x - xi) ^ 2 + di * (x - xi) ^ 3。由于有n个方程，所以存在4n个未知数，需要4n个方程进行求解。4n个方程通过以下方式得到： 3 | 1）由于方程Si必须通过散点，因此必定存在Si(xi) = yi，其中i = 0,1,2,...,n-1。由于还存在Sn-1(xn)=yn，所以此处可以得到n+1个方程。 4 | 2）由于方程之间端点必须连续，因此必定存在Si(xi+1) = Si+1(xi+1)，其中i = 0,1,2,...,n-2。此处可以得到n-1个方程。 5 | 3）由于方程之间端点必须平滑，因此必定纯在端点左右分段函数的一阶导数相同，即dSi/dx=dSi+1/dx，其中i = 0,1,2,...,n-2。此处可以得到n-1个方程。 6 | 4）由于方程之间端点必须平滑，因此必定纯在端点左右分段函数的二阶导数相同，即d2Si/dx2 = d2Si+1/dx2，其中i = 0,1,2,...,n-2。此处可以得到n-1个方程。 7 | 综上所述可以得到4n-2个方程，另外两个方程可以通过边界条件得到，即函数的左右边界二阶导数为0，这种情况称之为自由边界。通过这4n个方程就可以求解出4n个未知量，最终得到方程组表达式。 8 | 在这里，代码并不是直接利用x，y进行插值，而是计算出散点之间的直线路程s，分别做s，x的插值与s，y的差值。 -------------------------------------------------------------------------------- /cubic_spline/__pycache__/cubic_spline.cpython-35.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flztiii/path_planning_basic_algorithm/d1827cbb355416da8a14831eb37a929c5c15e644/cubic_spline/__pycache__/cubic_spline.cpython-35.pyc -------------------------------------------------------------------------------- /cubic_spline/__pycache__/cubic_spline.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flztiii/path_planning_basic_algorithm/d1827cbb355416da8a14831eb37a929c5c15e644/cubic_spline/__pycache__/cubic_spline.cpython-38.pyc -------------------------------------------------------------------------------- /cubic_spline/cubic_spline.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/python3 2 | # -*- coding:utf-8 -*- 3 | 4 | """ 5 | 6 | cubic spline planning 7 | author: flztiii 8 | 9 | """ 10 | 11 | import numpy as np 12 | import math 13 | import matplotlib.pyplot as plt 14 | import bisect 15 | 16 | # 构建三次样条曲线 17 | class Spline: 18 | def __init__(self, x, y): 19 | assert len(x) == len(y) 20 | # 输入散点 21 | self.x_ = x 22 | self.y_ = y 23 | # 散点数量 24 | self.number_ = len(x) 25 | print("len", self.number_) 26 | # 最大下标(也是方程式的个数，参数个数) 27 | self.n_ = len(x) - 1 28 | # 中间参数 29 | self.h_ = np.diff(x) 30 | # 未知参数 31 | self.a_, self.b_, self.c_, self.d_ = [], [], [], [] 32 | # 开始计算参数a 33 | self.a_ = y 34 | A = self.__getA() 35 | B = self.__getB() 36 | # 求解中间参量m 37 | m = np.linalg.solve(A, B) 38 | # 计算参数c 39 | self.c_ = m * 0.5 40 | # 计算参数d和b 41 | self.d_ = np.zeros(self.n_) 42 | self.b_ = np.zeros(self.n_) 43 | for i in range(0, len(self.d_)): 44 | self.d_[i] = (m[i + 1] - m[i]) / (6.0 * self.h_[i]) 45 | self.b_[i] = (self.y_[i + 1] - self.y_[i]) / self.h_[i] - 0.5 * self.h_[i] * m[i] - self.h_[i] * (m[i + 1] - m[i]) / 6.0 46 | 47 | # 获得矩阵A 48 | def __getA(self): 49 | # 初始化矩阵A 50 | A = np.zeros((self.number_, self.number_)) 51 | A[0, 0] = 1.0 52 | for i in range(1, self.n_): 53 | A[i, i] = 2 * (self.h_[i - 1] + self.h_[i]) 54 | A[i - 1, i] = self.h_[i - 1] 55 | A[i, i - 1] = self.h_[i - 1] 56 | A[0, 1] = 0.0 57 | A[self.n_ - 1, self.n_] = self.h_[self.n_ - 1] 58 | A[self.n_, self.n_] = 1.0 59 | return A 60 | 61 | # 获得向量B，自由边界条件 62 | def __getB(self): 63 | B = np.zeros(self.number_) 64 | for i in range(1, self.n_): 65 | B[i] = 6.0 * ((self.y_[i + 1] - self.y_[i]) / self.h_[i] - (self.y_[i] - self.y_[i - 1]) / self.h_[i - 1]) 66 | return B 67 | 68 | # 计算对应函数段 69 | def __getSegment(self, sample): 70 | return bisect.bisect(self.x_, sample) - 1 71 | 72 | # 计算位置 73 | def calc(self, sample): 74 | if sample < self.x_[0] or sample > self.x_[-1]: 75 | return None 76 | # 首先得到对应函数段 77 | index = self.__getSegment(sample) 78 | dx = sample - self.x_[index] 79 | return self.a_[index] + self.b_[index] * dx + self.c_[index] * dx ** 2 + self.d_[index] * dx ** 3 80 | 81 | # 计算一阶导数 82 | def calcd(self, sample): 83 | if sample < self.x_[0] or sample > self.x_[-1]: 84 | return None 85 | # 首先得到对应函数段 86 | index = self.__getSegment(sample) 87 | dx = sample - self.x_[index] 88 | return self.b_[index] + 2.0 * self.c_[index] * dx + 3.0 * self.d_[index] * dx ** 2 89 | 90 | # 计算二阶导数 91 | def calcdd(self, sample): 92 | if sample < self.x_[0] or sample > self.x_[-1]: 93 | return None 94 | # 首先得到对应函数段 95 | index = self.__getSegment(sample) 96 | dx = sample - self.x_[index] 97 | return 2.0 * self.c_[index] + 6.0 * self.d_[index] * dx 98 | 99 | # 构建2d三次样条曲线 100 | class CubicSpline2D: 101 | def __init__(self, x, y): 102 | assert len(x) == len(y) 103 | self.x_ = x 104 | self.y_ = y 105 | # 计算路程 106 | dx = np.diff(x) 107 | dy = np.diff(y) 108 | ds = [math.sqrt(idx ** 2 + idy ** 2) for (idx, idy) in zip(dx, dy)] 109 | self.s_ = [0.0] 110 | self.s_.extend(np.cumsum(ds)) 111 | # 开始构建曲线 112 | self.__generateSpline() 113 | 114 | # 构建曲线 115 | def __generateSpline(self): 116 | self.spline_x_ = Spline(self.s_, self.x_) 117 | self.spline_y_ = Spline(self.s_, self.y_) 118 | 119 | # 计算位置 120 | def calcPosition(self, samples): 121 | x, y = [], [] 122 | for sample in samples: 123 | if not self.spline_x_.calc(sample) is None: 124 | x.append(self.spline_x_.calc(sample)) 125 | y.append(self.spline_y_.calc(sample)) 126 | return x, y 127 | 128 | # 计算朝向 129 | def calcYaw(self, samples): 130 | yaws = [] 131 | for sample in samples: 132 | dx = self.spline_x_.calcd(sample) 133 | dy = self.spline_y_.calcd(sample) 134 | if not dx is None: 135 | yaws.append(math.atan2(dy, dx)) 136 | return yaws 137 | 138 | # 计算曲率 139 | def calcKappa(self, samples): 140 | kappas = [] 141 | for sample in samples: 142 | dx = self.spline_x_.calcd(sample) 143 | dy = self.spline_y_.calcd(sample) 144 | ddx = self.spline_x_.calcdd(sample) 145 | ddy = self.spline_y_.calcdd(sample) 146 | if not dx is None: 147 | kappa = (ddy * dx - ddx * dy) / ((dx ** 2 + dy ** 2)**(3 / 2)) 148 | kappas.append(kappa) 149 | return kappas 150 | 151 | # 主函数 152 | def main(): 153 | # 初始化散点 154 | x = [-2.5, 0.0, 2.5, 5.0, 7.5, 3.0, -1.0] 155 | y = [0.7, -6, 5, 6.5, 0.0, 5.0, -2.0] 156 | # 初始化采样间隔 157 | gap = 0.1 158 | 159 | # 构建2d三次样条曲线 160 | cubic_spline = CubicSpline2D(x, y) 161 | # 对2d三次样条曲线进行采样 162 | sample_s = np.arange(0.0, cubic_spline.s_[-1], gap) 163 | point_x, point_y = cubic_spline.calcPosition(sample_s) 164 | point_yaw = cubic_spline.calcYaw(sample_s) 165 | point_kappa = cubic_spline.calcKappa(sample_s) 166 | # 进行可视化 167 | plt.subplots(1) 168 | plt.plot(x, y, "xb", label="input") 169 | plt.plot(point_x, point_y, "-r", label="spline") 170 | plt.grid(True) 171 | plt.axis("equal") 172 | plt.xlabel("x[m]") 173 | plt.ylabel("y[m]") 174 | plt.legend() 175 | 176 | plt.subplots(1) 177 | plt.plot(sample_s, [np.rad2deg(iyaw) for iyaw in point_yaw], "-r", label="yaw") 178 | plt.grid(True) 179 | plt.legend() 180 | plt.xlabel("line length[m]") 181 | plt.ylabel("yaw angle[deg]") 182 | 183 | plt.subplots(1) 184 | plt.plot(sample_s, point_kappa, "-r", label="curvature") 185 | plt.grid(True) 186 | plt.legend() 187 | plt.xlabel("line length[m]") 188 | plt.ylabel("curvature [1/m]") 189 | 190 | plt.show() 191 | 192 | # 测试函数 193 | def test(): 194 | x = [1., 2., 3., 4., 5., 6., 7.] 195 | y = [0.7, -6, 5, 6.5, 0.0, 5.0, -2.0] 196 | spline = Spline(x, y) 197 | value = spline.calc(2.9) 198 | derivative = spline.calcd(2.9) 199 | print(value, derivative) 200 | 201 | if __name__ == "__main__": 202 | main() -------------------------------------------------------------------------------- /dubins_path/dubins_path_planning.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/python3 2 | # -*- coding: utf-8 -*- 3 | 4 | import numpy as np 5 | import matplotlib.pyplot as plt 6 | 7 | # 计算角度与2pi求余,计算出的角度在[-2pi,2pi]区间内 8 | def mod2Pi(theta): 9 | return theta - 2.0 * np.pi * np.floor(theta / 2.0 / np.pi) 10 | 11 | # 将[-2pi,2pi]转换到[-pi,pi] 12 | def pi2Pi(angle): 13 | return (angle + np.pi) % (2 * np.pi) - np.pi 14 | 15 | # 坐标转换，将原始坐标转换到coordinate坐标下 16 | def transformCoordinate(coordinate, origin): 17 | new_yaw = origin[2] - coordinate[2] 18 | new_x = (origin[0] - coordinate[0]) * np.cos(coordinate[2]) + (origin[1] - coordinate[1]) * np.sin(coordinate[2]) 19 | new_y = - (origin[0] - coordinate[0]) * np.sin(coordinate[2]) + (origin[1] - coordinate[1]) * np.cos(coordinate[2]) 20 | return [new_x, new_y, new_yaw] 21 | 22 | # 坐标恢复,将coordinate坐标转换到原始坐标 23 | def retransfromCoordinate(coordinate, transformed): 24 | origin_yaw = pi2Pi(transformed[2] + coordinate[2]) 25 | origin_x = coordinate[0] + transformed[0] * np.cos(coordinate[2]) - transformed[1] * np.sin(coordinate[2]) 26 | origin_y = coordinate[1] + transformed[0] * np.sin(coordinate[2]) + transformed[1] * np.cos(coordinate[2]) 27 | return [origin_x, origin_y, origin_yaw] 28 | 29 | # 定义模式LSL 30 | def LSL(alpha, d, beta): 31 | # 模式 32 | mode = ["L", "S", "L"] 33 | 34 | # 计算三角函数 35 | sa = np.sin(alpha) 36 | sb = np.sin(beta) 37 | ca = np.cos(beta) 38 | cb = np.cos(beta) 39 | cab = np.cos(alpha - beta) 40 | 41 | # 计算中间量，判断是否存在 42 | p_square = 2. + d ** 2 - 2. * cab + 2. * d * (sa - sb) 43 | if p_square < 0: 44 | return None, None, None, None 45 | 46 | # 计算t,p,q 47 | t = mod2Pi(-alpha + np.arctan((cb - ca) / (d + sa - sb))) 48 | p = np.sqrt(p_square) 49 | q = mod2Pi(beta - np.arctan((cb - ca) / (d + sa - sb))) 50 | 51 | return t, p, q, mode 52 | 53 | # 定义模式RSR 54 | def RSR(alpha, d, beta): 55 | # 模式 56 | mode = ["R", "S", "R"] 57 | 58 | # 计算三角函数 59 | sa = np.sin(alpha) 60 | sb = np.sin(beta) 61 | ca = np.cos(beta) 62 | cb = np.cos(beta) 63 | cab = np.cos(alpha - beta) 64 | 65 | # 计算中间量，判断是否存在 66 | p_square = 2. + d ** 2 - 2. * cab + 2. * d * (sb - sa) 67 | if p_square < 0: 68 | return None, None, None, None 69 | 70 | # 计算t,p,q 71 | t = mod2Pi(alpha - np.arctan((ca - cb) / (d - sa + sb))) 72 | p = np.sqrt(p_square) 73 | q = mod2Pi(-mod2Pi(beta) + np.arctan((ca - cb) / (d - sa + sb))) 74 | 75 | return t, p, q, mode 76 | 77 | # 定义模式RSL 78 | def RSL(alpha, d, beta): 79 | # 模式 80 | mode = ["R", "S", "L"] 81 | 82 | # 计算三角函数 83 | sa = np.sin(alpha) 84 | sb = np.sin(beta) 85 | ca = np.cos(beta) 86 | cb = np.cos(beta) 87 | cab = np.cos(alpha - beta) 88 | 89 | # 计算中间量，判断是否存在 90 | p_square = -2. + d ** 2 + 2. * cab + 2. * d * (sa + sb) 91 | if p_square < 0: 92 | return None, None, None, None 93 | 94 | # 计算t,p,q 95 | p = np.sqrt(p_square) 96 | t = mod2Pi(-alpha + np.arctan((- ca - cb) / (d + sa + sb)) - np.arctan(-2. / p)) 97 | q = mod2Pi(-mod2Pi(beta) + np.arctan((- ca - cb) / (d + sa + sb)) - np.arctan(-2. / p)) 98 | 99 | return t, p, q, mode 100 | 101 | # 定义模式LSR 102 | def LSR(alpha, d, beta): 103 | # 模式 104 | mode = ["L", "S", "R"] 105 | 106 | # 计算三角函数 107 | sa = np.sin(alpha) 108 | sb = np.sin(beta) 109 | ca = np.cos(beta) 110 | cb = np.cos(beta) 111 | cab = np.cos(alpha - beta) 112 | 113 | # 计算中间量，判断是否存在 114 | p_square = -2. + d ** 2 + 2. * cab - 2. * d * (sa + sb) 115 | if p_square < 0: 116 | return None, None, None, None 117 | 118 | # 计算t,p,q 119 | p = np.sqrt(p_square) 120 | t = mod2Pi(alpha - np.arctan((ca + cb) / (d - sa - sb)) + np.arctan(2. / p)) 121 | q = mod2Pi(mod2Pi(beta) - np.arctan((ca + cb) / (d - sa - sb)) + np.arctan(2. / p)) 122 | 123 | return t, p, q, mode 124 | 125 | # 定义模式RLR 126 | def RLR(alpha, d, beta): 127 | # 模式 128 | mode = ["R", "L", "R"] 129 | 130 | # 计算三角函数 131 | sa = np.sin(alpha) 132 | sb = np.sin(beta) 133 | ca = np.cos(beta) 134 | cb = np.cos(beta) 135 | cab = np.cos(alpha - beta) 136 | 137 | # 计算中间量，判断是否存在 138 | pc = (6. - d ** 2 + 2. * cab + 2. * d * (sa - sb)) / 8. 139 | if abs(pc) > 1: 140 | return None, None, None, None 141 | 142 | # 计算t,p,q 143 | p = np.arccos(pc) 144 | t = mod2Pi(alpha - np.arctan((ca - cb) / (d - sa + sb)) + p / 2.) 145 | q = mod2Pi(alpha - beta - t + p) 146 | 147 | return t, p, q, mode 148 | 149 | # 定义模式LRL 150 | def LRL(alpha, d, beta): 151 | # 模式 152 | mode = ["L", "R", "L"] 153 | 154 | # 计算三角函数 155 | sa = np.sin(alpha) 156 | sb = np.sin(beta) 157 | ca = np.cos(beta) 158 | cb = np.cos(beta) 159 | cab = np.cos(alpha - beta) 160 | 161 | # 计算中间量，判断是否存在 162 | pc = (6. - d ** 2 + 2. * cab + 2. * d * (sa - sb)) / 8. 163 | if abs(pc) > 1: 164 | return None, None, None, None 165 | 166 | # 计算t,p,q 167 | p = mod2Pi(np.arccos(pc)) 168 | t = mod2Pi(-alpha + np.arctan((-ca + cb) / (d + sa - sb)) + p / 2.) 169 | q = mod2Pi(mod2Pi(beta) - alpha + 2. * p) 170 | 171 | return t, p, q, mode 172 | 173 | # 原始dubins曲线规划 174 | def rawPlanning(transformed_start_pose, transformed_end_pose, curvature): 175 | print(transformed_start_pose) 176 | print(transformed_end_pose) 177 | # 首先初始化dubins曲线生成的6种模式 178 | func_modes = [LSL, RSR, LSR, RSL, RLR, LRL] 179 | shortest_t, shortest_p, shortest_q, shortest_mode = 0.0, 0.0, 0.0, [] 180 | # 分别调用不同模式计算各段长度t,p,q 181 | # 计算比较哪种模式下总长度最短，选中此模式 182 | D = transformed_end_pose[0] 183 | d = D * curvature 184 | min_cost = float("inf") 185 | for func_mode in func_modes: 186 | t, p, q, mode = func_mode(transformed_start_pose[2], d, transformed_end_pose[2]) 187 | if t is None: 188 | continue 189 | L = t + p + q 190 | if L < min_cost: 191 | min_cost = L 192 | shortest_t = t 193 | shortest_p = p 194 | shortest_q = q 195 | shortest_mode = mode 196 | print(shortest_t, shortest_p, shortest_q, shortest_mode) 197 | # 根据得到的t,p,q进行点采样 198 | x, y, yaw = generateCurve(transformed_start_pose, [shortest_t, shortest_p, shortest_q], shortest_mode, curvature) 199 | return x, y, yaw 200 | 201 | 202 | # 根据t,p,q进行点采样 203 | def generateCurve(start_pose, lengths, mode, curvature, gap = np.deg2rad(5.0)): 204 | x, y, yaw = [start_pose[0]], [start_pose[1]], [start_pose[2]] 205 | for l, m in zip(lengths, mode): 206 | print(l, m) 207 | # 走过的总路程 208 | v = 0.0 209 | # 每次采样的路程 210 | step = 0.0 211 | if m == "S": 212 | # 如果是直线，每次走长度c弧长 213 | step = 0.5 * curvature 214 | else: 215 | # 如果不是直线，每次走gap弧长 216 | step = gap 217 | while l - v > step: 218 | # 沿着当前朝向直线运动step / c真实距离，如果是曲线运动，由于gap较小，可以近似为直线 219 | new_x = x[-1] + step / curvature * np.cos(yaw[-1]) 220 | new_y = y[-1] + step / curvature * np.sin(yaw[-1]) 221 | new_yaw = 0.0 222 | if m == "S": 223 | new_yaw = yaw[-1] 224 | elif m == "L": 225 | new_yaw = mod2Pi(yaw[-1] + step) 226 | else: 227 | new_yaw = mod2Pi(yaw[-1] - step) 228 | x.append(new_x) 229 | y.append(new_y) 230 | yaw.append(new_yaw) 231 | v += step 232 | # 计算剩下的路程 233 | re = l - v 234 | print(re) 235 | if re > 1e-6: 236 | new_x = x[-1] + re / curvature * np.cos(yaw[-1]) 237 | new_y = y[-1] + re / curvature * np.sin(yaw[-1]) 238 | new_yaw = 0.0 239 | if m == "S": 240 | new_yaw = yaw[-1] 241 | elif m == "L": 242 | new_yaw = mod2Pi(yaw[-1] + re) 243 | else: 244 | new_yaw = mod2Pi(yaw[-1] - re) 245 | x.append(new_x) 246 | y.append(new_y) 247 | yaw.append(new_yaw) 248 | return x, y, yaw 249 | 250 | # 规划dubins path 251 | def dubinsPlanning(start_pose, end_pose, curvature): 252 | # 第一步进行坐标转换，转换到以start到end连线为x轴，以start为原点的坐标系下 253 | coordinate = [start_pose[0], start_pose[1], np.arctan2(end_pose[1] - start_pose[1], end_pose[0] - start_pose[0])] 254 | transformed_start_pose = transformCoordinate(coordinate, start_pose) 255 | transformed_end_pose = transformCoordinate(coordinate, end_pose) 256 | # 带入原始dubins曲线规划 257 | x, y, yaw = rawPlanning(transformed_start_pose, transformed_end_pose, curvature) 258 | # 转换到原来的坐标系下 259 | raw_path = [] 260 | for transformed in zip(x, y, yaw): 261 | print(transformed) 262 | raw_pose = retransfromCoordinate(coordinate, transformed) 263 | raw_path.append(raw_pose) 264 | #print(raw_path) 265 | return np.array(raw_path) 266 | # 主函数 267 | def main(): 268 | # 初始化起点和终点 269 | start_pose = [1.0, 1.0, np.deg2rad(45.0)] 270 | end_pose = [-3.0, -3.0, np.deg2rad(-45.0)] 271 | # 初始化最小曲率 272 | curvature = 1.0 273 | # 开始规划 274 | path = dubinsPlanning(start_pose, end_pose, curvature) 275 | plt.plot(path.T[0], path.T[1]) 276 | plt.show() 277 | 278 | if __name__ == "__main__": 279 | main() -------------------------------------------------------------------------------- /dynamic_astar/DynamicAstar.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | # -*- coding: utf-8 -*- 3 | 4 | import math 5 | from sys import maxsize # 导入最大数，2^63-1 6 | 7 | 8 | class State(object): 9 | 10 | def __init__(self, x, y): 11 | self.x = x 12 | self.y = y 13 | self.parent = None 14 | self.state = "." 15 | self.t = "new" 16 | self.h = 0 17 | self.k = 0 # k即为f 18 | 19 | def cost(self, state): 20 | if self.state == "#" or state.state == "#": 21 | return maxsize # 存在障碍物时，距离无穷大 22 | return math.sqrt(math.pow((self.x - state.x), 2) + 23 | math.pow((self.y - state.y), 2)) 24 | 25 | def set_state(self, state): 26 | if state not in ["S", ".", "#", "E", "*","+"]: 27 | return 28 | self.state = state 29 | 30 | 31 | class Map(object): 32 | ''' 33 | 创建地图 34 | ''' 35 | def __init__(self, row, col): 36 | self.row = row 37 | self.col = col 38 | self.map = self.init_map() 39 | 40 | def init_map(self): 41 | # 初始化map 42 | map_list = [] 43 | for i in range(self.row): 44 | tmp = [] 45 | for j in range(self.col): 46 | tmp.append(State(i, j)) 47 | map_list.append(tmp) 48 | return map_list 49 | 50 | def print_map(self): 51 | for i in range(self.row): 52 | tmp = "" 53 | for j in range(self.col): 54 | tmp += self.map[i][j].state + " " 55 | print(tmp) 56 | 57 | def get_neighbers(self, state): 58 | # 获取8邻域 59 | state_list = [] 60 | for i in [-1, 0, 1]: 61 | for j in [-1, 0, 1]: 62 | if i == 0 and j == 0: 63 | continue 64 | if state.x + i < 0 or state.x + i >= self.row: 65 | continue 66 | if state.y + j < 0 or state.y + j >= self.col: 67 | continue 68 | state_list.append(self.map[state.x + i][state.y + j]) 69 | return state_list 70 | 71 | def set_obstacle(self, point_list): 72 | # 设置障碍物的位置 73 | for x, y in point_list: 74 | if x < 0 or x >= self.row or y < 0 or y >= self.col: 75 | continue 76 | self.map[x][y].set_state("#") 77 | 78 | 79 | class Dstar(object): 80 | 81 | def __init__(self, maps): 82 | self.map = maps 83 | self.open_list = set() # 创建空集合 84 | 85 | def process_state(self): 86 | ''' 87 | D*算法的主要过程 88 | :return: 89 | ''' 90 | x = self.min_state() # 获取open list列表中最小k的节点 91 | if x is None: 92 | return -1 93 | k_old = self.get_kmin() #获取open list列表中最小k节点的k值 94 | self.remove(x) # 从openlist中移除 95 | # 判断openlist中 96 | if k_old < x.h: 97 | for y in self.map.get_neighbers(x): 98 | if y.h <= k_old and x.h > y.h + x.cost(y): 99 | x.parent = y 100 | x.h = y.h + x.cost(y) 101 | elif k_old == x.h: 102 | for y in self.map.get_neighbers(x): 103 | if y.t == "new" or y.parent == x and y.h != x.h + x.cost(y) \ 104 | or y.parent != x and y.h > x.h + x.cost(y): 105 | y.parent = x 106 | self.insert(y, x.h + x.cost(y)) 107 | else: 108 | for y in self.map.get_neighbers(x): 109 | if y.t == "new" or y.parent == x and y.h != x.h + x.cost(y): 110 | y.parent = x 111 | self.insert(y, x.h + x.cost(y)) 112 | else: 113 | if y.parent != x and y.h > x.h + x.cost(y): 114 | self.insert(x, x.h) 115 | else: 116 | if y.parent != x and x.h > y.h + x.cost(y) \ 117 | and y.t == "close" and y.h > k_old: 118 | self.insert(y, y.h) 119 | return self.get_kmin() 120 | 121 | def min_state(self): 122 | if not self.open_list: 123 | return None 124 | min_state = min(self.open_list, key=lambda x: x.k) # 获取openlist中k值最小对应的节点 125 | return min_state 126 | 127 | def get_kmin(self): 128 | # 获取openlist表中k(f)值最小的k 129 | if not self.open_list: 130 | return -1 131 | k_min = min([x.k for x in self.open_list]) 132 | return k_min 133 | 134 | def insert(self, state, h_new): 135 | if state.t == "new": 136 | state.k = h_new 137 | elif state.t == "open": 138 | state.k = min(state.k, h_new) 139 | elif state.t == "close": 140 | state.k = min(state.h, h_new) 141 | state.h = h_new 142 | state.t = "open" 143 | self.open_list.add(state) 144 | 145 | def remove(self, state): 146 | if state.t == "open": 147 | state.t = "close" 148 | self.open_list.remove(state) 149 | 150 | def modify_cost(self, x): 151 | if x.t == "close": # 是以一个openlist，通过parent递推整条路径上的cost 152 | self.insert(x, x.parent.h + x.cost(x.parent)) 153 | 154 | def run(self, start, end): 155 | self.open_list.add(end) 156 | while True: 157 | self.process_state() 158 | if start.t == "close": 159 | break 160 | start.set_state("S") 161 | s = start 162 | while s != end: 163 | s = s.parent 164 | s.set_state("+") 165 | s.set_state("E") 166 | print('障碍物未发生变化时，搜索的路径如下：') 167 | self.map.print_map() 168 | tmp = start # 起始点不变 169 | self.map.set_obstacle([(9, 3), (9, 4), (9, 5), (9, 6), (9, 7), (9, 8)]) # 障碍物发生变化 170 | ''' 171 | 从起始点开始，往目标点行进，当遇到障碍物时，重新修改代价，再寻找路径 172 | ''' 173 | while tmp != end: 174 | tmp.set_state("*") 175 | # self.map.print_map() 176 | # print("") 177 | if tmp.parent.state == "#": 178 | self.modify(tmp) 179 | continue 180 | tmp = tmp.parent 181 | tmp.set_state("E") 182 | print('障碍物发生变化时，搜索的路径如下(*为更新的路径)：') 183 | self.map.print_map() 184 | 185 | def modify(self, state): 186 | ''' 187 | 当障碍物发生变化时，从目标点往起始点回推，更新由于障碍物发生变化而引起的路径代价的变化 188 | :param state: 189 | :return: 190 | ''' 191 | self.modify_cost(state) 192 | while True: 193 | k_min = self.process_state() 194 | if k_min >= state.h: 195 | break 196 | 197 | 198 | if __name__ == '__main__': 199 | m = Map(20, 20) 200 | m.set_obstacle([(4, 3), (4, 4), (4, 5), (4, 6), (5, 3), (6, 3), (7, 3)]) 201 | start = m.map[1][2] 202 | end = m.map[17][11] 203 | dstar = Dstar(m) 204 | dstar.run(start, end) 205 | # m.print_map() 206 | 207 | -------------------------------------------------------------------------------- /dynamic_window/DynamicWindow: -------------------------------------------------------------------------------- 1 | Dynamic window方法 2 | 方法假设短时内（代码里为0.1秒）机器人运动的速度和角速度是不变的，在这段时间内机器人轨迹为弧形。于是可以通过确定（v,w）速度和角速度来确定机器人的运动轨迹。 3 | 首先获取当前机器人状态（x,y,theta,v,w），再确定机器人运动的固定参数（最大速度，最小速度，最大转向，最大加速度，最大转向变化率等）。 4 | 然后就可以获取（v,w）的动态窗口，也就是限制条件，可行域是v属于（最小速度->最大速度）,w属于（-最大转向->最大转向），限制域为v属于（当前速度-最大加速度*时间间隔0.1秒，当前速度+最大加速度*时间间隔0.1秒），w属于（当前转角-最大转角变化率*时间间隔0.1秒，当前转角+最大转角变化率*时间间隔0.1秒）。 5 | 在以上限制条件下，计算损失函数最小化的(v,w)作为最终的选择。代码中遍历可行域中全部的v和w（以特定的分辨率）,以临时的v和w来计算以此运动3秒后的机器人轨迹，然后以3秒后机器人朝向与终点连线角度差，机器人速度离最大速度，机器人3秒轨迹中离障碍物的最近距离的导数进行加权平均，得到最终损失。损失越小说明离障碍物越远、朝向指向终点、速度保持最大。以此得到最优的(v,w)作为之后0.1秒的运动选择。 6 | 重复上述过程直到机器人离终点距离小于阈值。 7 | 8 | 评价： 9 | 此算法相当于在当前位置根据速度和角速度规划出若干弧段（圆弧段长度为0.1秒变化后速度乘3秒），判断圆弧段中的最优路径，沿着路径走0.1秒后再重新规划。 10 | -------------------------------------------------------------------------------- /dynamic_window/dynamic_window.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/python3 2 | # -*- coding: utf-8 -*- 3 | 4 | """ 5 | 6 | dynamic window approach 7 | 8 | author: flztiii 9 | 10 | """ 11 | 12 | import math 13 | import numpy as np 14 | import matplotlib.pyplot as plt 15 | import copy 16 | 17 | # 机器人状态信息 18 | class RobotState: 19 | def __init__(self, x, y, yaw, v, w): 20 | self.x = x # 横坐标 21 | self.y = y # 纵坐标 22 | self.yaw = yaw # 朝向角 23 | self.v = v # 速度标量 24 | self.w = w #角速度标量 25 | 26 | # 机器人运动信息 27 | class Motion: 28 | def __init__(self, v, w): 29 | self.v = v # 速度标量 30 | self.w = w #角速度标量 31 | 32 | # 障碍物信息 33 | class Obstacle: 34 | def __init__(self, x, y): 35 | self.x = x # 横坐标 36 | self.y = y # 纵坐标 37 | 38 | # 目标点信息 39 | class Goal: 40 | def __init__(self, x, y): 41 | self.x = x # 横坐标 42 | self.y = y # 纵坐标 43 | 44 | # 机器人参数 45 | class RobotConfig: 46 | def __init__(self): 47 | self.max_velocity = 1.0 # 最大速度 48 | self.min_velocity = -0.5 # 最小速度 49 | self.max_acceleration = 0.2 # 最大加速度 50 | self.max_yaw_rate = 40.0 * math.pi / 180.0 # 最大转向角 51 | self.max_yaw_rate_change = 40.0 * math.pi / 180.0 # 最大转向角变化率 52 | self.velocity_gap = 0.01 # 速度分辨率 53 | self.yaw_rate_gap = 0.1 * math.pi / 180.0 # 转向角分辨率 54 | self.time_gap = 0.1 # 时间窗口 55 | self.time_prediction = 3.0 # 预测时间 56 | self.robot_size = 1.0 # 机器人大小 57 | self.heading_cost_weight = 0.15 # 朝向权重 58 | self.velocity_cost_weight = 1.0 # 速度权重 59 | self.obstacle_cost_weight = 1.0 # 离障碍物距离权重 60 | 61 | # 动态窗口 62 | class DynamicWindow: 63 | def __init__(self, min_velocity, max_velocity, min_yaw_rate, max_yaw_rate): 64 | self.min_velocity = min_velocity 65 | self.max_velocity = max_velocity 66 | self.min_yaw_rate = min_yaw_rate 67 | self.max_yaw_rate = max_yaw_rate 68 | 69 | def calcInteraction(self, dyanmic_window): 70 | self.min_velocity = max(self.min_velocity, dyanmic_window.min_velocity) 71 | self.max_velocity = min(self.max_velocity, dyanmic_window.max_velocity) 72 | self.min_yaw_rate = max(self.min_yaw_rate, dyanmic_window.min_yaw_rate) 73 | self.max_yaw_rate = min(self.max_yaw_rate, dyanmic_window.max_yaw_rate) 74 | 75 | # 获得滑动窗口 76 | def getDynamicWindow(current_motion, config): 77 | Vs = DynamicWindow(config.min_velocity, config.max_velocity, -config.max_yaw_rate, config.max_yaw_rate) 78 | Vd = DynamicWindow(current_motion.v - config.time_gap * config.max_acceleration, 79 | current_motion.v + config.time_gap * config.max_acceleration, 80 | current_motion.w - config.time_gap * config.max_yaw_rate_change, 81 | current_motion.w + config.time_gap * config.max_yaw_rate_change) 82 | # 求交集 83 | Vd.calcInteraction(Vs) 84 | return Vd 85 | 86 | # 根据行为更新状态 87 | def action(state, motion, t): 88 | new_state = state 89 | new_state.v = motion.v 90 | new_state.w = motion.w 91 | new_state.yaw = state.yaw + motion.w * t 92 | new_state.x = state.x + motion.v * math.cos(new_state.yaw) * t 93 | new_state.y = state.y + motion.v * math.sin(new_state.yaw) * t 94 | return new_state 95 | 96 | # 预测轨迹 97 | def predictTrajectory(current_state, predict_motion, config): 98 | predict_trajectory = [current_state] 99 | tmp_state = copy.copy(current_state) 100 | for tmp_t in np.arange(0, config.time_prediction, config.time_gap): 101 | tmp_state = action(tmp_state, predict_motion, config.time_gap) 102 | predict_trajectory.append(copy.copy(tmp_state)) 103 | return predict_trajectory 104 | 105 | # 计算朝向损失 106 | def calcHeadingCost(final_state, goal): 107 | error_angle = math.atan2(goal.y - final_state.y, goal.x - final_state.x) 108 | return abs(error_angle - final_state.yaw) 109 | 110 | # 计算障碍物损失 111 | def calcObstacleCost(predict_trajectory, obstacles, config): 112 | # 计算预测轨迹与障碍物的最近距离 113 | skip_n = 2 114 | min_distance = float("inf") 115 | for i in range(0, len(predict_trajectory), skip_n): 116 | for j in range(0, len(obstacles)): 117 | dx = predict_trajectory[i].x - obstacles[j].x 118 | dy = predict_trajectory[i].y - obstacles[j].y 119 | distance = math.sqrt(dx**2 + dy**2) 120 | if distance <= min_distance: 121 | min_distance = distance 122 | cost = 0.0 123 | if min_distance > config.robot_size: 124 | cost = 1.0 / min_distance 125 | else: 126 | cost = float("inf") 127 | return cost 128 | 129 | # 得到最优运动 130 | def getBestMotion(current_state, dynamic_window, obstacles, goal, config): 131 | min_cost = float("inf") 132 | best_prediction_trajectory = [current_state] 133 | best_motion = Motion(0.0, 0.0) 134 | for temp_v in np.arange(dynamic_window.min_velocity, dynamic_window.max_velocity, config.velocity_gap): 135 | for temp_w in np.arange(dynamic_window.min_yaw_rate, dynamic_window.max_yaw_rate, config.yaw_rate_gap): 136 | # 得到预测行为和轨迹 137 | predict_motion = Motion(temp_v, temp_w); 138 | prediction_trajectory = predictTrajectory(current_state, predict_motion, config) 139 | # 根据预测轨迹计算损失 140 | heading_cost = config.heading_cost_weight * calcHeadingCost(prediction_trajectory[-1], goal) 141 | velocity_cost = config.velocity_cost_weight * (config.max_velocity - prediction_trajectory[-1].v) 142 | obstacle_cost = config.obstacle_cost_weight * calcObstacleCost(prediction_trajectory, obstacles, config) 143 | total_cost = heading_cost + velocity_cost + obstacle_cost 144 | if total_cost <= min_cost: 145 | min_cost = total_cost 146 | best_motion = predict_motion 147 | best_prediction_trajectory = prediction_trajectory 148 | return best_motion, best_prediction_trajectory 149 | 150 | # 可视化当前状态 151 | def plotArrow(x, y, yaw, length=0.5, width=0.1): 152 | plt.arrow(x, y, length * math.cos(yaw), length * math.sin(yaw), head_length=width, head_width=width) 153 | plt.plot(x, y) 154 | 155 | # 主函数 156 | def main(): 157 | # 配置信息 158 | config = RobotConfig() 159 | # 当前状态 160 | current_state = RobotState(0.0, 0.0, math.pi / 8.0, 0.0, 0.0) 161 | # 目标点 162 | goal = Goal(10.0, 10.0); 163 | # 障碍物集合 164 | obstacles = [Obstacle(-1, -1), 165 | Obstacle(0, 2), 166 | Obstacle(4.0, 2.0), 167 | Obstacle(5.0, 4.0), 168 | Obstacle(5.0, 5.0), 169 | Obstacle(5.0, 6.0), 170 | Obstacle(5.0, 9.0), 171 | Obstacle(8.0, 9.0), 172 | Obstacle(7.0, 9.0), 173 | Obstacle(12.0, 12.0)] 174 | # 当前运动 175 | current_motion = Motion(current_state.v, current_state.w) 176 | # 轨迹保存 177 | trajectory = [copy.copy(current_state)] 178 | # 初始化信息完成开始规划 179 | while True: 180 | # 根据当前状态得到动态窗口 181 | dynamic_window = getDynamicWindow(current_motion, config) 182 | # 从动态窗口中得到最优的运动 183 | best_motion, prediction_trajectory = getBestMotion(current_state, dynamic_window, obstacles, goal, config) 184 | # 更新当前状态、运动和轨迹 185 | current_state = action(current_state, best_motion, config.time_gap) 186 | current_motion = best_motion 187 | trajectory.append(copy.copy(current_state)) 188 | # 可视化每一个状态 189 | vis_pred_trajectory = [] 190 | for i in range(0, len(prediction_trajectory)): 191 | vis_pred_trajectory.append([prediction_trajectory[i].x, prediction_trajectory[i].y]) 192 | vis_pred_trajectory = np.array(vis_pred_trajectory) 193 | vis_obstacles = [] 194 | for i in range(0, len(obstacles)): 195 | vis_obstacles.append([obstacles[i].x, obstacles[i].y]) 196 | vis_obstacles = np.array(vis_obstacles) 197 | plt.cla() 198 | plt.plot(vis_pred_trajectory[:, 0], vis_pred_trajectory[:, 1], "-g") 199 | plt.plot(current_state.x, current_state.y, "xr") 200 | plt.plot(goal.x, goal.y, "xb") 201 | plt.plot(vis_obstacles[:, 0], vis_obstacles[:, 1], "ok") 202 | plotArrow(current_state.x, current_state.y, current_state.yaw) 203 | plt.axis("equal") 204 | plt.grid(True) 205 | plt.pause(0.0001) 206 | # 判断是否到达终点 207 | distance_to_goal = math.sqrt((current_state.x - goal.x) ** 2 + (current_state.y - goal.y) ** 2) 208 | if distance_to_goal <= config.robot_size: 209 | print("Goal Reached") 210 | break; 211 | # 可视化全部轨迹 212 | print("Done") 213 | vis_trajectory = [] 214 | for i in range(0, len(trajectory)): 215 | vis_trajectory.append([trajectory[i].x, trajectory[i].y]) 216 | vis_trajectory = np.array(vis_trajectory) 217 | plt.plot(vis_trajectory[:, 0], vis_trajectory[:, 1], "-r") 218 | plt.pause(0.0001) 219 | plt.show() 220 | 221 | if __name__ == "__main__": 222 | main(); -------------------------------------------------------------------------------- /eta3spline/Eta3Spline.md: -------------------------------------------------------------------------------- 1 | eta^3 spline规划算法是一种多项式曲线生成算法，通过曲线参数方程来得到路径。其所使用的是7次多项式。这里以Piazzi2007年论文eta^3 Splines for the Smooth Path Generation of Wheeled Mobile Robots进行说明。 2 | 论文中，首先对曲线进行说明。论文表示曲线只有同时满足位置、朝向、曲率以及曲率变化率连续，才能够满足轮式移动机器人控制的需求。因此，要通过参数多项式来表示曲线时，其最低次数为7次。即 3 | 曲线P(u) = [a(u), b(u)]，其中u属于[0, 1]。 4 | a(u) = a0 + a1 * u + a2 * u ^ 2 + a3 * u ^ 3 + a4 * u ^ 4 + a5 * u ^ 5 + a6 * u ^ 6 + a7 * u ^ 7 5 | b(u) = b0 + b1 * u + b2 * u ^ 2 + b3 * u ^ 3 + b4 * u ^ 4 + b5 * u ^ 5 + b6 * u ^ 6 + b7 * u ^ 7 6 | 要得到曲线，就要求出16个未知量，根据端点的边界条件可以对这16个量进行表示。这16个量可以表示为6个变量eta组成的表达式。而通过这6个变量可以生成不同线形的路径。 -------------------------------------------------------------------------------- /eta3spline/Eta3SplineTra.md: -------------------------------------------------------------------------------- 1 | Eta^3 spline trajectory是在Eta^3 spline path的基础上给每一个点加上速度和角速度，相当于在之前规划的路径基础之上进行速度规划，最终得到轨迹。 2 | 速度规划的思路相对简单，只考虑以最大加速度达到最大速度，并在终点时达到速度和加速度同时为0即可。其分为了七个阶段：第一个阶段是变加速运动，即加速度以最大变化率达到最大加速度；第二阶段是匀加速运动，速度以最大加速度达到特定速度；第三阶段变加速运动，加速度以最大变化率变为0，并与此同时，速度由特定速度达到最大速度；第四阶段匀加速运动，以最大速度行驶；第五阶段变加速运动加速度以最大变化率达到最大减速度；第六阶段匀减速运动，以最大减速度减速到特定速度；第七阶段，变加速运动加速度以最大变化率达到0，并与此同时，速度也从特定速度变化为0。 3 | 以上过程必须确定最大速度是否能够达到，确定的方法没能找到相关资料，需要进一步思考。 4 | 在完成轨迹的速度属性定义后，下一步是如何得到时间t所对应的位置，朝向和速度信息。首先，可以判断t时间走过的路程s，这个可以通过之前得到的速度属性计算出来，下一步就是如何通过s得到对应的参数u。首先根据s可以知道特定点是处于哪一个曲线段，而由于对于特定曲线段，s(u)的方程是已知的，而且s(u)的导数也是已知的，因此可以通过牛顿迭代的方法找到特定s对应的u。（这里的迭代初始值十分重要，因为定义域只在[0,1]之间）得到u后，就可以通过eta^3 spline的方程直接得到t时刻对应的位置，一阶导数和二阶导数，也就可以进一步通过微分几何推导得到角速度的值。 -------------------------------------------------------------------------------- /eta3spline/__pycache__/eta3spline_path_planning.cpython-35.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flztiii/path_planning_basic_algorithm/d1827cbb355416da8a14831eb37a929c5c15e644/eta3spline/__pycache__/eta3spline_path_planning.cpython-35.pyc -------------------------------------------------------------------------------- /eta3spline/eta3spline_path_planning.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/python3 2 | # -*- coding: utf-8 -*- 3 | 4 | """ 5 | 6 | eta^3 spline path planning 7 | author: flztiii 8 | 9 | """ 10 | 11 | import numpy as np 12 | import matplotlib.pyplot as plt 13 | from scipy import integrate 14 | 15 | # eta spline set类（由多条收尾相连的eta spline构成一条长路径） 16 | class EtaSplineSet(object): 17 | def __init__(self, spline_segments): 18 | assert isinstance(spline_segments, list) 19 | assert len(spline_segments) > 0 and isinstance(spline_segments[0], EtaSpline) 20 | self.spline_segments_ = spline_segments 21 | self.n_ = len(spline_segments) 22 | 23 | # 计算特定参数对应的曲线上的坐标 24 | def calcPosition(self, u): 25 | assert u >= 0 and u <= self.n_ 26 | # 首先判断u属于哪一段 27 | index = 0 28 | for i in range(0, self.n_): 29 | if i < self.n_ - 1: 30 | if u < i + 1: 31 | index = i 32 | break 33 | else: 34 | index = i 35 | u = u - index 36 | return self.spline_segments_[index].calcPosition(u) 37 | 38 | # eta spline类 39 | class EtaSpline(object): 40 | def __init__(self, start_pos, end_pos, eta): 41 | # 初始化条件 42 | self.start_pos_ = start_pos 43 | self.end_pos_ = end_pos 44 | self.eta_ = eta 45 | # 计算多项式参数 46 | self.__generateCoefficient() 47 | # 计算路程总长度 48 | self.total_distance_ = self.funcS(1) 49 | 50 | # 构造eta spline曲线七次多项式参数 51 | def __generateCoefficient(self): 52 | # 初始化参数 53 | coefficient = np.zeros((2, 8)) 54 | # 根据公式计算多项式的参数 55 | coefficient[0, 0] = self.start_pos_[0] 56 | coefficient[0, 1] = self.eta_[0] * np.cos(self.start_pos_[2]) 57 | coefficient[0, 2] = 0.5 * self.eta_[2] * np.cos(self.start_pos_[2]) - 0.5 * self.eta_[0] ** 2 * self.start_pos_[3] * np.sin(self.start_pos_[2]) 58 | coefficient[0, 3] = self.eta_[4] * np.cos(self.start_pos_[2]) / 6.0 - (self.eta_[0] ** 3 * self.start_pos_[4] + 3 * self.eta_[0] * self.eta_[2] * self.start_pos_[3]) * np.sin(self.start_pos_[2]) / 6.0 59 | coefficient[0, 4] = 35.0 * (self.end_pos_[0] - self.start_pos_[0]) - (20 * self.eta_[0] + 5 * self.eta_[2] + 2.0/3.0 * self.eta_[4]) * np.cos(self.start_pos_[2]) + (5 * self.eta_[0] ** 2 * self.start_pos_[3] + 2.0/3.0 * self.eta_[0] ** 3 * self.start_pos_[4] + 2 * self.eta_[0] * self.eta_[2] * self.start_pos_[3]) * np.sin(self.start_pos_[2]) - (15 * self.eta_[1] - 5.0/2.0 * self.eta_[3] + 1.0/6.0 * self.eta_[5]) * np.cos(self.end_pos_[2]) - (5.0/2.0 * self.eta_[1] ** 2 * self.end_pos_[3] - 1.0/6.0 * self.eta_[1] ** 3 * self.end_pos_[4] - 0.5 * self.eta_[1] * self.eta_[3] * self.end_pos_[3]) * np.sin(self.end_pos_[2]) 60 | coefficient[0, 5] = -84.0 * (self.end_pos_[0] - self.start_pos_[0]) + (45 * self.eta_[0] + 10 * self.eta_[2] + self.eta_[4]) * np.cos(self.start_pos_[2]) - (10 * self.eta_[0] ** 2 * self.start_pos_[3] + self.eta_[0] ** 3 * self.start_pos_[4] + 3 * self.eta_[0] * self.eta_[2] * self.start_pos_[3]) * np.sin(self.start_pos_[2]) + (39 * self.eta_[1] - 7 * self.eta_[3] + 0.5 * self.eta_[5]) * np.cos(self.end_pos_[2]) + (7 * self.eta_[1] ** 2 * self.end_pos_[3] - 1.0/2.0 * self.eta_[1] ** 3 * self.end_pos_[4] - 3.0/2.0 * self.eta_[1] * self.eta_[3] * self.end_pos_[3]) * np.sin(self.end_pos_[2]) 61 | coefficient[0, 6] = 70.0 * (self.end_pos_[0] - self.start_pos_[0]) - (36 * self.eta_[0] + 15.0/2.0 * self.eta_[2] + 2.0/3.0 * self.eta_[4]) * np.cos(self.start_pos_[2]) + (15.0/2.0 * self.eta_[0] ** 2 * self.start_pos_[3] + 2.0/3.0 * self.eta_[0] ** 3 * self.start_pos_[4] + 2 * self.eta_[0] * self.eta_[2] * self.start_pos_[3]) * np.sin(self.start_pos_[2]) - (34 * self.eta_[1] - 13.0/2.0 * self.eta_[3] + 1.0/2.0 * self.eta_[5]) * np.cos(self.end_pos_[2]) - (13.0/2.0 * self.eta_[1] ** 2 * self.end_pos_[3] - 1.0/2.0 * self.eta_[1] ** 3 * self.end_pos_[4] - 3.0/2.0 * self.eta_[1] * self.eta_[3] * self.end_pos_[3]) * np.sin(self.end_pos_[2]) 62 | coefficient[0, 7] = -20.0 * (self.end_pos_[0] - self.start_pos_[0]) + (10 * self.eta_[0] + 2 * self.eta_[2] + 1.0/6.0 * self.eta_[4]) * np.cos(self.start_pos_[2]) - (2 * self.eta_[0] ** 2 * self.start_pos_[3] + 1.0/6.0 * self.eta_[0] ** 3 * self.start_pos_[4] + 1.0/2.0 * self.eta_[0] * self.eta_[2] * self.start_pos_[3]) * np.sin(self.start_pos_[2]) + (10 * self.eta_[1] - 2 * self.eta_[3] + 1.0/6.0 * self.eta_[5]) * np.cos(self.end_pos_[2]) + (2 * self.eta_[1] ** 2 * self.end_pos_[3] - 1.0/6.0 * self.eta_[1] ** 3 * self.end_pos_[4] - 1.0/2.0 * self.eta_[1] * self.eta_[3] * self.end_pos_[3]) * np.sin(self.end_pos_[2]) 63 | coefficient[1, 0] = self.start_pos_[1] 64 | coefficient[1, 1] = self.eta_[0] * np.sin(self.start_pos_[2]) 65 | coefficient[1, 2] = 1.0/2.0 * self.eta_[2] * np.sin(self.start_pos_[2]) + 1.0/2.0 * self.eta_[0] ** 2 * self.start_pos_[3] * np.cos(self.start_pos_[2]) 66 | coefficient[1, 3] = 1.0/6.0 * self.eta_[4] * np.sin(self.start_pos_[2]) + 1.0/6.0 * (self.eta_[0] ** 3 * self.start_pos_[4] + 3 * self.eta_[0] * self.eta_[2] * self.start_pos_[3]) * np.cos(self.start_pos_[2]) 67 | coefficient[1, 4] = 35 * (self.end_pos_[1] - self.start_pos_[1]) - (20 * self.eta_[0] + 5 * self.eta_[2] + 2.0/3.0 * self.eta_[4]) * np.sin(self.start_pos_[2]) - (5 * self.eta_[0] ** 2 * self.start_pos_[3] + 2.0/3.0 * self.eta_[0] ** 3 * self.start_pos_[4] + 2 * self.eta_[0] * self.eta_[2] * self.start_pos_[3]) * np.cos(self.start_pos_[2]) - (15 * self.eta_[1] - 5.0/2.0 * self.eta_[3] + 1.0/6.0 * self.eta_[5]) * np.sin(self.end_pos_[2]) + (5.0/2.0 * self.eta_[1] ** 2 * self.end_pos_[3] - 1.0/6.0 * self.eta_[1] ** 3 * self.end_pos_[4] - 1.0/2.0 * self.eta_[1] * self.eta_[3] * self.end_pos_[3]) * np.cos(self.end_pos_[2]) 68 | coefficient[1, 5] = -84 * (self.end_pos_[1] - self.start_pos_[1]) + (45 * self.eta_[0] + 10 * self.eta_[2] + self.eta_[4]) * np.sin(self.start_pos_[2]) + (10 * self.eta_[0] ** 2 * self.start_pos_[3] + self.eta_[0] ** 3 * self.start_pos_[4] + 3 * self.eta_[0] * self.eta_[2] * self.start_pos_[3]) * np.cos(self.start_pos_[2]) + (39 * self.eta_[1] - 7.0 * self.eta_[3] + 1.0/2.0 * self.eta_[5]) * np.sin(self.end_pos_[2]) - (7 * self.eta_[1] ** 2 * self.end_pos_[3] - 1.0/2.0 * self.eta_[1] ** 3 * self.end_pos_[4] - 3.0/2.0 * self.eta_[1] * self.eta_[3] * self.end_pos_[3]) * np.cos(self.end_pos_[2]) 69 | coefficient[1, 6] = 70 * (self.end_pos_[1] - self.start_pos_[1]) - (36 * self.eta_[0] + 15.0/2.0 * self.eta_[2] + 2.0/3.0 * self.eta_[4]) * np.sin(self.start_pos_[2]) - (15.0/2.0 * self.eta_[0] ** 2 * self.start_pos_[3] + 2.0/3.0 * self.eta_[0] ** 3 * self.start_pos_[4] + 2 * self.eta_[0] * self.eta_[2] * self.start_pos_[3]) * np.cos(self.start_pos_[2]) - (34 * self.eta_[1] - 13.0/2.0 * self.eta_[3] + 1.0/2.0 * self.eta_[5]) * np.sin(self.end_pos_[2]) + (13.0/2.0 * self.eta_[1] ** 2 * self.end_pos_[3] - 1.0/2.0 * self.eta_[1] ** 3 * self.end_pos_[4] - 3.0/2.0 * self.eta_[1] * self.eta_[3] * self.end_pos_[3]) * np.cos(self.end_pos_[2]) 70 | coefficient[1, 7] = -20 * (self.end_pos_[1] - self.start_pos_[1]) + (10 * self.eta_[0] + 2 * self.eta_[2] + 1.0/6.0 * self.eta_[4]) * np.sin(self.start_pos_[2]) + (2 * self.eta_[0] ** 2 * self.start_pos_[3] + 1.0/6.0 * self.eta_[0] ** 3 * self.start_pos_[4] + 1.0/2.0 * self.eta_[0] * self.eta_[2] * self.start_pos_[3]) * np.cos(self.start_pos_[2]) + (10 * self.eta_[1] - 2 * self.eta_[3] + 1.0/6.0 * self.eta_[5]) * np.sin(self.end_pos_[2]) - (2 * self.eta_[1] ** 2 * self.end_pos_[3] - 1.0/6.0 * self.eta_[1] ** 3 * self.end_pos_[4] - 1.0/2.0 * self.eta_[1] * self.eta_[3] * self.end_pos_[3]) * np.cos(self.end_pos_[2]) 71 | self.coefficient_ = coefficient 72 | 73 | # 根据输入参数计算对应曲线上点的坐标 74 | def calcPosition(self, u): 75 | assert u >= 0 and u <= 1 76 | result = np.dot(self.coefficient_, np.array([1.0, u, u ** 2, u ** 3, u ** 4, u ** 5, u ** 6, u ** 7]).T) 77 | return result 78 | 79 | # 根据输入参数计算对应曲线上点的一阶导数 80 | def calcDerivative(self, u): 81 | assert u >= 0 and u <= 1 82 | result = np.dot(self.coefficient_[:,1:], np.array([1.0, 2 * u, 3 * u ** 2, 4 * u ** 3, 5 * u ** 4, 6 * u ** 5, 7 * u ** 6]).T) 83 | return result 84 | 85 | # 根据输入参数计算对应曲线上点的二阶导数 86 | def calc2Derivative(self, u): 87 | assert u >= 0 and u <= 1 88 | result = np.dot(self.coefficient_[:,2:], np.array([2, 6 * u, 12 * u ** 2, 20 * u ** 3, 30 * u ** 4, 42 * u ** 5]).T) 89 | return result 90 | 91 | # 根据输入参数计算s的导数 92 | def funcSDerivative(self, u): 93 | return max(np.linalg.norm(self.calcDerivative(u)), 1e-6) 94 | 95 | # 根据输入参数计算路程s 96 | def funcS(self, u): 97 | assert u >= 0 and u <= 1 98 | # 对ds进行积分 99 | s = integrate.quad(self.funcSDerivative, 0, u)[0] 100 | return s 101 | 102 | # 主函数1，eta前两个参数发生变化 103 | def main1(): 104 | # 对于相同的边界条件使用不同的eta进行曲线的生成 105 | for i in range(0, 10): 106 | # 初始化eta 107 | eta = [i, i, 0, 0, 0, 0] # eta为6维度自由变量 108 | # 初始化边界条件 109 | start_pos = [0, 0, 0, 0, 0] # 起点位姿，形式为x坐标，y坐标，朝向yaw，曲率kappa，曲率导数 110 | end_pos = [4.0, 3.0, 0, 0, 0] # 终点位姿，形式同上 111 | # 构造eta spline 112 | eta_spline = EtaSpline(start_pos, end_pos, eta) 113 | # 构造eta spline set 114 | eta_spline_segments = [] 115 | eta_spline_segments.append(eta_spline) 116 | eta_spline_set = EtaSplineSet(eta_spline_segments) 117 | # 开始对eta spline进行采样，采样点数为1000个 118 | samples = np.linspace(0.0, len(eta_spline_segments), 1001) 119 | curve = [] 120 | for sample in samples: 121 | sample_point = eta_spline_set.calcPosition(sample) 122 | curve.append(sample_point) 123 | curve = np.array(curve) 124 | # 进行可视化 125 | plt.plot(curve.T[0], curve.T[1]) 126 | plt.axis("equal") 127 | plt.grid(True) 128 | plt.pause(1.0) 129 | plt.show() 130 | 131 | # 主函数2，eta中间两个参数发生变化 132 | def main2(): 133 | # 对于相同的边界条件使用不同的eta进行曲线的生成 134 | for i in range(0, 10): 135 | # 初始化eta 136 | eta = [0, 0, (i - 5) * 20, (5 - i) * 20, 0, 0] # eta为6维度自由变量 137 | # 初始化边界条件 138 | start_pos = [0, 0, 0, 0, 0] # 起点位姿，形式为x坐标，y坐标，朝向yaw，曲率kappa，曲率导数 139 | end_pos = [4.0, 3.0, 0, 0, 0] # 终点位姿，形式同上 140 | # 构造eta spline 141 | eta_spline = EtaSpline(start_pos, end_pos, eta) 142 | # 构造eta spline set 143 | eta_spline_segments = [] 144 | eta_spline_segments.append(eta_spline) 145 | eta_spline_set = EtaSplineSet(eta_spline_segments) 146 | # 开始对eta spline进行采样，采样点数为1000个 147 | samples = np.linspace(0.0, len(eta_spline_segments), 1001) 148 | curve = [] 149 | for sample in samples: 150 | sample_point = eta_spline_set.calcPosition(sample) 151 | curve.append(sample_point) 152 | curve = np.array(curve) 153 | # 进行可视化 154 | plt.plot(curve.T[0], curve.T[1]) 155 | plt.axis("equal") 156 | plt.grid(True) 157 | plt.pause(1.0) 158 | plt.show() 159 | 160 | # 主函数3，论文中曲线给出的参数 161 | def main3(): 162 | eta_spline_segments = [] 163 | # 第一段参数, lane-change curve 164 | # 初始化eta 165 | eta = [4.27, 4.27, 0, 0, 0, 0] # eta为6维度自由变量 166 | # 初始化边界条件 167 | start_pos = [0, 0, 0, 0, 0] # 起点位姿，形式为x坐标，y坐标，朝向yaw，曲率kappa，曲率导数 168 | end_pos = [4.0, 1.5, 0, 0, 0] # 终点位姿，形式同上 169 | # 构造eta spline 170 | eta_spline = EtaSpline(start_pos, end_pos, eta) 171 | eta_spline_segments.append(eta_spline) 172 | 173 | # 第二段参数, line segment 174 | # 初始化eta 175 | eta = [0, 0, 0, 0, 0, 0] # eta为6维度自由变量 176 | # 初始化边界条件 177 | start_pos = [4, 1.5, 0, 0, 0] # 起点位姿，形式为x坐标，y坐标，朝向yaw，曲率kappa，曲率导数 178 | end_pos = [5.5, 1.5, 0, 0, 0] # 终点位姿，形式同上 179 | # 构造eta spline 180 | eta_spline = EtaSpline(start_pos, end_pos, eta) 181 | eta_spline_segments.append(eta_spline) 182 | 183 | # 第三段参数, cubic spiral 184 | # 初始化eta 185 | eta = [1.88, 1.88, 0, 0, 0, 0] # eta为6维度自由变量 186 | # 初始化边界条件 187 | start_pos = [5.5, 1.5, 0, 0, 0] # 起点位姿，形式为x坐标，y坐标，朝向yaw，曲率kappa，曲率导数 188 | end_pos = [7.4377, 1.8235, 0.6667, 1, 1] # 终点位姿，形式同上 189 | # 构造eta spline 190 | eta_spline = EtaSpline(start_pos, end_pos, eta) 191 | eta_spline_segments.append(eta_spline) 192 | 193 | # 第四段参数, generic twirl arc 194 | # 初始化eta 195 | eta = [7, 10, 10, -10, 4, 4] # eta为6维度自由变量 196 | # 初始化边界条件 197 | start_pos = [7.4377, 1.8235, 0.6667, 1, 1] # 起点位姿，形式为x坐标，y坐标，朝向yaw，曲率kappa，曲率导数 198 | end_pos = [7.8, 4.3, 1.8, 0.5, 0] # 终点位姿，形式同上 199 | # 构造eta spline 200 | eta_spline = EtaSpline(start_pos, end_pos, eta) 201 | eta_spline_segments.append(eta_spline) 202 | 203 | # 第五段参数, circular arc 204 | # 初始化eta 205 | eta = [2.98, 2.98, 0, 0, 0, 0] # eta为6维度自由变量 206 | # 初始化边界条件 207 | start_pos = [7.8, 4.3, 1.8, 0.5, 0] # 起点位姿，形式为x坐标，y坐标，朝向yaw，曲率kappa，曲率导数 208 | end_pos = [5.4581, 5.8064, 3.3416, 0.5, 0] # 终点位姿，形式同上 209 | # 构造eta spline 210 | eta_spline = EtaSpline(start_pos, end_pos, eta) 211 | eta_spline_segments.append(eta_spline) 212 | 213 | eta_spline_set = EtaSplineSet(eta_spline_segments) 214 | # 开始对eta spline进行采样，采样点数为1000个 215 | samples = np.linspace(0.0, len(eta_spline_segments), 1001) 216 | curve = [] 217 | for sample in samples: 218 | sample_point = eta_spline_set.calcPosition(sample) 219 | curve.append(sample_point) 220 | curve = np.array(curve) 221 | # 进行可视化 222 | plt.plot(curve.T[0], curve.T[1]) 223 | plt.axis("equal") 224 | plt.grid(True) 225 | plt.show() 226 | 227 | 228 | if __name__ == "__main__": 229 | # main1() 230 | # main2() 231 | main3() -------------------------------------------------------------------------------- /frenet_optimal_trajectory/frenet_optimal_trajectory.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/python3 2 | # -*- coding: utf-8 -*- 3 | 4 | """ 5 | 6 | Frenet optimal trajectory generation algrithom 7 | author: flztiii 8 | 9 | """ 10 | 11 | import numpy as np 12 | import matplotlib.pyplot as plt 13 | import sys 14 | import os 15 | import copy 16 | 17 | sys.path.append(os.path.dirname(os.path.dirname(os.path.realpath(__file__))) + "/cubic_spline") 18 | import cubic_spline 19 | 20 | # 全局变量 21 | ROAD_WIDTH = 7.0 # 道路的最大宽度 22 | WIDTH_GAP = 1.0 # 道路的横向采样间隔 23 | MIN_T = 4.0 # 最短时间开销 24 | MAX_T = 5.0 # 最长时间开销 25 | T_GAP = 0.1 # 时间采样间隔 26 | ESP = 1e-6 # 精度要求 27 | MAX_VELOCITY = 50.0 / 3.6 # 最大速度 28 | TARGET_VELOCITY = 30.0 / 3.6 # 期望速度 29 | VELOCITY_GAP = 5.0 / 3.6 # 速度采样间隔 30 | VELOCITY_SAMPLE_N = 1 # 速度采样个数 31 | MAX_KAPPA = 1.0 # 最大曲率 32 | MAX_ACCELERATION = 2.0 # 最大加速度 33 | ROBOT_SIZE = 2.0 # 机器人的半径 34 | AREA = 20.0 # 可视化窗口大小 35 | 36 | # 评分加权参数 37 | KJ = 0.1 38 | KT = 0.1 39 | KD = 1.0 40 | KLON = 1.0 41 | KLAT = 1.0 42 | 43 | # frenet轨迹对象 44 | class FrenetPath(object): 45 | def __init__(self): 46 | self.t_ = [] # 时间采样 47 | 48 | # frenet系数据 49 | self.d_ = [] # 横向偏移 50 | self.d_derivative_ = [] # 横向偏移一阶导数 51 | self.d_2derivative_ = [] # 横向偏移二阶导数 52 | self.d_3derivative_ = [] # 横向偏移三阶导数 53 | self.s_ = [] # 纵向偏移 54 | self.s_derivative_ = [] # 纵向偏移一阶导数 55 | self.s_2derivative_ = [] # 纵向偏移二阶导数 56 | self.s_3derivative_ = [] # 纵向偏移三阶导数 57 | 58 | # 评分 59 | self.cost_ = 0.0 60 | 61 | # 世界系数据 62 | self.x_ = [] # x坐标 63 | self.y_ = [] # y坐标 64 | self.yaw_ = [] # 朝向角 65 | self.kappa_ = [] # 曲率 66 | self.ds_ = [] # 两点之间的距离 67 | 68 | # 五次多项式 69 | class QuinticPolynomial: 70 | def __init__(self, params, T): 71 | assert len(params) == 6 72 | self.params_ = params 73 | self.T_ = T 74 | # 生成多项式系数计算 75 | self.__coefficientGeneration() 76 | 77 | # 生成多项式系数计算 78 | def __coefficientGeneration(self): 79 | a0 = self.params_[0] 80 | a1 = self.params_[1] 81 | a2 = self.params_[2] * 0.5 82 | A = np.array([[self.T_ ** 3, self.T_ ** 4, self.T_ ** 5], [3. * self.T_ ** 2, 4. * self.T_ ** 3, 5. * self.T_ ** 4], [6. * self.T_, 12. * self.T_ ** 2, 20. * self.T_ ** 3]]) 83 | b = np.array([self.params_[3] - a0 - a1 * self.T_ - a2 * self.T_ ** 2, self.params_[4] - a1 - 2. * a2 * self.T_, self.params_[5] - 2. * a2]).T 84 | x = np.linalg.solve(A, b) 85 | a3 = x[0] 86 | a4 = x[1] 87 | a5 = x[2] 88 | self.coefficient_ = np.array([a0, a1, a2, a3, a4, a5]) 89 | 90 | # 给定输入时间，计算对应值 91 | def calcValue(self, t): 92 | return np.dot(self.coefficient_, np.array([1., t, t ** 2, t ** 3, t ** 4, t ** 5]).T) 93 | 94 | # 给定输入时间，计算导数 95 | def calcDerivation(self, t): 96 | return np.dot(self.coefficient_[1:], np.array([1., 2. * t, 3. * t ** 2, 4. * t ** 3, 5. * t ** 4])) 97 | 98 | # 给定输入时间，计算二阶导数 99 | def calc2Derivation(self, t): 100 | return np.dot(self.coefficient_[2:], np.array([2. , 6. * t, 12. * t ** 2, 20. * t ** 3])) 101 | 102 | # 给定输入时间，计算三阶导数 103 | def calc3Derivation(self, t): 104 | return np.dot(self.coefficient_[3:], np.array([6., 24. * t, 60. * t ** 2])) 105 | 106 | # 四次多项式 107 | class QuarticPolynomial: 108 | def __init__(self, xs, vs, acs, ve, ace, T): 109 | self.xs_ = xs 110 | self.vs_ = vs 111 | self.acs_ = acs 112 | self.ve_ = ve 113 | self.ace_ = ace 114 | self.T_ = T 115 | # 生成多项式系数计算 116 | self.__coefficientGeneration() 117 | 118 | # 生成多项式系数计算 119 | def __coefficientGeneration(self): 120 | a0 = self.xs_ 121 | a1 = self.vs_ 122 | a2 = self.acs_ * 0.5 123 | A = np.array([[3. * self.T_ ** 2, 4. * self.T_ ** 3], [6. * self.T_, 12. * self.T_ ** 2]]) 124 | B = np.array([[self.ve_ - a1 - 2. * a2 * self.T_], [self.ace_ - 2. * a2]]) 125 | x = np.linalg.solve(A, B) 126 | a3 = x[0, 0] 127 | a4 = x[1, 0] 128 | self.coefficient_ = np.array([a0, a1, a2, a3, a4]) 129 | 130 | # 给定输入时间，计算对应值 131 | def calcValue(self, t): 132 | return np.dot(self.coefficient_, np.array([1., t, t ** 2, t ** 3, t ** 4]).T) 133 | 134 | # 给定输入时间，计算导数 135 | def calcDerivation(self, t): 136 | return np.dot(self.coefficient_[1:], np.array([1., 2. * t, 3. * t ** 2, 4. * t ** 3])) 137 | 138 | # 给定输入时间，计算二阶导数 139 | def calc2Derivation(self, t): 140 | return np.dot(self.coefficient_[2:], np.array([2. , 6. * t, 12. * t ** 2])) 141 | 142 | # 给定输入时间，计算三阶导数 143 | def calc3Derivation(self, t): 144 | return np.dot(self.coefficient_[3:], np.array([6., 24. * t])) 145 | 146 | # frenet坐标架下的规划 147 | def frenetOptimalTrajectoryPlanning(current_s, current_s_derivative, current_s_2derivative, current_d, current_d_derivative, curent_d_2derivative): 148 | # 生成轨迹组 149 | frenet_path_set = [] 150 | # 首先是生横向偏移随时间变化，变化量为最终横向偏移和时间长度 151 | for df_offset in np.arange(- ROAD_WIDTH, ROAD_WIDTH, WIDTH_GAP): 152 | for T in np.arange(MIN_T, MAX_T + ESP, T_GAP): 153 | # 初始化轨迹对象 154 | frenet_path = FrenetPath() 155 | # 根据输入参数进行五次多项式拟合，输入参数[d0, d0_d, d0_dd, df, df_d, df_dd] 156 | quintic_polynomial = QuinticPolynomial([current_d, current_d_derivative, curent_d_2derivative, df_offset, 0.0, 0.0], T) 157 | # 对曲线进行采样 158 | sample_t = np.arange(0.0, T + ESP, T_GAP) 159 | frenet_path.t_ = [t for t in sample_t] 160 | frenet_path.d_ = [quintic_polynomial.calcValue(t) for t in frenet_path.t_] 161 | frenet_path.d_derivative_ = [quintic_polynomial.calcDerivation(t) for t in frenet_path.t_] 162 | frenet_path.d_2derivative_ = [quintic_polynomial.calc2Derivation(t) for t in frenet_path.t_] 163 | frenet_path.d_3derivative_ = [quintic_polynomial.calc3Derivation(t) for t in frenet_path.t_] 164 | 165 | # 开始生成纵向偏移随时间变化，采用的是速度保持模式，因此只需要用四次多项式 166 | for sf_derivative in np.arange(TARGET_VELOCITY - VELOCITY_GAP * VELOCITY_SAMPLE_N, TARGET_VELOCITY + VELOCITY_GAP * VELOCITY_SAMPLE_N + ESP, VELOCITY_GAP): 167 | # 赋值横向偏移数据 168 | frenet_path_copy = copy.deepcopy(frenet_path) 169 | # 根据输入参数进行四次多项式拟合（四次多项式不唯一，这里使用的一阶导数和二阶导数参数） 170 | quartic_polynomial = QuarticPolynomial(current_s, current_s_derivative, current_s_2derivative, sf_derivative, 0.0, T) 171 | frenet_path_copy.s_ = [quartic_polynomial.calcValue(t) for t in frenet_path.t_] 172 | frenet_path_copy.s_derivative_ = [quartic_polynomial.calcDerivation(t) for t in frenet_path.t_] 173 | frenet_path_copy.s_2derivative_ = [quartic_polynomial.calc2Derivation(t) for t in frenet_path.t_] 174 | frenet_path_copy.s_3derivative_ = [quartic_polynomial.calc3Derivation(t) for t in frenet_path.t_] 175 | 176 | # 得到轨迹在frenet系的全部信息 177 | # 开始给轨迹进行评分 178 | # 首先是横向评分，横向评分包括三部分 179 | # 第一部分是jerk 180 | lateral_jerk_cost = np.sum(np.power(frenet_path_copy.d_3derivative_, 2)) 181 | # 第二部分是时间开销 182 | lateral_time_cost = T 183 | # 第三部分是最终横向偏移的平方 184 | lateral_offset_cost = frenet_path_copy.d_[-1] ** 2 185 | # 对以上三部分进行加权求和 186 | lateral_cost = KJ * lateral_jerk_cost + KT * lateral_time_cost + KD * lateral_offset_cost 187 | # 接下来是纵向评分 188 | # 纵向评分有三个部分 189 | # 第一部分是jerk 190 | longitudinal_jerk_cost = np.sum(np.power(frenet_path_copy.s_3derivative_, 2)) 191 | # 第二部分是时间开销 192 | longitudinal_time_cost = T 193 | # 第三部分是最终速度偏差 194 | longitudinal_sderivative_offset = np.power(frenet_path_copy.s_derivative_[-1] - TARGET_VELOCITY, 2) 195 | # 对以上部分进行加权求和 196 | longitudinal_cost = KJ * longitudinal_jerk_cost + KT * longitudinal_time_cost + KD * longitudinal_sderivative_offset 197 | # 求解最终评分 198 | frenet_path_copy.cost_ = KLAT * lateral_cost + KLON * longitudinal_cost 199 | 200 | # 保存曲线到列表 201 | frenet_path_set.append(frenet_path_copy) 202 | 203 | return frenet_path_set 204 | 205 | # frenet坐标系到世界坐标系的转化 206 | def globalTrajectoryGeneration(frenet_path_set, reference_curve): 207 | global_path_set = [] 208 | for frenet_path in frenet_path_set: 209 | global_path = copy.deepcopy(frenet_path) 210 | # 第一步补全位置信息x和y 211 | rfc_x, rfc_y = reference_curve.calcPosition(global_path.s_) 212 | rfc_yaw = reference_curve.calcYaw(global_path.s_) 213 | for i in range(0, len(rfc_x)): 214 | x = rfc_x[i] - np.sin(rfc_yaw[i]) * frenet_path.d_[i] 215 | y = rfc_y[i] + np.cos(rfc_yaw[i]) * frenet_path.d_[i] 216 | global_path.x_.append(x) 217 | global_path.y_.append(y) 218 | # 完成x,y信息的补全后通过计算的方法得到yaw和ds，计算方法为yaw=atan2(dy, dx) 219 | for i in range(0, len(global_path.x_) - 1): 220 | yaw = np.arctan2(global_path.y_[i + 1] - global_path.y_[i], global_path.x_[i + 1] - global_path.x_[i]) 221 | global_path.yaw_.append(yaw) 222 | ds = np.sqrt((global_path.y_[i + 1] - global_path.y_[i]) ** 2 + (global_path.x_[i + 1] - global_path.x_[i]) ** 2) 223 | global_path.ds_.append(ds) 224 | global_path.yaw_.append(global_path.yaw_[-1]) 225 | # 完成yaw的补全后通过计算得到kappa，计算方法为kappa=dyaw/ds 226 | for i in range(0, len(global_path.yaw_) - 1): 227 | kappa = (global_path.yaw_[i + 1] - global_path.yaw_[i]) / global_path.ds_[i] 228 | global_path.kappa_.append(kappa) 229 | global_path.kappa_.append(global_path.kappa_[-1]) 230 | global_path_set.append(global_path) 231 | return global_path_set 232 | 233 | # 判断是否发生碰撞 234 | def isCollision(path, obstacles): 235 | for x, y in zip(path.x_, path.y_): 236 | for obstacle in obstacles: 237 | distance = np.sqrt((x - obstacle[0]) ** 2 + (y - obstacle[1]) ** 2) 238 | if distance <= ROBOT_SIZE: 239 | return True 240 | return False 241 | 242 | # 判断路径组中路径是否可行 243 | def checkPath(path_set, obstacles): 244 | checked_path = [] 245 | # 判断路径是否可行存在四个方面 246 | for i in range(0, len(path_set)): 247 | # 第一个方面判断是否有大于最大速度的速度 248 | if max([np.abs(v) for v in path_set[i].s_derivative_]) > MAX_VELOCITY: 249 | continue 250 | # 第二个方面判断是否有大于最大曲率的曲率 251 | if max([np.abs(kappa) for kappa in path_set[i].kappa_]) > MAX_KAPPA: 252 | continue 253 | # 第三个方面判断是否有大于最大加速度的加速度 254 | if max([np.abs(acceleration) for acceleration in path_set[i].s_2derivative_]) > MAX_ACCELERATION: 255 | continue 256 | # 第四个方面，判断是否与障碍物存在碰撞 257 | if isCollision(path_set[i], obstacles): 258 | continue 259 | checked_path.append(path_set[i]) 260 | return checked_path 261 | 262 | # 主函数 263 | def main(): 264 | # 初始化参考路点 265 | wx = [0.0, 10.0, 20.5, 35.0, 70.5] 266 | wy = [0.0, -6.0, 5.0, 6.5, 0.0] 267 | 268 | # 初始化障碍物所在位置 269 | obstacles = np.array([[20.0, 10.0], 270 | [30.0, 6.0], 271 | [30.0, 8.0], 272 | [35.0, 8.0], 273 | [50.0, 3.0] 274 | ]) 275 | 276 | # 初始化车辆状态信息 277 | current_s = 0.0 # frenet坐标下的s轴 278 | current_s_derivative = 10.0 / 3.6 # frenet坐标下的s轴对时间的一阶导数 279 | current_s_2derivative = 0.0 # frenet坐标下的s轴对时间的二阶导数 280 | current_d = 2.0 # frenet坐标下的d轴 281 | current_d_derivative= 0.0 # frenet坐标下的d轴对时间的一阶导数 282 | curent_d_2derivative = 0.0 # frenet坐标下的d轴对时间的二阶阶导数 283 | 284 | # 接下来是规划的主体内容 285 | 286 | # 第一步生成参考路点路径曲线 287 | # 利用三次样条差值生成参考曲线 288 | reference_curve = cubic_spline.CubicSpline2D(wx, wy) 289 | reference_samples = np.arange(0.0, reference_curve.s_[-1], 0.1) 290 | reference_waypoints_x, reference_waypoints_y = reference_curve.calcPosition(reference_samples) 291 | 292 | # 记录当前点的朝向和曲率 293 | yaw_set, kappa_set, s_set = [], [], [] 294 | 295 | # 每一次更新所花费时间 296 | gap = 1 297 | # 开始进行规划 298 | while True: 299 | # 原算法以10hz频率不断进行规划，此处以到达下一个点进行替代，每到规划的下一个点就进行重新规划 300 | # 进行frenet坐标架下的规划 301 | frenet_path_set = frenetOptimalTrajectoryPlanning(current_s, current_s_derivative, current_s_2derivative, current_d, current_d_derivative, curent_d_2derivative) 302 | 303 | # 将frenet坐标系转换到世界坐标系下 304 | global_path_set = globalTrajectoryGeneration(frenet_path_set, reference_curve) 305 | # 判断路径组中是否可行 306 | # print(len(global_path_set)) 307 | checked_path_set = checkPath(global_path_set, obstacles) 308 | # print(len(checked_path_set)) 309 | # 从其中选取一条代价最低的作为最终路径 310 | # min_cost = float("inf") 311 | final_path = min(checked_path_set, key=lambda p: p.cost_) 312 | 313 | # 新的下标 314 | current_index = min(gap, len(final_path.x_) - 1) 315 | # 得到新的起点 316 | current_s = final_path.s_[current_index] 317 | current_s_derivative = final_path.s_derivative_[current_index] 318 | current_s_2derivative = 0.0 319 | current_d = final_path.d_[current_index] 320 | current_d_derivative= final_path.d_derivative_[current_index] 321 | curent_d_2derivative = final_path.d_2derivative_[current_index] 322 | 323 | # 记录当前点的朝向和曲率 324 | for i in range(0, current_index): 325 | yaw_set.append(final_path.yaw_[i]) 326 | kappa_set.append(final_path.kappa_[i]) 327 | s_set.append(final_path.s_[i]) 328 | 329 | # 判断是否已经到达终点范围 330 | if np.sqrt((final_path.x_[current_index] - wx[-1]) ** 2 + (final_path.y_[current_index] - wy[-1]) ** 2) < ROBOT_SIZE: 331 | print("goal reached") 332 | break 333 | 334 | # 进行可视化 335 | # 首先清空之前的可视化内容 336 | plt.cla() 337 | # 可视化参考道路线 338 | plt.plot(reference_waypoints_x, reference_waypoints_y) 339 | # 可视化障碍物点 340 | plt.plot(obstacles[:, 0], obstacles[:, 1], "xk") 341 | # 可视化当前位置 342 | plt.plot(final_path.x_[1], final_path.y_[1], "vc") 343 | # 可视化路径 344 | plt.plot(final_path.x_[1:], final_path.y_[1:], "-or") 345 | # 可视化窗口 346 | plt.xlim(final_path.x_[1] - AREA, final_path.x_[1] + AREA) 347 | plt.ylim(final_path.y_[1] - AREA, final_path.y_[1] + AREA) 348 | plt.title("v[km/h]:" + str(current_s_derivative * 3.6)[0:4]) 349 | plt.grid(True) 350 | plt.pause(0.0001) 351 | plt.grid(True) 352 | plt.pause(0.0001) 353 | # 可视化朝向和曲率的变化 354 | # plt.subplots() 355 | # plt.grid(True) 356 | # plt.plot(s_set, yaw_set) 357 | plt.subplots() 358 | plt.title('hz is' + str(1.0/gap)) 359 | plt.grid(True) 360 | plt.plot(s_set, kappa_set) 361 | plt.show() 362 | 363 | if __name__ == "__main__": 364 | main() -------------------------------------------------------------------------------- /grid_astar/GridAstar: -------------------------------------------------------------------------------- 1 | 基于栅格的A星搜索算法在思路上与GridDijkstra基本类似，仅仅在其基础上增加了启发函数这一变化。 2 | 方法首先找到一个矩形区域，包含全部的障碍物和起终点。根据一定的分辨率，将此区域进行栅格化，判断每一个栅格与障碍物的距离，如果距离小于阈值则将此栅格设置为false，反之为true。随后，从起始点所在的栅格开始，将每一个栅格构造成一个节点。节点拥有位置、到达节点所需代价、到达节点的上一个节点的属性。再此基础上设立两个集合，开放集合openset和关闭集合closeset。将起点栅格加入openset，开始遍历操作:首先从openset中找出代价加上启发最小的节点作为当前搜索节点（代价越小说明起点当当前点所需行为越少，启发计算是通过当前点到目标点的距离得到的，因此启发越小表示离目标越近）。判断这个节点是否为终点节点，如果不是，将其从openset删除，加入closeset；从当前节点开始以固定行为进行探索，将其可达并满足一定条件的节点加入openset；新探索的节点的位置为其所在位置，代价为当前节点的代价加上运动代价，上一个节点为当前节点；加入openset需要满足的条件为，节点是可行的（不在范围外，不是障碍点），节点不在openset中或openset中相同节点代价更高。重复上述过程直到当前探索节点为终点节点。此时，从终点节点向前回溯前一个节点，就可以得到最终的路径。以上方法相比于GridDijkstra方法的优点在于可以利用目标点的信息进行搜索导向，使路径搜索更加快速。 -------------------------------------------------------------------------------- /grid_astar/grid_astar.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/python3 2 | # -*- coding: utf-8 -*- 3 | 4 | """ 5 | 6 | grid astar 7 | 8 | author: flztiii 9 | 10 | """ 11 | 12 | import matplotlib.pyplot as plt 13 | import numpy 14 | import math 15 | import copy 16 | 17 | # 点 18 | class Point: 19 | def __init__(self, x, y): 20 | self.x_ = x # x轴坐标 21 | self.y_ = y # y轴坐标 22 | 23 | # 重载减法操作 24 | def __sub__(self, point): 25 | return math.sqrt((self.x_ - point.x_) ** 2 + (self.y_ - point.y_) ** 2) 26 | 27 | # 节点 28 | class Node: 29 | def __init__(self, nx, ny, cost, prev_id): 30 | self.nx_ = nx # x轴下标 31 | self.ny_ = ny # y轴下标 32 | self.cost_ = cost # 代价 33 | self.prev_id_ = prev_id # 前一个节点id 34 | 35 | # 重置等于操作 36 | def __eq__(self, node): 37 | if self.nx_ == node.nx_ and self.ny_ == node.ny_: 38 | return True 39 | else: 40 | return False 41 | 42 | # 重置不等于操作 43 | def __ne__(self, node): 44 | if self.nx_ == node.nx_ and self.ny_ == node.ny_: 45 | return False 46 | else: 47 | return True 48 | 49 | # 行为 50 | class Motion: 51 | def __init__(self, x_mv, y_mv): 52 | self.x_mv_ = x_mv # x轴移动 53 | self.y_mv_ = y_mv # y轴移动 54 | self.cost_ = math.sqrt(x_mv**2 + y_mv**2) # 移动所需代价 55 | 56 | # 设定 57 | class Config: 58 | def __init__(self): 59 | self.resolution_ = 2.0 # 栅格分辨率 60 | self.robot_size_ = 1.0 # 碰撞判断范围 61 | self.motion_ = [ 62 | Motion(1, 0), 63 | Motion(0, 1), 64 | Motion(-1, 0), 65 | Motion(0, -1), 66 | Motion(-1, -1), 67 | Motion(-1, 1), 68 | Motion(1, -1), 69 | Motion(1, 1) 70 | ] # 可以采取的运动行为 71 | 72 | # 工具类 73 | class Tools: 74 | def __init__(self): 75 | return 76 | 77 | # 将点集的x坐标和y坐标分离 78 | @classmethod 79 | def departPoints(self, points): 80 | x = [] 81 | y = [] 82 | for point in points: 83 | x.append(point.x_) 84 | y.append(point.y_) 85 | return x, y 86 | 87 | # 将坐标转化为索引 88 | @classmethod 89 | def posToIndex(self, pos, min_pos, resolution): 90 | return round((pos - min_pos) / resolution) 91 | 92 | # 将索引转化为坐标 93 | @classmethod 94 | def indexToPos(self, index, min_pos, resolution): 95 | return float(index * resolution + min_pos) 96 | 97 | # 计算启发 98 | @classmethod 99 | def calc_heuristic(self, ngoal, node, weight = 1.0): 100 | return weight * math.sqrt((ngoal.nx_ - node.nx_) ** 2 + (ngoal.ny_ - node.ny_) ** 2) 101 | 102 | class AstarPlanner: 103 | def __init__(self, start, goal, obstacles, config): 104 | self.start_ = start # 起点 105 | self.goal_ = goal # 终点 106 | self.obstacles_ = obstacles # 障碍物信息 107 | self.config_ = config # 设置 108 | self.__buildGridMap() # 构造栅格地图 109 | 110 | # 构造栅格地图 111 | def __buildGridMap(self): 112 | # 栅格地图边界 113 | ox, oy = Tools.departPoints(self.obstacles_) 114 | self.gridmap_minx_ = min(ox) 115 | self.gridmap_maxx_ = max(ox) 116 | self.gridmap_miny_ = min(oy) 117 | self.gridmap_maxy_ = max(oy) 118 | print("gridmap min pos_x: ", self.gridmap_minx_) 119 | print("gridmap max pos_x: ", self.gridmap_maxx_) 120 | print("gridmap min pos_y: ", self.gridmap_miny_) 121 | print("gridmap max pos_y: ", self.gridmap_maxy_) 122 | self.gridmap_nxwidth_ = Tools.posToIndex(self.gridmap_maxx_, self.gridmap_minx_, self.config_.resolution_) 123 | self.gridmap_nywidth_ = Tools.posToIndex(self.gridmap_maxy_, self.gridmap_miny_, self.config_.resolution_) 124 | print("gridmap xwidth index: ", self.gridmap_nxwidth_) 125 | print("gridmap ywidth index: ", self.gridmap_nywidth_) 126 | # 初始化栅格地图 127 | gridmap = [[True for j in range(0, self.gridmap_nywidth_)] for i in range(0, self.gridmap_nxwidth_)] 128 | # 判断栅格地图中每一个栅格是否为障碍点 129 | for i in range(0, self.gridmap_nxwidth_): 130 | for j in range(0, self.gridmap_nywidth_): 131 | gridmap_point = Point(Tools.indexToPos(i, self.gridmap_minx_, self.config_.resolution_), Tools.indexToPos(j, self.gridmap_miny_, self.config_.resolution_)) 132 | for obstacle in self.obstacles_: 133 | if gridmap_point - obstacle <= self.config_.robot_size_: 134 | gridmap[i][j] = False 135 | break; 136 | self.gridmap_ = gridmap 137 | 138 | # 进行规划 139 | def planning(self): 140 | # 创建起始节点和目标节点 141 | nstart = Node(Tools.posToIndex(self.start_.x_, self.gridmap_minx_, self.config_.resolution_), Tools.posToIndex(self.start_.y_, self.gridmap_miny_, self.config_.resolution_), 0, -1) 142 | ngoal = Node(Tools.posToIndex(self.goal_.x_, self.gridmap_minx_, self.config_.resolution_), Tools.posToIndex(self.goal_.y_, self.gridmap_miny_, self.config_.resolution_), 0, -1) 143 | # 初始化搜索节点集合和结束搜索节点集合 144 | openset = dict() 145 | closeset = dict() 146 | openset[nstart.ny_ * self.gridmap_nxwidth_ + nstart.nx_] = nstart 147 | # 开始搜索 148 | while True: 149 | # 得到当前搜索节点 150 | current_node_id = min(openset, key=lambda o: openset[o].cost_ + Tools.calc_heuristic(ngoal, openset[o])) 151 | current_node = openset[current_node_id] 152 | # 可视化当前搜索点 153 | plt.plot(Tools.indexToPos(current_node.nx_, self.gridmap_minx_, self.config_.resolution_), Tools.indexToPos(current_node.ny_, self.gridmap_miny_, self.config_.resolution_), "xc") 154 | if len(closeset.keys()) % 10 == 0: 155 | plt.pause(0.001) 156 | # 判断当前搜索节点是否为目标点 157 | if current_node == ngoal: 158 | # 找到目标点 159 | print("Goal Finded") 160 | ngoal.prev_id_ = current_node.prev_id_ 161 | ngoal.cost_ = current_node.cost_ 162 | break 163 | else: 164 | # 未能找到目标点 165 | # 从搜索集合中删除节点并加入搜索完成集合 166 | del openset[current_node_id] 167 | closeset[current_node_id] = current_node 168 | # 遍历当前搜索点采取特定行为后的节点是否可以加入搜索集合 169 | for i in range(0, len(self.config_.motion_)): 170 | motion = self.config_.motion_[i] 171 | next_node = Node(current_node.nx_ + motion.x_mv_, current_node.ny_ + motion.y_mv_, current_node.cost_ + motion.cost_, current_node_id) 172 | next_node_id = next_node.nx_ + next_node.ny_ * self.gridmap_nxwidth_ 173 | # 判断下一个点是否可以为搜索点 174 | # 条件一不是搜索完成的点 175 | if next_node_id in closeset: 176 | continue 177 | # 条件二不是无效点 178 | if not self.verifyNode(next_node): 179 | continue 180 | # 条件三不在openset中或在openset中的代价更大 181 | if not next_node_id in openset: 182 | openset[next_node_id] = next_node 183 | else: 184 | if openset[next_node_id].cost_ > next_node.cost_: 185 | openset[next_node_id] = next_node 186 | # 由目标点得到最终路径 187 | pgoal = Point(Tools.indexToPos(ngoal.nx_, self.gridmap_minx_, self.config_.resolution_), Tools.indexToPos(ngoal.ny_, self.gridmap_miny_, self.config_.resolution_)) 188 | final_path = [pgoal] 189 | prev_node_id = ngoal.prev_id_ 190 | while prev_node_id != -1: 191 | prev_node = closeset[prev_node_id] 192 | pprev_node = Point(Tools.indexToPos(prev_node.nx_, self.gridmap_minx_, self.config_.resolution_), Tools.indexToPos(prev_node.ny_, self.gridmap_miny_, self.config_.resolution_)) 193 | final_path.append(pprev_node) 194 | prev_node_id = prev_node.prev_id_ 195 | final_path.reverse() 196 | return final_path 197 | 198 | # 有效性检测 199 | def verifyNode(self, node): 200 | px = Tools.indexToPos(node.nx_, self.gridmap_minx_, self.config_.resolution_) 201 | py = Tools.indexToPos(node.ny_, self.gridmap_miny_, self.config_.resolution_) 202 | # 判断是否在范围内 203 | if px >= self.gridmap_minx_ and px < self.gridmap_maxx_ and py >= self.gridmap_miny_ and py < self.gridmap_maxy_: 204 | # 判断是否存在障碍物 205 | if self.gridmap_[node.nx_][node.ny_]: 206 | return True 207 | else: 208 | return False 209 | else: 210 | return False 211 | 212 | # 主函数 213 | def main(): 214 | # 设定起点和终点 215 | start = Point(10.0, 10.0) 216 | goal = Point(50.0, 50.0) 217 | # 得到初始设定 218 | config = Config() 219 | # 构建障碍物 220 | obstacles = [] 221 | # 边界障碍物 222 | for ox in range(-10, 61): 223 | for oy in range(-10, 61): 224 | if ox == -10: 225 | obstacles.append(Point(ox, oy)) 226 | elif ox == 60: 227 | obstacles.append(Point(ox, oy)) 228 | elif oy == -10: 229 | obstacles.append(Point(ox, oy)) 230 | elif oy == 60: 231 | obstacles.append(Point(ox, oy)) 232 | elif ox == 20 and oy < 40: 233 | obstacles.append(Point(ox, oy)) 234 | elif ox == 40 and oy > 20: 235 | obstacles.append(Point(ox, oy)) 236 | # 对当前信息进行可视化 237 | obstalces_x, obstacles_y = Tools.departPoints(obstacles) 238 | plt.plot(obstalces_x, obstacles_y, ".k") 239 | plt.plot(start.x_, start.y_, "og") 240 | plt.plot(goal.x_, goal.y_, "xb") 241 | plt.grid(True) 242 | plt.axis("equal") 243 | # 构建A星规划器 244 | astar_planner = AstarPlanner(start, goal, obstacles, config) 245 | planned_path = astar_planner.planning() 246 | # 可视化最终路径 247 | path_x, path_y = Tools.departPoints(planned_path) 248 | plt.plot(path_x, path_y, "-r") 249 | plt.show() 250 | 251 | if __name__ == "__main__": 252 | main() -------------------------------------------------------------------------------- /grid_coverage/GridCoverage: -------------------------------------------------------------------------------- 1 | GridCoverage路径规划方法与其他方法存在本质上的不同，其他方法目的在于生成从起点到终点的无碰撞路径，而本方法的目的在于生成一条遍历整个区域的路径。 2 | GridCoverage方法首先需要确定一个区域，区域由若干个角点定义。计算出每相邻两个角点之间的距离，得到距离最大的两个角点，将其中一个作为栅格坐标系的坐标原点，两个角点的连线作为坐标系x轴方向。然后将全部角点转化到这个坐标系下，进行栅格地图的构建。 3 | 栅格地图的构建需要的参数为栅格中心，栅格的长和宽以及分辨率。栅格中心为栅格坐标系下全部角点的平均值，栅格的长和宽为全部角点在栅格坐标系下最大x轴坐标差和y轴坐标差除以分辨率。在栅格地图中，存在三种坐标系统，第一种是真实坐标，第二种是栅格横纵下标，第三种是存储队列下标，三者之间提供转换的接口，栅格地图提供两种进行修改的接口，一种是根据坐标点进行修改，第二种是给出多边形区域进行修改。 4 | 完成栅格地图构建后，根据区域坐标把栅格地图在区域外的栅格全部设置成1.0，并进行膨胀。之后确定遍历整个区域的方式，代码中选择的是从下到上扫描，从左到右运动的方式。在这种方式下，需要找出上下的区域边界，上边界作为完成遍历的条件，而下边界中最左侧的点作为遍历的起点。每经过一个点，就将这个点的栅格设置为0.5。从起点开始，按设定的运动方式（初始是向右）寻找到下一个点，判断下一个点栅格是否大于0.5，如果不大于说明不是无效区也没有经过，那就将这个点作为新点加入路径，并将这个点作为当前点继续上述过程。如果不大于0.5说明这个点经过过或不是有效区，此时进行上下扫描，判断上一行是否有效，如果有效则移动到上一行最右侧，并将运动反向，继续运动。如果无效则回退一个点进行判断。 -------------------------------------------------------------------------------- /grid_coverage/__pycache__/grid_map_lib.cpython-35.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flztiii/path_planning_basic_algorithm/d1827cbb355416da8a14831eb37a929c5c15e644/grid_coverage/__pycache__/grid_map_lib.cpython-35.pyc -------------------------------------------------------------------------------- /grid_coverage/grid_coverage.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/python3 2 | # -*- coding:utf-8 -*- 3 | 4 | """ 5 | 6 | Grid coverage search 7 | 8 | author: flztiii 9 | 10 | """ 11 | 12 | import matplotlib.pyplot as plt 13 | import numpy as np 14 | import math 15 | import copy 16 | from enum import IntEnum 17 | try: 18 | from grid_map_lib import GridMap 19 | except ImportError: 20 | raise 21 | 22 | # 点 23 | class Point: 24 | def __init__(self, x, y): 25 | self.x_ = x 26 | self.y_ = y 27 | 28 | # 重载减法 29 | def __sub__(self, point): 30 | return np.hypot(self.x_ - point.x_, self.y_ - point.y_) 31 | 32 | # 工具类 33 | class Tools: 34 | # 找出一个多边形最长边的角点和方向，多边形是闭合的 35 | @classmethod 36 | def findPointAndDirection(self, area): 37 | largest_distance = 0.0 38 | largest_distance_index = -1 39 | for i in range(0, len(area) - 1): 40 | current_point = area[i] 41 | next_point = area[(i + 1) % len(area)] 42 | distance = next_point - current_point 43 | if distance > largest_distance: 44 | largest_distance_index = i 45 | largest_distance = distance 46 | coordinate_center = Point(area[largest_distance_index].x_, area[largest_distance_index].y_) 47 | coordinate_vector = Point(area[(largest_distance_index + 1) % len(area)].x_ - area[largest_distance_index].x_, area[(largest_distance_index + 1) % len(area)].y_ - area[largest_distance_index].y_) 48 | return coordinate_center, coordinate_vector 49 | 50 | # 拆除点集的x和y 51 | @classmethod 52 | def departurePoints(self, points): 53 | point_x, point_y = [], [] 54 | for point in points: 55 | point_x.append(point.x_) 56 | point_y.append(point.y_) 57 | return point_x, point_y 58 | 59 | # 进行点的坐标转化 60 | @classmethod 61 | def transformPoint(self, coordinate_center, theta, point): 62 | new_x = (point.y_ - coordinate_center.y_) * np.sin(theta) + (point.x_ - coordinate_center.x_) * np.cos(theta) 63 | new_y = (point.y_ - coordinate_center.y_) * np.cos(theta) - (point.x_ - coordinate_center.x_) * np.sin(theta) 64 | return Point(new_x, new_y) 65 | 66 | # 进行点的还原 67 | @classmethod 68 | def restorePoint(self, coordinate_center, theta, point): 69 | origin_x = point.x_ * np.cos(theta) - point.y_ * np.sin(theta) + coordinate_center.x_ 70 | origin_y = point.x_ * np.sin(theta) + point.y_ * np.cos(theta) + coordinate_center.y_ 71 | return Point(origin_x, origin_y) 72 | 73 | # 进行点集坐标转换 74 | @classmethod 75 | def transformArea(self, area, coordinate_center, coordinate_vector): 76 | theta = np.arctan2(coordinate_vector.y_, coordinate_vector.x_) 77 | new_area = [] 78 | for point in area: 79 | new_point = self.transformPoint(coordinate_center, theta, point) 80 | new_area.append(new_point) 81 | return new_area 82 | 83 | # 进行点集坐标还原 84 | @classmethod 85 | def restoreArea(self, area, coordinate_center, coordinate_vector): 86 | theta = np.arctan2(coordinate_vector.y_, coordinate_vector.x_) 87 | new_area = [] 88 | for point in area: 89 | new_point = self.restorePoint(coordinate_center, theta, point) 90 | new_area.append(new_point) 91 | return new_area 92 | 93 | # 寻找栅格地图的有效边界 94 | @classmethod 95 | def searchBoundary(self, grid_map, to_upper=True): 96 | x_inds = [] 97 | y_ind = None 98 | 99 | # 如果to_upper为true则寻找上边界，如果to_upper为false则寻找下边界 100 | if to_upper: 101 | xrange = range(0, grid_map.width_nx_) 102 | yrange = range(0, grid_map.width_ny_)[::-1] 103 | else: 104 | xrange = range(0, grid_map.width_nx_) 105 | yrange = range(0, grid_map.width_ny_) 106 | 107 | for y_index in yrange: 108 | for x_index in xrange: 109 | if not grid_map.checkOccupationByIndex(x_index, y_index): 110 | y_ind = y_index 111 | x_inds.append(x_index) 112 | if not y_ind is None: 113 | break 114 | return x_inds, y_ind 115 | 116 | # 行为 117 | class Motion: 118 | # 扫描行为 119 | class SweepDirection(IntEnum): 120 | UP = 1 121 | DOWN = -1 122 | # 移动行为 123 | class MoveDirection(IntEnum): 124 | LEFT = -1 125 | RIGHT = 1 126 | 127 | # 规划器 128 | class CoveragePlanner: 129 | 130 | def __init__(self, area, resolution, sweep_direction=Motion.SweepDirection.UP, move_direction=Motion.MoveDirection.RIGHT): 131 | # 基础属性 132 | self.sweep_direction_ = sweep_direction # 扫描方向 133 | self.move_direction_ = move_direction # 移动方向 134 | self.turning_windows_ = [] # 移动行为 135 | self.__updateTurningWindows() # 更新移动行为 136 | # 构建代价地图 137 | self.__buildGridMap(area, resolution) 138 | 139 | # 构建代价地图 140 | def __buildGridMap(self, area, resolution): 141 | """ 构建栅格地图 142 | :param area: 探索区域 143 | :param resolution: 栅格地图分辨率 144 | """ 145 | 146 | # 第一步进行坐标转化 147 | # 找出区域最长边作为新坐标系的x轴 148 | self.coordinate_center_, self.coordinate_vector_ = Tools.findPointAndDirection(area) 149 | # 进行坐标转化 150 | transformed_area = Tools.transformArea(area, self.coordinate_center_, self.coordinate_vector_) 151 | # 初始化栅格地图 152 | self.__initGridMap(transformed_area, resolution) 153 | 154 | # 初始化栅格地图 155 | def __initGridMap(self, polyon, resolution, offset_grid=10): 156 | # 计算栅格的中心点 157 | polyon_x, polyon_y = Tools.departurePoints(polyon) 158 | center_px = np.mean(polyon_x) 159 | center_py = np.mean(polyon_y) 160 | # 计算栅格的x轴和y轴宽度 161 | width_nx = math.ceil((max(polyon_x) - min(polyon_x)) / resolution) + offset_grid 162 | width_ny = math.ceil((max(polyon_y) - min(polyon_y)) / resolution) + offset_grid 163 | # 得到栅格地图 164 | grid_map = GridMap(center_px, center_py, width_nx, width_ny, resolution) 165 | # 对栅格地图进行赋值 166 | grid_map.setGridMapbyPolyon(polyon_x, polyon_y, 1.0, False) 167 | # 进行膨胀 168 | grid_map.expandGrid() 169 | # 判断扫描是从下到上还是从上到下 170 | if self.sweep_direction_ == Motion.SweepDirection.UP: 171 | goal_nxes, goal_ny = Tools.searchBoundary(grid_map, True) 172 | else: 173 | goal_nxes, goal_ny = Tools.searchBoundary(grid_map, False) 174 | # 记录信息 175 | self.grid_map_ = grid_map 176 | self.goal_nxes_ = goal_nxes 177 | self.goal_ny_ = goal_ny 178 | 179 | # 更新移动行为 180 | def __updateTurningWindows(self): 181 | turning_windows = [ 182 | Point(self.move_direction_, 0), 183 | Point(self.move_direction_, self.sweep_direction_), 184 | Point(-self.move_direction_, self.sweep_direction_), 185 | Point(0, self.sweep_direction_), 186 | ] 187 | self.turning_windows_ = turning_windows 188 | 189 | # 开始规划 190 | def planning(self): 191 | # 首先找到起点 192 | start_nx, start_ny = None, None 193 | if self.sweep_direction_ == Motion.SweepDirection.UP: 194 | start_nxes, start_ny = Tools.searchBoundary(self.grid_map_, False) 195 | else: 196 | start_nxes, start_ny = Tools.searchBoundary(self.grid_map_, True) 197 | if self.move_direction_ == Motion.MoveDirection.RIGHT: 198 | start_nx = min(start_nxes) 199 | else: 200 | start_nx = max(start_nxes) 201 | nstart = Point(start_nx, start_ny) 202 | # 判断起点是否有效 203 | if not self.grid_map_.setGridMapValueByIndex(start_nx, start_ny, 0.5): 204 | print("start is not setable") 205 | return None 206 | # 将起始点设置为当前点 207 | ncurrent = copy.deepcopy(nstart) 208 | pcurrent = Point(*self.grid_map_.gridMapIndexToPointPos(ncurrent.x_, ncurrent.y_)) 209 | # 生成路径 210 | planned_path = [pcurrent] 211 | # 可视化 212 | # fig, ax = plt.subplots() 213 | # 从当前点开始移动 214 | while True: 215 | # 得到新点 216 | ncurrent = self.searchNextNode(ncurrent) 217 | # 判断新点是否存在 218 | if ncurrent is None: 219 | print("path cannot find") 220 | break 221 | # 计算当前点的坐标 222 | pcurrent = Point(*self.grid_map_.gridMapIndexToPointPos(ncurrent.x_, ncurrent.y_)) 223 | planned_path.append(pcurrent) 224 | # 更新栅格地图 225 | self.grid_map_.setGridMapValueByIndex(ncurrent.x_, ncurrent.y_, 0.5) 226 | # 可视化 227 | # self.grid_map_.plotGridMap(ax) 228 | # plt.pause(1.0) 229 | # 判断是否到达终点 230 | if self.__checkFinished(): 231 | print("Done") 232 | break 233 | self.grid_map_.plotGridMap() 234 | # 转化为原始坐标系 235 | final_path = Tools.restoreArea(planned_path, self.coordinate_center_, self.coordinate_vector_) 236 | return final_path 237 | # 寻找下一个点 238 | def searchNextNode(self, ncurrent): 239 | # 得到下一个点位置 240 | nnext = Point(ncurrent.x_ + self.move_direction_, ncurrent.y_) 241 | # 判断下一个点是否可以 242 | if not self.grid_map_.checkOccupationByIndex(nnext.x_, nnext.y_, 0.5): 243 | # 可以，直接返回 244 | return nnext 245 | else: 246 | # 不可以，进一步处理 247 | # 判断turning_windows中是否存在可以的 248 | for turning_window in self.turning_windows_: 249 | # 得到转向后的点 250 | nnext = Point(ncurrent.x_ + turning_window.x_, ncurrent.y_ + turning_window.y_) 251 | # 判断是否可以 252 | if not self.grid_map_.checkOccupationByIndex(nnext.x_, nnext.y_, 0.5): 253 | # 可以 254 | while not self.grid_map_.checkOccupationByIndex(nnext.x_ + self.move_direction_, nnext.y_, 0.5): 255 | nnext = Point(nnext.x_ + self.move_direction_, nnext.y_) 256 | self.__swapMotion() 257 | return nnext 258 | # 不可以 259 | nnext = Point(ncurrent.x_ - self.move_direction_, ncurrent.y_) 260 | if self.grid_map_.checkOccupationByIndex(nnext.x_, nnext.y_): 261 | return None 262 | else: 263 | return nnext 264 | 265 | # 更新运动 266 | def __swapMotion(self): 267 | self.move_direction_ *= -1 268 | self.__updateTurningWindows() 269 | 270 | # 判断是否结束 271 | def __checkFinished(self): 272 | for x_inx in self.goal_nxes_: 273 | if not self.grid_map_.checkOccupationByIndex(x_inx, self.goal_ny_, 0.5): 274 | return False 275 | return True 276 | 277 | # 主函数 278 | def main(): 279 | # 初始化区域角点 280 | area = [ 281 | Point(0.0, 0.0), 282 | Point(20.0, -20.0), 283 | Point(50.0, 0.0), 284 | Point(100.0, 30.0), 285 | Point(130.0, 60.0), 286 | Point(40.0, 80.0), 287 | Point(0.0, 0.0), 288 | ] 289 | # 初始化分辨率 290 | resolution = 5.0 291 | 292 | # 构建规划器 293 | coverage_planner = CoveragePlanner(area, resolution) 294 | planned_path = coverage_planner.planning() 295 | px, py = Tools.departurePoints(planned_path) 296 | ox, oy = Tools.departurePoints(area) 297 | for ipx, ipy in zip(px, py): 298 | plt.cla() 299 | plt.plot(ox, oy, "-xb") 300 | plt.plot(px, py, "-r") 301 | plt.plot(ipx, ipy, "or") 302 | plt.axis("equal") 303 | plt.grid(True) 304 | plt.pause(0.1) 305 | 306 | plt.cla() 307 | plt.plot(ox, oy, "-xb") 308 | plt.plot(px, py, "-r") 309 | plt.axis("equal") 310 | plt.grid(True) 311 | plt.pause(0.1) 312 | plt.show() 313 | 314 | if __name__ == "__main__": 315 | main() -------------------------------------------------------------------------------- /grid_coverage/grid_map_lib.py: -------------------------------------------------------------------------------- 1 | #! /sur/bin/python3 2 | # -*- coding: utf-8 -*- 3 | 4 | """ 5 | 6 | Grid map library in python 7 | 8 | author: flztiii 9 | 10 | """ 11 | 12 | import matplotlib.pyplot as plt 13 | import numpy as np 14 | 15 | # 栅格地图 16 | class GridMap: 17 | def __init__(self, center_px, center_py, width_nx, width_ny, resolution, init_val=0.0): 18 | self.center_px_ = center_px # 栅格中心x轴坐标 19 | self.center_py_ = center_py # 栅格中心y轴坐标 20 | self.width_nx_ = width_nx # 栅格在x轴的个数 21 | self.width_ny_ = width_ny # 栅格在y轴的个数 22 | self.resolution_ = resolution # 栅格的分辨率 23 | self.min_px_ = center_px - width_nx * resolution * 0.5 # 栅格在x轴的最小坐标 24 | self.min_py_ = center_py - width_ny * resolution * 0.5 # 栅格在y轴的最小坐标 25 | self.ndata_ = self.width_nx_ * self.width_ny_ # 栅格地图数据的总长度 26 | self.data_ = [init_val] * self.ndata_ # 栅格地图数据 27 | 28 | # 坐标与栅格转换 29 | def posToGridMapIndex(self, pos, min_pos, max_index): 30 | """ 将单个坐标信息转化为栅格的下标 31 | :param pos: 输入坐标 32 | :param min_pos: 最小坐标 33 | :param max_index: 最大下标 34 | """ 35 | 36 | index = int(np.floor((pos - min_pos) / self.resolution_)) 37 | if index >= 0 and index < max_index: 38 | return index 39 | else: 40 | return None 41 | 42 | def pointPosToGridMapIndex(self, pos_x, pos_y): 43 | """ 将坐标转化为对应栅格 44 | :param pos_x: x轴坐标 45 | :param pos_y: y轴坐标 46 | """ 47 | 48 | index_x = self.posToGridMapIndex(pos_x, self.min_px_, self.width_nx_) 49 | index_y = self.posToGridMapIndex(pos_y, self.min_py_, self.width_ny_) 50 | return index_x, index_y 51 | 52 | def gridMapIndexToPos(self, index, min_pos): 53 | """ 将栅格单个下标转化为栅格中心坐标 54 | :param index: 栅格单个下标 55 | :param min_pos: 栅格最小坐标 56 | """ 57 | 58 | return index * self.resolution_ + min_pos + self.resolution_ * 0.5 59 | 60 | def gridMapIndexToPointPos(self, index_x, index_y): 61 | """ 将栅格转化为栅格中心坐标 62 | :param index_x: 栅格x轴下标 63 | :param index_y: 栅格y轴下标 64 | """ 65 | 66 | pos_x = self.gridMapIndexToPos(index_x, self.min_px_) 67 | pos_y = self.gridMapIndexToPos(index_y, self.min_py_) 68 | return pos_x, pos_y 69 | 70 | def gridMapIndexToDataIndex(self, index_x, index_y): 71 | """ 将栅格下标转化为数据下标 72 | :param index_x: 栅格x轴下标 73 | :param index_y: 栅格y轴下标 74 | """ 75 | 76 | return index_x + index_y * self.width_nx_ 77 | 78 | # 读取和修改栅格地图 79 | def getGridMapValueByIndex(self, index_x, index_y): 80 | """ 根据给出的栅格下标读取栅格地图 81 | :param index_x: 栅格x轴下标 82 | :param index_y: 栅格y轴下标 83 | """ 84 | 85 | data_index = self.gridMapIndexToDataIndex(index_x, index_y) 86 | if data_index >=0 and data_index < self.ndata_: 87 | return self.data_[data_index] 88 | else: 89 | return None 90 | 91 | def setGridMapValueByIndex(self, index_x, index_y, value): 92 | """ 根据给出的栅格下标修改栅格地图 93 | :param index_x: 栅格x轴下标 94 | :param index_y: 栅格y轴下标 95 | :param value: 修改的值 96 | """ 97 | 98 | # 首先确定输入的是有效值 99 | if (index_x is None) or (index_y is None): 100 | print(index_x, index_y) 101 | return False, False 102 | # 修改对应值 103 | data_index = self.gridMapIndexToDataIndex(index_x, index_y) 104 | if data_index >=0 and data_index < self.ndata_: 105 | self.data_[data_index] = value 106 | return True 107 | else: 108 | return False 109 | 110 | def setGridMapValueByPos(self, pos_x, pos_y, value): 111 | """ 根据给出的坐标点修改栅格地图 112 | :param pos_x: x轴坐标 113 | :param pos_y: y轴坐标 114 | :param value: 修改的值 115 | """ 116 | 117 | # 首先找到坐标对应的栅格下标 118 | index_x, index_y = self.pointPosToGridMapIndex(pos_x, pos_y) 119 | # 验证转换是否有效 120 | if (index_x is None) or (index_y is None): 121 | return False 122 | # 设置值 123 | return self.setGridMapValueByIndex(index_x, index_y, value) 124 | 125 | def setGridMapbyPolyon(self, polyon_x, polyon_y, value, inside=True): 126 | """ 根据给出的多边形对栅格地图进行修改 127 | :param polyon_x: 多边形的横坐标 128 | :param polyon_y： 多边形的纵坐标 129 | :param value: 要修改的值 130 | :param inside: 修改的是多边形内还是多边形外 131 | """ 132 | 133 | # 首先判断多边形是否闭合 134 | if polyon_x[0] != polyon_x[-1] or polyon_y[0] != polyon_y[-1]: 135 | # 如果多边形非闭合，则将其进行闭合 136 | polyon_x.append(polyon_x[0]) 137 | polyon_y.append(polyon_y[0]) 138 | # 开始遍历每一个栅格中心是否在多边形内 139 | for index_x in range(0, self.width_nx_): 140 | for index_y in range(0, self.width_ny_): 141 | # 计算栅格中心坐标 142 | pos_x, pos_y = self.gridMapIndexToPointPos(index_x, index_y) 143 | # 判断点是否在多边形内 144 | is_inside = self.isInPolyon(polyon_x, polyon_y, pos_x, pos_y) 145 | if is_inside is inside: 146 | self.setGridMapValueByIndex(index_x, index_y, value) 147 | 148 | @staticmethod 149 | def isInPolyon(polyon_x, polyon_y, pos_x, pos_y): 150 | """ 判断点是否在多边形内 151 | 采用的方法是射线法，从当前引出任意一条射线，如果与多边形交点个数为奇数个则点在多边形内，反之点不在多边形内 152 | :param polyon_x: 多边形的x轴坐标 153 | :param polyon_y: 多边形的y轴坐标 154 | :param pos_x: 点的x坐标 155 | :param pos_y: 点的y坐标 156 | """ 157 | 158 | # 遍历多边形每一条边, 多边形必须是闭合的（即第一个点就是最后一个点） 159 | is_inside = False 160 | for current_point_index in range(0, len(polyon_x) - 1): 161 | neighbor_point_index = (current_point_index + 1) % len(polyon_x) 162 | # 判断pos_x是不是在两个点的x坐标之间 163 | if pos_x > min(polyon_x[current_point_index], polyon_x[neighbor_point_index]) and pos_x < max(polyon_x[current_point_index], polyon_x[neighbor_point_index]): 164 | # 在之间 165 | # 判断射线是否相交 166 | if (polyon_y[neighbor_point_index] - polyon_y[current_point_index]) / (polyon_x[neighbor_point_index] - polyon_x[current_point_index]) * (pos_x - polyon_x[current_point_index]) + polyon_y[current_point_index] - pos_y > 0.0: 167 | # 相交 168 | is_inside = not is_inside 169 | else: 170 | # 不在之间 171 | continue 172 | return is_inside 173 | 174 | def checkOccupationByIndex(self, index_x, index_y, occupied_value=1.0): 175 | """ 判断栅格地图对应下标位置的值是否大于occupied_value 176 | :param index_x: 栅格x轴下标 177 | :param index_y: 栅格y轴下标 178 | :param occupied_value: 比较的值 179 | """ 180 | 181 | value = self.getGridMapValueByIndex(index_x, index_y) 182 | if value >= occupied_value: 183 | return True 184 | else: 185 | return False 186 | 187 | def expandGrid(self): 188 | """ 扩展栅格地图中的有效值范围 189 | """ 190 | 191 | # 遍历栅格，判断栅格是否有效 192 | checked_index_x, checked_index_y = [], [] 193 | for index_x in range(0, self.width_nx_): 194 | for index_y in range(0, self.width_ny_): 195 | if self.checkOccupationByIndex(index_x, index_y): 196 | checked_index_x.append(index_x) 197 | checked_index_y.append(index_y) 198 | 199 | # 将有效栅格的8邻域进行有效化 200 | for index_x, index_y in zip(checked_index_x, checked_index_y): 201 | self.setGridMapValueByIndex(index_x, index_y + 1, 1.0) 202 | self.setGridMapValueByIndex(index_x + 1, index_y, 1.0) 203 | self.setGridMapValueByIndex(index_x, index_y - 1, 1.0) 204 | self.setGridMapValueByIndex(index_x - 1, index_y, 1.0) 205 | self.setGridMapValueByIndex(index_x + 1, index_y + 1, 1.0) 206 | # self.setGridMapValueByIndex(index_x + 1, index_y - 1, 1.0) 207 | self.setGridMapValueByIndex(index_x - 1, index_y - 1, 1.0) 208 | # self.setGridMapValueByIndex(index_x - 1, index_y + 1, 1.0) 209 | 210 | # 进行可视化 211 | def plotGridMap(self, ax=None): 212 | """ 将栅格地图进行可视化 213 | """ 214 | 215 | visual_data = np.reshape(np.array(self.data_), (self.width_ny_, self.width_nx_)) 216 | if not ax: 217 | fig, ax = plt.subplots() 218 | heat_map = ax.pcolor(visual_data, cmap="Blues", vmin=0.0, vmax=1.0) 219 | plt.axis("equal") 220 | # plt.show() 221 | 222 | return heat_map 223 | 224 | # 测试函数 225 | def testPositionSet(): 226 | grid_map = GridMap(10.0, -0.5, 100, 120, 0.5) 227 | 228 | grid_map.setGridMapValueByPos(10.1, -1.1, 1.0) 229 | grid_map.setGridMapValueByPos(10.1, -0.1, 1.0) 230 | grid_map.setGridMapValueByPos(10.1, 1.1, 1.0) 231 | grid_map.setGridMapValueByPos(11.1, 0.1, 1.0) 232 | grid_map.setGridMapValueByPos(10.1, 0.1, 1.0) 233 | grid_map.setGridMapValueByPos(9.1, 0.1, 1.0) 234 | 235 | grid_map.plotGridMap() 236 | 237 | # 测试函数 238 | def testPolygonSet(): 239 | ox = [0.0, 20.0, 50.0, 100.0, 130.0, 40.0] 240 | oy = [0.0, -20.0, 0.0, 30.0, 60.0, 80.0] 241 | 242 | grid_map = GridMap(60.0, 30.5, 600, 290, 0.7) 243 | 244 | grid_map.setGridMapbyPolyon(ox, oy, 1.0, inside=False) 245 | 246 | grid_map.plotGridMap() 247 | 248 | plt.axis("equal") 249 | plt.grid(True) 250 | 251 | # 主函数 252 | def main(): 253 | print("Test grid_map start") 254 | testPositionSet() 255 | testPolygonSet() 256 | plt.show() 257 | print("Done") 258 | 259 | if __name__ == "__main__": 260 | main() -------------------------------------------------------------------------------- /grid_dijkstra/Grid_Dijkstra: -------------------------------------------------------------------------------- 1 | Grid_Dijkstra方法是一种基于栅格地图进行搜索的路径规划方法，其所找出的是在固定行为下，从起点到终点的最短路径，是仅仅根据距离进行判断的方法。 2 | 方法首先找到一个矩形区域，包含全部的障碍物和起终点。根据一定的分辨率，将此区域进行栅格化，判断每一个栅格与障碍物的距离，如果距离小于阈值则将此栅格设置为false，反之为true。随后，从起始点所在的栅格开始，将每一个栅格构造成一个节点。节点拥有位置、到达节点所需代价、到达节点的上一个节点的属性。再此基础上设立两个集合，开放集合openset和关闭集合closeset。将起点栅格加入openset，开始遍历操作:从openset取出代价最低的节点，作为当前进行探索的节点，判断这个节点是否为终点节点，如果不是，将其从openset删除，加入closeset；从当前节点开始以固定行为进行探索，将其可达并满足一定条件的节点加入openset；新探索的节点的位置为其所在位置，代价为当前节点的代价加上运动代价，上一个节点为当前节点；加入openset需要满足的条件为，节点是可行的（不在范围外，不是障碍点），节点不在openset中或openset中相同节点代价更高。重复上述过程直到当前探索节点为终点节点。此时，从终点节点向前回溯前一个节点，就可以得到最终的路径。 -------------------------------------------------------------------------------- /grid_dijkstra/grid_dijkstra.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/python3 2 | # -*- coding: utf-8 -*- 3 | 4 | """ 5 | 6 | grid dijkstra 7 | 8 | author: flztiii 9 | 10 | """ 11 | 12 | import matplotlib.pyplot as plt 13 | import numpy 14 | import math 15 | import copy 16 | 17 | # 点 18 | class Point: 19 | def __init__(self, x, y): 20 | self.x_ = x # x轴坐标 21 | self.y_ = y # y轴坐标 22 | 23 | # 重载减法操作 24 | def __sub__(self, point): 25 | return math.sqrt((self.x_ - point.x_) ** 2 + (self.y_ - point.y_) ** 2) 26 | 27 | # 节点 28 | class Node: 29 | def __init__(self, nx, ny, cost, prev_id): 30 | self.nx_ = nx # x轴下标 31 | self.ny_ = ny # y轴下标 32 | self.cost_ = cost # 代价 33 | self.prev_id_ = prev_id # 前一个节点id 34 | 35 | # 重置等于操作 36 | def __eq__(self, node): 37 | if self.nx_ == node.nx_ and self.ny_ == node.ny_: 38 | return True 39 | else: 40 | return False 41 | 42 | # 重置不等于操作 43 | def __ne__(self, node): 44 | if self.nx_ == node.nx_ and self.ny_ == node.ny_: 45 | return False 46 | else: 47 | return True 48 | 49 | # 行为 50 | class Motion: 51 | def __init__(self, x_mv, y_mv): 52 | self.x_mv_ = x_mv # x轴移动 53 | self.y_mv_ = y_mv # y轴移动 54 | self.cost_ = math.sqrt(x_mv**2 + y_mv**2) # 移动所需代价 55 | 56 | # 设定 57 | class Config: 58 | def __init__(self): 59 | self.resolution_ = 2.0 # 栅格分辨率 60 | self.robot_size_ = 1.0 # 碰撞判断范围 61 | self.motion_ = [ 62 | Motion(0, 1), 63 | Motion(1, 0), 64 | Motion(1, 1), 65 | Motion(0, -1), 66 | Motion(-1, 0), 67 | Motion(-1, -1), 68 | Motion(1, -1), 69 | Motion(-1, 1) 70 | ] # 可以采取的运动行为 71 | 72 | # 工具类 73 | class Tools: 74 | def __init__(self): 75 | return 76 | 77 | # 将点集的x坐标和y坐标分离 78 | @classmethod 79 | def departPoints(self, points): 80 | x = [] 81 | y = [] 82 | for point in points: 83 | x.append(point.x_) 84 | y.append(point.y_) 85 | return x, y 86 | 87 | # 将坐标转化为索引 88 | @classmethod 89 | def posToIndex(self, pos, min_pos, resolution): 90 | return round((pos - min_pos) / resolution) 91 | 92 | # 将索引转化为坐标 93 | @classmethod 94 | def indexToPos(self, index, min_pos, resolution): 95 | return float(index * resolution + min_pos) 96 | 97 | class DijkstraPlanner: 98 | def __init__(self, start, goal, obstacles, config): 99 | self.start_ = start # 起点 100 | self.goal_ = goal # 终点 101 | self.obstacles_ = obstacles # 障碍物信息 102 | self.config_ = config # 设置 103 | self.__buildGridMap() # 构造栅格地图 104 | 105 | # 构造栅格地图 106 | def __buildGridMap(self): 107 | # 栅格地图边界 108 | ox, oy = Tools.departPoints(self.obstacles_) 109 | self.gridmap_minx_ = min(ox) 110 | self.gridmap_maxx_ = max(ox) 111 | self.gridmap_miny_ = min(oy) 112 | self.gridmap_maxy_ = max(oy) 113 | print("gridmap min pos_x: ", self.gridmap_minx_) 114 | print("gridmap max pos_x: ", self.gridmap_maxx_) 115 | print("gridmap min pos_y: ", self.gridmap_miny_) 116 | print("gridmap max pos_y: ", self.gridmap_maxy_) 117 | self.gridmap_nxwidth_ = Tools.posToIndex(self.gridmap_maxx_, self.gridmap_minx_, self.config_.resolution_) 118 | self.gridmap_nywidth_ = Tools.posToIndex(self.gridmap_maxy_, self.gridmap_miny_, self.config_.resolution_) 119 | print("gridmap xwidth index: ", self.gridmap_nxwidth_) 120 | print("gridmap ywidth index: ", self.gridmap_nywidth_) 121 | # 初始化栅格地图 122 | gridmap = [[True for j in range(0, self.gridmap_nywidth_)] for i in range(0, self.gridmap_nxwidth_)] 123 | # 判断栅格地图中每一个栅格是否为障碍点 124 | for i in range(0, self.gridmap_nxwidth_): 125 | for j in range(0, self.gridmap_nywidth_): 126 | gridmap_point = Point(Tools.indexToPos(i, self.gridmap_minx_, self.config_.resolution_), Tools.indexToPos(j, self.gridmap_miny_, self.config_.resolution_)) 127 | for obstacle in self.obstacles_: 128 | if gridmap_point - obstacle <= self.config_.robot_size_: 129 | gridmap[i][j] = False 130 | break; 131 | self.gridmap_ = gridmap 132 | 133 | # 进行规划 134 | def planning(self): 135 | # 创建起始节点和目标节点 136 | nstart = Node(Tools.posToIndex(self.start_.x_, self.gridmap_minx_, self.config_.resolution_), Tools.posToIndex(self.start_.y_, self.gridmap_miny_, self.config_.resolution_), 0, -1) 137 | ngoal = Node(Tools.posToIndex(self.goal_.x_, self.gridmap_minx_, self.config_.resolution_), Tools.posToIndex(self.goal_.y_, self.gridmap_miny_, self.config_.resolution_), 0, -1) 138 | # 初始化搜索节点集合和结束搜索节点集合 139 | openset = dict() 140 | closeset = dict() 141 | openset[nstart.ny_ * self.gridmap_nxwidth_ + nstart.nx_] = nstart 142 | # 开始搜索 143 | while True: 144 | # 得到当前搜索节点 145 | current_node_id = min(openset, key=lambda o: openset[o].cost_) 146 | current_node = openset[current_node_id] 147 | # 可视化当前搜索点 148 | plt.plot(Tools.indexToPos(current_node.nx_, self.gridmap_minx_, self.config_.resolution_), Tools.indexToPos(current_node.ny_, self.gridmap_miny_, self.config_.resolution_), "xc") 149 | if len(closeset.keys()) % 10 == 0: 150 | plt.pause(0.001) 151 | # 判断当前搜索节点是否为目标点 152 | if current_node == ngoal: 153 | # 找到目标点 154 | print("Goal Finded") 155 | ngoal.prev_id_ = current_node.prev_id_ 156 | ngoal.cost_ = current_node.cost_ 157 | break 158 | else: 159 | # 未能找到目标点 160 | # 从搜索集合中删除节点并加入搜索完成集合 161 | del openset[current_node_id] 162 | closeset[current_node_id] = current_node 163 | # 遍历当前搜索点采取特定行为后的节点是否可以加入搜索集合 164 | for i in range(0, len(self.config_.motion_)): 165 | motion = self.config_.motion_[i] 166 | next_node = Node(current_node.nx_ + motion.x_mv_, current_node.ny_ + motion.y_mv_, current_node.cost_ + motion.cost_, current_node_id) 167 | next_node_id = next_node.nx_ + next_node.ny_ * self.gridmap_nxwidth_ 168 | # 判断下一个点是否可以为搜索点 169 | # 条件一不是搜索完成的点 170 | if next_node_id in closeset: 171 | continue 172 | # 条件二不是无效点 173 | if not self.verifyNode(next_node): 174 | continue 175 | # 条件三不在openset中或在openset中的代价更大 176 | if not next_node_id in openset: 177 | openset[next_node_id] = next_node 178 | else: 179 | if openset[next_node_id].cost_ >= next_node.cost_: 180 | openset[next_node_id] = next_node 181 | # 由目标点得到最终路径 182 | pgoal = Point(Tools.indexToPos(ngoal.nx_, self.gridmap_minx_, self.config_.resolution_), Tools.indexToPos(ngoal.ny_, self.gridmap_miny_, self.config_.resolution_)) 183 | final_path = [pgoal] 184 | prev_node_id = ngoal.prev_id_ 185 | while prev_node_id != -1: 186 | prev_node = closeset[prev_node_id] 187 | pprev_node = Point(Tools.indexToPos(prev_node.nx_, self.gridmap_minx_, self.config_.resolution_), Tools.indexToPos(prev_node.ny_, self.gridmap_miny_, self.config_.resolution_)) 188 | final_path.append(pprev_node) 189 | prev_node_id = prev_node.prev_id_ 190 | final_path.reverse() 191 | return final_path 192 | 193 | # 有效性检测 194 | def verifyNode(self, node): 195 | px = Tools.indexToPos(node.nx_, self.gridmap_minx_, self.config_.resolution_) 196 | py = Tools.indexToPos(node.ny_, self.gridmap_miny_, self.config_.resolution_) 197 | # 判断是否在范围内 198 | if px >= self.gridmap_minx_ and px < self.gridmap_maxx_ and py >= self.gridmap_miny_ and py < self.gridmap_maxy_: 199 | # 判断是否存在障碍物 200 | if self.gridmap_[node.nx_][node.ny_]: 201 | return True 202 | else: 203 | return False 204 | else: 205 | return False 206 | 207 | # 主函数 208 | def main(): 209 | # 设定起点和终点 210 | start = Point(-5.0, -5.0) 211 | goal = Point(50.0, 50.0) 212 | # 得到初始设定 213 | config = Config() 214 | # 构建障碍物 215 | obstacles = [] 216 | # 边界障碍物 217 | for ox in range(-10, 61): 218 | for oy in range(-10, 61): 219 | if ox == -10: 220 | obstacles.append(Point(ox, oy)) 221 | elif ox == 60: 222 | obstacles.append(Point(ox, oy)) 223 | elif oy == -10: 224 | obstacles.append(Point(ox, oy)) 225 | elif oy == 60: 226 | obstacles.append(Point(ox, oy)) 227 | elif ox == 20 and oy < 40: 228 | obstacles.append(Point(ox, oy)) 229 | elif ox == 40 and oy > 20: 230 | obstacles.append(Point(ox, oy)) 231 | # 对当前信息进行可视化 232 | obstalces_x, obstacles_y = Tools.departPoints(obstacles) 233 | plt.plot(obstalces_x, obstacles_y, ".k") 234 | plt.plot(start.x_, start.y_, "og") 235 | plt.plot(goal.x_, goal.y_, "xb") 236 | plt.grid(True) 237 | plt.axis("equal") 238 | # 构建Dijkstra规划器 239 | dijkstra_planner = DijkstraPlanner(start, goal, obstacles, config) 240 | planned_path = dijkstra_planner.planning() 241 | # 可视化最终路径 242 | path_x, path_y = Tools.departPoints(planned_path) 243 | plt.plot(path_x, path_y, "-r") 244 | plt.show() 245 | 246 | if __name__ == "__main__": 247 | main() -------------------------------------------------------------------------------- /hybrid_astar/hybrid_astar.md: -------------------------------------------------------------------------------- 1 | 混合a星与a星的思路基本相同.第一步构建栅格地图,但是构建的栅格地图是3维的,而不是2维的,包括了x,y和yaw.以一定分辨率构建好栅格地图后,从起点开始先运行一次2维的a星搜索,得到相同x,y坐标对应的损失.之后,仍然从起点开始,将其加入到开集合中.从开集合中得到损失最小的节点作为当前节点,判断其是否可以与终点规划出一条无碰撞且可行的轨迹,如果可以,则搜索结束,得到最终路径.如果不行,则将当前节点从开集合中删除,加入到闭集合中.随后,遍历当前节点的邻居节点(邻居由三个维度的行为持续一定时间产生的),如果邻居不在闭集合中,且比开集合中相同节点损失更小,则加入开集合,重复上述过程直到找到终点.如果最后开集合没有元素仍然没有找到终点,则返回搜索失败. -------------------------------------------------------------------------------- /model_predictive_path_follower/model_predictive_path_follower.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/python 2 | #! -*- coding: utf-8 -*- 3 | 4 | """ 5 | 6 | model predictive path following 7 | author: flztiii 8 | 9 | """ 10 | 11 | import sys 12 | sys.path.append("./cubic_spline/") 13 | try: 14 | import cubic_spline 15 | except: 16 | raise 17 | import numpy as np 18 | import matplotlib.pyplot as plt 19 | import cvxpy 20 | import copy 21 | 22 | # 全局变量 23 | DL = 1.0 # [m] 轨迹点采样间距 24 | DT = 0.2 # [s] time tick 25 | T = 6 # horizon length 26 | NX = 4 # x = x, y, v, yaw 27 | NU = 2 # a = [accel, steer] 28 | 29 | GOAL_DIS = 1.5 # goal distance 30 | STOP_SPEED = 0.5 / 3.6 # stop speed 31 | MAX_TIME = 200.0 # max simulation time 32 | 33 | # iterative paramter 34 | MAX_ITER = 3 # Max iteration 35 | DU_TH = 0.1 # iteration finish param 36 | 37 | TARGET_SPEED = 10.0 / 3.6 # [m/s] target speed 38 | N_IND_SEARCH = 10 # Search index number 39 | 40 | # Vehicle parameters 41 | LENGTH = 4.5 # [m] 42 | WIDTH = 2.0 # [m] 43 | BACKTOWHEEL = 1.0 # [m] 44 | WHEEL_LEN = 0.3 # [m] 45 | WHEEL_WIDTH = 0.2 # [m] 46 | TREAD = 0.7 # [m] 47 | WB = 2.5 # [m] 48 | 49 | MAX_STEER = np.deg2rad(45.0) # maximum steering angle [rad] 50 | MAX_DSTEER = np.deg2rad(30.0) # maximum steering speed [rad/s] 51 | MAX_SPEED = 55.0 / 3.6 # maximum speed [m/s] 52 | MIN_SPEED = -20.0 / 3.6 # minimum speed [m/s] 53 | MAX_ACCEL = 1.0 # maximum accel [m/ss] 54 | 55 | def get_nparray_from_matrix(x): 56 | return np.array(x).flatten() 57 | 58 | def pi_2_pi(angle): 59 | while(angle > np.pi): 60 | angle = angle - 2.0 * np.pi 61 | 62 | while(angle < -np.pi): 63 | angle = angle + 2.0 * np.pi 64 | 65 | return angle 66 | 67 | def smoothYaw(yaw): 68 | for i in range(len(yaw) - 1): 69 | dyaw = yaw[i + 1] - yaw[i] 70 | 71 | while dyaw >= np.pi / 2.0: 72 | yaw[i + 1] -= np.pi * 2.0 73 | dyaw = yaw[i + 1] - yaw[i] 74 | 75 | while dyaw <= -np.pi / 2.0: 76 | yaw[i + 1] += np.pi * 2.0 77 | dyaw = yaw[i + 1] - yaw[i] 78 | 79 | return yaw 80 | 81 | # 判断是否到达终点 82 | def checkGoal(state, goal_x, goal_y): 83 | goal_dis = np.sqrt((state[0] - goal_x)**2 + (state[1] - goal_y)**2) 84 | if goal_dis < GOAL_DIS and np.abs(state[3]) < STOP_SPEED: 85 | return True 86 | else: 87 | return False 88 | 89 | def plotCar(x, y, yaw, steer=0.0, cabcolor="-r", truckcolor="-k"): # pragma: no cover 90 | 91 | outline = np.array([[-BACKTOWHEEL, (LENGTH - BACKTOWHEEL), (LENGTH - BACKTOWHEEL), -BACKTOWHEEL, -BACKTOWHEEL], 92 | [WIDTH / 2, WIDTH / 2, - WIDTH / 2, -WIDTH / 2, WIDTH / 2]]) 93 | 94 | fr_wheel = np.array([[WHEEL_LEN, -WHEEL_LEN, -WHEEL_LEN, WHEEL_LEN, WHEEL_LEN], 95 | [-WHEEL_WIDTH - TREAD, -WHEEL_WIDTH - TREAD, WHEEL_WIDTH - TREAD, WHEEL_WIDTH - TREAD, -WHEEL_WIDTH - TREAD]]) 96 | 97 | rr_wheel = np.copy(fr_wheel) 98 | 99 | fl_wheel = np.copy(fr_wheel) 100 | fl_wheel[1, :] *= -1 101 | rl_wheel = np.copy(rr_wheel) 102 | rl_wheel[1, :] *= -1 103 | 104 | Rot1 = np.array([[np.cos(yaw), np.sin(yaw)], 105 | [-np.sin(yaw), np.cos(yaw)]]) 106 | Rot2 = np.array([[np.cos(steer), np.sin(steer)], 107 | [-np.sin(steer), np.cos(steer)]]) 108 | 109 | fr_wheel = (fr_wheel.T.dot(Rot2)).T 110 | fl_wheel = (fl_wheel.T.dot(Rot2)).T 111 | fr_wheel[0, :] += WB 112 | fl_wheel[0, :] += WB 113 | 114 | fr_wheel = (fr_wheel.T.dot(Rot1)).T 115 | fl_wheel = (fl_wheel.T.dot(Rot1)).T 116 | 117 | outline = (outline.T.dot(Rot1)).T 118 | rr_wheel = (rr_wheel.T.dot(Rot1)).T 119 | rl_wheel = (rl_wheel.T.dot(Rot1)).T 120 | 121 | outline[0, :] += x 122 | outline[1, :] += y 123 | fr_wheel[0, :] += x 124 | fr_wheel[1, :] += y 125 | rr_wheel[0, :] += x 126 | rr_wheel[1, :] += y 127 | fl_wheel[0, :] += x 128 | fl_wheel[1, :] += y 129 | rl_wheel[0, :] += x 130 | rl_wheel[1, :] += y 131 | 132 | plt.plot(np.array(outline[0, :]).flatten(), 133 | np.array(outline[1, :]).flatten(), truckcolor) 134 | plt.plot(np.array(fr_wheel[0, :]).flatten(), 135 | np.array(fr_wheel[1, :]).flatten(), truckcolor) 136 | plt.plot(np.array(rr_wheel[0, :]).flatten(), 137 | np.array(rr_wheel[1, :]).flatten(), truckcolor) 138 | plt.plot(np.array(fl_wheel[0, :]).flatten(), 139 | np.array(fl_wheel[1, :]).flatten(), truckcolor) 140 | plt.plot(np.array(rl_wheel[0, :]).flatten(), 141 | np.array(rl_wheel[1, :]).flatten(), truckcolor) 142 | plt.plot(x, y, "*") 143 | 144 | # 计算路径与当前状态最近点下标 145 | def calcNearestIndex(path_x, path_y, state, last_index = 0): 146 | assert(len(path_x) == len(path_y) and len(path_x) > last_index) 147 | min_distance = float('inf') 148 | min_index = last_index 149 | for i in range(last_index, len(path_x)): 150 | distance = np.sqrt((state[0] - path_x[i])**2 + (state[1] - path_y[i])**2) 151 | if distance < min_distance: 152 | min_distance = distance 153 | min_index = i 154 | return min_index 155 | 156 | # 获取参考路径 157 | def getReference(path_x, path_y, path_yaw, path_speeds, state, start_index, steps): 158 | assert(start_index < len(path_x)) 159 | ref_x, ref_y, ref_yaw, ref_speeds, ref_steers = list(), list(), list(), list(), list() 160 | for i in range(0, steps): 161 | index = start_index + int(round(state[-1] * DT * float(i) / DL)) 162 | index = min(index, len(path_x) - 1) 163 | ref_x.append(path_x[index]) 164 | ref_y.append(path_y[index]) 165 | ref_yaw.append(path_yaw[index]) 166 | ref_speeds.append(path_speeds[index]) 167 | ref_steers.append(0.0) 168 | return ref_x, ref_y, ref_yaw, ref_speeds, ref_steers 169 | 170 | # 更新状态 171 | def updateState(state, action_a, action_t): 172 | # 初始化新状态 173 | new_state = [0.0] * len(state) 174 | # 防止输入越界 175 | if action_t >= MAX_STEER: 176 | action_t = MAX_STEER 177 | elif action_t <= -MAX_STEER: 178 | action_t = -MAX_STEER 179 | # 更新状态 180 | new_state[0] = state[0] + state[3] * np.cos(state[2]) * DT 181 | new_state[1] = state[1] + state[3] * np.sin(state[2]) * DT 182 | new_state[2] = state[2] + state[3]/ WB * np.tan(action_t) * DT 183 | new_state[3] = state[3] + action_a * DT 184 | # 防止状态越界 185 | if new_state[3] > MAX_SPEED: 186 | new_state[3] = MAX_SPEED 187 | elif state[3] < MIN_SPEED: 188 | new_state[3] = MIN_SPEED 189 | return new_state 190 | 191 | # 进行模型状态预测 192 | def modelPredictStates(state, actions_a, actions_t): 193 | pred_state = copy.deepcopy(state) 194 | pre_x = [pred_state[0]] 195 | pre_y = [pred_state[1]] 196 | pre_yaw = [pred_state[2]] 197 | pre_speeds = [pred_state[3]] 198 | for action_a, action_t in zip(actions_a, actions_t): 199 | pred_state = updateState(pred_state, action_a, action_t) 200 | pre_x.append(pred_state[0]) 201 | pre_y.append(pred_state[1]) 202 | pre_yaw.append(pred_state[2]) 203 | pre_speeds.append(pred_state[3]) 204 | return pre_x, pre_y, pre_yaw, pre_speeds 205 | 206 | # 计算状态转移矩阵 207 | def calcTransMatrix(yaw, speed, steer): 208 | A = np.zeros((NX, NX)) 209 | A[0, 0] = 1.0 210 | A[1, 1] = 1.0 211 | A[2, 2] = 1.0 212 | A[3, 3] = 1.0 213 | A[0, 2] = DT * np.cos(yaw) 214 | A[0, 3] = - DT * speed * np.sin(yaw) 215 | A[1, 2] = DT * np.sin(yaw) 216 | A[1, 3] = DT * speed * np.cos(yaw) 217 | A[3, 2] = DT * np.tan(steer) / WB 218 | 219 | B = np.zeros((NX, NU)) 220 | B[2, 0] = DT 221 | B[3, 1] = DT * speed / (np.cos(steer) ** 2 * WB) 222 | 223 | C = np.zeros(NX) 224 | C[0] = DT * speed * np.sin(yaw) * yaw 225 | C[1] = - DT * speed * np.cos(yaw) * yaw 226 | C[3] = - DT * speed * steer / (WB * np.cos(steer) ** 2) 227 | 228 | return A, B, C 229 | 230 | # 进行模型预测优化 231 | def modelPredictiveOptimization(ref_x, ref_y, ref_yaw, ref_speeds, ref_steers, pre_yaw, pre_speeds, state): 232 | # 使用cvxpy进行优化 233 | # 定义优化变量 234 | x = cvxpy.Variable((NX, T)) 235 | u = cvxpy.Variable((NU, T - 1)) 236 | # 定义损失函数 237 | cost = 0.0 238 | # 与参考路径的区别最小化 239 | for t in range(1, T): 240 | cost += (x[0, t] - ref_x[t])**2 241 | cost += (x[1, t] - ref_y[t])**2 242 | cost += 0.5 * (x[2, t] - ref_speeds[t])**2 243 | cost += 0.5 * (x[3, t] - ref_yaw[t])**2 244 | # 运动最小化 245 | for t in range(0, T - 1): 246 | cost += 0.01 * u[0, t]**2 247 | cost += 0.01 * u[1, t]**2 248 | # 运动变化最小化 249 | for t in range(0, T - 2): 250 | cost += 0.01 * (u[0, t+1] - u[0, t]) ** 2 251 | cost += (u[1, t+1] - u[1, t]) ** 2 252 | # 定义限制条件 253 | constraints = [] 254 | # 初始状态限制 255 | constraints += [x[0, 0] == state[0]] 256 | constraints += [x[1, 0] == state[1]] 257 | constraints += [x[2, 0] == state[3]] 258 | constraints += [x[3, 0] == state[2]] 259 | # 最大速度限制 260 | constraints += [x[2, :] <= MAX_SPEED] 261 | # 最小速度限制 262 | constraints += [x[2, :] >= MIN_SPEED] 263 | # 最大加速度限制 264 | constraints += [cvxpy.abs(u[0, :]) <= MAX_ACCEL] 265 | # 转向限制 266 | constraints += [cvxpy.abs(u[1, :]) <= MAX_STEER] 267 | # 转向变化限制 268 | for t in range(0, T - 2): 269 | constraints += [cvxpy.abs(u[1, t+1] - u[1, t]) <= MAX_DSTEER * DT] 270 | # 状态转移限制 271 | for t in range(0, T - 1): 272 | # 计算状态转移矩阵 273 | A, B, C = calcTransMatrix(pre_yaw[t], pre_speeds[t], ref_steers[t]) 274 | constraints += [x[:, t+1] == A * x[:, t] + B * u[:, t] + C] 275 | 276 | # 定义优化问题 277 | prob = cvxpy.Problem(cvxpy.Minimize(cost), constraints) 278 | # 求解优化问题 279 | prob.solve(solver=cvxpy.ECOS, verbose=False) 280 | # 判断结果 281 | if prob.status == cvxpy.OPTIMAL or prob.status == cvxpy.OPTIMAL_INACCURATE: 282 | ox = get_nparray_from_matrix(x.value[0, :]) 283 | oy = get_nparray_from_matrix(x.value[1, :]) 284 | oyaw = get_nparray_from_matrix(x.value[3, :]) 285 | ov = get_nparray_from_matrix(x.value[2, :]) 286 | oa = get_nparray_from_matrix(u.value[0, :]) 287 | ot = get_nparray_from_matrix(u.value[1, :]) 288 | else: 289 | print("no solution for optimization") 290 | ox, oy, oyaw, ov, oa, ot = None, None, None, None, None, None 291 | return ox, oy, oyaw, ov, oa, ot 292 | 293 | 294 | # 迭代进行模型预测控制 295 | def iterModelPredictiveControls(ref_x, ref_y, ref_yaw, ref_speeds, ref_steers, state): 296 | assert(len(ref_x) == len(ref_y) and len(ref_y) == len(ref_yaw) and len(ref_yaw) == len(ref_speeds) and len(ref_speeds) == len(ref_steers)) 297 | # 初始化控制 298 | actions_a = [0.0] * (len(ref_x) - 1) 299 | actions_t = [0.0] * (len(ref_x) - 1) 300 | opt_x, opt_y, opt_yaw, opt_speeds = None, None, None, None 301 | for i in range(0, MAX_ITER): 302 | # 根据当前控制生成预测状态 303 | pre_x, pre_y, pre_yaw, pre_speeds = modelPredictStates(state, actions_a, actions_t) 304 | assert(len(pre_x) == len(ref_x)) 305 | # 进行运动优化 306 | opt_x, opt_y, opt_yaw, opt_speeds, new_actions_a, new_actions_t = modelPredictiveOptimization(ref_x, ref_y, ref_yaw, ref_speeds, ref_steers, pre_yaw, pre_speeds, state) 307 | # 判断运动是否有效 308 | if new_actions_a is None: 309 | exit(0) 310 | # 判断运动的变化 311 | action_change = np.sum(np.array(actions_a) - np.array(new_actions_a)) + np.sum(np.array(actions_t) - np.array(new_actions_t)) 312 | # 更新运动 313 | actions_a = new_actions_a 314 | actions_t = new_actions_t 315 | # 判断变化是否小于阈值 316 | if action_change < DU_TH: 317 | print("mpc update success") 318 | break 319 | else: 320 | print("up to max iteration") 321 | 322 | return opt_x, opt_y, opt_yaw, opt_speeds, actions_a, actions_t 323 | 324 | # 路径跟随 325 | def pathFollowing(fpath_x, fpath_y, fpath_yaw, fpath_speeds, init_state): 326 | # initial yaw compensation 327 | if init_state[2] - fpath_yaw[0] >= np.pi: 328 | init_state[2] -= np.pi * 2.0 329 | elif init_state[2] - fpath_yaw[0] <= -np.pi: 330 | init_state[2] += np.pi * 2.0 331 | # 定义当前状态 332 | current_state = init_state 333 | # 定义当前状态对应下标 334 | current_index = 0 335 | # 定义时间 336 | time = 0.0 337 | # 定义记录量 338 | x = [current_state[0]] 339 | y = [current_state[1]] 340 | yaw = [current_state[2]] 341 | v = [current_state[3]] 342 | t = [0.0] 343 | d = [0.0] 344 | a = [0.0] 345 | # 开始进行路径跟随 346 | while time < MAX_TIME: 347 | # 计算路径与当前状态最近点下标 348 | current_index = calcNearestIndex(fpath_x, fpath_y, current_state, current_index) 349 | # 计算参考路径 350 | ref_x, ref_y, ref_yaw, ref_speeds, ref_steers = getReference(fpath_x, fpath_y, fpath_yaw, fpath_speeds, current_state, current_index, T) 351 | # 迭代进行模型预测控制 352 | opt_x, opt_y, opt_yaw, opt_speeds, actions_a, actions_t = iterModelPredictiveControls(ref_x, ref_y, ref_yaw, ref_speeds, ref_steers, current_state) 353 | # 进行状态更新 354 | current_state = updateState(current_state, actions_a[0], actions_t[0]) 355 | # 进行记录 356 | x.append(current_state[0]) 357 | y.append(current_state[1]) 358 | yaw.append(current_state[2]) 359 | v.append(current_state[3]) 360 | t.append(time) 361 | d.append(actions_a[0]) 362 | a.append(actions_t[0]) 363 | # 进行可视化 364 | plt.cla() 365 | if opt_x is not None: 366 | plt.plot(opt_x, opt_y, "xr", label="MPC") 367 | plt.plot(fpath_x, fpath_y, "-r", label="course") 368 | plt.plot(x, y, "ob", label="trajectory") 369 | plt.plot(ref_x, ref_y, "xk", label="xref") 370 | plt.plot(fpath_x[current_index], fpath_y[current_index], "xg", label="target") 371 | plotCar(current_state[0], current_state[1], current_state[2], steer=actions_t[0]) 372 | plt.axis("equal") 373 | plt.grid(True) 374 | plt.title("Time[s]:" + str(round(time, 2)) 375 | + ", speed[km/h]:" + str(round(current_state[3] * 3.6, 2))) 376 | plt.pause(0.0001) 377 | # 判断是否到达终点 378 | if checkGoal(current_state, fpath_x[-1], fpath_y[-1]): 379 | print("Goal Reached") 380 | break 381 | # 增加时间 382 | time += DT 383 | return x, y, yaw, v, t, d, a 384 | 385 | # 主函数 386 | def main(): 387 | # 给出路点 388 | # waypoints_x = [0.0, 5.0, 10.0, 20.0, 30.0, 40.0, 50.0] 389 | # waypoints_y = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 390 | waypoints_x = [0.0, 60.0, 125.0, 50.0, 75.0, 30.0, -10.0] 391 | waypoints_y = [0.0, 0.0, 50.0, 65.0, 30.0, 50.0, -20.0] 392 | # waypoints_x = [0.0, -10.0, -20.0, -40.0, -50.0, -60.0, -70.0] 393 | # waypoints_y = [0.0, -1.0, 1.0, 0.0, -1.0, 1.0, 0.0] 394 | # 生成引导轨迹 395 | spline = cubic_spline.CubicSpline2D(waypoints_x, waypoints_y) 396 | sample_s = np.arange(0.0, spline.s_[-1], DL) 397 | point_x, point_y = spline.calcPosition(sample_s) 398 | point_yaw = spline.calcYaw(sample_s) 399 | point_yaw = smoothYaw(point_yaw) 400 | point_kappa = spline.calcKappa(sample_s) 401 | # 生成对应速度 402 | speeds = [TARGET_SPEED] * len(point_x) 403 | speeds[-1] = 0.0 404 | # 定义初始状态 405 | init_state = [point_x[0], point_y[0], np.pi - point_yaw[0], 0.0] 406 | # 进行路径跟随 407 | x, y, yaw, v, t, d, a = pathFollowing(point_x, point_y, point_yaw, speeds, init_state) 408 | 409 | # 进行可视化 410 | plt.close("all") 411 | plt.subplots() 412 | plt.plot(point_x, point_y, "-r", label="spline") 413 | plt.plot(x, y, "-g", label="tracking") 414 | plt.grid(True) 415 | plt.axis("equal") 416 | plt.xlabel("x[m]") 417 | plt.ylabel("y[m]") 418 | plt.legend() 419 | 420 | plt.subplots() 421 | plt.plot(t, yaw, "-r", label="speed") 422 | plt.grid(True) 423 | plt.xlabel("Time [s]") 424 | plt.ylabel("Yaw [rad]") 425 | 426 | plt.show() 427 | 428 | if __name__ == "__main__": 429 | main() 430 | -------------------------------------------------------------------------------- /model_predictive_trajectory_generation/ModelPredictiveTrajectoryGeneration: -------------------------------------------------------------------------------- 1 | model predictive trajectory generator方法是一种以优化为主要思路的路径规划算法。算法思路大致如下：首先以带参数的表达式来表征某一个或一些运动参量变化，在通过这些运动参量变化得到车辆的运动路径，将生成路径终点与目标点进行比较，得到损失，再通过梯度下降方法，利用损失对表达式中参数进行更新，直到损失小于特定值，路径生成完成。 2 | 详细从代码上进行分析，代码构造了一个简单的车辆运动模型，模型存在x,y和yaw三个参量。通过一个以时间为自变量的二次方程对前轮转角进行表示。并初始化四个参数s,k0,km,kf。其中s为路程，给出固定速度10km/h，则可以通过s计算出所需时间time。以[0, k0],[time/2, km], [time, kf]求解二次方程，则可以得到前轮转角随时间变化，进一步就可以基础出time时间内车辆的运动轨迹。得到轨迹的最后一个点，与目标点进行比较，计算两者之间在三维空间（x,y,yaw）上的欧式距离，作为损失函数。下一步是计算雅克比矩阵，雅克比矩阵计算方法是直接通过离散的方式解决的，在路径最后一个点前取一个小区间，在这个小区间内计算各个轴的导数近似值就可以的到雅克比矩阵。利用雅克比矩阵和损失就可以利用二次范数梯度下降的方法对二次方程的参数进行更新，不断重复上诉过程，直到路径终点约等于目标点，得到最终路径。 3 | 为了加速上诉优化过程，可以引入查找表的方式，提前生成许多路径，然后在这些路径中找出终点与目标点最近的，将此路径参数作为初始参数进行优化。 -------------------------------------------------------------------------------- /model_predictive_trajectory_generation/__pycache__/model.cpython-35.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flztiii/path_planning_basic_algorithm/d1827cbb355416da8a14831eb37a929c5c15e644/model_predictive_trajectory_generation/__pycache__/model.cpython-35.pyc -------------------------------------------------------------------------------- /model_predictive_trajectory_generation/__pycache__/model_predictive_trajectory.cpython-35.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flztiii/path_planning_basic_algorithm/d1827cbb355416da8a14831eb37a929c5c15e644/model_predictive_trajectory_generation/__pycache__/model_predictive_trajectory.cpython-35.pyc -------------------------------------------------------------------------------- /model_predictive_trajectory_generation/lookup_table.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/python3 2 | # -*- coding: utf-8 -*- 3 | 4 | """ 5 | 6 | lookup table 7 | author: flztiii 8 | 9 | """ 10 | 11 | import matplotlib.pyplot as plt 12 | import numpy as np 13 | import math 14 | import model 15 | import model_predictive_trajectory 16 | import pandas as pd 17 | import os 18 | 19 | # 初始化查询表范围 20 | def initSearchRange(): 21 | # 初始化朝向范围 22 | max_yaw = np.deg2rad(-30.0) 23 | yaw_range = np.arange(-max_yaw, max_yaw, max_yaw) 24 | # 初始化xy范围 25 | x_range = np.arange(10.0, 30.0, 5.0) 26 | y_range = np.arange(0.0, 20.0, 2.0) 27 | # 得到范围 28 | states = [] 29 | for yaw in yaw_range: 30 | for y in y_range: 31 | for x in x_range: 32 | states.append([x, y, yaw]) 33 | return states 34 | 35 | # 计算距离 36 | def calcDistance(state, table): 37 | """ 38 | :param state: 要计算距离的状态 39 | :param table: 查找表中元素 40 | """ 41 | 42 | dx = state[0] - table[0] 43 | dy = state[1] - table[1] 44 | dyaw = state[2] - table[2] 45 | return math.sqrt(dx ** 2 + dy ** 2 + dyaw ** 2) 46 | 47 | # 保存查找表 48 | def saveLookupTable(fname, lookup_table): 49 | mt = np.array(lookup_table) 50 | # save csv 51 | df = pd.DataFrame() 52 | df["x"] = mt[:, 0] 53 | df["y"] = mt[:, 1] 54 | df["yaw"] = mt[:, 2] 55 | df["s"] = mt[:, 3] 56 | df["km"] = mt[:, 4] 57 | df["kf"] = mt[:, 5] 58 | 59 | df.to_csv(fname, index=None) 60 | 61 | # 构建查找表 62 | def generateLookupTable(): 63 | # 第一步构造查找表初始值 64 | # 查找表中每一个元素包括x, y, yaw, s, km, kf 65 | k0 = 0.0 66 | lookup_table = [[1.0, .0, .0, 1.0, .0, .0]] 67 | # 初始化查找表范围 68 | states = initSearchRange() 69 | # 遍历查找表范围内的全部初始状态 70 | for state in states: 71 | print("state", state) 72 | # 找到当前查找表中离当前点最近的点 73 | min_dis = float("inf") 74 | min_index = -1 75 | for i in range(len(lookup_table)): 76 | dis = calcDistance(state, lookup_table[i]) 77 | if dis <= min_dis: 78 | min_dis = dis 79 | min_index = i 80 | print("index", i, "min_dis", min_dis) 81 | table = lookup_table[min_index] 82 | print("bestp", table) 83 | # 得到路径优化初始条件 84 | init_p = np.array([math.sqrt(state[0] ** 2 + state[1] ** 2), table[4], table[5]]).reshape((3, 1)) 85 | target = model.State(state[0], state[1], state[2]) 86 | # 生成路径 87 | x, y, yaw, p = model_predictive_trajectory.optimizeTrajectory(target, init_p, k0) 88 | if not x is None: 89 | # 加入查找表 90 | lookup_table.append([x[-1], y[-1], yaw[-1], float(p[0]), float(p[1]), float(p[2])]) 91 | 92 | # 完成查找表的生成，将表进行保存 93 | # saveLookupTable((os.getcwd() + "/" + __file__).replace(".py", ".csv"), lookup_table) 94 | # 查找表中参数生成路径的可视化 95 | for table in lookup_table: 96 | x, y, yaw = model.generateTrajectory(table[3], k0, table[4], table[5]) 97 | plt.plot(x, y, "-r") 98 | x, y, yaw = model.generateTrajectory(table[3], k0, -table[4], -table[5]) 99 | plt.plot(x, y, "-r") 100 | plt.grid(True) 101 | plt.axis("equal") 102 | plt.show() 103 | # 主函数 104 | def main(): 105 | generateLookupTable() 106 | 107 | if __name__ == "__main__": 108 | main() -------------------------------------------------------------------------------- /model_predictive_trajectory_generation/model.py: -------------------------------------------------------------------------------- 1 | #！ /usr/bin/python3 2 | # -*- coding: utf-8 -*- 3 | 4 | """ 5 | 6 | motion model of vehicle 7 | author: flztiii 8 | 9 | """ 10 | 11 | import math 12 | import scipy.interpolate 13 | import numpy as np 14 | 15 | # 全局变量 16 | LEN = 1.0 # 车辆前后轴距 17 | VEL = 10 / 3.6 # 车辆速度10km/h 18 | ds = 0.1 # 车辆生成轨迹点采样间隔 19 | 20 | # 车辆模型状态，用的是后轴中心的阿克曼模型 21 | class State: 22 | def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0): 23 | self.x_ = x # 车辆位置x坐标 24 | self.y_ = y # 车辆位置y坐标 25 | self.yaw_ = yaw # 车辆后轴中心的朝向 26 | self.v_ = v # 车辆的速度 27 | 28 | # 弧度转化，将弧度转化到[-π,π]之间 29 | def pi2Pi(angle): 30 | return (angle + math.pi) % (2 * math.pi) - math.pi 31 | 32 | # 更新车辆状态 33 | def update(state, v, delta, dt, L): 34 | """ 35 | :param state: 当前状态 36 | :param v: 速度 37 | :param delta: 前轮转角 38 | :param dt: 时间间隔 39 | :param L: 前后轮轴距 40 | """ 41 | 42 | new_state = State() 43 | new_state.v_ = v 44 | new_state.x_ = state.x_ + v * dt * math.cos(state.yaw_) 45 | new_state.y_ = state.y_ + v * dt * math.sin(state.yaw_) 46 | new_state.yaw_ = state.yaw_ + v * math.tan(delta) / L * dt 47 | new_state.yaw_ = pi2Pi(new_state.yaw_) 48 | return new_state 49 | 50 | # 生成路径 51 | def generateTrajectory(s, k0, km, kf): 52 | """ 53 | :param s: 生成轨迹的长度 54 | :param k0: 初始时刻前轮转角 55 | :param km: 中间时刻前轮转角 56 | :param kf: 结束时刻前轮转角 57 | """ 58 | 59 | # 首先生成前轮转角随时间的变化关系 60 | # 根据泰勒公式，在较短时间内可以假设前轮转角随时间变化可以通过多项式表示，这里使用二次多项式 61 | time = s / VEL # 得到轨迹总时长 62 | n = s / ds # 得到轨迹的总采样点数 63 | dt = float(time / n) # 每个采样点之间的时间间隔 64 | tk = np.array([0.0, time * 0.5, time]) 65 | kk = np.array([k0, km, kf]) 66 | t = np.arange(0.0, time, time / n) 67 | fkp = scipy.interpolate.interp1d(tk, kk, kind="quadratic") # 得到二次表达式 68 | # 进行采样 69 | kp = [fkp(it) for it in t] 70 | # 根据得到的前轮转角随时间变化采样点得到车辆轨迹 71 | state = State() 72 | x, y, yaw = [state.x_], [state.y_], [state.yaw_] 73 | for i in range(0, len(kp)): 74 | state = update(state, VEL, kp[i], dt, LEN) 75 | x.append(state.x_) 76 | y.append(state.y_) 77 | yaw.append(state.yaw_) 78 | return x, y, yaw 79 | 80 | # 生成轨迹的最后一个点状态 81 | def generateFinalState(s, k0, km, kf): 82 | """ 83 | :param s: 生成轨迹的长度 84 | :param k0: 初始时刻前轮转角 85 | :param km: 中间时刻前轮转角 86 | :param kf: 结束时刻前轮转角 87 | """ 88 | 89 | # 首先生成前轮转角随时间的变化关系 90 | # 根据泰勒公式，在较短时间内可以假设前轮转角随时间变化可以通过多项式表示，这里使用二次多项式 91 | time = s / VEL # 得到轨迹总时长 92 | n = s / ds # 得到轨迹的总采样点数 93 | dt = float(time / n) # 每个采样点之间的时间间隔 94 | tk = np.array([0.0, time * 0.5, time]) 95 | kk = np.array([k0, km, kf]) 96 | t = np.arange(0.0, time, time / n) 97 | fkp = scipy.interpolate.interp1d(tk, kk, "quadratic") # 得到二次表达式 98 | # 进行采样 99 | kp = [fkp(it) for it in t] 100 | 101 | # 根据得到的前轮转角随时间变化采样点得到车辆轨迹 102 | state = State() 103 | for i in range(0, len(kp)): 104 | state = update(state, VEL, kp[i], dt, LEN) 105 | return state.x_, state.y_, state.yaw_ -------------------------------------------------------------------------------- /model_predictive_trajectory_generation/model_predictive_trajectory.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/python3 2 | # -*- coding: utf-8 -*- 3 | 4 | """ 5 | 6 | model predictive trajectory 7 | author: flztiii 8 | 9 | """ 10 | 11 | import numpy as np 12 | import model 13 | import math 14 | import matplotlib.pyplot as plt 15 | 16 | # 优化参数全局变量 17 | max_iter = 100 # 最大迭代次数 18 | h = np.array([0.5, 0.02, 0.02]).T # 采样距离参数 19 | cost_th = 0.1 # 迭代完成阈值 20 | 21 | # 角度转弧度 22 | def degToRed(degree): 23 | return model.pi2Pi(degree / 180.0 * np.pi) 24 | 25 | # 判断点与目标之间的差距 26 | def calcDiff(x, y, yaw, target): 27 | x_diff = target.x_ - x 28 | y_diff = target.y_ - y 29 | yaw_diff = model.pi2Pi(target.yaw_ - yaw) 30 | return np.array([x_diff, y_diff, yaw_diff]) 31 | 32 | # 可视化箭头 33 | def plotArrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"): 34 | plt.arrow(x, y, length * math.cos(yaw), length * math.sin(yaw), 35 | fc=fc, ec=ec, head_width=width, head_length=width) 36 | plt.plot(x, y) 37 | plt.plot(0, 0) 38 | 39 | # 可视化路径 40 | def plotTrajectory(xc, yc, target): 41 | plt.clf() 42 | plotArrow(target.x_, target.y_, target.yaw_) 43 | plt.plot(xc, yc, "-r") 44 | plt.axis("equal") 45 | plt.grid(True) 46 | plt.pause(1) 47 | 48 | # 计算雅克比矩阵 49 | def calcJaccobi(target, p, k0): 50 | # 雅克比矩阵为横轴为对变量不同维度的导数，此处利用离散化来计算每一个变量维度的导数 51 | # 首先是第一维度变量导数计算 52 | prev_x, prev_y, prev_yaw = model.generateFinalState(p[0,0] - h[0], k0, p[1, 0], p[2, 0]) 53 | next_x, next_y, next_yaw = model.generateFinalState(p[0,0] + h[0], k0, p[1, 0], p[2, 0]) 54 | prev_diff = calcDiff(prev_x, prev_y, prev_yaw, target) 55 | next_diff = calcDiff(next_x, next_y, next_yaw, target) 56 | d1 = ((next_diff - prev_diff) / (2.0 * h[0])).reshape((3, 1)) 57 | # 是第二维度变量导数计算 58 | prev_x, prev_y, prev_yaw = model.generateFinalState(p[0,0], k0, p[1, 0] - h[1], p[2, 0]) 59 | next_x, next_y, next_yaw = model.generateFinalState(p[0,0], k0, p[1, 0] + h[1], p[2, 0]) 60 | prev_diff = calcDiff(prev_x, prev_y, prev_yaw, target) 61 | next_diff = calcDiff(next_x, next_y, next_yaw, target) 62 | d2 = ((next_diff - prev_diff) / (2.0 * h[1])).reshape((3, 1)) 63 | # 是第三维度变量导数计算 64 | prev_x, prev_y, prev_yaw = model.generateFinalState(p[0,0], k0, p[1, 0], p[2, 0] - h[2]) 65 | next_x, next_y, next_yaw = model.generateFinalState(p[0,0], k0, p[1, 0], p[2, 0] + h[2]) 66 | prev_diff = calcDiff(prev_x, prev_y, prev_yaw, target) 67 | next_diff = calcDiff(next_x, next_y, next_yaw, target) 68 | d3 = ((next_diff - prev_diff) / (2.0 * h[2])).reshape((3, 1)) 69 | # 得到雅克比矩阵 70 | jaccobi = np.hstack((d1, d2, d3)) 71 | return jaccobi 72 | 73 | # 计算学习率 74 | def selectLearningRate(gradient, target, p, k0): 75 | # 初始化上下限 76 | min_rate = 1.0 77 | max_rate = 2.0 78 | rate_gap = 0.5 79 | # 初始化最小值 80 | min_cost = float("inf") 81 | final_rate = min_rate 82 | # 找出能使代价函数最小的学习率 83 | for rate in np.arange(min_rate, max_rate, rate_gap): 84 | tp = p + rate * gradient 85 | x, y, yaw = model.generateFinalState(tp[0, 0], k0, tp[1, 0], tp[2, 0]) 86 | diff = calcDiff(x, y, yaw, target).reshape((3, 1)) 87 | diff_norm = np.linalg.norm(diff) 88 | if diff_norm < min_cost and rate != 0.0: 89 | min_cost = diff_norm 90 | final_rate = rate 91 | return final_rate 92 | 93 | # 优化路径生成 94 | def optimizeTrajectory(target, p, k0): 95 | for i in range(0, max_iter): 96 | # print("interaction", i, "param", p) 97 | # if i == max_iter - 1: 98 | # print("reach max iteration number") 99 | # 首先利用当前参数进行路径生成 100 | x, y, yaw = model.generateTrajectory(p[0, 0], k0, p[1, 0], p[2, 0]) 101 | # 判断路径最后一个点与目标点的差值 102 | target_diff = calcDiff(x[-1], y[-1], yaw[-1], target).reshape((3, 1)) 103 | # 计算差值的欧式距离 104 | target_diff_norm = np.linalg.norm(target_diff) 105 | # 判断距离是否小于阈值，如果小于阈值则完成 106 | if target_diff_norm < cost_th: 107 | # 完成迭代 108 | print("iteration finished cost", target_diff_norm) 109 | break 110 | else: 111 | # 没有完成迭代 112 | # print("interation", i, "cost", target_diff_norm) 113 | # 计算二次范数的最速梯度 114 | # 首先计算雅克比矩阵 115 | jaccobi = calcJaccobi(target, p, k0) 116 | # 计算二次范数梯度方向 117 | try: 118 | inv_jaccobi = np.linalg.inv(jaccobi) 119 | except np.linalg.LinAlgError: 120 | # print("no jaccobi inv") 121 | x, y, yaw, p = None, None, None, None 122 | break 123 | # 得到梯度 124 | gradient = - inv_jaccobi @ target_diff 125 | # 计算学习率 126 | alpha = selectLearningRate(gradient, target, p, k0) 127 | # 更新参数 128 | p = p + alpha * gradient 129 | # 可视化当前轨迹 130 | plotTrajectory(x, y, target) 131 | return x, y, yaw, p 132 | 133 | # 优化路径生成算法测试函数 134 | def testOptimizeTrajectory(): 135 | # 确定目标点 136 | target = model.State(x = 8.0, y = 3.0, yaw = degToRed(60)) 137 | # 初始化参数 138 | p = np.array([6.0, 0.0, 0.0]).reshape((3,1)) 139 | # 初始方向盘转角 140 | k0 = 0.0 141 | # 开始优化路径生成 142 | x, y, yaw, p = optimizeTrajectory(target, p, k0) 143 | # 可视化最终轨迹 144 | plotTrajectory(x, y, target) 145 | plotArrow(target.x_, target.y_, target.yaw_) 146 | plt.axis("equal") 147 | plt.grid(True) 148 | plt.show() 149 | 150 | # 主函数 151 | def main(): 152 | # print(__file__ + " start!!") 153 | testOptimizeTrajectory() 154 | 155 | if __name__ == "__main__": 156 | main() -------------------------------------------------------------------------------- /model_predictive_trajectory_generation/reference: -------------------------------------------------------------------------------- 1 | 2007 2 | 由Howard和Kelly在2007年首次提出 3 | 4 | Optimal Rough Terrain Trajectory Generation for Wheeled Mobile Robots 5 | 6 | 2008 7 | 在DARPA Urban Challenge 2008的Boss车上面应用 8 | 9 | Motion Planning in Urban Environments: Part I 10 | 11 | 2014 12 | Howard更新了在这个方向上的一些最新研究进展 13 | 14 | Model-Predictive Motion Planning: Several Key Developments for Autonomous Mobile Robots 15 | -------------------------------------------------------------------------------- /potential_field/PotentialField: -------------------------------------------------------------------------------- 1 | 人工势场方法也是一种基于栅格地图的路径搜索算法，与之前的GridDijkstra和A星不同的是，它更注重于对于栅格地图的构建，在地图上添加复杂的属性信息，从而以一种较为简单的方法进行完成路径的搜索生成，栅格地图提供约束和启发信息；而前两种方法不重视栅格地图的生成而是注重于路径的搜索，栅格地图仅仅是约束。 2 | 人工势场方法将区域构造成一个场，障碍物对代理产生斥力，目标点对代理产生引力，从而使代理能够避免碰撞障碍物达到终点。方法构建一个比包括起点、目标点和障碍物更大的区域，将其以一定分辨率进行栅格化，每一个栅格具有势能这一属性。势能包括两个部分引力势能与斥力势能，引力势能为栅格点与目标点栅格距离（距离计算是通过欧式范数得到），斥力势能为障碍物引起，首先找到离栅格最近的障碍物点，计算出距离，距离如果大于阈值则斥力势能为0，反之斥力势能与距离近似反比。从以上叙述可以得知，离目标点越近，离障碍物越远，势能越小。因此，在完成栅格地图每一个点势能的计算后，要做的就是从起点开始不断找出做出一定行为后能够达到势能最低的栅格，重复上诉过程，直到到达目标点，完成规划。 -------------------------------------------------------------------------------- /potential_field/potential_field.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/python3 2 | # -*- coding: utf-8 -*- 3 | 4 | """ 5 | 6 | potential field algorithm 7 | 8 | author: flztiii 9 | 10 | """ 11 | 12 | import numpy as np 13 | import matplotlib.pyplot as plt 14 | 15 | # 点 16 | class Point: 17 | def __init__(self, x, y): 18 | self.x_ = x 19 | self.y_ = y 20 | 21 | # 重载等于 22 | def __eq__(self, point): 23 | if self.x_ == point.x_ and self.y_ == point.y_: 24 | return True 25 | else: 26 | return False 27 | # 重载减法 28 | def __sub__(self, point): 29 | return np.hypot(self.x_ - point.x_, self.y_ - point.y_) 30 | 31 | # 行为 32 | class Motion: 33 | def __init__(self, mv_x, mv_y): 34 | self.mv_x_ = mv_x 35 | self.mv_y_ = mv_y 36 | 37 | # 初始设置 38 | class Config: 39 | kp_ = 5.0 # 引力势能权重参数 40 | eta_ = 100.0 # 斥力势能权重参数 41 | area_width_ = 30.0 # 势能场扩展宽度 42 | resolution_ = 0.5 # 栅格地图分辨率 43 | robot_size_ = 1.0 # 机器人大小 44 | detect_radius_ = 5.0 # 障碍物判定范围，大于阈值则不考虑 45 | motion_ = [ 46 | Motion(1, 0), 47 | Motion(0, 1), 48 | Motion(-1, 0), 49 | Motion(0, -1), 50 | Motion(1, -1), 51 | Motion(-1, 1), 52 | Motion(1, 1), 53 | Motion(-1, -1) 54 | ] # 可以采取的行为 55 | 56 | # 工具函数 57 | class Tools: 58 | # 计算点采取行为后得到的新点 59 | @classmethod 60 | def action(self, point, motion): 61 | return Point(point.x_ + motion.mv_x_, point.y_ + motion.mv_y_) 62 | 63 | # 获取点集合的x, y列表 64 | @classmethod 65 | def departurePoints(self, points): 66 | x, y = [], [] 67 | for point in points: 68 | x.append(point.x_) 69 | y.append(point.y_) 70 | return x, y 71 | 72 | # 由序号转化为坐标 73 | @classmethod 74 | def indexToPos(self, index, min_pos, resolution): 75 | return index * resolution + min_pos 76 | 77 | # 由坐标转化为序号 78 | @classmethod 79 | def posToIndex(self, pos, min_pos, resolution): 80 | return round((pos - min_pos) / resolution) 81 | 82 | # 可视化人工势场 83 | @classmethod 84 | def drawHeatmap(self, data): 85 | data = np.array(data).T 86 | plt.pcolor(data, vmax=100.0, cmap=plt.cm.Blues) 87 | 88 | # 人工势场规划器 89 | class PotentialFieldPlanner: 90 | def __init__(self, p_start, p_goal, p_obstacles): 91 | self.p_start_ = p_start # 起点 92 | self.p_goal_ = p_goal # 目标点 93 | self.p_obstacles_ = p_obstacles # 障碍物点 94 | self.__buildPotentialField() # 构建人工势场 95 | 96 | # 构建人工势场 97 | def __buildPotentialField(self): 98 | # 首先计算区域边界 99 | self.min_px_ = min(self.p_obstacles_, key=lambda o: o.x_).x_ - Config.area_width_ * 0.5 100 | self.min_py_ = min(self.p_obstacles_, key=lambda o: o.y_).y_ - Config.area_width_ * 0.5 101 | self.max_px_ = max(self.p_obstacles_, key=lambda o: o.x_).x_ + Config.area_width_ * 0.5 102 | self.max_py_ = max(self.p_obstacles_, key=lambda o: o.y_).y_ + Config.area_width_ * 0.5 103 | print("field area minx:", self.min_px_) 104 | print("field area miny:", self.min_py_) 105 | print("field area maxx:", self.max_px_) 106 | print("field area maxy:", self.max_py_) 107 | # 计算宽度 108 | self.width_nx_ = round((self.max_px_ - self.min_px_) / Config.resolution_) 109 | self.width_ny_ = round((self.max_py_ - self.min_py_) / Config.resolution_) 110 | print("field nx width:", self.width_nx_) 111 | print("field ny width:", self.width_ny_) 112 | # 构建人工势场 113 | # 初始化 114 | potential_field = [[0.0 for j in range(0, self.width_ny_)] for i in range(0, self.width_nx_)] 115 | # 遍历势场中每个栅格 116 | for ix in range(0, self.width_nx_): 117 | for iy in range(0, self.width_ny_): 118 | # 计算当前栅格对应的坐标 119 | current_point = Point(Tools.indexToPos(ix, self.min_px_, Config.resolution_), Tools.indexToPos(iy, self.min_py_, Config.resolution_)) 120 | # 计算坐标的引力势能 121 | attractive_potential = self.__calcAttractivePotential(current_point) 122 | # 计算坐标的斥力势能 123 | repulsive_potential = self.__calcRepulsivePotential(current_point) 124 | # 计算总势能 125 | potential = attractive_potential + repulsive_potential 126 | potential_field[ix][iy] = potential 127 | # 可视化人工势场图 128 | Tools.drawHeatmap(potential_field) 129 | self.potential_field_ = potential_field 130 | 131 | # 计算引力势能 132 | def __calcAttractivePotential(self, point): 133 | return 0.5 * Config.kp_ * (point - self.p_goal_) 134 | 135 | # 计算斥力势能 136 | def __calcRepulsivePotential(self, point): 137 | # 计算离当前点最近的障碍物和距离 138 | min_distance = float("inf") 139 | min_index = -1 140 | for i, _ in enumerate(self.p_obstacles_): 141 | distance = point - self.p_obstacles_[i] 142 | if distance < min_distance: 143 | min_distance = distance 144 | min_index = i 145 | # 根据最小距离计算势能 146 | if min_distance >= Config.detect_radius_: 147 | # 如果障碍物距离过大，则不考虑 148 | return 0.0 149 | elif min_distance <= Config.robot_size_: 150 | # 如果障碍物距离过小，则势能无限大 151 | # return float("inf") 152 | return 10000.0 153 | else: 154 | return 0.5 * Config.eta_ * (1.0 / min_distance - 1.0 / Config.detect_radius_) ** 2 155 | 156 | # 利用人工势场进行规划 157 | def planning(self): 158 | # 获得起点、目标点对应的栅格 159 | nstart = Point(Tools.posToIndex(self.p_start_.x_, self.min_px_, Config.resolution_), Tools.posToIndex(self.p_start_.y_, self.min_py_, Config.resolution_)) 160 | ngoal = Point(Tools.posToIndex(self.p_goal_.x_, self.min_px_, Config.resolution_), Tools.posToIndex(self.p_goal_.y_, self.min_py_, Config.resolution_)) 161 | # 可视化起点和终点 162 | plt.plot(nstart.x_, nstart.y_, "*k") 163 | plt.plot(ngoal.x_, ngoal.y_, "*m") 164 | # 将当前点设置为起点 165 | current_node = nstart 166 | current_pos = self.p_start_ 167 | # 初始化最终路径 168 | planned_path = [self.p_start_] 169 | # 计算当前点到终点的距离 170 | distance_to_goal = current_pos - self.p_goal_ 171 | # 开始循环梯度下降寻找目标点 172 | while distance_to_goal > Config.resolution_: 173 | # 找出当前点以固定行为能够达到最小势能的点 174 | min_potential = float("inf") 175 | min_index = -1 176 | for i, _ in enumerate(Config.motion_): 177 | # 遍历行为 178 | motion = Config.motion_[i] 179 | # 计算采取行为后的新栅格 180 | next_node = Tools.action(current_node, motion) 181 | # 判断新栅格的势能是否更小 182 | if self.potential_field_[next_node.x_][next_node.y_] < min_potential: 183 | min_potential = self.potential_field_[next_node.x_][next_node.y_] 184 | min_index = i 185 | # 得到最低势能栅格进行更新 186 | current_node = Tools.action(current_node, Config.motion_[min_index]) 187 | current_pos = Point(Tools.indexToPos(current_node.x_, self.min_px_, Config.resolution_), Tools.indexToPos(current_node.y_, self.min_py_, Config.resolution_)) 188 | distance_to_goal = current_pos - self.p_goal_ 189 | planned_path.append(current_pos) 190 | # 可视化 191 | plt.plot(current_node.x_, current_node.y_, ".r") 192 | plt.pause(0.01) 193 | print("Goal Reached") 194 | return planned_path 195 | 196 | # 主函数 197 | def main(): 198 | # 起点和终点 199 | p_start = Point(0.0, 10.0) 200 | p_goal = Point(30.0, 30.0) 201 | # 障碍物点 202 | p_obstacles = [ 203 | Point(15.0, 25.0), 204 | Point(5.0, 15.0), 205 | Point(20.0, 26.0), 206 | Point(25.0, 25.0) 207 | ] 208 | # 网格可视化 209 | plt.grid(True) 210 | plt.axis("equal") 211 | # 进行规划 212 | potential_field_planner = PotentialFieldPlanner(p_start, p_goal, p_obstacles) 213 | planned_path = potential_field_planner.planning() 214 | plt.show() 215 | 216 | if __name__ == "__main__": 217 | print(__file__, "start") 218 | main() -------------------------------------------------------------------------------- /probabilistic_roadmap/ProbabilisticRoadMap: -------------------------------------------------------------------------------- 1 | Probabilistic RoadMap(PRM)规划方法是一种基于采样的搜索规划算法，与RRT类似。相比于使用栅格，基于采样的搜索算法可以有效提高规划效率，但是其只是在概率上具备完备性，因此无法找到最优路径。此外，由于是基于采样，每次规划出的路径都将不相同。 2 | PRM方法首先对规划区域进行采样，采样点数量预先设定。判断每个采样点是否会与障碍物发生碰撞，如果发生碰撞，则将此点去除。对于每一个点，寻找其的k个最近邻，将其进行连线，判断连线是否会与障碍物发生碰撞，如果不会发生碰撞，则认为两点之间的连线形成路径。以上过程就形成了一个无向图，图的节点就是有效的采样点，图的边就是采样点之间的有效连线。在此基础之上，利用搜索算法（可以是Dijkstra或A星等）找出从起点到终点的路径，就可以得到PRM生成的路径。 -------------------------------------------------------------------------------- /probabilistic_roadmap/probabilistic_roadmap.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/python3 2 | #-*- coding: utf-8 -*- 3 | 4 | """ 5 | 6 | Probabilistic roadmap planning 7 | author: flztiii 8 | 9 | """ 10 | 11 | import numpy as np 12 | import math 13 | import matplotlib.pyplot as plt 14 | import scipy.spatial 15 | import random 16 | import os 17 | 18 | # 全局变量 19 | SAMPLE_NUMBER = 200 # 采样点个数 20 | KNN = 10 # 节点邻域个数 21 | MAX_DIS = 30.0 # 邻域最长距离 22 | 23 | # 节点类 24 | class Node: 25 | def __init__(self, index, prev, cost): 26 | self.index_ = index # 对应下标 27 | self.prev_ = prev # 对应前一个节点下标 28 | self.cost_ = cost # 到达此节点花费的代价 29 | 30 | # 查找树类 31 | class KDTree: 32 | def __init__(self, data): 33 | self.tree_ = scipy.spatial.cKDTree(data) # 构造kdtree 34 | 35 | def search(self, input, k = 1): 36 | """ 37 | 查找输入数据在kdtree中的k个最近邻 38 | :param input: 输入数据，类型为numpy.array 39 | :param k: 需要查找的近邻数量 40 | """ 41 | 42 | distances, indexes = self.tree_.query(input, k) 43 | if k == 1: 44 | distances = [distances] 45 | indexes = [indexes] 46 | return indexes, distances 47 | 48 | # RoadMap类 49 | class RoadMap: 50 | def __init__(self, data, nexts): 51 | self.data_ = data # 地图中的节点数据 52 | self.nexts_= nexts # 每一个节点的邻域节点 53 | 54 | # 通过下标获取节点数据 55 | def getDataByIndex(self, index): 56 | return self.data_[index] 57 | 58 | # 通过下标获取图中相邻节点 59 | def getNextsByIndex(self, index): 60 | return self.nexts_[index] 61 | 62 | # 判断是否发生碰撞 63 | def isCollision(current_point, next_point, robot_size, obstacle_search_tree): 64 | dis = math.sqrt((current_point[0] - next_point[0]) ** 2 + (current_point[1] - next_point[1]) ** 2) 65 | # 注意math.atan2中的参数不能两个都是反过来，是有方向性的 66 | # theta = math.atan2(current_point[1] - next_point[1], current_point[0] - next_point[0])是错误的 67 | theta = math.atan2(next_point[1] - current_point[1], next_point[0] - current_point[0]) 68 | # 如果距离大于最大值，直接判断为碰撞 69 | if dis > MAX_DIS: 70 | return True 71 | # 计算路径上特定点是否与障碍物点距离过小 72 | nstep = round(dis / robot_size) 73 | x = current_point[0] 74 | y = current_point[1] 75 | for i in range(nstep): 76 | indexes, distances = obstacle_search_tree.search(np.array([x, y])) 77 | if distances[0] <= robot_size: 78 | return True 79 | x += robot_size * math.cos(theta) 80 | y += robot_size * math.sin(theta) 81 | # 计算终点与障碍物距离是否过小 82 | indexes, distances = obstacle_search_tree.search(next_point) 83 | if distances[0] <= robot_size: 84 | return True 85 | return False 86 | 87 | # 判断是否发生碰撞（原版） 88 | def is_collision(sx, sy, gx, gy, rr, okdtree): 89 | x = sx 90 | y = sy 91 | dx = gx - sx 92 | dy = gy - sy 93 | yaw = math.atan2(gy - sy, gx - sx) 94 | d = math.sqrt(dx**2 + dy**2) 95 | 96 | if d >= MAX_DIS: 97 | return True 98 | 99 | D = rr 100 | nstep = round(d / D) 101 | 102 | for i in range(nstep): 103 | idxs, dist = okdtree.search(np.array([x, y])) 104 | if dist[0] <= rr: 105 | return True # collision 106 | x += D * math.cos(yaw) 107 | y += D * math.sin(yaw) 108 | 109 | # goal point check 110 | idxs, dist = okdtree.search(np.array([gx, gy])) 111 | if dist[0] <= rr: 112 | return True # collision 113 | 114 | return False # OK 115 | 116 | # 构建road map 117 | def buildRoadMap(sx, sy, gx, gy, ox, oy, robot_size): 118 | # 构造障碍物搜索树 119 | obstacle_search_tree = KDTree(np.vstack((ox, oy)).T) 120 | # 得到区域边界 121 | min_x = min(ox) 122 | max_x = max(ox) 123 | min_y = min(oy) 124 | max_y = max(oy) 125 | width = max_x - min_x 126 | height = max_y - min_y 127 | # 生成随机点 128 | sampled_points = [] 129 | for i in range(SAMPLE_NUMBER): 130 | # 首先在区域中进行坐标采样 131 | sample_x = random.random() * width + min_x 132 | sample_y = random.random() * height + min_y 133 | # 判断采样点与障碍物的距离，如果满足条件则加入列表 134 | indexes, distances = obstacle_search_tree.search(np.array([sample_x, sample_y])) 135 | if distances[0] > robot_size: 136 | sampled_points.append(np.array([sample_x, sample_y])) 137 | # 将起点和终点也加入采样点之中 138 | sampled_points.append(np.array([sx, sy])) 139 | sampled_points.append(np.array([gx, gy])) 140 | # 得到随机点后，下一步计算每个点的邻域 141 | sampled_points_neighbors = [] 142 | sampled_points_tree = KDTree(sampled_points) 143 | for i in range(len(sampled_points)): 144 | # 首先计算每个点到这个点的距离，并以距离从小到大进行排序输出 145 | indexes, distances = sampled_points_tree.search(sampled_points[i], len(sampled_points)) 146 | # 从第二个开始判断是否满足条件 147 | neighbors = [] 148 | for j in range(1, len(indexes)): 149 | # 首先判断距离是否大于最大阈值，如果大于直接退出循环 150 | if distances[j] > MAX_DIS: 151 | break 152 | # 如果小于阈值，判断是否与障碍物相交 153 | current_point = sampled_points[i] 154 | next_point = sampled_points[indexes[j]] 155 | if not isCollision(current_point, next_point, robot_size, obstacle_search_tree): 156 | neighbors.append(indexes[j]) 157 | if len(neighbors) >= KNN: 158 | break 159 | sampled_points_neighbors.append(neighbors) 160 | # 构建road map 161 | road_map = RoadMap(sampled_points, sampled_points_neighbors) 162 | return road_map 163 | 164 | # 利用Dijkstra算法进行搜索 165 | def dijkstraSearch(road_map, sx, sy, gx, gy): 166 | # 首先构建起始节点和目标节点 167 | start_node = Node(len(road_map.data_) - 2, -1, 0.0) 168 | goal_node = Node(len(road_map.data_) - 1, -1, 0.0) 169 | # 初始化查询开放集合与查询关闭集合 170 | open_set, close_set = dict(), dict() 171 | open_set[len(road_map.data_) - 2] = start_node 172 | # 开始循环路径搜索 173 | while True: 174 | # 从查询开放集合 175 | if not open_set: 176 | print("can't find path") 177 | return None, None 178 | # 得到开放集合中代价最低的节点 179 | current_idx = min(open_set, key = lambda o: open_set[o].cost_) 180 | current_node = open_set[current_idx] 181 | # 进行可视化 182 | if len(close_set.keys()) % 2 == 0: 183 | plt.plot(road_map.getDataByIndex(current_node.index_)[0], road_map.getDataByIndex(current_node.index_)[1], "xg") 184 | plt.pause(0.001) 185 | # 判断当前点是否为目标点 186 | if current_idx == len(road_map.data_) - 1: 187 | # 当前点就是目标点 188 | print("find path") 189 | goal_node = current_node 190 | break 191 | # 如果不是目标点，将其从开放集合中删除，加入关闭集合 192 | del open_set[current_idx] 193 | close_set[current_idx] = current_node 194 | # 判断当前点的邻域能否加入查询开放集合 195 | for idx in road_map.getNextsByIndex(current_idx): 196 | # 构建下一个节点 197 | dis = math.sqrt((road_map.getDataByIndex(current_idx)[0] - road_map.getDataByIndex(idx)[0]) ** 2 + (road_map.getDataByIndex(current_idx)[1] - road_map.getDataByIndex(idx)[1]) ** 2) 198 | next_node = Node(idx, current_idx, current_node.cost_ + dis) 199 | # 如果当前下标在闭合集合内，不能加入开放集合 200 | if idx in close_set.keys(): 201 | continue 202 | # 如果不在闭合也不在开放集合，加入集合内 203 | if not idx in open_set.keys(): 204 | open_set[idx] = next_node 205 | else: 206 | # 如果在放开集合内 207 | if open_set[idx].cost_ > next_node.cost_: 208 | open_set[idx] = next_node 209 | 210 | # 完成路径的查询后，开始构建路径 211 | path_x, path_y = [gx], [gy] 212 | check_node = goal_node 213 | while check_node.prev_ != -1: 214 | check_node = close_set[check_node.prev_] 215 | path_x.append(road_map.getDataByIndex(check_node.index_)[0]) 216 | path_y.append(road_map.getDataByIndex(check_node.index_)[1]) 217 | return path_x, path_y 218 | 219 | # 进行road map可视化 220 | def plotRoadMap(road_map): 221 | for i in range(len(road_map.data_)): 222 | data = road_map.data_[i] 223 | plt.scatter(data[0], data[1], color = "b", s = 10) 224 | 225 | # 进行PRM规划 226 | def PRMPlanning(sx, sy, gx, gy, ox, oy, robot_size): 227 | # 构造road map 228 | road_map = buildRoadMap(sx, sy, gx, gy, ox, oy, robot_size) 229 | # 进行可视化 230 | plotRoadMap(road_map) 231 | # 利用Dijkstra算法进行搜索 232 | path_x, path_y = dijkstraSearch(road_map, sx, sy, gx, gy) 233 | return path_x, path_y 234 | 235 | # 主函数 236 | def main(): 237 | print(os.path.abspath(__file__) + " start!!") 238 | 239 | # 起点和终点的位置 240 | sx = 10.0 # [m] 241 | sy = 10.0 # [m] 242 | gx = 50.0 # [m] 243 | gy = 50.0 # [m] 244 | robot_size = 5.0 # [m] 245 | 246 | # 障碍物点位置 247 | ox = [] 248 | oy = [] 249 | 250 | for i in range(60): 251 | ox.append(i) 252 | oy.append(0.0) 253 | for i in range(60): 254 | ox.append(60.0) 255 | oy.append(i) 256 | for i in range(61): 257 | ox.append(i) 258 | oy.append(60.0) 259 | for i in range(61): 260 | ox.append(0.0) 261 | oy.append(i) 262 | for i in range(40): 263 | ox.append(20.0) 264 | oy.append(i) 265 | for i in range(40): 266 | ox.append(40.0) 267 | oy.append(60.0 - i) 268 | 269 | # 进行可视化 270 | plt.plot(sx, sy, '^r') 271 | plt.plot(gx, gy, '^c') 272 | plt.plot(ox, oy, '.k') 273 | plt.axis('equal') 274 | plt.grid(True) 275 | 276 | # 进行PRM规划 277 | path_x, path_y = PRMPlanning(sx, sy, gx, gy, ox, oy, robot_size) 278 | 279 | if path_x is None: 280 | return 281 | 282 | # 进行可视化 283 | plt.plot(path_x, path_y, '-r') 284 | plt.show() 285 | 286 | if __name__ == "__main__": 287 | main() -------------------------------------------------------------------------------- /quintic_polynomial/QuinticPolynomial.md: -------------------------------------------------------------------------------- 1 | Quintic polynomial算法是使用以时间t作为参数的五次多项式来表示x，y坐标的方式来得到曲线的方法。由于是以时间t作为变量参数，因此在曲线规划完成时，曲线上每一个点对应的时间，速度等信息已经是确定的了。 2 | 此算法的核心思路是保证jerk，也就是加速度变化率在一段时间内到达最小，因此，x(t)和y(t)三阶导数的平方和在此时间段内的积分也要达到最小。此时，根据欧拉方程，可以得到x(t)和y(t)的6阶导数为0。因此使用五次多项式是为了保证加速度变化率在一段时间内到达最小。 3 | 代码首先定义了起点和终点的边界条件，在此基础之上，可以计算出x和y关于t的五次多项式方程的系数，对其进行采样就可以得到最终的曲线。 -------------------------------------------------------------------------------- /quintic_polynomial/quintic_polynomial.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/python3 2 | # -*- coding: utf-8 -*- 3 | 4 | """ 5 | 6 | Quintic Polynomial Planning 7 | author: flztiii 8 | 9 | """ 10 | 11 | import numpy as np 12 | import matplotlib.pyplot as plt 13 | 14 | # 全局变量 15 | MIN_T = 5.0 # 到达终点的最小时间开销 16 | MAX_T = 100.0 # 到达终点的最大时间开销 17 | 18 | # 五次多项式 19 | class QuinticPolynomial: 20 | def __init__(self, params, T): 21 | self.params_ = params 22 | self.T_ = T 23 | # 生成多项式系数计算 24 | self.__coefficientGeneration() 25 | 26 | # 生成多项式系数计算 27 | def __coefficientGeneration(self): 28 | a0 = self.params_[0] 29 | a1 = self.params_[1] 30 | a2 = self.params_[2] * 0.5 31 | A = np.array([[self.T_ ** 3, self.T_ ** 4, self.T_ ** 5], [3. * self.T_ ** 2, 4. * self.T_ ** 3, 5. * self.T_ ** 4], [6. * self.T_, 12. * self.T_ ** 2, 20. * self.T_ ** 3]]) 32 | b = np.array([self.params_[3] - a0 - a1 * self.T_ - a2 * self.T_ ** 2, self.params_[4] - a1 - 2. * a2 * self.T_, self.params_[5] - 2. * a2]).T 33 | x = np.linalg.solve(A, b) 34 | a3 = x[0] 35 | a4 = x[1] 36 | a5 = x[2] 37 | self.coefficient_ = np.array([a0, a1, a2, a3, a4, a5]) 38 | 39 | # 给定输入时间，计算对应值 40 | def calcValue(self, t): 41 | return np.dot(self.coefficient_, np.array([1., t, t ** 2, t ** 3, t ** 4, t ** 5]).T) 42 | 43 | # 给定输入时间，计算导数 44 | def calcDerivation(self, t): 45 | return np.dot(self.coefficient_[1:], np.array([1., 2. * t, 3. * t ** 2, 4. * t ** 3, 5. * t ** 4])) 46 | 47 | # 给定输入时间，计算二阶导数 48 | def calc2Derivation(self, t): 49 | return np.dot(self.coefficient_[2:], np.array([2. , 6. * t, 12. * t ** 2, 20. * t ** 3])) 50 | 51 | # 给定输入时间，计算三阶导数 52 | def calc3Derivation(self, t): 53 | return np.dot(self.coefficient_[3:], np.array([6., 24. * t, 60. * t ** 2])) 54 | 55 | # 五次多项式规划 56 | def quinticPolynumialPlanning(start_pose, start_motion, goal_pose, goal_motion, max_accel, max_jerk, dt): 57 | # 初始化参数 58 | params_x = [start_pose[0], start_motion[0] * np.cos(start_pose[2]), start_motion[1] * np.cos(start_pose[2]) - start_motion[0]**2 * start_pose[3] * np.sin(start_pose[2]), goal_pose[0], goal_motion[0] * np.cos(goal_pose[2]), goal_motion[1] * np.cos(goal_pose[2]) - goal_motion[0]**2 * goal_pose[3] * np.sin(goal_pose[2])] 59 | params_y = [start_pose[1], start_motion[0] * np.sin(start_pose[2]), start_motion[1] * np.sin(start_pose[2]) + start_motion[0]**2 * start_pose[3] * np.cos(start_pose[2]), goal_pose[1], goal_motion[0] * np.sin(goal_pose[2]), goal_motion[1] * np.sin(goal_pose[2]) + goal_motion[0]**2 * goal_pose[3] * np.cos(goal_pose[2])] 60 | # 最终的路径，速度，加速度，加速度变化率在时间轴上的采样 61 | final_path, final_vels, final_accs, final_jerks, final_times = [], [], [], [], [] 62 | # 遍历时间 63 | for T in np.arange(MIN_T, MAX_T, MIN_T): 64 | # 分别生成x，y的五次多项式 65 | xqp = QuinticPolynomial(params_x, T) 66 | yqp = QuinticPolynomial(params_y, T) 67 | print(T, xqp.coefficient_, yqp.coefficient_) 68 | # 对多项式进行采样 69 | path, vels, accs, jerks, times = [], [], [], [], [] 70 | for sample_t in np.arange(0, T + dt, dt): 71 | # 计算对应时间的位置等信息 72 | sample_x = xqp.calcValue(sample_t) 73 | sample_dx = xqp.calcDerivation(sample_t) 74 | sample_ddx = xqp.calc2Derivation(sample_t) 75 | sample_dddx = xqp.calc3Derivation(sample_t) 76 | sample_y = yqp.calcValue(sample_t) 77 | sample_dy = yqp.calcDerivation(sample_t) 78 | sample_ddy = yqp.calc2Derivation(sample_t) 79 | sample_dddy = yqp.calc3Derivation(sample_t) 80 | # 采样点位置 81 | sample_pose = [sample_x, sample_y, np.arctan2(sample_dy, sample_dx), (sample_ddy * sample_dx - sample_ddx * sample_dy) / ((sample_dx ** 2 + sample_dy ** 2)**(3 / 2))] 82 | path.append(sample_pose) 83 | # 采样点运动信息 84 | sample_vel = np.hypot(sample_dx, sample_dy) 85 | vels.append(sample_vel) 86 | sample_acc = np.hypot(sample_ddx, sample_ddy) # 这里是加速度的大小，并不是速度大小的变化率，包括速度大小变化率和角速度变化率两部分 87 | if len(vels) > 1 and vels[-1] - vels[-2] < 0: 88 | sample_acc *= -1.0 89 | accs.append(sample_acc) 90 | sample_jerk = np.hypot(sample_dddx, sample_dddy) 91 | if len(accs) > 1 and accs[-1] - accs[-2] < 0: 92 | sample_jerk *= -1.0 93 | jerks.append(sample_jerk) 94 | times.append(sample_t) 95 | # 根据采样信息判断是否能够满足限制条件 96 | if max([np.abs(acc) for acc in accs]) <= max_accel and max([np.abs(jerk) for jerk in jerks]) <= max_jerk: 97 | # 满足限制条件 98 | final_path = np.array(path) 99 | final_vels = np.array(vels) 100 | final_accs = np.array(accs) 101 | final_jerks = np.array(jerks) 102 | final_times = np.array(times) 103 | print("find path") 104 | break 105 | return final_path, final_vels, final_accs, final_jerks, final_times 106 | 107 | # 绘制箭头 108 | def plotArrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"): # pragma: no cover 109 | """Plot arrow.""" 110 | if not isinstance(x, float): 111 | for (ix, iy, iyaw) in zip(x, y, yaw): 112 | plot_arrow(ix, iy, iyaw) 113 | else: 114 | plt.arrow(x, y, length * np.cos(yaw), length * np.sin(yaw), 115 | fc=fc, ec=ec, head_width=width, head_length=width) 116 | plt.plot(x, y) 117 | 118 | # 主函数 119 | def main(): 120 | # 初始化边界条件和限制条件 121 | start_pose = [10.0, 10.0, np.deg2rad(10.0), 0.0] # 起点位置x,y,yaw,kappa 122 | start_motion = [1.0, 0.1] # 起点运动v,a 123 | goal_pose = [30.0, -10.0, np.deg2rad(20.0), 0.0] # 终点位置 124 | goal_motion = [1.0, 0.1] # 终点运动 125 | max_accel = 1.0 # max accel [m/ss] 126 | max_jerk = 0.5 # max jerk [m/sss] 127 | dt = 0.1 # time tick [s] 128 | # 开始进行规划 129 | final_path, final_vels, final_accs, final_jerks, final_times = quinticPolynumialPlanning(start_pose, start_motion, goal_pose, goal_motion, max_accel, max_jerk, dt) 130 | 131 | # 进行可视化 132 | # 首先可视化代理的运动过程 133 | for i, time in enumerate(final_times): 134 | plt.cla() # 清楚之前的绘制信息 135 | plt.grid(True) # 绘制网格 136 | plt.axis("equal") # 绘制坐标 137 | plotArrow(start_pose[0], start_pose[1], start_pose[2]) # 绘制起点 138 | plotArrow(goal_pose[0], goal_pose[1], goal_pose[2]) # 绘制终点 139 | plotArrow(final_path[i][0], final_path[i][1], final_path[i][2]) # 绘制当前位置 140 | plt.title("Time[s]:" + str(time)[0:4] + 141 | " v[m/s]:" + str(final_vels[i])[0:4] + 142 | " a[m/ss]:" + str(final_accs[i])[0:4] + 143 | " jerk[m/sss]:" + str(final_jerks[i])[0:4], 144 | ) 145 | plt.pause(0.01) 146 | # 可视化曲线 147 | plt.plot(final_path[:, 0], final_path[:, 1]) 148 | # 可视化朝向随时间变化曲线 149 | plt.subplots() 150 | plt.plot(final_times, [np.rad2deg(i) for i in final_path[:, 2]], "-r") 151 | plt.xlabel("Time[s]") 152 | plt.ylabel("Yaw[deg]") 153 | plt.grid(True) 154 | # 可视化曲率随时间变化曲线 155 | plt.subplots() 156 | plt.plot(final_times, final_path[:, 3], "-r") 157 | plt.xlabel("Time[s]") 158 | plt.ylabel("Curvature[deg/s]") 159 | plt.grid(True) 160 | # 可视化速度随时间变化曲线 161 | plt.subplots() 162 | plt.plot(final_times, final_vels) 163 | plt.xlabel("Time[s]") 164 | plt.ylabel("Velocity[m/s]") 165 | plt.grid(True) 166 | # 可视化加速度随时间变化曲线 167 | plt.subplots() 168 | plt.plot(final_times, final_accs) 169 | plt.xlabel("Time[s]") 170 | plt.ylabel("Acceleration[m/s^2]") 171 | plt.grid(True) 172 | # 可视化加速度变化率随时间变化曲线 173 | plt.subplots() 174 | plt.plot(final_times, final_jerks) 175 | plt.xlabel("Time[s]") 176 | plt.ylabel("Jerk[m/s^3]") 177 | plt.grid(True) 178 | 179 | plt.show() 180 | 181 | 182 | if __name__ == "__main__": 183 | main() -------------------------------------------------------------------------------- /smoothed_astar/__pycache__/grid_astar.cpython-35.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flztiii/path_planning_basic_algorithm/d1827cbb355416da8a14831eb37a929c5c15e644/smoothed_astar/__pycache__/grid_astar.cpython-35.pyc -------------------------------------------------------------------------------- /smoothed_astar/__pycache__/map_loading.cpython-35.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flztiii/path_planning_basic_algorithm/d1827cbb355416da8a14831eb37a929c5c15e644/smoothed_astar/__pycache__/map_loading.cpython-35.pyc -------------------------------------------------------------------------------- /smoothed_astar/data/BIG_WHITE.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flztiii/path_planning_basic_algorithm/d1827cbb355416da8a14831eb37a929c5c15e644/smoothed_astar/data/BIG_WHITE.png -------------------------------------------------------------------------------- /smoothed_astar/data/SCURVE.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flztiii/path_planning_basic_algorithm/d1827cbb355416da8a14831eb37a929c5c15e644/smoothed_astar/data/SCURVE.png -------------------------------------------------------------------------------- /smoothed_astar/data/SideParking.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flztiii/path_planning_basic_algorithm/d1827cbb355416da8a14831eb37a929c5c15e644/smoothed_astar/data/SideParking.png -------------------------------------------------------------------------------- /smoothed_astar/data/WHITE.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flztiii/path_planning_basic_algorithm/d1827cbb355416da8a14831eb37a929c5c15e644/smoothed_astar/data/WHITE.png -------------------------------------------------------------------------------- /smoothed_astar/data/diku_bare.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flztiii/path_planning_basic_algorithm/d1827cbb355416da8a14831eb37a929c5c15e644/smoothed_astar/data/diku_bare.png -------------------------------------------------------------------------------- /smoothed_astar/data/diku_full.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flztiii/path_planning_basic_algorithm/d1827cbb355416da8a14831eb37a929c5c15e644/smoothed_astar/data/diku_full.png -------------------------------------------------------------------------------- /smoothed_astar/data/diku_lines.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flztiii/path_planning_basic_algorithm/d1827cbb355416da8a14831eb37a929c5c15e644/smoothed_astar/data/diku_lines.png -------------------------------------------------------------------------------- /smoothed_astar/data/diku_point.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flztiii/path_planning_basic_algorithm/d1827cbb355416da8a14831eb37a929c5c15e644/smoothed_astar/data/diku_point.png -------------------------------------------------------------------------------- /smoothed_astar/data/highway.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flztiii/path_planning_basic_algorithm/d1827cbb355416da8a14831eb37a929c5c15e644/smoothed_astar/data/highway.png -------------------------------------------------------------------------------- /smoothed_astar/data/highway_start.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flztiii/path_planning_basic_algorithm/d1827cbb355416da8a14831eb37a929c5c15e644/smoothed_astar/data/highway_start.png -------------------------------------------------------------------------------- /smoothed_astar/data/highway_start_0cm.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flztiii/path_planning_basic_algorithm/d1827cbb355416da8a14831eb37a929c5c15e644/smoothed_astar/data/highway_start_0cm.png -------------------------------------------------------------------------------- /smoothed_astar/data/highway_start_10cm.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flztiii/path_planning_basic_algorithm/d1827cbb355416da8a14831eb37a929c5c15e644/smoothed_astar/data/highway_start_10cm.png -------------------------------------------------------------------------------- /smoothed_astar/data/highway_start_20cm.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flztiii/path_planning_basic_algorithm/d1827cbb355416da8a14831eb37a929c5c15e644/smoothed_astar/data/highway_start_20cm.png -------------------------------------------------------------------------------- /smoothed_astar/data/highway_start_30cm.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flztiii/path_planning_basic_algorithm/d1827cbb355416da8a14831eb37a929c5c15e644/smoothed_astar/data/highway_start_30cm.png -------------------------------------------------------------------------------- /smoothed_astar/data/highway_start_40cm.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flztiii/path_planning_basic_algorithm/d1827cbb355416da8a14831eb37a929c5c15e644/smoothed_astar/data/highway_start_40cm.png -------------------------------------------------------------------------------- /smoothed_astar/data/highwayw.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flztiii/path_planning_basic_algorithm/d1827cbb355416da8a14831eb37a929c5c15e644/smoothed_astar/data/highwayw.png -------------------------------------------------------------------------------- /smoothed_astar/data/k-turn_parking.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flztiii/path_planning_basic_algorithm/d1827cbb355416da8a14831eb37a929c5c15e644/smoothed_astar/data/k-turn_parking.png -------------------------------------------------------------------------------- /smoothed_astar/data/k_turn_up40_down20.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flztiii/path_planning_basic_algorithm/d1827cbb355416da8a14831eb37a929c5c15e644/smoothed_astar/data/k_turn_up40_down20.png -------------------------------------------------------------------------------- /smoothed_astar/data/park.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flztiii/path_planning_basic_algorithm/d1827cbb355416da8a14831eb37a929c5c15e644/smoothed_astar/data/park.png -------------------------------------------------------------------------------- /smoothed_astar/data/scurve_with_cones.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flztiii/path_planning_basic_algorithm/d1827cbb355416da8a14831eb37a929c5c15e644/smoothed_astar/data/scurve_with_cones.png -------------------------------------------------------------------------------- /smoothed_astar/data/sida_with_parking_spot.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flztiii/path_planning_basic_algorithm/d1827cbb355416da8a14831eb37a929c5c15e644/smoothed_astar/data/sida_with_parking_spot.png -------------------------------------------------------------------------------- /smoothed_astar/data/uturn_4.8_3.5_1.5.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flztiii/path_planning_basic_algorithm/d1827cbb355416da8a14831eb37a929c5c15e644/smoothed_astar/data/uturn_4.8_3.5_1.5.png -------------------------------------------------------------------------------- /smoothed_astar/data/uturn_4.8_4._1.5.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flztiii/path_planning_basic_algorithm/d1827cbb355416da8a14831eb37a929c5c15e644/smoothed_astar/data/uturn_4.8_4._1.5.png -------------------------------------------------------------------------------- /smoothed_astar/data/uturn_5.2_3.5_1.5.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flztiii/path_planning_basic_algorithm/d1827cbb355416da8a14831eb37a929c5c15e644/smoothed_astar/data/uturn_5.2_3.5_1.5.png -------------------------------------------------------------------------------- /smoothed_astar/data/uturn_5.2_4._1.5.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flztiii/path_planning_basic_algorithm/d1827cbb355416da8a14831eb37a929c5c15e644/smoothed_astar/data/uturn_5.2_4._1.5.png -------------------------------------------------------------------------------- /smoothed_astar/data/uturn_5._3.5_1.5.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flztiii/path_planning_basic_algorithm/d1827cbb355416da8a14831eb37a929c5c15e644/smoothed_astar/data/uturn_5._3.5_1.5.png -------------------------------------------------------------------------------- /smoothed_astar/data/uturn_5._4._1.5.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flztiii/path_planning_basic_algorithm/d1827cbb355416da8a14831eb37a929c5c15e644/smoothed_astar/data/uturn_5._4._1.5.png -------------------------------------------------------------------------------- /smoothed_astar/data/uturn_6.5_3.5_1.5.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flztiii/path_planning_basic_algorithm/d1827cbb355416da8a14831eb37a929c5c15e644/smoothed_astar/data/uturn_6.5_3.5_1.5.png -------------------------------------------------------------------------------- /smoothed_astar/data/uturn_6.5_4._1.5.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flztiii/path_planning_basic_algorithm/d1827cbb355416da8a14831eb37a929c5c15e644/smoothed_astar/data/uturn_6.5_4._1.5.png -------------------------------------------------------------------------------- /smoothed_astar/data/uturn_blocked.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/flztiii/path_planning_basic_algorithm/d1827cbb355416da8a14831eb37a929c5c15e644/smoothed_astar/data/uturn_blocked.png -------------------------------------------------------------------------------- /smoothed_astar/data/起点信息.txt: -------------------------------------------------------------------------------- 1 | 原：5464px 1/183*200 2 | 0.5cm=1px，1m=200px 3 | 6000px*4000px = 30m*20m 4 | 范围：X:-30~0， Y：-7.5~12.5 -------------------------------------------------------------------------------- /smoothed_astar/map_loading.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/python3 2 | #! -*- coding: utf-8 -*- 3 | 4 | """ 5 | map loading tool 6 | author: flztiii 7 | 8 | """ 9 | 10 | import matplotlib.pyplot as plt 11 | import numpy as np 12 | import sys 13 | import cv2 14 | import grid_astar 15 | 16 | class MapLoader: 17 | def __init__(self): 18 | pass 19 | 20 | # 加载地图 21 | def loadMap(self, path, resolution = 0.05): 22 | img = cv2.imread(path) 23 | # 获取图片宽度和高度 24 | width, height = img.shape[:2][::-1] 25 | # 转为灰度图 26 | img_gray = cv2.cvtColor(img,cv2.COLOR_RGB2GRAY) 27 | print(img_gray.shape) 28 | print(width, height) 29 | # 读取障碍物栅格 30 | obstacles = [] 31 | for i in range(0, width): 32 | for j in range(0, height): 33 | if img_gray[j, i] < 122: 34 | point = grid_astar.Point(i * resolution, j * resolution) 35 | obstacles.append(point) 36 | return obstacles 37 | 38 | 39 | if __name__ == "__main__": 40 | map_loader = MapLoader() 41 | path = '/home/flztiii/pythonrobotic_myself/smoothed_astar/data/highwayw.png' 42 | obstacles = map_loader.loadMap(path) 43 | print(len(obstacles)) 44 | # 进行可视化 45 | obstalces_x, obstacles_y = grid_astar.Tools.departPoints(obstacles) 46 | plt.plot(obstalces_x, obstacles_y, ".k") 47 | plt.grid(True) 48 | plt.axis("equal") 49 | plt.show() 50 | -------------------------------------------------------------------------------- /smoothed_astar/test.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/python3 2 | #! -*- coding: utf-8 -*- 3 | 4 | import numpy as np 5 | import matplotlib.pyplot as plt 6 | import map_loading 7 | import grid_astar 8 | import copy 9 | 10 | # 测试函数1 11 | def test1(): 12 | # 设定起点和终点 13 | start = grid_astar.Point(2.5, 11.0) 14 | goal = grid_astar.Point(2.5, 21.0) 15 | # 得到初始设定 16 | config = grid_astar.Config() 17 | config.resolution_ = 0.5 18 | # 加载障碍物信息 19 | map_loader = map_loading.MapLoader() 20 | path = '/home/flztiii/pythonrobotic_myself/smoothed_astar/data/uturn_4.8_3.5_1.5.png' 21 | raw_obstacles = map_loader.loadMap(path) 22 | # 对障碍物进行过滤,在同一个栅格内的障碍物忽略 23 | obstacles = [] 24 | for obstacle in raw_obstacles: 25 | new_obstacle = grid_astar.Point(int(obstacle.x_ / config.resolution_) * config.resolution_, int(obstacle.y_ / config.resolution_) * config.resolution_) 26 | if not grid_astar.Tools.isInPointSet(new_obstacle, obstacles): 27 | obstacles.append(new_obstacle) 28 | print(len(raw_obstacles), len(obstacles)) 29 | # 对当前信息进行可视化 30 | obstalces_x, obstacles_y = grid_astar.Tools.departPoints(obstacles) 31 | plt.plot(obstalces_x, obstacles_y, ".k") 32 | plt.plot(start.x_, start.y_, "og") 33 | plt.plot(goal.x_, goal.y_, "xb") 34 | plt.grid(True) 35 | plt.axis("equal") 36 | # 构建A星规划器 37 | astar_planner = grid_astar.AstarPlanner(start, goal, obstacles, config) 38 | planned_path = astar_planner.planning() 39 | # 可视化规划路径 40 | path_x, path_y = grid_astar.Tools.departPoints(planned_path) 41 | # 对路径进行平滑 42 | smoother = grid_astar.Smoother() 43 | if len(planned_path) > 5: 44 | smoothed_path = smoother.smoothing(planned_path, obstacles) 45 | # 可视化平滑路径 46 | spath_x, spath_y = grid_astar.Tools.departPoints(smoothed_path) 47 | plt.cla() 48 | plt.plot(spath_x, spath_y, "-g") 49 | plt.plot(obstalces_x, obstacles_y, ".k") 50 | plt.plot(start.x_, start.y_, "og") 51 | plt.plot(goal.x_, goal.y_, "xb") 52 | plt.grid(True) 53 | plt.axis("equal") 54 | plt.plot(path_x, path_y, "-r") 55 | plt.show() 56 | 57 | # 测试函数2 58 | def test2(): 59 | # 设定起点和终点 60 | start = grid_astar.Point(2.5, 7.5) 61 | goal = grid_astar.Point(2.5, 21.0) 62 | # 得到初始设定 63 | config = grid_astar.Config() 64 | config.resolution_ = 0.5 65 | # 加载障碍物信息 66 | map_loader = map_loading.MapLoader() 67 | path = '/home/flztiii/pythonrobotic_myself/smoothed_astar/data/uturn_6.5_4._1.5.png' 68 | raw_obstacles = map_loader.loadMap(path) 69 | # 对障碍物进行过滤,在同一个栅格内的障碍物忽略 70 | obstacles = [] 71 | for obstacle in raw_obstacles: 72 | new_obstacle = grid_astar.Point(int(obstacle.x_ / config.resolution_) * config.resolution_, int(obstacle.y_ / config.resolution_) * config.resolution_) 73 | if not grid_astar.Tools.isInPointSet(new_obstacle, obstacles): 74 | obstacles.append(new_obstacle) 75 | print(len(raw_obstacles), len(obstacles)) 76 | # 对当前信息进行可视化 77 | obstalces_x, obstacles_y = grid_astar.Tools.departPoints(obstacles) 78 | plt.plot(obstalces_x, obstacles_y, ".k") 79 | plt.plot(start.x_, start.y_, "og") 80 | plt.plot(goal.x_, goal.y_, "xb") 81 | plt.grid(True) 82 | plt.axis("equal") 83 | # 构建A星规划器 84 | astar_planner = grid_astar.AstarPlanner(start, goal, obstacles, config) 85 | planned_path = astar_planner.planning() 86 | # 可视化规划路径 87 | path_x, path_y = grid_astar.Tools.departPoints(planned_path) 88 | # 对路径进行平滑 89 | smoother = grid_astar.Smoother() 90 | show_path = [] 91 | if len(planned_path) > 5: 92 | smoothed_path = smoother.smoothing(planned_path, obstacles) 93 | # 可视化平滑路径 94 | spath_x, spath_y = grid_astar.Tools.departPoints(smoothed_path) 95 | show_path = copy.deepcopy(smoothed_path) 96 | plt.cla() 97 | plt.plot(spath_x, spath_y, "-g") 98 | plt.plot(obstalces_x, obstacles_y, ".k") 99 | plt.plot(start.x_, start.y_, "og") 100 | plt.plot(goal.x_, goal.y_, "xb") 101 | plt.grid(True) 102 | plt.axis("equal") 103 | plt.plot(path_x, path_y, "-r") 104 | # 可视化路径的角度变化 105 | spath_x, spath_y = grid_astar.Tools.departPoints(show_path) 106 | yaws = [] 107 | for i in range(0, len(spath_x) - 1): 108 | yaw = np.arctan2(spath_y[i + 1] - spath_y[i], spath_x[i + 1] - spath_x[i]) 109 | yaws.append(yaw) 110 | yaws.append(yaws[-1]) 111 | fig = plt.figure() 112 | plt.plot(np.linspace(0, len(yaws), len(yaws)), yaws) 113 | # 对路径的角度进行平滑 114 | sigma_s, simga_yaw = 5.0, 0.5 115 | blurred_yaws = smoother.yawBlur(show_path, sigma_s, simga_yaw) 116 | fig = plt.figure() 117 | plt.plot(np.linspace(0, len(blurred_yaws), len(blurred_yaws)), blurred_yaws) 118 | plt.show() 119 | 120 | # 测试函数3 121 | def test3(): 122 | # 设定起点和终点 123 | start = grid_astar.Point(33, 5) 124 | goal = grid_astar.Point(133, 148) 125 | # 得到初始设定 126 | config = grid_astar.Config() 127 | config.resolution_ = 1.0 128 | # 加载障碍物信息 129 | map_loader = map_loading.MapLoader() 130 | path = '/home/flztiii/pythonrobotic_myself/smoothed_astar/data/scurve_with_cones.png' 131 | raw_obstacles = map_loader.loadMap(path) 132 | # 对障碍物进行过滤,在同一个栅格内的障碍物忽略 133 | obstacles = [] 134 | for obstacle in raw_obstacles: 135 | new_obstacle = grid_astar.Point(int(obstacle.x_ / config.resolution_) * config.resolution_, int(obstacle.y_ / config.resolution_) * config.resolution_) 136 | if not grid_astar.Tools.isInPointSet(new_obstacle, obstacles): 137 | obstacles.append(new_obstacle) 138 | print(len(raw_obstacles), len(obstacles)) 139 | # 对当前信息进行可视化 140 | obstalces_x, obstacles_y = grid_astar.Tools.departPoints(obstacles) 141 | plt.plot(obstalces_x, obstacles_y, ".k") 142 | plt.plot(start.x_, start.y_, "og") 143 | plt.plot(goal.x_, goal.y_, "xb") 144 | plt.grid(True) 145 | plt.axis("equal") 146 | # 构建A星规划器 147 | astar_planner = grid_astar.AstarPlanner(start, goal, obstacles, config) 148 | planned_path = astar_planner.planning() 149 | # 可视化规划路径 150 | path_x, path_y = grid_astar.Tools.departPoints(planned_path) 151 | # 对路径进行平滑 152 | show_path = [] 153 | smoother = grid_astar.Smoother() 154 | if len(planned_path) > 5: 155 | smoothed_path = smoother.smoothing(planned_path, obstacles) 156 | # 可视化平滑路径 157 | spath_x, spath_y = grid_astar.Tools.departPoints(smoothed_path) 158 | show_path = copy.deepcopy(smoothed_path) 159 | plt.cla() 160 | plt.plot(spath_x, spath_y, "-g") 161 | plt.plot(obstalces_x, obstacles_y, ".k") 162 | plt.plot(start.x_, start.y_, "og") 163 | plt.plot(goal.x_, goal.y_, "xb") 164 | plt.grid(True) 165 | plt.axis("equal") 166 | plt.plot(path_x, path_y, "-r") 167 | # 可视化路径的角度变化 168 | spath_x, spath_y = grid_astar.Tools.departPoints(show_path) 169 | yaws = [] 170 | for i in range(0, len(spath_x) - 1): 171 | yaw = np.arctan2(spath_y[i + 1] - spath_y[i], spath_x[i + 1] - spath_x[i]) 172 | yaws.append(yaw) 173 | yaws.append(yaws[-1]) 174 | fig = plt.figure() 175 | plt.plot(np.linspace(0, len(yaws), len(yaws)), yaws) 176 | # 对路径的角度进行平滑 177 | sigma_s, simga_yaw = 5.0, 0.4 178 | blurred_yaws = smoother.yawBlur(show_path, sigma_s, simga_yaw) 179 | fig = plt.figure() 180 | plt.plot(np.linspace(0, len(blurred_yaws), len(blurred_yaws)), blurred_yaws) 181 | plt.show() 182 | 183 | if __name__ == "__main__": 184 | test1() -------------------------------------------------------------------------------- /state_lattice/Boss2008Darpa: -------------------------------------------------------------------------------- 1 | 对于2008年DARPA城市挑战赛冠军Boss的规划论文进行梳理。 2 | Boss的规划分为了三个模块，分别是任务规划，行为规划和运动规划。任务规划是给出全局导航路径，行为规划是提供参数给运动规划（参数包括规划的终点状态、规划最大速度、规划期望加速度等等），运动规划根据行为规划给出的参数规划出车辆的行驶轨迹。 3 | 运动规划是通过基于model predictive trajectory（MPT）的state lattice算法实现的。MPT算法思路为通过定义一些参数对车辆控制进行调整，从而得到此控制下车辆未来的行驶路径，根据行驶轨迹和约束条件计算代价函数，最终根据代价优化定义的参数直到生成最优路径。从上述可知，这个算法最重要的两个部分为：第一，如何通过参数构建车辆运动模型；第二，如何确定约束条件。约束条件是根据车辆的初始状态和目标状态确定的（位置、朝向和曲率），而lattice采样的就是目标状态，从而生成不同目标状态的路径。参数构建车辆运动模型实现思路为：首先将速度和路径进行分离，将速度与时间进行建模。模型包括常量、线性、ramp、trapezoidal等。而在速度完成与时间的建模后，路径实际上就变成了曲率与路程的建模，这里采用二阶多项式来表示曲率与路程模型，因此存在三个自由度。因此需要三个参数来完成模型的求解，这三个参数就是路程总长度、中间曲率和目标曲率。确定模型后，就是通过约束条件和MPT对这三个参数进行优化，得到最终的路径。 4 | 接下来是将如何在真实环境中使用上述算法。第一步是提取道路信息，也就是得到期望行驶的道路的中心线。第二步是生成路径，路径的终点与起点在纵向上存在恒定距离，这个距离与车辆速度有关，横向上终点延道路垂直方向分布。道路起点生成分为两种，一种是平滑的，起点的曲率与真实曲率相同，另一种是快速的，曲率是与真实曲率存在一定偏移的。第三步是生成速度，速度的生成是与时间构成一定模型的。第四部是对每个路径进行判断得到最优路径，判断的依据包括与静态、动态障碍物的碰撞、与中心线的距离、平滑性和速度等等。 -------------------------------------------------------------------------------- /state_lattice/StateLattice: -------------------------------------------------------------------------------- 1 | state lattice算法思想是在state状态空间下进行采样，通过采样的方式得到多种线形的路径，并最终选出最优路径的方法。相比于在control空间采样，state空间采样优势在于可以引入更加丰富的边界条件如道路边界限制以及全局规划路径限制等。 2 | 算法实现如下：算法通过起点的位置、朝向、曲率作为初始限制条件，以终点的位置、朝向作为结束限制条件。利用这两个限制条件就可以使用model predictive trajectory generation算法生成从起点到终点并满足边界条件的路径，并且在此过程中使用查表法可以提升算法的效率。下一步就是对终点的位置、朝向进行采样，采样的方法有三种。第一种是均匀采样，保持终点到起点的距离不变，连线夹角在一定范围内变化，生成不同的终点位置；终点朝向在一定范围内直接进行采样，位置与朝向的组合就是采样state。第二种方式是加入了全局规划导向，在上一种方法的基础上，加入全局规划的朝向，则越接近全局规划朝向，位置采样越密集，越远离全局规划朝向，位置采样越稀疏。第三种方法是引入了道路信息，加入道路宽度和朝向，在这种方法中，采样点沿着道路的横向进行分布。通过采样不同的终点，可以生成不同的路径，再对路径进行判断，就可以选出最优路径。 -------------------------------------------------------------------------------- /state_lattice/lookuptable.csv: -------------------------------------------------------------------------------- 1 | x,y,yaw,s,km,kf 2 | 1.0,0.0,0.0,1.0,0.0,0.0 3 | 0.9734888894493215,-0.009758406565994977,0.5358080146312756,0.9922329557399788,-0.10222538550473198,3.0262632253145982 4 | 10.980728996433243,-0.0003093605787364978,0.522622783944529,11.000391678142623,0.00010296091030877934,0.2731556687244648 5 | 16.020309241920156,0.0001292339008200291,0.5243399938698222,16.100019813021202,0.00013263212395994706,0.18999138959173634 6 | 20.963495745193626,-0.00033031017429944326,0.5226120033275024,21.10082901143343,0.00011687467551566884,0.14550546012583987 7 | 6.032553475650599,2.008504211720188,0.5050517859971599,6.400329805864408,0.1520002249689879,-0.13105940607691127 8 | 10.977487445230075,2.0078696810700034,0.5263634407901872,11.201040572298973,0.04895863722280565,0.08356555007223682 9 | 15.994057699325753,2.025659106131227,0.5303858891065698,16.200300421483128,0.0235708657178127,0.10002225103921249 10 | 20.977228843605943,2.0281289825388513,0.5300376140865567,21.20043308669372,0.013795675421657671,0.09331700188063087 11 | 25.95453914157977,1.9926432818499131,0.5226203078411618,26.200880299840527,0.00888830054451281,0.0830622000626594 12 | 0.9999999999999999,0.0,0.0,1.0,0.0,0.0 13 | 5.999999999670752,5.231312388722274e-05,1.4636120911014667e-05,5.996117185283419,4.483756968024291e-06,-3.4422519205046243e-06 14 | 10.999749470720566,-0.011978787043239986,0.022694802592583763,10.99783855994015,-0.00024209503125174496,0.01370491008661795 15 | 15.999885224357776,-0.018937230084507616,0.011565580878015763,15.99839381389597,-0.0002086399372890716,0.005267247862667496 16 | 20.999882688881286,-0.030304200126461317,0.009117836885732596,20.99783120184498,-0.00020013159571184735,0.0034529188783636866 17 | 25.999911270030413,-0.025754431694664327,0.0074809531598503615,25.99674782258235,-0.0001111138115390646,0.0021946603965658368 18 | 10.952178818975062,1.993067260835455,0.0045572480669707136,11.17961498195845,0.04836813285436623,-0.19328716251760758 19 | 16.028306009258,2.0086286208315407,0.009306594796759554,16.122411866381054,0.02330689045417979,-0.08877002085985948 20 | 20.971603115419715,1.9864158336174966,0.007016819403167119,21.093006725076872,0.013439123130720928,-0.05238318300611623 21 | 25.997019676818372,1.9818581321430093,0.007020172975955249,26.074021794586585,0.00876496148602802,-0.03362579291686346 22 | 16.017903482982604,4.009490840390534,-5.293114796312698e-05,16.600937712976638,0.044543450568614244,-0.17444651314466567 23 | 20.98845988414163,3.956600199823583,-0.01050744134070728,21.40149119463485,0.02622674388276059,-0.10625681152519345 24 | 25.979448381017914,3.9968223055054977,-0.00012819253386682928,26.30504721211744,0.017467093413146118,-0.06914750106424669 25 | 25.96268055563514,5.9821266846168,4.931311239565056e-05,26.801388563459287,0.025426008913226557,-0.10047663078001536 26 | -------------------------------------------------------------------------------- /state_lattice/state_lattice.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/python3 2 | # -*- coding: utf-8 -*- 3 | 4 | """ 5 | 6 | State lattice method 7 | author: flztiii 8 | 9 | """ 10 | 11 | import sys 12 | import os 13 | import numpy as np 14 | import matplotlib.pyplot as plt 15 | import math 16 | import pandas as pd 17 | 18 | # 将model predictive trajectory generation的工作目录加入 19 | sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))) + "/model_predictive_trajectory_generation/") 20 | 21 | import model_predictive_trajectory 22 | import model 23 | 24 | # 在查找表中找到最近的对应参数 25 | def selectNearestTable(target, lookup_table): 26 | min_dis = float("inf") 27 | min_index = -1 28 | for i in range(len(lookup_table)): 29 | table = lookup_table[i] 30 | x_gap = table[0] - target.x_ 31 | y_gap = table[1] - target.y_ 32 | yaw_gap = table[2] - target.yaw_ 33 | dis = math.sqrt(x_gap ** 2 + y_gap ** 2 + yaw_gap ** 2) 34 | if dis < min_dis: 35 | min_dis = dis 36 | min_index = i 37 | return lookup_table[min_index] 38 | 39 | 40 | # 进行状态空间采样 41 | def calcUniformSampledState(d, da_min, da_max, n_xy, ha_min, ha_max, n_h): 42 | # 采样终点状态 43 | targets = [] 44 | for i in range(n_xy): 45 | # 起点到终点连线夹角 46 | da = da_min + i * (da_max - da_min) / (n_xy - 1) 47 | for j in range(n_h): 48 | # 终点车辆朝向角 49 | ha = ha_min + j * (ha_max - ha_min) / (n_h - 1) + da 50 | # 得到终点采样 51 | target = model.State(d * math.cos(da), d* math.sin(da), ha) 52 | targets.append(target) 53 | return targets 54 | 55 | # 进行状态空间采样 56 | def calcLaneSampledState(l_lateral, l_longitude, l_heading, l_width, vehicle_width, n_smapling): 57 | # 首先将车辆与道路路点之间的关系转化为路点的世界坐标系 58 | l_xc = l_longitude * math.cos(l_heading) + l_lateral * math.sin(l_heading) 59 | l_yc = l_longitude * math.sin(l_heading) + l_lateral * math.cos(l_heading) 60 | # 计算采样偏移 61 | offset = (l_width - vehicle_width) / (n_smapling - 1) 62 | # 得到采样状态 63 | targets = [] 64 | for i in range(n_smapling): 65 | x = l_xc - (- 0.5 * (l_width - vehicle_width) + i * offset) * math.sin(l_heading) 66 | y = l_yc + (- 0.5 * (l_width - vehicle_width) + i * offset) * math.cos(l_heading) 67 | print(x, y) 68 | target = model.State(x, y, l_heading) 69 | targets.append(target) 70 | return targets 71 | 72 | # 将终点转化为对应路径生成参数 73 | def stateToParam(targets, k0): 74 | # 输入参数targets为State类型列表，k0为初始方向盘转角 75 | # 第一步加载查找表 76 | lookup_table = np.array(pd.read_csv(os.path.dirname(os.path.abspath(__file__))+"/lookuptable.csv")) 77 | # 第二步找出每一个终点对应的路径生成参数 78 | ps = [] 79 | for target in targets: 80 | # 计算查找表中与当前终点最近的点 81 | selected_table = selectNearestTable(target, lookup_table) 82 | # 构造初始参数 83 | init_p = np.array([math.sqrt(target.x_ ** 2 + target.y_ ** 2), selected_table[4], selected_table[5]]).reshape((3, 1)) 84 | # 进行model predictive得到优化参数 85 | x, y, yaw, p = model_predictive_trajectory.optimizeTrajectory(target, init_p, k0) 86 | # 将优化后路径对应的参数p保存 87 | if not x is None: 88 | ps.append(p) 89 | return ps 90 | 91 | # 均匀状态空间采样 92 | def uniformStateSpaceSampling(): 93 | # 首先定义初始参数 94 | dis = 20.0 # 从起点到终点的距离 95 | dis_min_angle = np.deg2rad(-45) # 起点到终点连线最小角度 96 | dis_max_angle = np.deg2rad(45) # 起点到终点连线最大角度 97 | head_min_angle = np.deg2rad(-45) # 终点最小朝向角 98 | head_max_angle = np.deg2rad(45) # 终点最大朝向角 99 | n_xy_sample = 5 # 位置采样个数 100 | n_head_sample = 3 # 角度采样个数 101 | # 限制参数 102 | k0 = 0.0 # 初始方向盘转角 103 | # 进行终点采样，得到不同采样终点对应的路径生成参数 104 | assert(n_xy_sample > 1 and n_head_sample > 1) 105 | sampled_states = calcUniformSampledState(dis, dis_min_angle, dis_max_angle, n_xy_sample, head_min_angle, head_max_angle, n_head_sample) 106 | # 计算终点对应参数 107 | params = stateToParam(sampled_states, k0) 108 | # 用每一组参数生成路径，并进行可视化 109 | plt.clf() 110 | for param in params: 111 | x, y, yaw = model.generateTrajectory(param[0, 0], k0, param[1, 0], param[2, 0]) 112 | plt.plot(x, y, '-r') 113 | plt.grid(True) 114 | plt.axis("equal") 115 | plt.show() 116 | 117 | # 道路约束采样 118 | def laneStateSpaceSampling(): 119 | # 初始参数 120 | l_lateral = 10.0 # 起点在道路的横向偏移 121 | l_longitude = 6.0 # 起点在道路的纵向偏移 122 | l_heading = np.deg2rad(45) # 道路朝向 123 | l_width = 3.0 # 道路宽度 124 | vehicle_width = 1.0 # 车辆宽度 125 | n_smapling = 5 # 采样个数 126 | 127 | # 车辆初始方向盘转角 128 | k0 = 0.0 129 | 130 | # 得到状态采样 131 | assert(n_smapling > 1) 132 | targets = calcLaneSampledState(l_lateral, l_longitude, l_heading, l_width, vehicle_width, n_smapling) 133 | # 将采样状态转化为路径生成参数 134 | params = stateToParam(targets, k0) 135 | # 根据参数生成路径 136 | plt.clf() 137 | for param in params: 138 | x, y, yaw = model.generateTrajectory(param[0, 0], k0, param[1, 0], param[2, 0]) 139 | plt.plot(x, y, '-r') 140 | # 可视化 141 | plt.grid(True) 142 | plt.axis("equal") 143 | plt.show() 144 | 145 | 146 | # 主函数 147 | def main(): 148 | # 均匀状态空间采样 149 | uniformStateSpaceSampling() 150 | # 目标点朝向密集采样 151 | # 道路约束采样 152 | laneStateSpaceSampling() 153 | 154 | if __name__ == "__main__": 155 | main() -------------------------------------------------------------------------------- /voronoi_roadmap/VoronoiRoadMap: -------------------------------------------------------------------------------- 1 | VoronoiRoadMap方法与PRM方法类似，只不过PRM是通过随机采样生成路图，而VRM方法是通过维诺图来生成路图，通过维诺图的方法好处在于可以得到离障碍物点最远的路径。 2 | VRM方法首先根据障碍物点的信息得到维诺图，在维诺图基础上根据每个点之间的距离得到路图，最后在路图中采样Dijkstra方法进行搜索，得到最终路径。 -------------------------------------------------------------------------------- /voronoi_roadmap/voronoi_roadmap.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/python3 2 | # -*- coding: utf-8 -*- 3 | 4 | """ 5 | 6 | Voronoi roadmap planning 7 | author: flztiii 8 | 9 | """ 10 | 11 | import numpy as np 12 | import math 13 | import scipy.spatial 14 | import matplotlib.pyplot as plt 15 | 16 | # 全局变量 17 | KNN = 10 # 邻居点个数 18 | MAX_DISTANCE = 30.0 # 两邻居之间的最长距离 19 | 20 | # 节点类 21 | class Node: 22 | def __init__(self, index, cost, prev_index = -1): 23 | self.index_ = index 24 | self.cost_ = cost 25 | self.prev_index_ = prev_index 26 | 27 | # KD树类 28 | class KDTree: 29 | def __init__(self, data): 30 | # 输入的data为numpy数组 31 | self.kd_tree_ = scipy.spatial.KDTree(data) 32 | 33 | # 查找KD树中离给出点最近的k个点 34 | def searchKNeighbor(self, search_datas, k = 1): 35 | distances, indexes = [],[] 36 | for data in search_datas: 37 | distance, index = self.kd_tree_.query(data, k) 38 | distances.append(distance) 39 | indexes.append(index) 40 | return distances, indexes 41 | 42 | # 查找KD树中离给出点距离小于thred的点 43 | def searchRangeNeighbor(self, search_datas, thred): 44 | indexes = [] 45 | for data in search_datas: 46 | index = self.kd_tree_.query_ball_tree(data, thred) 47 | indexes.append(index) 48 | return indexes 49 | 50 | # 生成采样图 51 | def generateSamplingMap(sx, sy, gx, gy, ox, oy): 52 | obstacle_points = np.vstack((ox, oy)).T 53 | # 加入维诺图采样 54 | voronoi_map = scipy.spatial.Voronoi(obstacle_points) 55 | sample_x, sample_y = [], [] 56 | for point in voronoi_map.vertices: 57 | sample_x.append(point[0]) 58 | sample_y.append(point[1]) 59 | # 加入起点和终点 60 | sample_x.append(sx) 61 | sample_x.append(gx) 62 | sample_y.append(sy) 63 | sample_y.append(gy) 64 | return sample_x, sample_y 65 | 66 | # 判断是否发生碰撞 67 | def isCollision(sx, sy, ex, ey, obstacle_kdtree, robot_size): 68 | distance = math.sqrt((ex - sx) ** 2 + (ey - sy) ** 2) 69 | theta = math.atan2(ey - sy, ex - sx) 70 | steps = math.floor(distance / robot_size) 71 | if distance > MAX_DISTANCE: 72 | return True 73 | for i in range(0, steps): 74 | x = sx + i * robot_size * math.cos(theta) 75 | y = sy + i * robot_size * math.sin(theta) 76 | distances, _ = obstacle_kdtree.searchKNeighbor(np.array([[x, y]])) 77 | if distances[0] <= robot_size: 78 | return True 79 | distances, _ = obstacle_kdtree.searchKNeighbor(np.array([[ex, ey]])) 80 | if distances[0] <= robot_size: 81 | return True 82 | return False 83 | # 生成路图 84 | def generateRoadMap(sample_x, sample_y, ox, oy, robot_size): 85 | roadmap = [] 86 | # 构建采样点KD树 87 | sample_map_kdtree= KDTree(np.vstack((sample_x, sample_y)).T) 88 | # 构建障碍物KD树 89 | obstacle_kdtree = KDTree(np.vstack((ox, oy)).T) 90 | sample_length = len(sample_x) 91 | for (i, x, y) in zip(range(sample_length), sample_x, sample_y): 92 | # 遍历采样图中的每一个点，找到离其最近的knn个点下标 93 | _, indexes = sample_map_kdtree.searchKNeighbor(np.array([[x, y]]), KNN) 94 | indexes = indexes[0] 95 | # 判断到这knn个点的路径是否与障碍物发生碰撞 96 | no_collision_indexes = [] 97 | for index in indexes: 98 | if not isCollision(x, y, sample_x[index], sample_y[index], obstacle_kdtree, robot_size): 99 | no_collision_indexes.append(index) 100 | roadmap.append(no_collision_indexes) 101 | return roadmap 102 | 103 | # 进行Dijkstra搜索 104 | def dijkstraSearch(sample_x, sample_y, roadmap): 105 | # 初始化开放集合和关闭集合 106 | close_set, open_set = dict(), dict() 107 | # 初始化起点节点，初始化终点节点 108 | start_node = Node(len(sample_x) - 2, 0.0) 109 | end_node = Node(len(sample_x) - 1, 0.0) 110 | # 将起点加入开放集合 111 | open_set[len(sample_x) - 2] = start_node 112 | # 开始进行Dijkstra搜索 113 | while True: 114 | # 判断开放集合中是否存在元素 115 | if not open_set: 116 | print("can not find a path") 117 | return None, None 118 | # 从开放集合中找到代价最小的元素 119 | current_node_index = min(open_set, key = lambda o: open_set[o].cost_) 120 | current_node = open_set[current_node_index] 121 | # 可视化当前搜索点 122 | if len(close_set.keys()) % 2 == 0: 123 | plt.plot(sample_x[current_node_index], sample_y[current_node_index], 'xg') 124 | plt.pause(0.01) 125 | # 判断当前节点是否为最终节点 126 | if current_node.index_ == end_node.index_: 127 | print("find path") 128 | end_node = current_node 129 | break 130 | # 如果不是最终节点，将此节点从开放集合中删除，并加入关闭集合 131 | del open_set[current_node_index] 132 | close_set[current_node_index] = current_node 133 | # 判断当前节点邻居是否可以加入开放集合 134 | for neighbor_index in roadmap[current_node_index]: 135 | # 遍历当前节点的邻居 136 | # 构建邻居节点 137 | distance = math.sqrt((sample_x[neighbor_index] - sample_x[current_node_index]) ** 2 + (sample_y[neighbor_index] - sample_y[current_node_index]) ** 2) 138 | neighbor_node = Node(neighbor_index, current_node.cost_ + distance, current_node_index) 139 | # 判断邻居节点是否在关闭集合中 140 | if neighbor_index in close_set.keys(): 141 | continue 142 | # 如果不在关闭集合中 143 | # 判断邻居节点是否在开放集合中 144 | if neighbor_index in open_set.keys(): 145 | # 如果在开放集合中 146 | # 判断其与开放集合中相比代价是否更小 147 | if open_set[neighbor_index].cost_ > neighbor_node.cost_: 148 | # 替代开放集合 149 | open_set[neighbor_index] = neighbor_node 150 | else: 151 | # 如果不在开放集合中 152 | # 加入开放集合 153 | open_set[neighbor_index] = neighbor_node 154 | # 得到路径后 155 | current_node = end_node 156 | path_x = [sample_x[current_node.index_]] 157 | path_y = [sample_y[current_node.index_]] 158 | while True: 159 | current_node = close_set[current_node.prev_index_] 160 | path_x.append(sample_x[current_node.index_]) 161 | path_y.append(sample_y[current_node.index_]) 162 | if current_node.prev_index_ == -1: 163 | break 164 | path_x.reverse() 165 | path_y.reverse() 166 | return path_x, path_y 167 | 168 | 169 | # VRM规划函数 170 | def VRMPlanning(sx, sy, gx, gy, ox, oy, robot_size): 171 | # 第一步生成采样图 172 | sample_x, sample_y = generateSamplingMap(sx, sy, gx, gy, ox, oy) 173 | # 可视化采样图 174 | plt.plot(sample_x, sample_y, '.b') 175 | 176 | # 第二步生成路图 177 | roadmap = generateRoadMap(sample_x, sample_y, ox, oy, robot_size) 178 | 179 | # 第三步进行Dijkstra搜索 180 | path_x, path_y = dijkstraSearch(sample_x, sample_y, roadmap) 181 | 182 | return path_x, path_y 183 | 184 | # 主函数 185 | def main(): 186 | # 初始化起点和终点信息 187 | sx = 10.0 # [m] 188 | sy = 10.0 # [m] 189 | gx = 50.0 # [m] 190 | gy = 50.0 # [m] 191 | 192 | # 宽度信息 193 | robot_size = 5.0 # [m] 194 | 195 | # 初始化障碍物信息 196 | ox = [] 197 | oy = [] 198 | 199 | for i in range(60): 200 | ox.append(i) 201 | oy.append(0.0) 202 | for i in range(60): 203 | ox.append(60.0) 204 | oy.append(i) 205 | for i in range(61): 206 | ox.append(i) 207 | oy.append(60.0) 208 | for i in range(61): 209 | ox.append(0.0) 210 | oy.append(i) 211 | for i in range(40): 212 | ox.append(20.0) 213 | oy.append(i) 214 | for i in range(40): 215 | ox.append(40.0) 216 | oy.append(60.0 - i) 217 | 218 | # 初始化信息可视化 219 | plt.plot(ox, oy, ".k") 220 | plt.plot(sx, sy, "^r") 221 | plt.plot(gx, gy, "^c") 222 | plt.grid(True) 223 | plt.axis("equal") 224 | 225 | # 开始进行VRM规划 226 | path_x, path_y = VRMPlanning(sx, sy, gx, gy, ox, oy, robot_size) 227 | 228 | # 可视化最终路径 229 | plt.plot(path_x, path_y, 'r') 230 | plt.show() 231 | 232 | if __name__ == "__main__": 233 | main() --------------------------------------------------------------------------------