Rplidar掃描測距傳感器測試說明

1、Rplidar360度二維激光掃描測距傳感器測試說明目錄1.測距原理22.性能參數(shù)23.通訊與接口34.請求命令與數(shù)據(jù)獲取34.1 請求命令總覽34.2 停止掃描(STOP)命令請求44.3 測距核心軟重啟(RESET)命令請求44.4 開始掃描采樣(SCAN)命令請求與回應(yīng)數(shù)據(jù)格式54.5 強制掃描采樣(FORCE_SCAN)命令請求與回應(yīng)數(shù)據(jù)格式74.6 設(shè)備信息獲取(GET_INFO)命令請求74.7 設(shè)備健康狀態(tài)獲取(GET_HEALTH)命令請求94.8 總結(jié)105測試說明115.1供電與功耗情況115.2 測試時外圍接線115.3 數(shù)據(jù)接收125.4 曲線擬合13Rplidar36

2、0度二維激光掃描測距傳感器是指 ：RoboPeak開發(fā)的二維激光雷達(LIDAR)解決方案，下文簡稱RPLIDAR。 1.測距原理RPLIDAR 采用了激光三角測距技術(shù)，配合RoboPeak 研發(fā)的高速的視覺采集處理機構(gòu)，可進行每秒高達 2000次以上的測距動作。每次測距過程中，RPLIDAR將發(fā)射經(jīng)過調(diào)制的紅外激光信號，該激光信號在照射到目標物體后產(chǎn)生的反光將被RPLIDAR 的視覺采集系統(tǒng)接受。經(jīng)過嵌入在RPLIDAR 內(nèi)部的DSP 處理器實時解算，被照射到的目標物體與RPLIDAR的距離值以及當前的夾角信息將從通訊接口中輸出。 圖1 RPLIDAR測距工作原理2.性能參數(shù)RPLIDAR主

3、要技術(shù)參數(shù)如下表所示：圖2 RPLIDAR測試性能3.通訊與接口RPLIDAR 標準配置采用 3.3V 電平的串口(UART)作為通訊接口，并且可以隨著客戶需求采用 USB 等其他接口。圖3 RPLIDAR通訊接口和通訊參數(shù)4.請求命令與數(shù)據(jù)獲取4.1 請求命令總覽下表列出了被 RPLIDAR 支持的請求命令，他們的具體使用與 RPLIDAR 的回應(yīng)數(shù)據(jù)格式將在后文分別介紹。圖4請求命令總覽4.2 停止掃描(STOP)命令請求在外部系統(tǒng)發(fā)送了請求命令字段為停止掃描(STOP, 0x25)的請求報文后，RPLIDAR 將退出正在進行的掃描采樣狀態(tài)，關(guān)閉測距系統(tǒng)和激光器，進入空閑模式。如果 RPL

4、IDAR 先前已經(jīng)工作在空閑狀態(tài)或者保護停機狀態(tài)下，則該命令則會被忽略。RPLIDAR 不會為該請求發(fā)送回應(yīng)報文。建議外部系統(tǒng)需要在發(fā)送該請求命令后，延遲 1ms 以上時間后發(fā)送下一次請求。圖5 STOP請求的通訊時序4.3 測距核心軟重啟(RESET)命令請求在外部系統(tǒng)發(fā)送了 RESET 請求后，測距核心將進行軟重啟操作。軟重啟將測距系統(tǒng)恢復(fù)到與通電后一樣的狀態(tài)下。當 RPLIDAR 因為故障進入了保護性停機后，外部系統(tǒng)就可以嘗試發(fā)送 RESET 命令嘗試將 RPLIDAR 恢復(fù)至正常工作狀態(tài)。RPLIDAR 不會為該請求發(fā)送回應(yīng)報文。建議外部系統(tǒng)需要在發(fā)送該請求命令后，延遲 2ms 以上時

5、間后發(fā)送下一次請求。圖 6 RESET 請求的通訊時序4.4 開始掃描采樣(SCAN)命令請求與回應(yīng)數(shù)據(jù)格式RPLIDAR 工作在空閑狀態(tài)時，在外部系統(tǒng)發(fā)送了該請求后，將開始進入測距采樣。每個測距采樣點將使用數(shù)據(jù)應(yīng)答報文發(fā)送至外部系統(tǒng)。如果 RPLIDAR 先前已經(jīng)工作在測距采樣狀態(tài)，則 RPLIDAR 首先將停止正在進行的測距采樣功能，并重新開始新一輪的測距采樣操作。當 RPLIDAR 進入保護性停機后，該命令請求將被忽略。RPLIDAR 會在接受該請求后立刻發(fā)送起始應(yīng)答報文，表示 RPLIDAR 接受了進入掃描采樣狀態(tài)的請求。掃描采樣的數(shù)據(jù)應(yīng)答報文將在 RPLIDAR 的掃描電機穩(wěn)定旋轉(zhuǎn)后

6、不斷的發(fā)送給外部系統(tǒng)，直到外部系統(tǒng)發(fā)送新的請求而停止掃描采樣或者 RPLIDAR 出現(xiàn)故障為止。數(shù)據(jù)應(yīng)答報文格式：RPLIDAR 使用如下的數(shù)據(jù)應(yīng)答報文結(jié)構(gòu)：圖7 掃描測距輸出的數(shù)據(jù)應(yīng)答報文RPLIDAR 在掃描測據(jù)中會將每個采樣點通過上述結(jié)構(gòu)的數(shù)據(jù)應(yīng)答報文發(fā)送至外部系統(tǒng)。其中各字段定義如下：下圖展示了外部系統(tǒng)在發(fā)送掃描采樣請求后的通訊情況：圖8 SCAN請求的通訊時序4.5 強制掃描采樣(FORCE_SCAN)命令請求與回應(yīng)數(shù)據(jù)格式強制掃描采樣(FORCE_SCAN)使得 RPLIDAR 忽略當前掃描電機的工作狀態(tài)而強行進行掃描測距并發(fā)送數(shù)據(jù)應(yīng)答。該請求可以用于設(shè)備測試。RPLIDAR 采用

7、與開始掃描采樣(SCAN)命令類似的處理邏輯來響應(yīng)強制掃描采樣請求。起始應(yīng)答報文與數(shù)據(jù)應(yīng)答報文均與針對 SCAN 的應(yīng)答報文一致。4.6 設(shè)備信息獲取(GET_INFO)命令請求RPLIDAR 在收到外部系統(tǒng)發(fā)送該請求后，將自身諸如序列號、固件/硬件版本等信息作為應(yīng)答發(fā)送回外部系統(tǒng)。圖10 設(shè)備信息獲取對應(yīng)的應(yīng)答報文圖11 GET_INF請求的通訊時序4.7 設(shè)備健康狀態(tài)獲取(GET_HEALTH)命令請求外部系統(tǒng)可以通過發(fā)送該請求了解 RPLIDAR 測距核心的工作狀態(tài)。如果RPLIDAR 因為內(nèi)部故障進入了保護性停機模式，則會在該請求的應(yīng)答中發(fā)送對應(yīng)的錯誤代號。圖12 GET_HEALTH

8、請求報文當外部系統(tǒng)檢測到 RPLIDAR 進入保護性停機模式后，可以嘗試發(fā)送重啟(RESET)命令嘗試重啟 RPLIDAR 解決問題。如果 RPLIDAR 多次進入保護性停機模式，則表示內(nèi)部系統(tǒng)可能出現(xiàn)了不可恢復(fù)性損傷。4.8 總結(jié)上文只是對RPLIDAR的請求、應(yīng)答報文做簡要的說明，具體內(nèi)容參照:360度二維激光掃描測距傳感器rplidar360激光掃描測距傳感器資料rplidar_sdk_v1.4.5doczh-cnrplidar_interface_protocol_cn.pdf。5測試說明5.1供電與功耗情況RPLIDAR 采用測距邏輯系統(tǒng)與電機系統(tǒng)分別供電的模式。外部系統(tǒng)需要單獨為這

9、兩個部分供應(yīng)電能以確保掃描測距數(shù)據(jù)的精確性。 下圖展示了推薦的供電模式。具體的規(guī)格信息請參考下文表格。圖13 RPLIDAR供電與功耗5.2 測試時外圍接線測試時使用的為HLP的UART4作為與RPLIDAR的通訊口，設(shè)置波特率為115200,8位數(shù)據(jù)位，1位停止位，無校驗位。由于HLP板沒有引出定時器引腳，無法直接使用定時器產(chǎn)生PWM，因此采樣PA0口模擬PWM波，PA0經(jīng)過光耦隔離后控制RPLIDAR的MOTOCTL引腳來控制電機的旋轉(zhuǎn)。圖14 測試時外圍接線5.3 數(shù)據(jù)接收在rplidar_test中創(chuàng)建了兩個任務(wù)YY_Task和A1M1_Task，在YY_Task中，主要對RPLIDA

10、R進行連續(xù)的開啟10s，關(guān)斷5s的控制，用來測試對RPLIDAR的控制是否正常。RPLIDAR在接收到開始掃描采樣(SCAN)命令請求后，首先發(fā)送起始報文，接著不斷的發(fā)送報文，將掃描采樣的數(shù)據(jù)發(fā)送出來。為了獲取RPLIDAR發(fā)送的數(shù)據(jù)，在程序中開啟UART4接收中斷，在app_vect.c中編寫UART4中斷函數(shù)void BSP_IntHandlerUSART4 (void)，并在中斷處理函數(shù)中對接收的數(shù)據(jù)進行簡單處理。UART4中斷處理程序中不斷的將接收的數(shù)據(jù)存入A1M1_RXbuffer數(shù)組中，當接收的數(shù)據(jù)中連續(xù)出現(xiàn)A5 5A 05 00 00 40 81時說明RPLIDAR準備輸出采樣數(shù)

11、據(jù)，在緊接的5個字節(jié)在一個方向所測得距離值。以下為掃描采樣后返回的數(shù)據(jù)格式：為了方便處理接收到的數(shù)據(jù)，定義以下結(jié)構(gòu)體：/*角度距離結(jié)構(gòu)體*/typedef struct float sync_quality; / 信號強度 float angle_q6_checkbit; / 角度 float distance_q2;/距離 rplidar_response_measurement_node_t;/*版本信息結(jié)構(gòu)體*/typedef struct u8 model;/傳感器型號 u16 firmware_version;/固件版本號 u8 hardware_version;/硬件版本號 u8

12、serialnum16;/16字節(jié)的產(chǎn)品序列號 rplidar_response_device_info_t;/*設(shè)備狀態(tài)結(jié)構(gòu)體*/typedef struct u8 status;/A1M1狀態(tài) u16 error_code;/錯誤代碼 rplidar_response_device_health_t;在數(shù)據(jù)處理時根據(jù)RPLIDAR回應(yīng)的數(shù)據(jù)格式，對接收到的數(shù)據(jù)進行相應(yīng)的移位、組合等處理后，賦值給結(jié)構(gòu)體內(nèi)的變量。以掃描采樣的數(shù)據(jù)處理為例：A1M1_measurement_Datapos.angle_q6_checkbit = (px1|(px2<<8) >> RPLI

13、DAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0;/角度A1M1_measurement_Datapos.distance_q2 = (px3|(px4<<8)/4.0;/距離A1M1_measurement_Datapos.sync_quality = px0 >> RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT;/信號強度RPLIDAR在距離超出測量范圍（0.156米）或者反射光較強時會出現(xiàn)信號強度弱的情況，在程序中對采樣點的信號強度進行判斷，當出現(xiàn)信號強度小于一定值（10）時，認為本次采樣的數(shù)據(jù)無效，用以減

14、小采樣過程中出現(xiàn)的偏差。5.4 曲線擬合由于采樣的角度不連續(xù)，采樣角度之間有一定的間隔，為了獲取沒有采樣到的角度所對應(yīng)的距離，在程序中添加多項式擬合、拉格朗日插值、最小二乘法程序，根據(jù)所得到的角度、距離，輸入角度來獲取在該角度所對應(yīng)的距離。編寫了如下的函數(shù)實現(xiàn)曲線擬合：void Polynomial_Curve_Fitting(double x, double y, int n, int M, double c)；為多項式曲線擬合，根據(jù)數(shù)組x，y中的數(shù)據(jù)，n為已知點的個數(shù)，M為多項式的項數(shù)，將計算出的多項式的系數(shù)保存到數(shù)組c中，然后根據(jù)數(shù)組c中的系數(shù)得出多項式，再利用float Get_Center_Offset(float angle1,float angle2)函數(shù)得出對應(yīng)角度的距離，再算出兩個角度的中心點以及偏移量。int Smooth(double x12,double y12,double a4,int n,int m