李琦+謝作如

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

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

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

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

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

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

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

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

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口穩(wěn)定為低電平

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

delayMicroseconds（10）； //并使高電平持續(xù)10微秒

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

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

//如果30000微秒內(nèi)沒有出現(xiàn)高

//電平則返回0，

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

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

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

//現(xiàn)echo信號，則說明超過了測

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

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

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

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

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

distance=pulseIntime*0.17； }

return distance；

}

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

● 方向測量的數(shù)學幾何模型

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

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

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

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

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

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

，整理后可得：。

根據(jù)余弦定理推導的公式

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

● 結合Arduino實現(xiàn)方向的跟蹤

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

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；

}

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

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

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

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

s1.write（180-degree）；

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

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

#include "math.h"

Servos1；

//定義舵機對象s1

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

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

int degree； void setup（） {

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

s1.attach（SERVOPIN）；

}

void loop（） {

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

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

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）{//超聲波測距子函數(shù)

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）：舵機上連接的一條亞克力條會動態(tài)地指向前方的障礙物。

● 拓展思考

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

如果對相關內(nèi)容感興趣，請關注主持人博客。