帶有運(yùn)動跟蹤和激光雷達(dá)的機(jī)器人汽車路徑規(guī)劃

描述

該項(xiàng)目是 UIUC SE423 的最終項(xiàng)目。

該項(xiàng)目的目標(biāo)是讓機(jī)器人汽車在建造的軌道上導(dǎo)航，同時避開途中的隨機(jī)障礙物。這個項(xiàng)目有幾個主要組成部分。

• 控制機(jī)器人小車并使用航位推算獲得機(jī)器人小車的位姿
• 使用 F28379D Launchpad 處理激光雷達(dá)讀數(shù)
• 使用激光雷達(dá)識別障礙物
• 使用 A* 算法查找路徑
• 與運(yùn)動跟蹤系統(tǒng)通信以在路徑中導(dǎo)航

控制機(jī)器人汽車

我們使用 F28379D 啟動板的增強(qiáng)型正交編碼器脈沖 (eQEP) 外圍設(shè)備來獲取我們機(jī)器人汽車中使用的電機(jī)的角度。有了這個，很容易得到機(jī)器人汽車的速度與采樣時間和行駛距離。然后，我們使用 PI 控制器來控制機(jī)器人汽車。理想情況下，假設(shè)車輪和地板之間沒有滑動，我們可以通過航位推算來計(jì)算機(jī)器人汽車的姿態(tài)。

處理激光雷達(dá)數(shù)據(jù)

對于這個項(xiàng)目，便宜的 YDLidar X2 型號效果很好。它可以以 360° 視角感應(yīng)0.10 到 8.0m 的范圍。這款激光雷達(dá)的一大優(yōu)點(diǎn)是它會在開機(jī)時自動開始收集數(shù)據(jù)

激光雷達(dá)可以通過串口傳輸數(shù)據(jù)。在這個項(xiàng)目中，SCID用于接受數(shù)據(jù)，激光雷達(dá)的數(shù)據(jù)通過launchpad的pin 9進(jìn)來。

我使用 8 狀態(tài)狀態(tài)機(jī)來解釋和存儲激光雷達(dá)數(shù)據(jù)。得到的數(shù)據(jù)是一個包含 360 個條目的數(shù)組形式，每個條目包含激光雷達(dá)檢測到的點(diǎn)的距離和時間戳信息，這個數(shù)組的索引是收集數(shù)據(jù)的角度。

使用激光雷達(dá)檢測障礙物

得到上一步的激光雷達(dá)數(shù)據(jù)后，我們得到了機(jī)器人看到的信息：機(jī)器人在中心，有一段距離對應(yīng)從0到359°的所有整數(shù)角。因此，極坐標(biāo)圖足以讓我們可視化結(jié)果。

在我們的項(xiàng)目中，我們在機(jī)電實(shí)驗(yàn)室里搭建了一個12*12英尺的軌道，供機(jī)器人小車在里面跑，障礙物是2*2英尺的木塊，只能放在軌道的整數(shù)格子上。

這種設(shè)置極大地簡化了我們的尋障程序：根據(jù)收集到的數(shù)據(jù)，我們可以在給定機(jī)器人車的初始條件（pose_x、pose_y、pose_theta）的情況下，通過坐標(biāo)變換獲得軌道的“全局視圖”。然后，將軌道范圍內(nèi)的障礙物讀數(shù)四舍五入應(yīng)該足以讓我們得到障礙物的邊緣，因?yàn)檎系K物只能出現(xiàn)在整數(shù)網(wǎng)格上。下面的初始條件對應(yīng)上圖第二個，其中pose_x是-4，pose_y是6，pose_theta是30°。

//initial condition
double pose_x = -4; //x_position of the robot car
double pose_y = 6; //x_position of the robot car
//need to put negative initial angle because the Lidar is rotating CW
double pose_theta = 0; //angle of robot car
double pose_rad; //angle in rad

坐標(biāo)變換在 main 函數(shù)內(nèi)的 while 循環(huán)中完成。乒乓緩沖區(qū)用于確保有足夠的時間來處理所有數(shù)據(jù)。在下面的代碼中，我首先遍歷緩沖區(qū)數(shù)組中的所有點(diǎn)。如果激光雷達(dá) 0.1 m 范圍內(nèi)的障礙物（激光雷達(dá)無法檢測到，因此距離為 0），我將其放在原點(diǎn)，該原點(diǎn)對應(yīng)于全局原點(diǎn)。

if (pingpts[i].distance ==0) {
        x_f[i].distance = 0;
        y_f[i].distance = 0;
        x_f[i].timestamp = pingpts[i].timestamp;
        y_f[i].timestamp = pingpts[i].timestamp;
}

否則，我首先將極坐標(biāo)轉(zhuǎn)換為笛卡爾坐標(biāo)，并使用基方程的變化來獲得檢測點(diǎn)的“全局”位置。x_ori和y_ori是極坐標(biāo)到笛卡爾坐標(biāo)的直接結(jié)果，而x_f和y_f是變化基和平移的最終結(jié)果。

else {
        x_ori[i] = pingpts[i].distance*cos(i*0.01745329);//0.017453292519943 is pi/180
        y_ori[i] = pingpts[i].distance*sin(i*0.01745329);
        x_f[i].distance = (x_ori[i])*cos(pose_rad)+(y_ori[i])*sin(pose_rad)+pose_x; //change basis
        y_f[i].distance = -(x_ori[i])*sin(pose_rad)+(y_ori[i])*cos(pose_rad)+pose_y; //change basis

最后，通過僅考慮范圍內(nèi)的障礙物的簡單標(biāo)準(zhǔn)，我們可以在陣列x:(-6,6), y:(0,12),上將檢測到的點(diǎn)標(biāo)記為障礙物“x” 。mapCourseStart這mapCourseStart是一個包含 176 (11*16) 個元素的字符數(shù)組，用于存儲軌道網(wǎng)格交叉點(diǎn)的地圖。它在初始化期間包含所有“0”，這意味著還沒有障礙物。

char mapCourseStart[176] =      //16x11
{   '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0',
    '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0',
    '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0',
    '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0',
    '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0',
    '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0',
    '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0',
    '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0',
    '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0',
    '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0',
    '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0',
    '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0',
    '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0',
    '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0',
    '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0',
    '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0'   };

機(jī)器人 1 格內(nèi)的點(diǎn)不被考慮，因?yàn)樗赡苁且恍╇S機(jī)數(shù)據(jù)收集錯誤。mapCourseStart障礙物的全局位置(x, y) 與數(shù)組的索引之間存在雙射對應(yīng)關(guān)系，11*(11-y)+5-x.這里(x, y) 是經(jīng)過坐標(biāo)變換和平移后激光雷達(dá)讀數(shù)的位置。

// code to process the obstacle
        if ((round(x_f[i].distance) >-6) && (round(x_f[i].distance) <6) ) { // within x range
            if ((round(y_f[i].distance) > 0)&& round(y_f[i].distance) < 12) { //within y range
                if ((fabs(x_f[i].distance-pose_x) > 1) && (fabs(y_f[i].distance-pose_y) > 1)) { //neglect points that are too close to the robot car
                    mapCourseStart[(11*(11-(int)(round(y_f[i].distance))))+5+(int)(round(x_f[i].distance))] = 'x'; //set 'x' at correct location
                }
            }
        }
        x_f[i].timestamp = pingpts[i].timestamp;
        y_f[i].timestamp = pingpts[i].timestamp;
    }
}

下圖是mapCourseStart用網(wǎng)格覆蓋陣列的圖。的點(diǎn)位于mapCourseStart每個圖塊的中心。

因此，在初始條件pose_x= -4、pose_y= 6 和= 30° 的機(jī)器人上看，數(shù)組pose_theta中索引為 18、29、40、41、42 的點(diǎn)應(yīng)該從“0”變?yōu)椤皒” mapCourseStart.

從 Code Composer Studio 的表達(dá)式窗口檢查mapCourseStart數(shù)組的結(jié)果，我們從代碼中得到預(yù)期的結(jié)果，索引 18、29、40、41、42 是障礙物 ('x')。

下一步是使用路徑規(guī)劃算法來找到機(jī)器人要走的路徑。

使用對角線步驟實(shí)現(xiàn) A* 算法

A* 是一種路徑規(guī)劃算法，它同時考慮了行進(jìn)距離（g）和從當(dāng)前點(diǎn)到目的地的距離（啟發(fā)式值，h）。然后它會選擇去g+h值最小的點(diǎn)。更詳細(xì)的解釋可以在 https://www.redblobgames.com/pathfinding/a-star/introduction.html 找到。

前幾個學(xué)期的代碼只考慮了水平和垂直方向的移動，這里的啟發(fā)值是由 給出的曼哈頓距離|x1-x2|+|y1-y2|。但是，如果我們考慮對角線移動，它會更有效。下面是我添加到getNeighborsA* 算法函數(shù)中的代碼。要進(jìn)行對角線移動，我們需要確保相鄰的垂直或水平鄰居是可達(dá)的。也就是說，如果我們想去左上鄰居，我們需要確保我們可以同時到達(dá)左上鄰居。

//top left corner, only can travel when both top and left are reachable
	if ((canTravel(rowCurr - 1, colCurr) == 1) && (canTravel(rowCurr, colCurr - 1) == 1)) {
		if (canTravel(rowCurr - 1, colCurr - 1) == 1) {
			nodeToAdd.row = rowCurr - 1;
			nodeToAdd.col = colCurr - 1;
			neighbors[numNeighbors] = nodeToAdd;
			numNeighbors++;
		}
	}
	//top right, need top and right reachable
	if ((canTravel(rowCurr - 1, colCurr) == 1) && (canTravel(rowCurr, colCurr + 1) == 1)) {
		if (canTravel(rowCurr - 1, colCurr + 1) == 1) {
			nodeToAdd.row = rowCurr - 1;
			nodeToAdd.col = colCurr + 1;
			neighbors[numNeighbors] = nodeToAdd;
			numNeighbors++;
		}
	}
	//bottom left, need bottom and left reachable
	if ((canTravel(rowCurr + 1, colCurr) == 1) && (canTravel(rowCurr, colCurr - 1) == 1)) {
		if (canTravel(rowCurr + 1, colCurr - 1) == 1) {
			nodeToAdd.row = rowCurr + 1;
			nodeToAdd.col = colCurr - 1;
			neighbors[numNeighbors] = nodeToAdd;
			numNeighbors++;
		}
	}
	//bottom right, need bottom and right reachable
	if ((canTravel(rowCurr + 1, colCurr) == 1) && (canTravel(rowCurr, colCurr + 1) == 1)) {
		if (canTravel(rowCurr + 1, colCurr + 1) == 1) {
			nodeToAdd.row = rowCurr + 1;
			nodeToAdd.col = colCurr + 1;
			neighbors[numNeighbors] = nodeToAdd;
			numNeighbors++;
		}
	}

如果我們可以對角線旅行，則啟發(fā)式值更接近歐幾里得距離而不是曼哈頓距離。因此，我更新了啟發(fā)式值函數(shù)，并將所有與距離相關(guān)的值從整數(shù)類型更改為雙精度類型。

double heuristic(int rowCurr, int colCurr, int rowGoal, int colGoal)
{
	int rowDiff = rowCurr - rowGoal;
	int colDiff = colCurr - colGoal;
	return sqrt(rowDiff * rowDiff + colDiff * colDiff);
}

最后，對角線行進(jìn)的距離是 sqrt(2)，而不是之前的垂直和水平移動中的 1。所以我更新了行進(jìn)距離計(jì)算。

if (abs(next.row - minDistNode.row) + abs(next.col - minDistNode.col) == 2) {
    next.distTravelFromStart = currDist + sqrt(2);
} else {
    next.distTravelFromStart = currDist + 1;
}

以下是小型、中型和大型地圖路徑規(guī)劃的測試用例。我們可以看到，對角線行駛確實(shí)可以節(jié)省一些步數(shù)（pathLen）和行駛距離（next.distTravelFromStart值），因此效率更高。

A*算法中的點(diǎn)在nodeTrack數(shù)組中被跟蹤，調(diào)用函數(shù)后，我們可以得到從起點(diǎn)到終點(diǎn)的正確順序的reconstructPath路徑。pathRowpathCol

從點(diǎn)到點(diǎn)的導(dǎo)航是通過提供的xy_control函數(shù)完成的。我們需要將具有正確索引的pathRowand值傳遞給函數(shù)。pathCol

與運(yùn)動跟蹤系統(tǒng)通信

我們可以使用航位推算來獲取我們機(jī)器人汽車的位姿數(shù)據(jù)。但是，由于車輪在運(yùn)行過程中可能會打滑，因此航位推算會出現(xiàn)一些誤差，這些誤差可能會隨著時間的推移而累積。因此，為了確保機(jī)器人汽車按預(yù)期運(yùn)行，我們使用機(jī)電一體化實(shí)驗(yàn)室設(shè)置的 Optitrack 系統(tǒng)來獲得我們的機(jī)器人汽車更精確的姿勢。

為了使 Optitrack 系統(tǒng)正常工作，我們首先需要將機(jī)器人小車設(shè)置為 0°，機(jī)器人的坐標(biāo)方向與全局坐標(biāo)對齊。在Motive軟件中選擇設(shè)置在機(jī)器人車頂部的反光球，并運(yùn)行通過連接實(shí)驗(yàn)室計(jì)算機(jī)的路由器發(fā)送數(shù)據(jù)的腳本后，我們只需要找到一種接收數(shù)據(jù)的方法即可。在這里，我使用了 Wiznet W5500 芯片來實(shí)現(xiàn)這一點(diǎn)。W5500 芯片是一款硬連線 TCP/IP 嵌入式以太網(wǎng)控制器，可通過 SPI（串行外設(shè)接口）為嵌入式系統(tǒng)實(shí)現(xiàn)更輕松的互聯(lián)網(wǎng)連接。

W5500接GPIO122、123、124、125，分別對應(yīng)launchpad的12、13、17、18腳。數(shù)據(jù)接收采用UDP協(xié)議，機(jī)器人小車作為UDP服務(wù)器，與Optitrack連接的計(jì)算機(jī)作為UDP客戶端。

我使用的以太網(wǎng)轉(zhuǎn)換器模塊為我提供了將路由器連接到機(jī)器人汽車的能力。將路由器設(shè)置為客戶端模式（通常用于沒有wifi功能但有以太網(wǎng)端口的設(shè)備的模式）并將其連接到配置網(wǎng)頁中發(fā)送數(shù)據(jù)的路由器，機(jī)器人小車可以從Optitrack系統(tǒng)中獲取數(shù)據(jù).

為了避免launchpad的CPU1太忙而搞砸了，我在這個項(xiàng)目中使用了CPU2來設(shè)置UDP服務(wù)器，接收和處理來自O(shè)ptiTrack的數(shù)據(jù)。一般來說，要使用CPU2，我們首先需要確保CPU2中使用的GPIO在CPU1初始化GPIO時，其所有權(quán)設(shè)置為CPU2。這里，我們需要在CPU1的main函數(shù)中設(shè)置GPIO 0、122、123、124對CPU2的所有權(quán)。

//wiznet reset
    GPIO_SetupPinMux(0, GPIO_MUX_CPU2, 0);
    GPIO_SetupPinOptions(0, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPASET.bit.GPIO0 = 1;

    GPIO_SetupPinMux(125, GPIO_MUX_CPU2, 0); // wiznet cs
    GPIO_SetupPinOptions(125, GPIO_OUTPUT, GPIO_PUSHPULL); // Make GPIO2 an Output Pin
    GpioDataRegs.GPDSET.bit.GPIO125 = 1; //Initially Set GPIO2/SS High so wiznet is not selected
    GPIO_SetupPinMux(122, GPIO_MUX_CPU2, 6); //SPISIMOC/WIZNET
    GPIO_SetupPinMux(123, GPIO_MUX_CPU2, 6); //SPISOMIC/WIZNET
    GPIO_SetupPinMux(124, GPIO_MUX_CPU2, 6); //SPICLKC/WIZNET

然后我們需要在DevCfgRegs寄存器中將我們使用的外設(shè)設(shè)置為 1。這里我們使用 SPIC 進(jìn)行通信，因此我們使用CPUSEL6.

EALLOW;
DevCfgRegs.CPUSEL6.bit.SPI_C = 1;
EDIS;

在 CPU1 中進(jìn)行基本設(shè)置后，我們可以放心地將所有 UDP 服務(wù)器代碼放入 CPU2 的函數(shù)中。我在這里為 CPU2 使用了一個單獨(dú)的項(xiàng)目。

對于 Optitrack 數(shù)據(jù)，它采用 x、y、theta、剛體數(shù)和幀數(shù)的形式。這些數(shù)據(jù)每個是 2 個字節(jié)，但我們每次只從客戶端接收 1 個字節(jié)。因此，我們需要將數(shù)據(jù)組合在一起以進(jìn)行整個閱讀。這里我使用了一個 union 來存儲從 Optitrack 接收到的一組數(shù)據(jù)，它有一個rawData類型為 uint16_t 的Data數(shù)組和一個類型為 float 的數(shù)組。

typedef union optiData_s {
    uint16_t rawData[10];
    float Data[5];
} optiData_t;

然后通過組合接收到的數(shù)據(jù)，我們得到每個 2 字節(jié)的原始數(shù)據(jù)，并將這 2 字節(jié)的數(shù)據(jù)組合為 4 字節(jié)的浮點(diǎn)數(shù)，我們從 Optitrack 恢復(fù)數(shù)據(jù)。最后，通過將 CPU2 中的 IPC 寄存器設(shè)置為 1，我們中斷 CPU1 接收數(shù)據(jù)。

recvsize = recvfrom(0, (uint8_t *)buffer, len, recvfrom_ip, &recvfrom_port);
mydata.rawData[0] = buffer[0] | (buffer[1] << 8);
mydata.rawData[1] = buffer[2] | (buffer[3] << 8);
mydata.rawData[2] = buffer[4] | (buffer[5] << 8);
mydata.rawData[3] = buffer[6] | (buffer[7] << 8);
mydata.rawData[4] = buffer[8] | (buffer[9] << 8);
mydata.rawData[5] = buffer[10] | (buffer[11] << 8);
mydata.rawData[6] = buffer[12] | (buffer[13] << 8);
mydata.rawData[7] = buffer[14] | (buffer[15] << 8);
mydata.rawData[8] = buffer[16] | (buffer[17] << 8);
mydata.rawData[9] = buffer[18] | (buffer[19] << 8);
recvcount += 1;
cpu2tocpu1[0] = mydata.Data[0];
cpu2tocpu1[1] = mydata.Data[1];
cpu2tocpu1[2] = mydata.Data[2];
cpu2tocpu1[3] = mydata.Data[3];
cpu2tocpu1[4] = mydata.Data[4];
IpcRegs.IPCSET.bit.IPC0 = 1;

下圖是從 CPU2 讀取的結(jié)果。

然后，我們?yōu)閮蓚€ CPU 創(chuàng)建了一個浮點(diǎn)數(shù)組cpu2tocpu1來存儲來自 Optitrack 的數(shù)據(jù)。然后我們在 cmd 文件中找到cpu2tocpu1ram 的位置，將兩者設(shè)置在 CPU1 和 CPU2 中的相同位置，然后我們可以將值寫入 CPU2 中的這個數(shù)組并在 CPU1 中檢索它。

//IPC
__interrupt void CPU2toCPU1IPC0(void){
    GpioDataRegs.GPBTOGGLE.bit.GPIO52 = 1;
    x = cpu2tocpu1[0];
    y = cpu2tocpu1[1];
    theta = cpu2tocpu1[2];
    number = cpu2tocpu1[3];
    framecount = cpu2tocpu1[4];
    OPTITRACKps = UpdateOptitrackStates(ROBOTps);
    ROBOTps.x = OPTITRACKps.x;
    ROBOTps.y = OPTITRACKps.y;
    ROBOTps.theta = OPTITRACKps.theta;
    UARTPrint = 1;
    IpcRegs.IPCACK.bit.IPC0 = 1;
    PieCtrlRegs.PIEACK.all = PIEACK_GROUP1;
}

要運(yùn)行它，我們首先需要build 使用 CPU2 的項(xiàng)目和debug CPU1 項(xiàng)目。CPU1 運(yùn)行后，右鍵單擊 cpu2 和connect target. run菜單下 ， load CPU2.out 文件。最后，運(yùn)行 CPU1，然后運(yùn)行 ??CPU2。

有了 Optitrack 數(shù)據(jù)，我們可以使用它來控制xy_control功能。下面是一個演示視頻，展示了機(jī)器人汽車通過運(yùn)動跟蹤系統(tǒng)到達(dá)幾個預(yù)設(shè)點(diǎn)并告訴它它的位置。

結(jié)合障礙物檢測和 A*

把東西放在一起后，我們可以讓我們的機(jī)器人汽車自動行駛到目的地，避開障礙物。

下面是機(jī)器人避開障礙物并前往所需點(diǎn)的視頻。

它也可以穿過一個小迷宮。

學(xué)分

感謝 Dan Block 教授幫助我解決了我在這個項(xiàng)目中遇到的所有問題并提供了必要的入門代碼。

感謝 TA Hang Cui 對 UDP 通信的幫助和 CPU2 的入門代碼，

感謝 Scott Manhart 設(shè)計(jì)激光雷達(dá)支架。