使用激光雷達(dá)和運(yùn)動(dòng)捕捉進(jìn)行自主路徑規(guī)劃

激光雷達(dá)有什么作用？

汽車上的二維激光雷達(dá)

?

激光雷達(dá)只是用于距離測(cè)量的傳感器。激光從激光雷達(dá)中射出，返回所需的時(shí)間決定了與傳感器的距離。許多 2D 激光雷達(dá)，包括我在這個(gè)項(xiàng)目中使用的激光雷達(dá)，都會(huì)在整個(gè)傳感器周圍為您提供許多點(diǎn)和距離，如上圖所示。

本項(xiàng)目使用的YDLIDAR X2

?

?

以上是激光雷達(dá)可以做什么的一個(gè)例子。

動(dòng)作捕捉？

簡(jiǎn)單地說，我不需要使用動(dòng)作捕捉，因?yàn)槲铱梢允褂?a target='_blank' class='arckwlink_none'>電機(jī)的測(cè)量值來推算機(jī)器人汽車；但是，機(jī)電一體化室設(shè)置了動(dòng)作捕捉系統(tǒng)。這使得機(jī)器人小車的位置非常容易定位；因此，使精確的動(dòng)作易于執(zhí)行。我將一個(gè) Orange PI（一種便宜的 Raspberry PI）連接到動(dòng)作捕捉軟件的網(wǎng)絡(luò)并檢索數(shù)據(jù)。然后，這會(huì)將數(shù)據(jù)發(fā)送到微控制器。

機(jī)器人汽車電機(jī)控制

為了開始這個(gè)項(xiàng)目的編碼部分，我將回顧控制直流電機(jī)的代碼

// Read the motor rotation
    RightWheel = readEncRight();
    LeftWheel = -readEncLeft();
    
    // distance ft
    XLeftK = 1/(radftL/LeftWheel);
    XRightK = 1/(radftR/RightWheel);
    // vel ft/s
    VLeftK = (XLeftK - XLeftK1)/0.004;
    VRightK = (XRightK - XRightK1)/0.004;

下面的代碼讀取相應(yīng)電機(jī)的旋轉(zhuǎn)角度，并使用已知的車輪半徑，我可以計(jì)算出車輪的行駛距離和速度。

float readEncLeft(void) {
    int32_t raw = 0;
    uint32_t QEP_maxvalue = 0xFFFFFFFFU; //4294967295U
    raw = EQep1Regs.QPOSCNT;
    if (raw >= QEP_maxvalue/2) raw -= QEP_maxvalue; // I don't think this is needed an           d never true
    // 5 North South magnet poles in the encoder disk so 5 square waves per one revolution of the
    // DC motor's back shaft. Then Quadrature Decoder mode multiplies this by 4 so 20 counts per one rev
    // of the DC motor's back shaft. Then the gear motor's gear ratio is 30:1.
    return (raw*(1/(20*30/(2*PI))));
}

一旦每 1ms 計(jì)算一次速度和距離，我就實(shí)現(xiàn)了一個(gè) PI 控制器，以通過確定的 Vref 和 Turn 平滑地控制電機(jī)。

// PI

    eturn = turn + (VLeftK - VRightK);

    ekLeft = Vref - VLeftK - (Kturn*eturn);
    IkLeft = IkLeft1 + 0.004*((ekLeft + ekLeft1)/2);
    uLeft = (Kp*ekLeft) + (Ki*IkLeft);

    ekRight = Vref - VRightK + (Kturn*eturn);
    IkRight = IkRight1 + 0.004*((ekRight + ekRight1)/2);
    uRight = (Kp+ekRight) + (Ki*IkRight);

在 PI 控制器計(jì)算之后，我只需將電機(jī)設(shè)置為確定的 uLeft 和 uRight 值。我還必須使 uLeft 和 uRight 飽和，以防積分結(jié)束是 PID 控制的常見問題。我確保將當(dāng)前狀態(tài)與過去狀態(tài)相同。

//integral wind up
    if(fabs(uLeft) >= 10) {
        IkLeft = IkLeft1;
    }
    if(fabs(uRight) >= 10){
        IkRight = IkRight1;
    }


    setEPWM2A(uRight);
    setEPWM2B(-uLeft);

    //states
    XLeftK1 = XLeftK;
    XRightK1 = XRightK;
    IkLeft1 = IkLeft;
    IkRight1 = IkRight;
    ekLeft1 = ekLeft;
    ekRight1 = ekRight;

激光雷達(dá)數(shù)據(jù)設(shè)置

要使用激光雷達(dá)，我們必須找出我們從傳感器獲得的數(shù)據(jù)以及如何組織它。數(shù)據(jù)通過 SPIRXD 從激光雷達(dá)發(fā)送到微控制器。為與激光雷達(dá)通信而編寫的代碼使用狀態(tài)來檢查和讀取傳感器的初始化和數(shù)據(jù)。

激光雷達(dá)數(shù)據(jù)結(jié)構(gòu)

?

來自 LIDAR 的數(shù)據(jù)以數(shù)據(jù)包的形式發(fā)送，所有數(shù)據(jù)包都以數(shù)據(jù)包頭 0xAA 開頭。這是我們首先檢查的內(nèi)容，如果找到 0x55AA，我們可以進(jìn)入狀態(tài) 2，否則我們繼續(xù)尋找。

if(state == 0) { //check 0xaa
        if(data == 0xAA) {
            state = 1;
        } else {
            state = 0;
        }
} else if (state == 1) {//check 0x55
        if(data == 0x55) {
            state = 2;
        } else {
            state = 0;
        }

接下來，我們查看發(fā)送的數(shù)據(jù)包是起始數(shù)據(jù)包還是點(diǎn)云數(shù)據(jù)包，0x00。如果是我們推進(jìn)的點(diǎn)云包。

} else if (state == 2) {//check 0x0
        if (data == 0x0) {
            state = 3;
//            packetcount+=1;
        } else {
            state = 0;
        }

然后我們讀取數(shù)據(jù)包中的樣本數(shù)量并記錄下來。

} else if (state == 3){ //read sample size
        sampleSize = data;
        state = 4;
}

接下來，是讀取??并記錄開始和結(jié)束角度。

} else if (state == 4) {//read starting angle lsb
        startangleLSB = data;
        state = 5;
    } else if (state == 5) {//record starting angle
            start_angle = ((((data<<8)| startangleLSB)>>1)&0x7FFF)/64.0;
        state = 6;
    } else if (state == 6) { //read end angle
            endLSB = data;
        state = 7;
    } else if (state == 7) {//record end angle
        end_angle = ((((data<<8)| endLSB)>>1)&0x7FFF)/64.0;

        if (end_angle < start_angle) {
            cor_end = end_angle + 360;
        } else {
            cor_end = end_angle;
        }
        delta_angle = cor_end-start_angle;
        state = 8;
}

最后，我們可以記錄 LIDAR 數(shù)據(jù)點(diǎn)并告訴微控制器忽略檢查，因?yàn)槲覀兗僭O(shè)只要 LIDAR 運(yùn)行它就會(huì)保持不變。我們確保使用乒乓緩沖區(qū)來確保我們有足夠的時(shí)間來記錄數(shù)據(jù)并且不會(huì)遺漏任何數(shù)據(jù)。

} else if (state == 8) {//record samples and ignore the check code
        if(position > 1) {
            if (dis_state == 0){
                dLSB = data;
                dis_state = 1;
            } else if (dis_state == 1){

                    float raw_angle = (delta_angle)/(sampleSize - 1) * (sampleindex) +start_angle;
                    sampleindex = sampleindex+1;
                    if(sampleindex == sampleSize) {
                        sampleindex = 0;
                    }
                    pts[arrayIndex].rawAngle = raw_angle;
                    if(dist == 0){
                        cal_angle = raw_angle;
                    } else {
                        cal_angle = raw_angle + atan(21.8*(155.3-dist)/(155.3*dist))*57.296;
                    }
                    pts[arrayIndex].cor_angle = cal_angle;

                    if (pingpongflag == 1) {
                        pingpts[((int16_t)(cal_angle + 0.5))%360].distance = dist;
                        pingpts[((int16_t)(cal_angle + 0.5))%360].timestamp = numTimer0calls;
                    } else {
                        pongpts[((int16_t)(cal_angle + 0.5))%360].distance = dist;
                        pongpts[((int16_t)(cal_angle + 0.5))%360].timestamp = numTimer0calls;
                    }
                    dis_count += 1;
                    dis_state = 0;
                    if (arrayIndex < 599) {
                        arrayIndex += 1;
                    }
            }
        }

現(xiàn)在我們有數(shù)據(jù)了！我們還確保獲取數(shù)據(jù)并將其組織成一個(gè)很好的結(jié)構(gòu)，說明角度、距離和時(shí)間戳。我還包括了為我的機(jī)器人創(chuàng)建的自定義 3D 零件。

A* 算法

由于我們有激光雷達(dá)工作，是時(shí)候讓 A* 算法為機(jī)器人的路徑規(guī)劃工作了。如前所述，A* 是 Dijkstra 算法的修改版本。

A* 和 Dijkstra 算法都使用優(yōu)先級(jí)隊(duì)列來優(yōu)先考慮成本較低或較高的路徑。優(yōu)先級(jí)隊(duì)列與普通隊(duì)列不同，它基于先進(jìn)先出，而優(yōu)先級(jí)隊(duì)列按“最高優(yōu)先級(jí)”、最低成本組織其隊(duì)列。有關(guān)優(yōu)先級(jí)隊(duì)列背后的代碼的更多信息，請(qǐng)?jiān)L問下面的鏈接。

http://pages.cs.wisc.edu/~vernon/cs367/notes/11.PRIORITY-Q.html

解釋 A* 算法的最簡(jiǎn)單方法是，它既需要從起點(diǎn)的實(shí)際距離，又需要估計(jì)到目標(biāo)的距離。

f=g+h

上面的等式是 A* 的基礎(chǔ)，因?yàn)?F 是節(jié)點(diǎn)的總成本，G 是當(dāng)前節(jié)點(diǎn)和起始節(jié)點(diǎn)之間的距離，H 是啟發(fā)式距離，即從當(dāng)前節(jié)點(diǎn)到結(jié)束節(jié)點(diǎn)的估計(jì)距離。完整的 A* 算法函數(shù)可以在下面的源代碼中找到。我將分享幫助我為我的地圖創(chuàng)建算法的偽代碼。

// A* 
Function A*(start,goal)

// Initialize the closed list, the set of nodes already evaluated Closedset = the empty set 
//Initialize the open list, the set of tentative nodes to be evaluated //initially containing the start node 
Start g score = 0 Start 
f score = g score plus h “Manhattan distance from start to goal” 
Openset = {start} came_from = empty 
// The map of the navigated nodes
while the open list is not empty  
    Find the node with the least f on the open list, call it "q"  
    Pop q off the open list  
    Generate q's 4 neighbors  
    For each neighbor  If neighbor is the goal, stop the search  
        neighbor.g = q.g + distance between neighbor and q 
        // h distance is the “Manhattan” distance on a square grid  
        neighbor.h = distance from goal to neighbor  
        neighbor.f = neighbor.g + neighbor.h  

        If a node with the same position as neighbor is in the OPEN list \  which has a lower f than neighbor, skip adding this neighbor  
        
        if a node with the same position as neighbor is in the CLOSED list \  which has a lower f than neighbor, skip adding this neighbor  

        otherwise, add the node to the open list and came_from[neighbor] = q //set neighbor’s parent equal to q  
    end
    push q on the closed list
end

運(yùn)行 A* 算法

完成我的 A* 代碼后，我現(xiàn)在可以將地圖設(shè)置為我想要的任何大小，我選擇了 16X11，因?yàn)檫@是我將要駕駛的網(wǎng)格的大小。

int startRow = 4;
int startCol = 5;
int endRow = 15;
int endCol = 1;
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',
    'x', 'x', 'x', 'x', '0', '0', '0', 'x', 'x', 'x', 'x',    //start row
    '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0',
    '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0',
    '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0',
    '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0',
    '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0',
    '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0',
    '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0',
    '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0',
    '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0',
    '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0',
    '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0'   };

if (PathFunction == 1){ // 1 for A* path
            // create the map
            mapRowSize = 16;
            mapColSize = 11;
            //reset Search flag
            obsSearch = 0;
            if(astarC == 0){   //Starting position
                startRow = 4;
                startCol = 5;
            }else{
                //floor the astar start point
                startRow = floorf(ROBOTps.y);
                startCol = floorf(ROBOTps.x);
            }
            astarC++;
            int i = 0;
            for(i = 0; i//*mapcolsize;>copy m1 into map for solving
                map[i] = mapCourseStart[i];
            }

上面的代碼創(chuàng)建了一個(gè) 16X11 的地圖，并設(shè)置了一個(gè)起始行和起始列，并復(fù)制了一個(gè)沒有障礙物的預(yù)定地圖，這些地圖將在添加時(shí)更新。

astar(startRow,startCol,endRow,endCol); // solve path

然后，我們使用指定的端點(diǎn)運(yùn)行 A* 函數(shù)。這應(yīng)該導(dǎo)致路徑。經(jīng)過幾次檢查以確保起點(diǎn)和終點(diǎn)在界限內(nèi)并且它們不在障礙物上。A* 函數(shù)可以返回 3 個(gè)值 1、2 或 3 中的 1 個(gè)。

retvalue = astar(startRow,startCol,endRow,endCol);
                    if (retvalue == 1) {
                        serial_printf(&SerialA,"   pathLength %d\r\n", pathLen);
                        int i = 0;
                        for(i = 0; i< pathLen; i++)     //put solution on map
                        {
                            int mapIdx = pathRow[i]*mapColSize + pathCol[i];
                            map[mapIdx] = '-';
                        }
                        //put path nodes into structure node_path
                        int p = 0;
                        for(p = 0; p < pathLen; p++)
                        {
                            path[p].row = pathRow[(pathLen-1)-p];
                            path[p].col = pathCol[(pathLen-1)-p];
                        }

                        //print map with solution
                        i = 0;
                        int j = 0;
                        for(i=0; i                        {
                            for(j = 0; j                            serial_printf(&SerialA,"   %c  ", map[i*mapColSize+j]);
                            serial_printf(&SerialA,"\r\n");
                        };>;>

如果它返回 1，我們有一個(gè)路徑！然后我們采用這條路徑并將其添加到解決方案地圖并打印解決方案。這條路徑從結(jié)尾開始，一直到開頭，所以我確保創(chuàng)建一個(gè)路徑數(shù)組，以便將來更容易使用。

A* 路徑示例

?

如果函數(shù)返回 2，那么它已經(jīng)在這一點(diǎn)上，我們打印一個(gè) ERROR。

} else if (retvalue == 2) {
                        serial_printf(&SerialA,"\r\n");
                        serial_printf(&SerialA,"\r\n");
                        serial_printf(&SerialA,"\r\n");
                        serial_printf(&SerialA,"\r\n");
                        serial_printf(&SerialA,"!!!!!!!!!!!!!!!!Error!!!!!!!!!!!!!!!\r\n");
                        serial_printf(&SerialA,"Already at this point.  No Astar needed.\r\n");
                        serial_printf(&SerialA,"!!!!!!!!!!!!!!!!Error!!!!!!!!!!!!!!!\r\n");
                        serial_printf(&SerialA,"\r\n");
                        serial_printf(&SerialA,"\r\n");
                        serial_printf(&SerialA,"\r\n");
                        serial_printf(&SerialA,"\r\n");

如果未找到機(jī)器人的有效路徑，該函數(shù)將返回 3。

} else {
                        serial_printf(&SerialA,"\r\n");
                        serial_printf(&SerialA,"\r\n");
                        serial_printf(&SerialA,"\r\n");
                        serial_printf(&SerialA,"\r\n");
                        serial_printf(&SerialA,"!!!!!!!!!!!!!!!!Error!!!!!!!!!!!!!!!\r\n");
                        serial_printf(&SerialA,"No Path Found Obstacle in the path.\r\n");
                        serial_printf(&SerialA,"!!!!!!!!!!!!!!!!Error!!!!!!!!!!!!!!!\r\n");
                        serial_printf(&SerialA,"\r\n");
                        serial_printf(&SerialA,"\r\n");
                        serial_printf(&SerialA,"\r\n");
                        serial_printf(&SerialA,"\r\n");
                    }

XY 控制和運(yùn)動(dòng)捕捉

動(dòng)作捕捉裝置

?

我們的機(jī)器人有一條路徑，我們可以運(yùn)行輪子，但我們?nèi)绾巫寵C(jī)器人順利地跟隨軌道？首先，我們確保我們始終知道我們的機(jī)器人汽車在哪里。OptiTrack Motive 軟件允許我用我打印的 3D 打印將一些反光球綁在我的機(jī)器人車的頂部，它會(huì)跟蹤這些球的中心。然后我可以將它用作我的機(jī)器人汽車的中心，這樣我就可以始終準(zhǔn)確地知道它在網(wǎng)格中的位置。

OpitTrack Motive 中的剛體

?

我們使用下面的代碼讀取將通過 SPIRXC 使用的數(shù)據(jù)。

障礙物檢測(cè)

現(xiàn)在，困難的部分來了。機(jī)器人如何知道有什么東西擋住了它，我們?nèi)绾螄@它進(jìn)行計(jì)劃？我所做的是給機(jī)器人一些規(guī)則。“障礙只能在這些地方，如果你看到那個(gè)區(qū)域周圍的東西就是那個(gè)地方的障礙”。聽起來很容易，對(duì)吧？我繼續(xù)為可能成為障礙的 48 條邊制定規(guī)則。

obs[46].x = 5; //ROBOTps
    obs[46].y = 6;
    obs[46].tally = 0;
    obs[46].found = 0;
    obs[46].idx1 = 72;
    obs[46].idx2 = 71;
    obs[46].idx3 = 70;
    obs[47].x = 3; //ROBOTps
    obs[47].y = 6;
    obs[47].tally = 0;
    obs[47].found = 0;
    obs[47].idx1 = 70;
    obs[47].idx2 = 69;
    obs[47].idx3 = 68;
    obs[48].x = 1; //ROBOTps
    obs[48].y = 6;
    obs[48].tally = 0;
    obs[48].found = 0;
    obs[48].idx1 = 68;
    obs[48].idx2 = 67;
    obs[48].idx3 = 66;

示例障礙物地圖

?

然后，我確保找到每個(gè)障礙物的中心及其對(duì)應(yīng)的路線圖索引。如果發(fā)現(xiàn)障礙物，我會(huì)計(jì)算出來。如果它達(dá)到 4 個(gè)或更多計(jì)數(shù)，我將地圖中的那些“0”更改為“X”，然后再次運(yùn)行 A* 并獲得一條新路徑。

為此，我首先必須知道我的激光雷達(dá)讀數(shù)在全球范圍內(nèi)的距離。由于 LIDAR 和 OptiTrack 系統(tǒng)有不同的基礎(chǔ)，我必須確保它們是相同的，代碼如下。

if (UseLIDARping == 1) {
            UseLIDARping = 0;
            int i = 0;
            // change of basis
            for (i = 0; i < 360; i++) {
                //if the data is not available, set all of them to 0 to avoid messing things up
                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;
                } else {
                    x_ori[i] = pingpts[i].distance*cos(i*0.01745329)/304;//0.017453292519943 is pi/180
                    y_ori[i] = pingpts[i].distance*sin(i*0.01745329)/304;
                    x_f[i].distance = (x_ori[i])*sin(ROBOTps.theta)+(y_ori[i])*cos(ROBOTps.theta)+(ROBOTps.x); //change basis
                    y_f[i].distance = (x_ori[i])*cos(ROBOTps.theta)+(y_ori[i])*sin(ROBOTps.theta)+(ROBOTps.y); //change basis
//                    x_f[i].distance = (x_ori[i]+pose_x)*cos(pose_rad)-(y_ori[i]+pose_y)*sin(pose_rad); //change basis
//                    y_f[i].distance = (x_ori[i]+pose_x)*sin(pose_rad)+(y_ori[i]+pose_y)*cos(pose_rad); //change basis
                }
            }
            if (PathFunction == 2){
                searchObs();
            }

這也是搜索并查看我是否有障礙的好時(shí)機(jī)。為了搜索障礙物，我必須對(duì)每個(gè)障礙物進(jìn)行 FOR 循環(huán)，看看 LIDAR 讀數(shù)與其對(duì)應(yīng)的 x 和 y 位置之間的差異是否小于 0.35 英尺。

void searchObs(){
    int i = 0;
    for(i = 0; i < 49; i++){
        // search lidar angles
//        }
        if(x_ori[90] < 1.5 && x_ori[90] > 0){
            if(y_ori[90] < 1.5 && y_ori[90] > 0){
                obsDisx = (x_f[90].distance - obs[i].x);
                obsDisy = (y_f[90].distance - obs[i].y);
                obsDist = sqrtf((obsDisx*obsDisx)*(obsDisy*obsDisy));
                if(obsDist < 0.35){
                    obs[i].tally++;
                }
            }
        }

我又做了 4 個(gè)角度，45、135、170 和 10 度。如果計(jì)數(shù)達(dá)到 4，則它們正式成為障礙并標(biāo)記為已找到。然后，該函數(shù)給出一個(gè)標(biāo)志 obsSearch，表示發(fā)現(xiàn)了一個(gè)障礙物，告訴機(jī)器人停止并尋找新路徑。

if(obs[i].tally > 3){
            if (obs[i].found == 0){
                mapCourseStart[obs[i].idx1] = 'x';
                mapCourseStart[obs[i].idx2] = 'x';
                mapCourseStart[obs[i].idx3] = 'x';
                //Flag that an obs has been found
                obsSearch = 1;
                obs[i].found = 1;
            }
        }

把它們放在一起

把它們放在一起！首先，我制作了一個(gè)“PathFunction”標(biāo)志，它會(huì)告訴機(jī)器人它處于什么狀態(tài)。它可以是“1”，即 A*，“2”，即跟隨路徑并尋找障礙物，或“3”，即 STANDBYE . 機(jī)器人由一個(gè)按鈕啟動(dòng)。

void ReadSwitches(void) {
    if(GpioDataRegs.GPADAT.bit.GPIO4 == 0){
        PathFunction = 1;
    }
    if(GpioDataRegs.GPADAT.bit.GPIO5 == 0){
        PathFunction = 2;
    }
    if(GpioDataRegs.GPADAT.bit.GPIO6 == 0){
        PathFunction = 0;
    }
    if(GpioDataRegs.GPADAT.bit.GPIO7 == 0){
        PathFunction = 3;
    }
}

要開始按下按鈕運(yùn)行 A*，這將在空白地圖上提供到端點(diǎn)的路徑，并將“PathFunction”推到 2。

if(PathFunction == 2){ // DRIVE THE PATH AFTER A*  !!USE Lidar and obstacle indentification to interrupt PATHFUCTION = 2
        //Run xycontrol for first path node changing desired x and y after each iteration until PathLen amount of times.
        //This should end at the end of the path !!MUST WATCH FOR SLIPPING OF WHEELS!!
        if(target_near == 1){
            nodenumber++; //progress in path
            target_near = 0;
        }
        //A*
        if(obsSearch == 1){
            if(astarC < 50){
                PathFunction = 1;
                nodenumber = 0;
            }
//            PathFunction = 1;
//            nodenumber = 0;
        }

一旦“PathFunction = 2”，我們首先檢查我們是否靠近目標(biāo)。如果我們是，我們說要在道路上進(jìn)步。如果沒有，我們?cè)诶^續(xù)前進(jìn)之前檢查是否發(fā)現(xiàn)了障礙物。如果我們找到一個(gè)，obsSearch = 1，我們回到 A*。如果沒有，我們可以自由地轉(zhuǎn)到所需的節(jié)點(diǎn)。

x_desired = (path[nodenumber+1].col);
        y_desired = (path[nodenumber+1].row);
        x_pos = ROBOTps.x;
        y_pos = ROBOTps.y;
        thetaabs = ROBOTps.theta;
        xy_control(&vref_forxy, &turn_forxy,turn_thres,x_pos,y_pos,x_desired,y_desired,
                   thetaabs,target_radius,target_radius_near);
        Vref = vref_forxy;
        turn = turn_forxy;
        if(nodenumber == pathLen-1){  //end of path!
            PathFunction = 3;
            Vref = 0;
            turn = 0;
        }

我們計(jì)算所需的 x 和 y 并使用動(dòng)作捕捉更新機(jī)器人的位置。然后我們運(yùn)行 xy_control 來更新 Vref 和 Turn。最后，我們檢查我們是否到達(dá)了路徑的盡頭。如果是這樣，我們將等待進(jìn)一步指示。

項(xiàng)目成果！

在我了解結(jié)果之前，我想說我在這個(gè)項(xiàng)目上玩得很開心，我在這個(gè)項(xiàng)目中學(xué)到了比以前更多的機(jī)器人技術(shù)。如果您覺得這很有趣，請(qǐng)嘗試一下。

A* 路徑規(guī)劃結(jié)果！

A* 的結(jié)果非常棒，因?yàn)闄C(jī)器人在軌道上縮放，精確地避開了我在地圖上手動(dòng)放置的每一個(gè)障礙物。

A* 路徑示例

?

以上是項(xiàng)目的A*路徑規(guī)劃、xy_control、動(dòng)作捕捉組合的結(jié)果。機(jī)器人很容易跟隨給它的路徑，輕松地躲避障礙物。

障礙物檢測(cè)結(jié)果！

到目前為止，障礙物檢測(cè)是該項(xiàng)目中最難的部分。機(jī)器人汽車在靜止時(shí)能夠可靠地看到和映射障礙物，但在轉(zhuǎn)彎和移動(dòng)時(shí)會(huì)遇到一些麻煩。

?

?

?

?

1 / 4 ?路徑 1 無障礙

?

以上是自主機(jī)器人汽車的成功運(yùn)行之一。

在這里，您可以看到機(jī)器人清楚地發(fā)現(xiàn)了兩個(gè)障礙物并改變了路線以繞過它們。

上面你可以看到機(jī)器人嘗試 A* 離開然后找到一個(gè)幻影障礙物并撞到另一個(gè)障礙物。

總的來說，這個(gè)項(xiàng)目是一個(gè)巨大的成功！這顯然可以調(diào)整和改進(jìn)，但該項(xiàng)目的理論和代碼使一個(gè)現(xiàn)實(shí)生活中的自主機(jī)器人。

謝謝你

如果沒有 Dan Block 教授的幫助，我不可能做到這一點(diǎn)，因?yàn)樗粌H提供了材料，而且還以他力所能及的方式提供了幫助！謝謝，丹！

我希望你喜歡我的項(xiàng)目，并在此過程中學(xué)到了一些東西。感謝您的時(shí)間！