Pure Pursuit

개요

Pure Pursuit 알고리즘은 경로 추종 알고리즘 중 하나로, 차량의 운동 방정식과 경로의 지오메트리만을 사용하는 간단한 알고리즘이다.

Pure Pursuit

Pure Pursuit은 차량의 뒷바퀴 중심이 경로 상의 목표 지점에 도달하기 위한 조향각(steering angle)을 계산한다. 이때 뒷바퀴 중심과 목표 지점 사이의 거리는 Ld이고, 두 점을 지나는 직선과 차량이 바라보는 방향의 직선 사이의 각도는 α이다. 또한, 두 점을 지나는 원의 반지름이 R일 때, 다음과 같은 식으로 나타낼 수 있다.

이를 차량의 횡방향 움직임을 간소화한 bicycle model을 사용하여 정리하면 다음 수식으로 조향각을 계산할 수 있다.

Lookahead Distance

Lookahead Distance는 전방주시거리라고도 불리며, Pure Pursuit 알고리즘에서 전방 목표 지점을 지정하는 중요한 파라미터이다. 이 파라미터의 값이 너무 크다면 corner cutoff가 심해지며, 너무 작다면 overshoot이 심해져 적절한 값을 찾아야한다. 보통 차량의 속도에 비례하게 설정하여 저속에서는 작은 값을, 고속에서는 큰 값을 가지게 한다.

코드 구현

#include <iostream>
#include <array>
#include <cmath>

const float pi{3.141592};
const float L{0.5};
std::array<std::array<float, 2>, 13> path = {{
    {0,0},
    {0,1},
    {0,2},
    {0,3},
    {0,4},
    {0.2,4.98},
    {0.57,5.91},
    {1.07,6.77},
    {1.73,7.52},
    {2.47,8.2},
    {3.23,8.85},
    {4.03,9.45},
    {4.92,9.9}
}};

float getDist(std::array<float, 2> point1, std::array<float, 2> point2){
    float dist = sqrt(pow(point2[0] - point1[0],2) + pow(point2[1] - point1[1],2));
    return dist;
}

std::array<float, 2> Drive(std::array<float, 2> curr_pos, int curr_spd, float heading){
    curr_pos[0] += curr_spd * cos(heading);
    curr_pos[1] += curr_spd * sin(heading);
    return curr_pos;
}

class PurePursuit
{
public:
    std::array<float, 2> getLookaheadPoint(std::array<float, 2> curr_pos, int lookahead_distance){
        float min_dist{99999}, dist;
        int close_index;

        for (int i=0;i<13;i++){
            dist = getDist(path[i], curr_pos);
            if (dist < min_dist){
                min_dist = dist;
                close_index = i;
            }
        }

        std::array<float, 2> lookahead_point = path[close_index+lookahead_distance];
        return lookahead_point;
    }

    float getSteeringAngle(std::array<float, 2> curr_pos, std::array<float, 2> lookahead_point, float heading){
        float Ld, alpha, steering_angle;

        Ld = getDist(lookahead_point, curr_pos);
        alpha = atan2((lookahead_point[1] - curr_pos[1]), (lookahead_point[0] - curr_pos[0])) - heading;
        steering_angle = atan2(2 * L * sin(alpha), Ld);
        return steering_angle;
    }
};

int main(){
    float heading{atan2(1,0)}, steering_angle;
    int curr_spd{1}, lookahead_distance{curr_spd * 2};
    std::array<float, 2> curr_pos = {0,0}, lookahead_point;
    PurePursuit pure_pursuit;

    while (true){
        std::cout<<"current_position : ";
        for(int j=0;j<2;j++){
            std::cout<<curr_pos[j]<<'\t';
        }
        std::cout<<"heading : "<<(heading*180/pi)<<'\t';
        float dist = getDist(curr_pos, path[12]);
        std::cout<<"dist : "<<dist<<std::endl;
        if (dist < 0.5) break;        

        lookahead_point = pure_pursuit.getLookaheadPoint(curr_pos, lookahead_distance);
        steering_angle = pure_pursuit.getSteeringAngle(curr_pos, lookahead_point, heading);
        heading += curr_spd * tan(steering_angle) / L;
        curr_pos = Drive(curr_pos, curr_spd, heading);
    }
    return 0;
}

Reference

https://dingyan89.medium.com/three-methods-of-vehicle-lateral-control-pure-pursuit-stanley-and-mpc-db8cc1d32081