[DGPRM]Disk-Graph Probabilistic Roadmap 논문 구현 -1-

DGPRM

---

1. DGPRM (Disk-Graph PRM)

---

확률적 로드맵 방법인 PRM기법의 상위버전이라고 생각해서 논문을 읽기 시작했고 코드로 구현을 하고있다.

기존 PRM과의 차이점이 있다면 장애물(즉, Occupancy Grid map)을 고려해 Bubble형태의 Disk-Graph를 구성하게 되어 장애물과의 충돌이 적은 경로를 Path Planning할 수 있다는 장점이 있다.

 

Process는 간략하게 다음과 같다.

1. 시작 노드의 정보를 Bubble크기로 그래프에 추가한다.

2. 무작위 샘플링을 진행하고 Edge & Vertex의 Validity를 체크해 Graph에 추가한다.

3. 샘플링을 했음에도 불구하고 Goal까지 Path가 만들어지지 않는다면 재 샘플링을 진행한다.

4. 재샘플링은 Exponential distribution을 이용해 탐색성을 늘린다.

5. 동일하게 재샘플링을 하면서 최종 경로를 도출한다.

 

2. Code 구현 with MATLAB

진짜 어마어마하게 긴 코드다..

간략하게 보면

1. Occupancy Grid map을 불러온다. ( 시작점과 도착점도 )

2. 장애물의 위치정보를 뽑아낸다.

3. 샘플링과 재샘플링을 하면서 그래프를 확장한다.

4. 경로를 계획한다.

 

우선 MATLAB에선 binaryOccupancyMap 객체를 불러온다.

이후에 map이라는 변수에 저장.

% Load the map
map_struct = load("maze_rand_2.mat");
map = map_struct.map;
mat = occupancyMatrix(map);
grid_res = 1;

init_p = [4 10];
goal_p = [4 16];

obs = [];
drivable = [];
for i = 1:length(mat(:,1))
    for j = 1:length(mat(1,:))
        if mat(i,j) == 1
            obs = [obs; grid_res*j-grid_res/2 grid_res*(length(mat(:,1))-i)+grid_res/2];
        else
            drivable = [drivable; grid_res*j-grid_res/2 grid_res*(length(mat(:,1))-i)+grid_res/2];
        end
    end
end
map_dim = map.DataSize;
env = OccupancyEnv(obs, map_dim);

TODO : env라는 변수에  map의 Occupancy정보인 mat도 들어가야하나?

 

초기 PRM 파라미터 정의

%% PRM params
hyperparams_dict.sampling_cutoff_dist = 50;
hyperparams_dict.n_knn = 5;
hyperparams_dict.robot_rad = 2;
hyperparams_dict.max_rad = 1000;
hyperparams_dict.closing_rad_mult = 1.05;
hyperparams_dict.opening_rad_mult = 4;

 

DGPRM Class(Planner Class, 즉 Graph정보를 담고 있는 클래스)

classdef PlannerClass
    properties
        graph           
        nodes_ids       
        prng            
        samples_count   
        sampling_cutoff_dist 
        n_knn           
        robot_rad       
        max_rad         
        closing_rad_mult
        opening_rad_mult
    end
    
    methods
        function obj = PlannerClass(seed)
            
            obj.nodes_ids = [];
            if nargin < 1 || isempty(seed) || seed < 0 || seed >= 2^32
                obj.prng = RandStream('mt19937ar', 'Seed', 'shuffle');
            else
                obj.prng = RandStream('mt19937ar', 'Seed', seed);
            end
            obj.graph = containers.Map('KeyType', 'double', 'ValueType', 'any');
            obj.samples_count = 0;
        end
        function obj = initFromHyperparamsDict(obj,hyperparams_dict)
            obj.sampling_cutoff_dist = hyperparams_dict.sampling_cutoff_dist;
            obj.n_knn = hyperparams_dict.n_knn;
            obj.robot_rad = hyperparams_dict.robot_rad;
            obj.max_rad = hyperparams_dict.max_rad;
            obj.closing_rad_mult = hyperparams_dict.closing_rad_mult;
            obj.opening_rad_mult = hyperparams_dict.opening_rad_mult;

        end

 

앞서 지정해준 HyperParameter들과 nodes_ids를 포함하고 있다.

 

그래서 여기까지는 의심의 여지 없는 코드 완성. 그냥 파라미터 집어넣기,,

planner = PlannerClass();
planner = planner.initFromHyperparamsDict(hyperparams_dict);

 

이제부터 추가되는 노드는 BubbleNode의 형태로 정의한다.

BubbleNode의 요소는 pos, id, bubble_rad가 있다.

중요한 함수가 하나 존재하는데 Bubble의 pos와 장애물의 정보를 통해 Bubble_rad를 정해주기.

 

classdef BubbleNode
    % BubbleNode 클래스는 자유 공간 버블로서 그래프 노드를 나타냅니다.
    %
    % 속성:
    %    pos: 크기 2의 배열, 노드의 2D 위치
    %    id: int, 노드의 ID
    %    bubble_rad: float, 노드의 자유 공간 반경
    
    properties
        pos       % 노드의 2D 위치
        id        % 노드의 ID
        bubble_rad  % 노드의 자유 공간 반경
    end
    
    methods
        
        function obj = BubbleNode(pos, id)
            
            if nargin > 0
                obj.pos = pos;
                obj.id = id;
                obj.bubble_rad = 0;
            end
        end

        function obj = computeBubbleRad(obj, env, robot_rad)
            if nargin < 3
                robot_rad = 0.1;  % 기본 로봇 반경 값 설정
            end
            dist = env.queryDistAt(obj.pos);
            obj.bubble_rad = dist - robot_rad;
        end

그래서 이 함수는 단일로 호출되지는 않고, Bubble를 추가한다음에 planner클래스에서 반복문을 통해 계속 호출되는 거 같다.

        function [invalid_nodes, modified_nodes] = recomputeNodesRadii(obj, env)

            modified_nodes = [];
            invalid_nodes = [];
            nodes = values(obj.graph);
            
            for i = 1:length(nodes)
                node = nodes{i};
                old_rad = node.bubble_rad;
                node = node.computeBubbleRad(env, obj.robot_rad);
                obj.graph(node.id) = node;
                
                if node.bubble_rad < obj.robot_rad
                    invalid_nodes(end+1) = node.id;
                elseif node.bubble_rad ~= old_rad
                    modified_nodes(end+1) = node.id;
                end
            end
        end

이런식으로? main문에서는

[invalid_nodes, modified_nodes] = planner.recomputeNodesRadii(env); 이렇게 호출한다.

 

문제의 expandPlanPath함수는 다음 글에서,,