행동 계획 알고리즘의 ROS2 적용

행동 계획의 개요

행동 계획(Behavior Planning)은 로봇이 특정 목표를 달성하기 위해 필요한 일련의 행동을 선택하고 수행하는 과정이다. 이는 주로 복잡한 환경에서의 로봇 제어에서 중요하게 다루어지며, 다양한 센서 데이터를 통합하여 로봇의 의사 결정을 지원한다. ROS2는 이러한 행동 계획 알고리즘을 손쉽게 구현할 수 있도록 여러 도구와 기능을 제공한다.

ROS2와 행동 계획의 통합

ROS2에서는 행동 계획 알고리즘을 구현하기 위해 Behavior Tree (BT) 구조를 자주 사용한다. Behavior Tree는 트리 구조의 계층적 모델로, 로봇의 행동을 순차적으로 또는 병렬적으로 처리할 수 있게 한다. BT는 로봇이 실행할 수 있는 각 행동을 노드로 정의하며, 각 노드는 조건부(Conditional) 또는 행동(Execution) 노드로 구분된다. 이를 통해 복잡한 행동 계획을 체계적으로 관리하고 확장할 수 있다.

1. ROS2에서 Behavior Tree의 구성

ROS2에서는 Behavior Tree를 구성하기 위한 패키지인 behavior_tree_cpp와 같은 라이브러리가 제공된다. 이 라이브러리를 통해 각 노드를 정의하고 트리를 구성할 수 있다. ROS2와 BT의 통합은 각 노드가 ROS2의 퍼블리셔 또는 서브스크라이버와 연동되어 로봇의 센서 데이터를 사용하거나 제어 명령을 전달하는 형태로 이루어진다.

트리 구조의 기본 노드

행동 노드(Execution Node): 특정 행동을 수행하는 노드로, 로봇의 센서나 모터와 직접적인 상호작용을 담당한다. 예를 들어, 특정 목표 위치로 이동하는 행동이나 물체를 잡는 행동을 수행할 수 있다.
조건 노드(Conditional Node): 행동이 실행될 조건을 정의하는 노드로, 트리의 흐름을 제어한다. 예를 들어, 로봇이 목표 위치에 도달했는지 여부를 확인하거나 특정 환경 조건이 충족되었는지를 판단한다.

graph TD; A[시작 노드] --> B(조건 노드: 목표 위치 확인); B --> C[행동 노드: 이동 실행]; B --> D[조건 노드: 장애물 감지]; D --> E[행동 노드: 회피 실행];

위와 같은 트리 구조에서 조건 노드가 트리의 흐름을 결정하며, 행동 노드는 로봇이 실제로 수행해야 할 작업을 정의한다. 각 노드는 ROS2 퍼블리셔나 서브스크라이버를 통해 데이터를 주고받을 수 있다.

행동 계획 알고리즘 구현 과정

ROS2에서 행동 계획 알고리즘을 적용하기 위해서는 다음과 같은 과정이 필요하다:

환경 모델링: 로봇이 동작할 환경을 모델링하여 로봇의 행동을 계획하는 데 필요한 정보를 제공한다. ROS2에서는 rviz2와 같은 시각화 도구를 통해 환경 모델을 확인하고 설정할 수 있다.
목표 설정: 행동 계획 알고리즘이 처리할 목표를 설정한다. 이는 목표 위치나 특정 행동을 수행하는 시점 등을 포함할 수 있다. ROS2의 Action 서버를 사용하여 비동기적으로 목표를 설정할 수 있다.
행동 노드 및 조건 노드 작성: 트리 구조에 맞게 각 행동을 수행하는 행동 노드와 조건을 판단하는 조건 노드를 작성한다. ROS2에서는 각 노드를 ROS 노드로 작성하여 퍼블리셔와 서브스크라이버를 활용할 수 있다.

#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"

class MoveToGoalNode : public rclcpp::Node {
public:
    MoveToGoalNode() : Node("move_to_goal_node") {
        goal_publisher_ = this->create_publisher<geometry_msgs::msg::PoseStamped>("goal_pose", 10);
        // 목표 위치 설정
    }

    void publishGoal() {
        auto msg = geometry_msgs::msg::PoseStamped();
        // 목표 위치 메시지 설정
        goal_publisher_->publish(msg);
    }

private:
    rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr goal_publisher_;
};

트리 실행 및 결과 분석: Behavior Tree를 실행하고, 결과를 분석하여 행동 계획이 올바르게 작동하는지 확인한다. ROS2에서는 BT::NodeStatus를 활용하여 각 행동의 상태(성공, 실패, 진행 중)를 추적할 수 있다.

#include "behaviortree_cpp_v3/bt_factory.h"

BT::NodeStatus MoveToGoalAction() {
    // 목표로 이동하는 행동 실행
    return BT::NodeStatus::SUCCESS;
}

행동 계획의 실행 흐름

ROS2에서의 행동 계획 알고리즘은 실시간으로 동작하며, 각 노드의 상태가 주기적으로 갱신된다. 행동 계획이 성공적으로 이루어지기 위해서는 각 노드의 상태 전환이 원활하게 이루어져야 하며, 환경 변화에 따라 동적으로 계획이 수정될 수 있다.

행동 계획 알고리즘의 상태 전환은 아래와 같은 수식으로 표현할 수 있다.

$\mathbf{x}(t+1) = f(\mathbf{x}(t), \mathbf{u}(t))$

여기서,
- $\mathbf{x}(t)$ 는 시간 $t$ 에서의 상태 벡터,
- $\mathbf{u}(t)$ 는 시간 $t$ 에서의 제어 입력을 나타낸다.
이 상태 전이 방정식은 로봇의 행동이 현재 상태에서 다음 상태로 어떻게 전이되는지를 모델링한다.

행동 계획 알고리즘에서는 현재 상태와 목표 상태를 비교하여 다음 행동을 선택한다. 이때, 상태 벡터는 다음과 같이 정의될 수 있다:

$\mathbf{x} = \begin{bmatrix} x \\ y \\ \theta \end{bmatrix}$

여기서,
- $x, y$ 는 로봇의 위치,
- $\theta$ 는 로봇의 방향이다.

ROS2의 행동 계획 알고리즘에서는 이러한 상태 벡터와 목표 상태를 기반으로 제어 입력 $\mathbf{u}$ 를 계산하여 로봇의 행동을 결정한다.

상태 전이와 목표 설정

행동 계획 알고리즘에서 상태 전이는 로봇이 목표 상태에 도달하기 위한 경로를 결정하는 중요한 요소이다. ROS2의 행동 계획은 목표 상태 $\mathbf{x}_{goal}$ 에 도달하는 과정에서 다양한 제어 입력 $\mathbf{u}(t)$ 를 생성한다. 이때 목표 상태와 현재 상태 사이의 차이를 줄이기 위해, 제어 입력은 다음과 같이 계산된다.

$\mathbf{u}(t) = K_p (\mathbf{x}_\text{goal} - \mathbf{x}(t))$

여기서,
- $\mathbf{x}_\text{goal}$ 은 목표 상태 벡터,
- $\mathbf{x}(t)$ 는 현재 상태 벡터,
- $K_p$ 는 제어 이득으로, 목표 상태와 현재 상태의 차이에 비례하여 제어 입력을 결정한다.

이 상태 전이 모델을 사용하면, 로봇은 목표 위치에 도달하기 위한 제어 명령을 지속적으로 업데이트할 수 있다. 이 과정에서 로봇의 행동 계획 알고리즘은 다음과 같은 추가적인 고려 사항을 다룬다.

행동 계획에서 경로 계획 통합

행동 계획 알고리즘은 종종 경로 계획(Path Planning)과 통합된다. 경로 계획은 로봇이 장애물을 피하면서 목표에 도달할 수 있는 최적 경로를 찾는 과정으로, ROS2의 nav2 패키지를 통해 경로 계획을 손쉽게 구현할 수 있다.

경로 계획의 목표는 로봇이 안전하게 목표 지점까지 도달할 수 있는 경로 $\mathbf{p}(t)$ 를 생성하는 것이다. 이를 위한 경로 계획 알고리즘은 다음과 같은 수식으로 표현된다.

$\mathbf{p}(t) = \arg\min_{\mathbf{p}} \int_0^T \left( C(\mathbf{p}(t), \mathbf{u}(t)) + \lambda \|\mathbf{u}(t)\|^2 \right) dt$

여기서,
- $\mathbf{p}(t)$ 는 시간 $t$ 에서의 경로,
- $C(\mathbf{p}(t), \mathbf{u}(t))$ 는 비용 함수로, 주어진 경로에서의 위험도나 장애물 피하기를 나타낸다,
- $\lambda$ 는 제어 입력 $\mathbf{u}(t)$ 의 크기에 대한 가중치이다.

이 최적화 문제를 해결하면, 로봇이 최소한의 에너지 소비로 목표에 도달할 수 있는 최적 경로를 찾을 수 있다. ROS2에서 경로 계획과 행동 계획의 통합은 행동 계획 노드가 경로 계획의 결과를 활용하여 로봇의 다음 행동을 결정하는 방식으로 이루어진다.

행동 계획에서의 피드백 제어

ROS2에서 행동 계획을 수행하는 동안, 로봇은 실시간으로 센서 데이터를 수집하고 행동의 성공 여부를 피드백으로 받는다. 이 피드백을 기반으로 행동 계획을 동적으로 수정할 수 있다. ROS2의 Action 서버는 이러한 피드백 제어를 가능하게 하며, 행동 계획을 보다 유연하게 구성할 수 있다.

피드백 제어는 로봇의 현재 상태 $\mathbf{x}(t)$ 와 목표 상태 $\mathbf{x}_\text{goal}$ 사이의 오차를 기반으로 제어 입력을 수정하는 방식으로 동작한다.

$\mathbf{e}(t) = \mathbf{x}_\text{goal} - \mathbf{x}(t)$

여기서,
- $\mathbf{e}(t)$ 는 시간 $t$ 에서의 오차 벡터이다.
오차 벡터는 제어 입력을 수정하는 데 사용되며, 이를 통해 로봇이 목표에 더 정확하게 도달할 수 있다.

ROS2에서는 Action 클라이언트가 주기적으로 피드백 데이터를 받아서 제어 입력을 조정하며, 이를 통해 행동 계획의 성공 가능성을 높인다.

#include "rclcpp/rclcpp.hpp"
#include "action_msgs/msg/goal_status.hpp"

class FeedbackControlNode : public rclcpp::Node {
public:
    FeedbackControlNode() : Node("feedback_control_node") {
        feedback_subscriber_ = this->create_subscription<action_msgs::msg::GoalStatus>(
            "feedback_topic", 10, std::bind(&FeedbackControlNode::feedbackCallback, this, std::placeholders::_1));
    }

private:
    void feedbackCallback(const action_msgs::msg::GoalStatus::SharedPtr msg) {
        // 피드백 데이터를 기반으로 제어 입력 조정
    }

    rclcpp::Subscription<action_msgs::msg::GoalStatus>::SharedPtr feedback_subscriber_;
};

실시간 행동 계획

ROS2에서의 실시간 행동 계획은 로봇이 동적 환경에서 실시간으로 계획을 수정하고 행동을 결정해야 할 때 중요하다. 특히 장애물이 계속해서 움직이거나 환경이 빠르게 변화하는 상황에서는 실시간 계획이 필요하다. 이러한 상황에서 로봇은 목표 상태로의 경로를 실시간으로 수정하며 행동을 계속 업데이트한다.

실시간 행동 계획은 주기적으로 목표 상태 $\mathbf{x}_\text{goal}$ 를 갱신하고, 환경 변화를 반영한 새로운 제어 입력을 계산하는 과정을 반복한다.

$\mathbf{u}(t+1) = K_p (\mathbf{x}_\text{goal} - \mathbf{x}(t+1))$

여기서,
- $\mathbf{x}(t+1)$ 는 시간 $t+1$ 에서의 로봇의 상태,
- $\mathbf{u}(t+1)$ 는 시간 $t+1$ 에서 계산된 제어 입력이다.

이와 같이 행동 계획 알고리즘은 실시간 데이터를 기반으로 목표 상태를 지속적으로 재조정하고, 로봇이 최적 경로를 따라 목표에 도달하도록 제어한다.

실시간 제어와 행동 계획의 융합

ROS2에서 실시간 제어는 특히 자율 주행 로봇이나 드론과 같이 빠른 반응과 환경 적응이 요구되는 시스템에서 필수적이다. 실시간 제어는 로봇이 환경의 변화에 즉각적으로 반응하도록 제어 입력을 실시간으로 조정하는 과정을 포함한다. 이는 행동 계획과 융합되어 로봇이 목표를 달성하는 동시에 안전하게 동작할 수 있게 한다.

실시간 제어에서는 센서 피드백과 환경 정보가 제어 입력에 즉각적으로 반영된다. 예를 들어, 장애물이 감지되면 즉시 경로를 수정하거나 속도를 줄여야 할 수 있다. 이러한 과정에서의 제어 입력은 다음과 같이 표현된다.

$\mathbf{u}(t+1) = \mathbf{u}(t) + K_d (\mathbf{e}(t) - \mathbf{e}(t-1))$

여기서,
- $K_d$ 는 미분 이득,
- $\mathbf{e}(t)$ 는 시간 $t$ 에서의 오차,
- $\mathbf{e}(t-1)$ 는 이전 시간 $t-1$ 에서의 오차이다.

이 방정식은 PID 제어의 미분 제어(Derivative Control)를 포함한 형태로, 오차의 변화율을 제어 입력에 반영한다. 이는 로봇이 급격한 변화에 빠르게 대응할 수 있도록 도와준다.

PID 제어와 행동 계획

ROS2의 실시간 행동 계획에서 PID 제어는 일반적으로 사용되는 제어 방법이다. PID 제어는 비례(Proportional), 적분(Integral), 미분(Derivative) 제어를 통해 로봇의 목표 상태에 도달할 수 있게 한다.

비례 제어 (Proportional Control, P): 현재 오차에 비례하여 제어 입력을 조정한다.

$\mathbf{u}_P(t) = K_p \mathbf{e}(t)$

적분 제어 (Integral Control, I): 오차의 누적값을 기반으로 제어 입력을 보정하여 작은 오차도 장기적으로 해결할 수 있게 한다.

$\mathbf{u}_I(t) = K_i \int_{0}^{t} \mathbf{e}(\tau) d\tau$

미분 제어 (Derivative Control, D): 오차의 변화율을 기반으로 제어 입력을 조정하여 갑작스러운 변화에 빠르게 대응한다.

$\mathbf{u}_D(t) = K_d \frac{d\mathbf{e}(t)}{dt}$

PID 제어는 이 세 가지 요소를 결합하여 로봇이 안정적으로 목표에 도달할 수 있도록 보장한다.

$\mathbf{u}(t) = K_p \mathbf{e}(t) + K_i \int_{0}^{t} \mathbf{e}(\tau) d\tau + K_d \frac{d\mathbf{e}(t)}{dt}$

ROS2에서 PID 제어는 행동 계획의 일환으로 구현될 수 있으며, 특히 경로 추종(Path Following)과 같은 연속적인 제어 작업에서 유용하다.

ROS2에서의 동적 행동 계획

ROS2에서 동적 행동 계획을 구현하기 위해서는 환경 변화를 실시간으로 반영할 수 있어야 한다. 이를 위해 센서 피드백과 퍼블리셔/서브스크라이버 모델이 활용된다. 로봇은 실시간 센서 데이터를 받아서 행동 계획을 동적으로 조정하고, 목표 상태에 도달하는 경로를 변경하거나 장애물을 회피할 수 있다.

예제: 동적 장애물 회피

ROS2에서 동적 장애물 회피는 로봇이 실시간으로 환경을 스캔하고, 장애물 위치를 기반으로 경로를 수정하는 과정이다. 이는 행동 계획 알고리즘이 목표를 재설정하거나 경로를 변경하도록 유도한다.

#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/laser_scan.hpp"

class ObstacleAvoidanceNode : public rclcpp::Node {
public:
    ObstacleAvoidanceNode() : Node("obstacle_avoidance_node") {
        scan_subscriber_ = this->create_subscription<sensor_msgs::msg::LaserScan>(
            "scan", 10, std::bind(&ObstacleAvoidanceNode::scanCallback, this, std::placeholders::_1));
    }

private:
    void scanCallback(const sensor_msgs::msg::LaserScan::SharedPtr msg) {
        // 장애물 데이터를 처리하고 경로를 수정
    }

    rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr scan_subscriber_;
};

위 코드에서는 라이다 데이터를 받아서 장애물을 탐지하고, 탐지된 데이터를 기반으로 경로를 동적으로 수정할 수 있다. 로봇의 행동 계획은 이러한 실시간 데이터를 기반으로 즉시 변경되며, 목표 상태에 도달할 수 있도록 지속적으로 조정된다.

행동 계획의 의사결정 트리와 실행 흐름

로봇이 동적 환경에서 행동 계획을 수행할 때는 복잡한 의사결정 트리를 통해 행동을 결정한다. 예를 들어, 특정 조건이 충족되었을 때만 특정 행동을 실행하거나, 실패 시 다른 행동을 시도하는 방식이다.

graph TD; A[시작] --> B{장애물 탐지?}; B -->|예| C[회피 행동]; B -->|아니오| D[목표로 이동]; C --> E[목표 재설정]; D --> F[목표 도달];

위와 같은 의사결정 트리는 로봇이 주어진 환경에서 장애물을 탐지할 경우 회피 행동을 취하고, 장애물이 없는 경우 목표로 이동하는 과정을 나타낸다.

ROS2에서 의사결정 트리는 Behavior Tree나 상태 기계를 통해 구현될 수 있으며, 로봇이 주어진 목표에 도달하기 위한 다양한 경로를 고려할 수 있다. 의사결정 과정에서 각 노드는 상태를 갱신하고, 목표 상태와 환경 조건을 반영하여 최적의 행동을 선택한다.