基于激光雷達自導航的AGV 系統設計

（重慶工業賦能創新中心有限公司，重慶 400023）

0 前言

當下社會自動化、智能化在各行業各領域都有很大的發展，自動導航車的應用范圍也越來越廣泛，現代化自動化的工廠車間，自動導航的AGV（Automated Guided Vehicle）小車隨處可見；家庭中常見的掃地機器人也越來越多，但大多是傳統導航方法。

激光雷達可以實現非接觸式的空間感知，可以引導自動導航車進行路徑規劃和避障，針對農業溫室環境之下的導航車，可根據激光雷達實現溫室復雜環境內的運動路徑規劃。在激光雷達差動模型、里程計模型、激光傳感器模型建模的研究為機器人自定位以及自動導航進行理論鋪墊[1]，激光雷達技術已經在農用車輛、橋區船舶導航、道路車輛等領域已經開始有初步研究應用[2-11]，用激光雷達來做導航的AGV 偏少，因此，本文對基于激光雷達的AGV 進行路徑規劃與自主導航研究。

1 硬件系統設計

圖1 硬件連接圖

基于激光雷達的AGV 平臺車主要由平臺車體、激光雷達傳感模塊、點云數據采集、處理及平臺車運動控制器組成。其中平臺車是實現路徑規劃功能的承載主體，激光雷達傳感器則負責采集外部環境數據以及自身狀況并將其提供給樹莓派控制器，控制器對數據進行處理與分析，計算獲取平臺車的運動路徑，并控制車輪電機驅動器實現平臺車自主導航[12]。硬件連接如圖1 所示。

激光雷達傳感器識別周圍環境障礙，通過UART 通信協議將數據傳送給樹莓派，電機驅動器進行數據處理并經由某種算法規劃完路徑，然后樹莓派通過GPIO 控制向電機驅動器發送命令，使其產生PWM 信號給小車的電機，最終能夠實現小車在某些路況中的自動導航。采用的AGV 小車為應用于大棚、農場等結構化的場地作業平臺車，該小車主要由兩個伺服電機，以及分別驅動它們的兩個伺服電機驅動器構成（DMS-055A 全數字交流伺服電機驅動器）。在結構化的場地中工作，就要求該作業平臺車能夠自動導航行駛于直線路段、簡單轉彎路段以及貼壁行駛路段。為了達到這些要求，作業平臺車采用了兩輪中置驅動的驅動模式，并且能夠實現兩輪的正反轉向，同時利用差速原理實現直線行駛與轉彎[13]。為了提高平臺車的靈活性，在底盤下安裝了四個萬向輪，使得它們配合電機控制下的兩個驅動輪實現原地轉向的功能。而驅動電機采用了48V400W 的伺服直流電機，可以準確按照占空比的要求輸出相應的轉速。

1.1 RPLidar A2

RPLidar A2 是思嵐科技開發的新一代低成本二維激光雷達，RPLidar A2 的參數如表1 所示。

表1 RPLidar A2 參數

RPLidar A2 采用的是激光三角測距技術，加上思嵐科技開發的高速視覺采集處理機構，它可以以4 kHz 的高頻率進行測距操作。在每次激光測距操作中，RPLidar A2先發射出經過調制的紅外線，在碰到障礙物之后返回，并通過高速視覺采集處理機構再次接受，經過內置的DSP處理器進行實施解算，最后得到角度與距離數據并通過串口輸出。

圖2 RPLidar A2 的引腳定義與接線圖

RPLidar A2 激光雷達與外接控制的接線方式如圖2所示。其中UART 表示控制器上的通用異步收發傳輸器，在本文中指樹莓派控制器。PWM(Pulse Width Modulation)Generator 指的是GPIO 引腳下的PWM 控制功能。

如圖3 所示，激光雷達的發射端和接收端位于激光雷達的頂部，其頂部相對底部可進行旋轉運動，從而采集360°一周的點云數據，激光雷達的正面為數據線的對應面，正面所對應的激光雷達轉角為0°與360°，激光雷達按逆時針方向旋轉，采集到的角度分別從0°到360°，采集到的距離為坐標軸原點到障礙物的直線距離d，通過極坐標的方式輸出障礙物的位置參數。

圖3 激光雷達坐標系

1.2 點云數據采集控制器

Raspberry Pi 3 Model B 的計算機體積小裝載輕便，對AGV 小車的行駛影響較小；參數配置足夠，能夠承載激光雷達輸出的數據并進行儲存與處理。Raspberry Pi 3 Model B 參數表如表2 所示，其功能完全可以實現數據的采集存儲以及處理。

表2 Raspberry Pi 3 Model B 參數表

樹莓派實物如圖4 所示。

圖4 Raspberry Pi 3 Model B 實物圖

1.3 電機驅動器模塊

驅動伺服電機的驅動器采用的是DMS-055A 全數字交流伺服電機驅動器，該驅動器采用32 位高速DSP 芯片，支持速度閉環控制，速度控制模式下支持PWM 占空比信號，供電電壓20V～50V，并且支持50W～500W 的交流伺服電機，符合上述采用的48V400W 的伺服電機。在本研究中所使用的激光雷達是思嵐科技出品的RPLidar A2激光雷達，隸屬于單線激光雷達。這款雷達是國內廠商思嵐科技開發的一款低成本二維激光雷達，最主要的優點就是價格低廉、開發方便、精度較高。

1.4 基于激光雷達的AGV 導航系統框架連接

激光雷達通過USB 適配器將其通信接口與上位機Raspberry Pi 3 Model B 連接并進行數據通信；樹莓派通過外接電源供電；GP IO 引腳與AGV 小車的兩個伺服電機驅動器連接，下位機通過上位機處理完的數據信息分配PWM 波，設置好占空比后輸出給伺服電機中的PWM 驅動器，實現對平臺車運動控制。圖5 為基于激光雷達的AGV 導航系統的硬件連接模擬圖。

圖5 硬件連接模擬圖

2 基于RPLidar A2 的數據采集方法

為實時獲取激光雷達采集到的點云數據并且進行實時處理，需編寫實時數據采集程序，同時為了驗證采集數據的準確性將數據保存為txt 文件。由于RPLidar A2 的頭文件基于C++與C 語言的混編，故編程邏輯為先采用C++語言進行編程，調試成功后導入樹莓派中，在Linux 系統python 2.7 的環境下進行編譯和運行。

具體的數據采集編程思路為：先基于VS2010 的C++環境進行數據獲取程序的編寫，采用激光雷達所提供的SDK 包調用相關的已封裝好的頭文件和指令函數，實現指令通信和激光雷達作業控制，在獲取了正確編譯的C++源文件后，利用Xshell 軟件將其導入進樹莓派中進行基于python2 的編譯，最后生成可執行文件，運行文件，樹莓派便可控制激光雷達的工作以及數據的采集。本章在這些內容的基礎上還添加了進行正式程序編寫工作前的準備工作，包括PC 端的配置，樹莓派的初始化設置以及二者的連接配置。如6 圖所示。

圖6 數據采集編程思路

2.1 基于VS2010 環境下的數據采集

圖7 數據采集程序流程圖

程序的編寫在PC 端的VS2010 環境下完成，RPLidar A2 提供了能夠自由開發的SDK（Software Development Kit，軟件開發工具包），基于SDK 目錄下的已存在的頭文件進行數據采集程序的編寫。頭文件主要包括：rplidar.h（在程序編寫時只需引用該頭文件即可使用RPLidar SDK 的所有功能）、rplidar_driver.h（定義了核心驅動接口，RPLidarDriver 的類申明，包括了一些函數的接口）、rplidar_protocol.h（定義通訊協議的底層數據結構以及常量）、rplidar.h（定義通訊協議中各類請求命令、應答相關的數據結構以及常量定義）、rptypes.h（定義平臺無關的結構和常量）。

圖7 為RPLidar 數據采集流程圖。程序運行時給激光雷達通電，通過GET_HEALTH 函數獲取激光雷達的健康狀況，檢測是否發生了故障，沒有發生故障則創建驅動實例（無論發生故障與否，都需要輸出設備健康信息），接著檢測硬件信息并輸出，最后檢測工作環境是否安全，若不安全則會中斷程序[14]。在保障激光雷達健康工作后，設備正常運轉并開始掃描同時獲取數據。掃描過程處在一個while(1)的死循環中，只有當外部給出中斷命令時，數據采集程序才會結束。

2.2 Raspberry Pi 在Linux 環境下的編譯

由于Raspberry Pi 使用的是Linux 系統，在VS 中用C++語言編寫的程序無法直接運行，因此，需要進行Linux中的編譯。進行Linux 下的編譯就需要將源文件導入進Raspberry Pi 中，需要使用Xshell 軟件，該軟件可以實現遠程控制Raspberry Pi 的系統的功能。將樹莓派與PC 連接，從而實現用Xshell 遠程控制Linux 系統。

首先對樹莓派進行初始化設置，本文選用Linux 發行的centOS 系統。使用W32DiskImager 將下載好的鏡像系統文件燒寫進SD 卡中，并與PC 連接使用SSH 遠程登錄進行文件的配置，且將點云數據采集程序導入樹莓派。

3 實況模擬下的AGV 導航路徑擬合方法研究

3.1 路徑擬合算法

路徑擬合的算法可以在圖8 中體現，在障礙物兩邊分別取若干采集到的數據點，圖中在障礙左和障礙右中分別取3 個點，兩兩相連求出其3 個中點坐標，根據最小二乘法[15]將這些中點坐標進行擬合，最后可以得到一條較為準確的方向導航線。

圖8 路徑擬合原理圖

通過本文第2 章中的數據采集程序所獲取到的數據實際上是極坐標下的數據（A，D），一個角度對應一個距離，在進行最小二乘法擬合之前，需要用公式（1）將其轉化成直角坐標系的數據（X,Y）。通過公式（2）計算左障礙Ai與Bi中點Ci的坐標（其中i＝1,2,3），再通過公式（3）獲得直線的斜率a 與截距值b。

3.2 特征點的提取

首先對實驗場所進行數據采集，圖9 為實驗場所的環境實物圖。獲取數據后在MATLAB 軟件下進行繪圖，未進行數據過濾時的環境模擬圖如圖10 所示。

圖9 環境實物圖

圖10 未過濾數據環境模擬圖

觀察圖9、圖10 可以發現，障礙物C、D 以及直角彎道與當前測量點距離過大，對當前點進行路徑規劃沒有幫助，因此進行數據過濾，將距離值大于1 000mm 的數據點過濾。數據過濾環境模擬圖如圖11 所示。

圖11 數據過濾后環境模擬圖

如圖11 所示，大致可以判斷從30°～330°范圍內的數據對于規劃當前的路徑是有效的，于是在數組中隔斷選取6 個點，模擬中選擇了數組中第30 個、50 個、70 個（這三者分布在路徑的一邊），第260 個、280 個、300 個（這三者分布在路徑的另一邊），這是因為數據在數組中的位置與其角度值相近似。

3.2.1 無偏差直線路徑工況下的路徑導航

在理想的直線路徑工況中，保證AGV 可沿路徑中線運動，采用最小二乘法直線路徑擬合方法，圖12 為試驗路況圖，可見該路況兩側障礙物平行，距離為200 mm，且障礙物均為平板狀物，沒有凸起點對導航路徑擬合的影響。

將激光雷達采集到的數據導出，再導入Excel 中生成表格，對距離這一參數進行閾值濾波，將1 000mm 以上較遠的無關數據點過濾，完成數據的導入之后進行MATLAB 的程序編寫。

根據最小二乘法擬合方法擬合出來的方向導航線如圖13 所示，6 個點被擬合成了一條幾乎指向0°的直線，對于激光雷達來說也就是正前方。

3.2.2 存在導航偏距的直線路徑工況下的路徑導航

假設平臺車仍然行駛在一條直線路段上，但是并不是沿著中線的理想路線上行走，而是產生了橫向的導航偏距，與上述情況一樣，將激光雷達采集到的數據導入進Excel 中，進行距離小于1 000mm 的濾波后導入MATLAB中，再次定義第一列為angle 變量，第二列為distance 變量。仍然選擇30/50/70/260/280/300 這6 個點，進行最小二乘法直線擬合。

圖13 無偏差時的模擬路徑

通過最小二乘法擬合后得到的方向導航線如圖14 所示，平臺車處于該極坐標的中央位置，相對于上述情況來說，平臺車向中線的左邊產生了距離上的橫向偏移量，而為了讓平臺車能夠保持在中線上行駛，此刻通過最小二乘法擬合得到的路徑指示平臺車向右前方行駛，直至其可以正確行駛在中線上。

圖14 有導航偏距時的模擬路徑

3.2.3 存在航偏角的直線路徑工況下的路徑導航

圖15 航偏角時的模擬路徑

假設平臺車行駛在直線路段上，雖然此刻仍處于直線路段的中線上，但是角度上產生了偏移，于是規劃該路況下的路徑。同樣將激光雷達采集到的數據導入至Excel中，進行距離小于1 000mm 的濾波后導入MATLAB 中，再次定義angle 與distance 變量，通過選取關鍵點的方法繪制路徑模擬圖。

這種情況下通過最小二乘法擬合得到的方向導航線如圖15 所示，在粗線所描出的環境地圖上可以看出，地圖相對于平臺車向左產生了偏轉，也就是說平臺車相對于地圖向右產生了偏轉，于是需要進行符合細線行駛方向的操作才能回轉到正確的正前方向[16]。

3.2.4 同時具有導航偏距和航偏角的直線路徑工況下的路徑導航

假設平臺車行駛在直線路段，但是此刻既出現了導航偏距也出現了航偏角，于是對這種情況進行路徑的模擬規劃。同樣將激光雷達采集到的數據導入到Excel 中，經過距離小于1 000mm 的濾波后導入進MATLAB 中，再次定義angle 與distance 變量，通過選取關鍵點的方法擬合路徑。

在這種情況下通過最小二乘法擬合獲得的方向導航線如圖16 所示，可以明顯看出，極坐標中心（也就是平臺車的當前位置）與粗實線描出的地圖兩邊的墻壁的距離不一致，同時整個地圖也相對于平臺車向左偏轉了一定的角度。為了使平臺車能夠沿著中心路線行駛，需要根據圖中細實線（也就是這種情況下的規劃路徑）所示的方向進行調整。

圖16 導航偏距與航偏角都存在時的模擬路徑

3.3 導航偏距值與航偏角的獲取算法

以圖16 為例，給出獲取偏差值的算法。

3.3.1 導航偏距值的獲取

取0°與270°兩個點的數據，設距離分別為D(0)與D(270)，同時定義0°～180°內距離為正值，180°～360°范圍內距離為負值，D(0)+D(270)就是導航偏距值。當D(0)+D(270)等于0 時，就表示不存在導航偏距值，也就是平臺車處于當前路段的中心線上。

3.3.2 航偏角的獲取

航偏角的獲取更加便利，利用路徑擬合算法中取得同側障礙中的3 個點，同樣是通過最小二乘法進行路徑擬合，得到該障礙路線的斜率，對斜率求反正切即可得到當前的航偏角。同樣，當斜率為0 時則表示不存在航偏角，當前平臺車行駛的方向是沿著中心線方向的。

4 車輛自主導航算法研究

最小二乘擬合直線法是一種有效的尋找路徑的方法，它只需要一些關鍵點就可以在不同的路況下擬合出滿足AGV 小車行駛的路線；并且能夠通過計算行駛路線的直線方程式得到此時角度的偏轉量以及距離的偏移量，這對使用PID 算法實現車輛的自主導航將會起到非常大的作用。

4.1 車輛運動控制

用樹莓派的GPIO 引腳與電機驅動器連接，通過PWM（脈沖寬度調試）進行驅動。用python2.7 編寫一個程序通過接受擬合路徑之后所得到的導航偏距以及航偏角來進行脈沖寬度調試，并輸出給電機驅動器讓其可以控制電機產生轉速，實現平臺車的前進運動，或是使得電機產生差速，實現平臺車的轉向運動。

總的來說，利用GPIO 引腳與電機驅動器連接，通過脈沖寬度調制，可以實現平臺車的運動控制。

為了使用樹莓派上的GPIO 引腳控制DMS-055A 電機驅動器，需要與DMS-055A 電機驅動器的Signal 模塊進行連接，圖17 所示即為DMS-055A 電機驅動器的Signal 模塊功能區。

圖17 DMS-055A 的信號模塊

GPIO 與DMS-055A 接線如圖18 所示，EN-、DR-、PU-都是接地，因此合并成一條線接在GPIO 引腳的GND上。連接完成后，進行引腳的初始化設置。

圖18 GPIO 與DMS-055A 接線圖

4.2 基于PID 路徑導航控制方法與實現

4.2.1 變量初始化

PID 算法是比例、積分、微分的總稱，以導航偏距值以及航偏角為輸入量，電機的轉速為輸出量進行PID 控制，從而糾正平臺車的行駛路徑[17-19]。首先列出實現PID 控制程序所需要的初始化變量。

1）導航偏距值變量。通過獲取導航偏距的算法可以得到這個偏差值distance_error。按PID 算法的思想，取其中的PD（分別是比例與微分）對導航偏距進行控制，糾正路徑回歸正確中線。這是因為如果只使用比例環節，平臺車將會來回以Z 字形行駛，并不能達到沿中心線行駛的要求，而同時采取了微分環節后，將會使Z 字的幅度越來越小，直至趨于0，平臺車得以沿中心線穩定行駛。使用P（比例）環節時，需要獲取到導航偏距值，在上述過程中已經獲得，即為distance_error 這個值；而在使用D（微分）環節時，則需要獲取到“導航偏距值的微分”，也就是這一次的偏差值與上一次的偏差值的差值。在python 程序中實現時，將取得的兩個點的距離值進行初始化定義，分別為direction_left 和direction_right，并且將distance_error＝direction_centre 同時將這個偏差值置為0。

2）航偏角變量。對航偏角的控制同樣選取PD 兩個環節，在P（比例）環節中，需要獲取到航偏角；在D（微分）環節中，則需要獲取到航偏角的微分。同樣在python 程序中實現時，將取得的兩個點的距離值進行初始化定義，分別為theta_initial 和theta，并且將theta_error＝theta_initial，同時將這個偏差值置為0。

4.2.2 結構路況下直線行駛控制

在這種路況下行駛，可能會出現導航偏距，可能會出現航偏角，更多的情況下是兩者同時出現。這里的處理方法是分別計算出導航偏距以及航偏角的控制量，進行簡單的權重耦合[15]，相當于兩個控制量并聯，各取50%的權重進行疊加。最后的到一個總的控制量輸出給PWM 進行調制。下面的代碼用于計算導航偏距值的控制量。

下面的代碼用于計算航偏角的控制量。

獲取兩個控制量后，用權重耦合，最后的控制量為：

speed_control＝(speed_control_direction +speed_control_theta)/2

將這個控制量通過GPIO 引腳輸出給電機驅動器對PWM 進行調制，從而糾正平臺車的路線。

4.2.3 結構路況下轉彎控制

此路況下與直線路況大致相同，但是要多一個數據的處理。在初始化的代碼中添加一個變量“direction_behind”，用來表示平臺車此刻與前方障礙物的距離。并為其設置一個閾值a，當距離小于這個值的時候進行控制量的調節并輸出給電機驅動器。而這個偏差值控制量是施加在左輪的電機驅動器還是右輪的電機驅動器則取決于左右兩端的距離大小，左端距離大則施加在右輪的電機驅動器上使PWM 占空比提高，右輪加速運轉從而產生差速向左邊轉彎。這種情況下的控制調節一直持續到direction_behind 升到閾值a 之上。顯然，在控制調節期間，direction_behind 在時刻變大，但平臺車也是在時刻前進的，前進之后direction_behind 又再次減小降到閾值之下，又一次進行控制調節，在轉彎完全進行完之前，控制調節的行為是以非常小的周期在斷續進行的。

4.2.4 貼壁行駛路況下的控制

這種路況下，比沿中心線行駛更加容易實現，程序的運行只需要一個變量，加快了運算速度，提高了行駛的穩定性。仍然采用PD 兩個環節來控制。主要控制左端距離即可，為其設置一個閾值b，當direction_left 不等于b 時則開始控制調節工作。此時的導航偏距控制量計算方法發生了變化，主要是distance_error 這個偏差值的定義將會改變。當偏差值小于0 時，將控制量加在左輪的電機驅動器上，當偏差值大于0 時，將控制量加在右輪的電機驅動器上。

5 總結

本文基于思嵐科技生產的RPLidar A2 激光雷達對AGV 平臺車進行路徑規劃研究。

1）采用基于ARM 和linux 操作系統的樹莓派卡片式電腦作為核心控制器，通過VS 平臺編寫激光雷達通信協議，并在樹莓派python 解釋器環境下進行編譯運行，實現了激光配搭平面點云動態實時數據采集。

2）通過編寫相應的串口通信指令，發送指令報文數據，實現激光雷達單次應答模式和多次應答模式的切換選擇；并對采集到的數據進行閾值濾波以及導航特征點提取，基于采用最小二乘法直線路徑擬合方法對特征點中點進行導航線的擬合，根據激光雷達的原點與導航線的相對位置獲取導航偏距及航偏角參數。

3）以導航偏距以及航偏角作為AGV 小車的運動控制輸入，采用比例微分PD 算法對AGV平臺車電機進行速度控制，實現對擬合路徑的跟蹤。實驗結果表明基于激光雷達的AGV 能夠實現自主路徑規劃與導航。