ROS 2: 粒子群最適化 Particle Swarm Optimization(PSO)のC++/python実装
海洋ロボコンをやってた人です。
今回は最適化手法の中でも、汎用的な問題解決手法であるメタヒューリスティクスの粒子群最適化：Particle Swarm Optimizationについての概要から、C++、ROS2実装までを書いていこうと思います。
経緯としては、水中ロボットの動作生成の際に、様々なパラメータを決めることがあり、どのような手法を用いるとパラメータを簡潔かつ最適に決めれるか？と探しており、気がついた「進化計算」、「群知能」、「最適化手法」に行き着き、今に至る形です。
あくまでも上記は私の思想の一部であり、ロボットで最適化手法を用いるべき箇所は、市場や論文等を俯瞰し、それぞれ活用していただければと思います。
本記事では、最適化手法の実装プログラムって、こんな感じで実装できるんだ！という認知を広げる、自分自身の備忘録としていますので、お役に立てば幸いです。
何か、間違っている箇所等あれば、お気軽にご教授いただけると嬉しく思います。
※2023/02/26 追記: 本記事の「ROS2」表記、正しくは「ROS 2」です。
※2023/10/13 追記: python用のプログラムを追加
※また、各説明で必要なものは引用検索の手間削減のためChatGPTを活用しています。
※ご理解いただけますと幸いです。
粒子群最適化(PSO)をC++で実装する
粒子群最適化：PSOとは？
はじめに、粒子群最適化：PSOとは、鳥を例に説明すると
鳥の群が飛行するような振る舞いを模倣していることからPSOという名前が付けられています。
想像するとしたら、複数の鳥が一つの目的地に向かって飛んでいるとしましょう。各鳥は今までに見つけた最良の位置を記憶していて、他の鳥の最良の位置も参考にすることで、自分自身の位置を更新します。最終的に鳥たちは目的地に到達するか、近いところに到達することができます。
同じように、粒子群最適化アルゴリズムでは、複数の粒子が目的とする最適解に向かって探索を行います。粒子は今までに見つけた最良の解を記憶していて、他の粒子の最良の解も参考にして、自分自身の位置を更新します。最終的に、粒子たちは最適解に到達するか、近いところに到達することができます。
引用：ChatGPT
簡潔に言えば、鳥や魚の群れの動きを数理アルゴリズムにしたものということでしょう。
これを、数理アルゴリズムで定義したものがPSOとなり、理論は以下のようになります。
※ Particle swarm optimization (PSO). A tutorialより数式は引用していきます。
各候補解は「粒子:Particle」と呼ばれ、この粒子がD次元空間を動く場合、i番目の粒子の位置は、以下のベクトル
この粒子がN個集まってできた群れ:SwarmをXとすると
と表すことができます。
ここで、問題の最適解を探す際、粒子は運動方程式に基づいてD次元空間を移動していきます。
この運動方程式を考えるとき、各次元にごとにベクトルの移動を考えればよく、D次元に沿って粒子が移動する速度viを用いて、粒子の位置は
と表すことができ、速度vは
と定義されます。
ここで
を示します。
これらの式を、論文のFig. 1. Scheme of the basic PSO algorithm.にあるように実装していけば良いというわけです。
簡単に記載しておくと
という流れになると思います。
詳しくは原著やその他書籍等をご参照ください。
以下は私が学ぶ際に熟読させていただいたもので、とても勉強になりました。ありがとうございました。
PSOのC++実装例
大枠の理論がわかったらC++で実装していきます。
実装の簡略化のため、目的関数
#include <stdlib.h>
#include<stdio.h>
#include <time.h>
#include <cfloat>
class Pso{
public:
Pso();
~Pso();
void swarm_move();
struct particle
{
double *position_ = nullptr;
double *velocity_ = nullptr;
double *personal_best_pos_ = nullptr;
double personal_best_value_ = DBL_MIN;
double value;
};
struct particle *particles; //Declarations that use pointers in structures
double rand01 = ((double)rand() / RAND_MAX);
double *global_best_pos = nullptr;
double global_best_value = DBL_MIN;
int gbest_num;
private:
void evaluate(int swarm);
void particle_move(int swarm);
void personal_update(int swarm);
double xminj = -1;
double xmaxj = 1;
double vminj = -1;
double vmaxj = 1;
int swarm_size_ = 100; // particle swarm size = particle * size
int dimension_ = 2; // particle dimensions = number of solutions
int dt_ = 30; // period
double inertia_ = 0.9; // Inertia
double c1_ = 0.8; // acc rand range
double c2_ = 0.8;
};
Pso::Pso(){
// Initialize the particle
particles = new particle[swarm_size_];
for (int i=0; i<swarm_size_; i++){
particles[i].position_ = new double[dimension_];
particles[i].velocity_ = new double[dimension_];
particles[i].personal_best_pos_ = new double[dimension_];
for (int j=0; j<dimension_; j++){
particles[i].position_[j] = xminj + (xmaxj - xminj) * ((double)rand() / RAND_MAX);
particles[i].velocity_[j] = vminj + (vmaxj - vminj) * ((double)rand() / RAND_MAX);
particles[i].personal_best_pos_[j] = particles[i].position_[j];
}
// printf("Init pos %f, %f", particles[i].position_[0], particles[i].position_[1]);
this->evaluate(i);
}
// Initialize global best pos and value
global_best_pos = new double[dimension_];
gbest_num = 0;
for (int i=0; i<swarm_size_; i++){
if(particles[i].value < particles[gbest_num].value){
gbest_num = i;
}
}
global_best_value = particles[gbest_num].value;
for (int j=0; j<dimension_; j++){
global_best_pos[j] = particles[gbest_num].position_[j];
}
}
Pso::~Pso(){
delete [] particles->position_;
delete [] particles->velocity_;
delete [] particles->personal_best_pos_;
delete [] global_best_pos;
}
void Pso::evaluate(int swarm){
double A = particles[swarm].position_[0];
double B = particles[swarm].position_[1];
particles[swarm].value = A*A + B*B;
// printf("value: %f\n", particles[swarm].value);
}
void Pso::particle_move(int swarm){
for (int j=0; j<dimension_; j++){
particles[swarm].velocity_[j] = inertia_ * particles[swarm].velocity_[j]
+ c1_ * ((double)rand() / RAND_MAX) * (particles[swarm].personal_best_pos_[j] - particles[swarm].position_[j])
+ c2_ * ((double)rand() / RAND_MAX) * (global_best_pos[j] - particles[swarm].position_[j]);
particles[swarm].position_[j] = particles[swarm].position_[j] + particles[swarm].velocity_[j];
}
this->evaluate(swarm);
}
void Pso::personal_update(int swarm){
if (particles[swarm].value < particles[swarm].personal_best_value_){
for(int j=0; j<dimension_; j++){
particles[swarm].personal_best_pos_[j] = particles[swarm].position_[j];
}
particles[swarm].personal_best_value_ = particles[swarm].value;
}
}
void Pso::swarm_move(){
for (int i=0; i < swarm_size_; i++){
this->particle_move(i);
this->personal_update(i);
if (particles[i].value < global_best_value){
gbest_num = i;
global_best_value = particles[gbest_num].value;
for(int j=0; j<dimension_; j++){
global_best_pos[j] = particles[gbest_num].position_[j];
}
}
}
printf("global best: (x, y) = %f, %f: value = %f\n",
global_best_pos[0], global_best_pos[1], global_best_value);
}
int main(){
srand((unsigned int)time(NULL));
Pso swarm;
for (int i=0; i < 30; i++){
printf("Generation %d, ", i);
swarm.swarm_move();
}
return 0;
}
上記では、Psoという一つのクラスに実装で必要なものはすべて入れ込んであります。
粒子単体は構造体 struct particle内で定義されており、一つ一つの粒子が現在位置、現在速度、個人の自己ベスト位置、個人の自己ベスト評価値、現在の位置に対する評価値の5を持っています。
それをstruct particle *particlesでポインタの構造体として、群れ：swarmを作るようプログラムしています。
ポインタを使う場合、どうやらコンストラクタに引数を与えられないようだったので、クラスの中に構造体を入れ込む形としました。
コンストラクタPso::Pso()では群れの中の鳥・魚の数：swarm_size分だけ、鳥・魚の固体を準備し、初期位置と評価値を算出しています。
また、コンストラクタ内では群れの初期化後に、初期状態においてのグローバルベスト位置、値も算出しています。
Pso::evaluateは
Pso::particle_moveは各粒子の
Pso::personal_updateは各粒子に対して、現在の評価値が自己ベスト評価値よりも低い場合(勾配降下法のとき)に、グローバルベスト位置、値を更新するようになっています。
Pso::swarm_moveは粒子の合計数(swarm_size)分、粒子をmove, updateして、グローバルベストを求め、局所最適解である
実際に動作させると、このように局所最適解が求められます。
粒子群最適化(PSO)をpythonで実装する
よりpip3 install pyswarmsでインストール
# https://knowledge-bridge.info/technology/swarm-intelligence/429/
import numpy as np
import pyswarms as ps
# Rosenbrock function
def rosenbrock(x, a, b, c):
f = ( a - x[:,0]) **2 + b * (x[:,1] - x[:,0]**2 ) **2 + c
return f
# setup hpyerparameters
options = {'c1':0.5, 'c2':0.3, 'w':0.9 }
# create bounds
param_ranges = (np.array([-10, -10]), np.array([10, 10]))
# PSO instance
optimizer = ps.single.GlobalBestPSO( n_particles = 10, dimensions=2, options=options, bounds=param_ranges )
# performe optimization
best_params, best_value = optimizer.optimize( rosenbrock, iters=1000, a=1, b=100, c=0 )
# 結果を出力
print("最適なパラメータ: ", best_params)
print("最小値: ", best_value)
ROS2/C++でPSOを実装する
ROS2/C++でPSOを実装するに当たり、以下のように調整していきます。
入力データ(Float32 /input)を受け取ると、PSOにより局所最適解を求めるようなプログラムとし、粒子の総数(swarm_size)、解候補の次元(dimension)、繰り返し回数(dt)、慣性の重み(inertia)、加速係数(c1_, c2_)をparameterで変更できるようにしています。
ROS 2 C++/Pythonでプログラムを書くときは、パラメータで値を動的に変更できるようにしておくと、後々便利かと思います。
また、最終的に算出した局所最適解は出力データ(Float32 /output)としてpublishできるようにしています。
PSOのROS2/C++実装例
各関数はC++実装のときと変わらないので、とりあえず全文載せていきます。
各パスの関係は、お手数をおかけしますがご自身の環境・スタイル似合わせて変更してください。
#ifndef SWARM_INTELLIGENCE_ROS2__PSO__PSO_TOPIC__PSO_TOPIC_COMPONENT_HPP_
#define SWARM_INTELLIGENCE_ROS2__PSO__PSO_TOPIC__PSO_TOPIC_COMPONENT_HPP_
#include <stdlib.h>
#include<stdio.h>
#include <time.h>
#include <cfloat>
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/float32.hpp"
class PsoComponent : public rclcpp::Node
{
public:
PsoComponent(const rclcpp::NodeOptions & options);
~PsoComponent();
struct particle
{
double *position_ = nullptr;
double *velocity_ = nullptr;
double *personal_best_pos_ = nullptr;
double personal_best_value_ = DBL_MIN;
double value;
};
struct particle *particles; //Declarations that use pointers in structures
double rand01 = ((double)rand() / RAND_MAX);
double *global_best_pos = nullptr;
double global_best_value = DBL_MIN;
int gbest_num;
private:
rclcpp::TimerBase::SharedPtr timer_;
rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr publisher_;
rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr subscriber_;
void timer_callback(const std_msgs::msg::Float32::SharedPtr msg_data);
void particle_init();
void evaluate(int swarm);
void particle_move(int swarm);
void personal_update(int swarm);
void swarm_move();
double xminj = -1;
double xmaxj = 1;
double vminj = -1;
double vmaxj = 1;
int swarm_size_; // particle swarm size = particle * size
int dimension_; // particle dimensions = number of solutions
int dt_; // period
double inertia_; // Inertia
double c1_, c2_; // acc rand range
};
#endif // SWARM_INTELLIGENCE_ROS2__PSO__PSO_TOPIC__PSO_TOPIC_COMPONENT_HPP_
#include "swarm_intelligence_ros2/pso/pso_topic/pso_topic_component.hpp"
using namespace std::chrono_literals;
PsoComponent::PsoComponent(const rclcpp::NodeOptions & options)
: Node("pso_node", options)
{
this->declare_parameter("swarm_size", 100);
this->declare_parameter("dimension", 2);
this->declare_parameter("dt", 50);
this->declare_parameter("inertia", 0.9);
this->declare_parameter("c1", 0.8);
this->declare_parameter("c2", 0.8);
subscriber_ = this->create_subscription<std_msgs::msg::Float32>(
"/input",
10,
std::bind(&PsoComponent::timer_callback, this, std::placeholders::_1)\
);
publisher_ =
this->create_publisher<std_msgs::msg::Float32>("/output", 10);
}
PsoComponent::~PsoComponent(){
delete [] particles->position_;
delete [] particles->velocity_;
delete [] particles->personal_best_pos_;
delete [] global_best_pos;
}
void PsoComponent::timer_callback(
const std_msgs::msg::Float32::SharedPtr msg_data)
{
// Get parameter
swarm_size_ = this->get_parameter("swarm_size").get_parameter_value().get<int>();
dimension_ = this->get_parameter("dimension").get_parameter_value().get<int>();
dt_ = this->get_parameter("dt").get_parameter_value().get<int>();
inertia_ = this->get_parameter("inertia").get_parameter_value().get<double>();
c1_ = this->get_parameter("c1").get_parameter_value().get<double>();
c2_ = this->get_parameter("c2").get_parameter_value().get<double>();
// Initialize particle
this->particle_init();
double input_data = msg_data->data;
RCLCPP_INFO(this->get_logger(), "subscribe data %f:", input_data);
std_msgs::msg::Float32 data;
std_msgs::msg::Float32 data2;
// request->a = number of iterations
for (int i=0; i < dt_; i++){
RCLCPP_INFO(this->get_logger(), "Generation %d:", i);
this->swarm_move();
}
data.data = global_best_pos[0];
data2.data = global_best_pos[1];
publisher_->publish(data);
publisher_->publish(data2);
RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "global best (x, y): (%f, %f)", data.data, data2.data);
}
void PsoComponent::swarm_move()
{
// RCLCPP_INFO(this->get_logger(), "swarm %d:", swarm_size_);
for (int i=0; i < swarm_size_; i++){
this->particle_move(i);
this->personal_update(i);
if (particles[i].value < global_best_value){
gbest_num = i;
global_best_value = particles[gbest_num].value;
for(int j=0; j<dimension_; j++){
global_best_pos[j] = particles[gbest_num].position_[j];
}
}
}
RCLCPP_INFO(this->get_logger(), "global best: (x, y) = %f, %f: value = %f\n",
global_best_pos[0], global_best_pos[1], global_best_value);
}
void PsoComponent::particle_init(){
// Initialize the particle
particles = new particle[swarm_size_];
for (int i=0; i<swarm_size_; i++){
particles[i].position_ = new double[dimension_];
particles[i].velocity_ = new double[dimension_];
particles[i].personal_best_pos_ = new double[dimension_];
for (int j=0; j<dimension_; j++){
particles[i].position_[j] = xminj + (xmaxj - xminj) * ((double)rand() / RAND_MAX);
particles[i].velocity_[j] = vminj + (vmaxj - vminj) * ((double)rand() / RAND_MAX);
particles[i].personal_best_pos_[j] = particles[i].position_[j];
}
// printf("Init pos %f, %f", particles[i].position_[0], particles[i].position_[1]);
this->evaluate(i);
}
// Initialize global best pos and value
global_best_pos = new double[dimension_];
gbest_num = 0;
for (int i=0; i<swarm_size_; i++){
if(particles[i].value < particles[gbest_num].value){
gbest_num = i;
}
}
global_best_value = particles[gbest_num].value;
for (int j=0; j<dimension_; j++){
global_best_pos[j] = particles[gbest_num].position_[j];
}
}
void PsoComponent::particle_move(int swarm){
for (int j=0; j<dimension_; j++){
particles[swarm].velocity_[j] = inertia_ * particles[swarm].velocity_[j]
+ c1_ * ((double)rand() / RAND_MAX) * (particles[swarm].personal_best_pos_[j] - particles[swarm].position_[j])
+ c2_ * ((double)rand() / RAND_MAX) * (global_best_pos[j] - particles[swarm].position_[j]);
particles[swarm].position_[j] = particles[swarm].position_[j] + particles[swarm].velocity_[j];
}
this->evaluate(swarm);
}
// Optimize the parameters of this function
void PsoComponent::evaluate(int swarm){
double A = particles[swarm].position_[0];
double B = particles[swarm].position_[1];
particles[swarm].value = A*A + B*B;
}
void PsoComponent::personal_update(int swarm){
if (particles[swarm].value < particles[swarm].personal_best_value_){
for(int j=0; j<dimension_; j++){
particles[swarm].personal_best_pos_[j] = particles[swarm].position_[j];
}
particles[swarm].personal_best_value_ = particles[swarm].value;
}
}
#include "rclcpp/rclcpp.hpp"
#include "swarm_intelligence_ros2/pso/pso_topic/pso_topic_component.hpp"
int main(int argc, char * argv[])
{
rclcpp::init(argc, argv);
rclcpp::NodeOptions options;
auto component = std::make_shared<PsoComponent>(options);
rclcpp::spin(component);
rclcpp::shutdown();
return 0;
}
あとは下記のように、入力データとして適当にFloat32のデータ/inputをsubsclibeして、outputでPSOの局所最適解を/outputとしてpublishできていればOKになります。
以上、どなたかのお力添えになれば幸いです。
LikeやGithubのスター頂けると大変励みになるので、よろしくお願いいたします。
また、Githubの方には、PSOをROS2のservice、client形式で通信できるパッケージも掲載しているのでTopic通信以外を検討される方は、そちらをご参照ください。
本記事のプログラム
本記事で扱ったプログラムは、下記のGithubからもご覧いただけます。
References
・Github: hexonxons/ros2_service_test
Discussion