如何編寫RRT算法 初始化參數(shù)設(shè)置

使用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)就停止搜索