無(wú)人駕駛路徑規(guī)劃

眾所周知，無(wú)人駕駛大致可以分為三個(gè)方面的工作：感知，決策及控制。 路徑規(guī)劃是感知和控制之間的決策階段，主要目的是考慮到車輛動(dòng)力學(xué)、機(jī)動(dòng)能力以及相應(yīng)規(guī)則和道路邊界條件下，為車輛提供通往目的地的安全和無(wú)碰撞的路徑。 路徑規(guī)劃問(wèn)題可以分為兩個(gè)方面： （一）全局路徑規(guī)劃：全局路徑規(guī)劃算法屬于靜態(tài)規(guī)劃算法，根據(jù)已有的地圖信息（SLAM）為基礎(chǔ)進(jìn)行路徑規(guī)劃，尋找一條從起點(diǎn)到目標(biāo)點(diǎn)的最優(yōu)路徑。 通常全局路徑規(guī)劃的實(shí)現(xiàn)包括Dijikstra算法，A*算法，RRT算法等經(jīng)典算法，也包括蟻群算法、遺傳算法等智能算法； （二）局部路徑規(guī)劃：局部路徑規(guī)劃屬于動(dòng)態(tài)規(guī)劃算法，是無(wú)人駕駛汽車根據(jù)自身傳感器感知周圍環(huán)境，規(guī)劃處一條車輛安全行駛所需的路線，常應(yīng)用于超車，避障等情景。通常局部路徑規(guī)劃的實(shí)現(xiàn)包括動(dòng)態(tài)窗口算法（DWA），人工勢(shì)場(chǎng)算法，貝塞爾曲線算法等，也有學(xué)者提出神經(jīng)網(wǎng)絡(luò)等智能算法。

全局路徑規(guī)劃 - RRT算法原理

RRT算法，即快速隨機(jī)樹(shù)算法（Rapid Random Tree），是LaValle在1998年首次提出的一種高效的路徑規(guī)劃算法。RRT算法以初始的一個(gè)根節(jié)點(diǎn)，通過(guò)隨機(jī)采樣的方法在空間搜索，然后添加一個(gè)又一個(gè)的葉節(jié)點(diǎn)來(lái)不斷擴(kuò)展隨機(jī)樹(shù)。 當(dāng)目標(biāo)點(diǎn)進(jìn)入隨機(jī)樹(shù)里面后，隨機(jī)樹(shù)擴(kuò)展立即停止，此時(shí)能找到一條從起始點(diǎn)到目標(biāo)點(diǎn)的路徑。算法的計(jì)算過(guò)程如下： step1：初始化隨機(jī)樹(shù)。將環(huán)境中起點(diǎn)作為隨機(jī)樹(shù)搜索的起點(diǎn)，此時(shí)樹(shù)中只包含一個(gè)節(jié)點(diǎn)即根節(jié)點(diǎn)；? stpe2：在環(huán)境中隨機(jī)采樣。在環(huán)境中隨機(jī)產(chǎn)生一個(gè)點(diǎn)，若該點(diǎn)不在障礙物范圍內(nèi)則計(jì)算隨機(jī)樹(shù)中所有節(jié)點(diǎn)到的歐式距離，并找到距離最近的節(jié)點(diǎn)，若在障礙物范圍內(nèi)則重新生成并重復(fù)該過(guò)程直至找到;?? stpe3：生成新節(jié)點(diǎn)。在和連線方向，由指向固定生長(zhǎng)距離生成一個(gè)新的節(jié)點(diǎn)，并判斷該節(jié)點(diǎn)是否在障礙物范圍內(nèi)，若不在障礙物范圍內(nèi)則將添加到隨機(jī)樹(shù) 中，否則的話返回step2重新對(duì)環(huán)境進(jìn)行隨機(jī)采樣； step4：停止搜索。當(dāng)和目標(biāo)點(diǎn)之間的距離小于設(shè)定的閾值時(shí)，則代表隨機(jī)樹(shù)已經(jīng)到達(dá)了目標(biāo)點(diǎn)，將作為最后一個(gè)路徑節(jié)點(diǎn)加入到隨機(jī)樹(shù)中，算法結(jié)束并得到所規(guī)劃的路徑。 RRT算法由于其隨機(jī)采樣及概率完備性的特點(diǎn)，使得其具有如下優(yōu)勢(shì)： （1）不需要對(duì)環(huán)境具體建模，有很強(qiáng)空間搜索能力； （2）路徑規(guī)劃速度快； （3）可以很好解決復(fù)雜環(huán)境下的路徑規(guī)劃問(wèn)題。 但同樣是因?yàn)殡S機(jī)性，RRT算法也存在很多不足的方面： （1）隨機(jī)性強(qiáng)，搜索沒(méi)有目標(biāo)性，冗余點(diǎn)多，且每次規(guī)劃產(chǎn)生的路徑都不一樣，均不一是最優(yōu)路徑； （2）可能出現(xiàn)計(jì)算復(fù)雜、所需的時(shí)間過(guò)長(zhǎng)、易于陷入死區(qū)的問(wèn)題； （3）由于樹(shù)的擴(kuò)展是節(jié)點(diǎn)之間相連，使得最終生成的路徑不平滑； （4）不適合動(dòng)態(tài)環(huán)境，當(dāng)環(huán)境中出現(xiàn)動(dòng)態(tài)障礙物時(shí)，RRT算法無(wú)法進(jìn)行有效的檢測(cè)； （5）對(duì)于狹長(zhǎng)地形，可能無(wú)法規(guī)劃出路徑。

RRT算法Matlab實(shí)現(xiàn)

使用matlab2019來(lái)編寫RRT算法，下面將貼出部分代碼進(jìn)行解釋。

1、生成障礙物 在matlab中模擬柵格地圖環(huán)境，自定義障礙物位置。

%% 生成障礙物
ob1 = [0,-10,10,5]; ? ? ? ? ? ? % 三個(gè)矩形障礙物
ob2 = [-5,5,5,10];
ob3 = [-5,-2,5,4];


ob_limit_1 = [-15,-15,0,31]; ? ?% 邊界障礙物
ob_limit_2 = [-15,-15,30,0];
ob_limit_3 = [15,-15,0,31];
ob_limit_4 = [-15,16,30,0];


ob = [ob1;ob2;ob3;ob_limit_1;ob_limit_2;ob_limit_3;ob_limit_4]; ?% 放到一個(gè)數(shù)組中統(tǒng)一管理


x_left_limit = -16; ? ? ? ? ? ? % 地圖的邊界
x_right_limit = 15;
y_left_limit = -16;
y_right_limit = 16;

?

我在這隨便選擇生成三個(gè)矩形的障礙物，并統(tǒng)一放在ob數(shù)組中管理，同時(shí)定義地圖的邊界。

2、初始化參數(shù)設(shè)置 初始化障礙物膨脹范圍、地圖分辨率，機(jī)器人半徑、起始點(diǎn)、目標(biāo)點(diǎn)、生長(zhǎng)距離和目標(biāo)點(diǎn)搜索閾值。

%% 初始化參數(shù)設(shè)置
extend_area = 0.2; ? ? ? ?% 膨脹范圍
resolution = 1; ? ? ? ? ? % 分辨率
robot_radius = 0.2; ? ? ? % 機(jī)器人半徑


goal = [-10, -10]; ? ? ? ?% 目標(biāo)點(diǎn)
x_start = [13, 10]; ? ? ? % 起點(diǎn)


grow_distance = 1; ? ? ? ?% 生長(zhǎng)距離
goal_radius = 1.5; ? ? ? ?% 在目標(biāo)點(diǎn)為圓心，1.5m內(nèi)就停止搜索

? 3、初始化隨機(jī)樹(shù) 初始化隨機(jī)樹(shù)，定義樹(shù)結(jié)構(gòu)體tree以保存新節(jié)點(diǎn)及其父節(jié)點(diǎn)，便于后續(xù)從目標(biāo)點(diǎn)回推規(guī)劃的路徑。

%% 初始化隨機(jī)樹(shù)
tree.child = []; ? ? ? ? ? ? ? % 定義樹(shù)結(jié)構(gòu)體，保存新節(jié)點(diǎn)及其父節(jié)點(diǎn)
tree.parent = [];
tree.child = x_start; ? ? ? ? ?% 起點(diǎn)作為第一個(gè)節(jié)點(diǎn)
flag = 1; ? ? ? ? ? ? ? ? ? ? ?% 標(biāo)志位
new_node_x = x_start(1,1); ? ? % 將起點(diǎn)作為第一個(gè)生成點(diǎn)
new_node_y = x_start(1,2);
new_node = [new_node_x, new_node_y];

4、主函數(shù)部分 主函數(shù)中首先生成隨機(jī)點(diǎn)，并判斷是否在地圖范圍內(nèi)，若超出范圍則將標(biāo)志位置為0。

rd_x = 30 * rand() - 15; ? ?% 生成隨機(jī)點(diǎn)
rd_y = 30 * rand() - 15; ? ?
if (rd_x >= x_right_limit || rd_x <= x_left_limit ||... % 判斷隨機(jī)點(diǎn)是否在地圖邊界范圍內(nèi)
 ? ?rd_y >= y_right_limit || rd_y <= y_left_limit)
 ? ?flag = 0;
end

調(diào)用函數(shù)cal_distance計(jì)算tree中距離隨機(jī)點(diǎn)最近的節(jié)點(diǎn)的索引，并計(jì)算該節(jié)點(diǎn)與隨機(jī)點(diǎn)連線和x正向的夾角。

[angle, min_idx] = cal_distance(rd_x, rd_y, tree); ? ?% 返回tree中最短距離節(jié)點(diǎn)索引及對(duì)應(yīng)的和x正向夾角

cal_distance函數(shù)定義如下：

function [angle, min_idx] = cal_distance(rd_x, rd_y, tree)
 ? ?distance = [];
 ? ?i = 1;
 ? ?while i<=size(tree.child,1)
 ? ? ? ?dx = rd_x - tree.child(i,1);
 ? ? ? ?dy = rd_y - tree.child(i,2);
 ? ? ? ?d = sqrt(dx^2 + dy^2);
 ? ? ? ?distance(i) = d;
 ? ? ? ?i = i+1;
 ? ?end
 ? ?[~, min_idx] = min(distance);
 ? ?angle = atan2(rd_y - tree.child(min_idx,2),rd_x - tree.child(min_idx,1));
end

隨后生成新節(jié)點(diǎn)。

new_node_x = tree.child(min_idx,1)+grow_distance*cos(angle);% 生成新的節(jié)點(diǎn)
new_node_y = tree.child(min_idx,2)+grow_distance*sin(angle);
new_node = [new_node_x, new_node_y];

接下來(lái)需要對(duì)該節(jié)點(diǎn)進(jìn)行判斷： ① 新節(jié)點(diǎn)是否在障礙物范圍內(nèi)； ② ?新節(jié)點(diǎn)和父節(jié)點(diǎn)的連線線段是否和障礙物有重合部分。 若任意一點(diǎn)不滿足，則將標(biāo)志位置為0。實(shí)際上可以將兩個(gè)判斷結(jié)合，即判斷新節(jié)點(diǎn)和父節(jié)點(diǎn)的連線線段上的點(diǎn)是否在障礙物范圍內(nèi)。

for k=1:1:size(ob,1) 
 ? ?for i=min(tree.child(min_idx,1),new_node_x):0.01:max(tree.child(min_idx,1),new_node_x) ? ?% 判斷生長(zhǎng)之后路徑與障礙物有無(wú)交叉部分
 ? ? ? ?j = (tree.child(min_idx,2) - new_node_y)/(tree.child(min_idx,1) - new_node_x) *(i - new_node_x) + new_node_y;
 ? ? ? ?if(i >=ob(k,1)-resolution && i <= ob(k,1)+ob(k,3) && j >= ob(k,2)-resolution && j <= ob(k,2)+ob(k,4))
 ? ? ? ? ? ?flag = 0;
 ? ? ? ? ? ?break
 ? ? ? ?end
 ? ?end
end

?

在這我采用的方法是寫出新節(jié)點(diǎn)和父節(jié)點(diǎn)連線的直線方程，然后將x變化范圍限制在min(tree.child(min_idx,1),new_node_x)max(tree.child(min_idx,1),new_node_x)內(nèi)，0.01即坐標(biāo)變換的步長(zhǎng)，步長(zhǎng)越小判斷的越精確，但同時(shí)會(huì)增加計(jì)算量；

步長(zhǎng)越大計(jì)算速度快但是很可能出現(xiàn)誤判，如下圖所式。 左圖：合適的步長(zhǎng) ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?? ? 右圖：步長(zhǎng)過(guò)大 判斷標(biāo)志位若為1，則可以將該新節(jié)點(diǎn)加入到tree中，注意保存新節(jié)點(diǎn)和它的父節(jié)點(diǎn)，同時(shí)顯示在figure中，之后重置標(biāo)志位。

if (flag == true) ? ? ? ? ? % 若標(biāo)志位為1，則可以將該新節(jié)點(diǎn)加入tree中
 ? ?tree.child(end+1,:) = new_node;
 ? ?tree.parent(end+1,:) = [tree.child(min_idx,1), tree.child(min_idx,2)];
 ? ?plot(rd_x, rd_y, '.r');hold on
 ? ?plot(new_node_x, new_node_y,'.g');hold on
 ? ?plot([tree.child(min_idx,1),new_node_x], [tree.child(min_idx,2),new_node_y],'-b');
end
 ? ?
flag = 1; ? ? ? ? ? ? ? ? ? % 標(biāo)志位歸位

最后就是把障礙物、起點(diǎn)終點(diǎn)等顯示在figure中，并判斷新節(jié)點(diǎn)到目標(biāo)點(diǎn)距離。若小于閾值則停止搜索，并將目標(biāo)點(diǎn)加入到node中，否則重復(fù)該過(guò)程直至找到目標(biāo)點(diǎn)。

%% 顯示
for i=1:1:size(ob,1) ? ? ? ?% 繪制障礙物
 ? ?fill([ob(i,1)-resolution, ob(i,1)+ob(i,3),ob(i,1)+ob(i,3),ob(i,1)-resolution],...
 ? ? ? ? [ob(i,2)-resolution,ob(i,2)-resolution,ob(i,2)+ob(i,4),ob(i,2)+ob(i,4)],'k');
end
hold on


plot(x_start(1,1)-0.5*resolution, x_start(1,2)-0.5*resolution,'b^','MarkerFaceColor','b','MarkerSize',4*resolution); % 起點(diǎn)
plot(goal(1,1)-0.5*resolution, goal(1,2)-0.5*resolution,'m^','MarkerFaceColor','m','MarkerSize',4*resolution); % 終點(diǎn)
set(gca,'XLim',[x_left_limit x_right_limit]); % X軸的數(shù)據(jù)顯示范圍
set(gca,'XTick',[x_left_limitx_right_limit]); % 設(shè)置要顯示坐標(biāo)刻度
set(gca,'YLim',[y_left_limit y_right_limit]); % Y軸的數(shù)據(jù)顯示范圍
set(gca,'YTick',[y_left_limity_right_limit]); % 設(shè)置要顯示坐標(biāo)刻度
grid on
title('D-RRT');
xlabel('橫坐標(biāo) x'); 
ylabel('縱坐標(biāo) y');
pause(0.05);
if (sqrt((new_node_x - goal(1,1))^2 + (new_node_y- goal(1,2))^2) <= goal_radius) % 若新節(jié)點(diǎn)到目標(biāo)點(diǎn)距離小于閾值，則停止搜索，并將目標(biāo)點(diǎn)加入到node中
 ? ?tree.child(end+1,:) = goal; ? ? ? ? % 把終點(diǎn)加入到樹(shù)中
 ? ?tree.parent(end+1,:) = new_node;
 ? ?disp('find goal!');
 ? ?break
end

5、繪制最優(yōu)路徑 從目標(biāo)點(diǎn)開(kāi)始，依次根據(jù)節(jié)點(diǎn)及父節(jié)點(diǎn)回推規(guī)劃的路徑直至起點(diǎn)，要注意tree結(jié)構(gòu)體中parent的長(zhǎng)度比child要小1。最后將規(guī)劃的路徑顯示在figure中。

%% 繪制最優(yōu)路徑
temp = tree.parent(end,:);
trajectory = [tree.child(end,1)-0.5*resolution, tree.child(end,2)-0.5*resolution];
for i=size(tree.child,1):-1:2
 ? ?if(size(tree.child(i,:),2) ~= 0 & tree.child(i,:) == temp)
 ? ? ? ?temp = tree.parent(i-1,:);
 ? ? ? ?trajectory(end+1,:) = tree.child(i,:);
 ? ?if(temp == x_start)
 ? ? ? ?trajectory(end+1,:) = [temp(1,1) - 0.5*resolution, temp(1,2) - 0.5*resolution];
 ? ?end
 ? ?end
end
plot(trajectory(:,1), trajectory(:,2), '-r','LineWidth',2);
pause(2);

程序運(yùn)行最終效果如下： ?紅點(diǎn)都是生成點(diǎn)隨機(jī)點(diǎn)，綠點(diǎn)是tree中節(jié)點(diǎn)，紅色路徑即為RRT算法規(guī)劃的路徑。 6、路徑平滑（B樣條曲線） 由于規(guī)劃的路徑都是線段連接，在節(jié)點(diǎn)處路徑不平滑，這也是RRT算法的弊端之一。一般來(lái)說(shuō)軌跡平滑的方法有很多種，類似于貝塞爾曲線，B樣條曲線等。 我在這采用B樣條曲線對(duì)規(guī)劃的路徑進(jìn)行平滑處理，具體的方法和原理我后續(xù)有時(shí)間再進(jìn)行說(shuō)明，這里先給出結(jié)果： ?黑色曲線即位平滑處理后的路徑。 多組結(jié)果對(duì)比 ① 相鄰兩次仿真結(jié)果對(duì)比： 可以看出由于隨機(jī)采樣的原因，任意兩次規(guī)劃的路徑都是不一樣的。? ② 復(fù)雜環(huán)境下的路徑規(guī)劃。選取一個(gè)相對(duì)復(fù)雜的環(huán)境，仿真結(jié)果如下： 可以看出RRT算法可以很好解決復(fù)雜環(huán)境下的路徑規(guī)劃問(wèn)題。 ③ 狹窄通道下的路徑規(guī)劃。選取一個(gè)狹窄通道環(huán)境，仿真結(jié)果如下： 由于環(huán)境采樣的隨機(jī)性，在狹長(zhǎng)通道內(nèi)生成隨機(jī)點(diǎn)的概率相對(duì)較低，導(dǎo)致可能無(wú)法規(guī)劃出路徑。

編輯：黃飛

?