F. Tree with Maximum Cost

You are given a tree consisting exactly of ?n vertices. Tree is a connected undirected graph with ?−1n−1 edges. Each vertex ?v of this tree has a value ??av assigned to it.

Let ????(?,?)dist(x,y) be the distance between the vertices ?x and ?y. The distance between the vertices is the number of edges on the simple path between them.

Let's define the cost of the tree as the following value: firstly, let's fix some vertex of the tree. Let it be ?v. Then the cost of the tree is ∑?=1?????(?,?)⋅??∑i=1ndist(i,v)⋅ai.

Your task is to calculate the maximum possible cost of the tree if you can choose ?v arbitrarily.

Input

The first line contains one integer ?n, the number of vertices in the tree (1≤?≤2⋅1051≤n≤2⋅105).

The second line of the input contains ?n integers ?1,?2,…,??a1,a2,…,an (1≤??≤2⋅1051≤ai≤2⋅105), where ??ai is the value of the vertex ?i.

Each of the next ?−1n−1 lines describes an edge of the tree. Edge ?i is denoted by two integers ??ui and ??vi, the labels of vertices it connects (1≤??,??≤?1≤ui,vi≤n, ??≠??ui≠vi).

It is guaranteed that the given edges form a tree.

Output

Print one integer — the maximum possible cost of the tree if you can choose any vertex as ?v.

Note

Picture corresponding to the first example: 

You can choose the vertex 33 as a root, then the answer will be 2⋅9+1⋅4+0⋅1+3⋅7+3⋅10+4⋅1+4⋅6+4⋅5=18+4+0+21+30+4+24+20=1212⋅9+1⋅4+0⋅1+3⋅7+3⋅10+4⋅1+4⋅6+4⋅5=18+4+0+21+30+4+24+20=121.

In the second example tree consists only of one vertex so the answer is always 00.

思路:

            首先设定点1为根,设a[i]为点i的子树的权值和。dp[v]储存点在该点v处的∑i=dist(i,v)⋅ai。(先用dfs遍历树预处理处a[i],dp[i])。

           接下来换根思想。u为该树的根节点,设v是u的子节点 。那么这时候如果换做v为根节点,那么这时候相当于v的子节点距离根节点的距离都减1,非子节点距离都加1.那么dp[v]就等于原有的值减去子节点权值和,加上非子节点权值和。即dp[v]=dp[v]+(dp[1]-dp[v])-dp[v]。

#include<iostream>
#include<cstdio>
#include<cstring>
#define ll long long
using namespace std;
const int maxn=2*1e5+10;
ll dp[200000+10];
ll a[maxn];
int head[maxn];
int n,cnt;
ll ans;
struct node{
    int v,next;
}edge[maxn*2];
void add(int u,int v){
    edge[cnt].v=v;
    edge[cnt].next=head[u];
    head[u]=cnt++;
}
void dfs(int u,int fa){
    for(int i=head[u];i!=-1;i=edge[i].next){
        int v=edge[i].v;
        if(v!=fa){
            dfs(v,u);
            a[u]+=a[v];
            dp[u]+=a[v]+dp[v];
        }
    }
}
void dfs2(int u,int fa){
    if(u!=1)
        dp[u]=dp[fa]+a[1]-2*a[u];
    for(int i=head[u];i!=-1;i=edge[i].next){
        int v=edge[i].v;
        if(v!=fa){
            dfs2(v,u);
        }
    }
    if(ans<dp[u])ans=dp[u];
}
int main(){
    int u,v;
    cin>>n;
    cnt=0;ans=0;
    memset(head, -1, sizeof(head));
    memset(dp,0,sizeof(dp));
    for(int i=1;i<=n;i++){
        cin>>a[i];
    }
    for(int i=1;i<n;i++){
        cin>>u>>v;
        add(u,v);
        add(v,u);
    }
    dfs(1,0);//预处理a[i],dp[i]
    dfs2(1,0);//更新dp[i]查找最大值ans
    cout<<ans<<endl;
    return 0;
}

 

 

 

lass RRTStar3D: def __init__(self, start, goal, builds, bounds, max_iter=RRT_MAX_ITER, step_size=RRT_STEP, neighbor_radius=RRT_NEIGHBOR_RADIUS): self.start = np.array(start) self.goal = np.array(goal) # Pre-calculate builds with safety height buffer self.builds_with_safety = builds.copy() self.builds_with_safety[:, 4] -= SAFE_HEIGHT # Decrease zmin self.builds_with_safety[:, 5] += SAFE_HEIGHT # Increase zmax # Ensure zmin is not negative if SAFE_HEIGHT is large self.builds_with_safety[:, 4] = np.maximum(0, self.builds_with_safety[:, 4]) self.bounds = np.array(bounds) # Ensure bounds is numpy array self.max_iter = max_iter self.step_size = step_size self.neighbor_radius = neighbor_radius self.nodes = [self.start] self.parent = {tuple(self.start): None} self.cost = {tuple(self.start): 0.0} # Initialize KDTree with the start node self.kdtree = cKDTree(np.array([self.start])) # Ensure it's a 2D array def sample(self): # Biased sampling towards goal occasionally if np.random.rand() < 0.1: # 10% chance to sample goal return self.goal # Sample within bounds return np.random.uniform(self.bounds[:, 0], self.bounds[:, 1]) def nearest(self, q): _, idx = self.kdtree.query(q) # Handle case where KDTree might have only one node initially if isinstance(idx, (int, np.integer)): return self.nodes[idx] else: # Should not happen if tree has >= 1 node, but safety check return self.nodes[0] def steer(self, q_near, q_rand): delta = q_rand - q_near dist = np.linalg.norm(delta) if dist == 0: # Avoid division by zero return q_near ratio = self.step_size / dist if ratio >= 1.0: return q_rand return q_near + delta * ratio def near_neighbors(self, q_new): # Ensure nodes list is not empty before querying if not self.nodes: return [] # Ensure kdtree has points before querying if self.kdtree.n == 0: return [] # Use query_ball_point which is efficient for radius searches indices = self.kdtree.query_ball_point(q_new, self.neighbor_radius) # Filter out the index of q_new itself if it's already in nodes (might happen during rewiring) q_new_tuple = tuple(q_new) neighbors = [] for i in indices: # Check bounds and ensure it's not the node itself if already added # This check might be redundant if q_new isn't added before calling this if i < len(self.nodes): # Ensure index is valid node = self.nodes[i] if tuple(node) != q_new_tuple: neighbors.append(node) return neighbors def plan(self): for i in range(self.max_iter): q_rand = self.sample() # Check if nodes list is empty (shouldn't happen after init) if not self.nodes: print("Warning: Node list empty during planning.") continue # Or handle appropriately q_near = self.nearest(q_rand) q_new = self.steer(q_near, q_rand) # Check collision for the new segment using the pre-calculated safe builds if check_segment_collision(q_near, q_new, self.builds_with_safety) > 0: continue # If collision-free, add the node and update KD-Tree periodically q_new_tuple = tuple(q_new) q_near_tuple = tuple(q_near) # Choose parent with minimum cost among neighbors min_cost = self.cost[q_near_tuple] + np.linalg.norm(q_new - q_near) best_parent_node = q_near neighbors = self.near_neighbors(q_new) # Find neighbors first for q_neighbor in neighbors: q_neighbor_tuple = tuple(q_neighbor) # Check connectivity collision if check_segment_collision(q_neighbor, q_new, self.builds_with_safety) == 0: new_cost = self.cost[q_neighbor_tuple] + np.linalg.norm(q_new - q_neighbor) if new_cost < min_cost: min_cost = new_cost best_parent_node = q_neighbor # Add the new node with the best parent found self.nodes.append(q_new) q_best_parent_tuple = tuple(best_parent_node) self.parent[q_new_tuple] = q_best_parent_tuple self.cost[q_new_tuple] = min_cost # Rebuild KDTree periodically if len(self.nodes) % KD_REBUILD_EVERY == 0 or i == self.max_iter - 1: # Important: Ensure nodes is a list of arrays before creating KDTree if self.nodes: # Check if nodes is not empty self.kdtree = cKDTree(np.array(self.nodes)) # Rewire neighbors to go through q_new if it provides a shorter path for q_neighbor in neighbors: q_neighbor_tuple = tuple(q_neighbor) # Check if rewiring through q_new is shorter and collision-free cost_via_new = min_cost + np.linalg.norm(q_neighbor - q_new) if cost_via_new < self.cost[q_neighbor_tuple]: if check_segment_collision(q_new, q_neighbor, self.builds_with_safety) == 0: self.parent[q_neighbor_tuple] = q_new_tuple self.cost[q_neighbor_tuple] = cost_via_new # Check if goal is reached if np.linalg.norm(q_new - self.goal) < self.step_size: # Check final segment collision if check_segment_collision(q_new, self.goal, self.builds_with_safety) == 0: goal_tuple = tuple(self.goal) self.nodes.append(self.goal) # Add goal node self.parent[goal_tuple] = q_new_tuple self.cost[goal_tuple] = min_cost + np.linalg.norm(self.goal - q_new) print(f"RRT*: Goal reached at iteration {i+1}") # Rebuild KDTree one last time if goal is reached self.kdtree = cKDTree(np.array(self.nodes)) break # Exit planning loop else: # Loop finished without reaching goal condition print(f"RRT*: Max iterations ({self.max_iter}) reached. Connecting nearest node to goal.") # Find node closest to goal among existing nodes if not self.nodes: print("Error: No nodes generated by RRT*.") return None # Or raise error nodes_arr = np.array(self.nodes) distances_to_goal = np.linalg.norm(nodes_arr - self.goal, axis=1) nearest_node_idx = np.argmin(distances_to_goal) q_final_near = self.nodes[nearest_node_idx] q_final_near_tuple = tuple(q_final_near) goal_tuple = tuple(self.goal) # Try connecting nearest found node to goal if check_segment_collision(q_final_near, self.goal, self.builds_with_safety) == 0: self.nodes.append(self.goal) self.parent[goal_tuple] = q_final_near_tuple self.cost[goal_tuple] = self.cost[q_final_near_tuple] + np.linalg.norm(self.goal - q_final_near) print("RRT*: Connected nearest node to goal.") else: print("RRT*: Could not connect nearest node to goal collision-free. Returning path to nearest node.") # Path will be constructed to q_final_near instead of goal goal_tuple = q_final_near_tuple # Target for path reconstruction # Backtrack path from goal (or nearest reachable node) path = [] # Start backtracking from the actual last node added (goal or nearest) curr_tuple = goal_tuple if curr_tuple not in self.parent and curr_tuple != tuple(self.start): print(f"Warning: Target node {curr_tuple} not found in parent dict. Path reconstruction might fail.") # Fallback to the last added node if goal wasn't reachable/added correctly if self.nodes: curr_tuple = tuple(self.nodes[-1]) else: return None # No path possible while curr_tuple is not None: # Ensure the node corresponding to the tuple exists # This requires searching self.nodes, which is inefficient. # A better approach is to store nodes in the dict or use indices. # For now, let's assume tuple keys match numpy arrays. path.append(np.array(curr_tuple)) curr_tuple = self.parent.get(curr_tuple, None) if not path: print("Error: Path reconstruction failed.") return None if tuple(path[-1]) != tuple(self.start): print("Warning: Path does not end at start node.") return np.array(path[::-1]) # Reverse to get start -> goal order # --------------------- Path Cost Function (Use safe builds) --------------------- def path_cost(path_pts, builds_with_safety, drone_speed=DRONE_SPEED, penalty_k=PENALTY_K): total_time = 0.0 total_penalty = 0.0 num_segments = len(path_pts) - 1 if num_segments < 1: return 0.0 # No cost for a single point path # Vectorized calculations where possible p = path_pts[:-1] # Start points of segments q = path_pts[1:] # End points of segments segments = q - p distances = np.linalg.norm(segments, axis=1) # Avoid division by zero for zero-length segments valid_segments = distances > 1e-6 if not np.any(valid_segments): return 0.0 # Path has no length p = p[valid_segments] q = q[valid_segments] segments = segments[valid_segments] distances = distances[valid_segments] dir_unit = segments / distances[:, np.newaxis] # Interpolate wind at midpoints for better average (optional, could use start/end) midpoints = p + segments / 2.0 # try: # wind_vectors = interp(midpoints) # except ValueError as e: # print(f"Interpolation error: {e}") # print(f"Midpoints shape: {midpoints.shape}") # # Handle error, e.g., return a large cost or use zero wind # wind_vectors = np.zeros_like(midpoints) # Calculate ground speed component along each segment # Ensure wind_vectors and dir_unit have compatible shapes for dot product # np.einsum is efficient for row-wise dot products # wind_along_path = np.einsum('ij,ij->i', wind_vectors, dir_unit) ground_speeds = np.maximum(drone_speed , 1e-3) # Avoid zero/negative speed # Calculate time for each segment segment_times = distances / ground_speeds total_time = np.sum(segment_times) # Calculate collision penalty (iterate segments as check_segment_collision is per-segment) for i in range(len(p)): # Pass pre-calculated builds_with_safety penetration = check_segment_collision(p[i], q[i], builds_with_safety) if penetration > 0: total_penalty += penalty_k * penetration**2 # Quadratic penalty return total_time + total_penalty生成注释
05-14
import os import sys import math import heapq import time import numpy as np import matplotlib.pyplot as plt import scipy.spatial as kd import imageio.v2 as imageio import pandas as pd import openpyxl sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../MotionPlanning/") import astar as astar import reeds_shepp as rs class C: # Parameter config PI = np.pi XY_RESO = 2.0 # [m] YAW_RESO = np.deg2rad(15.0) # [rad] GOAL_YAW_ERROR = np.deg2rad(3.0) # [rad] MOVE_STEP = 0.2 # [m] path interporate resolution N_STEER = 20.0 # number of steer command COLLISION_CHECK_STEP = 10 # skip number for collision check EXTEND_AREA = 5.0 # [m] map extend length GEAR_COST = 100.0 # switch back penalty cost BACKWARD_COST = 5.0 # backward penalty cost STEER_CHANGE_COST = 5.0 # steer angle change penalty cost STEER_ANGLE_COST = 1.0 # steer angle penalty cost SCISSORS_COST = 200.0 # scissors cost H_COST = 10.0 # Heuristic cost W = 3.75 # [m] width of vehicle WB = 4.5 # [m] wheel base: rear to front steer WD = 2.1 # [m] distance between left-right wheels RF = 7.23 # [m] distance from rear to vehicle front end of vehicle RB = 0.77 # [m] distance from rear to vehicle back end of vehicle RTR = 15.596 # [m] rear to trailer wheel RTF = 4.35 # [m] distance from rear to vehicle front end of trailer RTB = 35.15 # [m] distance from rear to vehicle back end of trailer TR = 0.5 # [m] tyre radius TW = 0.285 # [m] tyre width TR1 = 0.447 # [m] tyre radius TW1 = 0.283 # [m] tyre width MAX_STEER = 0.52 # [rad] maximum steering angle TR2 = 0.565 TW2 = 0.419+0.8636 TR3 = 0.343 TW3 = 0.1968+0.4064 TR4 = 0.536 TW4 = 0.385 # 拖车参数 HINGE_DISTANCE = 1.45 # 铰接点距离牵引车后轴的距离 HINGE_DISTANCE_Front = 4.09 # 铰接点距离水平部分前端的距离 HINGE_DISTANCE_RW = 15.596 # 铰接点距离主起的距离 DISTANCE_MAIN_GEAR = 5.7146 # 主起的距离 TRAILER_VERTICAL_LENGTH = 16.65*2 # 垂直部分的总长度 TRAILER_VERTICAL_WIDTH = 5.0 # 垂直部分的宽度 TRAILER_HORIZONTAL_LENGTH = 39.5 # 水平部分的长度 TRAILER_HORIZONTAL_WIDTH = 3.759 # 水平部分的宽度 Length_hinge_end = TRAILER_HORIZONTAL_LENGTH - HINGE_DISTANCE_Front Length_hinge_front = HINGE_DISTANCE_Front Length_rear_main_gear = HINGE_DISTANCE_RW class Node: def __init__(self, xind, yind, yawind, direction, x, y, yaw, yawt, directions, steer, cost, pind): self.xind = xind self.yind = yind self.yawind = yawind self.direction = direction self.x = x self.y = y self.yaw = yaw self.yawt = yawt self.directions = directions self.steer = steer self.cost = cost self.pind = pind class Para: def __init__(self, minx, miny, minyaw, minyawt, maxx, maxy, maxyaw, maxyawt, xw, yw, yaww, yawtw, xyreso, yawreso, ox, oy, kdtree): self.minx = minx self.miny = miny self.minyaw = minyaw self.minyawt = minyawt self.maxx = maxx self.maxy = maxy self.maxyaw = maxyaw self.maxyawt = maxyawt self.xw = xw self.yw = yw self.yaww = yaww self.yawtw = yawtw self.xyreso = xyreso self.yawreso = yawreso self.ox = ox self.oy = oy self.kdtree = kdtree class Path: def __init__(self, x, y, yaw, yawt, direction, cost): self.x = x self.y = y self.yaw = yaw self.yawt = yawt self.direction = direction self.cost = cost class QueuePrior: def __init__(self): self.queue = [] def empty(self): return len(self.queue) == 0 # if Q is empty def put(self, item, priority): heapq.heappush(self.queue, (priority, item)) # reorder x using priority def get(self): return heapq.heappop(self.queue)[1] # pop out element with smallest priority def hybrid_astar_planning(sx, sy, syaw, syawt, gx, gy, gyaw, gyawt, ox, oy, xyreso, yawreso): """ planning hybrid A* path. :param sx: starting node x position [m] :param sy: starting node y position [m] :param syaw: starting node yaw angle [rad] :param syawt: starting node trailer yaw angle [rad] :param gx: goal node x position [m] :param gy: goal node y position [m] :param gyaw: goal node yaw angle [rad] :param gyawt: goal node trailer yaw angle [rad] :param ox: obstacle x positions [m] :param oy: obstacle y positions [m] :param xyreso: grid resolution [m] :param yawreso: yaw resolution [m] :return: hybrid A* path """ sxr, syr = round(sx / xyreso), round(sy / xyreso) gxr, gyr = round(gx / xyreso), round(gy / xyreso) syawr = round(rs.pi_2_pi(syaw) / yawreso) gyawr = round(rs.pi_2_pi(gyaw) / yawreso) nstart = Node(sxr, syr, syawr, 1, [sx], [sy], [syaw], [syawt], [1], 0.0, 0.0, -1) ngoal = Node(gxr, gyr, gyawr, 1, [gx], [gy], [gyaw], [gyawt], [1], 0.0, 0.0, -1) kdtree = kd.KDTree([[x, y] for x, y in zip(ox, oy)]) P = calc_parameters(ox, oy, xyreso, yawreso, kdtree) hmap = astar.calc_holonomic_heuristic_with_obstacle(ngoal, P.ox, P.oy, P.xyreso, 1.0) steer_set, direc_set = calc_motion_set() open_set, closed_set = {calc_index(nstart, P): nstart}, {} qp = QueuePrior() qp.put(calc_index(nstart, P), calc_hybrid_cost(nstart, hmap, P)) while True: if not open_set: return None ind = qp.get() n_curr = open_set[ind] closed_set[ind] = n_curr open_set.pop(ind) update, fpath = update_node_with_analystic_expantion(n_curr, ngoal, gyawt, P) if update: fnode = fpath break yawt0 = n_curr.yawt[0] for i in range(len(steer_set)): node = calc_next_node(n_curr, ind, steer_set[i], direc_set[i], P) if not is_index_ok(node, yawt0, P): continue node_ind = calc_index(node, P) if node_ind in closed_set: continue if node_ind not in open_set: open_set[node_ind] = node qp.put(node_ind, calc_hybrid_cost(node, hmap, P)) else: if open_set[node_ind].cost > node.cost: open_set[node_ind] = node print("final expand node: ", len(open_set) + len(closed_set)) return extract_path(closed_set, fnode, nstart) def extract_path(closed, ngoal, nstart): rx, ry, ryaw, ryawt, direc = [], [], [], [], [] cost = 0.0 node = ngoal while True: rx += node.x[::-1] ry += node.y[::-1] ryaw += node.yaw[::-1] ryawt += node.yawt[::-1] direc += node.directions[::-1] cost += node.cost if is_same_grid(node, nstart): break node = closed[node.pind] rx = rx[::-1] ry = ry[::-1] ryaw = ryaw[::-1] ryawt = ryawt[::-1] direc = direc[::-1] direc[0] = direc[1] path = Path(rx, ry, ryaw, ryawt, direc, cost) return path def update_node_with_analystic_expantion(n_curr, ngoal, gyawt, P): path = analystic_expantion(n_curr, ngoal, P) # rs path: n -> ngoal if not path: return False, None steps = [C.MOVE_STEP * d for d in path.directions] yawt = calc_trailer_yaw(path.yaw, n_curr.yawt[-1], steps) if abs(rs.pi_2_pi(yawt[-1] - gyawt)) >= C.GOAL_YAW_ERROR: return False, None fx = path.x[1:-1] fy = path.y[1:-1] fyaw = path.yaw[1:-1] fd = [] for d in path.directions[1:-1]: if d >= 0: fd.append(1.0) else: fd.append(-1.0) # fd = path.directions[1:-1] fcost = n_curr.cost + calc_rs_path_cost(path, yawt) fpind = calc_index(n_curr, P) fyawt = yawt[1:-1] fsteer = 0.0 fpath = Node(n_curr.xind, n_curr.yind, n_curr.yawind, n_curr.direction, fx, fy, fyaw, fyawt, fd, fsteer, fcost, fpind) return True, fpath def analystic_expantion(node, ngoal, P): sx, sy, syaw = node.x[-1], node.y[-1], node.yaw[-1] gx, gy, gyaw = ngoal.x[-1], ngoal.y[-1], ngoal.yaw[-1] maxc = math.tan(C.MAX_STEER) / C.WB paths = rs.calc_all_paths(sx, sy, syaw, gx, gy, gyaw, maxc, step_size=C.MOVE_STEP) if not paths: return None pq = QueuePrior() for path in paths: steps = [C.MOVE_STEP * d for d in path.directions] yawt = calc_trailer_yaw(path.yaw, node.yawt[-1], steps) pq.put(path, calc_rs_path_cost(path, yawt)) # while not pq.empty(): path = pq.get() steps = [C.MOVE_STEP * d for d in path.directions] yawt = calc_trailer_yaw(path.yaw, node.yawt[-1], steps) ind = range(0, len(path.x), C.COLLISION_CHECK_STEP) pathx = [path.x[k] for k in ind] pathy = [path.y[k] for k in ind] pathyaw = [path.yaw[k] for k in ind] pathyawt = [yawt[k] for k in ind] if not is_collision(pathx, pathy, pathyaw, pathyawt, P): return path return None def calc_next_node(n, ind, u, d, P): step = C.XY_RESO * 2.0 nlist = math.ceil(step / C.MOVE_STEP) xlist = [n.x[-1] + d * C.MOVE_STEP * math.cos(n.yaw[-1])] ylist = [n.y[-1] + d * C.MOVE_STEP * math.sin(n.yaw[-1])] yawlist = [rs.pi_2_pi(n.yaw[-1] + d * C.MOVE_STEP / C.WB * math.tan(u))] yawtlist = [rs.pi_2_pi(n.yawt[-1] + d * C.MOVE_STEP / C.RTR * math.sin(n.yaw[-1] - n.yawt[-1]))] for i in range(nlist - 1): xlist.append(xlist[i] + d * C.MOVE_STEP * math.cos(yawlist[i])) ylist.append(ylist[i] + d * C.MOVE_STEP * math.sin(yawlist[i])) yawlist.append(rs.pi_2_pi(yawlist[i] + d * C.MOVE_STEP / C.WB * math.tan(u))) yawtlist.append(rs.pi_2_pi(yawtlist[i] + d * C.MOVE_STEP / C.RTR * math.sin(yawlist[i] - yawtlist[i]))) xind = round(xlist[-1] / P.xyreso) yind = round(ylist[-1] / P.xyreso) yawind = round(yawlist[-1] / P.yawreso) cost = 0.0 if d > 0: direction = 1.0 cost += abs(step) else: direction = -1.0 cost += abs(step) * C.BACKWARD_COST if direction != n.direction: # switch back penalty cost += C.GEAR_COST cost += C.STEER_ANGLE_COST * abs(u) # steer penalyty cost += C.STEER_CHANGE_COST * abs(n.steer - u) # steer change penalty cost += C.SCISSORS_COST * sum([abs(rs.pi_2_pi(x - y)) for x, y in zip(yawlist, yawtlist)]) # jacknif cost cost = n.cost + cost directions = [direction for _ in range(len(xlist))] node = Node(xind, yind, yawind, direction, xlist, ylist, yawlist, yawtlist, directions, u, cost, ind) return node def is_collision(x, y, yaw, yawt, P): for ix, iy, iyaw, iyawt in zip(x, y, yaw, yawt): d = 1 deltal = (C.RF - C.RB) / 2.0 rc = (C.RF + C.RB) / 2.0 + d cx = ix + deltal * math.cos(iyaw) cy = iy + deltal * math.sin(iyaw) ids = P.kdtree.query_ball_point([cx, cy], rc) if ids: for i in ids: xo = P.ox[i] - cx yo = P.oy[i] - cy dx_car = xo * math.cos(iyaw) + yo * math.sin(iyaw) dy_car = -xo * math.sin(iyaw) + yo * math.cos(iyaw) if abs(dx_car) <= rc and \ abs(dy_car) <= C.W / 2.0 + d: return True d1 = 7.5 deltal = C.HINGE_DISTANCE_RW rt = C.TRAILER_VERTICAL_LENGTH / 2.0 + d1 ctx = ix + C.HINGE_DISTANCE * math.cos(iyaw) - deltal * math.cos(iyawt) cty = iy + C.HINGE_DISTANCE * math.sin(iyaw) - deltal * math.sin(iyawt) idst = P.kdtree.query_ball_point([ctx, cty], rt) if idst: for i in idst: xot = P.ox[i] - ctx yot = P.oy[i] - cty dx_trail = xot * math.cos(iyawt) + yot * math.sin(iyawt) dy_trail = -xot * math.sin(iyawt) + yot * math.cos(iyawt) if abs(dx_trail) <= rt and \ abs(dy_trail) <= rt: return True # 添加横摆角差值的判断 yaw_diff = abs(rs.pi_2_pi(iyaw - iyawt)) if np.rad2deg(yaw_diff) > 70: return True return False def calc_trailer_yaw(yaw, yawt0, steps): yawt = [0.0 for _ in range(len(yaw))] yawt[0] = yawt0 for i in range(1, len(yaw)): yawt[i] += yawt[i - 1] + steps[i - 1] / C.RTR * math.sin(yaw[i - 1] - yawt[i - 1]) return yawt def trailer_motion_model(x, y, yaw, yawt, D, d, L, delta): x += D * math.cos(yaw) y += D * math.sin(yaw) yaw += D / L * math.tan(delta) yawt += D / d * math.sin(yaw - yawt) return x, y, yaw, yawt def calc_rs_path_cost(rspath, yawt): cost = 0.0 for lr in rspath.lengths: if lr >= 0: cost += 1 else: cost += abs(lr) * C.BACKWARD_COST for i in range(len(rspath.lengths) - 1): if rspath.lengths[i] * rspath.lengths[i + 1] < 0.0: cost += C.GEAR_COST for ctype in rspath.ctypes: if ctype != "S": cost += C.STEER_ANGLE_COST * abs(C.MAX_STEER) nctypes = len(rspath.ctypes) ulist = [0.0 for _ in range(nctypes)] for i in range(nctypes): if rspath.ctypes[i] == "R": ulist[i] = -C.MAX_STEER elif rspath.ctypes[i] == "WB": ulist[i] = C.MAX_STEER for i in range(nctypes - 1): cost += C.STEER_CHANGE_COST * abs(ulist[i + 1] - ulist[i]) cost += C.SCISSORS_COST * sum([abs(rs.pi_2_pi(x - y)) for x, y in zip(rspath.yaw, yawt)]) return cost def calc_motion_set(): s = [i for i in np.arange(C.MAX_STEER / C.N_STEER, C.MAX_STEER, C.MAX_STEER / C.N_STEER)] steer = [0.0] + s + [-i for i in s] direc = [1.0 for _ in range(len(steer))] + [-1.0 for _ in range(len(steer))] steer = steer + steer return steer, direc def calc_hybrid_cost(node, hmap, P): cost = node.cost + \ C.H_COST * hmap[node.xind - P.minx][node.yind - P.miny] return cost def calc_index(node, P): ind = (node.yawind - P.minyaw) * P.xw * P.yw + \ (node.yind - P.miny) * P.xw + \ (node.xind - P.minx) yawt_ind = round(node.yawt[-1] / P.yawreso) ind += (yawt_ind - P.minyawt) * P.xw * P.yw * P.yaww return ind def is_index_ok(node, yawt0, P): if node.xind <= P.minx or \ node.xind >= P.maxx or \ node.yind <= P.miny or \ node.yind >= P.maxy: return False steps = [C.MOVE_STEP * d for d in node.directions] yawt = calc_trailer_yaw(node.yaw, yawt0, steps) ind = range(0, len(node.x), C.COLLISION_CHECK_STEP) x = [node.x[k] for k in ind] y = [node.y[k] for k in ind] yaw = [node.yaw[k] for k in ind] yawt = [yawt[k] for k in ind] if is_collision(x, y, yaw, yawt, P): return False return True def calc_parameters(ox, oy, xyreso, yawreso, kdtree): minxm = min(ox) - C.EXTEND_AREA minym = min(oy) - C.EXTEND_AREA maxxm = max(ox) + C.EXTEND_AREA maxym = max(oy) + C.EXTEND_AREA ox.append(minxm) oy.append(minym) ox.append(maxxm) oy.append(maxym) minx = round(minxm / xyreso) miny = round(minym / xyreso) maxx = round(maxxm / xyreso) maxy = round(maxym / xyreso) xw, yw = maxx - minx, maxy - miny minyaw = round(-C.PI / yawreso) - 1 maxyaw = round(C.PI / yawreso) yaww = maxyaw - minyaw minyawt, maxyawt, yawtw = minyaw, maxyaw, yaww P = Para(minx, miny, minyaw, minyawt, maxx, maxy, maxyaw, maxyawt, xw, yw, yaww, yawtw, xyreso, yawreso, ox, oy, kdtree) return P def is_same_grid(node1, node2): if node1.xind != node2.xind or \ node1.yind != node2.yind or \ node1.yawind != node2.yawind: return False return True def draw_arrow(x, y, yaw, length, car_color): plt.arrow(x, y, length * math.cos(yaw), length * math.sin(yaw), head_length=0.5, head_width=0.5, color=car_color) def draw_model(x, y, yaw, yawt, steer, car_color='blue', trailer_color='green'): car = np.array([[-C.RB, -C.RB, C.RF, C.RF, -C.RB], [C.W / 2, -C.W / 2, -C.W / 2, C.W / 2, C.W / 2]]) # 定义车轮的几何形状 wheel1 = np.array([[-C.TR1, -C.TR1, C.TR1, C.TR1, -C.TR1], [C.TW1 / 2, -C.TW1 / 2, -C.TW1 / 2, C.TW1 / 2, C.TW1 / 2]]) wheel3 = np.array([[-C.TR3, -C.TR3, C.TR3, C.TR3, -C.TR3], [C.TW3 / 2, -C.TW3 / 2, -C.TW3 / 2, C.TW3 / 2, C.TW3 / 2]]) wheel2 = np.array([[-C.TR2, -C.TR2, C.TR2, C.TR2, -C.TR2], [C.TW2 / 2, -C.TW2 / 2, -C.TW2 / 2, C.TW2 / 2, C.TW2 / 2]]) wheel4 = np.array([[-C.TR4, -C.TR4, C.TR4, C.TR4, -C.TR4], [C.TW4 / 2, -C.TW4 / 2, -C.TW4 / 2, C.TW4 / 2, C.TW4 / 2]]) # 复制车轮的几何形状 rlWheel = wheel4.copy() rrWheel = wheel4.copy() frWheel = wheel1.copy() flWheel = wheel1.copy() # 定义拖车的两个矩形 # 拖车主体(水平部分) trail_horizontal = np.array([[-C.Length_hinge_end, -C.Length_hinge_end, C.Length_hinge_front, C.Length_hinge_front, -C.Length_hinge_end], [-C.TRAILER_HORIZONTAL_WIDTH / 2, C.TRAILER_HORIZONTAL_WIDTH / 2, C.TRAILER_HORIZONTAL_WIDTH / 2, -C.TRAILER_HORIZONTAL_WIDTH / 2, -C.TRAILER_HORIZONTAL_WIDTH / 2]]) # 拖车垂直部分 trail_vertical = np.array([[-C.Length_rear_main_gear + C.TRAILER_VERTICAL_WIDTH / 2, -C.Length_rear_main_gear + C.TRAILER_VERTICAL_WIDTH / 2, -C.Length_rear_main_gear - C.TRAILER_VERTICAL_WIDTH / 2, -C.Length_rear_main_gear - C.TRAILER_VERTICAL_WIDTH / 2, -C.Length_rear_main_gear + C.TRAILER_VERTICAL_WIDTH / 2], [-C.TRAILER_VERTICAL_LENGTH / 2, C.TRAILER_VERTICAL_LENGTH / 2, C.TRAILER_VERTICAL_LENGTH / 2, -C.TRAILER_VERTICAL_LENGTH / 2, -C.TRAILER_VERTICAL_LENGTH / 2]]) # 复制拖车轮 rltWheel = wheel2.copy() rrtWheel = wheel2.copy() rltWheel[0, :] -= C.HINGE_DISTANCE_RW rltWheel[1, :] += C.DISTANCE_MAIN_GEAR / 2 rrtWheel[0, :] -= C.HINGE_DISTANCE_RW rrtWheel[1, :] += - C.DISTANCE_MAIN_GEAR / 2 fltWheel = wheel3.copy() # 定义旋转矩阵 Rot1 = np.array([[math.cos(yaw), -math.sin(yaw)], [math.sin(yaw), math.cos(yaw)]]) Rot2 = np.array([[math.cos(steer), -math.sin(steer)], [math.sin(steer), math.cos(steer)]]) Rot3 = np.array([[math.cos(yawt), -math.sin(yawt)], [math.sin(yawt), math.cos(yawt)]]) # 旋转前轮 frWheel = np.dot(Rot2, frWheel) flWheel = np.dot(Rot2, flWheel) # 调整车轮的位置 frWheel += np.array([[C.WB], [-C.WD / 2]]) flWheel += np.array([[C.WB], [C.WD / 2]]) rrWheel[1, :] -= C.WD / 2 rlWheel[1, :] += C.WD / 2 # 旋转车轮和车身 frWheel = np.dot(Rot1, frWheel) flWheel = np.dot(Rot1, flWheel) rrWheel = np.dot(Rot1, rrWheel) rlWheel = np.dot(Rot1, rlWheel) car = np.dot(Rot1, car) hinge_point = np.array([C.HINGE_DISTANCE * math.cos(yaw), C.HINGE_DISTANCE * math.sin(yaw)]) # 调整拖车轮和拖车的位置 fltWheel = np.dot(Rot3, fltWheel) rltWheel = np.dot(Rot3, rltWheel) rrtWheel = np.dot(Rot3, rrtWheel) fltWheel[0, :] += hinge_point[0] fltWheel[1, :] += hinge_point[1] rltWheel[0, :] += hinge_point[0] rltWheel[1, :] += hinge_point[1] rrtWheel[0, :] += hinge_point[0] rrtWheel[1, :] += hinge_point[1] # 旋转拖车主体 trail_horizontal = np.dot(Rot3, trail_horizontal) trail_horizontal[0, :] += hinge_point[0] trail_horizontal[1, :] += hinge_point[1] # 旋转拖车垂直部分90度 trail_vertical = np.dot(Rot3, trail_vertical) trail_vertical[0, :] += hinge_point[0] trail_vertical[1, :] += hinge_point[1] # 平移所有部件到车辆中心 frWheel += np.array([[x], [y]]) flWheel += np.array([[x], [y]]) rrWheel += np.array([[x], [y]]) rlWheel += np.array([[x], [y]]) rrtWheel += np.array([[x], [y]]) rltWheel += np.array([[x], [y]]) fltWheel += np.array([[x], [y]]) car += np.array([[x], [y]]) trail_horizontal += np.array([[x], [y]]) trail_vertical += np.array([[x], [y]]) # 绘制车辆和拖车 plt.plot(car[0, :], car[1, :], car_color) plt.plot(trail_horizontal[0, :], trail_horizontal[1, :], trailer_color) plt.plot(trail_vertical[0, :], trail_vertical[1, :], trailer_color) plt.plot(frWheel[0, :], frWheel[1, :], car_color) plt.plot(rrWheel[0, :], rrWheel[1, :], car_color) plt.plot(flWheel[0, :], flWheel[1, :], car_color) plt.plot(rlWheel[0, :], rlWheel[1, :], car_color) plt.plot(rrtWheel[0, :], rrtWheel[1, :], trailer_color) plt.plot(rltWheel[0, :], rltWheel[1, :], trailer_color) plt.plot(fltWheel[0, :], fltWheel[1, :], trailer_color) draw_arrow(x, y, yaw, C.WB * 0.8, car_color) def design_obstacles(): ox, oy = [], [] for i in range(40, 150): ox.append(i) oy.append(70) for i in range(40, 150): ox.append(i) oy.append(10) for j in range(-100, 160): ox.append(-50) oy.append(j) for j in range(-100, 10): ox.append(40) oy.append(j) for j in range(70, 160): ox.append(40) oy.append(j) for j in range(80, 90): ox.append(-20) oy.append(j) for j in range(80, 90): ox.append(-30) oy.append(j) for i in range(-30, -20): ox.append(i) oy.append(80) for i in range(-30, -20): ox.append(i) oy.append(90) return ox, oy def test(x, y, yaw, yawt, ox, oy): d = 0.5 deltal = (C.RTF - C.RTB) / 2.0 rt = (C.RTF + C.RTB) / 2.0 + d ctx = x + deltal * math.cos(yawt) cty = y + deltal * math.sin(yawt) deltal = (C.RF - C.RB) / 2.0 rc = (C.RF + C.RB) / 2.0 + d xot = ox - ctx yot = oy - cty dx_trail = xot * math.cos(yawt) + yot * math.sin(yawt) dy_trail = -xot * math.sin(yawt) + yot * math.cos(yawt) if abs(dx_trail) <= rt - d and \ abs(dy_trail) <= C.W / 2.0: print("test1: Collision") else: print("test1: No collision") # test 2 cx = x + deltal * math.cos(yaw) cy = y + deltal * math.sin(yaw) xo = ox - cx yo = oy - cy dx_car = xo * math.cos(yaw) + yo * math.sin(yaw) dy_car = -xo * math.sin(yaw) + yo * math.cos(yaw) if abs(dx_car) <= rc - d and \ abs(dy_car) <= C.W / 2.0: print("test2: Collision") else: print("test2: No collision") theta = np.linspace(0, 2 * np.pi, 200) x1 = ctx + np.cos(theta) * rt y1 = cty + np.sin(theta) * rt x2 = cx + np.cos(theta) * rc y2 = cy + np.sin(theta) * rc plt.plot(x1, y1, 'b') plt.plot(x2, y2, 'g') plt.plot(ox, oy, 'sr') plt.plot([-rc, -rc, rc, rc, -rc], [C.W / 2, -C.W / 2, -C.W / 2, C.W / 2, C.W / 2]) plt.plot([-rt, -rt, rt, rt, -rt], [C.W / 2, -C.W / 2, -C.W / 2, C.W / 2, C.W / 2]) plt.plot(dx_car, dy_car, 'sr') plt.plot(dx_trail, dy_trail, 'sg') def main(): print("start!") sx, sy = 70, 40 # [m] syaw0 = np.deg2rad(0.0) syawt = np.deg2rad(0.0) gx, gy = 10, 130 # [m] gyaw0 = np.deg2rad(90.0) gyawt = np.deg2rad(90.0) ox, oy = design_obstacles() plt.plot(ox, oy, 'sk') draw_model(sx, sy, syaw0, syawt, 0.0) draw_model(gx, gy, gyaw0, gyawt, 0.0) # test(sx, sy, syaw0, syawt, 3.5, 32) plt.xlabel('横坐标/米', fontdict={'family': 'SimSun', 'size': 18}) plt.ylabel('纵坐标/米', fontdict={'family': 'SimSun', 'size': 18}) plt.axis("equal") plt.show() # 保存初始图像 plt.savefig('frame_000.png') frames = ['frame_000.png'] oox, ooy = ox[:], oy[:] t0 = time.time() path = hybrid_astar_planning(sx, sy, syaw0, syawt, gx, gy, gyaw0, gyawt, oox, ooy, C.XY_RESO, C.YAW_RESO) t1 = time.time() print("running T: ", t1 - t0) x = path.x y = path.y yaw = path.yaw yawt = path.yawt direction = path.direction plt.pause(10) # 创建动图 with imageio.get_writer('gif/hybrid_astar_4.gif', mode='I') as writer: # 输出牵引车和挂车的横摆角 vehicle_yaws = [] trailer_yaws = [] HINGE_x = [] HINGE_y = [] rear_x = [] rear_y = [] wingtip_l_x = [] wingtip_l_y = [] wingtip_r_x = [] wingtip_r_y = [] hinge_angle = [] # 添加代码记录路径规划过程中的牵引车和挂车的横摆角 for k in range(len(x)): plt.cla() plt.plot(ox, oy, "sk") plt.xlabel('横坐标/米', fontdict={'family': 'SimSun', 'size': 18}) plt.ylabel('纵坐标/米', fontdict={'family': 'SimSun', 'size': 18}) plt.plot(x, y, linewidth=1.5, color='r') # 记录牵引车和飞机的横摆角 current_vehicle_yaw = yaw[k] current_trailer_yaw = yawt[k] vehicle_yaws.append(current_vehicle_yaw) trailer_yaws.append(current_trailer_yaw) hinge_angle.append(current_vehicle_yaw - current_trailer_yaw) hinge_x = x[k] + C.HINGE_DISTANCE * math.cos(yaw[k]) hinge_y = y[k] + C.HINGE_DISTANCE * math.sin(yaw[k]) rear_x.append(x[k]) rear_y.append(y[k]) HINGE_x.append(hinge_x) HINGE_y.append(hinge_y) wingtip_l_current_x = np.array(-C.HINGE_DISTANCE_RW) wingtip_l_current_y = np.array(C.TRAILER_VERTICAL_LENGTH / 2) wingtip_r_current_x = np.array(-C.HINGE_DISTANCE_RW) wingtip_r_current_y = np.array(-C.TRAILER_VERTICAL_LENGTH / 2) final_wingtip_l_current_x = wingtip_l_current_x * math.cos(yawt[k]) - \ math.sin(yawt[k])*wingtip_l_current_y + hinge_x final_wingtip_l_current_y = wingtip_l_current_x * math.sin(yawt[k]) + \ math.cos(yawt[k])*wingtip_l_current_y + hinge_y final_wingtip_r_current_x = wingtip_r_current_x * math.cos(yawt[k]) - \ math.sin(yawt[k]) * wingtip_r_current_y + hinge_x final_wingtip_r_current_y = wingtip_r_current_x * math.sin(yawt[k]) + \ math.cos(yawt[k]) * wingtip_r_current_y + hinge_y wingtip_l_x.append(final_wingtip_l_current_x) wingtip_l_y.append(final_wingtip_l_current_y) wingtip_r_x.append(final_wingtip_r_current_x) wingtip_r_y.append(final_wingtip_r_current_y) if k < len(x) - 2: dy = (yaw[k + 1] - yaw[k]) / C.MOVE_STEP steer = math.atan(C.WB * dy / direction[k]) else: steer = 0.0 draw_model(gx, gy, gyaw0, gyawt, 0.0, 'gray') draw_model(x[k], y[k], yaw[k], yawt[k], steer) plt.axis("equal") # 保存当前帧 frame_name = f'frame_{k + 1:03d}.png' plt.savefig(frame_name) frames.append(frame_name) # 将当前帧添加到GIF image = imageio.imread(frame_name) writer.append_data(image) plt.pause(0.0001) # 在路径规划完成后,输出牵引车和挂车的横摆角 time_steps = list(range(len(vehicle_yaws))) data = { 'rear_x': rear_x, 'rear_y': rear_y, 'vehicle_yaws': vehicle_yaws, 'trailer_yaws': trailer_yaws, 'HINGE_x': HINGE_x, 'HINGE_y': HINGE_y, 'hinge_angle': hinge_angle, 'wingtip_l_x': wingtip_l_x, 'wingtip_l_y': wingtip_l_y, 'wingtip_r_x': wingtip_r_x, 'wingtip_r_y': wingtip_r_y, } # 创建DataFrame df = pd.DataFrame(data) # 保存到Excel df.to_excel('vehicle_data3.xlsx', index=False) # 绘制牵引车横摆角 plt.figure(figsize=(10, 6)) plt.plot(time_steps, [np.rad2deg(vy) for vy in vehicle_yaws], 'b-', label='牵引车横摆角') plt.xlabel('时间步长', fontdict={'family': 'SimSun', 'size': 18}) plt.ylabel('横摆角/°', fontdict={'family': 'SimSun', 'size': 18}) plt.xlim(0, 900) plt.xticks(np.arange(0, 900, 100)) plt.legend(labels=['牵引车横摆角'], prop={'family': 'SimSun', 'size': 16}) plt.savefig('TLTV-yaw3.png') # 绘制挂车横摆角 plt.figure(figsize=(10, 6)) plt.plot(time_steps, [np.rad2deg(tr_y) for tr_y in trailer_yaws], 'r--', label='飞机横摆角') plt.xlabel('时间步长', fontdict={'family': 'SimSun', 'size': 18}) plt.ylabel('横摆角/°', fontdict={'family': 'SimSun', 'size': 18}) plt.xlim(0, 900) plt.xticks(np.arange(0, 900, 100)) plt.legend(labels=['飞机横摆角'], prop={'family': 'SimSun', 'size': 16}) # 保存图表为图片文件 plt.savefig('aircraft-yaw3.png') plt.figure(figsize=(10, 6)) plt.cla() plt.plot(time_steps, [np.rad2deg(hinge) for hinge in hinge_angle], 'k-', label='铰接角' ) plt.xlim(0, 900) plt.xticks(np.arange(0, 900, 100)) plt.ylim(-70, 70) plt.xlabel('时间步长', fontdict={'family': 'SimSun', 'size': 18}) plt.ylabel('铰接角/°', fontdict={'family': 'SimSun', 'size': 18}) plt.savefig('articulated-angle3.png') plt.figure(figsize=(10, 6)) plt.cla() plt.plot(ox, oy, "sk") plt.xlabel('横坐标/米', fontdict={'family': 'SimSun', 'size': 18}) plt.ylabel('纵坐标/米', fontdict={'family': 'SimSun', 'size': 18}) plt.plot(wingtip_l_x, wingtip_l_y, linewidth=1.5, color='r') plt.savefig('wingtip_left3.png') plt.figure(figsize=(10, 6)) plt.cla() plt.plot(ox, oy, "sk") plt.xlabel('横坐标/米', fontdict={'family': 'SimSun', 'size': 18}) plt.ylabel('纵坐标/米', fontdict={'family': 'SimSun', 'size': 18}) plt.plot(wingtip_r_x, wingtip_r_y, linewidth=1.5, color='r') plt.savefig('wingtip_right3.png') # 清理临时文件 #for frame in frames: # os.remove(frame) plt.show() print("Done") if __name__ == '__main__': main() 地图构建部分在哪
最新发布
07-08
评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值