Stanley

개요

Stanley 알고리즘은 이전의 Pure Pursuit과 마찬가지로 경로 추종 알고리즘 중 하나이다. 하지만, 뒷바퀴 중심을 기준점으로 하는 Pure Pursuit과 달리 앞바퀴 중심을 기준점으로 한다는 차이점이 있다.

Stanley

위 그림에서 e는 앞바퀴 중심점과 경로의 거리 오차이며, ψ는 경로의 진행 방향과 차량의 진행 방향 사이의 각도이다. 또한, 스티어링 각도는 δ이다. 이때 Stanley Method는 다음과 같다.

1. 헤딩 오차를 제거한다.
   
2. 경로와의 거리 오차를 제거한다.
   
3. 최대 조향 각도 범위를 준수한다.
   

이를 정리하면 다음과 같으며, 분모를 0으로 하지 않기 위해 상수를 추가할 수 있다.

Stanley는 헤딩 오차와 경로 오차를 모두 고려하기에, 헤딩 오차가 크고 경로 오차가 적은 경우와 헤딩 오차가 작고 경로 오차가 큰 경우 모두 효과적으로 주행할 수 있다.

코드 구현

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

const double pi{3.141592};
const double L{0.5};
std::array<std::array<double, 3>, 13> path = {{ 
    {0,0,1.57}, //x, y, yaw
    {0,1,1.57},
    {0,2,1.57},
    {0,3,1.57},
    {0,4,1.37},
    {0.2,4.98,1.19},
    {0.57,5.91,1.04},
    {1.07,6.77,0.85},
    {1.73,7.52,0.74},
    {2.47,8.2,0.71},
    {3.23,8.85,0.64},
    {4.03,9.45,0.47},
    {4.92,9.9,0}
}};

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

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

class Stanley
{
private:
    double k{0.53};
public:
    int getCloseIndex(std::array<double, 3> curr_pos){
        double min_dist{9999}, dist;
        int min_index;

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

        return min_index;
    }

    double getTrackError(std::array<double, 3> curr_pos, int curr_spd, int close_index){
        std::array<double, 3> curr_wp = path[close_index], next_wp = path[close_index+1];

        double e_numerator = (next_wp[0] - curr_wp[0]) * (curr_wp[1] - curr_pos[1]) - (curr_wp[0] - curr_pos[0]) * (next_wp[1] - curr_wp[1]);
        if (e_numerator < 0) e_numerator = -e_numerator;
        double e_denominator = sqrt(pow(next_wp[0] - curr_wp[0], 2) + pow(next_wp[1] - curr_wp[1], 2));
        double track_error = atan2(k * (e_numerator / e_denominator), curr_spd);

        return track_error;    
    }

    double getHeadingError(std::array<double, 3> curr_pos, std::array<double, 3> curr_wp){
        double heading_error = curr_wp[2] - curr_pos[2];

        return heading_error;
    }

    double getSteeringAngle(double track_error, double heading_error, int max_steer){
        double steering_angle = track_error + heading_error;

        if (steering_angle > (max_steer*pi/180)) steering_angle = max_steer*pi/180;
        else if (steering_angle < -(max_steer*pi/180)) steering_angle = -max_steer*pi/180;

        return steering_angle;
    }
};

int main(){
    double steering_angle, heading_error, track_error;
    int curr_spd{1}, max_steer{30}, close_index;
    std::array<double, 3> curr_rear_pos = {0,0,atan2(1,0)};
    std::array<double, 3> curr_front_pos;
    Stanley stanley;

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

        curr_front_pos = {curr_rear_pos[0] + L * cos(curr_rear_pos[2]), curr_rear_pos[1] + L * sin(curr_rear_pos[2]), curr_rear_pos[2]};
        close_index = stanley.getCloseIndex(curr_rear_pos);
        track_error = stanley.getTrackError(curr_front_pos, curr_spd, close_index);
        heading_error = stanley.getHeadingError(curr_front_pos, path[close_index]);
        steering_angle = stanley.getSteeringAngle(track_error, heading_error, max_steer);
        curr_rear_pos[2] += curr_spd * tan(steering_angle) / L;
        curr_rear_pos = Drive(curr_rear_pos, curr_spd);
    }
    return 0;
}

Reference

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