MPC Cutting corners

Hi all!

I have a rather general question: I’m using an MPC for drone control. I’m planning a collision free route using an RRT* planner to plan a collision free route. This results in collision free waypoints and I can guarentee that the straight lines in between these waypoints are also collision free. Additionally, a drone is able to track straight lines, so no problems there. However, I’m curious to what the preferred way is to let an MPC track straight lines. My MPC currently has a stage cost only based on control inputs and a terminal cost based on the state (pos, vel, orientation). The MPC controls a quadcopter (roll,pitch,yawrate,throttle).

If I only set the next collision-free waypoint as a reference, I have two problems:

  1. Reference velocity is zero, thus the quadcopter stops at the waypoint and when the distance is low enough I give the next waypoint. This can be solved by also setting a reference velocity.
  2. The MPC is free to cut corners. If throttle cost is cheap and rotating the quadcopter is expensive, it will climb up stairs if we avoid a lying cylinder. This can be solved with tuning I think.

My question is: Should I just properly tune my MPC (to track straight lines) and give a reference velocity for my waypoints (to avoid the braking), or should I explicitely give a trajectory to my controller (i.e. a stage cost that also involves states).

Thanks in advance for your expertise!