自走車線軌跡的曲率偵測 Curvature calculation in Robotrace contests

假設自走車的直線速度是 $v_C$ 而且被控制為定值，角速度是 $\omega_C$，重心位置與前方紅外線感測器用來估測線軌跡誤差控制中心點的距離為 $L$。 那麼控制中心點的速度 $v_{LC}$ 的大小，以及它與自走車直線速度方向的夾角為 $\phi$，可以用下列的方程式算出來 \[ v^2_{LC} = v^2_C + (\omega_C L)^2 \] \[ \phi = \tan^{-1}\left(\dfrac{\omega_C L}{v_C}\right) \]
假設我們開始觀測的時間為 $t_0$，而且自走車的直線速度 $v_C$ 在 $t_0$ 到 $t$ 時間內的角度變化為 $\theta(t)-\theta(t_0)$。其中 \[ \theta(t) - \theta(t_0) = \int^t_{t_0} \omega_C(\tau) d\tau \] 
那麼控制中心點速度 $v_{LC}$ 在 $t_0$ 到 $t$ 時間內的角度變化 $\alpha$ 就是 \[ \alpha = \theta(t)-\theta(t_0) + \tan^{-1}\left(\dfrac{\omega_C(t) L}{v_C}\right) - \tan^{-1}\left(\dfrac{\omega_C(t_0) L}{v_C}\right) \]
如果控制中心點能夠一直沿著線軌跡沒有誤差的移動，那麼在時間 $t$ 時線軌跡的曲率半徑 $r$ 就可以利用下列方程式來估測 \[ r = \dfrac{l}{\alpha} \] \[ l = \int^t_{t_0} v_{LC}(\tau) d\tau \]
 另一個估測的方法是 \[ r = \dfrac{v_{LC}}{\dot{\alpha}} \] \[ \dot{\alpha} = \dfrac{d\alpha}{dt} = \omega_C + \dfrac{Lv_C}{v^2_C+(\omega_C L)^2} \dfrac{d\omega_C}{dt} \]
 這是因為 \[ \dfrac{d\tan^{-1}(x)}{dx} = \dfrac{1}{1+x^2} \] 但是當控制中心點無法一直沿著線軌跡沒有誤差的移動時，該怎麼修正呢？ 目前的想法是 \[ r_t = r - \delta x \] 其中 $r_t$ 是修正後的估測半徑，$\delta x$ 是控制中心點與線軌跡間的誤差大小。

PD 控制的數位化設計

我們希望針對電腦鼠 PD 控制器的「數位化」做一些討論。 在 Peter 的文章 (Characterising the drive system on the micromouse) 中提到，電腦鼠的直走或旋轉運動，可以利用實驗資料找出對應的動態系統，還有對應的參數 $\tau_m$ 和 $K_m$ \[ G(s) = \frac{K_m}{\tau_ms + 1} \] 這一篇文章要討論的是 PD 控制器，以及它數位化後的結果。Peter 也有一篇類似的文章 Designing the motor controller 。整體的系統架構是 \[ e(s) -> K_p+K_ds -> \frac{K_m}{\tau_ms + 1} \] 其中 $e(s)$ 代表命令與位置輸出之間的誤差。 $G(s)$ 的狀態空間表示式是 \[ \dot{x}(t) = Ax(t) + Bu(t), \\ y(t) = Cx(t) \\ A = \begin{bmatrix} 0 & 1 \\ 0 & \frac{1}{\tau_m} \end{bmatrix} , B = \begin{bmatrix} 0 \\ 1 \end{bmatrix} , C = \begin{bmatrix} \frac{K_m}{\tau_m} & 0 \end{bmatrix} \] $G(s)$ 狀態空間表示式以取樣時間 $\Delta$ 數位化後的結果 $G(z)$ 是 \[ x[n+1] = Fx[n] + Gu[n], \\ y[n] = Hx[n] \\ F = e^{A\Delta}, B = , C = \begin{bmatrix} \frac{K_m}{\tau_m} & 0 \end{bmatrix} \]

加速規信號的零點校正

這一個問題緣起於我們希望對四旋翼直升機上的加速度信號做「零點校正」。

假設從 MPU6500 取得的加速度信號分別是 $x_i$, $y_i$, 以及 $z_i$，這三個軸未知零點的數值分別是 $x_0$, $y_0$, 以及 $z_0$，而且重力加速度 $g$ 不變。

如果我們量測三次，得到 $(x_1, y_1, z_1)$, $(x_2, y_2, z_2)$, $(x_3, y_3, z_3)$ 三組數據，那麼我們就可以有以下三個關係式
\[ (x_1-x_0)^2+(y_1-y_0)^2+(z_1-z_0)^2 = g^2, \] \[ (x_2-x_0)^2+(y_2-y_0)^2+(z_2-z_0)^2 = g^2, \] \[ (x_3-x_0)^2+(y_3-y_0)^2+(z_3-z_0)^2 = g^2. \]

第一式減去第二式可以得到
\[ x_1^2 + y_1^2 + z_1^2 - x_2^2 - y_2^2 - z_2^2 = 2(x_1-x_2)x_0 + 2(y_1-y_2)y_0 + 2(z_1-z_2)z_0 \]
第一式減去第三式可以得到
\[ x_1^2 + y_1^2 + z_1^2 - x_3^2 - y_3^2 - z_3^2 = 2(x_1-x_3)x_0 + 2(y_1-y_3)y_0 + 2(z_1-z_3)z_0 \]

第二式減去第三式可以得到
\[ x_2^2 + y_2^2 + z_2^2 - x_3^2 - y_3^2 - z_3^2 = 2(x_2-x_3)x_0 + 2(y_2-y_3)y_0 + 2(z_2-z_3)z_0 \]

寫成矩陣的形式
\[
\left[
\begin{array}
 .2(x_1-x_2) & 2(x_1-x_3) & 2(x_2-x_3) \\
 2(y_1-y_2) & 2(y_1-y_3) & 2(y_2-y_3) \\
 2(z_1-z_2) & 2(z_1-z_3) & 2(z_2-z_3)
\end{array}
\right]
\left[
\begin{array}
 .x_0 \\
 y_0 \\
 z_0
\end{array}
\right] =
\left[
\begin{array}
 .x_1^2 + y_1^2 + z_1^2 - x_2^2 - y_2^2 - z_2^2 \\
 x_1^2 + y_1^2 + z_1^2 - x_3^2 - y_3^2 - z_3^2 \\
 x_2^2 + y_2^2 + z_2^2 - x_3^2 - y_3^2 - z_3^2
\end{array}
\right]
\]

因此，這三個軸未知零點的數值 $x_0$, $y_0$, 以及 $z_0$，就可以利用以下的公式求出來，當然前提是反矩陣存在。
\[
\left[
\begin{array}
 .x_0 \\
 y_0 \\
 z_0
\end{array}
\right] =
\left[
\begin{array}
 .2(x_1-x_2) & 2(x_1-x_3) & 2(x_2-x_3) \\
 2(y_1-y_2) & 2(y_1-y_3) & 2(y_2-y_3) \\
 2(z_1-z_2) & 2(z_1-z_3) & 2(z_2-z_3)
\end{array}
\right]^{-1}
\left[
\begin{array}
 .x_1^2 + y_1^2 + z_1^2 - x_2^2 - y_2^2 - z_2^2 \\
 x_1^2 + y_1^2 + z_1^2 - x_3^2 - y_3^2 - z_3^2 \\
 x_2^2 + y_2^2 + z_2^2 - x_3^2 - y_3^2 - z_3^2
\end{array}
\right]
\]

只是從 MPU6500 取得的加速度信號通常很髒，因此 $(x_1, y_1, z_1)$, $(x_2, y_2, z_2)$, $(x_3, y_3, z_3)$ 這三組數據，究竟應該是濾波後的結果，還是不需要呢？另外，反矩陣的計算其實對寫程式而言不是很友善，因此如何利用其他的演算法(如 Recursive Least Square的方法)來實現，也是值得討論。

台灣的電腦鼠與競速自走車競賽 Taiwan Micromouse and Robotrace contests

這幾年受邀的國際選手們...

2009 年

2011年

2012年

2013年

2014年

2015年

2016年

一階的數位 IIR 低通濾波器

這是一篇網路上可以參考的文章。

First Order Digital Filters - An Audio Cookbook

這一個數位濾波器的數學式是以下的樣子 (直流增益值為 1) \[ y_n = ay_{n-1} + (1-a)u_n \] 其中 $y_n$、$y_{n-1}$ 與 $u_n$ 分別代表濾波器現在的輸出值、濾波器過去一個取樣時間的輸出值，和現在的原始信號。

 $a$ 這一個係數定義成 \[ a = e^{-\pi f/F_n} \] 
其中 $F_n$ 是取樣頻率 $F_s$ 的一半，$f$ 代表設定的頻寬。

畫出對應的頻率響應圖，的確一如預期。

Matlab 的程式碼

% constants

Fs=20000;       % sampling rate

t=1/Fs;         % sampling interval

Fn=Fs/2;        % Nyquist frequency

numPts=2^13; % number of points of analysis

%

n = [0.002 0.01 0.05 0.1 0.2];

a = exp(-pi*n*Fn/Fn);

%

% First order IIR filter

% y(n) = a*y(n-1) + (1-a)*u(n)

% find frequency response

%

h1 = figure(1);

set(h1,'color','white');

for i = 1:5

    [h,w] = freqz([1-a(i) 0],[1 -a(i)], numPts);

    % calculate dB values, for power quantities

    y(:,i) = 20*log10(abs(h));

end;

semilogx(w*10000/pi, y,'-.','LineWidth',2);

ax3 = gca;

set(ax3,'fontsize',14,'linewidth',1.0);

grid;

xlabel('frequency in Hz','fontsize',16);

ylabel('Gain in dB, 20log_{10}(Gain)','fontsize',16);

title('Frequency Responses, y_n = ay_{n-1} + (1-a)u_n, a=exp(-\pif/F_n), F_n=10000Hz','fontsize',16); 

axis([2 10^4 -45 5])

l1=legend('f = 20Hz','f = 100','f = 500','f = 1000','f = 2000','Location','SouthWest')

set(l1,'fontsize',16);

但為何如此呢？

我知道了！其實上述的想法，並非是一個公式，而是近似如此(工程師觀點)！

為了證明這樣的觀點，我自己針對特定的頻率值 $\pi f/F_n$ 把頻率響應在認為是3dB頻寬與實際值做了一下比較

\[ \frac{Y(z)}{U(z)} = \frac{1-e^{-\pi f/F_n}}{z-e^{-\pi f/F_n}} \]

其中 $z = e^{-j\Omega}$ ，而且 $\Omega = e^{-j\pi f/F_n}$。

結果如下：

MATLAB 程式

h2 = figure(2);

set(h2,'color','white');

aa = (0.02:0.02:0.4)*pi;

bb= 1-exp(-aa);

cc=exp(j*aa)-exp(-aa);

dd = abs(bb)./abs(cc);

plot(aa,dd,'--o','LineWidth',2);grid;

ax2 = gca;

set(ax2,'fontsize',14,'linewidth',1.0);

axis([0.02*pi 0.4*pi 0.4 0.8]);

xlabel('Values of \pif/F_n','fontsize',16);

title('Ratio of abs(1-exp(\pif/F_n))/abs(exp(-j\pif/F_n))-exp(-\pif/F_n))','fontsize',16); 

當 $f/F_n$ 在 0.4 以下，也就是 $\pi f/F_n$ 在 1.26 以下時，這一個數位低通濾波器關於增益的頻率響應，在 $\pi f/F_n$ 的位置上的確是在 3dB (0.707) 左右的位置。

由上圖可以看出，這樣的想法，在 $\pi f/F_n$ 越來越大時，會越不準！只是因為是低通濾波器的關係，以工程師的觀點來看，應該還是可以接受的！

但究竟是如何看出這樣的關係呢？

想的出來的話，鐵定功力大增！

很吃力的一篇技術文章！

數位信號處理教學心得 part I - IIR低通濾波器設計

這是一個實際處理數位信號的例子，我覺得應該有助於整合上課學習到的概念。

左上角的藍色波形，是學生錄製的人聲 DO。

右上角是原始聲音的頻譜分析，以及 6 階 butterworth 低通濾波器的增益頻譜圖形。

左下角是將原始信號經過 6 階 butterworth 低通濾波器後的波形，振幅有些縮小了。

右下角是濾波過後信號的頻譜分析，配合右上角的的圖形，希望學生可以很容易理解低通濾波器針對信號頻譜的相乘作用。(因為對現性非時變系統而言，輸出信號是由輸入信號與系統的脈衝響應做摺積而得，也可以看成是輸入信號的頻譜與系統轉移函數的頻譜相乘)

以下是 MATLAB 的原始程式碼，其中也包含如何在微控制器中去實現對應的低通濾波器，只是程式中並沒有考慮整數運算的限制，全都是浮點數運算。

clear;

clc;

%

Fs = 44100;

y1 = audioread('d974323027.wav');

u = y1(1:44101,1);

n1=1:max(size(u));

tt=n1/Fs;

yy = [tt' u];

  

[b,a]=butter(6,0.03);

sys = tf(b, a, 1/Fs);

bs = max(size(b));

sb=zeros(size(u));

for k = bs:max(size(u))

    sbk = sb(k-1:-1:k-6);

    uk = u(k:-1:k-6);

    sb(k) = -a(2:7)*sbk+b*uk;

end

sz = max(size(sb));

u2 = u(15001:25000);

sb2= sb(15001:25000);

fu = fft(u2);

fb = fft(sb2);

df = 0:Fs/max(size(u2)):(max(size(u2))-1)*Fs/max(size(u2));

dfb= 0:Fs/max(size(sb2)):(max(size(sb2))-1)*Fs/max(size(sb2));

[mag, phase]=bode(sys, df*2*pi);

mag2 = squeeze(mag);

h = figure(1);

set(h,'color','white');

n2=1:max(size(sb));

subplot(221);plot(n1,u,'r');axis([4000 14000 -0.8 0.8]);

ax1 = gca;

set(ax1,'fontsize',14,'linewidth',1.0);

title('Original sound signal','fontsize',16);

xlabel('points','fontsize',16);

%

subplot(223);plot(sb,'b');axis([4000 14000 -0.8 0.8]);

ax3 = gca;

set(ax3,'fontsize',14,'linewidth',1.0);

title('Filtered sound signal','fontsize',16);

xlabel('points','fontsize',16);

%

subplot(222);

[ax,h1,h2]=plotyy(df(1:400),abs(fu(1:400)),df(1:400),mag2(1:400));

grid;

axis(ax(1),[0 1500 0 1200]);

axis(ax(2),[0 1500 0 1.2]);

set(ax(1),'fontsize',14,'linewidth',1.0);

h1.Color = 'blue';

h1.LineWidth = 1;

h2.LineWidth = 1;

set(ax(2),'fontsize',14,'linewidth',1.0);

title('Spectrum of the original sound signal and filter','fontsize',16);

xlabel('Hz','fontsize',16);

ylabel(ax(2),'Gain','fontsize',16);

%

subplot(224);

p4 = plot(dfb,abs(fb),'b');

axis([0 1500 0 1200]);

set(p4,'linewidth',1);

ax4 = gca;

set(ax4,'fontsize',14,'linewidth',1.0);

grid;

title('Spectrum of the filtered sound signal','fontsize',16);

xlabel('Hz','fontsize',16);

相位超前補償器 Phase Lead controller design for DC motors

假設直流有刷馬達由輸入電壓(V)到位置輸出(pulses)的轉移函數如下
Assume the following dynamic function stands for transfer function of DC brush motor input voltage (V) to the position output (pulses)

\[ G(s) = \frac{K_m}{s(1+\tau_m s)}, \]

其中 $k_m$ 是穩態增益，$\tau_m$ 是時間常數。

假設相位超前補償器的形式如下：
The form of phase lead controller $C(s)$ is

\[ C(s) = \frac{1+T_zs}{1+\alpha T_z s} \]

我們的目標是利用 $\alpha$ 和 $T_z$ 這兩個參數來調整閉迴路控制系統的迴路增益(Loop Gain) $C(s)G(s)$ 波德圖的零交越點 $\omega_c$ 頻率，以及相位邊限 $\phi_c$ (phase margin)。

設計參數是零交越點 $\omega_c$ 頻率，以及相位邊限 $\phi_c$ (phase margin)。因此先確認相位超前補償器在 $\omega_c$ 頻率處，可以提供多少相位

\[ \phi_m(\omega) = \angle C(j\omega) = tan^{-1}(\omega T_z) - tan^{-1}(\alpha\omega T_z)\]

其中 $\phi_m(\omega)$ 的最大值發生在微分等於 0 的頻率，也就是我們要的零交越點頻率 $\omega_c$。因此

\[ \frac{d\phi_m(\omega)}{d\omega} = \frac{T_z}{1+\omega^2T_z^2} - \frac{\alpha T_z}{1+\alpha^2\omega^2T_z^2} \]

也就是

\[ \frac{T_z}{1+\omega_c^2T_z^2} = \frac{\alpha T_z}{1+\alpha^2\omega_c^2T_z^2} \Rightarrow \alpha^2\omega_c^2T_z^2 - \alpha (1+\omega_c^2T_z^2) + 1 = 0 \Rightarrow (\alpha\omega_c^2T_z^2 - 1)(\alpha - 1) = 0 \]

由於 $\alpha \ne 1$ (否則$C(s)=1$)，所以

\[ \alpha = \frac{1}{\omega_c^2T_z^2} \Rightarrow \omega_c = \frac{1}{\sqrt{\alpha}T_z} \]

這時提供的最大相位是

\[ \phi_{max} = \max_{\omega} \phi_m = \phi_m(\omega_c) = tan^{-1}(\frac{1}{\sqrt{\alpha}}) - tan^{-1}(\sqrt{\alpha}) \]