二维无人机非线性MPC优化问题及绘图标记旋转需求

我正在模拟二维月球表面的无人机，该无人机可沿机体Z轴施加推力，机体角度可在-90°至+90°范围内调整。目前遇到两个问题：

1. MPC函数输出的首个Y方向加速度为负值且超过设定的月球重力加速度accel_g（1.635m/s²），导致无人机快速抵消初始速度。但按照机体角度约束，推力不应能降低垂直速度，垂直速度应仅受月球重力影响，找不到代码问题。
2. 能否对绘图的十字标记进行旋转，使其能够体现姿态变化？

function run_mpc(initial_position, initial_velocity, initial_angle)

model = Model(Ipopt.Optimizer)

Δt = 0.1
num_time_steps = 20 # Change this -> Affects Optimization
max_acceleration_Thr = 3 # Max Thrust / Mass
max_pitch_angle = 90
accel_g = 1.635 # 1/6 of Earth G
des_pos = [-1,0]

@variables model begin
    position[1:2, 1:num_time_steps]
    velocity[1:2, 1:num_time_steps]
    acceleration[1:2, 1:num_time_steps]
    -max_pitch_angle <= angle[1:num_time_steps] <= max_pitch_angle
    0 <= accel_Thr[1:num_time_steps] <= max_acceleration_Thr
end

# Dynamics constraints
@NLconstraint(model, [i=2:num_time_steps, j=[1]], acceleration[j, i] == accel_Thr[i-1]*sind(angle[i-1]))

@NLconstraint(model, [i=2:num_time_steps, j=[2]], acceleration[j, i] == (accel_Thr[i-1]*cosd(angle[i-1]))-accel_g)

@NLconstraint(model, [i=2:num_time_steps, j=1:2],
            velocity[j, i] == velocity[j, i - 1] + (acceleration[j, i - 1]) * Δt)
@NLconstraint(model, [i=2:num_time_steps, j=1:2],
            position[j, i] == position[j, i - 1] + velocity[j, i - 1] * Δt)


# Cost function: minimize final position and final velocity
# For Moving to [-2,0] with min. vertical velocity,
# sum(([-2,0]-position[:, end]).^2)+ sum(velocity[[2], end].^2)
@NLobjective(model, Min, 
    100 * sum((des_pos[i]-position[i, num_time_steps])^2 for i in 1:2)+ sum(velocity[i, num_time_steps]^2 for i in 1:2))

# Initial conditions:
@NLconstraint(model, [i=1:2], position[i, 1] == initial_position[i])
@NLconstraint(model, [i=1:2], velocity[i, 1] == initial_velocity[i])
@NLconstraint(model, angle[1] == initial_angle)

optimize!(model)
return value.(position), value.(velocity), value.(acceleration), value.(angle[2:end])
end;

begin
# The robot's starting position and velocity
q = [1.0, 0.0]
v = [-2.0, 2.0]
ang = 45
Δt = 0.1

# Recording Position, Acceleration, Attitude, Planned Positions
qs_x = []
qs_y = []
as_x = []
as_y = []
angs = []
q_plans = []
u_plans = []

anim = @animate for i in 1:90 # This determies the number of MPC to be run
    # Plot the current position & Attitude
    plot(label = "Drone",[q[1]], [q[2]], marker=(:rect, 10), xlim=(-2, 2), ylim=(-2, 2))
    plot!(label = "Body Axis",[q[1]], [q[2]], marker=(:cross, 18, :grey))
    push!(qs_x,q[1])
    push!(qs_y,q[2])
    
    # Run the MPC control optimization
    q_plan, v_plan, u_plan, ang_plan = run_mpc(q, v, ang)
    
    # Draw the planned future states from the MPC optimization
    plot!(label = "Opt. Path", q_plan[1, :], q_plan[2, :], linewidth=5, arrow=true, c=:orange)
    # Draw the planned acceleration
    plot!(label = "Opt. Accel",u_plan[1, 1:2], u_plan[2, 1:2], linewidth=3, arrow=true, c=:red)
    
    # Save Acceleration & Angle Data to csv
    u = u_plan[:, 1]
    push!(as_x, u[1])
    push!(as_y, u[2])
    push!(angs, ang)
    push!(u_plans, u_plan)
    
    # Apply the planned acceleration&Attitude and simulate one step in time
    global ang = ang_plan[1]
    global v += u * Δt
    global q += v * Δt
end
gif(anim, "~/Downloads/NLmpc_angle.gif", fps=60)
end

1. Y方向加速度异常的修复 #

问题根源 #

代码中动力学约束的索引逻辑错位：

• 原约束中，acceleration[j,i]依赖accel_Thr[i-1]和angle[i-1]，但速度更新时却使用acceleration[j,i-1]，导致控制输入和加速度的对应关系断裂，MPC可以计算出不符合物理规则的加速度值。
• 理论上Y方向加速度最小值应为-accel_g（推力为0时），但索引错位让约束失效，出现了更负的加速度。

修复代码 #

将动力学约束的索引统一，让每一步的控制输入直接对应下一步的加速度：

# 修正后的动力学约束
@NLconstraint(model, [i=1:num_time_steps-1, j=[1]], acceleration[j, i+1] == accel_Thr[i]*sind(angle[i]))

@NLconstraint(model, [i=1:num_time_steps-1, j=[2]], acceleration[j, i+1] == (accel_Thr[i]*cosd(angle[i]))-accel_g)

@NLconstraint(model, [i=2:num_time_steps, j=1:2],
            velocity[j, i] == velocity[j, i - 1] + (acceleration[j, i]) * Δt)
@NLconstraint(model, [i=2:num_time_steps, j=1:2],
            position[j, i] == position[j, i - 1] + velocity[j, i - 1] * Δt)

说明 #

现在accel_Thr[i]和angle[i]产生的加速度acceleration[j,i+1]，直接用于更新velocity[j,i+1]，逻辑链完全匹配，Y方向加速度最小值被限制为-accel_g，符合物理规则。

2. 旋转十字标记体现姿态变化 #

Plots.jl的默认十字标记无法直接旋转，可通过绘制旋转线段模拟带姿态的机体轴：
替换原代码中绘制十字标记的行：

# 替换原十字标记代码，绘制旋转后的机体轴
axis_length = 0.2
ang_rad = deg2rad(ang)
# 计算旋转后的轴端点
x1 = q[1] + axis_length * sind(ang_rad)
y1 = q[2] + axis_length * cosd(ang_rad)
x2 = q[1] - axis_length * sind(ang_rad)
y2 = q[2] - axis_length * cosd(ang_rad)
x3 = q[1] + axis_length * cosd(ang_rad)
y3 = q[2] - axis_length * sind(ang_rad)
x4 = q[1] - axis_length * cosd(ang_rad)
y4 = q[2] + axis_length * sind(ang_rad)
# 绘制十字轴
plot!(label = "Body Axis", [q[1], x1], [q[2], y1], linewidth=2, c=:grey)
plot!(label = "", [q[1], x2], [q[2], y2], linewidth=2, c=:grey)
plot!(label = "", [q[1], x3], [q[2], y3], linewidth=2, c=:grey)
plot!(label = "", [q[1], x4], [q[2], y4], linewidth=2, c=:grey)

说明 #

• 通过三角函数计算旋转后的线段端点，模拟十字轴随机体角度旋转。
• axis_length可调整轴的显示长度，匹配需求。

内容的提问来源于stack exchange，提问作者모구리모구리