描述

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

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

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

控制機器人汽車

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

處理激光雷達數據

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

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

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

使用激光雷達檢測障礙物

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

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

這種設置極大地簡化了我們的尋障程序：根據收集到的數據，我們可以在給定機器人車的初始條件（pose_x、pose_y、pose_theta）的情況下，通過坐標變換獲得軌道的“全局視圖”。然后，將軌道范圍內的障礙物讀數四舍五入應該足以讓我們得到障礙物的邊緣，因為障礙物只能出現在整數網格上。下面的初始條件對應上圖第二個，其中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 函數內的 while 循環中完成。乒乓緩沖區用于確保有足夠的時間來處理所有數據。在下面的代碼中，我首先遍歷緩沖區數組中的所有點。如果激光雷達 0.1 m 范圍內的障礙物（激光雷達無法檢測到，因此距離為 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;
}

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

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

最后，通過僅考慮范圍內的障礙物的簡單標準，我們可以在陣列x:(-6,6), y:(0,12),上將檢測到的點標記為障礙物“x” 。mapCourseStart這mapCourseStart是一個包含 176 (11*16) 個元素的字符數組，用于存儲軌道網格交叉點的地圖。它在初始化期間包含所有“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 格內的點不被考慮，因為它可能是一些隨機數據收集錯誤。mapCourseStart障礙物的全局位置(x, y) 與數組的索引之間存在雙射對應關系，11*(11-y)+5-x.這里(x, y) 是經過坐標變換和平移后激光雷達讀數的位置。

// 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用網格覆蓋陣列的圖。的點位于mapCourseStart每個圖塊的中心。

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

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

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

使用對角線步驟實現 A* 算法

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

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

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

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

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

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

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

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

與運動跟蹤系統通信

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

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

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

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

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

//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寄存器中將我們使用的外設設置為 1。這里我們使用 SPIC 進行通信，因此我們使用CPUSEL6.

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

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

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

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

然后通過組合接收到的數據，我們得到每個 2 字節的原始數據，并將這 2 字節的數據組合為 4 字節的浮點數，我們從 Optitrack 恢復數據。最后，通過將 CPU2 中的 IPC 寄存器設置為 1，我們中斷 CPU1 接收數據。

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 讀取的結果。

然后，我們為兩個 CPU 創建了一個浮點數組cpu2tocpu1來存儲來自 Optitrack 的數據。然后我們在 cmd 文件中找到cpu2tocpu1ram 的位置，將兩者設置在 CPU1 和 CPU2 中的相同位置，然后我們可以將值寫入 CPU2 中的這個數組并在 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 數據，我們可以使用它來控制xy_control功能。下面是一個演示視頻，展示了機器人汽車通過運動跟蹤系統到達幾個預設點并告訴它它的位置。

結合障礙物檢測和 A*

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

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

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

學分

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

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

感謝 Scott Manhart 設計激光雷達支架。