Share
Embed
Nullspaceย MPC
Explanation of Nullspace MPC, a novel control framework that incorporates explicit task prioritization and accommodates diverse objectives. Demonstrated its effectiveness in narrow-space navigation of a swerve drive robot.
Project website: mizuhoaoki.github.io/projects/nullspace_mpc
Open-source code: github.com/MizuhoAOKI/nullspace_mpc
MizuhoAOKI
September 12, 2025
Transcript
38 slides
-
Nullspace MPC Note: This slide deck is excerpted and edited
from the doctoral public defense presentation โNonlinear Model Predictive Control for Autonomous Vehicles: Enhancement via Simplified Physics-Aware Prediction and Decomposed Optimization,โ held on August 22, 2025. One-Shot Trajectory Optimization by MPPI (10000 samples) One-Shot Trajectory Optimization by Nullspace MPC (100 samples) Mizuho Aoki, Nagoya University -
Background: Diversifying Mobility Designs 2 Reference [1] https://lexus.jp/models/?padid=from_not_ljptop_footer_carlineup [2] https://www.youtube.com/watch?v=QC6Q4Fge3uE
[3] https://response.jp/article/2014/12/12/239488.html [4] https://www.youtube.com/watch?v=Bbw_smfOgVA [4] [2] [3] Mobilities with different degrees of freedom (DoF) are appearing. [1] 2 DoF 5 DoF 8 DoF 8 DoF Positive Aspects Negative Aspects Higher mobility performance will be beneficial. Capable of meeting a wide variety of user needs. Difficult to exploit performance of each vehicle. Keeping safety must be ensured for any vehicle. Consider the challenge of adding Autonomous Driving ability in these vehicles. -
Diversity of Strategies Enabled by Increased Degrees of Freedom 3
2 DoF Vehicle 5 DoF Vehicle 8 DoF Vehicle Ex. How can parallel parking be achieved in a narrow space? Higher degrees of freedom allow for multiple strategies to achieve the same goal. -
Control Target: What is 4WIDS Vehicle? 4 4WIDS Vehicle stands
for Four-wheel Independent Drive and Steering Vehicle. Flexible driving behavior ๐ฟ๐๐ ๐ฟ๐๐ ๐ฟ๐๐ ๐ฟ๐๐ ๐๐๐ ๐ ๐๐ ๐๐๐ ๐๐๐ Tire angle (4 DoFs) ๐ฟ๐๐ , ๐ฟ๐๐ , ๐ฟ๐๐ , ๐ฟ๐๐ Wheel Velocity (4 DoFs) ๐๐๐ , ๐๐๐ , ๐๐๐ , ๐ ๐๐ 8 DoFs Parallel Shift Turn in Place Shift + Turn -
Formulate a Simplified Prediction Model of 4WIDS Vehicle 5 8
DoF control input is reducible to 3 DoF with appropriate assumptions. ๐ฟ๐๐ ๐ฟ๐๐ ๐ฟ๐๐ ๐ฟ๐๐ ๐๐๐ ๐ ๐๐ ๐๐๐ ๐๐๐ Tire angle (4 DoFs) ๐ฟ๐๐ , ๐ฟ๐๐ , ๐ฟ๐๐ , ๐ฟ๐๐ Wheel Velocity (4 DoFs) ๐๐๐ , ๐๐๐ , ๐๐๐ , ๐ ๐๐ 8 DoF ๐ ๐ฅ ๐ ๐ฆ ๐ 3 DoF The vehicle is a rigid body. Wheels do not slip. Assumptions Holonomic vehicle model 4WIDS vehicle model Note: Since friction is neglected, motion patterns that exploit wheel slip are not modeled in the prediction. -
Baseline Optimizer: Model Predictive Path-Integral (MPPI) Control 6 Sampled Trajectories
Optimal Trajectory 1 2 3 Sampling control input sequences Calculate cost for each sample. Obtain optimal input sequence. Bad samples, higher costs Good samples, lower costs Weighted sum of samples Optimal input sequence obstacle obstacle obstacle MPPI is a type of MPC that uses sampling-based approach to obtain an optimal solution. -
Issue: Balancing Task Priorities Through Weights is Difficult 7 Vehicle
controller need to consider multiple objectives. But balancing weight between tasks is difficult. User Initial Position Reference Path โฆ Velocity limit โฆ Avoid obstacles of any shape โฆ Follow a reference path โฆ Smooth driving ( small acceleration ) โฆ Smooth operation of vehicle commands Task List Obstacle Optimal Trajectory -
Ex. How to balance Collision Avoidance and Smooth Driving Minimize
a scalar cost function Penalty for collisions Cost function Penalty for non-smooth motion ๐1 ๐2 ร ร Hard to find the best weight parameters. 8 Key Question: How can the best motion be found across multiple tasks? -
Ex. How to balance Collision Avoidance and Smooth Driving Minimize
a scalar cost function Penalty for collisions Cost function Penalty for non-smooth motion ๐1 ๐2 ร ร Hard to find the best weight parameters. Narrow down the solution stepwise. Set of all motions Set of motions without any collisions Select the smoothest motion Obtain the smoothest motion within the collision-free solution set. 9 Key Question: How can the best motion be found across multiple tasks? -
10 ๐ฎ๐ฏ ๐ ๐ฎ๐ฏ ๐ ๐ฎ๐ฏ ๐ ๐ฎ๐ฏ ๐ ๐ฎ๐ฏ
๐ โฉ > ๐ฎ๐ฏ ๐ Introducing a Priority-Aware Operator โฉ to Narrow Down a Solution Set โข Let ๐ be a solution vector of a motion planning problem. > โข ๐ข๐ฃ๐ is a solution set of ๐ satisfying Task ๐ฃ๐ , ๐ข๐ฃ๐ is a solution set of ๐ satisfying Task ๐ฃ๐ โข Now Task ๐ฃ๐ has higher priority than Task ๐ฃ๐ , obtain an intersection set between them considering the task priority with a new operator . ๐ฎ๐ฏ ๐ โฉ > ๐ฎ๐ฏ ๐ is defined as follows. ๐ฎ๐ฏ ๐ โฉ๐ฎ๐ฏ ๐ โ ๐ (๐) ๐๐ ๐ฎ๐ฏ ๐ โฉ๐ฎ๐ฏ ๐ = ๐ (๐) ๐๐ โฉ > Here, (Closest satisfying ๐ฃ๐ ) ๐ฎ๐ฏ ๐ โฉ > ๐ฎ๐ฏ ๐ (= ๐ฎ๐ฏ ๐ โฉ ๐ฎ๐ฏ ๐ ) -
Hierarchical Prioritization Framework Only for Linear Tasks 11 Question Key
Idea Task: แ ๐๐ = ๐ ๐๐ โค ๐ Definition Linear Equality Linear Inequality Task is a set of conditions that solution ๐ tries to satisfy. Move solution ๐ to achieve a target task while keeping higher-level tasks satisfied. Task 1 > Task 2 > โฆ > Task N High Priority Low Priority List of values to be found How can multiple tasks be achieved considering priorities? -
Hierarchical Prioritization Framework Only for Linear Tasks 12 Question Key
Idea Task: แ ๐๐ = ๐ ๐๐ โค ๐ Definition Linear Equality Linear Inequality Task is a set of conditions that solution ๐ tries to satisfy. Move solution ๐ to achieve a target task while keeping higher-level tasks satisfied. Task 1 > Task 2 > โฆ > Task N High Priority Low Priority Apply Task 1 ๐โ : tentative optimal solution All solution set for ๐ ๐1 ๐ = ๐1 , ๐1 ๐ โค ๐1 Apply Task 2 ๐2 ๐ = ๐2 , ๐2 ๐ โค ๐2 Apply Task 3 ๐3 ๐ = ๐3 , ๐3 ๐ โค ๐3 This ๐โ follows Task 1 > Task 2 > Task 3 priority. 2 1 3 2 1 1 List of values to be found How can multiple tasks be achieved considering priorities? -
Mathematical Tools for Prioritization 13 Linear Inequality Linear Equality ๐๐
= ๐ ๐๐ โค ๐ ๐2 = ๐1 + ๐ ๐1 ๐ , ๐1 ๐1 = ๐1 . ๐1 ๐ ๐1 = ๐ . Let ๐1 be a vector which satisfies a linear equality, Here ๐ ๐1 is a nullspace basis matrix of ๐1 , Then ๐2 below always satisfies ๐1 ๐2 = ๐1 ๐1 ๐2 = ๐1 ๐1 + ๐ ๐1 ๐ง , = ๐1 ๐1 + ๐1 ๐ ๐1 ๐ง, = ๐1 ๐1 + ๐ = ๐1 . arbitrary vector minimize ๐๐ ๐๐ 2 โต ๐๐ ๐ โ ๐๐ โค ๐ ๐๐ ๐ โ ๐๐ โค ๐๐ ๐. ๐. Try to satisfy ๐๐ ๐ โค ๐๐ while keeping ๐๐ ๐ โค ๐๐ High Priority Low Priority Low Priority High Priority ๐๐ ๐ โค ๐๐ ๐๐ = ๐ ๐๐ ๐ โค ๐๐ ๐๐ ๐ โค ๐๐ ๐๐ โ ๐ ๐๐ ๐ โ ๐๐ โค ๐๐ -
Formulate Optimization Problem for Task p+1 14 Update tentative solution
๐โ to satisfy Task p+1 ( ๐๐+1 ๐โ = ๐๐+1 and ๐๐+๐ ๐ โค ๐๐+๐ ) without disrupting the higher-level tasks (Task 1 > โฆ > Task p) nullspace basis matrix of all from ๐1 to ๐p ๐โ is a solution considering Task 1 > โฆ > p ๐โ + ๐๐ ๐ง๐+1 is the updated solution considering Task 1 ~ p+1 -
Priority-Based Trajectory Optimization 15 Just stopping at the start point
๐ = ๐๐ฅ 0, ๐๐ฆ 0, ๐0, ๐ฃ๐ฅ 0, ๐ฃ๐ฆ 0, ๐0, โฆ , ๐๐ฅ ๐โ1, ๐๐ฆ ๐โ1, ๐๐โ1, ๐ฃ๐ฅ ๐โ1, ๐ฃ๐ฆ ๐โ1, ๐๐โ1, ๐๐ฅ ๐, ๐๐ฆ ๐, ๐๐ ๐ Time series states for T steps: Find position velocity Set the initial pose at 0 step Follow the state update rules Limit velocity for all steps ๐1 ๐ = ๐1 ๐ซ1 ๐ โค ๐1 Task 1 start ๐ฎ๐ฏ 1 ๐โ โ ๐ฏ 1 ๐ฎ๐ฏ 1 ๐ฎ ๐ โ ๐ฎ, Apply ๐โ -
Priority-Based Trajectory Optimization 16 Just stopping at the start point
Passing through the via pose and reaching the goal ๐ = ๐๐ฅ 0, ๐๐ฆ 0, ๐0, ๐ฃ๐ฅ 0, ๐ฃ๐ฆ 0, ๐0, โฆ , ๐๐ฅ ๐โ1, ๐๐ฆ ๐โ1, ๐๐โ1, ๐ฃ๐ฅ ๐โ1, ๐ฃ๐ฆ ๐โ1, ๐๐โ1, ๐๐ฅ ๐, ๐๐ฆ ๐, ๐๐ ๐ Time series states for T steps: Find position velocity Set the initial pose at 0 step Follow the state update rules Limit velocity for all steps ๐1 ๐ = ๐1 ๐ซ1 ๐ โค ๐1 Pass through a via pose at Tmid step Reach to a goal pose at T step None ๐2 ๐ = ๐2 ๐ซ2 ๐ โค ๐2 Task 1 Task 2 start goal via pose start ๐ฎ๐ฏ 1 โฉ > ๐ฎ๐ฏ 2 ๐ฎ๐ฏ 1 ๐โ โ ๐โ โ ๐ฏ 1 ๐ฏ 2 ๐ฎ๐ฏ 2 ๐ฎ๐ฏ 1 ๐ฎ๐ฏ 1 ๐ฎ ๐ โ ๐ฎ, Apply Apply ๐โ -
Priority-Based Trajectory Optimization 17 Just stopping at the start point
Passing through the via pose and reaching the goal Moving more smoothly while passing the via pose. ๐ = ๐๐ฅ 0, ๐๐ฆ 0, ๐0, ๐ฃ๐ฅ 0, ๐ฃ๐ฆ 0, ๐0, โฆ , ๐๐ฅ ๐โ1, ๐๐ฆ ๐โ1, ๐๐โ1, ๐ฃ๐ฅ ๐โ1, ๐ฃ๐ฆ ๐โ1, ๐๐โ1, ๐๐ฅ ๐, ๐๐ฆ ๐, ๐๐ ๐ Time series states for T steps: Find position velocity Set the initial pose at 0 step Follow the state update rules Limit velocity for all steps ๐1 ๐ = ๐1 ๐ซ1 ๐ โค ๐1 Pass through a via pose at Tmid step Reach to a goal pose at T step None ๐2 ๐ = ๐2 ๐ซ2 ๐ โค ๐2 Set zero acceleration for all steps None ๐3 ๐ = ๐3 ๐ซ3 ๐ โค ๐3 Task 1 Task 2 Task 3 start goal via pose start goal start via pose ๐ฎ๐ฏ 1 โฉ > ๐ฎ๐ฏ 2 ๐ฎ๐ฏ 1 ๐โ โ ๐โ โ ๐ฎ๐ฏ 1 โฉ > ๐ฎ๐ฏ 2 ๐โ โ โฉ > ๐ฎ๐ฏ 3 ๐ฏ 1 ๐ฏ 2 ๐ฏ 3 ๐ฎ๐ฏ 2 ๐ฎ๐ฏ 1 ๐ฎ๐ฏ 1 ๐ฎ ๐ โ ๐ฎ, ๐ฎ๐ฏ 2 ๐ฎ๐ฏ 1 ๐ฎ๐ฏ 3 Apply Apply Apply ๐โ -
Example 18 Limitations of Linearly Represented Tasks Issue Task: แผ
๐จ๐ = ๐, Task to avoid a circular obstacle How can nonlinear task expressions be considered in the priority-based optimization framework? Linear equality and inequality constructing a task are not sometimes expressive enough. ๐ซ๐ โค ๐ } Converted to a linear task avoiding a straight wall Never reach the goal Question circular obstacle linearized boundary start goal -
Nullspace MPC: Sample-Based Extension for Handling Nonlinearity ๐ฎ๐2๐ ๐ฎ ๐ฏ
1 ๐ ๐ฎ ๐ฏ 2 ๐ฟ Optimal Solution Considering Task Priorities Difficult to solve directly ๐ฎ๐2๐ ๐ฎ ๐ฏ 1 ๐ ๐ฎ ๐ฏ 2 ๐ฟ ๐ฎ ๐ฏ 1 ๐ฟ ๐ฎ๐2๐ ๐ฎ ๐ฏ 1 ๐ ๐ฎ ๐ฏ 2 ๐ฟ ๐ฎ ๐ฏ 1 ๐ฟ ๐ฎ๐2๐ ๐ฎ ๐ฏ 1 ๐ ๐ฎ ๐ฏ 2 ๐ฟ ๐ฎ ๐ฏ 1 ๐ฟ Obstacle Avoiding Trajectory Among the Samples via pose via pose via pose Approximately equivalent 19 Go through a via pose Select a via pose for avoiding obstacle driving smoothly Sampling-Based Optimizer Linear Task Linear Task ๐ฏ 1 ๐ฟ: Drive smoothly ๐ฏ 2 ๐ฟ: Avoid obstacles Nonlinear Task Linear Task ๐ฏ 1 ๐: Drive smoothly ๐ฏ 2 ๐ฟ: Decompose โNonlinear Tasksโ into โLinear Tasks and Nonlinear Parameter Searchโ Find Solution set reaching the goal -
20 Where Nullspace MPC Stands Among Existing Methods Approach A:
Wide application range Difficult priority weight tuning Approach B: Approach C: Combine strict task prioritization with soft weighting Minimize a single cost function Consider strict task prioritization (lexicographic optimization) MPPI Nullspace MPC HQP Clearer task priority than MPPI Smaller application range than MPPI Rigid task prioritization Limited application range Wider application range than HQP Less rigid priority guarantee than HQP (Hierarchical Quadratic Programming) -
Start point is the current vehicle location. The vehicle try
to follow the reference path. Set Start Pose and Reference Path 21 Start Reference Path Nullspace MPC 01 -
22 Start Local Goal Reference Path L [m] Set Local
Goal Pose Nullspace MPC 02 Set a local goal ๐ฟ [m] ahead along the reference path, assuming perfect tracking. -
Sample via poses according to gaussian distributions. There are K
samples in total on each axis. 23 Start Reference Path Sample Via Poses Nullspace MPC 03 Local Goal -
Select kth via point sample on each axis and plan
a smooth trajectory trying to pass start and via points. 24 Start Reference Path Get Sample Trajectories Nullspace MPC 04 Local Goal -
Select kth via point sample on each axis and plan
a smooth trajectory trying to pass start and via points. 25 Start Reference Path Get Sample Trajectories Nullspace MPC 04 Local Goal -
Select kth via point sample on each axis and plan
a smooth trajectory trying to pass start and via points. 26 Start Reference Path Get Sample Trajectories Nullspace MPC 04 Local Goal -
Select kth via point sample on each axis and plan
a smooth trajectory trying to pass start and via points. 27 Start Reference Path Get Sample Trajectories Nullspace MPC 04 Local Goal -
Select kth via point sample on each axis and plan
a smooth trajectory trying to pass start and via points. 28 Start Reference Path Get Sample Trajectories Nullspace MPC 04 Local Goal -
Select kth via point sample on each axis and plan
a smooth trajectory trying to pass start and via points. 29 Start Reference Path Get Sample Trajectories Nullspace MPC 04 Local Goal -
Select kth via point sample on each axis and plan
a smooth trajectory trying to pass start and via points. 30 Start Reference Path Get Sample Trajectories Nullspace MPC 04 Local Goal -
Plan smooth paths for all samples with gradient-based optimizer, and
calculate cost of sample trajectories for sampling-based optimization. 31 Start Reference Path Calculate Cost for Each Sample Trajectory Nullspace MPC 05 Local Goal cost 1 = 3.07 cost 3 = 8.13 cost 2 = 2.35 cost K โ 2 = 5.19 cost K = 15.3 cost K โ 1 = 6.77 sample costs smaller the better Cost = costcollision_penalty +costsmooth_driving +costtracking_error +costcmd_change avoid obstacles drive smoothly track reference path accurately penalty for big command change -
Convert sample costs into normalized weights. 32 Start Reference Path
Calculate Sample Weights Nullspace MPC 06 Local Goal cost 1 = 3.07 cost 3 = 8.13 cost 2 = 2.35 cost K โ 2 = 5.19 cost K = 15.3 cost K โ 1 = 6.77 sample costs w 1 = 0.05 w 3 = 0.01 w 2 = 0.08 w K โ 2 = 0.02 w K โ 1 = 0.03 sample weights better samples get more weights w K = 0.005 Cost = costcollision_penalty +costsmooth_driving +costtracking_error +costcmd_change avoid obstacles drive smoothly track reference path accurately penalty for big command change -
Get the Optimal Via Poses Nullspace MPC 07 Get the
optimal via poses calculating the weighted sum of all sampled via poses. Start Reference Path Local Goal Prior Distribution Posterior Distribution Variational Inference 33 -
Get the Optimal Via Poses Nullspace MPC 07 Get the
optimal via poses calculating the weighted sum of all sampled via poses. Start Reference Path Local Goal Prior Distribution Posterior Distribution Variational Inference 34 -
Optimal Via Poses Get the Optimal Via Poses Nullspace MPC
07 Get the optimal via poses calculating the weighted sum of all sampled via poses. Start Reference Path Local Goal 35 -
Optimal Trajectory Get the Optimal Trajectory Nullspace MPC 08 The
optimal trajectory is one which passes start and optimal via points. Start Reference Path Local Goal 36 -
Simulation of Narrow Space Navigation 37 Sampling-Based MPC (MPPI) Nullspace
MPC (Proposed) Nullspace MPC achieved 11% shorter trajectory length, 36% shorter travel time, with a higher success rate. Nullspace MPC required 12 times more computation time (though real-time operation was still feasible). In sequential goal-reaching simulations over 100 episodes, -
Summary 38 How can the best motion be found across
multiple tasks? Future Work โข Improve computational efficiency. โข Broaden applicability to more complex and realistic robotic systems, especially robots with nonlinear dynamics and high-dimensional states. Contribution โข Proposed Nullspace MPC, a control framework that incorporates explicit task prioritization and accommodates diverse objectives โข Demonstrated its effectiveness in narrow-space navigation of a 4WIDS vehicle, whose dynamics was approximated as holonomic motion, outperforming the MPPI controller. Strength of gradient-based approach Strength of sampling-based approach