PatternGeneratorJRL::OptimalControllerSolver Class Reference

This class computes the gains for preview control for a given discrete system. The discrete system is defined by three matrix A, b, c such as : More...

#include <PreviewControl/OptimalControllerSolver.hh>

Public Member Functions
	OptimalControllerSolver (Eigen::MatrixXd &A, Eigen::MatrixXd &b, Eigen::MatrixXd &c, double Q, double R, unsigned int Nl)
	~OptimalControllerSolver ()
void	ComputeWeights (unsigned int Mode)
void	DisplayWeights ()
void	GetF (Eigen::MatrixXd &LF)
void	GetK (Eigen::MatrixXd &LK)

Static Public Attributes
static const unsigned int	MODE_WITHOUT_INITIALPOS = 1
static const unsigned int	MODE_WITH_INITIALPOS = 0

Protected Types
typedef Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor >	MatrixRXd

Protected Member Functions
bool	GeneralizedSchur (MatrixRXd &A, MatrixRXd &B, Eigen::VectorXd &alphar, Eigen::VectorXd &alphai, Eigen::VectorXd &beta, MatrixRXd &L, MatrixRXd &R)

Protected Attributes
MatrixRXd	m_A
MatrixRXd	m_b
MatrixRXd	m_c
double	m_Q
double	m_R
MatrixRXd	m_K
MatrixRXd	m_F
int	m_Nl

Detailed Description

This class computes the gains for preview control for a given discrete system. The discrete system is defined by three matrix A, b, c such as :

\begin{eqnarray*} {\bf x}_{k+1} & =& {\bf A} x_k + {\bf b} u_k \\ p_k &=& {\bf cx}_k\\ \end{eqnarray*}

The optimal critera considered here is :

\[ J = \sum^{\infty}_{j=1} \{ Q(p^{ref}_j -p_j)^2 + Ru_j^2 \} \]

where \( Q \) and \( R \) are also given as inputs.

the solution is then:

\begin{eqnarray*} u_j = - {\bf K}x_k + [ f_1, f_2, \cdots, f_N] \left[ \begin{matrix} p^{ref}_{k+1} \\ \vdots \\ p^{ref}_{k+N} \end{matrix} \right] \end{eqnarray*}

\begin{eqnarray*} {\bf K} & \equiv & (R + {\bf b}^T{\bf Pb})^{-1}{\bf b}^T{\bf PA} \\ K_p(i) & \equiv & (R + {\bf b}^T{\bf Pb})^{-1}{\bf b}^T({\bf A}- {\bf bK})^{T*(i-1)}{\bf c}^TQ \ \ \end{eqnarray*}

where \( {\bf P} \) is solution of the following Riccati equation:

\[ {\bf P} = {\bf A}^T {\bf PA} + {\bf c}^TQ{\bf c} - {\bf A}^T{\bf Pb}(R + {\bf b}^T{\bf Pb})^{-1}{\bf b}^T{\bf PA} \]

The resolution of the Riccati equation is taken from Laub1979, and is based on a Schur form .

To suppress the problem of the initial CoM position, we can reformulate the discrete problem by posing the following:

\begin{eqnarray*} \begin{matrix} {\bf x}^*_{k+1} &= \widetilde{\bf A} {\bf x}^*_{k} + \widetilde{\bf b}\Delta u_k \ \ p_k &= \widetilde{\bf c}{\bf x}^*_{k} \end{matrix} \end{eqnarray*}

with

\begin{eqnarray*} \Delta u_k \equiv u_k - u_{k-1} & \Delta {\bf x}_k \equiv {\bf x}_k - {\bf x}_{k-1}\ \ {\bf x}_k \equiv \left[ \begin{matrix} p_k\\ \Delta {\bf x}_k \end{matrix} \right] \end{eqnarray*}

The augmented system is then

\begin{eqnarray*} \widetilde{\bf A} &\equiv & \left[ \begin{matrix} 1 & {\bf cA} \\ {\bf 0} & {\bf A} \\ \end{matrix} \right] \\ \tilde{\bf b} & \equiv & \left[ \begin{matrix} {\bf cb} \\ {\bf c} \end{matrix} \right] \\ \tilde{\bf c} & \equiv & [ 1 \; 0 \; 0 \; 0] \\ \end{eqnarray*}

with the following cost function:

\[ J = \sum^{\infty}_{j=1} \{ Q(p^{ref}_j -p_j)^2 + R \Delta u_j^2 \} \]

the solution is then:

\begin{eqnarray*} u_j = - K_1 \sum_{i=0}^k e(i) - {\bf K}_2 x(k) - \sum_{j=1}^{N_L} K_p(j)p^{ref}_j(k+j) \end{eqnarray*}

where

\begin{eqnarray*} \left[ \begin{matrix} K_1 \\ {\bf K}_2 \\ \end{matrix} \right]= \widetilde{\bf K} \end{eqnarray*}

Alan J. Laub A Schur method for solving Algebraic Riccati Equations, IEEE Transaction on Automatic Control, Vol AC-24, No.6 December 1979

Member Typedef Documentation

MatrixRXd

typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> PatternGeneratorJRL::OptimalControllerSolver::MatrixRXd

protected

Constructor & Destructor Documentation

OptimalControllerSolver()

OptimalControllerSolver::OptimalControllerSolver	(	Eigen::MatrixXd &	A,
		Eigen::MatrixXd &	b,
		Eigen::MatrixXd &	c,
		double	Q,
		double	R,
		unsigned int	Nl
	)

A constructor

~OptimalControllerSolver()

OptimalControllerSolver::~OptimalControllerSolver

(

)

Destructor.

Member Function Documentation

ComputeWeights()

void OptimalControllerSolver::ComputeWeights

(

unsigned int

Mode

)

Compute the weights Following the mode, there is a the inclusion of the P matrix inside the weights.

DisplayWeights()

void OptimalControllerSolver::DisplayWeights

(

)

Display the weights

GeneralizedSchur()

bool OptimalControllerSolver::GeneralizedSchur ( MatrixRXd &  A, MatrixRXd &  B, Eigen::VectorXd &  alphar, Eigen::VectorXd &  alphai, Eigen::VectorXd &  beta, MatrixRXd &  L, MatrixRXd &  R  )

protected

GetF()

void OptimalControllerSolver::GetF

(

Eigen::MatrixXd &

LF

)

To take matrix F aka the weights of the preview window .

GetK()

void OptimalControllerSolver::GetK

(

Eigen::MatrixXd &

LK

)

To take matrix K, aka the weight of the other part of the command

Member Data Documentation

m_A

MatrixRXd PatternGeneratorJRL::OptimalControllerSolver::m_A

protected

The matrices needed for the dynamical system such as

\begin{eqnarray*} {\bf x}_{k+1} & =& {\bf A} x_k + {\bf b} u_k \\ p_k &=& {\bf cx}_k\\ \end{eqnarray*}

m_b

MatrixRXd PatternGeneratorJRL::OptimalControllerSolver::m_b

protected

m_c

MatrixRXd PatternGeneratorJRL::OptimalControllerSolver::m_c

protected

m_F

MatrixRXd PatternGeneratorJRL::OptimalControllerSolver::m_F

protected

m_K

MatrixRXd PatternGeneratorJRL::OptimalControllerSolver::m_K

protected

The weights themselves

m_Nl

int PatternGeneratorJRL::OptimalControllerSolver::m_Nl

protected

The size of the window for the preview

m_Q

double PatternGeneratorJRL::OptimalControllerSolver::m_Q

protected

The coefficent of the index criteria:

\[ J = \sum^{\infty}_{j=1} \{ Q(p^{ref}_j -p_j)^2 + Ru_j^2 \} \]

m_R

double PatternGeneratorJRL::OptimalControllerSolver::m_R

protected

MODE_WITH_INITIALPOS

const unsigned int PatternGeneratorJRL::OptimalControllerSolver::MODE_WITH_INITIALPOS = 0

static

MODE_WITHOUT_INITIALPOS

const unsigned int PatternGeneratorJRL::OptimalControllerSolver::MODE_WITHOUT_INITIALPOS = 1

static

---

The documentation for this class was generated from the following files:

• src/PreviewControl/OptimalControllerSolver.hh
• src/PreviewControl/OptimalControllerSolver.cpp