四旋翼无人机动力学与控制(3)-路径规划

四旋翼无人机
发布日期:   2024-11-05

        本篇是这个小项目的最后一篇，仓库地址：四旋翼无人机仿真建模: 本仓库实现了四旋翼无人机的动力学仿真建模，并在此基础上完成了控制器的设计，包括串级PID、LQR、MPC等，实现了位置跟踪和路径规划。

        主要是做了一个路径规划用于验证之前的控制器设计是否能够跟踪上规划的路径进而评判控制器的优劣，采用APF-RRT路径规划算法，话不多说上代码： 

  1. # Author：gyx
  2. # Usage: plan
  3. # Last change: 2024-7-20
  4.  
  5. import numpy as np
  6. import matplotlib.pyplot as plt
  7. import random
  8. from scipy.interpolate import CubicSpline
  9. from scipy.interpolate import BSpline
  10. from scipy.ndimage import gaussian_filter1d
  11.  
  12. class APF_RRTStar:
  13.     def __init__(self, start, goal, obstacle_list, x_limits, y_limits, robot_radius=0.5):
  14.         self.start = start
  15.         self.goal = goal
  16.         self.obstacle_list = obstacle_list
  17.         self.x_limits = x_limits
  18.         self.y_limits = y_limits
  19.         self.robot_radius = robot_radius  # Added robot radius
  20.         self.tree_start = [start]
  21.         self.tree_goal = [goal]
  22.         self.parent_start = {start: None}
  23.         self.parent_goal = {goal: None}
  24.         self.goal_reached = False
  25.         self.step_size = 0.8
  26.         self.max_iterations = 10000  # Reasonable iteration limit
  27.         self.connection_point = None  # To store the connection point of both trees
  28.  
  29.     def distance(self, point1, point2):
  30.         return np.linalg.norm(np.array(point1) - np.array(point2))
  31.  
  32.     def is_collision_free(self, point):
  33.         for (ox, oy, radius) in self.obstacle_list:
  34.             if self.distance((ox, oy), point) <= (radius + self.robot_radius):
  35.                 return False
  36.         return True
  37.  
  38.     def get_random_point(self, goal_bias):
  39.         if random.random() < goal_bias:
  40.             return self.goal
  41.         while True:
  42.             point = (random.uniform(self.x_limits[0], self.x_limits[1]),
  43.                      random.uniform(self.y_limits[0], self.y_limits[1]))
  44.             if self.is_collision_free(point):
  45.                 return point
  46.  
  47.     def get_nearest_node(self, point, tree):
  48.         distances = [self.distance(node, point) for node in tree]
  49.         nearest_index = np.argmin(distances)
  50.         return tree[nearest_index]
  51.  
  52.     def generate_new_node(self, from_node, to_point):
  53.         direction = np.array(to_point) - np.array(from_node)
  54.         length = np.linalg.norm(direction)
  55.         if length == 0:
  56.             return None
  57.         direction = direction / length
  58.         new_node = tuple(np.array(from_node) + self.step_size * direction)
  59.         if self.is_collision_free(new_node):
  60.             return new_node
  61.         return None
  62.  
  63.     def connect_trees(self, node_from_start, node_from_goal):
  64.         return self.distance(node_from_start, node_from_goal) < self.step_size
  65.  
  66.     def potential_field(self, point):
  67.         attractive_force = np.array(self.goal) - np.array(point)
  68.         repulsive_force = np.zeros(2)
  69.         for (ox, oy, radius) in self.obstacle_list:
  70.             obstacle_vector = np.array(point) - np.array((ox, oy))
  71.             distance = np.linalg.norm(obstacle_vector)
  72.             if distance <= (radius + self.robot_radius):
  73.                 repulsive_force += obstacle_vector / (distance ** 2)
  74.         return attractive_force + repulsive_force
  75.  
  76.     def search_path(self, goal_bias=0.1):
  77.         iterations = 0
  78.         while not self.goal_reached and iterations < self.max_iterations:
  79.             iterations += 1
  80.  
  81.             # Extend the start tree
  82.             random_point_start = self.get_random_point(goal_bias)
  83.             nearest_node_start = self.get_nearest_node(random_point_start, self.tree_start)
  84.             new_node_start = self.generate_new_node(nearest_node_start, random_point_start)
  85.  
  86.             if new_node_start and self.is_collision_free(new_node_start):
  87.                 self.tree_start.append(new_node_start)
  88.                 self.parent_start[new_node_start] = nearest_node_start
  89.  
  90.                 nearest_node_goal = self.get_nearest_node(new_node_start, self.tree_goal)
  91.                 if self.connect_trees(new_node_start, nearest_node_goal):
  92.                     self.goal_reached = True
  93.                     self.connection_point = (new_node_start, nearest_node_goal)
  94.  
  95.             # Extend the goal tree
  96.             random_point_goal = self.get_random_point(goal_bias)
  97.             nearest_node_goal = self.get_nearest_node(random_point_goal, self.tree_goal)
  98.             new_node_goal = self.generate_new_node(nearest_node_goal, random_point_goal)
  99.  
  100.             if new_node_goal and self.is_collision_free(new_node_goal):
  101.                 self.tree_goal.append(new_node_goal)
  102.                 self.parent_goal[new_node_goal] = nearest_node_goal
  103.  
  104.                 nearest_node_start = self.get_nearest_node(new_node_goal, self.tree_start)
  105.                 if self.connect_trees(new_node_goal, nearest_node_start):
  106.                     self.goal_reached = True
  107.                     self.connection_point = (nearest_node_start, new_node_goal)
  108.  
  109.         if not self.goal_reached:
  110.             print("Failed to find a path within the iteration limit.")
  111.             return []
  112.  
  113.         # Construct the path
  114.         path_start = []
  115.         path_goal = []
  116.         node = self.connection_point[0]
  117.         while node is not None:
  118.             path_start.append(node)
  119.             node = self.parent_start[node]
  120.  
  121.         node = self.connection_point[1]
  122.         while node is not None:
  123.             path_goal.append(node)
  124.             node = self.parent_goal[node]
  125.  
  126.         path_start.reverse()
  127.         path = [self.start]+path_start + path_goal+[self.goal]
  128.         return path
  129.  
  130.     def smooth_path(self, path):
  131.         if len(path) < 3:  # No need to smooth if path has less than 3 points
  132.             return path
  133.  
  134.         x = [point[0] for point in path]
  135.         y = [point[1] for point in path]
  136.  
  137.         # 调整采样点数量
  138.         num_points = len(path) * 5  # 增加采样点数量
  139.         t = np.linspace(0, len(path) - 1, len(path))
  140.         t_new = np.linspace(0, len(path) - 1, num=num_points)
  141.  
  142.         # 使用三次样条进行插值
  143.         cs_x = CubicSpline(t, x)
  144.         cs_y = CubicSpline(t, y)
  145.         smooth_x = cs_x(t_new)
  146.         smooth_y = cs_y(t_new)
  147.  
  148.         # 使用高斯滤波器平滑路径
  149.         smooth_x = gaussian_filter1d(smooth_x, sigma=10)
  150.         smooth_y = gaussian_filter1d(smooth_y, sigma=10)
  151.  
  152.         smooth_path = list(zip(smooth_x, smooth_y))
  153.  
  154.         return smooth_path
  155.  
  156.  
  157.     def draw_graph(self, path=None, smooth_path=None):
  158.         plt.figure()
  159.         for (ox, oy, radius) in self.obstacle_list:
  160.             circle = plt.Circle((ox, oy), radius, color='r', alpha=0.5)
  161.             plt.gca().add_patch(circle)
  162.         for node in self.tree_start:
  163.             if self.parent_start[node] is not None:
  164.                 plt.plot([node[0], self.parent_start[node][0]], [node[1], self.parent_start[node][1]], "g-")
  165.         for node in self.tree_goal:
  166.             if self.parent_goal[node] is not None:
  167.                 plt.plot([node[0], self.parent_goal[node][0]], [node[1], self.parent_goal[node][1]], "b-")
  168.         if path:
  169.             plt.plot([x for (x, y) in path], [y for (x, y) in path], '-o', color='orange', label='Original Path')
  170.         if smooth_path:
  171.             plt.plot([x for (x, y) in smooth_path], [y for (x, y) in smooth_path], '-', color='purple',
  172.                      label='Smoothed Path')
  173.         plt.plot(self.start[0], self.start[1], "bo")
  174.         plt.plot(self.goal[0], self.goal[1], "ro")
  175.         plt.xlim(self.x_limits)
  176.         plt.ylim(self.y_limits)
  177.         plt.grid(True)
  178.         plt.legend()
  179.         # plt.show()
  180.  
  181.  
  182. def main():
  183.     start = (0, 0)
  184.     goal = (10, 10)
  185.     obstacle_list = [
  186.         (5, 5, 0.2),
  187.         (7, 6, 1),
  188.         (2, 3, 1.2),
  189.         (4, 6, 0.4),
  190.         (8, 2, 0.5),
  191.         (2, 8, 0.4)
  192.     ]
  193.     x_limits = (0, 10)
  194.     y_limits = (0, 10)
  195.     robot_radius = 0.25  # Set the robot radius
  196.  
  197.     apf_rrt_star = APF_RRTStar(start, goal, obstacle_list, x_limits, y_limits, robot_radius)
  198.     path = apf_rrt_star.search_path()
  199.  
  200.     if path:
  201.         smooth_path = apf_rrt_star.smooth_path(path)
  202.         apf_rrt_star.draw_graph(path, smooth_path)
  203.         print(smooth_path)
  204.     else:
  205.         print("No path found.")
  206.  
  207.  
  208. class RTT_planer:
  209.     def __init__(self, start, goal, obstacle_list, num_trials=10):
  210.         self.start = start
  211.         self.goal = goal
  212.         self.obstacle_list = obstacle_list
  213.         self.num_trials = num_trials  # 进行多次规划的次数
  214.  
  215.     def way_plan(self):
  216.         x_limits = (0, 10)
  217.         y_limits = (0, 10)
  218.         robot_radius = 0.5  # Set the robot radius
  219.  
  220.         best_path = None
  221.         best_smooth_path = None
  222.         best_rrt = None
  223.         min_length = float('inf')  # 初始化为正无穷
  224.  
  225.         for _ in range(self.num_trials):
  226.             apf_rrt_star = APF_RRTStar(self.start, self.goal, self.obstacle_list, x_limits, y_limits, robot_radius)
  227.             path = apf_rrt_star.search_path()
  228.  
  229.             if path:
  230.                 smooth_path = apf_rrt_star.smooth_path(path)
  231.                 # 计算路径的总长度
  232.                 length = sum(np.linalg.norm(np.array(smooth_path[i]) - np.array(smooth_path[i+1]))
  233.                              for i in range(len(smooth_path) - 1))
  234.  
  235.                 # 比较并更新最优路径
  236.                 if length < min_length:
  237.                     min_length = length
  238.                     best_path = path
  239.                     best_smooth_path = smooth_path
  240.                     best_rrt = apf_rrt_star
  241.  
  242.         if best_path:
  243.             # apf_rrt_star = APF_RRTStar(self.start, self.goal, self.obstacle_list, x_limits, y_limits, robot_radius)
  244.             best_rrt.draw_graph(best_path, best_smooth_path)
  245.             return best_path
  246.         else:
  247.             print("No path found.")
  248.             return None
  249.  
  250. if __name__ == '__main__':
  251.     planner = RTT_planer(start=(0, 0), goal=(10, 10), obstacle_list=[
  252.         (5, 3, 0.6),
  253.         (7, 6, 1),
  254.         (2, 3, 1.2),
  255.         (4, 6, 0.4),
  256.         (8, 2, 0.5),
  257.         (2, 8, 0.4),
  258.         (8,9,0.5)
  259.     ], num_trials=10)  # 进行10次规划
  260.     best_path = planner.way_plan()
  261.     if best_path:
  262.         print("Best path found:", best_path)
  263.         print(len(best_path))
  264.     plt.show()
  265.  
  266.  
  267.  
  268.  
  269. # if __name__ == '__main__':
  270. #     main()
  271. #     plt.show()

最后是尝试了一下结合模糊控制进行跟踪，具体代码在仓库中，这里就不贴出来了，效果如下： 

            </div><div data-report-view="{&quot;mod&quot;:&quot;1585297308_001&quot;,&quot;spm&quot;:&quot;1001.2101.3001.6548&quot;,&quot;dest&quot;:&quot;https://blog.csdn.net/perfectdisaster/article/details/144170119&quot;,&quot;extend1&quot;:&quot;pc&quot;,&quot;ab&quot;:&quot;new&quot;}"><div></div></div>
    </div>
文章作者: 郭昱鑫
文章链接: http://ggggggggx.work.gd/2024/11/05/quad03/
版权声明: 本博客所有文章除特別声明外，均采用 CC BY 4.0 许可协议。转载请注明来源 郭昱鑫 !
四旋翼无人机
赏
  目录