ROS2: TSファジィ推論・制御を実装してみた
海洋ロボコンをやってた人です。
今回は、C++およびC++/ROS2でTSファジィ推論・制御を実装してみたので備忘録としてまとめます。
経緯としては、ChatGPT使いながら新しい知見(ファジィ制御)を学ぼうと思ったためで
目的は、学内外でのイベントで水中ロボットをプールで泳がせる際に、ファジィ制御による自動運転をさせて手離れさせることです。
（上記実現には、外界認知、自己位置・速度がわかるセンサがあるのが適切）
2022年12月初旬、OpenAIが開発した言語モデルを用いたチャットツール「ChatGPT」が世界中で話題になり、様々な意見が飛び交う中で、これからAIと向き合う手法としてどんなものがあるか？と考えていたことも、きっかけの一つになったと言えるでしょう。
実際にChatGPTに様々な問を与えてみると、かなり的を得た返答を得ることができます。
ただプログラムの記述の場合、細かく要件を与えないとこちらの意図したものにならないことがあるため、ChatGPTを辞書・参考書（もしくは気軽に質問できる知人）と考えて表題のファジィ推論・制御のプログラムを実装したことが本記事の始まりになります。
手順は
- 参考書、ネット記事でファジィ推論・制御を学ぶ
- ChatGPTに理論とプログラムを聞いてみる
- 1.2.を元に理論を理解し、プログラムを実装する
として進めていきました。
"もっとこうすれば簡単に使用できる"や"ここは違うのではないか"というご意見がありましたら、是非ご教授いただけますと幸いです。
また、私がファジィについて話すのは恐れ多いため、以下説明は「ChatGPT」を引用しています。
内容が間違っている箇所があるかもしれませんので、色々とご指摘いただけると幸いです。
※2023/02/26追記
本記事の「ROS2」表記、正しくは「ROS 2」です。
ファジィ制御とは
ファジィ制御（Fuzzy Control）とは、システムの制御を行う手法の1つです。通常の制御では、制御量を数値的に決める必要がありますが、ファジィ制御では制御量を「ぼやけた」状態で表現することができます。(引用：ChatGPT)
簡単に要約すると、「あいまいさ」を含む言語表現を制御量として表現し、制御を行うこと。と私は理解しています。
こちらの制御方法を用いて
- 入力を距離の近い、普通、遠い
- 出力をロボット操作量の小さい、普通、大きい
として実装していきます。
ファジィ集合
ファジィ集合（fuzzy set）とは、集合理論における概念で、不確定な情報や模糊な情報を表現するために使われる集合のことです。通常の集合では、要素が集合に含まれるかどうかは「含む」か「含まない」の2つの状態しか存在しません。しかし、ファジィ集合では、要素が集合に含まれる確率を定義することができます。そのため、ファジィ集合では、要素が集合に含まれる確率を「階級値」と呼ばれる値で表現することができます。(引用: ChatGPT)
こちらも要約すると、「あいまいな」集合を合理的に処理するために使用される集合ということでしょう。
このファジィ集合Aは、全体集合をXとすると、要素が集合に含まれる確率を「階級値」として定義した関数「ファジィメンバーシップ関数：uA」を用いて以下のように定義できます。
また、メンバーシップ関数には以下のような関数が一般的に用いられます。
ガウス型関数
三角型関数
引用：ファジィ推論システムの能力と学習法に関する研究:2016/3 宮島 洋文
他にも
ファジィ制御で使われるメンバーシップ関数には、次のような種類があります。
1.トライアングル関数：3つのパラメータ（a、b、c）をもつ関数で、aより小さい値を0、bを1、cより大きい値を0とするような関数です。
2.トラピズム関数：4つのパラメータ（a、b、c、d）をもつ関数で、aより小さい値を0、bを1、cを1、dより大きい値を0とするような関数です。
3.シグモイド関数：2つのパラメータ（a、b）をもつ関数で、xが無限大のときを1、xが無限小のときを0とするようなS字カーブを描く関数です。
4.ガウス関数：2つのパラメータ（μ、σ）をもつ関数で、正規分布を表す関数です。
（引用：ChatGPT）
といったメンバーシップ関数があり、これらを適切に選択することで、より自然な表現ができるようになるそうです。
今回は、ファジィ制御の基礎を試したいので、以下のメンバーシップ関数を使用することにします。
距離という全体集合Xに対して、短い、普通、遠いという3つのファジィ集合をもつメンバーシップ関数を三角型関数として定義しました。
ファジィ条件
ファジィ条件（fuzzy conditional）とは、模糊条件を含む条件文を表すものです。ファジィ条件分は、通常の条件文と同様に、「もし〜ならば〜」という形をしています。しかし、通常の条件文では、条件部分と結果部分が明確に定義されているのに対し、ファジィ条件分では、条件部分や結果部分が模糊的であることがあります。(引用：ChatGPT)
簡単に要約すると
- If x is A, then y is B
という「IF-THEN形式」で記述した条件をファジィ条件といい
「x is A」の部分を前件部、「y is B」の部分を後件部というそうです。
以下、簡略型ファジィ推論法を一般化したTS(Takagi Sugeno)ファジィ推論法を用いることにします。
TSファジィ推論法の推論規則は
と与えられており、推論規則Riに対する適応度ui、定数をfiとすると、推論出力は以下で導出ができます。
引用：ファジィ推論システムの能力と学習法に関する研究:2016/3 宮島 洋文
定数fiは私達が任意に設定して出力を調整することが可能です。
上記のTSファジィ推論規則に基づき、距離の短い、普通、遠いに対しての操作量を決めるルールを定義しました。
ファジィ制御系の構築
ファジィメンバーシップ関数、ファジィ条件文を決めたら、これらをモデル化していきます。
こちらでは、不確実な情報を数値化するための脱ファジィ化：defuzzificationのブロックが入っていますが、TSファジィ推論の出力は不確実である可能性があるので、脱ファジィ化とは異なります。
繰り返しになりますが、以下のTSファジィ推論出力にdefuzzifyといった定義がありますが、不確実性を含むため脱ファジィ化ではないということは記憶に残しておいてください。
↑もChatGPTに聞きました。
C++/ROS2でTSファジィ推論・制御を実装
以上を踏まえ、C++やROS2でTSファジィ推論・制御を実装していきます。
C++でTSファジィ制御を実装
とりあえず、ChatGPTにプログラムを聞いてみました。
上記のようにメンバーシップ関数を定義して、プログラムを記述してくれます。
ある程度はChatGPTに頼り、細かいところは自分で追記・変更していきます。
#include <iostream>
#include <algorithm>
#include <vector>
const double MAX_DIST = 4.0;
const double INPUT_MAX = 1.0;
const double INPUT_MIN = 0.0;
const int kNumSamples = 100; // サンプル数
double dist_min = MAX_DIST/4;
double dist_mid = MAX_DIST/2;
double dist_max = MAX_DIST*3/4;
class FuzzyInference{
public:
double short_distance(double d) {
if (d < dist_min) {
return INPUT_MAX;
} else if (d < dist_mid) {
return dist_mid - d;
} else {
return INPUT_MIN;
}
}
double medium_distance(double d) {
if (d < dist_min || d > dist_max) {
return INPUT_MIN;
} if (d < dist_mid) {
return d - dist_min;
} else { // d < dist_max
return dist_max - d;
}
}
double long_distance(double d) {
if (d < dist_mid) {
return INPUT_MIN;
} else if (d < dist_max) {
return d - dist_mid;
} else {
return INPUT_MAX;
}
}
double apply_rule1(double short_distance) {
return short_distance;
}
double apply_rule2(double medium_distance) {
return medium_distance;
}
double apply_rule3(double long_distance) {
return long_distance;
}
double defuzzify(double rule1_output, double rule2_output, double rule3_output) {
// 重心計算のために、各ルールの貢献度を計算する
double w1 = 0.0;
double w2 = 1.0;
double w3 = 2.0;
double numerator = rule1_output * w1 + rule2_output * w2 + rule3_output * w3;
double denominator = rule1_output + rule2_output + rule3_output;
// 重心を計算して、それを出力とする
double defuzzify_output = numerator / denominator;
return defuzzify_output/w3;
}
};
int main() {
FuzzyInference fuzzy_inference;
double d;
std::cout << "Enter distance: ";
std::cin >> d;
// 3つのメンバーシップ関数を呼び出す
double short_distance = fuzzy_inference.short_distance(d);
double medium_distance = fuzzy_inference.medium_distance(d);
double long_distance = fuzzy_inference.long_distance(d);
std::cout << "short_dist: " << short_distance << std::endl;
std::cout << "medium_dist: " << medium_distance << std::endl;
std::cout << "long_dist: " << long_distance << std::endl;
// 3つのルールを適用する
double rule1 = fuzzy_inference.apply_rule1(short_distance);
double rule2 = fuzzy_inference.apply_rule2(medium_distance);
double rule3 = fuzzy_inference.apply_rule3(long_distance);
// 脱ファジィ化を行う
double output = fuzzy_inference.defuzzify(rule1, rule2, rule3);
std::cout << "Output: " << output << std::endl;
return 0;
}
ファジィルールとしては、各メンバーシップ関数の出力値0〜1をそのまま返すだけのものです。
また、TSファジィ推論出力の定数は、ルール3：R3の貢献度を高し、ルール1：R1の貢献度を低くするように設定しています。
プログラム記述後、ビルドし以下を実行してみると、距離が遠くなるほど、出力の操作量が大きいものが得られることが分かります。
ros2 run fuzzy_controller ts_fuzzy_test1
Enter distance: 0.5
short_dist: 1
medium_dist: 0
long_dist: 0
Output: 0
Enter distance: 1.7
short_dist: 0.3
medium_dist: 0.7
long_dist: 0
Output: 0.35
Enter distance: 2.6
short_dist: 0
medium_dist: 0.4
long_dist: 0.6
Output: 0.8
ROS2でTSファジィ制御を実装
続いて上記のC++プログラムをROS2でも実装していきます。
パッケージの構成として以下のようになっており
fuzzy_controller/
├ package.xml
├ CMakeLists.txt
├ config/
├ ros_param.yaml
├ include/
├ fuzzy_controller/
├ ts_fuzzy_controller_component.hpp
├ src/
├ ts_fuzzy_controller_component.cpp
├ ts_fuzzy_node.cpp
├ ts_fuzzy_test1.cpp
├ ts_fuzzy_test2.cpp
xxx.hppおよびmain(){}を記述したxxx_node.cppは関数・変数定義および関数の実行のみなので、割愛しています。詳しくは以下よりご確認ください。
本記事ではプログラムの構成を担っているcomponent.cppを以下に記載します。
#include <iostream>
#include <algorithm>
#include <vector>
#include "fuzzy_controller/ts_fuzzy_controller_component.hpp"
using namespace std::chrono_literals;
using std::placeholders::_1;
FuzzyControllerComponent::FuzzyControllerComponent(const rclcpp::NodeOptions & options)
: Node("ts_fuzzy_node", options)
{
declare_parameter("dist_range", 4.0);
get_parameter("dist_range", dist_range_);
declare_parameter("dist_min", dist_range_/4);
get_parameter("dist_min", dist_min_);
declare_parameter("dist_mid", dist_range_/2);
get_parameter("dist_mid", dist_mid_);
declare_parameter("dist_max", dist_range_*3/4);
get_parameter("dist_max", dist_max_);
declare_parameter("weight_1", 0.0);
get_parameter("weight_1", weight_1_);
declare_parameter("weight_2", 1.0);
get_parameter("weight_2", weight_2_);
declare_parameter("weight_3", 2.0);
get_parameter("weight_3", weight_3_);
auto default_qos = rclcpp::QoS(rclcpp::SystemDefaultsQoS());
subscriber_ = this->create_subscription<sensor_msgs::msg::LaserScan>(
"/scan",
default_qos,
std::bind(&FuzzyControllerComponent::timer_callback, this, _1)\
);
publisher_ = this->create_publisher<std_msgs::msg::Float32>("/defuzzy_input", 10);
}
void FuzzyControllerComponent::timer_callback(const sensor_msgs::msg::LaserScan::SharedPtr scan_msg)
{
double d = scan_msg->ranges[(scan_msg->ranges.size())/2];
// debag
RCLCPP_INFO(this->get_logger(), "distance: %f",d);
// Call 3 membership functions
double short_distance = this->short_distance(d);
double medium_distance = this->medium_distance(d);
double long_distance = this->long_distance(d);
// Apply 3 fuzzy rules
double rule1 = this->apply_rule1(short_distance);
double rule2 = this->apply_rule2(medium_distance);
double rule3 = this->apply_rule3(long_distance);
// perform defuzzification
double defuzzy = this->defuzzify(rule1, rule2, rule3);
RCLCPP_INFO(this->get_logger(), "defuzzy_input: %f",defuzzy);
std_msgs::msg::Float32 defuzzy_msg;
defuzzy_msg.data = float(defuzzy);
publisher_->publish(defuzzy_msg);
}
double FuzzyControllerComponent::short_distance(double d) {
if (d < dist_min_) {
return INPUT_MAX;
} else if (d < dist_mid_) {
return dist_mid_ - d;
} else {
return INPUT_MIN;
}
}
double FuzzyControllerComponent::medium_distance(double d) {
if (d < dist_min_ || d > dist_max_) {
return INPUT_MIN;
} if (d < dist_mid_) {
return d - dist_min_;
} else { // d < dist_max
return dist_max_ - d;
}
}
double FuzzyControllerComponent::long_distance(double d) {
if (d < dist_mid_) {
return INPUT_MIN;
} else if (d < dist_max_) {
return d - dist_mid_;
} else {
return INPUT_MAX;
}
}
double FuzzyControllerComponent::apply_rule1(double short_distance) {
return short_distance;
}
double FuzzyControllerComponent::apply_rule2(double medium_distance) {
return medium_distance;
}
double FuzzyControllerComponent::apply_rule3(double long_distance) {
return long_distance;
}
double FuzzyControllerComponent::defuzzify(double rule1_output, double rule2_output, double rule3_output) {
// Define weight for each rule
double w1 = weight_1_;
double w2 = weight_2_;
double w3 = weight_3_;
double numerator = rule1_output * w1 + rule2_output * w2 + rule3_output * w3;
double denominator = rule1_output + rule2_output + rule3_output;
double defuzzify_output = numerator / denominator;
return defuzzify_output/w3;
}
基本的には、各項目ごとに関数化しているだけです。
入力として、/scanトピック（scanレンジの中央の値）を受け取り、出力として/defuzzy_inputを送信するようにしています。
プログラムを実行し可視化してみると、距離に対応した推論出力を得られていることがわかります。
ROS2でパラメータを指定して記述
上記のプログラムでは、パラメータを指定できるような記述もしています。
こちらは
の記述方法をものすご〜く真似しています。
OUXT-Polaris関係者の皆様、大変勉強になります。
オープンソースで公開して頂き、ありがとうございます😆😆
- paramの定義で指定する方法
ROS2のC++でパラメータを設定するために、rclcpp::NodeOptions、declare_parameter、get_parameterを用いて以下のような記述をしています。
FuzzyControllerComponent::FuzzyControllerComponent(const rclcpp::NodeOptions & options)
: Node("ts_fuzzy_node", options)
{
declare_parameter("dist_range", 4.0);
get_parameter("dist_range", dist_range_);
declare_parameter("dist_min", dist_range_/4);
get_parameter("dist_min", dist_min_);
declare_parameter("dist_mid", dist_range_/2);
get_parameter("dist_mid", dist_mid_);
declare_parameter("dist_max", dist_range_*3/4);
get_parameter("dist_max", dist_max_);
declare_parameter("weight_1", 0.0);
get_parameter("weight_1", weight_1_);
declare_parameter("weight_2", 1.0);
get_parameter("weight_2", weight_2_);
declare_parameter("weight_3", 2.0);
get_parameter("weight_3", weight_3_);
この記述により
ros2 run fuzzy_controller ts_fuzzy_controller
と実行して、param listを打つと
ros2 param list
/ts_fuzzy_node:
dist_max
dist_mid
dist_min
dist_range
use_sim_time
weight_1
weight_2
weight_3
とパラメータのリストが表示され、パラメータをセットすると
ros2 param get ts_fuzzy_node dist_range
Double value is: 4.0
ros2 param set ts_fuzzy_node dist_range 6.0
Set parameter successful
ros2 param get ts_fuzzy_node dist_range
Double value is: 6.0
値を変更できます。
- yamlを使用してパラメータを渡す
また他にもyamlを用いてパラメータを設定する方法もあります。
config/ros_param.yamlというファイルを以下のように準備し
ts_fuzzy_node:
ros__parameters:
dist_range: 6.0
weight_1: 0.0
weight_2: 1.0
weight_3: 2.0
以下のように実行すると
ros2 run fuzzy_controller ts_fuzzy_controller --ros-args --params-file ~/manta_ws/src/fuzzy_controller/config/ros_param.yaml
yamlファイルにて設定した値がパラメータとして反映されます。
Discussion