Rplidar掃描測(cè)距傳感器測(cè)試說(shuō)明

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

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

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

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

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

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

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

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

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

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

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

12、serialnum16;/16字節(jié)的產(chǎn)品序列號(hào) rplidar_response_device_info_t;/*設(shè)備狀態(tài)結(jié)構(gòu)體*/typedef struct u8 status;/A1M1狀態(tài) u16 error_code;/錯(cuò)誤代碼 rplidar_response_device_health_t;在數(shù)據(jù)處理時(shí)根據(jù)RPLIDAR回應(yīng)的數(shù)據(jù)格式，對(duì)接收到的數(shù)據(jù)進(jìn)行相應(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;/信號(hào)強(qiáng)度RPLIDAR在距離超出測(cè)量范圍（0.156米）或者反射光較強(qiáng)時(shí)會(huì)出現(xiàn)信號(hào)強(qiáng)度弱的情況，在程序中對(duì)采樣點(diǎn)的信號(hào)強(qiáng)度進(jìn)行判斷，當(dāng)出現(xiàn)信號(hào)強(qiáng)度小于一定值（10）時(shí)，認(rèn)為本次采樣的數(shù)據(jù)無(wú)效，用以減

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