基于距離差的方向跟蹤

李琦+謝作如

當前，在很多領域中都需要用到方向跟蹤，如拍攝某一運動物體時需要將攝像機實時對準被攝物體；在自動駕駛系統中需要確定前方障礙物的方向；在自動化武器的火控系統中也需要確定目標的方向；等等。

方向的確定有很多種方法，其中有一種方法就是通過距離差，根據平面幾何原理計算出被測物體與觀察者正前方視線的水平夾角。如圖1所示，角θ就是測物體與觀察者正前方視線的水平夾角。

● 距離測量工具：超聲波測距傳感器

人可以聽到的聲波頻率范圍大約是20Hz到20kHz，低于20Hz的聲波稱為次聲波，超過20kHz的聲音稱為超聲波，超聲波遇到障礙物時容易被障礙物反射回來。超聲波測距傳感器就是通過發射40kHz超聲波同時接收被測物體反射的超聲波，并根據發射與接收的時間間隔來計算出被測物體的距離，如圖2所示。

假設t0時刻超聲波測距傳感器向被測物體發出超聲波，t1時刻接收到被物體反射的超聲波，則時間間隔Δt=t1-t0；設聲音在空氣中傳播速度為v，則可以計算出被測物體的距離d為：d=（Δt*v）/2。

市面上所售的超聲波測距傳感器一般有4個針腳，分別為VCC、GND、TRIG、ECHO，圖3所示的是HC-SR04超聲波測距模塊。

使用時先將trig和echo端口都置低，然后向trig端發送持續時間至少10us的高電平脈沖，模塊會自動向外發送8個40kHz的超聲波方波。接著捕捉來自echo端的高電平持續時間，這個時間間隔就是超聲波在空氣中運行的時間。

根據HC-SR04的測距原理，我們可以寫出如下Arduino超聲波測距函數：

float ultrasonic_distance（int trig，int echo）{ //定義端口號

float distance； long pulseIntime；

pinMode（trig，OUTPUT）； //設置trig口為輸出模式

pinMode（echo，INPUT）； //設置echo口為輸入模式

digitalWrite（trig， LOW）； //將trig設置為低電平

delayMicroseconds（2）； //保證trig口穩定為低電平

digitalWrite（trig，HIGH）； //將trig設置為高電平

delayMicroseconds（10）； //并使高電平持續10微秒

digitalWrite（trig，LOW）； //10微秒后，trig置回低電平

pulseIntime=pulseIn（echo，HIGH，30000）；//檢測echo出現的高電平時間，

//如果30000微秒內沒有出現高

//電平則返回0，

//考慮聲音來回以及HC-SR0

//的測量最大距離約4.5米，那

//么30000微秒時間如果沒有出

//現echo信號，則說明超過了測

//量距離或者測量失敗了。

if（pulseIntime==0）{ //如果超過測量距離或者測量失敗

distance=-1； //則定義距離為-1

{else{ //否則，計算公式如下：

//測量距離（毫米=持續時間（微秒）/1000（微秒/毫秒）*340（毫米/毫秒）/2//化簡后得：測量距離（毫米）=持續時間（微秒）*0.17

distance=pulseIntime*0.17； }

return distance；

}

以上函數的兩個參數分別為trig口引腳號和echo口引腳號，并且近似地取聲速為20℃時較為典型的340m/s，該函數的返回值就是傳感器與被測物體的距離（單位為毫米）。

● 方向測量的數學幾何模型

超聲波測距傳感器雖然能檢測出傳感器與被測物體的距離，但卻無法知道被測物體的方位，對于傳感器而言，物體有可能分布在它可測夾角內以距離為半徑的弧的任意位置，如圖4所示。

但是，如果我們考慮在一條直線上放置兩個一定間距的超聲波測距傳感器，如圖5所示。不難發現，兩條弧最多只有一個交點，而被測物體就在這個交點上。我們可以將問題抽象為一個平面幾何問題，如圖6所示。

在圖6中，O點為線段LR的中點并且已知LR的長度為l，兩個超聲波測距傳感器分別放置在L點和R點上，并且對同一物體M進行測量，測得的距離分別是dl和dr，如果能計算出θ的角度，便可以知道方向。

余弦定理是描述三角形中三邊長度與一個角的余弦值關系的數學定理，是勾股定理在一般三角形情形下的推廣。如圖7所示，是余弦定理分析。

在我們所測量的幾何模型中，對于△LOM，我們已知LM和LO兩邊的邊長分別為dl和l/2，如果我們再已知OM的邊長do，則可以通過余弦定理計算出以下數值：

而對于OM的邊長do，則可以通過平行四邊形四邊對角線平方和定理求出。對于一個平行四邊形而言，其兩條對角線的平方和等于四邊的平方和。在我們的平面幾何模型基礎上，作OM的反向延長線ON，使得OM=ON，則根據平行四邊形（如圖8）四邊對角線平方和定理，我們得出：

，整理后可得：。

根據余弦定理推導的公式

我們便可以計算出θ的值。

● 結合Arduino實現方向的跟蹤

用兩個超聲波測距傳感器SL和SR測量同一個物體的距離，經過計算得出舵機的偏轉角度，進而實現方向跟蹤。我們根據前面推導的公式編寫如下的自定義函數，即：

float get_degree（float distance_l，float distance_r，float sensor_distance）{

float x；

float l；

x=sensor_distance/2；

l=sqrt（（2*（pow（distance_l，2）+pow（distance_r，2））-pow（2*x，2））/4）；

return acos（（pow（x，2）+pow（l，2）-pow（distance_l，2））/（2*x*distance_l））*180/PI；

}

上述自定義函數中，需要的三個參數分別就是SL的測量值、SR的測量值以及SL和SR的中心點距離，在圖9中，這個距離為165毫米。函數的返回值為弧度值。

由于舵機輸出的是角度值的整數，而上述函數計算的結果是弧度，因此還需要在主函數中將弧度值轉換為角度值并取整：

degree=int（get_degree（distance1，distance2，SENSORDISTANCE）+0.5）

根據舵機的運行方向，在圖中的實例中，最終舵機輸出的角度為θ的補角，即：

s1.write（180-degree）；

下面給出完整的Arduino代碼并附上簡單注釋：

#include"Servo.h" //需要用到舵機庫和數學函數庫

#include "math.h"

Servos1；

//定義舵機對象s1

#define SERVOPIN 11 //定義宏：舵機所連接的數字口

#define SENSORDISTANCE 165 //定義宏：兩個超聲波測距傳感器的中心點距離

int degree； void setup（） {

Serial.begin（9600）； //打開串口，便于看到計算結果

s1.attach（SERVOPIN）；

}

void loop（） {

float distance1=ultrasonic_distance（5，6）；//超聲波測距傳感器SL連接在5，6號數字口

float distance2=ultrasonic_distance（7，8）；//超聲波測距傳感器SR連接在5，6號數字口

Serial.print（distance1）；

Serial.print（" "）；

Serial.print（distance2）；

Serial.print（" "）； //串口打印出兩個傳感器的測量值便于查看

if （int（get_degree（distance1，distance2，SENSORDISTANCE）+0.5）！=0 ）{

//有可能會測量失敗，如果某個傳感器測量失敗，則最終計算結果會是0

degree=int（get_degree（distance1，distance2，SENSORDISTANCE）+0.5）；

//如果計算結果不為0，將角度值更新給degree變量

}

Serial.println（degree）；

s1.write（180-degree）；

delay（100）；

}

float get_degree（float distance_l，float distance_r，float sensor_distance）{

float x；

x=sensor_distance/2；

float l；

l=sqrt（（2*（pow（distance_l，2）+pow（distance_r，2））-pow（2*x，2））/4）；

return acos（（pow（x，2）+pow（l，2）-pow（distance_l，2））/（2*x*distance_l））*180/PI；

}

float ultrasonic_distance（int trig，int echo）{//超聲波測距子函數

float distance；

long pulseIntime；

pinMode（trig，OUTPUT）；

pinMode（echo，INPUT）；

digitalWrite（trig， LOW）；

delayMicroseconds（20）；

digitalWrite（trig，HIGH）；

delayMicroseconds（20）；

digitalWrite（trig，LOW）；

pulseIntime=pulseIn（echo，HIGH，30000）；

if（pulseIntime==0）{

distance=-1；

}else{

distance=pulseIntime*0.34/2；

}

return distance；

}

我們可以看一下效果（如圖10、圖11）：舵機上連接的一條亞克力條會動態地指向前方的障礙物。

● 拓展思考

利用兩個超聲波測距傳感器，結合數學公式研究方向追蹤是一個有趣的STEAM項目。雖然我們已經成功地實現了方向追蹤，但還是存在一些問題，還需要進一步研究：①因為超聲波測距傳感器測量障礙物的距離并不是很穩定，而上述程序只是簡單地把不合理的值忽略掉，因此會出現舵機抖動的情況，我們可不可以進一步分析測量值，如從一段時間的大量測量值中過濾出較為正確的測量結果？②超聲波測距傳感器的測量角度并不是很大，因此如果障礙物的方位過于偏向左右，則會出現某個傳感器檢測不到障礙物的情況，我們可以怎樣改進，如調整超聲波測距傳感器的方向是否有用？

如果對相關內容感興趣，請關注主持人博客。