i2LQR: Iterative LQR for Iterative Tasks in Dynamic Environments

TL;DR

i2LQR is a new control method that iteratively optimizes trajectories for tasks in dynamic environments, improving performance over existing methods by using historical data and adapting to changing constraints.

Contribution

The paper introduces i2LQR, a reference-free, iterative control algorithm that enhances closed-loop performance for iterative tasks in dynamic environments using local trajectory optimization.

Findings

i2LQR outperforms LMPC in dynamic environments with obstacles.

i2LQR achieves better completion time optimization in simulations.

The method adapts to changing constraints across iterations.

Abstract

This work introduces a novel control strategy called Iterative Linear Quadratic Regulator for Iterative Tasks (i2LQR), which aims to improve closed-loop performance with local trajectory optimization for iterative tasks in a dynamic environment. The proposed algorithm is reference-free and utilizes historical data from previous iterations to enhance the performance of the autonomous system. Unlike existing algorithms, the i2LQR computes the optimal solution in an iterative manner at each timestamp, rendering it well-suited for iterative tasks with changing constraints at different iterations. To evaluate the performance of the proposed algorithm, we conduct numerical simulations for an iterative task aimed at minimizing completion time. The results show that i2LQR achieves an optimized performance with respect to learning-based MPC (LMPC) as the benchmark in static environments, and outperforms LMPC in dynamic environments with both static and dynamics obstacles.

Tables

Table 1. TABLE I : Symbol Notations

Symbols for iterative tasks
Symbol	Description
${𝐱}_t^i$	System state at time step $t$ of iteration $i$
${𝐮}_t^i$	System input at time step $t$ of iteration $i$
${𝐱}_0$	Initial state for all iterations
${𝐱}_{\text{target}}$	Target state for all iterations
$h\left({𝐱}_t^i\right)$	Cost-to-go associated with system state ${𝐱}_t^j$
${𝒳}^i$	Set of historical states for iteration $i$
$\mathscr{H}$	Set of all historical states from previous iterations
${𝒞}_t^i$	Constraints on the system at time step $t$ in iteration $i$
$ϵ$	A small positive number
Symbols for i2LQR
Symbol	Description
${\overline{𝐱}}_r$	Guided state for $r$-th target terminal set
${𝒵}_r$	$r$-th target terminal set
${𝐳}_r\left(j\right)$	$j$-th state from $r$-th target terminal set
$N$	Prediction horizon for the optimization problem
${𝐱}_r^m\left(j\right)$	Open-loop states of $m$-th iteration of iLQR for ${𝐳}_r\left(j\right)$
${𝐮}_r^m\left(j\right)$	Open-loop inputs of $m$-th iteration of iLQR for ${𝐳}_r\left(j\right)$
${𝐱}_r^{\ast }\left(j\right)$	Optimized open-loop states from iLQR for ${𝐳}_r\left(j\right)$
${𝐮}_r^{\ast }\left(j\right)$	Optimized open-loop inputs from iLQR for ${𝐳}_r\left(j\right)$
${𝐱}_r^{\ast }\left(j_r^{\ast }\right)$	Best open-loop states for $r$-th optimization cycle
${𝐮}_r^{\ast }\left(x_r^{\ast }\right)$	Best open-loop inputs for $r$-th optimization cycle
$J_{𝐳}\left({\overline{𝐱}}_r\right)$	Cost for nearest point selection on ${\overline{𝐱}}_r$
$J_l({𝐱}_t^i,{𝐳}_r\left(j\right))$	Cost for local iLQR Optimization
$m_{\text{max}}$	Maximum iteration for iterative optimization
$r_{\text{max}}$	Maximum iteration for target terminal set

Equations

$$J_{\mathbf{z}}({\bar{\mathbf{x}}}_r)=\underset{{\mathbf{z}}_r(j)}{\min }$$

$$\text{s.t.}{z}_j\ne z_l,$$

$$z_j\in \mathcal{H},$$

$$J_l({\mathbf{x}}_t^i,{\mathbf{z}}_r(j))=\underset{{\mathbf{u}}_r^{\ast }(j)}{\min }p({\mathbf{x}}_r^{\ast }(j{)}_{1+N},{\mathbf{z}}_r(j)$$

$$\text{s.t.}{\mathbf{x}}_r^{\ast }(j{)}_{k+1}=f({\mathbf{x}}_r^{\ast }(j{)}_k,{\mathbf{u}}_r^{\ast }(j{)}_k),k$$

$${\mathbf{x}}_r^{\ast }(j{)}_{k+1},{\mathbf{u}}_r^{\ast }(j{)}_k\in {\mathcal{C}}_{t+k|t}^i,k$$

$${\mathbf{x}}_r^{\ast }(j{)}_1={\mathbf{x}}_t^i,$$

$$p({\mathbf{x}}_r^{\ast }(j{)}_{1+N},{\mathbf{z}}_r(j))$$

$$({\mathbf{x}}_r^{\ast }(j{)}_{1+N}$$

$$j_r^{\ast }=\underset{j=1,...,K}{\mathrm{argmin}}{w}_{h}h({\mathbf{z}}_r(j_r))+w_d||{\mathbf{x}}_r^{\ast }(j_r{)}_N-{\mathbf{z}}_r(j_r)|{|}_{D_1}^2,$$

Full text

I Introduction

I-A Motivation

One important objective of control algorithms is to optimize the performance of autonomous systems. This can be usually formulated as minimizing completion time [1] or energy consumption [2] during task execution. The performance-optimal controller can be applied to iterative tasks, where the autonomous system must do the same task repeatedly. However, the surrounding environment around the autonomous system may be dynamic. This means that constraints of the system are not always the same along the process, such as constraints that are changed in some given iterations, e.g. new obstacles appear after a particular iteration or moving obstacles in a single iteration. The existing algorithms cannot handle these changes effectively. This motivates us to propose a control strategy that could handle additional constraints in different iterations while improving the system’s closed-loop performance with local trajectory optimization.

I-B Related Work

Researchers have applied different methods to optimize performance. Model-based approaches usually leverage a high-level planner to generate the optimized trajectory and a low-level controller is deployed to track the planned trajectory [3, 4, 5, 6]. However, calculating the best possible trajectory can be time consuming, and the low-level controller may not track the trajectory perfectly due to discretization in the planning problem. Furthermore, when the planned trajectory encounters conflicts with newly appeared constraints, local re-planning is required to deal with these additional constraints [7]. This may result in the loss of optimality for the trajectory in local trajectory optimization.

Several attempts are made to address the problem through data-driven based model-free approaches, where the system’s historical data is used to train an end-to-end control policy directly. For instance, in [1, 8], the learning-based control policy shows its capability to push an autonomous racing car to its dynamics limit. In [9, 10], quadrotors are shown to fly with aggressive maneuvers in an autonomous drone racing competition. Nevertheless, these methods still have limitations. Learning-based methods are data hungry and require significant time to get the policy, which means that such methods may not be suitable for some real-time applications. Moreover, all the aforementioned policies operate in a static environment. However, in practice, the control policy is required to be functional in a dynamic environment. More importantly, since the performance of the trained policy is highly related to the used training data set, the learned policy may not guarantee optimal performance.

Recently, reference-free, model-based methods are proposed to provide optimized performance for iterative tasks. In [11], a model predictive contouring controller (MPCC) with dynamics learning is used for autonomous racing cars. The optimization algorithm called learning-based MPC (LMPC) is introduced in [12, 13], where the system’s historical data is used to formulate the local MPC optimization problem. This allows the system to improve its performance over each iteration and the system is proved theoretically to achieve closed-loop optimal performance [12]. The proposed algorithm is implemented on autonomous vehicles [14], aerial robots [15] and robotic arms [16]. However, these methods still have shortcomings. Although the local objective function in [11] considers optimal performance along the prediction horizon, the result is not closed-loop optimal without global planning. The LMPC strategy in [12, 13] must work in a static environment, which means that the scenario should be exactly same for every iteration. This is due to the limitation of the optimization setup, where local MPC’s feasibility strictly depends on the reachability of historical states from previous iterations. If new obstacles are introduced, these historical states could be infeasible, rendering the MPC problem infeasible. One possible solution to this problem is local-replanning [17], but the performance may be limited due to the nonsmooth switching in the high-level planner.

To ensure that the autonomous system can smoothly adapt to newly introduced constraints, the optimal trajectory should be computed in an iterative manner. Therefore, we want to use a method that could handle this without resulting in infeasibility. The iterative linear quadratic regulator (iLQR) shows potential in solving this problem. iLQR is an extension of LQR control, where the optimization is solved iteratively and linearization of cost function and system dynamics is conducted in each iteration. In [18, 19], the iLQR computes the open-loop predicted trajectory for control problems in an iterative manner. This motivates us to investigate the above challenging problem using an iLQR-based algorithm.

I-C Contribution

The contributions of this paper are as follows:

• •

We propose a novel optimal control strategy called Iterative LQR for Iterative Tasks (i2LQR), which improves closed-loop performance with local trajectory optimization for a general dynamic system for iterative tasks in dynamic environments.

• •

We demonstrate the utilization of historical data from preceding iterations to formulate the local optimization problem within each time step of i2LQR. Additionally, we outline an iterative approach to efficiently solve this optimization problem.

• •

Through numerical simulation, our proposed control strategy is shown to achieve the same optimal performance as the LMPC algorithm for iterative tasks in static environments and outperform the LMPC algorithm for iterative tasks in dynamic environments.

II Problem Setup

In this section, we show the problem setup of iterative tasks.

For an iterative task at time step $t$ of iteration $i$, an autonomous system with dynamics $\mathbf{x}^{i}_{t+1}=f(\mathbf{x}^{i}_{t},\mathbf{u}^{i}_{t})$ performs the task repeatedly until completion. In each iteration, the system starts from the same initial state $\mathbf{x}_{0}$ and ends at the same target state $\mathbf{x}_{\text{target}}$. The system’s historical data (e.g. state and completion time) is saved in a data set $\mathcal{H}$. The system’s controller computes the optimal input based on this data set $\mathcal{H}$, state $\mathbf{x}^{i}_{t}$ and constraint $\mathcal{C}_{t}^{i}$ at current time step. The constraint $\mathcal{C}_{t}^{i}$ includes the constraints on both system states and inputs. If the system works in a static environment [13, 14], the constraint $\mathcal{C}_{t}^{i}$ will be same for the same time step $t$ of different iterations (i.e., $\mathcal{C}_{t}^{i}=\mathcal{C}_{t}^{i+1}$). If the system works in a dynamic environment (as in this work), the constraint $\mathcal{C}_{t}^{i}$ will change along the entire process (i.e., $\mathcal{C}_{t}^{i}\neq\mathcal{C}_{t}^{i+1}$ or $\mathcal{C}_{t+1}^{i}\neq\mathcal{C}_{t}^{i}$). The cost-to-go $h(\textbf{x}_{t}^{i})$ is the time to finish the iteration $i$ from the point $\textbf{x}_{t}^{i}$ to $\mathbf{x}_{\text{target}}$, which means that the algorithm aims to minimize the time to finish each iteration. This describes the cost to finish the corresponding iteration from that point to the target state $\mathbf{x}_{\text{target}}$. The general form of this problem is shown in Alg. 1.

In this work, we focus on designing a performance optimal controller, which could minimize the cost $h(\mathbf{x}^{i}_{0})$ of the iterative task. Details about this algorithm will be presented in Sec. III. Parameters used in this work are listed in TABLE I along with their notations.

III Algorithm

After introducing the problem setup for iterative tasks, the design of the proposed Iterative LQR for Iterative Tasks (i2LQR) in a dynamic environment will be presented in this section. The general idea of the algorithm will be introduced in Sec. III-A. Details about the i2LQR will be shown in the following subsections.

III-A Structure of i2LQR

Consider the problem of achieving the system’s optimal performance for iterative tasks in a dynamic environment in the form of Alg. 1. The proposed control strategy i2LQR computes the optimal input using historical data from previous iterations. This includes states that the system has visited in previous iterations and the cost-to-go $h(\mathbf{x}^{i}_{t})$ associated with each historical state. In the first iteration, any open-loop controller could be used to generate a feasible trajectory as the initial history data. Then, the proposed i2LQR algorithm is deployed to calculate the system’s optimal input at each time step. Fig. 1(a) shows the structure of the proposed algorithm. As a comparison, the structure of the LMPC algorithm is also presented in Fig. 1(b).

Different from LMPC algorithm, the proposed i2LQR controller consists of several optimization cycles (yellow block in Fig. 1(a)). For the $r$-th optimization cycle, we define a guided state $\bar{\mathbf{x}}_{r}$, which will be the initial guess of the open-loop terminal state in the $r$-th optimization cycle and we select $K$ nearest points with respect to it (see Fig. 2). For the first cycle, the state at the current time step $\mathbf{x}^{i}_{t}$ will be used as the guided state, while the best open-loop predicted terminal state ${\mathbf{x}}_{r-1}^{*}(j_{r-1}^{*})_{N}$ from the last cycle will be used as the guided state for $r$-th cycle. Then $K$-nearest points to $\bar{\mathbf{x}}_{r}$ from the historical states set $\mathcal{H}$ will build the target terminal set $\mathcal{Z}_{r}$, which consists of the target terminal state $\mathbf{z}_{r}(j)$ of the $j$-th iLQR optimization. To reduce the computational time of the proposed algorithm, these iLQR optimizations will be solved through parallel computing, colored in blue in Fig. 1(a). Then the best open-loop predicted solution for the $r$-th optimization cycle will be selected. The algorithm will continue doing optimization until either the set $\mathcal{Z}_{r}$ remains unchanged or the maximum cycle number $r_{\text{max}}$ is reached. As a result, the algorithm will select the iLQR’s optimal target terminal state $\mathbf{z}_{r}(j_{r}^{*})$ from historical data in an iterative manner. Details about this algorithm will be illustrated in the following subsections.

III-B Nearest Points Selection

To build the target terminal set $\mathcal{Z}_{r}$, the following criteria will be used to select the $K$-nearest points:

[TABLE]

where $j$ refers to the index of points in the $r$-th optimization cycle; $D_{0}$ is a diagonal matrix that contains weighting factors for state variables.

Remark 1

The number of maximum iterations $r_{\text{max}}$ and the number of selected nearest points $K$ are the hyperparameters of the proposed algorithm. A larger value of $r_{\text{max}}$ or $K$ will make the system converge to the optimal performance more quickly. However, this will also increase the computational burden at each time step.

III-C Local iLQR Optimization

The following constrained finite-time optimal control problem will be solved through iLQR for each $\mathbf{z}_{r}(j)$:

[TABLE]

where (2a) is the optimization problem’s objective function; (2b) represents the system dynamics; (2c) shows the constraints of the system along the prediction horizon; (2d) is the initial constraint. Specifically, the terminal cost introduces the difference between the open-loop predicted terminal state and the target terminal state in the quadratic form:

[TABLE]

where $P$ is a diagonal matrix consisting of weighting factors.

Alg. 2 shows how to solve the above optimization problem through iLQR. System constraints along the prediction horizon will be converted to part of the new cost function $J_{s}(\cdot)$ through the exponential function as done in [18]. Firstly, the algorithm starts with an initial input sequence, such as zero control inputs in this work. Then, $g(\cdot)$ will calculate the open-loop states based on open-loop inputs and initial state through system dynamics (2b) during the forward pass at line 4. It will be linearized along with the cost function $J_{s}(\cdot)$ around $\mathbf{x}_{r}^{m}(j)$ and $\mathbf{u}_{r}^{m}(j)$. The optimal solution $\delta^{*}(\mathbf{u}_{r}^{m}(j))$ could be obtained efficiently and is used to generate the input sequence $\mathbf{u}_{r}^{m+1}(j)$ for the next iteration. The algorithm will do this computation repeatedly until the cost $J_{s}(\cdot)$ has converged or the maximum iteration $m_{\text{max}}$ is reached.

III-D Best Open-Loop Solution Selection

In each optimization cycle, the best open-loop solution will be selected among $K$ solutions. The following local cost will be used in this process:

[TABLE]

where $w_{h}$, $w_{d}$ are weighting factors; $h(.)$ is the cost-to-go associated with the state $\mathbf{z}_{r}(j_{r})$; $||\mathbf{x}^{*}_{r}(j_{r})_{N}-\mathbf{z}_{r}(j_{r})||^{2}_{D_{1}}$ describes the penalty for the state difference between $\mathbf{x}^{*}_{r}(j_{r})_{N}$ and $\mathbf{z}_{r}(j_{r})$ with a diagonal weighting matrix $D_{1}$. Since cost-to-go function $h(.)$ is considered in (4), this allows the iterative optimization to find the best terminal state in the outer loop of i2LQR, shown in red connected lines in Fig. 1(a).

IV Results

Having presented the framework that uses i2LQR for iterative tasks in dynamic environments in the previous sections, we now show the performance of the proposed algorithm. In Sec. IV-A, the simulation setup is introduced. Then, the performance of the proposed controller is compared with state-of-the-art learning-based MPC algorithm for iterative tasks in both static environments and dynamic environments.

IV-A Simulation Setup

A nonlinear kinematic bicycle model with input constraints as in [18] is used to evaluate the proposed algorithm. The kinematic bicycle model has states and inputs at time step $t$ given by $\textbf{x}_{t}=[x_{t},y_{t},v_{t},\theta_{t}]^{T},\textbf{u}_{t}=[a_{t},\delta_{t}]^{T}$, where $x_{t}$ and $y_{t}$ describe the system’s position; $v_{t}$ and $\theta_{t}$ show the system’s speed and heading angle; $a_{t}$ is the acceleration; $\delta_{t}$ represents the steering angle. The sampling time $\Delta t$ is set to 1 second, which is consistent with the open-source code of [13]. The system is subject to the following input constraints -2\text{\,}\mathrm{m}\mathrm{/}\mathrm{{}^{2}}\mathrm{s}$\leq a_{t}\leq$2\text{\,}\mathrm{m}\mathrm{/}\mathrm{{}^{2}}\mathrm{s}, $-\frac{\pi}{2}$\text{,}\mathrm{rad}$\leq\delta_{t}\leq\frac{\pi}{2}$\text{,}\mathrm{rad}$$ with the initial state $\textbf{x}_{0}$ and the target state $\textbf{x}_{\text{target}}$ as $[$0\text{,}\mathrm{m}$,$0\text{,}\mathrm{m}$,$0\text{,}\mathrm{m}\mathrm{/}\mathrm{s}$,$0\text{,}\mathrm{rad}$]^{T}$ and $[$201.5\text{,}\mathrm{m}$,$0\text{,}\mathrm{m}$,$0\text{,}\mathrm{m}\mathrm{/}\mathrm{s}$,$0\text{,}\mathrm{rad}$]^{T}$ for each iteration, respectively. In iteration 0, a brute force algorithm [13] is used to calculate the initial feasible trajectory, which is used by both i2LQR and LMPC algorithms for all simulations. In iteration 1, both algorithms use the historical data from iteration 0, and historical data from the two previous iterations are used by both algorithms in subsequent iterations. In this work, numerical simulation is carried out in Python. For the LMPC algorithm, CasADi [20] is used as modeling language and the resulting optimization is solved with IPOPT [21].

IV-B Iterative Tasks In Static Environments

This subsection compares the performance of our proposed i2LQR and LMPC for iterative tasks in static environments.

In the first group of simulations, no obstacle exists in the environment. In each iteration, the system travels from the initial state $\textbf{x}_{0}$ to the target state $\textbf{x}_{\text{target}}$. The simulation results using the two algorithms are shown in Fig. 3. It indicates that both algorithms minimize the system’s completion time to reach the target state and have the same optimal performance. Specifically, given the same initial trajectory, the trajectories in iteration 10 are almost straight lines between the initial and target state for both algorithms. The system accelerates for the first half of the simulation and then decelerate to reach the target state with zero velocity in the second half for both algorithms in iteration 10.

Remark 2

The acceleration profile is not exactly symmetric since the the sampling time $\Delta t$ is set to $1\text{\,}\mathrm{s}$ which is quite large and cannot be considered as a continuous system anymore. All this could result in multiple optimal trajectories for the same completion time.

In the second group of simulations, an ellipse-shaped static obstacle with center $(x_{\text{obs}},y_{\text{obs}})=(100\text{ m},-5\text{ m})$ exists in the environment. The system must travel from the state $\textbf{x}_{0}$ to the state $\textbf{x}_{\text{target}}$ while avoiding this obstacle. Simulation results using the two algorithms are shown in Fig. 4.

It’s shown that both i2LQR and LMPC algorithms minimize the completion time for the system to reach the target state even when a static obstacle exists in the environment. Given the same initial trajectory, both algorithms can find the optimal trajectory that avoids the obstacle. In iteration 10, the system accelerates for the first half of the simulation and then decelerate to reach the target state with zero velocity.

IV-C Iterative Tasks In Dynamic Environments

To show the proposed i2LQR algorithm’s performance for iterative tasks in dynamic environments, we conduct two groups of simulations with different environments.

In the third group of simulations (Fig. 5), a static circle-shaped obstacle with center $(x_{\text{obs}},y_{\text{obs}})=(35\text{ m},0\text{ m})$ exists in the environment for iteration 6. According to Fig. 5(b), systems with the both algorithms have reached their optimal performance before the static obstacle is introduced. During iteration 6, i2LQR spends $25\text{\,}\mathrm{s}$ to reach the target state while avoiding the static obstacle. Then, it returns to its optimal performance after the obstacle is removed. However, the LMPC cannot reach the target state after more than $100\text{\,}\mathrm{s}$. The reason is that all the nearby historical states except the initial state are occupied by the obstacle, which results in the infeasibility of the optimization problem.

To further present the proposed i2LQR algorithm’s performance in a more complicated dynamic environment, in the fourth simulation, a circle-shaped moving obstacle moves upwards from the initial point $(x_{\text{obs}},y_{\text{obs}})=(35\text{ m},-16\text{ m})$ with a speed of 1 m/s in iteration 6. The obstacle is removed in the next iteration. Fig. 6(a) and Fig. 6(b) show the snapshots and trajectories for both i2LQR and LMPC algorithms in iteration 6, respectively. It’s shown that i2LQR is able to avoid this moving obstacle even when the obstacle is close to the system. However, since historical states with smaller time costs are occupied by the obstacle, these states become infeasible for the local MPC optimization of the LMPC algorithm; therefore, the controller cannot drive the system towards the target state at the beginning. After the obstacle goes away from the system, it moves towards the target state. The i2LQR spends $32\text{\,}\mathrm{s}$ to finish iteration 6, while the LMPC needs $63\text{\,}\mathrm{s}$ to finish this in the same environment. Both algorithms return to their optimal performance after the moving obstacle is removed from the environment.

Remark 3

It’s possible to get a solution by adding slack variables to the terminal state constraint of LMPC in [13]. This converts the hard constraints on the terminal state into cost-based soft constraints. However, using slack variables is not in line with the design of the LMPC algorithm, which relies on the feasibility of the terminal state. Furthermore, this may not guarantee the algorithm’s performance.

V Conclusion

This work introduces the Iterative Linear Quadratic Regulator for Iterative Tasks (i2LQR) control strategy, which enhances performance in dynamic environments through local trajectory optimization using historical data. Four simulations are conducted: i2LQR matches state-of-the-art LMPC in static environments and outperforms it in changing environments. Future work will include stability and feasibility analysis of this controller.