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

描述

該項目是 UIUC SE423 的最終項目。

該項目的目標是讓機器人汽車在建造的軌道上導航，同時避開途中的隨機障礙物。這個項目有幾個主要組成部分。

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

控制機器人汽車

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

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

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

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

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

使用激光雷達檢測障礙物

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

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

這種設(shè)置極大地簡化了我們的尋障程序：根據(jù)收集到的數(shù)據(jù)，我們可以在給定機器人車的初始條件（pose_x、pose_y、pose_theta）的情況下，通過坐標變換獲得軌道的“全局視圖”。然后，將軌道范圍內(nèi)的障礙物讀數(shù)四舍五入應該足以讓我們得到障礙物的邊緣，因為障礙物只能出現(xiàn)在整數(shù)網(wǎ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

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

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;
}

否則，我首先將極坐標轉(zhuǎn)換為笛卡爾坐標，并使用基方程的變化來獲得檢測點的“全局”位置。x_ori和y_ori是極坐標到笛卡爾坐標的直接結(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)的障礙物的簡單標準，我們可以在陣列x:(-6,6), y:(0,12),上將檢測到的點標記為障礙物“x” 。mapCourseStart這mapCourseStart是一個包含 176 (11*16) 個元素的字符數(shù)組，用于存儲軌道網(wǎng)格交叉點的地圖。它在初始化期間包含所有“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'   };

機器人 1 格內(nèi)的點不被考慮，因為它可能是一些隨機數(shù)據(jù)收集錯誤。mapCourseStart障礙物的全局位置(x, y) 與數(shù)組的索引之間存在雙射對應關(guān)系，11*(11-y)+5-x.這里(x, y) 是經(jīng)過坐標變換和平移后激光雷達讀數(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)格覆蓋陣列的圖。的點位于mapCourseStart每個圖塊的中心。

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

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

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

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

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

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

//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);
}

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

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

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

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

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

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

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

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

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

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

為了避免launchpad的CPU1太忙而搞砸了，我在這個項目中使用了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 進行通信，因此我們使用CPUSEL6.

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

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

對于 Optitrack 數(shù)據(jù)，它采用 x、y、theta、剛體數(shù)和幀數(shù)的形式。這些數(shù)據(jù)每個是 2 個字節(jié)，但我們每次只從客戶端接收 1 個字節(jié)。因此，我們需要將數(shù)據(jù)組合在一起以進行整個閱讀。這里我使用了一個 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é)的浮點數(shù)，我們從 Optitrack 恢復數(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)建了一個浮點數(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;
}

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

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

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

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

下面是機器人避開障礙物并前往所需點的視頻。

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

學分

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

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

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