The final position of Nonlinear optimization of trajectory is not on the desired position

Specific Domains Optimization (Mathematical)
, , ,
1

NLmpc_angle
The starting position is set as [1.0, 0.0], and the desired position is [-1,0]. However, the object stops short before touching the desired position. The x coordinate gets close enough, but the issue is with the y coordinate. Why is this happening? Any issue with the @NLobjective?

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
		-max_acceleration_Thr <= 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

	qs_x = []
	qs_y = []
	as_x = []
	as_y = []
	angs = []
	
	anim = @animate for i in 1:100 # This determies the number of MPC to be run
	    # Plot the current position
		# Attitude of the vehicle is not considered
	    plot([q[1]], [q[2]], marker=(:rect, 10), xlim=(-2, 2), ylim=(-2, 2))
		push!(qs_x,q[1])
		push!(qs_y,q[2])
		plot!([q[1]], [q[2]], marker=(:cross, 18, :grey))
	    
	    # 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!(q_plan[1, :], q_plan[2, :], linewidth=5, arrow=true, c=:orange)
	    
	    # Save Acceleration & Angle Data to csv
	    u = u_plan[:, 1]
		global ang = ang_plan[1]
		push!(as_x, u[1])
		push!(as_y, u[2])
		push!(angs, ang_plan[1])
		
		# Apply the planned acceleration and simulate one step in time
	    global v += u * Δt
	    global q += v * Δt
	end
	gif(anim, "~/Downloads/NLmpc_angle.gif", fps=60)
end
2

You have a mis-match between your constraints and the simulation. Here you use the velocity in time i-1 to update the position.

But here you use the updated velocity.

global v += u * Δt
global q += v * Δt

You can see this because the orange line (the optimization) looks good, but the blue box doesn’t follow the orange line.

3

I’m not sure if I understood your comment correctly.
Let me state what I think is happening.

  1. Optimization is done with the information of current position.
  2. The function returns optimized position, velocity, acceleration, and angle.
  3. The first value of planned(optimized) acceleration is used to update the velocity and position of the blue box, updating the “current position.”
  4. goto #1 with this new “current position.”

If I only run the optimization once, and let the blue box follow that optimized path, it would follow the yellow curve exactly, but since I’m running optimization after every frame and updating the velocity & position with the optimized acceleration at that frame, the blue box wouldn’t follow the yellow path exactly?

4

Also, this may be irrelevant to this question, but I noticed that if I change the lower bound of acceleration of thrust to 0, which it should be. I noticed that the acceleration in y-axis goes beyond the its maximum value. The acceleration in y-axis should not exceed the accel_g, yet it does… and now, blue box reaches the desired location.

0 <= accel_Thr[1:num_time_steps] <= max_acceleration_Thr

NLmpc_angle

5

In the optimization, the position is updated using the velocity in the previous time-step. In the simulation, the position is updated in the current time-step. So your simulation is not following the trajectory of the optimization.

Try swapping the order of the updates:

global q += v * Δt
global v += u * Δt
6

MPC runs in every frame using the “current” position and velocity; the drone’s velocity & position are updated using the optimized acceleration from MPC. But the issue I’m having is that the acceleration in y-axis exceeds lunar gravity.

image
image1828×1218 90.3 KB

I tried swapping the order, but unfortunately, it didn’t do much.

7

Try this:

using JuMP, Ipopt, Plots

function run_mpc(initial_position, initial_velocity, initial_angle)
    model = Model(Ipopt.Optimizer)
    set_silent(model)
    T, Δt = 20, 0.1
	desired_position = [-1, 0]
    @variables(model, begin
        position[1:2, 1:T]
        velocity[1:2, 1:T]
		acceleration[1:2, 1:T]
		-90 <= angle[1:T] <= 90
		-3 <= thrust[1:T] <= 3
    end)
	@NLconstraints(model, begin
        [t=1:T], acceleration[1, t] == thrust[t] * sind(angle[t])
        [t=1:T], acceleration[2, t] == thrust[t] * cosd(angle[t]) - 1.635
    end)
    @constraints(model, begin
        [t=2:T, j=1:2], velocity[j, t] == velocity[j, t-1] + acceleration[j, t-1] * Δt
        [t=2:T, j=1:2], position[j, t] == position[j, t-1] + velocity[j, t-1] * Δt
        [i=1:2], position[i, 1] == initial_position[i]
        [i=1:2], velocity[i, 1] == initial_velocity[i]
        angle[1] == initial_angle
    end)
    @objective(
        model,
        Min,
        100 * sum((desired_position[i] - position[i, T])^2 for i in 1:2) +
        sum(velocity[i, T]^2 for i in 1:2)
    )
    optimize!(model)
    return value.(position), value.(velocity), value.(acceleration), value.(angle)
end

begin
	q, v, ang = [1.0, 0.0], [-2.0, 2.0], 45
	Δt = 0.1
	anim = @animate for i in 1:100
        # Run MPC
	    q_plan, v_plan, u_plan, ang_plan = run_mpc(q, v, ang)
        # Plot solution
	    plot([q[1]], [q[2]], marker=(:rect, 10), xlim=(-2, 2), ylim=(-2, 2))
		plot!([q[1]], [q[2]], marker=(:cross, 18, :grey))
	    plot!(q_plan[1, :], q_plan[2, :], linewidth=5, arrow=true, c=:orange)
        # Update positions
	    global q .= q_plan[:, 2]
	    global v .= v_plan[:, 2]
		global ang = ang_plan[2]
	end
	gif(anim, "NLmpc_angle.gif")
end

NLmpc_angle

1 Like