一個電腦鼠運與自走車運動控制的新方法 A new method for the motion control of micromouse and robotracers

為了解決因編碼器解析度帶來差分的雜訊問題，我做了一個二階的位置與速度估測器，並且用這一些估測值來控制電腦鼠運與自走車的位置與速度。
A second order filter is devised in this article to estimate the position and velocity of wheeled mobile robots such that the noise effect arose from difference methods can be lessened.

我在 MPLAB 中利用以下的副程式實現了這樣一個演算法 The algorithm is implemented in the MPLAB environment

//--------- Functions -----------------------------------------
// Function Name : EST_FILTER
// Description   : 2nd order filter for vc and w
// input              : struct ESTIMATOR_S est_ps, or est_ths
// output            : struct ESTIMATOR_S est_ps, or est_ths
//-------------------------------------------------------------
struct ESTIMATOR_S EST_FILTER(struct ESTIMATOR_S states, long ref)
{
//long er_estp2;

states.yv.n = states.yv.n1 + states.erv.n1;
states.yp.n = states.yp.n1 + states.yv.n1;
states.er = ref - states.yp.n;
states.erv.n = (states.er>>2) - states.yv.n;
states.erv.n1 = states.erv.n;
states.yv.n1 = states.yv.n;
states.yp.n1 = states.yp.n;

return (states);
}

struct ESTIMATOR_S { union POSITION yp; struct FILTER_S yv; struct FILTER_S erv; long er; };

struct FILTER_S { int n; int n1; };

union POSITION { struct { long n; // in pulses, 81 pulses ~ 1mm, Q26.5 long n1; }; struct { unsigned int Ln; unsigned int Hn; unsigned int Ln1; unsigned int Hn1; }; } ;

其中 ref 代表真實的編碼器位置信號，states.yp.n 與 states.yp.n1 分別表示現在與過去一個取樣時間的編碼器位置信號估測值，states.yv.n 與 states.yv.n1 分別表示現在與過去一個取樣時間的編碼器速度信號估測值，states.er 是編碼器位置信號真實值與估測值之間的誤差。

states.erv.n = (states.er>>2) - states.yv.n;

上述方程式中的 states.erv.n 是使用編碼器位置信號真實值與估測值之間的誤差乘以 P 增益 0.25，加上編碼器速度信號估測值乘以 D 增益 1的結果，然後再積分得到編碼器速度信號估測值。

states.yv.n = states.yv.n1 + states.erv.n1; $\leftarrow y_v[n] = y_v[n-1] + erv[n-1]*T_s$

真正執行程式時，為了提高解析度，我將編碼器位置信號真實值放大 32 倍。

est_psR = EST_FILTER(est_psR, (out_pR.n<<5));
est_psL = EST_FILTER(est_psL, (out_pL.n<<5));

做位置控制的程式段

// find feedback variables
POS_con.pQEI = ((est_psR.yp.n + est_psL.yp.n)>>1);
ANG_con.pQEI = (est_psR.yp.n - est_psL.yp.n);

// find controlled errors
POS_con.perr = POS_con.pcom - POS_con.pQEI;
ANG_con.perr = ANG_con.pcom - ANG_con.pQEI;

// find motor command ->  $K_pe_p - K_v v_c^e$, and $K_p\theta_p - K_v \omega_c^e$
POS_PWM = (POS_con.perr>>1) - 9*((est_psL.yv.n+est_psR.yv.n)>>1);
ANG_PWM = (ANG_con.perr>>2) - 4*(est_psR.yv.n-est_psL.yv.n);

// find right and left motor commands
R_PWM = POS_PWM + ANG_PWM;
if (R_PWM>PWM_100) R_PWM = PWM_100;
else if (R_PWM<-PWM_100) R_PWM = -PWM_100;
//
L_PWM = POS_PWM - ANG_PWM;
if (L_PWM>PWM_100) L_PWM = PWM_100;
else if (L_PWM<-PWM_100) L_PWM = -PWM_100;

實驗結果 Experimental results for center and angular velocity