├── ObstacleDetection ├── calculate_pose.py ├── obstacles_detect.py └── 说明.txt ├── README.md ├── UavAgent.py ├── airsim_avoid_APF.py ├── airsim_avoid_APF_3d.py ├── airsim_tracking_carrot.py ├── code_python ├── map_avoid │ ├── 1.png │ ├── 2.png │ ├── 3.png │ ├── 4.png │ ├── 5.png │ ├── 6.png │ ├── 7.png │ ├── map2.bmp │ └── map4.bmp ├── path_for_airsim.npy ├── python_CarrotChasing.py ├── python_avoid_APF.py ├── python_avoid_RRT.py ├── python_tracking_and_avoid.py └── readme.md ├── geometricControl.py ├── mymath.py └── readme.md /ObstacleDetection/calculate_pose.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | 4 | def calculate_pose(depth_planar, depth_perspective): 5 | # get the width and height of img 6 | height = depth_planar.shape[0] 7 | width = depth_planar.shape[1] 8 | 9 | # get orientation 10 | y_orientation = np.arange(0, width).reshape(1, -1) 11 | y_orientation = np.repeat(y_orientation, height, axis=0) 12 | y_orientation = y_orientation - (width - 1) / 2 13 | 14 | z_orientation = np.arange(0, height).reshape(-1, 1) 15 | z_orientation = np.repeat(z_orientation, width, axis=1) 16 | z_orientation = z_orientation - (height - 1) / 2 17 | 18 | scale_orientation = np.sqrt(np.square(y_orientation) + np.square(z_orientation)) 19 | 20 | # calculate position 21 | x_pos = depth_planar.copy() 22 | dis_from_center = np.sqrt(np.square(depth_perspective) - np.square(depth_planar)) 23 | y_pos = dis_from_center * y_orientation / scale_orientation 24 | z_pos = dis_from_center * z_orientation / scale_orientation 25 | 26 | return x_pos, y_pos, z_pos 27 | -------------------------------------------------------------------------------- /ObstacleDetection/obstacles_detect.py: -------------------------------------------------------------------------------- 1 | import airsim 2 | import cv2 3 | from .calculate_pose import * 4 | 5 | 6 | def get_images(client, img_request, vehicle_name): 7 | responses = client.simGetImages(requests=img_request, vehicle_name=vehicle_name) 8 | return responses 9 | 10 | 11 | def get_min_and_max_with_mask(pos, mask, threshold): 12 | pos_for_min = pos.copy() 13 | pos_for_min[mask == 0] = threshold + 1 14 | pos_min = np.min(np.min(pos_for_min)) 15 | 16 | pos_for_max = pos.copy() 17 | pos_for_max[mask == 0] = -(threshold + 1) 18 | pos_max = np.max(np.max(pos_for_max)) 19 | 20 | return pos_min, pos_max 21 | 22 | 23 | def obstacles_detect(client, dis_threshold, vehicle_name=''): 24 | """ 25 | detect obstacles 26 | :param client: multi-rotor client 27 | :param dis_threshold: distance threshold 28 | :return: obstacles(list[Box3D]) 29 | """ 30 | obstacles = [] 31 | 32 | # get response 33 | image_requests = [ 34 | airsim.ImageRequest("front_center", airsim.ImageType.DepthPerspective, pixels_as_float=True, compress=False), 35 | airsim.ImageRequest("front_center", airsim.ImageType.DepthPlanar, pixels_as_float=True, compress=False), 36 | airsim.ImageRequest("front_center", airsim.ImageType.Segmentation, pixels_as_float=False, compress=False)] 37 | image_responses = get_images(client, image_requests, vehicle_name) 38 | 39 | # get:(1)img_DPer(depth_perspective); (2)img_DPla(depth_planar); (3)img_Seg(segment) 40 | img_response_DPer = image_responses[0] 41 | img_DPer = airsim.get_pfm_array(img_response_DPer) 42 | img_response_DPla = image_responses[1] 43 | img_DPla = airsim.get_pfm_array(img_response_DPla) 44 | img_response_Seg = image_responses[2] 45 | img_Seg = np.frombuffer(img_response_Seg.image_data_uint8, dtype=np.uint8) 46 | img_Seg = img_Seg.reshape(img_response_Seg.height, img_response_Seg.width, 3) 47 | img_Seg = cv2.cvtColor(img_Seg, cv2.COLOR_RGB2GRAY) 48 | 49 | # get relative pose 50 | x_pos, y_pos, z_pos = calculate_pose(depth_planar=img_DPla, depth_perspective=img_DPer) 51 | 52 | # seg and detect 53 | seg_labels = np.unique(img_Seg) 54 | for seg_label in seg_labels: 55 | # get connected components 56 | img_Seg_binary = (img_Seg == seg_label).astype(np.uint8) * 255 57 | num_objects, labels = cv2.connectedComponents(img_Seg_binary) 58 | 59 | # detect by threshold 60 | for i in range(num_objects - 1): 61 | label = i + 1 62 | mask = (labels == label).astype(int) # get mask 63 | mask_dist = mask * img_DPer # get mask distance 64 | mask_dist[mask_dist == 0] = dis_threshold + 1 # turn 0 to (threshold + 1) to get min 65 | 66 | # judge by threshold 67 | if np.min(np.min(mask_dist)) < dis_threshold: 68 | x_min, x_max = get_min_and_max_with_mask(pos=x_pos, mask=mask, threshold=dis_threshold) 69 | y_min, y_max = get_min_and_max_with_mask(pos=y_pos, mask=mask, threshold=dis_threshold) 70 | z_min, z_max = get_min_and_max_with_mask(pos=z_pos, mask=mask, threshold=dis_threshold) 71 | obstacle = airsim.Box3D() 72 | obstacle.min = airsim.Vector3r(x_val=x_min, y_val=y_min, z_val=z_min) 73 | obstacle.max = airsim.Vector3r(x_val=x_max, y_val=y_max, z_val=z_max) 74 | obstacles.append(obstacle) 75 | 76 | return obstacles 77 | -------------------------------------------------------------------------------- /ObstacleDetection/说明.txt: -------------------------------------------------------------------------------- 1 | sxb 2 | 3 | calculate_pose.py用于利用深度图计算位置信息 4 | obstacles_detect.py用于检测障碍物信息 5 | 6 | 使用obstacles_detect.py中的obstacles_detect函数进行目标检测 7 | （1）输入参数： 8 | client：airsim.MultirotorClient类 9 | dis_threshold：距离阈值（检测障碍物的距离范围，大于此距离的障碍物不检测） 10 | （2）返回值： 11 | list（airsim.Box3D类） 12 | 其中每个Box3D包含：min：airsim.Vector3r(x_val=x_min, y_val=y_min, z_val=z_min) 13 | max：airsim.Vector3r(x_val=x_min, y_val=y_min, z_val=z_min) 14 | 相当于用长方体表示障碍物 15 | 是相对于相机（front_center）的位置 16 | 其中x为前后方向（向前为正） 17 | y为左右方向（向右为正） 18 | z为上下方向（向下为正） 19 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | ### airsim_python 2 | 3 | ## 目录结构： 4 | 5 | ``` 6 | ├── Readme.md 7 | 8 | ├── airsim_avoid_APF.py // 可运行于AirSim环境中的航路点跟踪+避障代码脚本文件 9 | 10 | ├── airsim_tracking_carrot.py // 可运行于AirSim中的航路点跟踪代码脚本文件 11 | 12 | ├── UavAgent.py // 无人机控制和状态获取相关接口 13 | 14 | ├── mymath.py // 代码中使用到的自定义数学运算 15 | 16 | ├── code_python // python环境下算法实现和仿真相关文件（不依赖AirSim） 17 | 18 | ├── map_avoid // 避障算法仿真使用的地图图像 19 | 20 | ├── python_avoid_APF.py // 人工势场法避障代码脚本文件 21 | 22 | ├── python_avoid_RRT.py // RRT方法路径规划代码脚本文件 23 | 24 | ├── python_CarrotChasing.py // 航路点跟踪代码脚本文件 25 | 26 | └── python_tracking_and_avoid.py // 航路点跟踪+避障代码脚本文件 27 | 28 | └── ObstacleDetectioon // 障碍物检测接口 29 | 30 | ├── obstacles_detect.py 31 | 32 | ├── calculate_pose.py 33 | 34 | └── 说明.txt 35 | ``` 36 | -------------------------------------------------------------------------------- /UavAgent.py: -------------------------------------------------------------------------------- 1 | import math 2 | import airsim 3 | import numpy as np 4 | from scipy import linalg 5 | 6 | 7 | def get_state(client, vehicle_name=''): 8 | """ 9 | 获取无人机状态 10 | :param client: AirSim连接客户端 11 | :param vehicle_name: 无人机名称 12 | :return: 无人机状态 13 | """ 14 | # 获取无人机状态 15 | DIG = 6 16 | State = client.getMultirotorState(vehicle_name=vehicle_name) 17 | kinematics = State.kinematics_estimated 18 | state = { 19 | "timestamp": str(State.timestamp), 20 | "position": [round(ele, DIG) for i, ele in 21 | enumerate(kinematics.position.to_numpy_array().tolist())], 22 | "orientation": [round(i, DIG) for i in airsim.to_eularian_angles(kinematics.orientation)], 23 | "linear_velocity": [round(i, DIG) for i in kinematics.linear_velocity.to_numpy_array().tolist()], 24 | "linear_acceleration": [round(i, DIG) for i in kinematics.linear_acceleration.to_numpy_array().tolist()], 25 | "angular_velocity": [round(i, DIG) for i in kinematics.angular_velocity.to_numpy_array().tolist()], 26 | "angular_acceleration": [round(i, DIG) for i in kinematics.angular_acceleration.to_numpy_array().tolist()] 27 | } 28 | return state 29 | 30 | 31 | def move_by_acceleration(client, ax_cmd, ay_cmd, az_cmd, duration=1, waited=False, vehicle_name=''): 32 | """ 33 | 四旋翼加速度控制，不改变偏航角 34 | :param client: AirSim连接客户端 35 | :param ax_cmd: x轴方向加速度 36 | :param ay_cmd: y轴方向加速度 37 | :param az_cmd: z轴方向加速度 38 | :param duration: 命令持续时间 39 | :param waited: 命令是否阻塞 40 | :param vehicle_name: 无人机名称 41 | :return: 无 42 | """ 43 | # 读取自身yaw角度 44 | state = get_state(client) 45 | angles = state['orientation'] 46 | z = state['position'][2] 47 | yaw_my = angles[2] 48 | g = 9.8 # 重力加速度 49 | sin_yaw = math.sin(yaw_my) 50 | cos_yaw = math.cos(yaw_my) 51 | A_psi = np.array([[cos_yaw, sin_yaw], [-sin_yaw, cos_yaw]]) 52 | A_psi_inverse = np.linalg.inv(A_psi) 53 | angle_h_cmd = 1 / (g + az_cmd) * np.dot(A_psi_inverse, np.array([[ax_cmd], [ay_cmd]])) 54 | theta = math.atan(angle_h_cmd[0, 0]) 55 | phi = math.atan(angle_h_cmd[1, 0] * math.cos(theta)) 56 | # client.moveToZAsync(z_cmd, vz).join() 57 | # client.moveByRollPitchYawZAsync(phi, theta, 0, z_cmd, duration) 58 | throttle = -0.3 * az_cmd + 0.6 59 | if throttle < 0: 60 | throttle = 0 61 | elif throttle > 1: 62 | throttle = 1 63 | if waited: 64 | client.moveByRollPitchYawThrottleAsync(phi, theta, 0, throttle, duration, vehicle_name=vehicle_name).join() 65 | else: 66 | client.moveByRollPitchYawThrottleAsync(phi, theta, 0, throttle, duration, vehicle_name=vehicle_name) 67 | 68 | 69 | def move_by_acceleration_horizontal_yaw(client, ax_cmd, ay_cmd, z_cmd, yaw, duration=1, waited=False, vehicle_name=''): 70 | """ 71 | 四旋翼x-y平面加速度控制，可设置偏航角 72 | :param client: AirSim连接客户端 73 | :param ax_cmd: x轴方向加速度 74 | :param ay_cmd: y轴方向加速度 75 | :param z_cmd: 无人机飞行高度 76 | :param yaw: 无人机偏航角 77 | :param duration: 命令持续时间 78 | :param waited: 命令是否阻塞 79 | :param vehicle_name: 无人机名称 80 | :return: 无 81 | """ 82 | # 读取自身yaw角度 83 | # state = client.simGetGroundTruthKinematics() 84 | # angles = airsim.to_eularian_angles(state.orientation) 85 | # yaw_my = angles[2] 86 | yaw_my = -yaw 87 | g = 9.8 # 重力加速度 88 | sin_yaw = math.sin(yaw_my) 89 | cos_yaw = math.cos(yaw_my) 90 | A_psi = np.array([[cos_yaw, sin_yaw], [-sin_yaw, cos_yaw]]) 91 | A_psi_inverse = np.linalg.inv(A_psi) 92 | angle_h_cmd = 1 / g * np.dot(A_psi_inverse, np.array([[ax_cmd], [ay_cmd]])) 93 | theta = math.atan(angle_h_cmd[0, 0]) 94 | phi = math.atan(angle_h_cmd[1, 0] * math.cos(theta)) 95 | 96 | if waited: 97 | client.moveByRollPitchYawZAsync(phi, theta, yaw_my, z_cmd, duration, vehicle_name=vehicle_name).join() 98 | else: 99 | client.moveByRollPitchYawZAsync(phi, theta, yaw_my, z_cmd, duration, vehicle_name=vehicle_name) 100 | 101 | 102 | def move_tracking_lqr(client, p_aim, v_aim, height, a_aim, duration, waited=False): 103 | """ 104 | x-y平面内，LQR方法控制无人机跟踪目标状态 105 | :param client: AirSim连接客户端 106 | :param p_aim: 目标位置 107 | :param v_aim: 目标速度 108 | :param height: 飞行高度 109 | :param a_aim: 目标加速度 110 | :param duration: 命令持续时间 111 | :param waited: 命令是否阻塞 112 | :return: 无 113 | """ 114 | A = np.array([[1, 0, duration, 0], 115 | [0, 1, 0, duration], 116 | [0, 0, 1, 0], 117 | [0, 0, 0, 1]]) 118 | B = np.array([[0, 0], 119 | [0, 0], 120 | [duration, 0], 121 | [0, duration]]) 122 | Q = np.diag([1, 1, 1, 1]) 123 | R = np.diag([.4, .4]) 124 | S = np.matrix(linalg.solve_discrete_are(A, B, Q, R)) 125 | K = np.matrix(linalg.inv(B.T * S * B + R) * (B.T * S * A)) 126 | # 读取当前的位置和速度 127 | UAV_state = client.simGetGroundTruthKinematics() 128 | pos_now = np.array([[UAV_state.position.x_val], 129 | [UAV_state.position.y_val], 130 | [UAV_state.position.z_val]]) 131 | vel_now = np.array([[UAV_state.linear_velocity.x_val], 132 | [UAV_state.linear_velocity.y_val], 133 | [UAV_state.linear_velocity.z_val]]) 134 | state_now = np.vstack((pos_now[0:2], vel_now[0:2])) 135 | # 目标状态 136 | state_des = np.array([[p_aim[0]], [p_aim[1]], [v_aim[0]], [v_aim[1]]]) 137 | # LQR轨迹跟踪 138 | a = np.dot(K, state_des - state_now) + np.array([[a_aim[0]], [a_aim[1]]]) 139 | # 四旋翼加速度控制 140 | yaw = math.atan2(state_des[3], state_des[2]) 141 | move_by_acceleration_horizontal_yaw(client, a[0, 0], a[1, 0], height, yaw, duration, waited=waited) 142 | 143 | -------------------------------------------------------------------------------- /airsim_avoid_APF.py: -------------------------------------------------------------------------------- 1 | """ 2 | python_tracking_and_avoid.py 3 | CarrotChasing轨迹跟踪算法与APF避障算法融合 4 | """ 5 | 6 | import math 7 | import numpy as np 8 | import airsim 9 | import time 10 | from ObstacleDetection.obstacles_detect import obstacles_detect 11 | from UavAgent import move_by_acceleration_horizontal_yaw, get_state, move_tracking_lqr 12 | from mymath import distance, myatan, isClockwise 13 | 14 | 15 | def move_by_path_and_avoid_APF(client, Path, K_track=None, delta=1, K_avoid=None, 16 | Q_search=10, epsilon=2, Ul=None, dt=0.6, vehicle_name='UAV0'): 17 | """ 18 | :param client: AirSim连接客户端 19 | :param Path: 被跟踪航路点 20 | :param K_track: 轨迹跟踪控制器参数 21 | :param delta: 向前搜索下一个航路点的距离 22 | :param K_avoid: 轨迹跟踪控制器参数 23 | :param Q_search: 搜索障碍物距离 24 | :param epsilon: 误差上限 25 | :param Ul: 控制器输出上限 26 | :param dt: 迭代时间 27 | :param vehicle_name: 被控无人机名称 28 | :return: 无 29 | """ 30 | 31 | # 读取初始参数 32 | [K0, K1, K2] = K_track 33 | [Kg, Kr] = K_avoid 34 | [Ul_avoid, Ul_track] = Ul 35 | # 读取无人机初始状态 36 | state = get_state(client, vehicle_name=vehicle_name) 37 | P_start = np.array(state['position']) # 初始位置 38 | V_start = np.array(state['linear_velocity'])[0:2] # 初始速度 39 | pos_record = [P_start] # 记录位置 40 | I2 = np.matrix([[1, 0], # 用于计算轨迹跟踪控制器参数 41 | [0, 1]]) 42 | # 开始飞行 43 | count = 0 # 已经记录的位置的总数 44 | P_curr = P_start # 当前位置 45 | V_curr = V_start 46 | V_last = np.array([0, 0]) 47 | plot_p2 = [airsim.Vector3r(P_curr[0], P_curr[1], P_curr[2])] 48 | height = -3 49 | Wb = P_curr[0:2] 50 | nowtime, lasttime = 0, 0 51 | for path_num in range(len(Path)): 52 | Wa = Wb # 出发航路点 53 | Wb = np.array([Path[path_num].x_val, 54 | Path[path_num].y_val]) # 目标航路点 55 | Wa_sub_Wb_matrix = np.matrix((Wa - Wb)).T # 计算差矩阵 56 | A = I2 - Wa_sub_Wb_matrix.dot(Wa_sub_Wb_matrix.T) / (distance(Wa, Wb) ** 2) 57 | Pt_matrix = np.matrix(P_curr[0:2] - Wb).T # 用于计算的中间矩阵，目标点到当前点的向量 58 | d_to_Wb = np.linalg.norm(Pt_matrix - A.dot(Pt_matrix)) # 沿着轨迹方向，当前点距离目标点的距离 59 | # 开始跟踪当前航路Wa-Wb， 60 | # 如Wb为最后一个航点，则P距离Wb小于epsilon时终止 61 | # 否则，在delta距离内发现航点Wb时终止 62 | while (path_num != len(Path) - 1 or distance(Wb, P_curr[0:2]) > epsilon) and \ 63 | (path_num == len(Path) - 1 or d_to_Wb > delta): 64 | Pt_matrix = np.matrix(P_curr[0:2] - Wb).T 65 | Frep = np.array([0, 0]) # 斥力 66 | info_obstacles = obstacles_detect(client, Q_search, vehicle_name=vehicle_name) 67 | num_obstacles = 0 68 | for obstacle in info_obstacles: 69 | state = get_state(client, vehicle_name=vehicle_name) 70 | yaw = -state['orientation'][2] 71 | Rz = np.array([[math.cos(yaw), math.sin(yaw)], 72 | [-math.sin(yaw), math.cos(yaw)]]) 73 | pos_obstacle_min = obstacle.min 74 | pos_obstacle_max = obstacle.max 75 | P_search_list = np.array([np.array([pos_obstacle_min.x_val, pos_obstacle_min.y_val]), 76 | np.array([pos_obstacle_min.x_val, pos_obstacle_max.y_val]), 77 | np.array([pos_obstacle_max.x_val, pos_obstacle_min.y_val]), 78 | np.array([pos_obstacle_max.x_val, pos_obstacle_max.y_val])]).dot(Rz.T) 79 | for P_search in P_search_list: 80 | d_search = np.linalg.norm(P_search) 81 | if d_search <= Q_search: 82 | num_obstacles += 1 83 | Frep = Frep + \ 84 | Kr * (1 / d_search - 1 / Q_search) / (d_search ** 3) * \ 85 | (-P_search) * (distance(P_search + P_curr[0:2], Wb) ** 2) 86 | # 未检测到障碍物，执行航路点跟踪 87 | if num_obstacles == 0: 88 | U1 = np.array((K0 * Pt_matrix + K1 * A.dot(Pt_matrix)).T)[0] 89 | if np.linalg.norm(U1, ord=np.inf) > Ul_track: 90 | U1 = U1 * Ul_track / np.linalg.norm(U1, ord=np.inf) 91 | U = -(U1 + V_curr[0:2]) / K2 # 计算控制器输出并转换为array 92 | # 检测到障碍物，执行避障 93 | else: 94 | Frep = Frep / num_obstacles 95 | Fatt = -Kg * (P_curr[0:2] - Wb) # 计算引力 96 | if count >= 1: 97 | # 计算上两个时刻物体相对终点的位移，以判断是否陷入局部极小值 98 | p0 = pos_record[-1] 99 | p1 = pos_record[-2] 100 | nowtime = time.time() 101 | Vra = (distance(p0, Wb) - distance(p1, Wb)) / (nowtime - lasttime) 102 | lasttime = nowtime 103 | if abs(Vra) < 0.95 * Ul_avoid and len(info_obstacles) != 0 \ 104 | and np.linalg.norm(V_curr[0:2], ord=np.inf) < np.linalg.norm(V_last, ord=np.inf): # 陷入局部极小值 105 | # 之前不是局部极小状态时，根据当前位置计算斥力偏向角theta 106 | angle_g = myatan([0, 0], [Fatt[0], Fatt[1]]) 107 | angle_g = 0 if angle_g is None else angle_g 108 | angle_r = myatan([0, 0], [Frep[0], Frep[1]]) 109 | angle_r = 0 if angle_r is None else angle_r 110 | if isClockwise(angle_g, angle_r): 111 | theta = 60 * math.pi / 180 112 | else: 113 | theta = -60 * math.pi / 180 114 | # 之前为局部极小，则继续使用上一时刻的斥力偏向角theta 115 | Frep = [math.cos(theta) * Frep[0] - math.sin(theta) * Frep[1], 116 | math.sin(theta) * Frep[0] + math.cos(theta) * Frep[1]] 117 | l = Ul_avoid 118 | Kv = 3 * l / (2 * l + abs(Vra)) 119 | Kd = 15 * math.exp(-(distance(P_curr[0:2], Wb) - 1.5) ** 2 / 2) + 1 120 | Ke = 5 121 | Fatt = Kv * Kd * Ke * Fatt # 改进引力 122 | U = Fatt + Frep # 计算合力 123 | if np.linalg.norm(U, ord=np.inf) > Ul_avoid: # 控制器输出限幅 124 | U = Ul_avoid * U / np.linalg.norm(U, ord=np.inf) 125 | U = (U - V_curr[0:2]) / K2 126 | # 执行 127 | V_next = V_curr[0:2] + U * dt # 计算速度 128 | P_next = P_curr[0:2] + V_next * dt 129 | move_tracking_lqr(client, P_next, V_next, height, U[0:2], dt) 130 | # 画图和记录 131 | V_last = V_curr 132 | plot_p1 = plot_p2 133 | state = get_state(client, vehicle_name=vehicle_name) 134 | P_curr = np.array(state['position']) 135 | V_curr = np.array(state['linear_velocity']) 136 | plot_p2 = [airsim.Vector3r(P_curr[0], P_curr[1], P_curr[2])] 137 | client.simPlotArrows(plot_p1, plot_p2, arrow_size=8.0, color_rgba=[0.0, 0.0, 1.0, 1.0]) 138 | client.simPlotLineStrip(plot_p1 + plot_p2, color_rgba=[1.0, 0.0, 0.0, 1.0], is_persistent=True) 139 | 140 | d_to_Wb = np.linalg.norm(Pt_matrix - A.dot(Pt_matrix)) 141 | pos_record.append(P_curr) 142 | count += 1 143 | 144 | 145 | if __name__ == "__main__": 146 | client = airsim.MultirotorClient(port=41451) 147 | client.confirmConnection() 148 | client.reset() 149 | client.enableApiControl(True, vehicle_name='UAV0') 150 | client.armDisarm(True, vehicle_name='UAV0') 151 | client.simFlushPersistentMarkers() 152 | client.takeoffAsync(vehicle_name='UAV0') 153 | client.moveToZAsync(-3, 1, vehicle_name='UAV0').join() 154 | 155 | # 手动设置航路点 156 | points = [airsim.Vector3r(60, 0, -3), 157 | airsim.Vector3r(70, -80, -3), 158 | airsim.Vector3r(55, -120, -3), 159 | airsim.Vector3r(0, 0, -3)] 160 | 161 | # # 读取RRT方法获取的航路点 162 | # path_for_airsim = np.load('code_python/path_for_airsim.npy') 163 | # points = [] 164 | # for p in path_for_airsim: 165 | # points.append(airsim.Vector3r(p[0], p[1], p[2])) 166 | # 167 | move_by_path_and_avoid_APF(client, points, K_track=[1.5, 6, 1], delta=5, K_avoid=[3, 60], Q_search=8, epsilon=1, 168 | Ul=[2, 3], dt=0.3, vehicle_name='UAV0') 169 | 170 | client.landAsync(vehicle_name='UAV0').join() 171 | client.armDisarm(False, vehicle_name='UAV0') 172 | client.enableApiControl(False, vehicle_name='UAV0') 173 | -------------------------------------------------------------------------------- /airsim_avoid_APF_3d.py: -------------------------------------------------------------------------------- 1 | """ 2 | airsim_avoid_APF_3d.py 3 | CarrotChasing轨迹跟踪 + 几何控制 + APF避障 4 | """ 5 | 6 | import math 7 | import numpy as np 8 | import airsim 9 | import time 10 | from ObstacleDetection.obstacles_detect import obstacles_detect 11 | from UavAgent import move_by_acceleration_horizontal_yaw, get_state, move_tracking_lqr 12 | from mymath import distance, myatan, isClockwise, distance_3d 13 | import os 14 | from platform import uname 15 | from matplotlib import pyplot as plt 16 | from geometricControl import move_by_geometricControl 17 | 18 | def rotation_matrix(yaw, pitch, roll): 19 | """ 20 | Generate a 3D rotation matrix given yaw, pitch, and roll angles (in radians). 21 | 22 | Parameters: 23 | yaw (float): Yaw angle in radians. 24 | pitch (float): Pitch angle in radians. 25 | roll (float): Roll angle in radians. 26 | 27 | Returns: 28 | numpy.ndarray: 3x3 rotation matrix. 29 | """ 30 | # Rotation around z-axis (yaw) 31 | Rz = np.array([[math.cos(yaw), -math.sin(yaw), 0], 32 | [math.sin(yaw), math.cos(yaw), 0], 33 | [0, 0, 1]]) 34 | 35 | # Rotation around y-axis (pitch) 36 | Ry = np.array([[math.cos(pitch), 0, math.sin(pitch)], 37 | [0, 1, 0], 38 | [-math.sin(pitch), 0, math.cos(pitch)]]) 39 | 40 | # Rotation around x-axis (roll) 41 | Rx = np.array([[1, 0, 0], 42 | [0, math.cos(roll), -math.sin(roll)], 43 | [0, math.sin(roll), math.cos(roll)]]) 44 | 45 | # Combined rotation 46 | R = Rz.dot(Ry.dot(Rx)) 47 | 48 | return R 49 | 50 | 51 | def move_by_path_and_avoid_APF(client, Path, K_track=None, delta=1, K_avoid=None, 52 | Q_search=10, epsilon=2, Ul=None, dt=0.6, vehicle_name='Drone1'): 53 | """ 54 | :param client: AirSim连接客户端 55 | :param Path: 被跟踪航路点 56 | :param K_track: 轨迹跟踪控制器参数 57 | :param delta: 向前搜索下一个航路点的距离 58 | :param K_avoid: 避障控制器参数 59 | :param Q_search: 搜索障碍物距离 60 | :param epsilon: 误差上限 61 | :param Ul: 控制器输出上限 62 | :param dt: 迭代时间 63 | :param vehicle_name: 被控无人机名称 64 | :return: 无 65 | """ 66 | 67 | # 读取初始参数 68 | [K0, K1, K2] = K_track 69 | [Kg, Kr] = K_avoid 70 | [Ul_avoid, Ul_track] = Ul 71 | # 读取无人机初始状态 72 | state = get_state(client, vehicle_name=vehicle_name) 73 | P_start = np.array(state['position']) # 初始位置 74 | V_start = np.array(state['linear_velocity']) # 初始速度 75 | pos_record = [P_start] # 记录位置 76 | I2 = np.matrix([[1, 0], # 用于计算轨迹跟踪控制器参数 77 | [0, 1]]) 78 | I3 = np.matrix([[1,0,0], 79 | [0,1,0], 80 | [0,0,1]]) 81 | # 开始飞行 82 | count = 0 # 已经记录的位置的总数 83 | P_curr = P_start # 当前位置 84 | print("P_curr: {}".format(P_curr)) 85 | V_curr = V_start 86 | V_last = np.array([0, 0, 0]) 87 | plot_p2 = [airsim.Vector3r(P_curr[0], P_curr[1], P_curr[2])] 88 | height = -1.5 # 飞行高度 89 | Wb = P_curr 90 | nowtime, lasttime = 0, 0 91 | for path_num in range(len(Path)): 92 | Wa = Wb # 出发航路点 93 | Wb = np.array([Path[path_num].x_val, 94 | Path[path_num].y_val, 95 | Path[path_num].z_val]) # 目标航路点 96 | Wa_sub_Wb_matrix = np.matrix((Wa - Wb)).T # 计算差矩阵 97 | # print("wa-wb={}".format(Wa_sub_Wb_matrix)) 98 | A = I3 - Wa_sub_Wb_matrix.dot(Wa_sub_Wb_matrix.T) / (distance_3d(Wa, Wb) ** 2) 99 | Pt_matrix = np.matrix(P_curr - Wb).T # 用于计算的中间矩阵，目标点到当前点的向量 100 | d_to_Wb = np.linalg.norm(Pt_matrix - A.dot(Pt_matrix)) # 沿着轨迹方向，当前点距离目标点的距离 101 | # 开始跟踪当前航路Wa-Wb， 102 | # 如Wb为最后一个航点，则P距离Wb小于epsilon时终止 103 | # 否则，在delta距离内发现航点Wb时终止 104 | while (path_num != len(Path) - 1 or distance(Wb, P_curr) > epsilon) and \ 105 | (path_num == len(Path) - 1 or d_to_Wb > delta): 106 | Pt_matrix = np.matrix(P_curr - Wb).T 107 | # print("Pt_matrix={}".format(Pt_matrix)) 108 | Frep = np.array([0, 0, 0]) # 斥力 109 | info_obstacles = obstacles_detect(client, Q_search, vehicle_name=vehicle_name) 110 | num_obstacles = 0 111 | for obstacle in info_obstacles: 112 | state = get_state(client, vehicle_name=vehicle_name) 113 | roll = state['orientation'][0] 114 | pitch = state['orientation'][1] 115 | yaw = state['orientation'][2] 116 | R = rotation_matrix(yaw, pitch, roll) 117 | # Rz = np.array([[math.cos(yaw), math.sin(yaw)], 118 | # [-math.sin(yaw), math.cos(yaw)]]) 119 | pos_obstacle_min = obstacle.min 120 | pos_obstacle_max = obstacle.max 121 | 122 | P_search_list = np.array([ 123 | [pos_obstacle_min.x_val, pos_obstacle_min.y_val, pos_obstacle_min.z_val], 124 | [pos_obstacle_min.x_val, pos_obstacle_min.y_val, pos_obstacle_max.z_val], 125 | [pos_obstacle_min.x_val, pos_obstacle_max.y_val, pos_obstacle_min.z_val], 126 | [pos_obstacle_min.x_val, pos_obstacle_max.y_val, pos_obstacle_max.z_val], 127 | [pos_obstacle_max.x_val, pos_obstacle_min.y_val, pos_obstacle_min.z_val], 128 | [pos_obstacle_max.x_val, pos_obstacle_min.y_val, pos_obstacle_max.z_val], 129 | [pos_obstacle_max.x_val, pos_obstacle_max.y_val, pos_obstacle_min.z_val], 130 | [pos_obstacle_max.x_val, pos_obstacle_max.y_val, pos_obstacle_max.z_val] 131 | ]).dot(R.T) 132 | # P_search_list = np.array([np.array([pos_obstacle_min.x_val, pos_obstacle_min.y_val]), 133 | # np.array([pos_obstacle_min.x_val, pos_obstacle_max.y_val]), 134 | # np.array([pos_obstacle_max.x_val, pos_obstacle_min.y_val]), 135 | # np.array([pos_obstacle_max.x_val, pos_obstacle_max.y_val])]).dot(R.T) 136 | for P_search in P_search_list: 137 | d_search = np.linalg.norm(P_search) 138 | if d_search <= Q_search: 139 | num_obstacles += 1 140 | Frep = Frep + \ 141 | Kr * (1 / d_search - 1 / Q_search) / (d_search ** 3) * \ 142 | (-P_search) * (distance_3d(P_search + P_curr, Wb) ** 2) 143 | 144 | # 未检测到障碍物，执行航路点跟踪 145 | if num_obstacles == 0: 146 | avoid = False 147 | U1 = np.array((K0 * Pt_matrix + K1 * A.dot(Pt_matrix)).T)[0] 148 | 149 | # 限幅 150 | if np.linalg.norm(U1, ord=np.inf) > Ul_track: 151 | U1 = U1 * Ul_track / np.linalg.norm(U1, ord=np.inf) 152 | U = -(U1 + V_curr) / K2 # 计算控制器输出并转换为array 153 | 154 | # 检测到障碍物，执行避障 155 | else: 156 | avoid = True 157 | Frep = Frep / num_obstacles 158 | Fatt = -Kg * (P_curr - Wb) # 计算引力 159 | if count >= 1: 160 | # 计算上两个时刻物体相对终点的位移，以判断是否陷入局部极小值 161 | p0 = pos_record[-1] 162 | p1 = pos_record[-2] 163 | nowtime = time.time() 164 | Vra = (distance(p0, Wb) - distance(p1, Wb)) / (nowtime - lasttime) 165 | lasttime = nowtime 166 | 167 | # 若陷入局部极小值 168 | # if abs(Vra) < 0.95 * Ul_avoid and len(info_obstacles) != 0 \ 169 | # and np.linalg.norm(V_curr[0:2], ord=np.inf) < np.linalg.norm(V_last, ord=np.inf): 170 | # # 之前不是局部极小状态时，根据当前位置计算斥力偏向角theta 171 | # angle_g = myatan([0, 0], [Fatt[0], Fatt[1]]) 172 | # angle_g = 0 if angle_g is None else angle_g 173 | # angle_r = myatan([0, 0], [Frep[0], Frep[1]]) 174 | # angle_r = 0 if angle_r is None else angle_r 175 | # if isClockwise(angle_g, angle_r): 176 | # theta = 60 * math.pi / 180 177 | # else: 178 | # theta = -60 * math.pi / 180 179 | # # 之前为局部极小，则继续使用上一时刻的斥力偏向角theta 180 | # Frep = [math.cos(theta) * Frep[0] - math.sin(theta) * Frep[1], 181 | # math.sin(theta) * Frep[0] + math.cos(theta) * Frep[1]] 182 | l = Ul_avoid 183 | Kv = 3 * l / (2 * l + abs(Vra)) 184 | Kd = 15 * math.exp(-(distance(P_curr, Wb) - 1.5) ** 2 / 2) + 1 185 | Ke = 5 186 | Fatt = Kv * Kd * Ke * Fatt # 改进引力 187 | U = Fatt + Frep # 计算合力 188 | 189 | # 限幅 190 | if np.linalg.norm(U, ord=np.inf) > Ul_avoid: 191 | U = Ul_avoid * U / np.linalg.norm(U, ord=np.inf) 192 | U = (U - V_curr) / K2 193 | 194 | # 执行 195 | V_next = V_curr + U * dt # 计算速度 196 | P_next = P_curr + V_next * dt 197 | # if P_next[2] < -1: 198 | # P_next[2] = -1 199 | # V_next[2] = 0 200 | # U[2] = - V_curr[2] / dt 201 | Yaw = np.arctan2(V_next[1], V_next[0]) 202 | move_by_geometricControl(client, P_next.reshape(3, 1), V_next.reshape(3, 1), U.reshape(3, 1), Yaw, P_curr, V_curr, dt) 203 | 204 | # move_tracking_lqr(client, P_next, V_next, height, U[0:2], dt) 205 | # V_series.append(V_curr[0:2]) 206 | 207 | # 画图和记录 208 | V_last = V_curr 209 | plot_p1 = plot_p2 210 | state = get_state(client, vehicle_name=vehicle_name) 211 | P_curr = np.array(state['position']) 212 | V_curr = np.array(state['linear_velocity']) 213 | plot_p2 = [airsim.Vector3r(P_curr[0], P_curr[1], P_curr[2])] 214 | client.simPlotArrows(plot_p1, plot_p2, arrow_size=8.0, color_rgba=[0.0, 0.0, 1.0, 1.0]) 215 | client.simPlotLineStrip(plot_p1 + plot_p2, color_rgba=[1.0, avoid * 1.0, 0.0, 1.0], is_persistent=True) 216 | 217 | d_to_Wb = np.linalg.norm(Pt_matrix - A.dot(Pt_matrix)) 218 | pos_record.append(P_curr) 219 | count += 1 220 | 221 | if __name__ == "__main__": 222 | # HOST = '172.24.192.1' # Standard loopback interface address (localhost) 223 | # if 'linux' in uname().system.lower() and 'microsoft' in uname().release.lower(): # In WSL2 224 | # if 'WSL_HOST_IP' in os.environ: 225 | # HOST = os.environ['WSL_HOST_IP'] 226 | # print("Using WSL2 Host IP address: ", HOST) 227 | # client = airsim.MultirotorClient(ip=HOST) 228 | 229 | client = airsim.MultirotorClient(port=41451) 230 | client.confirmConnection() 231 | client.reset() 232 | client.enableApiControl(True, vehicle_name='Drone1') 233 | client.armDisarm(True, vehicle_name='Drone1') 234 | client.simFlushPersistentMarkers() 235 | client.takeoffAsync(vehicle_name='Drone1') 236 | client.moveToZAsync(-1.5, 1, vehicle_name='Drone1').join() 237 | 238 | # 手动设置航路点 239 | points = [airsim.Vector3r(-10, -1, -1.5), 240 | airsim.Vector3r(-30, 10, -3)] 241 | 242 | # # 读取RRT方法获取的航路点 243 | # path_for_airsim = np.load('code_python/path_for_airsim.npy') 244 | # points = [] 245 | # for p in path_for_airsim: 246 | # points.append(airsim.Vector3r(p[0], p[1], p[2])) 247 | # 248 | time_start = time.time() 249 | # client.moveToPositionAsync(-10, 0, -1.5, 7, vehicle_name="Drone1").join() 250 | move_by_path_and_avoid_APF(client, points, K_track=[1.5, 6, 1], delta=5, K_avoid=[10, 60], 251 | Q_search=10, epsilon=1, Ul=[5, 7], dt=0.1, vehicle_name='Drone1') 252 | # V_series = move_by_path_and_avoid_APF(client, points, K_track=[1.5, 6, 1], delta=5, K_avoid=[10, 60], 253 | # Q_search=5, epsilon=1, Ul=[5, 7], dt=0.1, vehicle_name='Drone1') 254 | 255 | # 默认参数： 256 | # K_track=[1.5,6,1] 257 | # Ul=[2,3]; Ul = [U_avoid, U_track] 258 | # dt=0.3 259 | time_arrive = time.time() 260 | print("Flying time: {}".format(time_arrive - time_start)) 261 | 262 | client.landAsync(vehicle_name='Drone1').join() 263 | client.armDisarm(False, vehicle_name='Drone1') 264 | client.enableApiControl(False, vehicle_name='Drone1') -------------------------------------------------------------------------------- /airsim_tracking_carrot.py: -------------------------------------------------------------------------------- 1 | """ 2 | tracking_carrot.py 3 | airsim 四旋翼轨迹跟踪 Carrot控制算法 4 | """ 5 | 6 | import airsim 7 | import math 8 | import numpy as np 9 | from UavAgent import get_state, move_by_acceleration 10 | from mymath import distance, myatan, distance_3d 11 | 12 | 13 | def move_by_path(client, Va, Path, Pz, delta=1, K=0.5, dt=0.02, epsilon=0.2): 14 | """ 15 | :param client: AirSim连接客户端 16 | :param Va: 速率上限 17 | :param Path: 被跟踪航路点 18 | :param Pz: 飞行高度 19 | :param delta: 向前搜索下一个航路点的距离 20 | :param K: 控制器参数 21 | :param dt: 迭代时间 22 | :param epsilon: 误差上限 23 | :return: 无 24 | """ 25 | 26 | state = client.simGetGroundTruthKinematics() 27 | psi = airsim.to_eularian_angles(state.orientation)[2] 28 | Px = state.position.x_val 29 | Py = state.position.y_val 30 | Wb = [Px, Py] 31 | 32 | for i in range(len(Path)): 33 | Wa = Wb 34 | Wb = [Path[i].x_val, Path[i].y_val] 35 | theta = myatan(Wa, Wb) 36 | xt = Wa[0] 37 | yt = Wa[1] 38 | while (((xt - Wb[0]) * (Wb[0] - Wa[0]) < 0 and (yt - Wb[1]) * (Wb[1] - Wa[1]) < 0) and 39 | ((i != (len(Path) - 1)) or 40 | distance([xt, yt], Wb)) > epsilon): 41 | theta_u = myatan(Wa, [Px, Py]) 42 | beta = 0 if theta_u is None else theta - theta_u 43 | Ru = distance(Wa, [Px, Py]) 44 | R = Ru * math.cos(beta) 45 | e = Ru * math.sin(beta) 46 | xt = Wa[0] + (R + delta) * math.cos(theta) 47 | yt = Wa[1] + (R + delta) * math.sin(theta) 48 | psi_d = myatan([Px, Py], [xt, yt]) 49 | u = K * (psi_d - psi) * Va 50 | u = (u / abs(u)) if abs(u) > 1 else u 51 | psi = psi_d 52 | Vy = Va * math.sin(psi) + u * dt 53 | if abs(Vy) >= Va: 54 | Vy = Va 55 | Vx = np.sign(math.cos(psi)) * math.sqrt(Va ** 2 - Vy ** 2) 56 | client.moveByVelocityZAsync(Vx, Vy, Pz, dt).join() 57 | # 画图 58 | plot_p1 = [airsim.Vector3r(Px, Py, Pz)] 59 | state = client.simGetGroundTruthKinematics() 60 | Px = state.position.x_val 61 | Py = state.position.y_val 62 | plot_p2 = [airsim.Vector3r(Px, Py, Pz)] 63 | client.simPlotArrows(plot_p1, plot_p2, arrow_size=8.0, color_rgba=[0.0, 0.0, 1.0, 1.0]) 64 | client.simPlotLineStrip(plot_p1 + plot_p2, color_rgba=[1.0, 0.0, 0.0, 1.0], is_persistent=True) 65 | 66 | 67 | def move_by_path_3d(client, Path, K0=1.5, K1=4, K2=0.6, dt=0.5, a0=1, delta=0.7): 68 | """ 69 | :param client: AirSim连接客户端 70 | :param Path: 被跟踪航路点 71 | :param K0: 控制器参数 72 | :param K1: 控制器参数 73 | :param K2: 控制器参数 74 | :param dt: 迭代时间 75 | :param a0: 控制器输出上限 76 | :param delta: 向前搜索下一个航路点的距离 77 | :return: 无 78 | """ 79 | 80 | state = get_state(client) 81 | P = state['position'] 82 | V = state['linear_velocity'] 83 | Wb = P 84 | Wb_m = np.matrix(Wb).T 85 | P_m = np.matrix(P).T 86 | V_m = np.matrix(V).T 87 | I3 = np.matrix([[1, 0, 0], 88 | [0, 1, 0], 89 | [0, 0, 1]]) 90 | for i in range(len(Path)): 91 | Wa = Wb 92 | Wb = [Path[i].x_val, Path[i].y_val, Path[i].z_val] 93 | Wa_m = Wb_m 94 | Wb_m = np.matrix(Wb).T 95 | A = I3 - (Wa_m - Wb_m).dot((Wa_m - Wb_m).T) / (distance_3d(Wa_m, Wb_m) ** 2) 96 | Pt = P_m - Wb_m 97 | d = np.linalg.norm(Pt - A.dot(Pt)) 98 | while d >= delta or \ 99 | (i == len(Path) - 1 100 | and ((P[0] - Wb[0]) * (Wb[0] - Wa[0]) < 0 101 | or (P[1] - Wb[1]) * (Wb[1] - Wa[1]) < 0 102 | or (P[2] - Wb[2]) * (Wb[2] - Wa[2]) < 0)): 103 | Pt = P_m - Wb_m 104 | U1 = K0 * Pt + K1 * A.dot(Pt) 105 | if np.linalg.norm(U1, ord=np.inf) > a0: 106 | U1 = U1 * a0 / np.linalg.norm(U1, ord=np.inf) 107 | U = -(U1 + V_m) / K2 108 | U_cmd = np.array(U)[:, 0] 109 | move_by_acceleration(client, U_cmd[0], U_cmd[1], U_cmd[2], dt * 10) 110 | d = np.linalg.norm(Pt - A.dot(Pt)) 111 | # 画图 112 | plot_p1 = [airsim.Vector3r(P[0], P[1], P[2])] 113 | state = get_state(client) 114 | P = state['position'] 115 | V = state['linear_velocity'] 116 | P_m = np.matrix(P).T 117 | V_m = np.matrix(V).T 118 | plot_p2 = [airsim.Vector3r(P[0], P[1], P[2])] 119 | client.simPlotArrows(plot_p1, plot_p2, arrow_size=8.0, color_rgba=[0.0, 0.0, 1.0, 1.0]) 120 | client.simPlotLineStrip(plot_p1 + plot_p2, color_rgba=[1.0, 0.0, 0.0, 1.0], is_persistent=True) 121 | 122 | 123 | if __name__ == "__main__": 124 | client = airsim.MultirotorClient() # 创建连接 125 | client.confirmConnection() # 检查连接 126 | client.reset() 127 | client.enableApiControl(True) # 获取控制权 128 | client.armDisarm(True) # 电机启转 129 | client.takeoffAsync().join() # 起飞 130 | client.moveToZAsync(-3, 1).join() # 上升到3米高度 131 | client.simSetTraceLine([1, 0, 0, 1], thickness=5) 132 | client.simFlushPersistentMarkers() # 清空画图 133 | 134 | # 二维航路点跟踪 135 | points = [airsim.Vector3r(5, 0, -3), 136 | airsim.Vector3r(5, 8, -3), 137 | airsim.Vector3r(8, 12, -3), 138 | airsim.Vector3r(4, 9, -3)] 139 | client.simPlotPoints(points, color_rgba=[0, 1, 0, 1], size=30, is_persistent=True) 140 | move_by_path(client, 3, points, -3, delta=0.5, epsilon=0.05) 141 | 142 | # 三维航路点跟踪（也可执行二维） 143 | # points = [airsim.Vector3r(5, 0, -3), 144 | # airsim.Vector3r(3, 10, -1), 145 | # airsim.Vector3r(8, 12, -7), 146 | # airsim.Vector3r(-5, 9, -2)] 147 | # client.simPlotPoints(points, color_rgba=[0, 1, 0, 1], size=30, is_persistent=True) 148 | # client.simPlotLineStrip(points, color_rgba=[0, 1, 0, 1], thickness=5, is_persistent=True) 149 | # move_by_path_3d(client, points, delta=0.5, a0=2, dt=0.12, K0=1, K1=2, K2=0.8) 150 | 151 | # 仿真结束 152 | client.landAsync().join() # 降落 153 | client.armDisarm(False) # 电机上锁 154 | client.enableApiControl(False) # 释放控制权 155 | -------------------------------------------------------------------------------- /code_python/map_avoid/1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Kun-k/airsim_python/66fa72a855c6d23fe01c4b15e31d3b4a55117dc5/code_python/map_avoid/1.png -------------------------------------------------------------------------------- /code_python/map_avoid/2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Kun-k/airsim_python/66fa72a855c6d23fe01c4b15e31d3b4a55117dc5/code_python/map_avoid/2.png -------------------------------------------------------------------------------- /code_python/map_avoid/3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Kun-k/airsim_python/66fa72a855c6d23fe01c4b15e31d3b4a55117dc5/code_python/map_avoid/3.png -------------------------------------------------------------------------------- /code_python/map_avoid/4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Kun-k/airsim_python/66fa72a855c6d23fe01c4b15e31d3b4a55117dc5/code_python/map_avoid/4.png -------------------------------------------------------------------------------- /code_python/map_avoid/5.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Kun-k/airsim_python/66fa72a855c6d23fe01c4b15e31d3b4a55117dc5/code_python/map_avoid/5.png -------------------------------------------------------------------------------- /code_python/map_avoid/6.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Kun-k/airsim_python/66fa72a855c6d23fe01c4b15e31d3b4a55117dc5/code_python/map_avoid/6.png -------------------------------------------------------------------------------- /code_python/map_avoid/7.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Kun-k/airsim_python/66fa72a855c6d23fe01c4b15e31d3b4a55117dc5/code_python/map_avoid/7.png -------------------------------------------------------------------------------- /code_python/map_avoid/map2.bmp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Kun-k/airsim_python/66fa72a855c6d23fe01c4b15e31d3b4a55117dc5/code_python/map_avoid/map2.bmp -------------------------------------------------------------------------------- /code_python/map_avoid/map4.bmp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Kun-k/airsim_python/66fa72a855c6d23fe01c4b15e31d3b4a55117dc5/code_python/map_avoid/map4.bmp -------------------------------------------------------------------------------- /code_python/path_for_airsim.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Kun-k/airsim_python/66fa72a855c6d23fe01c4b15e31d3b4a55117dc5/code_python/path_for_airsim.npy -------------------------------------------------------------------------------- /code_python/python_CarrotChasing.py: -------------------------------------------------------------------------------- 1 | ''' 2 | CarrotChasing.py 3 | CarrotChasing轨迹跟踪算法的python实现 4 | ''' 5 | 6 | 7 | import math 8 | import matplotlib.pyplot as plt 9 | import numpy as np 10 | 11 | 12 | def move_to_point(P, Va, psi, Wa, Wb, delta=1, K=0.5, epsilon=0.02, dt=0.01): 13 | """ 14 | 在二维空间内，控制无人机跟踪路径Wa-Wb 15 | :param P: 起始位置 16 | :param Va: 速率上限 17 | :param psi: 起始速度角度 18 | :param Wa: 被跟踪路径出发点 19 | :param Wb: 被跟踪路径终点 20 | :param delta: 向前搜索下一个航路点的距离 21 | :param K: 控制器参数 22 | :param epsilon: 误差上限 23 | :param dt: 迭代时间 24 | :return: 无 25 | """ 26 | print(Wa, Wb, P) 27 | [Px, Py] = P 28 | theta = math.atan((Wb[1] - Wa[1]) / (Wb[0] - Wa[0])) 29 | if Wb[0] < Wa[0]: 30 | theta += math.pi 31 | pos_record = [[Px], [Py]] 32 | e = epsilon 33 | while abs(e) > epsilon: 34 | beta = theta - math.atan((Py - Wa[1]) / (Px - Wa[0])) 35 | if Px < Wa[0]: 36 | beta -= math.pi 37 | Ru = math.sqrt((Px - Wa[0]) ** 2 + (Py - Wa[1]) ** 2) 38 | R = Ru * math.cos(beta) 39 | e = Ru * math.sin(beta) 40 | print(Px, Py, e) 41 | xt = Wa[0] + (R + delta) * math.cos(theta) 42 | yt = Wa[1] + (R + delta) * math.sin(theta) 43 | psi_d = math.atan((yt - Py) / (xt - Px)) 44 | if xt < Px: 45 | psi_d += math.pi 46 | u = K * (psi_d - psi) * Va 47 | if u > 1: # 限制u的范围 48 | u = 1 49 | psi = psi_d 50 | Vy = Va * math.sin(psi) + u * dt 51 | if abs(Vy) >= Va: 52 | Vy = Va 53 | Vx = 0 54 | else: 55 | Vx = np.sign(math.cos(psi)) * math.sqrt(Va ** 2 - Vy ** 2) 56 | Px = Px + Vx * dt 57 | Py = Py + Vy * dt 58 | pos_record[0].append(Px) 59 | pos_record[1].append(Py) 60 | Vx = Va * math.cos(theta) 61 | Vy = Va * math.sin(theta) 62 | epsilon = math.sqrt(e ** 2 + (Va * dt / 2) ** 2) 63 | while True: 64 | e = math.sqrt((Px - Wb[0]) ** 2 + (Py - Wb[1]) ** 2) 65 | if e < epsilon: 66 | break 67 | Px = Px + Vx * dt 68 | Py = Py + Vy * dt 69 | pos_record[0].append(Px) 70 | pos_record[1].append(Py) 71 | print(Px, Py, e) 72 | plt.plot([Wa[0], Wb[0]], [Wa[1], Wb[1]]) 73 | plt.plot(pos_record[0], pos_record[1]) 74 | plt.axis('equal') 75 | plt.grid() 76 | plt.show() 77 | 78 | 79 | def move_by_path(P, Va, psi, Path, delta=1, K=0.5, dt=0.01): 80 | """ 81 | 在二维空间内，控制无人机跟踪路径Path 82 | :param P: 起始位置 83 | :param Va: 速率上限 84 | :param psi: 起始速度角度 85 | :param Path: 被跟踪航路点集合 86 | :param delta: 向前搜索下一个航路点的距离 87 | :param K: 控制器参数 88 | :param dt: 迭代时间 89 | :return: 无 90 | """ 91 | def distance(A, B): 92 | return math.sqrt((A[0] - B[0]) ** 2 + (A[1] - B[1]) ** 2) 93 | 94 | def myatan(A, B): 95 | x1, y1, x2, y2 = A[0], A[1], B[0], B[1] 96 | if x1 != x2: 97 | if x1 > x2: 98 | return math.atan((y1 - y2) / (x1 - x2)) + math.pi 99 | else: 100 | return math.atan((y1 - y2) / (x1 - x2)) 101 | if x1 == x2 and y1 == y2: 102 | return None 103 | if x1 == x2 and y1 != y2: 104 | if y1 > y2: 105 | return -math.pi / 2 106 | else: 107 | return math.pi / 2 108 | 109 | plt.rcParams['font.sans-serif'] = ['SimHei'] 110 | plt.rcParams["axes.unicode_minus"] = False 111 | [Px, Py] = P 112 | pos_record = [[Px, Py]] 113 | aim_record = [] 114 | for i in range(1, len(Path)): 115 | Wa = Path[i - 1] 116 | Wb = Path[i] 117 | theta = myatan(Wa, Wb) 118 | while True: 119 | theta_u = myatan(Wa, [Px, Py]) 120 | if theta_u is None: 121 | theta_u = theta 122 | beta = theta - theta_u 123 | Ru = distance(Wa, [Px, Py]) 124 | R = Ru * math.cos(beta) 125 | e = Ru * math.sin(beta) 126 | print(Px, Py, e) 127 | xt = Wa[0] + (R + delta) * math.cos(theta) 128 | yt = Wa[1] + (R + delta) * math.sin(theta) 129 | if (xt - Wb[0]) * (Wb[0] - Wa[0]) > 0 \ 130 | or (yt - Wb[1]) * (Wb[1] - Wa[1]) > 0: 131 | break 132 | psi_d = myatan([Px, Py], [xt, yt]) 133 | u = K * (psi_d - psi) * Va 134 | if u > 1: # 限制u的范围 135 | u = 1 136 | psi = psi_d 137 | Vy = Va * math.sin(psi) + u * dt 138 | if abs(Vy) >= Va: 139 | Vy = Va 140 | Vx = 0 141 | else: 142 | Vx = np.sign(math.cos(psi)) * math.sqrt(Va ** 2 - Vy ** 2) 143 | Px = Px + Vx * dt 144 | Py = Py + Vy * dt 145 | pos_record.append([Px, Py]) 146 | aim_record.append([xt, yt]) 147 | # 点采样和绘图 148 | pos_plot = [] 149 | aim_plot = [] 150 | num = 25 151 | gap = int(len(pos_record) / num) 152 | for i in range(num): 153 | pos_plot.append(pos_record[i * gap]) 154 | aim_plot.append(aim_record[i * gap]) 155 | pos_plot = np.array(pos_plot).T 156 | aim_plot = np.array(aim_plot).T 157 | path = np.array(Path).T 158 | plt.plot(path[0], path[1]) 159 | plt.plot(pos_plot[0], pos_plot[1], '--') 160 | plt.plot(aim_plot[0], aim_plot[1], '*') 161 | plt.legend(['目标轨迹', '实际轨迹', '目标航点']) 162 | plt.axis('equal') 163 | plt.grid() 164 | plt.show() 165 | 166 | 167 | def move_to_point_3d(P, V, Wa, Wb, K0=2, K1=2, K2=1, eposilon=0.02, dt=0.1, a0=2): 168 | """ 169 | 在三维空间内，控制无人机跟踪路径Wa-Wb 170 | :param P: 起始位置 171 | :param V: 起始速度 172 | :param Wa: 被跟踪路径出发点 173 | :param Wb: 被跟踪路径终点 174 | :param K0: 控制器参数 175 | :param K1: 控制器参数 176 | :param K2: 控制器参数 177 | :param epsilon: 误差上限 178 | :param dt: 迭代时间 179 | :param a0: 控制器输出上限 180 | :return: 无 181 | """ 182 | 183 | def distance(A, B): 184 | return math.sqrt((A[0] - B[0]) ** 2 + 185 | (A[1] - B[1]) ** 2 + 186 | (A[2] - B[2]) ** 2) 187 | 188 | P = np.matrix(P).T 189 | Wa = np.matrix(Wa).T 190 | Wb = np.matrix(Wb).T 191 | V = np.matrix(V).T 192 | I3 = np.matrix([[1,0,0], 193 | [0,1,0], 194 | [0,0,1]]) 195 | A = I3 - (Wa - Wb).dot((Wa - Wb).T) / (distance(Wa, Wb) ** 2) 196 | e = np.linalg.norm(A.dot(P - Wb)) 197 | pos_record = [P] 198 | print('P:', P, 'V:', V, 'e:', e) 199 | while e >= eposilon: 200 | Pt = P - Wb 201 | U1 = K0 * Pt + K1 * A.dot(Pt) 202 | if np.linalg.norm(U1, ord=np.inf) > a0: 203 | U1 = U1 * a0 / np.linalg.norm(U1, ord=np.inf) 204 | U = -(U1 + V) / K2 205 | V = V + U * dt 206 | P = P + V * dt 207 | e = np.linalg.norm(A.dot(Pt)) 208 | print('P:', P, 'V:', V, 'e:', e) 209 | pos_record.append(P) 210 | pos_record = np.array(pos_record).T[0] 211 | path = np.array([Wa, Wb]).T[0] 212 | ax = plt.subplot(projection='3d') 213 | ax.plot(path[0], path[1], path[2]) 214 | ax.plot(pos_record[0], pos_record[1], pos_record[2], '--') 215 | # ax.scatter(pos_record[0], 216 | # pos_record[1], 217 | # pos_record[2], c='#00DDAA') 218 | plt.show() 219 | 220 | 221 | def move_by_path_3d(P, V, Path, K0=2, K1=2, K2=1, dt=0.1, a0=2, delta=0.5): 222 | """ 223 | 在三维空间内，控制无人机跟踪路径Path 224 | :param P: 起始位置 225 | :param V: 起始速度 226 | :param Path: 被跟踪路径 227 | :param K0: 控制器参数 228 | :param K1: 控制器参数 229 | :param K2: 控制器参数 230 | :param dt: 迭代时间 231 | :param a0: 控制器输出上限 232 | :param delta: 向前搜索下一个航路点的距离 233 | :return: 无 234 | """ 235 | def distance(A, B): 236 | return math.sqrt((A[0] - B[0]) ** 2 + 237 | (A[1] - B[1]) ** 2 + 238 | (A[2] - B[2]) ** 2) 239 | 240 | P = np.matrix(P).T 241 | pos_record = [P] 242 | e_record = [] 243 | V = np.matrix(V).T 244 | I3 = np.matrix([[1, 0, 0], 245 | [0, 1, 0], 246 | [0, 0, 1]]) 247 | 248 | for i in range(len(Path) - 1): 249 | Wa = np.matrix(Path[i]).T 250 | Wb = np.matrix(Path[i + 1]).T 251 | A = I3 - (Wa - Wb).dot((Wa - Wb).T) / (distance(Wa, Wb) ** 2) 252 | Pt = P - Wb 253 | e = np.linalg.norm(A.dot(Pt)) 254 | e_record.append(e) 255 | d = np.linalg.norm(Pt - A.dot(Pt)) 256 | print('i,', 'Start:', Path[i], ',Aim:', Path[i + 1]) 257 | print('\tP:', P, 'V:', V, 'e:', e) 258 | while d >= delta: 259 | Pt = P - Wb 260 | U1 = K0 * Pt + K1 * A.dot(Pt) 261 | if np.linalg.norm(U1, ord=np.inf) > a0: 262 | U1 = U1 * a0 / np.linalg.norm(U1, ord=np.inf) 263 | U = -(U1 + V) / K2 264 | V = V + U * dt 265 | P = P + V * dt 266 | e = np.linalg.norm(A.dot(Pt)) 267 | e_record.append(e) 268 | d = np.linalg.norm(Pt - A.dot(Pt)) 269 | print('\tP:', P, 'V:', V, 'e:', e) 270 | pos_record.append(P) 271 | pos_record = np.array(pos_record).T[0] 272 | path_plot = np.array(Path).T 273 | ax = plt.subplot(projection='3d') 274 | ax.plot(path_plot[0], path_plot[1], path_plot[2]) 275 | ax.plot(pos_record[0], pos_record[1], pos_record[2], '--') 276 | plt.show() 277 | plt.plot(e_record) 278 | plt.show() 279 | 280 | 281 | if __name__ == "__main__": 282 | # Path = [[0, 0], [10, 15], [15, 20], [20, 5]] 283 | # move_by_path([6, 3], 3, 0.5, Path, delta=0.1) 284 | Path = [[0, 0, 0], [10, 15, 15], [15, 20, 20], [20, 5, 5]] 285 | move_by_path_3d([1, 1, 1], [1, 1, 1], Path, K0=1, K1=2, K2=0.8) 286 | 287 | -------------------------------------------------------------------------------- /code_python/python_avoid_APF.py: -------------------------------------------------------------------------------- 1 | """ 2 | python_avoid_APF.py 3 | 人工势场法避障的python实现 4 | """ 5 | 6 | 7 | import math 8 | import matplotlib.pyplot as plt 9 | import numpy as np 10 | import cv2 11 | 12 | 13 | def avoid_APF(P_start, V_start, P_aim, mymap, Kg=0.5, kr=20, 14 | Q_search=20, epsilon=2, Vl=2, Ul=2, dt=0.2, draw_ontime=False): 15 | """ 16 | :param P_start: 初始位置 17 | :param V_start: 初始速度 18 | :param P_aim: 目标点 19 | :param mymap: 储存有障碍物信息的0-1二值化地图，0表示障碍物 20 | :param Kg: 避障控制器参数（引力） 21 | :param kr: 避障控制器参数（斥力） 22 | :param Q_search: 搜索障碍物距离阈值 23 | :param epsilon: 误差上限 24 | :param Vl: 速率上限 25 | :param Ul: 控制器输出上限 26 | :param dt: 迭代时间 27 | :param draw_ontime: 28 | :return: 无 29 | """ 30 | 31 | def distance(A, B): 32 | return math.sqrt((A[0] - B[0]) ** 2 + (A[1] - B[1]) ** 2) 33 | 34 | def myatan(a, b): 35 | if a == 0 and b == 0: 36 | return None 37 | if a == 0: 38 | return math.pi / 2 if b > 0 else math.pi * 3 / 2 39 | if b == 0: 40 | return 0 if a > 0 else -math.pi 41 | if b > 0: 42 | return math.atan(b / a) if a > 0 else (math.atan(b / a) + math.pi) 43 | return math.atan(b / a + 2 * math.pi) if a > 0 else math.atan(b / a) + math.pi 44 | 45 | def isClockwise(a, b): 46 | da = b - a 47 | if 0 < da < math.pi or -math.pi * 2 < da < -math.pi: 48 | return False 49 | return True 50 | 51 | # 读取初始状态 52 | P_start = np.array(P_start) # 初始位置 53 | V_start = np.array(V_start).T # 初始速度 54 | pos_record = [P_start] # 记录位置 55 | # 地图尺寸 56 | size_x = mymap.shape[0] 57 | size_y = mymap.shape[1] 58 | # 设置绘图参数 59 | plt.axis('equal') 60 | plt.xlim(0, size_x) 61 | plt.ylim(0, size_y) 62 | # 绘制地图（障碍物和航路点） 63 | plt.imshow(mymap.T) 64 | plt.plot([P_start[0], P_aim[0]], [P_start[1], P_aim[1]], 'o') 65 | # 计算周边障碍物搜素范围（在-90到90度范围） 66 | direction_search = np.array([-2, -1, 0, 1, 2]) * math.pi / 4 67 | # 开始飞行 68 | pos_num = 0 # 已经记录的位置的总数 69 | P_curr = P_start # 当前位置 70 | V_curr = V_start # 当前速度 71 | ob_flag = False # 是否处于局部极小值 72 | while distance(P_curr, P_aim) > epsilon: 73 | angle_curr = myatan(V_curr[0], V_curr[1]) # 当前无人机朝向 74 | Frep = np.array([0, 0]) # 斥力初始化为0 75 | for dir in direction_search: # 搜索周围障碍物 76 | angle_search = angle_curr + dir 77 | for dis_search in range(Q_search): 78 | P_search = [int(P_curr[0] + dis_search * math.sin(angle_search)), 79 | int(P_curr[1] + dis_search * math.cos(angle_search))] 80 | if not (0 <= P_search[0] < size_x and # 超出地图范围和地图内障碍，均视作障碍物 81 | 0 <= P_search[1] < size_y and 82 | mymap[int(P_search[0])][int(P_search[1])] == 1): 83 | d_search = distance(P_curr, P_search) # 被搜索点与当前点的距离 84 | Frep = Frep + \ 85 | kr * (1 / d_search - 1 / Q_search) / (d_search ** 3) * \ 86 | (P_curr - P_search) * (distance(P_search, P_aim) ** 2) 87 | break 88 | Fatt = -Kg * (P_curr - P_aim) # 计算引力 89 | if pos_num >= 1: # 从第二个时刻开始，判断是否陷入局部极小值 90 | p0 = pos_record[pos_num - 1] # 取上两个时刻物体相对终点的位移 91 | p1 = pos_record[pos_num - 2] 92 | Vra = (distance(p0, P_aim) - distance(p1, P_aim)) / dt 93 | if abs(Vra) < 0.6 * Vl: # 陷入局部极小值 94 | if not ob_flag: # 之前不是局部极小状态时，根据当前状态计算斥力偏向角theta 95 | angle_g = myatan(Fatt[0], Fatt[1]) 96 | angle_r = myatan(Frep[0], Frep[1]) 97 | if angle_r is None or angle_g is None: 98 | print('111') 99 | if isClockwise(angle_g, angle_r): 100 | theta = 15 * math.pi / 180 101 | else: 102 | theta = -15 * math.pi / 180 103 | ob_flag = True 104 | # 之前为局部极小，则继续使用上一时刻的斥力偏向角theta 105 | Frep = [math.cos(theta) * Frep[0] - math.sin(theta) * Frep[1], 106 | math.sin(theta) * Frep[0] + math.cos(theta) * Frep[1]] 107 | else: # 离开局部极小值 108 | ob_flag = False 109 | l = Vl 110 | Kv = 3 * l / (2 * l + abs(Vra)) 111 | Kd = 15 * math.exp(-(distance(P_curr, P_aim) - 3) ** 2 / 2) + 1 112 | Ke = 3 113 | Fatt = Kv * Kd * Ke * Fatt # 改进引力 114 | U = Fatt + Frep # 计算合力 115 | if np.linalg.norm(U, ord=np.inf) > Ul: # 控制器输出限幅 116 | U = Ul * U / np.linalg.norm(U, ord=np.inf) 117 | V_curr = V_curr + U * dt # 计算速度 118 | if np.linalg.norm(V_curr) > Vl: # 速度限幅 119 | V_curr = Vl * V_curr / np.linalg.norm(V_curr) 120 | P_curr = P_curr + V_curr * dt # 计算位置 121 | print(P_curr, V_curr, distance(P_curr, P_aim)) 122 | pos_record.append(P_curr) 123 | pos_num += 1 124 | if draw_ontime: 125 | plt.plot([float(pos_record[pos_num - 1][0]), float(pos_record[pos_num][0])], 126 | [float(pos_record[pos_num - 1][1]), float(pos_record[pos_num][1])], 'r') 127 | plt.pause(0.0001) 128 | plt.ioff() 129 | pos_record = np.array(pos_record).T 130 | plt.plot(pos_record[0], pos_record[1], '--') 131 | plt.show() 132 | 133 | 134 | if __name__ == "__main__": 135 | # 读取地图图像并二值化 136 | img = cv2.imread('map_avoid/6.png') 137 | gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) 138 | retval, dst = cv2.threshold(gray, 0, 255, cv2.THRESH_OTSU) 139 | dst = cv2.dilate(dst, None, iterations=1) 140 | dst = cv2.erode(dst, None, iterations=4) / 255 141 | avoid_APF(P_start=[15, 15], V_start=[0, 2], P_aim=[400, 400], Q_search=15, mymap=dst) 142 | 143 | -------------------------------------------------------------------------------- /code_python/python_avoid_RRT.py: -------------------------------------------------------------------------------- 1 | ''' 2 | python_avoid_RRT.py 3 | 二维空间内RRT法避障的python实现 4 | ''' 5 | 6 | 7 | import matplotlib.pyplot as plt 8 | import numpy as np 9 | import cv2 10 | import math 11 | import sortedcontainers 12 | 13 | 14 | # 树节点 15 | class rrt_treenode(): 16 | def __init__(self, value, parent=None, cost_from_start=0, dis_to_aim=0): 17 | self.value = value 18 | self.children = [] 19 | self.cost = cost_from_start 20 | self.dis = dis_to_aim 21 | self.parent = parent 22 | 23 | def __lt__(self, other): 24 | return self.cost + self.dis < other.cost + other.dis 25 | 26 | def path(self): 27 | node, path_back = self, [] 28 | while node: 29 | path_back.append(node.value) 30 | node = node.parent 31 | return list(reversed(path_back)) 32 | 33 | 34 | # 优先级队列 35 | class PriorityQueue(object): 36 | def __init__(self): 37 | self._queue = sortedcontainers.SortedList([]) 38 | 39 | def push(self, node): 40 | self._queue.add(node) 41 | 42 | def pop(self): 43 | return self._queue.pop(index=0) 44 | 45 | def size(self): 46 | return len(self._queue) 47 | 48 | 49 | def A_star(root, eposilon): 50 | q = PriorityQueue() 51 | q.push(root) 52 | while q.size() != 0: 53 | node_curr = q.pop() 54 | for child in node_curr.children: 55 | # 生成树时已经去重，搜索时不再进行 56 | if child.dis < eposilon: # 目标点 57 | return child.path() 58 | q.push(child) 59 | return [] 60 | 61 | 62 | def avoid_RRT(mymap, start, aim, p_sample, maxlimit, step, eposilon, isshow, Q_save): 63 | """ 64 | :param mymap: 0-1二值化地图，0表示障碍物 65 | :param start: 起始点 66 | :param aim: 目标点 67 | :param p_sample: 随机采样概率 68 | :param maxlimit: 最大迭代次数 69 | :param step: 搜索步长 70 | :param eposilon: 误差上限 71 | :param isshow: 是否显示搜索结果 72 | :param Q_save: 安全距离 73 | :return: 储存有二维信息的航路点列表 74 | """ 75 | 76 | def distance(A, B): 77 | return math.sqrt((A[0] - B[0]) ** 2 + (A[1] - B[1]) ** 2) 78 | 79 | start = np.array(start) 80 | aim = np.array(aim) 81 | mapsize = mymap.shape 82 | dir_save = [] # 安全距离计算，使所有障碍物向外膨胀Q_save 83 | for i in range(-Q_save, Q_save + 1): 84 | for j in range(-Q_save, Q_save + 1): 85 | if distance([i, j], [0, 0]) <= Q_save: 86 | dir_save.append(np.array([i, j])) 87 | tree_map = [] # 记录不同位置产生的节点，用于生成树时去重 88 | for i in range(mapsize[0]): 89 | tree_map.append([]) 90 | for j in range(mapsize[1]): 91 | if mymap[i][j] == 0: 92 | tree_map[i].append(0) 93 | else: 94 | tree_map[i].append(None) 95 | if tree_map[start[0]][start[1]] is None: 96 | root = rrt_treenode(start, dis_to_aim=distance(start, aim)) 97 | tree_map[start[0]][start[1]] = root 98 | else: 99 | print("出发点处存在障碍物！") 100 | return [] 101 | # 开始迭代 102 | p_record = [start] # 记录已经走过的位置，用于寻找采样点的最近点 103 | count = 0 104 | isArrive = False 105 | while count < maxlimit and not isArrive: 106 | # 获取采样点 107 | sample = [0, 0] 108 | while tree_map[sample[0]][sample[1]] is not None: 109 | if np.random.rand() < p_sample: 110 | sample = np.array([np.random.randint(0, mapsize[0]), 111 | np.random.randint(0, mapsize[1])]) 112 | else: 113 | sample = aim 114 | # 在已搜索的点中，寻找距离采样点最近的点p_curr 115 | min_dis = float('inf') 116 | p_curr = start 117 | for p_tmp in p_record: 118 | dis = distance(p_tmp, sample) 119 | if dis < min_dis: 120 | p_curr = p_tmp 121 | min_dis = dis 122 | # 向采样点前进，获取点p_next 123 | direction = (sample - p_curr) / np.linalg.norm((sample - p_curr)) 124 | p_next = p_curr + direction * step 125 | # 判断p_next位置是否可行 126 | flag = False 127 | if 0 <= p_next[0] < mapsize[0] and 0 <= p_next[1] < mapsize[1] \ 128 | and tree_map[int(p_next[0])][int(p_next[1])] is None: # 是否为非障碍物点 129 | flag = True # True表示该点可行 130 | if flag: 131 | for dir in dir_save: # 判断点是否为安全点 132 | p_search = p_next + dir 133 | if not (0 <= p_search[0] < mapsize[0] and 0 <= p_search[1] < mapsize[1] 134 | and tree_map[int(p_search[0])][int(p_search[1])] != 0): 135 | flag = False 136 | break 137 | if flag: # 判断路径上是否存在碰撞 138 | d_next = distance(p_curr, p_next) 139 | direction = (p_next - p_curr) / np.linalg.norm((p_next - p_curr)) 140 | for d_search in range(1, int(d_next) + 1): 141 | p_search = p_curr + d_search * direction 142 | if not (0 <= p_search[0] < mapsize[0] and 0 <= p_search[1] < mapsize[1] 143 | and tree_map[int(p_search[0])][int(p_search[1])] != 0): # 是否为安全点 144 | flag = False 145 | break 146 | if flag: # 可行 147 | parenttree = tree_map[int(p_curr[0])][int(p_curr[1])] 148 | newtree = rrt_treenode(p_next, parent=parenttree, 149 | cost_from_start=parenttree.cost + step, 150 | dis_to_aim=distance(p_next, aim)) 151 | tree_map[int(p_next[0])][int(p_next[1])] = newtree 152 | parenttree.children.append(newtree) 153 | p_record.append(p_next) 154 | # if count % 50 == 0: 155 | plt.plot(p_next[0], p_next[1], 'o', c='blue') 156 | plt.plot([p_curr[0], p_next[0]], [p_curr[1], p_next[1]], c='green') 157 | e = newtree.dis 158 | # print(count, ': (%.3f, %.3f)' %(p_next[0], p_next[1]), '; %.3f' %e) 159 | if e < eposilon: 160 | isArrive = True 161 | count += 1 162 | # 设置绘图参数 163 | plt.axis('equal') 164 | plt.xlim(0, mapsize[0]) 165 | plt.ylim(0, mapsize[1]) 166 | # 绘制地图 167 | plt.imshow(mymap.T) 168 | plt.plot(start[0], start[1], 'x', c='y') 169 | plt.plot(aim[0], aim[1], 'x', c='m') 170 | # 未到达 171 | if not isArrive: 172 | if isshow: 173 | plt.show() 174 | return [] 175 | # A*搜索最优路径 176 | path = A_star(root, eposilon) 177 | # 绘制 178 | if isshow: 179 | path_plot = np.array(path).T 180 | plt.plot(path_plot[0], path_plot[1], c='red') 181 | plt.show() 182 | return path 183 | 184 | 185 | if __name__ == "__main__": 186 | # 读取地图图像并二值化 187 | img = cv2.imread('map_avoid/6.png') 188 | gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) 189 | retval, mymap = cv2.threshold(gray, 0, 255, cv2.THRESH_OTSU) 190 | mymap = cv2.dilate(mymap, None, iterations=1).T 191 | mymap = cv2.erode(mymap, None, iterations=4) / 255 192 | path = [] 193 | while len(path) == 0: 194 | path = avoid_RRT(mymap, [0, 0], [380, 700], 0.1, 5000, 100, 50, True, 10) 195 | for i in range(len(path)): 196 | print(i, ': (%.3f, %.3f)' %(path[i][0], path[i][1])) 197 | if len(path) != 0: 198 | print('Find!') 199 | else: 200 | print('False!') 201 | # 增加高度信息并保存到本地，便于AirSim设置航路点 202 | path_for_airsim = [] 203 | for p in path: 204 | path_for_airsim.append([p[0] / 10, p[1] / 10, -3]) # 按需调整单位，这里使用的地图为10像素=1米 205 | path_for_airsim.append([38, 70, -3]) 206 | path_for_airsim = np.array(path_for_airsim) 207 | np.save('path_for_airsim', path_for_airsim) 208 | -------------------------------------------------------------------------------- /code_python/python_tracking_and_avoid.py: -------------------------------------------------------------------------------- 1 | """ 2 | python_tracking_and_avoid.py 3 | CarrotChasing轨迹跟踪算法与APF避障算法融合 4 | """ 5 | 6 | 7 | import math 8 | import matplotlib.pyplot as plt 9 | import numpy as np 10 | import cv2 11 | 12 | 13 | def move_by_path_and_avoid(P_start, V_start, Path, mymap, K_track=None, delta=1, 14 | K_avoid=None, Q_search=5, epsilon=2, Vl=5, Ul=2, dt=0.2, ontime=False): 15 | """ 16 | :param P_start: 初始位置 17 | :param V_start: 初始速度 18 | :param Path: 航路点集合 19 | :param mymap: 储存有障碍物信息的0-1二值化地图，0表示障碍物 20 | :param K_track: 轨迹跟踪控制器参数 21 | :param delta: 向前搜索下一个航路点的距离 22 | :param K_avoid: 避障控制器参数（引力，斥力） 23 | :param Q_search: 搜索障碍物距离 24 | :param epsilon: 误差上限 25 | :param Vl: 速率上限 26 | :param Ul: 控制器输出上限 27 | :param dt: 迭代时间 28 | :param ontime: 29 | :return: 无 30 | """ 31 | def distance(A, B): 32 | return math.sqrt((A[0] - B[0]) ** 2 + (A[1] - B[1]) ** 2) 33 | 34 | def myatan(a, b): 35 | if a == 0 and b == 0: 36 | return None 37 | if a == 0: 38 | return math.pi / 2 if b > 0 else math.pi * 3 / 2 39 | if b == 0: 40 | return 0 if a > 0 else -math.pi 41 | if b > 0: 42 | return math.atan(b / a) if a > 0 else (math.atan(b / a) + math.pi) 43 | return math.atan(b / a + 2 * math.pi) if a > 0 else math.atan(b / a) + math.pi 44 | 45 | def isClockwise(a, b): 46 | da = b - a 47 | if 0 < da < math.pi or -math.pi * 2 < da < -math.pi: 48 | return False 49 | return True 50 | 51 | # 读取初始状态 52 | if K_track == None: 53 | K_track = [2, 2, 2] 54 | [K0, K1, K2] = K_track 55 | if K_avoid == None: 56 | K_avoid = [0.5, 20] 57 | [Kg, Kr] = K_avoid 58 | P_start = np.array(P_start) # 初始位置 59 | pos_record = [P_start] # 记录位置 60 | # e_record = [] # 记录误差 61 | V_start = np.array(V_start).T # 初始速度 62 | I2 = np.matrix([[1, 0], # 用于计算轨迹跟踪控制器参数 63 | [0, 1]]) 64 | # 地图尺寸 65 | size_x = mymap.shape[0] 66 | size_y = mymap.shape[1] 67 | # 设置绘图参数 68 | plt.axis('equal') 69 | plt.xlim(0, size_x) 70 | plt.ylim(0, size_y) 71 | # plt.ion() # 开启实时绘图 72 | # 绘制地图（障碍物和航路点） 73 | plt.imshow(mymap.T) 74 | path_plot = np.array(Path).T 75 | plt.plot(path_plot[0], path_plot[1], 'o') 76 | # plt.savefig('tmp.png') 77 | # 计算周边障碍物搜素位置 78 | direction_search = np.array([-2, -1, 0, 1, 2]) * math.pi / 4 79 | # direction_search = [] 80 | # for t in range(- Q_search, Q_search + 1): 81 | # for s in range(-Q_search, Q_search + 1): 82 | # if (t != 0 or s != 0) and distance([s, t], [0, 0]) <= Q_search: 83 | # direction_search.append(np.array([t, s])) 84 | # 开始飞行 85 | pos_num = 0 # 已经记录的位置的总数 86 | P_curr = P_start # 当前位置 87 | V_curr = V_start 88 | Wb = P_curr 89 | ob_flag = False # 用于判断局部极小值 90 | for path_num in range(len(Path)): 91 | Wa = Wb # 出发航路点 92 | Wb = np.array(Path[path_num]) # 目标航路点 93 | Wa_sub_Wb_matrix = np.matrix((Wa - Wb)).T # 计算差矩阵 94 | A = I2 - Wa_sub_Wb_matrix.dot(Wa_sub_Wb_matrix.T) / (distance(Wa, Wb) ** 2) 95 | Pt_matrix = np.matrix(P_curr - Wb).T # 用于计算的中间矩阵，目标点到当前点的向量 96 | # e = np.linalg.norm(A.dot(Pt_matrix)) # 误差，当前到目标轨迹的垂直距离 97 | # e_record.append(e) 98 | d_to_Wb = np.linalg.norm(Pt_matrix - A.dot(Pt_matrix)) # 沿着轨迹方向，当前点距离目标点的距离 99 | print('跟踪航路点:', path_num, 'Start:', Wa, ',Aim:', Wb) 100 | # 开始跟踪当前航路Wa-Wb， 101 | # 如Wb为最后一个航点，则P距离Wb小于epsilon时终止 102 | # 否则，在delta距离内发现航点Wb时终止 103 | while (path_num != len(Path) - 2 or distance(Wb, P_curr) > epsilon) and \ 104 | (path_num == len(Path) - 2 or d_to_Wb > delta): 105 | Pt_matrix = np.matrix(P_curr - Wb).T 106 | Frep = np.array([0, 0]) # 斥力 107 | # # 搜索障碍物 108 | # for dir in direction_search: 109 | # P_search = P_curr + dir # 被搜索点的位置 110 | # if not (0 <= P_search[0] < size_x and # 超出地图范围，地图内障碍，均视作障碍物 111 | # 0 <= P_search[1] < size_y and 112 | # mymap[int(P_search[0])][int(P_search[1])] == 1): 113 | # d_search = distance(P_curr, P_search) # 被搜索点与当前点的距离 114 | # Frep = Frep + \ 115 | # Kr * (1 / d_search - 1 / Q_search) / (d_search ** 3) * \ 116 | # (P_curr - P_search) * (distance(P_search, Wb) ** 2) 117 | angle_curr = myatan(V_curr[0], V_curr[1]) 118 | for dir in direction_search: 119 | angle_search = angle_curr + dir 120 | for dis_search in range(Q_search): 121 | P_search = [int(P_curr[0] + dis_search * math.sin(angle_search)), 122 | int(P_curr[1] + dis_search * math.cos(angle_search))] 123 | if not (0 <= P_search[0] < size_x and # 超出地图范围，地图内障碍，均视作障碍物 124 | 0 <= P_search[1] < size_y and 125 | mymap[int(P_search[0])][int(P_search[1])] == 1): 126 | d_search = distance(P_curr, P_search) # 被搜索点与当前点的距离 127 | Frep = Frep + \ 128 | Kr * (1 / d_search - 1 / Q_search) / (d_search ** 3) * \ 129 | (P_curr - P_search) * (distance(P_search, Wb) ** 2) 130 | break 131 | # 未检测到障碍物，执行航路点跟踪 132 | if Frep[0] == 0 and Frep[1] == 0: 133 | U1 = np.array((K0 * Pt_matrix + K1 * A.dot(Pt_matrix)).T)[0] 134 | if np.linalg.norm(U1, ord=np.inf) > Ul: 135 | U1 = U1 * Ul / np.linalg.norm(U1, ord=np.inf) 136 | U = -(U1 + V_curr) / K2 # 计算控制器输出并转换为array 137 | # 检测到障碍物，执行避障 138 | else: 139 | Fatt = -Kg * (P_curr - Wb) # 计算引力 140 | if pos_num >= 1: 141 | # 计算上两个时刻物体相对终点的位移，以判断是否陷入局部极小值 142 | p0 = pos_record[pos_num - 1] 143 | p1 = pos_record[pos_num - 2] 144 | Vra = (distance(p0, Wb) - distance(p1, Wb)) / dt 145 | if abs(Vra) < 0.6 * Vl and Frep[0] != 0 and Frep[1] != 0: # 陷入局部极小值 146 | if ob_flag == False: 147 | # 之前不是局部极小状态时，根据当前位置计算斥力偏向角theta 148 | angle_g = myatan(Fatt[0], Fatt[1]) 149 | angle_r = myatan(Frep[0], Frep[1]) 150 | if isClockwise(angle_g, angle_r): 151 | theta = 15 * math.pi / 180 152 | else: 153 | theta = -15 * math.pi / 180 154 | ob_flag = True 155 | # 之前为局部极小，则继续使用上一时刻的斥力偏向角theta 156 | Frep = [math.cos(theta) * Frep[0] - math.sin(theta) * Frep[1], 157 | math.sin(theta) * Frep[0] + math.cos(theta) * Frep[1]] 158 | else: # 离开局部极小值 159 | ob_flag = False 160 | l = Vl 161 | Kv = 3 * l / (2 * l + abs(Vra)) 162 | Kd = 15 * math.exp(-(distance(P_curr, Wb) - 3) ** 2 / 2) + 1 163 | Ke = 3 164 | Fatt = Kv * Kd * Ke * Fatt # 改进引力 165 | U = Fatt + Frep # 计算合力 166 | if np.linalg.norm(U, ord=np.inf) > Ul: # 控制器输出限幅 167 | U = Ul * U / np.linalg.norm(U, ord=np.inf) 168 | # 执行 169 | V_curr = V_curr + U * dt # 计算速度 170 | if np.linalg.norm(V_curr) > Vl: # 速度限幅 171 | V_curr = Vl * V_curr / np.linalg.norm(V_curr) 172 | P_curr = P_curr + V_curr * dt # 计算位置 173 | pos_num += 1 174 | # 记录和绘图 175 | print(P_curr, V_curr, distance(P_curr, Wb)) 176 | # e = np.linalg.norm(A.dot(Pt_matrix)) 177 | # e_record.append(e) 178 | d_to_Wb = np.linalg.norm(Pt_matrix - A.dot(Pt_matrix)) 179 | pos_record.append(P_curr) 180 | 181 | if ontime: 182 | plt.plot([float(pos_record[pos_num - 1][0]), float(pos_record[pos_num][0])], 183 | [float(pos_record[pos_num - 1][1]), float(pos_record[pos_num][1])], 'r') 184 | plt.pause(0.001) 185 | plt.ioff() 186 | 187 | pos_record = np.array(pos_record).T 188 | plt.plot(pos_record[0], pos_record[1], '--') 189 | plt.show() 190 | 191 | 192 | if __name__ == "__main__": 193 | # 读取地图图像并二值化 194 | img = cv2.imread('map_avoid/1.png') 195 | gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) 196 | retval, dst = cv2.threshold(gray, 0, 255, cv2.THRESH_OTSU) 197 | dst = cv2.dilate(dst, None, iterations=1) 198 | dst = cv2.erode(dst, None, iterations=4) / 255 199 | 200 | K_track = [0.5, 0, 2] 201 | K_avoid = [0.5, 20] 202 | K = [1, 0, 2, 20] 203 | Path = [[65, 65]] 204 | move_by_path_and_avoid([0, 0], [1, 1], Path, dst, Q_search=15, K_avoid=K_avoid, ontime=False) 205 | -------------------------------------------------------------------------------- /code_python/readme.md: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /geometricControl.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import airsim 3 | 4 | # 输入：期望的位置、速度、加速度 5 | # 输出：力和力矩 6 | def Geometric_control(p_des, v_des, a_des, yaw_des, p_now, v_now, R_now, omega_now, m): 7 | g = 9.81 # 重力加速度 8 | e3 = np.array([[0], [0], [1]]) 9 | # 4个控制系数 10 | kp = 2 11 | kv = 2 12 | kR = 0.4 13 | komega = 0.08 14 | # 位置和速度误差 15 | e_p = p_now - p_des 16 | e_v = v_now - v_des 17 | # 求合力 f 18 | acc = -kp*e_p -kv*e_v - m*g*e3 + m*a_des 19 | f = -np.dot((acc).T, np.dot(R_now, e3)) 20 | # 求期望的旋转矩阵 R_des 21 | proj_xb = np.array([np.cos(yaw_des), np.sin(yaw_des), 0]) 22 | z_b = - acc.reshape(3) / np.linalg.norm(acc) 23 | y_b = np.cross(z_b, proj_xb) 24 | y_b = y_b / np.linalg.norm(y_b) 25 | x_b = np.cross(y_b, z_b) 26 | x_b = x_b / np.linalg.norm(x_b) 27 | R_des = np.hstack([np.hstack([x_b.reshape([3, 1]), y_b.reshape([3, 1])]), z_b.reshape([3, 1])]) 28 | # 求合力矩 M 29 | e_R_tem = np.dot(R_des.T, R_now) - np.dot(R_now.T, R_des)/2 30 | e_R = np.array([[e_R_tem[2, 1]], [e_R_tem[0, 2]], [e_R_tem[1, 0]]]) 31 | M = -kR * e_R - komega * omega_now 32 | return f[0, 0], M 33 | 34 | # 力和力矩到电机控制的转换 35 | def fM2u(f, M): 36 | mat = np.array([[4.179446268, 4.179446268, 4.179446268, 4.179446268], 37 | [-0.6723341164784, 0.6723341164784, 0.6723341164784, -0.6723341164784], 38 | [0.6723341164784, -0.6723341164784, 0.6723341164784, -0.6723341164784], 39 | [0.055562, 0.055562, -0.055562, -0.055562]]) 40 | fM = np.vstack([f, M]) 41 | u = np.dot(np.linalg.inv(mat), fM) 42 | u1 = u[0, 0] 43 | u2 = u[1, 0] 44 | u3 = u[2, 0] 45 | u4 = u[3, 0] 46 | return u1, u2, u3, u4 47 | 48 | # 欧拉角到旋转矩阵的转换 49 | def angle2R(roll, pitch, yaw): 50 | sphi = np.sin(roll) 51 | cphi = np.cos(roll) 52 | stheta = np.sin(pitch) 53 | ctheta = np.cos(pitch) 54 | spsi = np.sin(yaw) 55 | cpsi = np.cos(yaw) 56 | R = np.array([[ctheta*cpsi, sphi*stheta*cpsi-cphi*spsi, cphi*stheta*cpsi+sphi*spsi], 57 | [ctheta*spsi, sphi*stheta*spsi+cphi*cpsi, cphi*stheta*spsi-sphi*cpsi], 58 | [-stheta, sphi*ctheta, cphi*ctheta]]) 59 | return R 60 | 61 | def move_by_geometricControl(client, p_des, v_des, a_des, yaw, pos_now, vel_now, dt): 62 | """ 63 | 几何控制无人机飞行 64 | client: AirSim client 65 | p_des: desired position 66 | v_des: desired velocity 67 | a_des: desired acceleration 68 | yaw: yaw angle 69 | pos_now: current position 70 | vel_now: current velocity 71 | dt: duration 72 | """ 73 | m = 1 74 | state = client.getMultirotorState() 75 | pos_now = np.array([[state.kinematics_estimated.position.x_val], 76 | [state.kinematics_estimated.position.y_val], 77 | [state.kinematics_estimated.position.z_val]]) 78 | vel_now = np.array([[state.kinematics_estimated.linear_velocity.x_val], 79 | [state.kinematics_estimated.linear_velocity.y_val], 80 | [state.kinematics_estimated.linear_velocity.z_val]]) 81 | omega_now = np.array([[state.kinematics_estimated.angular_velocity.x_val], 82 | [state.kinematics_estimated.angular_velocity.y_val], 83 | [state.kinematics_estimated.angular_velocity.z_val]]) 84 | pitch_now, roll_now, yaw_now = airsim.to_eularian_angles(state.kinematics_estimated.orientation) 85 | R_now = angle2R(roll_now, pitch_now, yaw_now) 86 | 87 | f, M = Geometric_control(p_des, v_des, a_des, yaw, pos_now, vel_now, R_now, omega_now, m) 88 | u1, u2, u3, u4 = fM2u(f, M) 89 | client.moveByMotorPWMsAsync(u1, u2, u3, u4, dt) -------------------------------------------------------------------------------- /mymath.py: -------------------------------------------------------------------------------- 1 | import math 2 | 3 | 4 | def distance(A, B): 5 | return math.sqrt((A[0] - B[0]) ** 2 + (A[1] - B[1]) ** 2) 6 | 7 | 8 | def distance_3d(A, B): 9 | return math.sqrt((A[0] - B[0]) ** 2 + 10 | (A[1] - B[1]) ** 2 + 11 | (A[2] - B[2]) ** 2) 12 | 13 | 14 | # 计算向量AB的角度 15 | def myatan(A, B): 16 | x1, y1, x2, y2 = A[0], A[1], B[0], B[1] 17 | if x1 != x2: 18 | if x1 > x2: 19 | return math.atan((y1 - y2) / (x1 - x2)) + math.pi 20 | else: 21 | return math.atan((y1 - y2) / (x1 - x2)) 22 | if x1 == x2 and y1 == y2: 23 | return None # 两点重合时返回None，可自行处理 24 | if x1 == x2 and y1 != y2: 25 | if y1 > y2: 26 | return -math.pi / 2 27 | else: 28 | return math.pi / 2 29 | 30 | 31 | def isClockwise(a, b): 32 | da = b - a 33 | if 0 < da < math.pi or -math.pi * 2 < da < -math.pi: 34 | return False 35 | return True 36 | 37 | -------------------------------------------------------------------------------- /readme.md: -------------------------------------------------------------------------------- 1 | ### 目录结构： 2 | 3 | ├── Readme.md 4 | ├── airsim_avoid_APF.py // 可运行于AirSim环境中的航路点跟踪+避障代码脚本文件 5 | ├── airsim_tracking_carrot.py // 可运行于AirSim中的航路点跟踪代码脚本文件 6 | ├── UavAgent.py // 无人机控制和状态获取相关接口 7 | ├── mymath.py // 代码中使用到的自定义数学运算 8 | ├── code_python // python环境下算法实现和仿真相关文件（不依赖AirSim） 9 | │ ├── map_avoid // 避障算法仿真使用的地图图像 10 | │ ├── python_avoid_APF.py // 人工势场法避障代码脚本文件 11 | │ ├── python_avoid_RRT.py // RRT方法路径规划代码脚本文件 12 | │ ├── python_CarrotChasing.py // 航路点跟踪代码脚本文件 13 | │ ├── python_tracking_and_avoid.py // 航路点跟踪+避障代码脚本文件 14 | ├── ObstacleDetectioon // 障碍物检测接口 15 | │ ├── obstacles_detect.py 16 | │ ├── calculate_pose.py 17 | │ ├── 说明.txt --------------------------------------------------------------------------------