A new type of anti-slip controller based on TS fuzzy models

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers ANTI_SLIP_FUZZY_CONTROL.cpp Source File

ANTI_SLIP_FUZZY_CONTROL.cpp

00001 #include "ANTI_SLIP_FUZZY_CONTROL.h"
00002 
00003 //------------------------------------------//
00004 // The template for building a library
00005 //   for control system apllication
00006 //------------------------------------------//
00007 
00008 // The plant is a (p, n, q) system
00009 // Dimensions:
00010 //
00011 // Inputs, u   |   States, x    |   outputs, y
00012 //     p     -->      n        -->      q
00013 //
00014 
00015 //
00016 // The number of vertex systems is m_vertex.
00017 //
00018 
00019 ANTI_SLIP_FUZZY_CONTROL::ANTI_SLIP_FUZZY_CONTROL(size_t num_state, size_t num_in, size_t num_out, size_t num_vertex, float samplingTime):
00020         n(num_state), p(num_in), q(num_out), m_vertex(num_vertex), Ts(samplingTime),
00021         E_out(num_out, vector<float>(num_state,0.0)),
00022         Nxd(num_state, vector<float>(num_in,0.0)),
00023         Nud(num_in, vector<float>(num_in,0.0)),
00024         ver_K_matrix(num_vertex ,vector<vector<float> >(num_in, vector<float>( (num_state + num_out), 0.0)) ),
00025         SA_r(1.0, 0.0),
00026         SA_l(1.0, 0.0)
00027 {
00028 
00029     // To enble, run *.start() function
00030     enable = false;
00031     // If is_usingFeedForward, Nxd and Nud are used to calculate the x_d and u_d
00032     is_usingFeedForward = false;
00033 
00034 
00035     // Normally, q = p
00036     if (q > p)
00037         q = p;
00038     //
00039     zeros_n.assign(n, 0.0);
00040     zeros_p.assign(p, 0.0);
00041     zeros_q.assign(q, 0.0);
00042     zeros_nPq.assign((n+q), 0.0); // (n+q)
00043     zeros_m_vertex.assign(m_vertex, 0.0);
00044     //
00045     ones_p.assign(p, 1.0);
00046 
00047     // States
00048     // Input signal ---
00049     states = zeros_n;
00050     command = zeros_q; // r, commands, q = p
00051     // Output signal ---
00052     sys_inputs = zeros_p;
00053 
00054     // Internal states ---
00055     ver_u.assign(m_vertex, zeros_p);
00056     sys_outputs = zeros_q;
00057     // Integral state
00058     state_int = zeros_q;
00059     // Total states, [states; state_int]
00060     state_total = zeros_nPq;
00061 
00062     // Equalibrium states for command tracking
00063     x_d = zeros_n;
00064     u_d = zeros_p;
00065 
00066     // The composition ratio of each vertex system
00067     ver_ratio = zeros_m_vertex; // ver_ratio \in R^m_vertex, its values are in [0, 1]
00068     ver_ratio[3] = 1.0; // Ratio for no-slip system
00069 
00070 }
00071 void ANTI_SLIP_FUZZY_CONTROL::start(){
00072     enable = true;
00073 }
00074 void ANTI_SLIP_FUZZY_CONTROL::pause(){
00075     enable = false;
00076 }
00077 void ANTI_SLIP_FUZZY_CONTROL::stop(){
00078     if (!enable){
00079         return;
00080     }
00081     enable = false;
00082     // Reset
00083     reset();
00084 }
00085 void ANTI_SLIP_FUZZY_CONTROL::reset(){
00086     //
00087     // States
00088     // Input signal ---
00089     states = zeros_n;
00090     command = zeros_q; // r, commands, q = p
00091     // Output signal ---
00092     sys_inputs = zeros_p;
00093 
00094     // Internal states ---
00095     // ver_u.assign(m_vertex, zeros_p);
00096     sys_outputs = zeros_q;
00097     // Integral state
00098     state_int = zeros_q;
00099     // Total states, [states; state_int]
00100     state_total = zeros_nPq;
00101 
00102     // Equalibrium states for command tracking
00103     x_d = zeros_n;
00104     u_d = zeros_p;
00105 
00106     // The composition ratio of each vertex system
00107     ver_ratio = zeros_m_vertex; // ver_ratio \in R^m_vertex, its values are in [0, 1]
00108     ver_ratio[3] = 1.0; // Ratio for no-slip system
00109 
00110 }
00111 void ANTI_SLIP_FUZZY_CONTROL::reset_integrator(){ // Reset the state_int only
00112     // Integral state
00113     state_int = zeros_q;
00114 }
00115 // Assign Parameters
00116 void ANTI_SLIP_FUZZY_CONTROL::assign_E_out(float* E_out_in){
00117     // E_out_in is the pointer of a mutidimentional array with size q by n
00118     E_out.assign(q, zeros_n);
00119     //
00120     for (size_t i = 0; i < q; ++i){
00121         for (size_t j = 0; j < n; ++j){
00122             // E_out[i][j] = E_out_in[i][j];
00123             E_out[i][j] = *E_out_in;
00124             E_out_in++;
00125         }
00126     }
00127 }
00128 void ANTI_SLIP_FUZZY_CONTROL::assign_Nxd(float* Nxd_in){
00129     // Nxd_in is the pointer of a mutidimentional array with size n by p
00130     Nxd.assign(n, zeros_p);
00131     //
00132     for (size_t i = 0; i < n; ++i){
00133         for (size_t j = 0; j < p; ++j){
00134             // Nxd[i][j] = Nxd_in[i][j];
00135             Nxd[i][j] = *Nxd_in;
00136             Nxd_in++;
00137         }
00138     }
00139     //
00140     is_usingFeedForward = true;
00141 }
00142 void ANTI_SLIP_FUZZY_CONTROL::assign_Nud(float* Nud_in){
00143     // Nxd_in is the pointer of a mutidimentional array with size p by p
00144     Nud.assign(p, zeros_p);
00145     //
00146     for (size_t i = 0; i < p; ++i){
00147         for (size_t j = 0; j < p; ++j){
00148             // Nud[i][j] = Nud_in[i][j];
00149             Nud[i][j] = *Nud_in;
00150             Nud_in++;
00151         }
00152     }
00153     //
00154     is_usingFeedForward = true;
00155 }
00156 // 1st controller Parameters (no-slip plant, sys_dVs)
00157 void ANTI_SLIP_FUZZY_CONTROL::assign_ver_K_matrix(float* ver_K_matrix_in){
00158     // ver_K_matrix_in is the pointer of a list of mutidimentional array with size p by (n+q)
00159     ver_K_matrix.assign(m_vertex, vector<vector<float> >(p, zeros_nPq) );
00160     //
00161     for (size_t k = 0; k < m_vertex; ++k){
00162         // For each vertex
00163         for (size_t i = 0; i < p; ++i){
00164             for (size_t j = 0; j < (n+q); ++j){
00165                 // (ver_K_matrix[k])[i][j] = ver_K_matrix_in[i][j];
00166                 (ver_K_matrix[k])[i][j] = *ver_K_matrix_in;
00167                 ver_K_matrix_in++;
00168             }
00169         }
00170         //
00171     }
00172     //
00173 }
00174 
00175 
00176 //
00177 void ANTI_SLIP_FUZZY_CONTROL::set_ver_ratio(float ratio_ft_right, float ratio_ft_left){
00178     float one_ratio_ft_right;
00179     float one_ratio_ft_left;
00180     // Input Saturation
00181     ratio_ft_right = SA_r.filter(ratio_ft_right);
00182     ratio_ft_left = SA_l.filter(ratio_ft_left);
00183     //
00184     one_ratio_ft_right = 1.0 - ratio_ft_right;
00185     one_ratio_ft_left = 1.0 - ratio_ft_left;
00186     //
00187     ver_ratio[0] = one_ratio_ft_right   * one_ratio_ft_left;
00188     ver_ratio[1] = ratio_ft_right       * one_ratio_ft_left;
00189     ver_ratio[2] = one_ratio_ft_right   * ratio_ft_left;
00190     ver_ratio[3] = ratio_ft_right       * ratio_ft_left;
00191 }
00192 //
00193 void ANTI_SLIP_FUZZY_CONTROL::iterateOnce(void){
00194 
00195     if(!enable){
00196         return;
00197     }
00198 
00199     //
00200     if (is_usingFeedForward){
00201         get_equilibriumState();
00202         // Get the state_total
00203         get_state_total();
00204         // u = u_d + yc;
00205         sys_inputs = u_d;
00206     }else{
00207         // Get the state_total
00208         get_state_total();
00209         // u = yc;
00210         sys_inputs = zeros_p;
00211     }
00212 
00213 
00214     /*
00215     // Slower solution, record the outout of each vertex controller
00216     sys_inputs = zeros_p;
00217     //
00218     for (size_t k = 0; k < m_vertex; ++k){
00219         ver_u[k] = Get_VectorScalarMultiply( Mat_multiply_Vec(ver_K_matrix[k], state_total), -1.0 );
00220         Get_VectorIncrement(sys_inputs, Get_VectorScalarMultiply( ver_u[k], ver_ratio[k]), false); // +=
00221     }
00222     */
00223 
00224     // Faster solution, no recording the output of each vertex controller
00225     // sys_inputs = zeros_p;
00226     //
00227     for (size_t k = 0; k < m_vertex; ++k){
00228         // sys_inputs -= ver_ratio[k]*(ver_K_matrix[k]*state_total);
00229         Get_VectorIncrement(sys_inputs, Get_VectorScalarMultiply( Mat_multiply_Vec(ver_K_matrix[k], state_total), ver_ratio[k] ), true); // -=
00230     }
00231 
00232     /*
00233     // Add the u_d
00234     if (is_usingFeedForward){
00235         Get_VectorIncrement(sys_inputs, u_d, false); // +=
00236     }
00237     */
00238 
00239     // Integral action
00240     get_integral();
00241 }
00242 
00243 // Private functions
00244 // Calculate the equilibrium states
00245 void ANTI_SLIP_FUZZY_CONTROL::get_equilibriumState(void){
00246     Mat_multiply_Vec(x_d, Nxd, command);
00247     Mat_multiply_Vec(u_d, Nud, command);
00248 }
00249 // Calculate the sys_outputs
00250 void ANTI_SLIP_FUZZY_CONTROL::get_sys_outputs(void){ // Calculate the sys_outputs from states, by mutiplying E_out
00251     // sys_outputs = E_out*states
00252     Mat_multiply_Vec(sys_outputs, E_out, states);
00253     // sys_outputs = Mat_multiply_Vec(E_out, states);
00254 }
00255 // Calculate the Integral
00256 void ANTI_SLIP_FUZZY_CONTROL::get_integral(){ // Calculate the state_int
00257     if(!enable){
00258         return;
00259     }
00260     //
00261     // Calculate the sys_outputs
00262     get_sys_outputs();
00263     //
00264     // Integral action
00265     // state_int += sys_outputs - command
00266     Get_VectorIncrement(state_int, Get_VectorScalarMultiply(Get_VectorPlus(sys_outputs,command,true),Ts) ,false); // +=
00267 }
00268 // Concatenate the state and state_int
00269 void ANTI_SLIP_FUZZY_CONTROL::get_state_total(void){ // Total states, [states; state_int]
00270     //
00271     size_t idx = 0;
00272 
00273     // states
00274     if (is_usingFeedForward){
00275         for (size_t i = 0; i < n; ++i){
00276             state_total[idx] = states[i] - x_d[i];
00277             idx++;
00278         }
00279     }else{
00280         for (size_t i = 0; i < n; ++i){
00281             state_total[idx] = states[i];
00282             idx++;
00283         }
00284     }
00285 
00286     // state_int
00287     for (size_t i = 0; i < q; ++i){
00288         state_total[idx] = state_int[i];
00289         idx++;
00290     }
00291 }
00292 
00293 // Utilities
00294 void ANTI_SLIP_FUZZY_CONTROL::Mat_multiply_Vec(vector<float> &v_out, const vector<vector<float> > &m_left, const vector<float> &v_right){ // v_out = m_left*v_right
00295 
00296     // Size check
00297     if (v_out.size() != m_left.size()){
00298         v_out.resize(m_left.size());
00299     }
00300 
00301     /*
00302     // Iterators
00303     static vector<float>::iterator it_out;
00304     static vector<float>::iterator it_m_row;
00305     static vector<float>::iterator it_v;
00306 
00307     //
00308     it_out = v_out.begin();
00309     for (size_t i = 0; i < m_left.size(); ++i){
00310         *it_out = 0.0;
00311         it_m_row = vector<vector<float> >(m_left)[i].begin();
00312         it_v = vector<float>(v_right).begin();
00313         for (size_t j = 0; j < m_left[i].size(); ++j){
00314             // *it_out += m_left[i][j] * v_right[j];
00315             if (*it_m_row != 0.0 && *it_v != 0.0){
00316                 (*it_out) += (*it_m_row) * (*it_v);
00317             }else{
00318                 // (*it_out) += 0.0
00319             }
00320             // (*it_out) += (*it_m_row) * (*it_v);
00321             //
00322             it_m_row++;
00323             it_v++;
00324         }
00325         it_out++;
00326     }
00327     */
00328 
00329     // Indexing
00330     for (size_t i = 0; i < m_left.size(); ++i){
00331         //
00332         v_out[i] = 0.0;
00333         //
00334         for (size_t j = 0; j < v_right.size(); ++j){
00335             if (m_left[i][j] != 0.0 && v_right[j] != 0.0)
00336                 v_out[i] += m_left[i][j]*v_right[j];
00337         }
00338     }
00339 
00340 }
00341 vector<float> ANTI_SLIP_FUZZY_CONTROL::Mat_multiply_Vec(const vector<vector<float> > &m_left, const vector<float> &v_right){ // v_out = m_left*v_right
00342     vector<float> v_out;
00343     // Size check
00344     if (v_out.size() != m_left.size()){
00345         v_out.resize(m_left.size());
00346     }
00347 
00348     /*
00349     // Iterators
00350     static vector<float>::iterator it_out;
00351     static vector<const float>::iterator it_m_row;
00352     static vector<const float>::iterator it_v;
00353     //
00354     it_out = v_out.begin();
00355     for (size_t i = 0; i < m_left.size(); ++i){
00356         *it_out = 0.0;
00357         it_m_row = m_left[i].begin();
00358         it_v = v_right.begin();
00359         for (size_t j = 0; j < m_left[i].size(); ++j){
00360             // *it_out += m_left[i][j] * v_right[j];
00361             if (*it_m_row != 0.0 && *it_v != 0.0){
00362                 (*it_out) += (*it_m_row) * (*it_v);
00363             }else{
00364                 // (*it_out) += 0.0
00365             }
00366             // (*it_out) += (*it_m_row) * (*it_v);
00367             //
00368             it_m_row++;
00369             it_v++;
00370         }
00371         it_out++;
00372     }
00373     */
00374 
00375     // Indexing
00376     for (size_t i = 0; i < m_left.size(); ++i){
00377         //
00378         v_out[i] = 0.0;
00379         //
00380         for (size_t j = 0; j < v_right.size(); ++j){
00381             if (m_left[i][j] != 0.0 && v_right[j] != 0.0)
00382                 v_out[i] += m_left[i][j]*v_right[j];
00383         }
00384     }
00385 
00386     return v_out;
00387 }
00388 vector<float> ANTI_SLIP_FUZZY_CONTROL::Get_VectorPlus(const vector<float> &v_a, const vector<float> &v_b, bool is_minus) // v_a + (or -) v_b
00389 {
00390     static vector<float> v_c;
00391     // Size check
00392     if (v_c.size() != v_a.size()){
00393         v_c.resize(v_a.size());
00394     }
00395     //
00396     for (size_t i = 0; i < v_a.size(); ++i){
00397         if (is_minus){
00398             v_c[i] = v_a[i] - v_b[i];
00399         }else{
00400             v_c[i] = v_a[i] + v_b[i];
00401         }
00402     }
00403     return v_c;
00404 }
00405 vector<float> ANTI_SLIP_FUZZY_CONTROL::Get_VectorScalarMultiply(const vector<float> &v_a, float scale) // scale*v_a
00406 {
00407     static vector<float> v_c;
00408     // Size check
00409     if (v_c.size() != v_a.size()){
00410         v_c.resize(v_a.size());
00411     }
00412     // for pure negative
00413     if (scale == -1.0){
00414         for (size_t i = 0; i < v_a.size(); ++i){
00415             v_c[i] = -v_a[i];
00416         }
00417         return v_c;
00418     }
00419     // else
00420     for (size_t i = 0; i < v_a.size(); ++i){
00421         v_c[i] = scale*v_a[i];
00422 
00423     }
00424     return v_c;
00425 }
00426 // Increment
00427 void ANTI_SLIP_FUZZY_CONTROL::Get_VectorIncrement(vector<float> &v_a, const vector<float> &v_b, bool is_minus){ // v_a += (or -=) v_b
00428     // Size check
00429     if (v_a.size() != v_b.size()){
00430         v_a.resize(v_b.size());
00431     }
00432     //
00433     if (is_minus){ // -=
00434         for (size_t i = 0; i < v_b.size(); ++i){
00435             v_a[i] -= v_b[i];
00436         }
00437     }else{ // +=
00438         for (size_t i = 0; i < v_b.size(); ++i){
00439             v_a[i] += v_b[i];
00440         }
00441     }
00442 
00443 }