PathTracking.cpp

00001 //-----------------------------------------
00002 // 軌道追従や位置のPID制御を行うためのクラス
00003 // 作成：2019/05/15 by Yuki Ueno
00004 // 編集：Miki Nakaone
00005 //-----------------------------------------
00006 
00007 #include "PathTracking.h"
00008 
00009 extern coords gPosi;
00010 
00011 PID posiPIDx(POSI_X_KP, POSI_X_KI, POSI_X_KD, INT_TIME);
00012 PID posiPIDy(POSI_Y_KP, POSI_Y_KI, POSI_Y_KD, INT_TIME);
00013 PID posiPIDz(POSI_Z_KP, POSI_Z_KI, POSI_Z_KD, INT_TIME);
00014 
00015 PID yokozurePID(YOKOZURE_KP, YOKOZURE_KI, YOKOZURE_KD, INT_TIME);//(3.0, 0.0, 0.0, INT_TIME);
00016 PID kakudoPID(KAKUDO_KP, KAKUDO_KI, KAKUDO_KD, INT_TIME);
00017 
00018 // 二次遅れ使えるようになる
00019 Filter sokduo_filter(INT_TIME);
00020 Filter kakudo_filter(INT_TIME);
00021 
00022 // コンストラクタ
00023 PathTracking::PathTracking(int xmode){
00024     path_num = 0;
00025     max_pathnum = 0;
00026     t_be = 0.0;
00027     pre_t_be = 0.1;
00028     epsilon = 1.0;
00029 
00030     refVx = 0;
00031     refVy = 0;
00032     refVz = 0;
00033 
00034     count_acc = 0;
00035 
00036     mode = xmode;
00037 
00038     mode_changed = true;
00039     init_done = false;
00040 }
00041 
00042 // tを求めるための方程式
00043 double PathTracking::func(int p, double t)
00044 {
00045     return a_be[p] * pow(t, 5.0) + b_be[p] * pow(t,4.0) + c_be[p] * pow(t,3.0) + d_be[p] * pow(t,2.0) + e_be[p] * t + f_be[p];
00046 }
00047 // tを求めるための方程式の1階微分
00048 double PathTracking::dfunc(int p, double t)
00049 {
00050     return 5.0 * a_be[p] * pow(t, 4.0) +  4.0 * b_be[p] * pow(t,3.0) + 3.0 * c_be[p] * pow(t,2.0) + 2.0 * d_be[p] * t + e_be[p];
00051 }
00052 
00053 // tにおけるベジエ曲線の座標を求める関数
00054 double PathTracking::bezier_x(int p, double t)
00055 {
00056     return Ax[p]*pow(t,3.0) + 3.0*Bx[p]*pow(t,2.0) + 3.0*Cx[p]*t + Dx[p];
00057 }
00058 double PathTracking::bezier_y(int p, double t)
00059 {
00060     return Ay[p]*pow(t,3.0) + 3.0*By[p]*pow(t,2.0) + 3.0*Cy[p]*t + Dy[p];
00061 }
00062 
00063 // ベジエ曲線式の1階微分
00064 double PathTracking::dbezier_x(int p, double t)
00065 {
00066     return 3.0*Ax[p]*pow(t,2.0) + 6.0*Bx[p]*t + 3.0*Cx[p];
00067 }
00068 double PathTracking::dbezier_y(int p, double t)
00069 {
00070     return 3.0*Ay[p]*pow(t,2.0) + 6.0*By[p]*t + 3.0*Cy[p];
00071 }
00072 
00073 // ニュートン法のための係数の初期化
00074 void PathTracking::initSettings(){
00075     // PID関連初期化
00076     posiPIDx.PIDinit(0.0, 0.0);
00077     posiPIDy.PIDinit(0.0, 0.0);
00078     posiPIDz.PIDinit(0.0, 0.0);
00079     
00080     yokozurePID.PIDinit(0.0, 0.0);
00081     kakudoPID.PIDinit(0.0, 0.0);
00082     
00083     sokduo_filter.setSecondOrderPara(FILT_SOKUDO_OMEGA, FILT_SOKUDO_DZETA, 0.0);//(15.0, 1.0, 0.0);
00084     kakudo_filter.setSecondOrderPara(FILT_KAKUDO_OMEGA, FILT_KAKUDO_DZETA, 0.0);//(7.0, 1.0, 0.0);
00085     
00086     for(int i = 0; i < PATHNUM; i++) {
00087         Ax[i] = Px[3*i+3] -3*Px[3*i+2] + 3*Px[3*i+1] - Px[3*i+0];
00088         Ay[i] = Py[3*i+3] -3*Py[3*i+2] + 3*Py[3*i+1] - Py[3*i+0];
00089         Bx[i] = Px[3*i+2] -2*Px[3*i+1] + Px[3*i+0];
00090         By[i] = Py[3*i+2] -2*Py[3*i+1] + Py[3*i+0];
00091         Cx[i] = Px[3*i+1] - Px[3*i+0];
00092         Cy[i] = Py[3*i+1] - Py[3*i+0];
00093         Dx[i] = Px[3*i+0];
00094         Dy[i] = Py[3*i+0];
00095     }
00096 
00097     for(int i = 0; i < PATHNUM; i++) {
00098         a_be[i] = pow(Ax[i], 2.0) + pow(Ay[i], 2.0);
00099         b_be[i] = 5*(Ax[i]*Bx[i] + Ay[i]*By[i]);
00100         c_be[i] = 2*((3*pow(Bx[i],2.0)+2*Ax[i]*Cx[i]) + (3*pow(By[i],2.0)+2*Ay[i]*Cy[i]));
00101         d_be_[i] = 9*Bx[i]*Cx[i] + 9*By[i]*Cy[i];
00102         e_be_[i] = 3*pow(Cx[i],2.0) + 3*pow(Cy[i],2.0);
00103         f_be_[i] = 0;
00104     }
00105     init_done = true;
00106 }
00107 
00108 // ベジエ曲線までの垂線距離をニュートン法で求めて，そこまでの距離と接線角度を計算する
00109 void PathTracking::calcRefpoint(){
00110     if(init_done){
00111         double tmpx = Px[path_num * 3] - gPosi.x;
00112         double tmpy = Py[path_num * 3] - gPosi.y;
00113                 
00114         d_be[path_num] = d_be_[path_num] + Ax[path_num] * tmpx + Ay[path_num] * tmpy;
00115         e_be[path_num] = e_be_[path_num] + 2*Bx[path_num] * tmpx + 2*By[path_num] * tmpy;
00116         f_be[path_num] = f_be_[path_num] + Cx[path_num] * tmpx + Cy[path_num] * tmpy;
00117                 
00118         int count_newton = 0;
00119         do {
00120             t_be = pre_t_be - func(path_num, pre_t_be)/dfunc(path_num, pre_t_be);
00121             epsilon = fabs((t_be - pre_t_be)/pre_t_be);
00122             
00123             pre_t_be = t_be;
00124             count_newton++;
00125         }while(epsilon >= 1e-4 && count_newton <= 50);
00126         
00127         //double onx, ony;    //ベジエ曲線上の点
00128         onx = bezier_x(path_num, t_be);
00129         ony = bezier_y(path_num, t_be);
00130                 
00131         // 外積による距離導出
00132         //double angle;
00133         angle = atan2(dbezier_y(path_num, t_be), dbezier_x(path_num, t_be)); // ベジエ曲線の接線方向
00134         if(fabs(angle - preAngle) > M_PI){
00135             angle += 2 * M_PI;
00136         }
00137         //double dist;
00138         dist = (ony - gPosi.y)*cos(angle) - (onx - gPosi.x)*sin(angle);
00139 
00140         epsilon = 1.0;
00141     }
00142 }
00143 
00144 // モードによって，それぞれ指令速度を計算する
00145 int PathTracking::calcRefvel(){
00146     double refVxg, refVyg, refVzg; // グローバル座標系の指定速度
00147     double tmpPx, tmpPy;
00148     //static int counter = 0;
00149 
00150     if(init_done){
00151         if(path_num <= max_pathnum){ // パスが存在する場合は以下の処理を行う
00152             if(mode == FOLLOW_TANGENT || mode == FOLLOW_COMMAND){ // ベジエ曲線追従モード
00153                 calcRefpoint();
00154 
00155                 double refVtan, refVper, refVrot;
00156                 if((acc_mode[path_num] == MODE_START || acc_mode[path_num] == MODE_START_STOP) && count_acc <= acc_count[path_num]){
00157                     count_acc++;
00158                     refVtan = refvel[path_num] * count_acc/(double)acc_count[path_num];
00159                 }else if(t_be < dec_tbe[path_num]){
00160                     refVtan = refvel[path_num];
00161                 }else if((acc_mode[path_num] == MODE_STOP || acc_mode[path_num] == MODE_START_STOP) && t_be >= dec_tbe[path_num]){
00162                     refVtan = refvel[path_num] - refvel[path_num] * (t_be - dec_tbe[path_num]) / (1.0 - dec_tbe[path_num]);
00163                 }else if(t_be >= dec_tbe[path_num]){
00164                     refVtan = refvel[path_num] - (refvel[path_num] - refvel[path_num + 1]) * (t_be - dec_tbe[path_num]) / (1.0 - dec_tbe[path_num]);
00165                 }
00166 
00167                 //refVtan = sokduo_filter.SecondOrderLag(refvel[path_num]); // 接線方向速度
00168                 refVper = yokozurePID.getCmd(dist, 0.0, refvel[path_num] * 2.0); // 横方向速度
00169 
00170                 // 旋回は以下の2種類を mode によって変える
00171                 if(mode == FOLLOW_TANGENT){
00172                     if(mode_changed){
00173                         kakudoPID.PIDinit(angle, gPosi.z);
00174                         mode_changed = false;
00175                     }
00176                     refVrot = kakudoPID.getCmd(angle, gPosi.z, 1.57);//(refKakudo, gPosiz, 1.57);
00177                 }else{
00178                     if(mode_changed){
00179                         kakudoPID.PIDinit(refKakudo, gPosi.z);
00180                         kakudo_filter.initPrevData(refKakudo);
00181                         mode_changed = false;
00182                     }
00183                     refKakudo = kakudo_filter.SecondOrderLag(refangle[path_num]);
00184                     refVrot = kakudoPID.getCmd(refKakudo, gPosi.z, 1.57);
00185                 }
00186 
00187                 per = refVper;
00188                 rot = refVrot;
00189                 // ローカル座標系の指令速度(グローバル座標系のも込み込み)
00190                 //refVxとrefVyをコメントアウトするとkakudoPIDのパラメータ調整が出来る(手で押してみて…)
00191                 //refVperだけにするとyokozurePIDのパラメータ調整できる
00192                 refVx =  refVtan * cos( gPosi.z - angle ) + refVper * sin( gPosi.z - angle );
00193                 refVy = -refVtan * sin( gPosi.z - angle ) + refVper * cos( gPosi.z - angle );
00194                 refVz =  refVrot;
00195             }else{ // PID位置制御モード
00196                 if( mode_changed ){
00197                     Px[3 * path_num] = gPosi.x;
00198                     Py[3 * path_num] = gPosi.y;
00199                     posiPIDx.PIDinit(Px[3 * path_num], gPosi.x);    // ref, act
00200                     posiPIDy.PIDinit(Py[3 * path_num], gPosi.y);
00201                     posiPIDz.PIDinit(refKakudo, gPosi.z);
00202                     kakudo_filter.initPrevData(refKakudo);
00203                     setRefKakudo();
00204                     mode_changed = false;
00205                 }
00206 
00207                 if(count_acc <= acc_count[path_num]){
00208                     count_acc++;
00209                     tmpPx = Px[3 * path_num] + (Px[3 * path_num + 3] - Px[3 * path_num]) * count_acc / (double)acc_count[path_num];
00210                     tmpPy = Py[3 * path_num] + (Py[3 * path_num + 3] - Py[3 * path_num]) * count_acc / (double)acc_count[path_num];
00211                 }else{
00212                     tmpPx = (Px[3 * path_num + 3]);
00213                     tmpPy = (Py[3 * path_num + 3]);
00214                 }
00215 
00216                 // PIDクラスを使って位置制御を行う(速度の指令値を得る)
00217                 refVxg = posiPIDx.getCmd(tmpPx, gPosi.x, refvel[path_num]);//(Px[30], gPosix, refvel[phase]);
00218                 refVyg = posiPIDy.getCmd(tmpPy, gPosi.y, refvel[path_num]);//(Py[30], gPosiy, refvel[phase]);
00219                 refKakudo = kakudo_filter.SecondOrderLag(refangle[path_num]);
00220                 refVzg = posiPIDz.getCmd(refKakudo, gPosi.z, 1.57);//角速度に対してrefvelは遅すぎるから　refvel[path_num]);//(0.0, gPosiz, refvel[phase]);
00221 
00222                 // 上記はグローバル座標系における速度のため，ローカルに変換
00223                 refVx =  refVxg * cos(gPosi.z) + refVyg * sin(gPosi.z);
00224                 refVy = -refVxg * sin(gPosi.z) + refVyg * cos(gPosi.z);
00225                 refVz =  refVzg;
00226             }
00227 
00228             // 収束判定して，収束していたら　1　を返す
00229             dist2goal = sqrt(pow(gPosi.x - Px[3 * path_num + 3], 2.0) + pow(gPosi.y - Py[3 * path_num + 3], 2.0));
00230             if(mode == FOLLOW_TANGENT || mode == FOLLOW_COMMAND){
00231                 // 軌道追従制御なら，到達位置からの距離とベジエ曲線の t のどちらかの条件
00232                 if(dist2goal <= conv_length || t_be >= conv_tnum){
00233                     //counter = 0;
00234                     return 1;
00235                 }
00236             }if(mode == POSITION_PID){
00237                 // 位置制御なら，目標位置と角度両方を見る
00238                 if(dist2goal <= conv_length && fabs(refangle[path_num] - gPosi.z)){
00239                     //counter = 0;
00240                     return 1;
00241                 }
00242             }
00243             
00244             // 収束していなかったら　0　を返す
00245             return 0;
00246         }else{
00247             // path_num が設定された max_pathnum を超えたら　-2　を返す
00248             return -2;
00249         }
00250     
00251     }else{
00252         // 初期化されていなかったら　-1　を返す
00253         return -1;
00254     }
00255 }
00256 
00257 // ベジエ曲線のパス番号をインクリメントする
00258 void PathTracking::incrPathnum(double xconv_length, double xconv_tnum = 0.997){
00259     path_num++;
00260     pre_t_be = 0.1;
00261     count_acc = 0;
00262     setConvPara(xconv_length, xconv_tnum);
00263 }
00264 
00265 int PathTracking::getPathNum(){
00266     return path_num;
00267 }
00268 
00269 void PathTracking::setPathNum(int num){
00270     path_num = num;
00271 }
00272 
00273 // 収束判定に用いる距離などをセットする
00274 void PathTracking::setConvPara(double xconv_length, double xconv_tnum = 0.997){
00275     conv_length = xconv_length;
00276     conv_tnum = xconv_tnum;
00277 }
00278 
00279 void PathTracking::setMode(int xmode){
00280     mode = xmode;
00281     mode_changed = true;
00282 }
00283 
00284 int PathTracking::getMode(){
00285     return mode;
00286 }
00287 
00288 void PathTracking::setMaxPathnum(int num){
00289     max_pathnum = num;
00290 }
00291 
00292 void PathTracking::setPosiPIDxPara(float xKp, float xKi, float xKd){
00293     posiPIDx.setPara(xKp, xKi, xKd);
00294 }
00295 
00296 void PathTracking::setPosiPIDyPara(float xKp, float xKi, float xKd){
00297     posiPIDy.setPara(xKp, xKi, xKd);
00298 }
00299 
00300 void PathTracking::setPosiPIDzPara(float xKp, float xKi, float xKd){
00301     posiPIDz.setPara(xKp, xKi, xKd);
00302 }
00303 
00304 void PathTracking::setYokozurePIDPara(float xKp, float xKi, float xKd){
00305     yokozurePID.setPara(xKp, xKi, xKd);
00306 }
00307 
00308 void PathTracking::setKakudoPIDPara(float xKp, float xKi, float xKd){
00309     kakudoPID.setPara(xKp, xKi, xKd);
00310 }
00311 
00312 
00313 void PathTracking::kakudoPIDinit(){
00314     kakudoPID.PIDinit(refKakudo, gPosi.z);
00315 }
00316 
00317 void PathTracking::setRefKakudo(){
00318     refKakudo = refangle[path_num];
00319 }
00320 
00321 double PathTracking::getRefVper(){
00322     return per;
00323 }
00324 
00325 double PathTracking::getRefVrot(){
00326     return rot;
00327 }