[02] Feedback Motion Planning using LQR-Trees
Reference papers
Russ Tedrake, Ian R. Manchester, Mark M. Tobenkin, and John W. Roberts. LQR-Trees: Feedback motion planning via sums of squares verification. International Journal of Robotics Research, 29:1038-1052, July 2010
Philipp Reist, Pascal V. Preiswerk, and Russ Tedrake. Feedback-motion-planning with simulation-based lqr-trees. To appear in the International Journal of Robotics Research, 2016.
2. Implementation Strategies
- Implementation using MATLAB Toolbox provided Russ Tedrake Lab
- Presenting results of LQR-Tree in case of very simple system (1DOF inverted Pendulum model)
- Programming LQR-Trees in Python or other language
- Applying LQR-Trees to control mobile robot based on the trust model
3. Toolbox provided by Russ Tedrak Lab
4. 1DOF inverted Pendulum Model
b. The system dynamics
c. The state equation
- msspoly deon't support sin and cos function so that we approximate the sin value using Taylor series
5. Conditions for simulation
a. m = 1.0 kg, l=0.5 m, b=0.1 Nms, g= 9.81 m/s^2
b. u = -0.001*t*(t - 5) *(t + 5)
c. t= [0, 5]
d x0 = [0, 0]
6. Simulation Results under different conditions of R, Q and inputs
7. Issues for Programming
a. In MATLAB Toolbox, SPOT does't provide 'sin' for input arguments of type 'msspoly'. -> replace sin(th) approx (th)
b. In Python, there are some issues of CVXPY in the process of implementation of J*
8. Implementations in detail
a. Approximation of the state equation
b. The linearized the system equation becomes
c. The LQR Problem of Cost-to-go function
and Optimal feedback policy is
In case of initial conditions are x0=[pi ; 0], u0 =0 and the final goal state is xG=[0; 0], we can get results of S, K,
d.Time-Invariant LQR Verification
: The time derivative of Lyapunov function becomes
and then, we should search for h which must be of sufficiently large order to counteract higher order terms in \dot J*
In this problem, the system dynamics is not polynomial, we can approximate the condition using Taylor expansion. In the paper, 2nd order expansion was applied but we utilized 3rd order in this example.
As a result, We can obtain the figure which is exactly same to the Figure 3. in the Paper. (The Region of Attraction: ROA)
5. Our Application Scenario ( Time-varying Motion Planning considering Pedestrians)
: we are going to apply this LQR-tree to safety guaranteed robot navigation. Our work has very similar purpose to that of "Nonlinear Controller Synthesis and Automatic Workspace Partitioning for Reactive High-Level Behaviors."
We want to modify ROA in terms of trust zone for pedestrians, and then, to make a shooting method for stable motion planning under dynamic environment where the pedestrians are moving.
