A new type of anti-slip controller based on TS fuzzy models
Embed:
(wiki syntax)
Show/hide line numbers
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 }
Generated on Sun Jul 31 2022 02:10:45 by
1.7.2