Simultaneous active parameter estimation and control using sampling-based Bayesian reinforcement learning
Patrick Slade Preston Culbertson Zachary Sunberg Mykel Kochenderfer
cs.SY


Bandit Convex Optimization for
 Scalable and Dynamic IoT Management
    
Tianyi Chen and Georgios B. Giannakis
Work in this paper was supported by NSF 1509040, 1508993, and 1711471.

T. Chen and G. B. Giannakis are with the Department of Electrical and Computer Engineering and the Digital Technology Center, University of Minnesota, Minneapolis, MN 55455 USA. Emails: {chen3827, georgios}@umn.edu




    December 30, 2023
=============================================================================================================================================================================================================================================================================================================================================

empty
empty




Robots performing manipulation tasks must operate under uncertainty about both their pose and the dynamics of the system. In order to remain robust to modeling error and shifts in payload dynamics, agents must simultaneously perform estimation and control tasks. However, the optimal estimation actions are often not the optimal actions for accomplishing the control tasks, and thus agents trade between exploration and exploitation. This work frames the problem as a Bayes-adaptive Markov decision process and solves it online using Monte Carlo tree search and an extended Kalman filter to handle Gaussian process noise and parameter uncertainty in a continuous space. MCTS selects control actions to reduce model uncertainty and reach the goal state nearly optimally. Certainty equivalent model predictive control is used as a benchmark to compare performance in simulations with varying process noise and parameter uncertainty.







§ INTRODUCTION



 §.§ Motivation


Flexibility and robustness in real-world robotic systems is essential to making them human-friendly and safe. Planning for important tasks in robotics, such as localization and manipulation, requires an accurate model of the dynamics <cit.>. However, the dynamics are often only partially known. For example, order-fulfillment robots move containers with varying loads in warehouses  <cit.>, autonomous vehicles traverse variable terrain and interact with human drivers <cit.>, and nursing robotic systems use tools and interact with people <cit.>. Payload shifts, environment conditions, and human decisions can dramatically alter the system dynamics. Even for motion determined by physical laws, there are often unknown parameters like friction and inertial properties. 

In these cases, the robot must estimate the system dynamics to achieve its goals. Unfortunately, this estimation task often conflicts with the original goal task. The robot must balance exploration to gain knowledge of the dynamics and exploitation of its current knowledge to obtain rewards.



 §.§ Related Work


Often this is solved with certainty equivalent control where a robot plans assuming an exact  dynamics model<cit.>. This is usually the most likely or mean dynamics model. Typically, this dynamics model will update every time it receives an observation and make a new plan up to a fixed horizon that is optimal for the updated dynamics model. This approach is known as model predictive control (MPC) <cit.>. In some cases, performance can improve if a range of dynamic models are considered. This approach is known as robust MPC <cit.>. These approaches work well in many cases, however they do not encourage exploration, and are thus suboptimal for some problems <cit.>.

There have been a variety of principled approaches to handle the exploration-exploitation trade-off. One body of research has referred to this challenge as the “dual control” problem <cit.>. It was shown that an optimal solution could be found using dynamic programming (DP). However, since the state, action, and belief spaces are continuous, the exact solution is generally intractable. Approximate solutions include a variety of methods such as adaptive control, sliding mode control, and stochastic optimal control <cit.>. Adaptive controllers typically first perform an estimation task to reveal unknown parameters and then perform the control task. This is suboptimal, the agent could potentially use the estimation actions to also begin performing the control task.

Another popular approach is reinforcement learning (RL) <cit.>. In RL, the underlying planning problem is a discrete or continuous Markov decision process (MDP) with unknown transition probability distributions. RL agents interact with the environment to accrue rewards and may learn the transition probabilities along the way if it helps in this task. If the prior distribution of these transition probabilities is known, the policy that will collect the most reward in expectation, optimally balancing exploration and exploitation, may be calculated by solving a Bayes-adaptive Markov decision process (BAMDP) <cit.>. Unfortunately BAMDPs are, in general, computationally intractable, so approximate methods are used <cit.>. Since any BAMDP can be recast as a partially observable Markov decision process (POMDP), similar approximate solution methods are used. Both offline <cit.> and online <cit.> POMDP methods have been used for RL in discrete state spaces, and others <cit.> are easily adapted.

There has been limited work on continuous spaces. Value iteration has been used for problems with Gaussian uncertainty <cit.>. Controls are locally optimized along a pre-computed trajectory as an extended Kalman filter (EKF) updates the belief over the model parameters. Exploration is encouraged by heuristically penalizing model uncertainty in the reward function. The control designer chooses how much to explore rather than the algorithm optimally calculating it.



 §.§ Contribution


This research applies Monte Carlo tree search (MCTS) to solve a close approximation to the BAMDP for a problem with continuous state, action, and observation spaces, an arbitrary reward function, Gaussian process noise, and Gaussian uncertainty in the model parameters. An EKF updates the belief of the unknown parameters and double progressive widening (DPW) <cit.> guides the expansion of the tree in continuous spaces. <Ref> gives an introduction to the problems and methods considered, <Ref> give detailed descriptions of the problem and approach, and <Ref> shows simulations comparing MCTS with a certainty-equivalent MPC benchmark for two problems.



§ BACKGROUND
 
This section reviews techniques underlying our work: sequential decision making models, MCTS, and EKFs.


 §.§ MDPs, POMDPs, BAMDPs

	A Markov decision process is a mathematical framework for a sequential decision process in which an agent will move, typically stochastically, between states over time, accruing various rewards for entering certain states, but may affect their trajectory and rewards by taking actions at each time step. A MDP is defined by the tuple (𝒮,𝒜,T,R,γ), where:


  * 𝒮 is the set of states the system may reach,

  * 𝒜 is the set of actions the agent may take,

  * T(s'| s,a) is the probability of transitioning to state s' by taking action a at state s,

  * R(s,a) is the reward (or cost) of taking action a at state s, and

  * γ∈ [0,1] is the discount factor for future rewards.


Solving an MDP develops a policy, π(s):𝒮→𝒜, which maps each state to an optimal action that will accrue the most rewards in expectation over some planning horizon.

For infinite-horizon, discounted MDPs, the optimal policy has been shown to satisfy the Bellman equation <cit.>.

For small, discrete state and action spaces, the Bellman equation may be solved explicitly with DP. However, for problems with large or continuous state and action spaces approximate solutions use approximate DP (ADP) <cit.>. Next, we discuss MCTS, the online ADP algorithm applied in this work.

A partially observable MDP (POMDP) is an extension of an MDP, where the agent cannot directly observe its true state, only receiving observations which are stochastically dependent on this state. A POMDP thus contains a model of the probability of seeing a certain observation at a specific state. The agent forms a belief state, b which encodes the probability of being in each state s. The agent updates its belief at each step, depending on its previous action, the reward received, and the observation it took. Since the agent may hold any combination of beliefs about its location in the state space, the belief space ℬ is continuous, making POMDPs computationally intensive to solve.

A Bayes-adaptive MDP (BAMDP) has transition probabilities that are only partially known. That is, the decision-making agent initially only knows a prior distribution of the transition probabilities. As the agent interacts with the environment, it extracts information of the transition probabilities from the history of states and actions that it has visited. A BAMDP becomes a POMDP by augmenting the state with the unknown parameters defining the transition probabilities.

A POMDP is actually an MDP where the state space is the belief space of the original POMDP <cit.>, sometimes called a belief MDP. The transition dynamics of the belief MDP are defined by a Bayesian update of the belief when an action is taken and an observation received. Not all belief MDPs are POMDPs; in a POMDP, the reward function is specified only in terms of the POMDP state and action and is not a general function of the belief, so penalties cannot be explicitly assigned to uncertainty in the belief. When the belief update is computationally tractable, ADP techniques designed for MDPs may be applied to POMDPs by applying them to the corresponding belief MDP. 



 §.§ Monte Carlo Tree Search


MCTS is a sampling-based online approach for approximately solving MDPs. MCTS uses a generative model G to get a random state and reward (s',r) ∼ G(s,a). It performs a forward search through the state space, using G to draw prospective trajectories and rewards. In MCTS, a tree is created with alternating states and actions, with a roll-out performed to select a policy <cit.>.
In general, MCTS involves stages:
selection, expansion, rollout, and propagation <cit.>.




 §.§ Extended Kalman Filter

	In problems with linear Gaussian dynamics and observation functions, it has been shown that the optimal observer is a Kalman filter. A Kalman filter is an iterative algorithm that can exactly update Gaussian beliefs over the state, given the action taken, the observation received, and the transition and observation models <cit.>.
    
    For systems with nonlinear dynamics, an EKF may be used to approximately update the beliefs with each step <cit.>. Let x_k, a_k, and o_k be the state, action, and observation at time t=k. For a system with nonlinear-Gaussian dynamics, the transition and observation models can be expressed as


    x_k+1 = f(x_k,a_k) + v_k



    o_k = h(x_k,a_k) + w_k,


where f, h are nonlinear functions and v, w are normally distributed. The Gaussian belief has mean x̂ and covariance Σ. The EKF updates the belief at each timestep by predicting a new state and covariance based on the action taken, i.e.


    x̂_k| k-1 = f(x̂_k-1| k-1,a_k-1)



    Σ_k| k-1 = F_k-1Σ_k-1 | k-1F_k-1^T + Q_k-1,


where F is the linearized dynamics about the current belief.


    F_k-1 = . ∂ f/∂ x |_x̂_k-1 | k-1,u_k-1


A residual between the observation and predicted state is:


    r_k = o_k - h(x̂_k | k-1,a_k-1).


The residual covariance, S_k and Kalman gain, K_k are


    S_k = H_k Σ_k | k-1 H_k^T + R_k



    K_k = Σ_k | k-1H_k^T S_k^-1


where H_k is the linearized observation function.


    H_k = . ∂ h/∂ x |_x̂_k | k-1,u_k-1


Finally, the belief update is completed according to


    x̂_k | k = x̂_k | k-1 + K_k r_k



    Σ_k | k = (I-K_k H_k)P_k | k-1.


Due to the linearization of the dynamics and observation models, EKFs will not always converge and are generally not optimal observers. However, in practice they perform  well for most systems where the true belief is unimodal.



§ PROBLEM FORMULATION
 
Let us consider a robot trying to control a system with linear-Gaussian dynamics. The system is described by

    x_k+1 = f(x_k,u_k;p) + v_k,


where x_k, u_k are the state and control action at time t=k, p is a set of parameters that the system dynamics depend on, the process noise v_k ∼𝒩(0,Q), and f is a time-invariant function that is linear with respect to x_k and u_k,


    f(x_k,u_k;p) = A(p)x_k + B(p)u_k.


The observation model is described by the linear equation


    o_k = h(x_k, u_k;p) + w_k,


with observation, o_k taken at time t=k, the measurement noise w_k ∼𝒩(0,R), and h is a time-invariant linear function


    h(x_k,u_k;p) = C(p)x_k+D(p)u_k.


While f and h are physical equations known a priori, the parameters p are not known beforehand. They can be appended to the state vector to form a state-parameter vector,


    s_k = [ [ x_k;   p ]].


Thus, the system dynamics for s_k may be described by


    s_k+1 = [ [ A(p)    0;    0    I ]] s_k + [ [ B(p);    0 ]] u_k + ṽ_k,


where ṽ∼𝒩(0,(Q,P)) and P is a parameter drift matrix.

The observation model is described by


    o_k = [ [ C(p)    0 ]]s_k + D(p)u_k + w_k.




	If we use an EKF to describe the belief about the current state in the state-parameter space, we form a belief MDP over all possible EKF states. This belief MDP is described by the tuple (𝒮,𝒜,T,R), where:


  * 𝒮 is the space of all possible beliefs. Since the belief maintained by the EKF is Gaussian, it can be described by the mean and covariance, b_k = (ŝ_k,Σ_k).

  * 𝒜 is all possible actions that the agent may take.

  * T(b' | b,a) is a distribution over possible EKF states after a belief update. This distribution depends on the observation model. It is difficult to represent explicitly, so it is implicitly defined by the generative model, G.

  * R(b,a) is a reward function for a given belief and action. It is constructed as desired for a given control task. In our work, we approximate R(b,a) = R(ŝ_k,a), a linear reward for the estimated mean state and action.


The generative model for the belief MDP is

    b_k+1 = G(b_k, a_k)

with G defined by equations (<ref>), (<ref>), (<ref>), (<ref>), and (<ref>). The observation, o_k, used in (<ref>) is a random variable determined by (<ref>) and (<ref>); it is not the most likely observation. Solving this belief MDP gives a policy that optimally maximizes the sum of expected rewards over some planning horizon.



§ APPROACH
 

This section discusses the use of MCTS and MPC with an EKF to control a system with unknown parameters.



 §.§ MCTS


Our approach uses the upper confidence tree (UCT) <cit.> and DPW <cit.> extensions of MCTS. The tree is built as UCT expands action nodes maximizing an upper confidence estimate


    UCB(s, u) = Q̃(s,u) + c √(log N(s)/N(s,u)),


where Q̃(s,u) estimates the state-action value function from rollout simulations and tree search, N(s,u) counts the times action u is taken from state s, with exploration constant, c. This balances exploration-exploitation as the tree expands.

DPW defines tree growth for large or continuous state and action spaces. To avoid a shallow search, the number of children of each state-action node (s, u) is limited to


    k N(s,u)^α,


where k and α are parameter constants tuned to control the widening of the tree. With an increase in N(s, u) the number of children also grows, widening the tree. The number of actions explored at each state is limited similarly.



 §.§ Model Predictive Control


MPC is a technique for online calculation of a policy <cit.>. Its extensive use in control system design is due to its ability to explicitly meet state and control constraints. For our implementation of MPC, the optimization problem is constrained by the dynamics, f(x_k, u_k, p), and the maximum control effort, u_max. At each step a series of control actions to maximize a reward function are found for a fixed horizon from the current state and the first action is taken <cit.>.



 §.§ Basic Approach


A large number of nonlinear stochastic systems can be near-optimally controlled with an EKF and MCTS or MPC. The purpose of this method is to produce and execute a control policy. Any system described by a model in the form of (<ref>) with an approximately Gaussian initial belief state can use MCTS or MPC to find a suitable control action.

Taking this action propagates the true state, s, which will be partially observed. This observation and action will update the belief state with the EKF, improving our estimates about the parameters. This process is shown in Algorithm <ref>.







 §.§ Implementation Details


Measurement noise was removed from the tests so the effects of process noise and estimated parameter uncertainty were isolated. The magnitude of control inputs was limited to u_max (see Table <ref>). Filtering the control signal is recommended for experimental validation to not damage actuators.

A linear reward function was implemented with a weighted L1 norm penalty for the position, speed, and control effort


    R_L1(x_k,u_k) = [[ R_pos     0;     0 R_vel ]] x_k + R_u u_k .


The values for R_pos, R_vel, and R_u are given in Table <ref>. For performance comparison purposes a quadratic reward function is used with a L2 norm penalty for the same terms


    R_L2(x_k,u_k) = x_k^T [[ R_pos     0;     0 R_vel ]] x_k + u_k^T R_u u_k.


Real-world constraints were included as a minimum threshold for physical values such as mass, friction, and inertia so the dynamic models held. The MCTS rollout used a position controller to select a force proportional to the distance from the goal state. The MCTS with DPW implementation is from the POMDPs.jl package <cit.>. The optimization in the MPC controller was solved with the Convex.jl package <cit.>.
    


§ SIMULATION AND RESULTS
 

Models of a 1D double integrator and robot performing planar manipulation tested our MCTS approach against MPC, a certainty equivalent optimal control benchmark.



 §.§ 1D Double Integrator Model


We first considered the control problem where an agent applies force to a point mass in one dimension with dynamics


    x_k+1 = [ [   1   0; Δ t   1 ]] x_k + [ [      Δ t/m; ( Δ t/m)^2 ]] f_k + v_k



    o_k = [ [ 1 0; 0 1 ]]x_k + w_k.


Measurement noise is 0 to reduce the number of parameters.

Fig. <ref> gives physical intuition to the 1D double integrator model. The point mass starts at a given position and velocity with a goal state at the origin. The state profiles for MCTS appear smoother than MPC. This is due to MPC applying large forces for short duration caused by the large penalty an L1 reward function gives small errors.





 §.§ Planar Manipulation (PM) Model

We then consider a robot R pushing a box B in the plane, where the agent may apply an arbitrary force in the x- and y-directions, in addition to a torque. This problem, with its related quantities, is illustrated in Figure <ref>.



We can describe the dynamics of the system about its center of mass, B_cm. These are given by


    ∑F⃗_B = F⃗ - μ_v v⃗ =  m a⃗



    ∑T⃗_B= T⃗ + r⃗×F⃗ = J α⃗,


where μ_v is linear friction. The system in discrete-time is


    v_k+1 = a_k  Δ t+ v_k
    
    	p_k+1 = v_k Δ t + p_k,


where Δ t is the discretization step. The system can be put in state-space form with the state vector x_k = [ [ p_x,k, p_y,k,   θ_k, v_x,k, v_y,k,    ω_k ]]^T, corresponding to the linear and angular positions and velocities of B in the global frame N. Using (<ref>) and (<ref>) and an explicit-time integration


    x_k+1 = f(x_k,u_k) = A x_k 
     + B(θ) u_k + v_k,


where


    A = [ [           𝕀^3x3        𝕀^3x3Δ t;                ; [-0.8em]
     0^3x3           𝕀^3x3 ]],



    B=  [[ 3c 0^3x3;         ;    Δ t/m        0        0;        0    Δ t/m        0;    B^3,1    B^3,2    Δ t/J ]],



    B^3,1 =  Δ t/J(cos(θ)r_y + sin(θ)r_x),



    B^3,2 =  Δ t/J(cos(θ)r_x - sin(θ)r_y),



    u_k = [ [ F_x,k; F_y,k;   T_k ]],


where m is the mass of B and J is B's moment of inertia.

For a robot with noisy sensors which measure its position, velocity, and acceleration in N, the observation model is

    y_k = h(x_k,u_k) + w_k,


where y_k = [ p_x,k,  p_y,k,  θ_k,  v_x,k,  v_y,k,  ω_k,  a_x,k,  a_y,k,  α_k ]^T.

The measurement functions are given by


    p⃗   = r⃗_cm + r⃗_b 
    v⃗   = v⃗_cm + ω⃗×r⃗_b
    α⃗   = ∑T⃗_B/J
    a⃗   = ∑F⃗_⃗B⃗/m+ α⃗×r⃗_b + ω⃗×(ω⃗×r⃗_b).


This model uses the same stationary goal at the origin with an orientation of 0 degrees. It is given an initial position and orientation with Cartesian and angular velocities.



 §.§ Results


The simulations performed on both models compare the rewards for MCTS and MPC for two varied parameters: process noise and initial parameter uncertainty. Fig. <ref> compares rewards of MCTS and MPC for the 1D double integrator model while varying process noise. The simulation parameters are detailed in Table <ref>. While MPC and MCTS perform similarly for small process noise, the reward accrued by MPC decreases significantly faster as process noise increases. Process noise greater than 1.0 has little impact on the reward, indicating saturation when the process noise is so large the policy cannot reach the goal region. 30 trials were run for each case with standard error of the mean indicated by the error bars. 

The standard deviation of the initial parameter estimate was then varied for a constant process noise with variance 3.0. This process noise level was chosen because it highlights the rapid change in reward caused by a poorer initial mass estimate. Fig. <ref> shows the MCTS reward mostly unaffected by this uncertainty, while the MPC reward decreases rapidly. This highlights the advantage of exploration-exploitation from MCTS. Searching for actions using multiple estimates of the mass and moving towards the goal allows it to get better knowledge of the system and increase reward. MPC takes the first action of an open-loop certainty-equivalent plan recalculated every step.

Another interesting topic to consider is the performance of MCTS and MPC considering parameter uncertainty with different reward functions. 
The relative rewards of MCTS and MPC with linear (L1) and quadratic (L2) reward functions are shown in Fig. <ref>. 
In both cases the process noise had variance 1.0 and the initial mass estimate had variance 10.0.
While MCTS performs significantly better with an L1 reward, it performs slightly worse with an L2 reward.
These results indicate that, with an L2 reward in these domains, taking uncertainty into account with MCTS is not beneficial.
This is due to the L2 reward function placing a small penalty on small errors, allowing MPC to perform well without considering parameter uncertainty.
In another study with a quadratic state reward <cit.>, the reward function is augmented with a term penalizing parameter uncertainty.
We suspect that, without this explicit stimulus to explore rather than simply exploit, planning algorithms would not need to actively gather information about the parameters to perform well in domains with quadratic reward.
A chief advantage of the POMDP approach is that the solver automatically gathers information necessary to maximize the reward. Explicit penalization of uncertainty should not be necessary if the state reward function is chosen to accurately reflect the  performance requirements.
It should be noted that convex reward functions were used to allow easy comparison with MPC, but there is no need for the reward function used by MCTS to be convex. Designers can use reward functions that precisely prescribe the desired behavior.









The simulation for varying process noise in the PM model is displayed in Fig. <ref>. It behaves like the 1D model; MCTS has higher reward for all noise levels. As iterations per step decreases, MCTS tracks closer to MPC. This smaller reward difference than in the 1D case indicates the EKF has trouble reducing uncertainty for five estimated parameters in a non-linear and time-varying system. Thus, rewards decrease with increasing process noise for both controllers.

For a constant process noise with standard deviation 0.1, again where MCTS and MPC have a large difference in reward, the standard deviation of the initial parameter estimate is varied for the PM model in Fig. <ref>. As the uncertainty of the initial parameters increases, the reward of MPC decreases at a slower rate than seen in the 1D double integrator model. This indicates difficulty stabilizing and reaching the goal for any process noise. The large standard deviations of the initial parameter estimations negatively impact MPC reward while MCTS is unaffected. For real-world tasks like warehouse robots manipulating varying loads, MCTS performs better.





These results indicate MPC's open-loop planning executed in a closed-loop fashion cannot estimate parameters or system propagation as well as MCTS for even simple models with substantial noise. In terms of real-time performance, MCTS using 10 iterations per step ran with an average of 0.156 seconds per step. All simulations were run on an i7 processor. This runtime depends on implementation and hardware, but the same order of magnitude run-time indicates MCTS could be used on systems in real-time. Increasing the number of iterations per step to improve performance when computation time is available provides flexibility onboard.



§ CONCLUSIONS

This paper posed the control of a robot performing a manipulation task in 1D and 2D for a payload with unknown parameters as a BAMDP. An online, sampling-based approach, MCTS, was used to approximately solve these continuous control problems with Gaussian uncertainty. Empirically, it was shown that this approach effectively balances exploration and exploitation. Even with few samples, MCTS improved parameter estimation and exploitation of the system dynamics to reach a goal state. In simulations with large process and parameter uncertainty, this approach provides policies with significantly higher reward than the commonly used MPC. MCTS algorithms may be a promising way to address estimation and control for real-world autonomous systems.



§ ACKNOWLEDGEMENTS

	
    The authors thank Maxime Bouton for help with the EKF. 
        
ieeetr






