【2025年】作ったもの振り返り

今年作ったり買ったりしたもの(買ったけど積んであるもの)を振り返ってみます。 (以下の記事からの続きなのでぎりぎり2024年末のものも入っています)

kotakku-07.hatenablog.com

ジャイロコンパス

2024年の大晦日は自作のマルチIMU基板でジャイロコンパスを作ってました。

(大晦日に他にすることもなかったのかという話ですが、修論に追われている時期であんまり外に出る気もなかったので基本部屋に引きこもっていました)

もともと1万円程度でマルチIMU基板を自作していたものの「そういえばジャイロコンパスやったことないな」と思ってやってみたところ、本当に地球の自転の角速度が計測できてしまい、そこから方位を算出することができました。

理屈自体は高専生だった頃から分かっていたし他人に説明するのも簡単ですが、本当に実現できるんだと感動していました。

IMUの積分で位置を出してみる

そこまで真面目にやったわけではないですが気になったので自作マルチIMU基板で加速度を2回積分して位置を出してみました。 激しく動かさなければ10秒程度まで発散せずに耐えてました。

最近SPRESENSEの公式exampleに位置を計算するサンプルが追加されたらしいのでいつか比較してみたいです。

XROBOCON 倒立振子ロボット

今年の個人的な一番の話題なんですが、大阪万博で行われたXROBOCONという大会に参加してきました。この大会のルールは20cm、30cm、40cmの段差を登りフィールド上に存在するコインをより多く取ったチームの勝ちというものでした。もともと大会の存在は知っていましたが自分は最初は参加するつもりはありませんでした。

そんな中、大学ロボコンサークルの時の同期から「ただ登るだけでは面白くない、ジャンプして登るロボットを作ろう」という話を社会人になったばかりの自分に持ちかけられ、まんまと(ノリノリで)誘いに乗って参加したのが本大会でした。

結果は振るわなかったですが、練習ではしっかり40cmの段差もきれいにジャンプで登ることができていたので個人的には満足でした。 初めてジャンプが成功したときはメンバー全員ドーパミンがドバドバだったと思います。

Ethernet CAN変換基板修正

ずっとやるやる詐欺していたEthernet CAN変換基板の修正をXROBOCONに合わせて発注しました。 この基板もXROBOCONの開発中に使われました。

CasADiでFATROPを使ってみた

自作のMPCライブラリでFATROPを使えるようにしました。 IPOPTからソルバーを切り替えるだけで4〜5倍も高速になることもあり、最近はもうデフォルトでFATROPを使用するようになりました。

こちらももちろんXROBOCONで大活躍しました。これがなければ実時間でMPCを回すのは無理だったでしょうね。

CasADiのMPCライブラリ JIT/AOTコンパイル実装

FATROPを使えるようにしてかなりパフォーマンスが改善しましたが、そもそもCasADiのシンボリック変数の評価のオーバーヘッドが大きかったため、JITコンパイルを実装したところさらに3倍以上の高速化ができました。

JITコンパイルはコンパイル結果がキャッシュされるように設定しているので2回目以降はほぼノータイムで起動できますが、規模によっては1回目のコンパイル時間が馬鹿になりません。

なのでAOTコンパイルにも対応しました。これはJITコンパイルに相当するコンパイルをプロジェクト全体のコンパイル時にcmakeマクロでフックして同時にビルドしてしまいます。 追加のcppファイルとCMakeLists.txtの修正が必要になりますが、実行時のコンパイルなしに高パフォーマンスを得ることができます。

STM32N6 Lチカ

これはArm Heliumを使ってみたくてNucleoボードを買ってみたけどLチカで力尽きたままです...。Arm Heliumの面白さのみに惹かれて購入したものの適用するアプリケーションが思いつかず...。自分の知り合いで試食してみたい人いたら貸出可能です。

robstride 05 倒立振子

XROBOCONでRobStrideモーターを使って割と使いやすかったので一番小さい05を買って倒立振子を作ってみました。 高専時代に似たようなものを作って立たせられなかったのでリベンジ成功です。

ros2-for-unity jazzy&android対応

2023年にhumble版をandroid対応したのですがそろそろjazzy移行しそうなのでこちらも合わせて対応しました。 そもそもros2-for-unityのjazzy対応がまだだったのでUbuntu用のjazzy対応もしています。

ポストの画像に写っていますがOdin2というハンドヘルド機を新しく購入しました。 これで将来ロボットを作ったとき用のコントローラも確保できました。

最適経路のパフォーマンス改善

ほぼNHKロボコン専用ですが最適経路のソルバーをちまちま改善してました。 最近は大体の経路が40ms〜1sぐらいで引けるようになってきて、最適化の成功率も9割超えてきた気がします。 経路の複雑さに比例して最適化にかかる時間が伸びるとは限らないようでココらへんは深追いしていきたいところでもあります(obstacleの数かな？)

去年はまだ10秒ぐらい掛かったり成功率も6〜7割ぐらいだったのを考えるとだいぶ改善してきました。 最近は調子に乗って各年のロボコンの経路を引いて遊んでます。

もう参加することもない(と思っている)ロボコンでの運用を考えるとやはり理想的には安定して100ms以下ぐらいで解けるようにしたいです。 MPCと違って問題構造や運用面を考えるとJIT/AOTコンパイルがなかなかしづらい気がしていてまだできていないので、次に高速化するとしたらここですかね〜。

simple_casadi_mpcのpinocchio対応(WIP)

もともとsimple_casadi_mpcのissueに寄せられていたもので(当時対応できなくて申し訳ない)今になって自分も欲しくなって来たので現在実装途中です。

urdfを用意すればpinocchioが勝手に状態方程式を導出してくれるので、より簡単にMPCを定式化することができるようになりました。 まだ実験的な実装でmainにマージされるまでには時間がかかりそうです。

To Be Continued?

基本的にここに上げた個人開発は「ロボットを作りたい」というモチベーションから来ていて、現在はそこに繋がりそうな技術を集めている段階です。 来年、再来年のうちに何か一つ実機が完成することを目標に引き続き頑張っていきたいです。

ロボコンを引退してから趣味で作ったものを振り返ってみる

　久しぶりにはてなブログに記事を書きます。こたっくです。 自分は元々ロボコンに関係なく趣味でいろいろ作ったりしているのですが、学生生活最後の年末のこの機会に自分が作ってきたもの(と少しだけ遊んだもの)を振り返ってみようと思います。 振り返る期間はABUロボコン2022の大会後からとし、基本的にTwitter(現:X)での自分の呟きをもとに振り返っていきます。

2022年

jsk-visualizationのROS 2移植版の公開

　ROS1を使っていた人ならほとんどの人が一度は目にしたであろうjsk-visualizationは、rvizでいろんなトピックを可視化するのにとても便利です。 しかしROS 2には移植されていなかったので自分で移植しました。 元々一人でコツコツ移植してて、ABUロボコン期間中も活用していたのですが、ロボコンを引退したタイミングで公開しました。

　需要もあってかこのリポジトリが一番リアクションが多かった気がします。 実は作り始めたのは高専5年生のときで、robosemiで途中経過を発表していたりします。

cpp-roboticsの公開

　ロボコン中に書いたコードや趣味で勉強した制御工学に関するコードをまとめたライブラリです。 思いついたときに更新しているだけなのでかなり散らかってます。 実はABUロボコンのプログラムにも一部流用していたりします。

github.com

線形MPCの実装

Eigenのみ縛りで線形モデル予測制御を実装しました。 ロボコンではcasadiを用いてMPCを実装していたのですが、自分で実装してみることでかなり理解が深まりました。

SBDBTのSTM32版

　SBDBTの完全ピンコンパチなSTM32基板を作りました。 DualShock4とも簡単に繋がってくれます。 ネタ基板として思いつきで作ったわりにはかなり実用的でした。

STM32でファミコンエミュレータを作った

　APUは実装してないです。 12FPSしか出ませんでした(悲しい)

2023年

webotsに触れる

　ちょっとだけwebotsを触りました。普段使いするようにはならなかったです。 シミュレータってどれが良いんですかね？

ros2-for-unityのAndroid移植

　ロボコン現役の時にスマホコントローラを導入し、ロボットに搭載しているPC(ROS2)との通信はROS TCP Connector/Endpointを使っていましたが、すぐに通信が重くなってしまいいろいろとチューニングが必要でした。独自のTCP/IPパケットに変換するオーバーヘッドが効いてそうなのでUnityから直接DDSをしゃべらせたくなります。 ROS2とUnityを連携させるためにはもう一つros2-for-unityを使うという手段があり、これはUnityが直接DDSをしゃべってくれます。 しかしWindows/Ubuntuのみのサポートだったため、Androidようにビルドができるようにビルドスクリプトを書き、Android端末でも動作するようにしました。 これでUnityの豊富なUI作成用のコンポーネントを活用しつつROS2と快適な通信ができるようになりました。

qiita.com

github.com

VRFTを実装

　PIDゲインのデータドリブンなチューニング方法として知られるVRFTを実装してみました。 シミュレータでしか確認してないですが一発でゲインが求まるのは気持ちいいです。 いつか実機で確認してみたいですね。

simple_casadi_mpc

　このころはcasadiで遊ぶのにハマっていてcasadiでモデル予測制御をするためのライブラリを作りました。 ロボコン現役の時はここまできれいにコードがまとまらなかったのでかなり満足しています。 exampleもいくつか用意したのでよかったら動かしてみてください。

github.com

小型BLDC基板(設計だけ)

　まるた先生のツイートに触発されて設計だけしました。

　最近はODrive microという基板が発売されたのでいつか買ってみたいなと思っています。

odriverobotics.com

SwitchにUbuntu入れて遊んだ

　良い子はマネしないでね。

micro-ROSのC++ラッパ

　ArduinoやSTM32マイコンなどの組み込み環境で動作するmicro-ROSはrclcで書く必要がありなかなか面倒だったため、rclcppライクにかけるようにしたヘッダオンリーのライブラリです。 今見返すと何やってるかさっぱり覚えてないですが、マクロの黒魔術が展開されていました。 (今でも動くかな？)

github.com

FeetechシリアルサーボのユーティリティソフトのUbuntu移植

　Feetechのシリアルサーボは比較的入手性も性能もよくホビー用として遊ぶにはかなり使えると思います。 動作確認、パラメータチューニング、データロギングなどに使うユーティリティソフトが公式から配布されているのですがUbuntu上では動きません。 自分は普段使いがUbuntuで、このためだけにWindowsに切り替えるのも面倒なので、Ubuntu版を作りました。 本家のソフトと見比べながらできるだけ同じような見た目になるように調整するのはなかなか楽しかったです。 ビルド済みイメージも配布しているので使ってみたい方は下のGithubリンクのReleaseからダウンロードしてってください。

github.com

micro_behaviortree_cpp

　ロボコンでは自動制御の意思決定の部分をBehaviorTree.CPPというライブラリで行っていました。 小規模な自動制御を行うにはとても便利なライブラリなのでマイコン上で動いたら高専ロボコンなどでも使えるんじゃないかと思って依存関係を極限までなくして作りました。 STM32F4上で動作することを確認しています。 基本的な使い方は本家BehaviorTree.CPPとほとんど同じです。 ぜひ興味があれば使ってみてください。

github.com

2024年

AL-iLQR

　高速な軌道最適化手法のALTROに用いられるAL-iLQRを実装してみました。 分かりやすい動画などは無いのですが倒立振子を立たせる軌道を計算するのに自分のノートPCで14msぐらいでした。

FMT*とDWAでナビゲーション

　FMT*という経路計画法を実装したついでにDWAで追従までシミュレーションさせてみた動画です。 なかなか簡単かつ強力な方法ではないでしょうか。

OSQPに使われているADMMベースのQPソルバの実装

　二次計画法ソルバーとして知られるOSQPはADMMベースの手法でかなり性能が良いとされています。 もとの論文を読んでみると分かりますがかなり分かりやすく細かなところまで解説をされており、自分でも実装できそうと思ったので実装してみました。 cpp-roboticsで元々内点法ベースの別のQPソルバーは実装していたのですが、性能ではADMMベースの方法が圧勝でした。

ロボコン用軌道生成ライブラリ

　ABUロボコン中に使用していた軌道生成プログラムがかなり遅く、かつ成功率がそこそこ低かったので別の手法で実用的なものが作れないかと作ったものです。 4〜10秒くらいで解けるようにはなりましたが理想的には0.1秒ぐらいで解いてほしいです(だれか作って)

CADの勉強

　Fusionで見たことあるロボットっぽい形状を作ってurdfを作って遊んでました。 まだ表示しただけ

マルチIMU基板

　こちらもかなりリアクションを貰ったものです。 元々はSONYがSPRESENSE用の基板として作成しMaker Faireなどのイベントで展示していたものですが、発売が待てず自分で作ってみたものになります。 本家は1基板に16個のIMUが載っていますが自作基板は欲張って32個載せています。 アラン分散を計測してもかなり本家に近いデータが取れていて感動しました。 (これで何か作りたいけどまだ思いついてないです) 本家が発売されたらどの程度再現できているのか比較実験もしたいですね。

ガーバーデータとファームウェアを公開しているので作ってみたい方はJLCPCBで発注して動かしてみてください。

github.com

github.com

EthernetCAN変換基板

　ロボコンでROS(というかPC)を使うようになると「もう少しマイコンとPC簡単につながらないかな～」とよく考えるようになります。 通信レイヤーの開発とデバッグで時間を取られるのは無駄でしかなく、作りやすくて使いやすいやつ欲しいな～ってことでEthernetCAN変換基板を作ってみました。 1回作ってみて配線ミスと仕様変更したい箇所ができたので設計の修正まではしたんですが、お金の事情により発注の段階で止まっています。

FPGA入門

　Ariexpressで安いZynqボードを買って入門してみました。 VerilogでLチカして、PSからHello Worldして、HLSでモジュールを作ったところで自作のHLSのモジュールを入れるとプログラムが走らなくってしまい、デバッグ中に挫折して一旦止まっています。 あとで再チャレンジします。 (FPGAもっと安くならないですか)

React入門

　今までwebで使われている技術にほとんど触れてこなかったのでNode.jsやらReactやらに触れてみています。 コードで思った通りのUI作るの難しすぎませんか...？

今後作ってみたいものとか

　振り返ってみるとかなりソフトウェア寄りでしたね。社会人になったらハードウェアにも挑戦していきたいです。 いつかはロボットを一人で作ってみたいですね。

今の目標はDisney Researchの二足歩行ロボットです。 (ギリギリ部屋に置けそう)

www.youtube.com

連続な関数の最適化手法を紹介したい

この記事は TUT Advent Calender 2022 の7日目の記事です。

はじめに

はじめまして、豊橋技科大1系B4のこたっくです。ついこの前まで豊橋技科大のロボコンサークルであるとよはし☆ロボコンズの制御班に所属し、NHK学生ロボコン2022・ABU Robocon 2022に参加していました。

Twitter twitter.com

サイト tutrobo.rm.me.tut.ac.jp

またロボコンが終わってからはOUXTのメンバーとして船の速度制御系の開発を担当しています。OUXTはMaritime Robotx Challengeという完全自動運転な船を作って様々なタスクにチャレンジするロボコンに参加する団体で日本各地から学生・社会人いろんな人が集まって活動しています。

www.ouxt.jp

OUXTではROSを活用してロボットを動かしますがなんとコードは全て公開されています。「なんとなくROSは知ってるけど実際にコード書こうと思ったら分からない」「パッケージの設計どうしたらいいか分からない」みたいに思ったらOUXTのリポジトリを覗いてみると解決するかもしれません。自分も学生ロボコン中にめちゃくちゃ参考にさせていただきました。

github.com

豊橋の大学のアドベントカレンダーなので豊橋のことも軽く触れておきましょう。皆さん中華料理屋「美楽」はご存知でしょうか？ とよはし☆ロボコンズでは美楽の唐揚げ定食を食べきれると良いロボットが作れるとされています。詳しくは以下の記事で詳しく解説されています。ロボコンやってる人もやってない人も豊橋に来た際にはぜひ食べてみてください。

shouyuzukenotako.hatenablog.com

本題

最近はロボコンを言い訳に後回しにして遅れに遅れていた研究を巻き返すためにひーひー言ってる毎日です。無事に卒業できるといいですね(白目)
タスクに追われてぎりぎりで生きてると次こそは効率的にやろうとか思うんですけどなかなか上手くいかないもんですね。特に試行錯誤でいい感じにするタスクは時間を使うばかりで進捗効率は悪いことが多いです。

ということでやはりそういうタスクは最適化問題に落とし込んで効率的に行うべきです。

ということで今回は制約付き非線形最適化法である逐次二次最適化(SQP法)について紹介しようと思います。

二次計画法(QP法)

逐次二次最適化に触れる前にいくつかの前提となる手法について紹介します。 まずは二次計画法です。二次計画法とは以下の二次形式で表される問題を最適化する手法を指します。

\displaystyle \begin{align} \underset{x}{\text{min}} \space & \dfrac{1}{2}x^TQx + c^T x\\ \text{s.t.} \space & Ax = b\\ & A_{eq}x \leq b_{eq} \\ & x \in \mathbb{R}^n \end{align}\\ \\

制約がなければ最急降下法やニュートン法で解くことが出来ます。最急降下法やニュートン法は高専や技科大の授業でやった人も多いのではないでしょうか。

制約が在る最適化問題ではKKT条件というものを満たすような解を探します。

まずはスラック変数を導入して不等式制約を等式制約に置き換えます。

\displaystyle \begin{align} \underset{x}{\text{min}} \space & \dfrac{1}{2}x^TQx + c^T x\\ \text{s.t.} \space & Ax = b\\ & A_{eq}x + s = b_{eq} \\ & s > 0 \end{align}\\ \\

この式に問題におけるKKT条件は

\displaystyle \begin{eqnarray} Qx+c + A^T\lambda + A_{eq}\lambda_{eq} &=& 0 \\ \lambda_i s_i &=& \rho\\ Ax-b+s &=& 0\\ A_{eq}x-b_{eq} &=& 0\\ s &>& 0\\ \lambda &>& 0\\ \end{eqnarray}\\ \\

ここで各変数を微少量だけ変化させた場合を考えます

\displaystyle \begin{eqnarray} x &\leftarrow& x + \Delta x\\ s &\leftarrow& s + \Delta s\\ \lambda &\leftarrow& \lambda + \Delta \lambda\\ \lambda_{eq} &\leftarrow& \lambda_{eq} + \Delta \lambda_{eq}\\ \end{eqnarray}\\ \\

上式をKKT条件に代入して微小変化量についてまとめると以下のようになります。

\displaystyle \begin{bmatrix} Q & 0 & A^T & A_{eq}^T \\ A & I & 0 & 0\\ 0 & D_\lambda & D_s & 0 \\ A_eq & 0 & 0 & 0 \\ \end{bmatrix} \begin{bmatrix} \Delta x \\ \Delta s \\ \Delta \lambda \\ \Delta \lambda_{eq} \\ \end{bmatrix} = - \begin{bmatrix} Qx + c + A^T\lambda + A_{eq}^T \lambda_{eq}\\ Ax - b + s\\ -\rho I + \lambda \bigodot s \\ A_{eq} x - b_{eq}\\ \end{bmatrix}\\ \\

ここで

\displaystyle \begin{array} \\ \lambda \bigodot s &= \text{(アダマール積)}\\ D_\lambda &= \text{diag}(\lambda)\\ D_s &= \text{diag}(s)\\ \\ \end{array}

です。

この連立方程式を解いて各変数の更新量を求めて更新する作業を収束するまで繰り返すことで解を求めることが出来ます。

なにやら複雑な式に見えますが制約の数が0の時を考えると左辺は係数行列の左上の Qだけ残り右辺は一番上の成分の Qx + cが残る形になり、ニュートン法と一致することが分ります。ここでは制約を考慮しながら解くニュートン法ぐらいに捉えてもらって大丈夫です。

BFGS法

次にBFGS法について紹介します。

BFGS法とは準ニュートン法等で使用される目的関数のヘッシアンを近似する行列を求める手法です。 連続関数の最適化問題が収束するためには目的関数のヘッシアンが半正定値である必要がありますが、任意の非線形な目的関数のヘッシアンが半正定値である保証はないのでヘッシアンを半正定値の行列で近似してやります。

以下の更新式を使ってイテレーションごとに近似ヘッシアンを更新します

\displaystyle B_{k+1} = B_{k} - \dfrac{B_k s_s \left( B_k s_k \right)^\top}{ (s_k)^\top B_k s_k } + \dfrac{y_k (y_k)^\top}{(s_k)^\top y_k}\\ \\

近似ヘッシアンの初期値としては単位行列などが使われます。

また実際にはSQP法では {(s_k)^\top y_k} > 0を満たすとは限らないためパウエルの修正BFGS公式が使われます。

逐次二次最適化(SQP法)

ようやくですが本題の逐次二次最適化について紹介します。 逐次二次最適化とは以下のような形式の問題を最適化する手法の一つです。 目的関数や制約関数が非線形の問題に対して使用することが出来ます。

\displaystyle \begin{align} \underset{x}{\text{min}} \space & f(x)\\ \text{s.t.} \space & g_i(x) = 0, \space i = 0, \dots , m_e\\ & g_i(x) \leq 0, \space i = m_e+1, \dots , l\\ & x \in \mathbb{R}^n\\ \\ \end{align}

「逐次」で「二次最適化」な名前から分かるようにこの最適化手法は任意の最適化問題を最適化変数 x周りで二次近似して問題を解き最適化変数を更新する手順を解が収束するまで繰り返し行います。

SQP法の手順はおおまかに以下になります。

  1. 最適化変数 x、近似ヘッシアン Bの初期値を決定する
  2. 現在の最適化変数 x周りで目的関数と制約関数のヤコビアン、制約関数を評価する。
  3. サブ問題であるQP問題を解き探索方向を決定する。
  4. メリット関数の直線探索でステップ幅を計算する
  5. xを更新する
  6. メリット関数の制約重みを更新する
  7. BFGS法で Bを更新する
  8. 収束していなければ2.に戻る

ここで一番大事なのは3. のサブ問題を解いて探索方向を決定することです。 二次形式に近似して二次計画法で解くことを考えるとサブ問題であるQPは以下のように書くことが出来ます。

\displaystyle \begin{align} \underset{x}{\text{min}} \space & \dfrac{1}{2}d^TBd + \nabla f(x)^T x\\ \text{s.t.} \space & \nabla g_i(x)^Td + g_i(x) = 0, \space i = 0, \dots , m_e\\ & \nabla g_i(x)^Td + g_i(x) \leq 0, \space i = m_e+1, \dots , l\\ \\ \end{align}

この問題を解くと探索方向 dが求まるのでステップ幅を直線探索で求めて xを更新します。
この作業を収束するまで繰り返します。

ここではサブ問題をニュートン法ベースの二次計画法で解く方法を紹介しましたが、修正コレスキー分解と最小二乗法によって解くものをSLSQP(Sequential Least SQuares Programming)と言います。 SLSQPは論文とFORTRANのルーチンが公開された後、PythonのScipyやCのnloptにも移植されています。

気になる方は以下のタイトルで論文を調べてみてください

Kraft, D. A software package for sequential quadratic programming. 1988. Tech. Rep. DFVLR-FB 88-28, DLR German Aerospace Center -- Institute for Flight Mechanics, Koln, Germany.

例題

実際にSQP法を使ってみましょう。

以下の個人開発しているライブラリの中にSQP法を実装してあります。 github.com

cpp_roboticsは自分が実装したいものを実装する趣味用のC++ライブラリです。

問題設定

以下の問題を最適化してみましょう。

\displaystyle \begin{align} \underset{x}{\text{min}} \space & 100 (x_2-x_1^2)^2 + (1 - x_1)^2 \\ \text{s.t.} \space & x_1^2 + x_2^2 \leq 1 \\ & x_1 \geq 0 \\ \\ \end{align}

目的関数はグラフ化してみるとこんな感じの関数です。

図は下記より引用

qiita.com

制約なしの時の最適解は (x_1, x_2) = (\pm 1, 1) です。

制約の1つ目は解が原点から半径1の円の中に収まっていること、2つ目が2つある最適解のうち正の方に収束することを制約にしています。

コード

#define CR_USE_MATPLOTLIB
#include <iostream>
#include <cpp_robotics/optimize/optimize.hpp>
#include "cpp_robotics/third_party/matplotlib-cpp/matplotlibcpp.h"

int main()
{
    using namespace cpp_robotics;
    SQP solver;
    SQP::Problem prob;

    //////////////////// 問題設定 ////////////////////
    // 目的関数
    prob.func = [](Eigen::VectorXd x) -> double
    {
        return 100*( std::pow( (x(1) - std::pow(x(0),2)), 2) ) + std::pow(1 - x(1), 2);
    };
    
    // 制約1
    prob.con.push_back({
        Constraint::Ineq,
        [](Eigen::VectorXd x)
        {
            // |x| <= 1
            return (x(0)*x(0) + x(1)*x(1)) - 1;
        },
    });

    // 制約2
    prob.con.push_back({
        Constraint::Ineq,
        [](Eigen::VectorXd x)
        {
            // x1 >= 0
            return -x(0);
        },
    });

    // 解の初期値
    Eigen::VectorXd x0(2);
    x0 << 0, 0;

    //////////////////// 解く ////////////////////
    // 反復によって生成される点列の可視化用
    std::vector<double> x1_, x2_;

    // 解く
    auto result = solver.solve(prob, x0, [&](auto x){ x1_.push_back(x(0)); x2_.push_back(x(1)); });

    //////////////////// 結果表示 ////////////////////
    std::cout << "iter: " << result.iter_cnt << std::endl;
    std::cout << "x1 = " << result.x(0) << std::endl;
    std::cout << "x2 = " << result.x(1) << std::endl;
    std::cout << "x norm: " << result.x.norm() << std::endl;
    
    // 点列のグラフ化
    namespace plt = matplotlibcpp;
    plt::plot(x1_, x2_, "o--");
    // 補助線
    plt::plot({1,-1}, {1,1}, "xk"); // 最適解
    std::vector<double> edge_x(100), edge_y(100);
    for(size_t i = 0; i < 100; i++) // 制約によるxの取る範囲の境界
    {
        double theta = i/99.0 * 2 * 3.141592;
        edge_x[i] = std::max(0.0, std::cos(theta));
        edge_y[i] = std::sin(theta);
    }
    plt::plot(edge_x, edge_y, "--");

    plt::xlim(-1.1, 1.1);
    plt::ylim(-1.1, 1.1);
    plt::set_aspect(1.0);
    plt::show();
}

実行結果

実行するとこんな感じで解とグラフが表示されます。グラフは反復による解の収束を見ることが出来ます。

ターミナルの表示

iter: 19
x1 = 0.785066
x2 = 0.618036
x norm: 0.999149

青が反復によって生成された点列、オレンジが制約による実行可能領域の境界、黒のバツが制約なしの時の最適解です。 原点からスタートして最適解に近づきつつも制約を満たすところで止まってるのが分ります。実際にターミナルの出力を確認すると最適化した変数のノルムがほぼ1になっています。

最後に

今回は最適化手法の紹介と自前実装のソルバーを使って実際に最適化問題を解いてみました。 整数の最適化問題とか整数混合問題とかも勉強してみたいですね。

記事にミスがあった場合はコメントしてもらえるとありがたいです。

最後に宣伝ですが来年は学生ロボコン2023で後輩達がかっこいいロボット作って優勝してくれると思うので応援よろしくおねがいします！

Twitter twitter.com

サイト tutrobo.rm.me.tut.ac.jp