< cos_value < 1.0 + eps:cos_value = 1.0elif -1.0 - eps < cos_value < -1.0:cos_value = -1.0arc_value = np.arccos(cos_value)angle_value = arc_value * (180 / np.pi)return angle_valuedef get_total_distance(path):"""计算路径总长度:param path::return:"""total_distance = 0for index in range(2, len(path)-1):one = path[index-1]two = path[index]total_distance += np.sqrt(np.square(two[0] - one[0]) + np.square(two[1] - one[0]))print(total_distance)def main():print("============================Start planning your path============================")rand_area = [0, 100]# 区域大小step = 10# 基础步长angle = 60# 最大转向角distance = 5# 最小航行距离start = [0, 0]# 起点goal = [100, 100]# 终点safe = [20, 20]recover = [90, 90]# 障碍物obstacle_list = [(50, 50, 15),(62, 13, 12),(50, 87, 11)]bi_rrt = BiRRT(start=start, goal=goal, angle=angle, step=step, distance=distance, obstacle_list=obstacle_list,rand_area=rand_area, safe=safe, recover=recover)path, path_1, path_2 = bi_rrt.planning()bi_rrt.draw_statistic(path)get_total_distance(path)print("==========================End of planned path==========================")if __name__ == '__main__':main()4. 效果图
文章插图
【Python实现改进后的Bi-RRT算法实例】
经验总结扩展阅读
- Java实现6种常见排序
- 京东app是什么
- C++实现双向RRT算法
- frp服务利用云主机实现Windows远程连接
- WPF开发经验-实现自带触控键盘的TextBox
- frp服务利用云主机docker服务实现Windows远程连接
- 1 python-数据描述与分析
- 51单片机下实现软件模拟IIC通信
- 含源码 手把手教你使用LabVIEW OpenCV DNN实现手写数字识别
- 分享一个Vue实现图片水平瀑布流的插件