我正在尝试使用 pyomo 进行非线性规划轨迹优化。
我使用简单的无人机模型来优化从初始位置到最终位置的最短旅行时间轨迹。我想添加一个由一些路径点和时间描述的移动障碍物。
waypoints = np.array([
[100, 100, 30, 0], # Start at time = 0, structure [x,y,z,time]
[75, 75, 15, 3], # Reach this point by time = 3 seconds
[50, 50, 0, 5], # Reach this point by time = 5 seconds
[50, 50, 0, 12], # Stay until time = 12 seconds
[25, 25, 25, 17], # Move to this point by time = 17 seconds
[0, 0, 50, 20] # Final position by time = 20 seconds
])
请注意,我希望障碍物在着陆坐标(50,50,0)处停留相当长的一段时间,然后再再次移动到最终位置。
无人机的最终位置为着陆坐标(50,50,0)。我实现了避免碰撞约束(请参阅代码),但不知怎的,IPOPT 求解器说找到了最佳解决方案,即使违反了碰撞避免约束也是如此。无人机不断成功降落在障碍物上方。
这是代码的碰撞避免部分。我怀疑问题出在无人机和障碍物之间的时间映射和同步上,但我无法弄清楚问题出在哪里。我不知道我是否犯了逻辑错误。请帮助我找出我犯了哪个错误,导致了这种行为。
# Constants for example, make sure these are defined in your code context
safety_distance = 20 # safety distance in meters
T_obstacle = 20 # total time for obstacle to complete its path based on the last waypoint time
# Obstacle waypoints
waypoints = np.array([
[100, 100, 30, 0], # Start at time = 0
[75, 75, 15, 3], # Reach this point by time = 3 seconds
[50, 50, 0, 5], # Reach this point by time = 5 seconds
[50, 50, 0, 12], # Stay until time = 12 seconds
[25, 25, 25, 17], # Move to this point by time = 17 seconds
[0, 0, 50, 20] # Final position by time = 20 seconds
])
# Extract positions and corresponding times from waypoints
positions = waypoints[:, :3]
times = waypoints[:, 3]
# Create interpolation functions using specified times
obstacle_x = interp1d(times, positions[:, 0], kind='linear', fill_value='extrapolate')
obstacle_y = interp1d(times, positions[:, 1], kind='linear', fill_value='extrapolate')
obstacle_z = interp1d(times, positions[:, 2], kind='linear', fill_value='extrapolate')
def scaled_time(m, t):
return t * value(m.tf)
# Collision avoidance constraint using the time scaled by model.tf directly
def collision_avoidance_constraint(m, t):
obs_t_value = scaled_time(m, t)
distance_squared = ((m.x[t] - obstacle_x(obs_t_value))**2 +
(m.y[t] - obstacle_y(obs_t_value))**2 +
(m.z[t] - obstacle_z(obs_t_value))**2)
print(f"Time {t}, Scaled Time {obs_t_value}, Distance Squared {distance_squared}, Safety Distance Squared {safety_distance**2}")
return distance_squared >= safety_distance**2
model.collision_avoidance = Constraint(model.t, rule=collision_avoidance_constraint)
这是完整的代码。
from pyomo.environ import *
from pyomo.dae import *
import numpy as np
from scipy.interpolate import interp1d
import pandas as pd
# Constants
g = 9.81 # acceleration due to gravity in m/s^2
mass = 0.625 # mass in kg
Ix = Iy = Iz = 2.3e-3 # moment of inertia in kg*m^2
# Model setup
model = ConcreteModel()
model.t = ContinuousSet(bounds=(0, 1))
model.tf = Var(domain=NonNegativeReals, initialize=20)
# Position and velocity
model.x = Var(model.t, bounds=(0, 100))
model.y = Var(model.t, bounds=(0, 100))
model.z = Var(model.t, bounds=(0, 100))
model.vx = Var(model.t, bounds=(-5, 5))
model.vy = Var(model.t, bounds=(-5, 5))
model.vz = Var(model.t, bounds=(-5, 5))
# Angles and angular velocities
model.phi = Var(model.t, bounds=(-np.pi/2, np.pi/2))
model.theta = Var(model.t, bounds=(-np.pi/2, np.pi/2))
model.psi = Var(model.t, bounds=(-np.pi/2, np.pi/2))
model.p = Var(model.t, bounds=(-2, 2))
model.q = Var(model.t, bounds=(-2, 2))
model.r = Var(model.t, bounds=(-2, 2))
# Control inputs
model.U1 = Var(model.t, bounds=(-21, 21))
model.U2 = Var(model.t, bounds=(-0.67, 0.67))
model.U3 = Var(model.t, bounds=(-0.67, 0.67))
model.U4 = Var(model.t, bounds=(-0.11, 0.11))
# Derivative variables
model.x_dot = DerivativeVar(model.x, wrt=model.t)
model.y_dot = DerivativeVar(model.y, wrt=model.t)
model.z_dot = DerivativeVar(model.z, wrt=model.t)
model.vx_dot = DerivativeVar(model.vx, wrt=model.t)
model.vy_dot = DerivativeVar(model.vy, wrt=model.t)
model.vz_dot = DerivativeVar(model.vz, wrt=model.t)
model.phi_dot = DerivativeVar(model.phi, wrt=model.t)
model.theta_dot = DerivativeVar(model.theta, wrt=model.t)
model.psi_dot = DerivativeVar(model.psi, wrt=model.t)
model.p_dot = DerivativeVar(model.p, wrt=model.t)
model.q_dot = DerivativeVar(model.q, wrt=model.t)
model.r_dot = DerivativeVar(model.r, wrt=model.t)
# Differential equations
def _ode_x(m, t):
return m.x_dot[t] == m.tf * m.vx[t]
model.ode_x = Constraint(model.t, rule=_ode_x)
def _ode_y(m, t):
return m.y_dot[t] == m.tf * m.vy[t]
model.ode_y = Constraint(model.t, rule=_ode_y)
def _ode_z(m, t):
return m.z_dot[t] == m.tf * m.vz[t]
model.ode_z = Constraint(model.t, rule=_ode_z)
def _ode_vx(m, t):
return m.vx_dot[t] == m.tf * (cos(m.phi[t]) * sin(m.theta[t]) * cos(m.psi[t]) + sin(m.phi[t]) * sin(m.psi[t])) * m.U1[t] / mass
model.ode_vx = Constraint(model.t, rule=_ode_vx)
def _ode_vy(m, t):
return m.vy_dot[t] == m.tf * (cos(m.phi[t]) * sin(m.theta[t]) * sin(m.psi[t]) - sin(m.phi[t]) * cos(m.psi[t])) * m.U1[t] / mass
model.ode_vy = Constraint(model.t, rule=_ode_vy)
def _ode_vz(m, t):
return m.vz_dot[t] == m.tf * (-g + (cos(m.phi[t]) * cos(m.theta[t])) * m.U1[t] / mass)
model.ode_vz = Constraint(model.t, rule=_ode_vz)
def _ode_phi(m, t):
return m.phi_dot[t] == m.tf * m.p[t]
model.ode_phi = Constraint(model.t, rule=_ode_phi)
def _ode_theta(m, t):
return m.theta_dot[t] == m.tf * m.q[t]
model.ode_theta = Constraint(model.t, rule=_ode_theta)
def _ode_psi(m, t):
return m.psi_dot[t] == m.tf * m.r[t]
model.ode_psi = Constraint(model.t, rule=_ode_psi)
def _ode_p(m, t):
return m.p_dot[t] == m.tf * (m.U2[t] + m.q[t] * m.r[t] * (Iy - Iz)) / Ix
model.ode_p = Constraint(model.t, rule=_ode_p)
def _ode_q(m, t):
return m.q_dot[t] == m.tf * (m.U3[t] + m.p[t] * m.r[t] * (Iz - Ix)) / Iy
model.ode_q = Constraint(model.t, rule=_ode_q)
def _ode_r(m, t):
return m.r_dot[t] == m.tf * (m.U4[t] + m.p[t] * m.q[t] * (Ix - Iy)) / Iz
model.ode_r = Constraint(model.t, rule=_ode_r)
# Load data from CSV
data = pd.read_csv('simulation_results.csv')
# Set up interpolation functions for each variable
interpolators = {
var: interp1d(data['time'], data[var], kind='cubic', fill_value="extrapolate")
for var in ['x', 'y', 'z', 'vx', 'vy', 'vz', 'phi', 'theta', 'psi', 'p', 'q', 'r', 'U1', 'U2', 'U3', 'U4']
}
# The actual time span of your data
actual_start_time = data['time'].min()
actual_end_time = data['time'].max()
# Set interpolated values as initial guesses
for t in model.t: # Iterate through the ContinuousSet
# Scale the time correctly to match the data timeline
scaled_time = actual_start_time + t * (actual_end_time - actual_start_time)
# Assign interpolated values to the model variables
model.x[t].set_value(interpolators['x'](scaled_time).item()) # Convert to scalar with .item()
model.y[t].set_value(interpolators['y'](scaled_time).item())
model.z[t].set_value(interpolators['z'](scaled_time).item())
model.vx[t].set_value(interpolators['vx'](scaled_time).item())
model.vy[t].set_value(interpolators['vy'](scaled_time).item())
model.vz[t].set_value(interpolators['vz'](scaled_time).item())
model.phi[t].set_value(interpolators['phi'](scaled_time).item())
model.theta[t].set_value(interpolators['theta'](scaled_time).item())
model.psi[t].set_value(interpolators['psi'](scaled_time).item())
model.p[t].set_value(interpolators['p'](scaled_time).item())
model.q[t].set_value(interpolators['q'](scaled_time).item())
model.r[t].set_value(interpolators['r'](scaled_time).item())
model.U1[t].set_value(interpolators['U1'](scaled_time).item())
model.U2[t].set_value(interpolators['U2'](scaled_time).item())
model.U3[t].set_value(interpolators['U3'](scaled_time).item())
model.U4[t].set_value(interpolators['U4'](scaled_time).item())
# Initial and final conditions
model.x[0].fix(30)
model.y[0].fix(30)
model.z[0].fix(20)
model.vx[0].fix(0)
model.vy[0].fix(0)
model.vz[0].fix(0)
model.phi[0].fix(0)
model.theta[0].fix(0)
model.psi[0].fix(0)
model.p[0].fix(0)
model.q[0].fix(0)
model.r[0].fix(0)
model.x[1].fix(50)
model.y[1].fix(50)
model.z[1].fix(0)
model.vx[1].fix(0)
model.vy[1].fix(0)
model.vz[1].fix(0)
model.phi[1].fix(0)
model.theta[1].fix(0)
# Constants for example, make sure these are defined in your code context
safety_distance = 20 # safety distance in meters
T_obstacle = 20 # total time for obstacle to complete its path based on the last waypoint time
# Obstacle waypoints
waypoints = np.array([
[100, 100, 30, 0], # Start at time = 0
[75, 75, 15, 3], # Reach this point by time = 3 seconds
[50, 50, 0, 5], # Reach this point by time = 5 seconds
[50, 50, 0, 12], # Stay until time = 12 seconds
[25, 25, 25, 17], # Move to this point by time = 17 seconds
[0, 0, 50, 20] # Final position by time = 20 seconds
])
# Extract positions and corresponding times from waypoints
positions = waypoints[:, :3]
times = waypoints[:, 3]
# Create interpolation functions using specified times
obstacle_x = interp1d(times, positions[:, 0], kind='linear', fill_value='extrapolate')
obstacle_y = interp1d(times, positions[:, 1], kind='linear', fill_value='extrapolate')
obstacle_z = interp1d(times, positions[:, 2], kind='linear', fill_value='extrapolate')
def scaled_time(m, t):
return t * value(m.tf)
# Collision avoidance constraint using the time scaled by model.tf directly
def collision_avoidance_constraint(m, t):
obs_t_value = scaled_time(m, t)
distance_squared = ((m.x[t] - obstacle_x(obs_t_value))**2 +
(m.y[t] - obstacle_y(obs_t_value))**2 +
(m.z[t] - obstacle_z(obs_t_value))**2)
print(f"Time {t}, Scaled Time {obs_t_value}, Distance Squared {distance_squared}, Safety Distance Squared {safety_distance**2}")
return distance_squared >= safety_distance**2
model.collision_avoidance = Constraint(model.t, rule=collision_avoidance_constraint)
# Objective function to minimize energy or time
def objective_rule(m):
return m.tf
model.objective = Objective(rule=objective_rule, sense=minimize)
# Apply discretization
discretizer = TransformationFactory('dae.collocation')
discretizer.apply_to(model, nfe=800, ncp=10, wrt=model.t, scheme='LAGRANGE-LEGENDRE')
solver = SolverFactory('ipopt')
solver.options.update({
'halt_on_ampl_error': 'yes',
'tol': 1e-6,
'max_iter': 2000,
'print_level': 5, # High verbosity for detailed solver output
#'linear_solver': 'ma27' # Recommended for larger-scale problems if available
})
result = solver.solve(model, tee=True) # Execute the solver with console output
#critical_points = [t for t in sorted(model.t) if value(model.x[t]) == 50 and value(model.y[t]) == 50 and value(model.z[t]) <= 0.5]
for t in model.t:
scaled_time = value(model.tf * t)
ox = obstacle_x(scaled_time)
oy = obstacle_y(scaled_time)
oz = obstacle_z(scaled_time)
distance_squared = (value(model.x[t]) - ox)**2 + (value(model.y[t]) - oy)**2 + (value(model.z[t]) - oz)**2
print(f"Critical Time {scaled_time:.2f}: UAV Position ({value(model.x[t]):.2f}, {value(model.y[t]):.2f}, {value(model.z[t]):.2f})")
print(f"Obstacle Position ({ox:.2f}, {oy:.2f}, {oz:.2f})")
print(f"Distance squared to obstacle: {distance_squared:.2f} (Constraint satisfied: {distance_squared >= safety_distance**2})")
# Check solver status and results
if (result.solver.status == SolverStatus.ok) and (result.solver.termination_condition == TerminationCondition.optimal):
print("Solver found an optimal solution.")
elif result.solver.termination_condition == TerminationCondition.infeasible:
print("No feasible solution found.")
else:
print("Solver status:", result.solver.status)
print("Termination condition:", result.solver.termination_condition)
# Dictionary to hold the data
results_data = {
'time': [],
'x': [],
'y': [],
'z': [],
'vx': [],
'vy': [],
'vz': [],
'phi': [],
'theta': [],
'psi': [],
'p': [],
'q': [],
'r': [],
'U1': [],
'U2': [],
'U3': [],
'U4': []
}
# Fill the dictionary with values from the model
for t in model.t:
results_data['time'].append(t)
results_data['x'].append(value(model.x[t]))
results_data['y'].append(value(model.y[t]))
results_data['z'].append(value(model.z[t]))
results_data['vx'].append(value(model.vx[t]))
results_data['vy'].append(value(model.vy[t]))
results_data['vz'].append(value(model.vz[t]))
results_data['phi'].append(value(model.phi[t]))
results_data['theta'].append(value(model.theta[t]))
results_data['psi'].append(value(model.psi[t]))
results_data['p'].append(value(model.p[t]))
results_data['q'].append(value(model.q[t]))
results_data['r'].append(value(model.r[t]))
results_data['U1'].append(value(model.U1[t]))
results_data['U2'].append(value(model.U2[t]))
results_data['U3'].append(value(model.U3[t]))
results_data['U4'].append(value(model.U4[t]))
# Create a DataFrame from the dictionary
results_df = pd.DataFrame(results_data)
# Save to CSV (optional)
results_df.to_csv('C:/Users/dtdav/anaconda3/envs/thesis/project/simulation_results.csv', index=False)
import plotly.graph_objs as go
from scipy.interpolate import interp1d
from plotly.subplots import make_subplots
# Function to extract data from Pyomo model
def extract_data(var, model, obstacle_timeset):
return np.array([value(var[t]) for t in obstacle_timeset])
# Extracting time and position data
time = np.array([t * value(model.tf) for t in model.t])
x = extract_data(model.x, model, model.t)
y = extract_data(model.y, model, model.t)
z = extract_data(model.z, model, model.t)
phi = extract_data(model.phi, model, model.t)
theta = extract_data(model.theta, model, model.t)
psi = extract_data(model.psi, model, model.t)
# Extracting time and position data for UAV
total_duration_uav = value(model.tf) # Total duration of UAV flight in seconds
fps = 30 # Frames per second
total_frames_uav = int(fps * total_duration_uav) # Make sure this is an integer
uniform_time_uav = np.linspace(0, total_duration_uav, total_frames_uav)
# Interpolating data for uniform time steps for UAV
interp_x = interp1d(time, x, kind='linear')(uniform_time_uav)
interp_y = interp1d(time, y, kind='linear')(uniform_time_uav)
interp_z = interp1d(time, z, kind='linear')(uniform_time_uav)
interp_phi = interp1d(time, phi, kind='linear')(uniform_time_uav)
interp_theta = interp1d(time, theta, kind='linear')(uniform_time_uav)
interp_psi = interp1d(time, psi, kind='linear')(uniform_time_uav)
# For the obstacle
total_frames_obstacle = fps * T_obstacle # Frames based on fixed obstacle duration
uniform_time_obstacle = np.linspace(0, T_obstacle, total_frames_obstacle)
# Interpolating obstacle data for uniform time steps
interp_obs_x = obstacle_x(uniform_time_obstacle)
interp_obs_y = obstacle_y(uniform_time_obstacle)
interp_obs_z = obstacle_z(uniform_time_obstacle)
def rotation_matrix(phi, theta, psi):
# Calculate rotation matrix based on Euler angles
Rz = np.array([
[np.cos(psi), -np.sin(psi), 0],
[np.sin(psi), np.cos(psi), 0],
[0, 0, 1]
])
Ry = np.array([
[np.cos(theta), 0, np.sin(theta)],
[0, 1, 0],
[-np.sin(theta), 0, np.cos(theta)]
])
Rx = np.array([
[1, 0, 0],
[0, np.cos(phi), -np.sin(phi)],
[0, np.sin(phi), np.cos(phi)]
])
return Rz @ Ry @ Rx
def update_uav_shape(x, y, z, phi, theta, psi):
uav_size = 10 # Define the size of the UAV for visualization
corners = np.array([
[-uav_size/2, -uav_size/2, 0], # Back-left
[uav_size/2, -uav_size/2, 0], # Back-right
[uav_size/2, uav_size/2, 0], # Front-right
[-uav_size/2, uav_size/2, 0] # Front-left
])
center_of_mass = np.array([0, 0, 0])
front_nose_line = np.array([[0, 0, 0], [uav_size/2, 0, 0]]) # Line from center to front
R = rotation_matrix(phi, theta, psi)
transformed_corners = np.array([R @ corner + np.array([x, y, z]) for corner in corners])
transformed_center_of_mass = R @ center_of_mass + np.array([x, y, z])
transformed_front_nose_line = np.array([R @ point + np.array([x, y, z]) for point in front_nose_line])
return transformed_corners, transformed_center_of_mass, transformed_front_nose_line
# Create Plotly figure with 3D subplot
fig = make_subplots(rows=1, cols=1, specs=[[{'type': 'scatter3d'}]])
# Compute initial UAV shape
initial_corners, initial_center_of_mass, initial_front_nose_line = update_uav_shape(
interp_x[0], interp_y[0], interp_z[0], interp_phi[0], interp_theta[0], interp_psi[0]
)
# Initialize the traces for UAV and Obstacle
fig.add_trace(go.Scatter3d(
x=[],
y=[],
z=[],
mode='lines',
line=dict(color='blue', width=2),
name='UAV Path'
))
fig.add_trace(go.Scatter3d(
x=[interp_x[0]],
y=[interp_y[0]],
z=[interp_z[0]],
mode='markers',
marker=dict(size=5, color='blue'),
name='UAV Position'
))
fig.add_trace(go.Mesh3d(
x=initial_corners[:, 0],
y=initial_corners[:, 1],
z=initial_corners[:, 2],
color='blue',
opacity=0.5,
name='UAV Shape'
))
fig.add_trace(go.Scatter3d(
x=initial_front_nose_line[:, 0],
y=initial_front_nose_line[:, 1],
z=initial_front_nose_line[:, 2],
mode='lines',
line=dict(color='yellow', width=4),
name='Front Nose'
))
fig.add_trace(go.Scatter3d(
x=[],
y=[],
z=[],
mode='lines',
line=dict(color='red', width=2),
name='Obstacle Path'
))
fig.add_trace(go.Scatter3d(
x=[interp_obs_x[0]],
y=[interp_obs_y[0]],
z=[interp_obs_z[0]],
mode='markers',
marker=dict(size=5, color='red'),
name='Obstacle Position'
))
fig.add_trace(go.Scatter3d(x=[], y=[], z=[], mode='lines', line=dict(color='red', width=2), name='Obstacle Path'))
fig.add_trace(go.Scatter3d(x=[interp_obs_x[0]], y=[interp_obs_y[0]], z=[interp_obs_z[0]], mode='markers', marker=dict(size=5, color='red'), name='Obstacle Position'))
# Create frames to animate the markers and dynamically draw the trace paths
frames = []
for k in range(1, total_frames_uav):
corners, center_of_mass, front_nose_line = update_uav_shape(interp_x[k], interp_y[k], interp_z[k], interp_phi[k], interp_theta[k], interp_psi[k])
frame_data = [
go.Mesh3d(x=corners[:, 0], y=corners[:, 1], z=corners[:, 2], color='blue', opacity=0.5),
go.Scatter3d(x=interp_x[:k+1], y=interp_y[:k+1], z=interp_z[:k+1], mode='lines', line=dict(color='blue', width=2)),
go.Scatter3d(x=[center_of_mass[0]], y=[center_of_mass[1]], z=[center_of_mass[2]], mode='markers', marker=dict(size=5, color='blue')),
go.Scatter3d(x=front_nose_line[:, 0], y=front_nose_line[:, 1], z=front_nose_line[:, 2], mode='lines', line=dict(color='yellow', width=4))
]
if k < total_frames_obstacle:
frame_data.extend([
go.Scatter3d(x=interp_obs_x[:k+1], y=interp_obs_y[:k+1], z=interp_obs_z[:k+1], mode='lines', line=dict(color='red', width=2)),
go.Scatter3d(x=[interp_obs_x[k]], y=[interp_obs_y[k]], z=[interp_obs_z[k]], mode='markers', marker=dict(color='red', size=5))
])
frames.append(go.Frame(data=frame_data, name=f'frame{k}'))
fig.frames = frames
# Add play and pause buttons
fig.update_layout(
updatemenus=[{
'type': 'buttons',
'buttons': [
{'label': 'Play', 'method': 'animate', 'args': [None, {'frame': {'duration': 1000 / fps, 'redraw': True}, 'fromcurrent': True, 'transition': {'duration': 0, 'easing': 'linear'}}]},
{'label': 'Pause', 'method': 'animate', 'args': [[None], {'frame': {'duration': 0, 'redraw': False}, 'mode': 'immediate', 'transition': {'duration': 0}}]}
],
'direction': 'left',
'pad': {'r': 10, 't': 87},
'showactive': False,
'x': 0.1,
'xanchor': 'right',
'y': 0,
'yanchor': 'top'
}]
)
# Set axis limits and display the figure
fig.update_layout(scene=dict(
xaxis=dict(range=[0, 100], autorange=False),
yaxis=dict(range=[0, 100], autorange=False),
zaxis=dict(range=[0, 100], autorange=False),
aspectratio=dict(x=1, y=1, z=1)
))
fig.show()
这是我尝试过的。我期望无人机“等待”或至少寻找其他机动,同时等待障碍物远离着陆坐标(50,50,0)然后着陆。
警告:我没有阅读您的所有代码或深入研究。
建议:如果您能以某种方式将问题缩减为最小的可重现示例,您可能会在这个/未来的问题上获得更好的帮助。这个问题有点像你在寻找研究合作伙伴。 ;)
看来你正在用你的时间尺度做一些奇怪的事情,我不确定为什么 1 时间尺度总体上不是正确的答案,但在这个“辅助函数”中:
def scaled_time(m, t):
return t * value(m.tf)
您在碰撞约束中使用您要求的m.tf
的
值。我认为你不想这样做。当您这样做时,您将在任何求解活动之前检索变量的当前值。含义:它只是将初始化值替换为 constant (如果提供的话,你就是这么做的)。如果您希望该变量成为优化中的变量,则您应该never要求
value()
。这是为了后期解决。
如果您习惯(您应该)用小/微小数据打印模型,那么如果您
m.pprint()
或 m.display()
,您应该会看到这个问题