manvaのエンジニアリング魂

エンジニアリング・ものづくり・DIYをもっと身近にするためのブログ。インスピレーションを刺激します。

一番わかりやすい座標変換の説明

ロボット制御 数学

ロボット工学で座標変換をよく使うが、結構混乱しがちだ。
例えば、

  • 同次変換行列はなぜ右からかけるのか
  • 絶対座標系の回転と相対座標系の回転はなぜかける順番が逆になるのか

など、答えは「知っている」ものの、なぜそうなるのか、きちんと理解して説明できる人は意外に少ないのではないだろうか。教科書を見返してみても、結構ややこしい。私が一番わかりやすく説明できる気がするので書いてみる。

簡単のため、２次元の回転で説明するが、３次元の回転や、並進を含めた「同次変換」でも同様である。
まず、回転行列の使い方が複数あるので整理しておく。

回転行列の使い方①：ベクトルの回転

最も基本的な使い方は、ある座標系\Sigmaの中での、ベクトルxの回転である。

回転行列によるベクトルの回転

角度\thetaだけ回転させる回転行列R(\theta)を用いて、回転後のベクトルx'は次式で求められる。

x'=R(\theta)x
R(\theta)=\begin{bmatrix} \cos\theta & -\sin\theta \\ \sin\theta & \cos\theta \\ \end{bmatrix}
この回転行列R(\theta)は、左からかけるとベクトルが\thetaだけ回転するように作った行列であることを覚えておく。絶対座標系で回転させるには、回転行列を左からかける。

回転行列の使い方②：座標系自体を表す

座標系の軸をベクトルと考えれば、座標系も同じ使い方で回転させることができる。

回転行列による座標系の回転

座標系\Sigmaを、X軸、Y軸の向きを表す単位ベクトルを並べた行列で

と表すと、回転後の座標系\Sigma'は
\Sigma'=R(\theta)\Sigma
で求められ、回転後のX軸、Y軸の向きを表す単位ベクトルを並べた行列として表される。\Sigmaは単位行列なので、回転行列R(\theta)自体が、回転後の座標系の座標軸の向きを表していると見ることができる。
(３次元の場合も同様に、回転行列は回転後のX軸、Y軸、Z軸の向きを表す単位ベクトルを並べた行列である)。

回転行列の使い方③：ベクトルの座標変換

もう一つの使い方は、座標変換である。ある座標系\Sigmaで、Xで表されるベクトルがあるとする。座標系\Sigmaのみを回転行列R(\theta)で角度\thetaだけ回転させると、回転後の座標系\Sigma'ではベクトルXが-\thetaだけ回転したベクトルX'の位置に見える。

したがって、回転「後」の座標系\Sigma'で表したベクトルX'を回転「前」の座標系で表したベクトルXに変換するために回転行列R(\theta)が使える。

回転行列による座標変換

つまり、

X=R(\theta)X'
である。

座標系の変換

\Sigma_0、\Sigma_1、\Sigma_2の３つの座標系がある場合に、上記③の使い方の応用で、「座標系」を座標変換する。座標系\Sigma_1は、絶対座標系\Sigma_0を回転行列 {}_0R_1で回転させたものであるとする。

\Sigma_1={}_0R_1\Sigma_0
上記③と同様に、回転「後」の座標系\Sigma_1から見た\Sigma_2(これを\Sigma_2'とする)を、回転「前」の座標系\Sigma_0から見た\Sigma_2に変換するために上記の回転行列 {}_0R_1 が使える。
つまり、
\Sigma_2={}_0R_1\Sigma_2'\tag{1}
である。

絶対座標系\Sigma_0は単位行列であり、上記使い方②のように、それぞれの座標系自体が回転行列で表せる。

\Sigma_1={}_0R_1\Sigma_0={}_0R_1
\Sigma_2={}_0R_2\Sigma_0={}_0R_2\tag{2}
また、\Sigma_2'は\Sigma_1に対する相対的な回転で表せる。これを {}_1R_2とすると
\Sigma_2'={}_1R_2\tag{3}
である。

したがって、(1)(2)(3)式より

{}_0R_2={}_0R_1 \; {}_1R_2
となる。相対座標での回転を表す回転行列 {}_1R_2 を右からかけると合成した回転行列が求められることがわかる。
また、これらの回転行列が左からかけると絶対座標で回転させるように作られた行列であることを思い出すと、絶対座標系で {}_1R_2 だけ回転させた後に絶対座標系で {}_0R_1 だけ回転させることと、絶対座標系で {}_0R_1 だけ回転させた後に、相対座標系で(つまり回転後の軸回りに) {}_1R_2 だけ回転させることは同じ結果になることがわかる。(２次元だと回転軸が１つしかないので意識しなくて良いのだが、３次元の場合は、どの座標系のどの軸まわりに回転させたかを考える必要がある。回転行列 {}_1R_2は \Sigma_1 座標系の軸まわりの回転を表している)。
座標系の数がもっと増えても同様である。

ロボット工学では、ロボットアームの各リンクとともに動く「リンク座標系」を定義する。リンク座標系で表した位置が、絶対座標系でどのように表されるか、を求めたいことが多いが、このときに座標系を相対回転させる行列をそのまま順に右からかけていくのは上記のような理屈による。

Jetson NanoにFlaxをインストールする方法

Jetson Flax

前の記事↓
manva.hatenablog.com
で、Jetson Nanoに JAXをインストールし、次はFlaxをインストールできたのでやり方を書いておく。

Flaxとは、JAXベースのニューラルネット(NN)ライブラリである。知らんけど。NNライブラリと言えば、TensorFlowとPyTorchがメジャーだが、JAX/Flaxは、また新しい候補として伸びてきている(あまりいろいろ増えないで欲しい)。

公式のドキュメント↓を参考に
Flax documentation — Flax documentation

$ pip install flax

と打つと、↓のように，Bazelがない、というエラー。

error: command 'bazel' failed: No such file or directory


Bazelとは、makeに代わるビルドツールで、多数のプログラミング言語やOSに対応した物らしい。知らんけど。
そのBazelをインストールしないといけないのだが、公式↓
Bazel - a fast, scalable, multi-language and extensible build system" - Bazel
の通りインストールしようとしても，これがまたなかなかすんなりいかない。
↓の記事
【Jetson Nano】TensorFlow Probabilityをインストール - Qiita
の最初の部分で、Jetson NanoにBazelをインストールする方法を書いてくれており、そのやり方でできた。

手順

１．curlをインストール
$ sudo apt install curl
２．Java JDK8をインストール
$ sudo apt install openjdk-8-jdk
３．Bazelをインストール
$ git clone https://github.com/PINTO0309/Bazel_bin.git
$ cd Bazel_bin/2.0.0/Ubuntu1804_JetsonNano_aarch64/openjdk-8-jdk/
$ ./install.sh

Katsuya Hyodo氏のリポジトリからビルド済バイナリをクローンして使っている。記事を書かれた方と併せて感謝する。

Bazelをインストールできたので、

４．Flaxをインストール
$ pip install flax

でできた。

Jetson NanoにJAXをインストールする方法

Jetson JAX

Jetson Nano 2GB版↓にJAXをインストールできたのでやり方を書いておく。
www.switch-science.com


JAXとは、GPU対応のNumpyのようなものらしい。知らんけど。なぜJetson NanoやJAXを始めたのかは後日別途語りたい。

まずはJAXの公式サイト↓
github.com
の手順で普通にインストールしようとしたが、この手順はCUDA10+cuDNN7かCUDA11+cuDNN8の組み合わせにしか対応しておらず、Jetson NanoのデフォルトのSDカードイメージでは，CUDAはバージョン10.2、cuDNNはバージョン8であるため、うまく行かなかった。それ以外の組み合わせはソースからのビルドが必要になる。
調べてみると、ソースからビルドすることで、Jetson Nano 4GB版でインストールできたという人がいて、やり方を書いてくれている↓(以下、これのことを「インストラクション★」と記載する)。
https://forums.developer.nvidia.com/t/jax-on-jetson-nano/182593/9
Jetson NanoのデフォルトのSDカードイメージでは、Pythonのバージョンが3.6.9なのだが、JAXをビルドするにはバージョン3.9に上げる必要がある。しかし、デフォルトのpython3をバージョン3.9に置き換えてしまうと他の多くのソフトの依存関係が壊れてしまうようだ。そのため、virtualenvというのを使ってデフォルトのPythonバージョンを3.6.9のまま変えないで、必要なときだけPython3.9を使えるようにインストールしている。
この情報のおかげで非常に助かったが、その通りにやってもまだうまくいかない。ビルドの途中で，以下のようなエラーが出て失敗する。

jaxlib/cuda_lu_pivot_kernels.cu.cc failed: (Exit 1): crosstool_wrapper_driver_is_not_gcc failed: error executing command 
nvcc fatal   : Unsupported gpu architecture 'compute_80'
Target //build:build_wheel failed to build
$ nvcc --help

と打って確認してみると，--gpu-architectureオプションの説明のところに'compute_80'がない。'compute_75'まで。'compute_80' はCUDAのバージョンが11以降でないと使えないらしい。たぶん，インストラクション★が書かれた後でJAXがバージョンアップしたためにこのエラーが出るようになったと思われる。CUDAのバージョンを上げるとまたいろいろ問題発生しそうなので、JAXの方を古いバージョンに落として使うことにした。
Jetson Nanoを公式の手順、または↓の本の手順で立ち上げたところからの手順を以下にまとめる。

Jetson Nano 超入門 改訂第2版

Jetson Nano 超入門 改訂第2版

Amazon
(以下、この本を「超入門」と記載する)

インストラクション★では、USB3.0のポートにSSDをつないで使っているが、SDカード64GB以上あればそれだけでできそう。

手順

１．スワップ領域追加

インストラクション★は，Jetson Nanoのメモリ4GB版用に書かれている。Jetson Nano 4GB版ではデフォルトでスワップ領域が設定されておらず，スワップ領域は5GBでも足りないくらいで10GBにしたとのこと。Jetson Nano 2GB版ではデフォルトでスワップ領域が5GBあるが，10GBに増やしておく。
まずはデフォルトのスワップ領域を一旦削除。

$ sudo swapoff /swapfile

スワップ領域の追加は「超入門」の通りにした。

$ sudo dd if=/dev/zero of=/var/swapfile bs=1G count=10
$ sudo mkswap /var/swapfile
$ sudo chmod 600 /var/swapfile

/etc/fstabというファイルを修正する。

$ sudo gedit /etc/fstab

と打ってgeditで開く(geditはsudoで使うと終了時に警告が出るが別に問題なさそう)と、最後の方に

# <file system> <mount point>             <type>          <options>                               <dump> <pass>
/dev/root            /                     ext4           defaults                                     0 1
/swapfile            swap                  swap           defaults                                     0 0

とある。最後の行の/swapfileを/var/swapfileに書き換えて保存。

# <file system> <mount point>             <type>          <options>                               <dump> <pass>
/dev/root            /                     ext4           defaults                                     0 1
/var/swapfile        swap                  swap           defaults                                     0 0

↓のように打つと

$ sudo swapon /var/swapfile

スワップ領域が確保される。
正しく設定できているかどうかは、

$ free -m

と打つと確認できる。

２．.bashrcにパスを追加
$ gedit ~/.bashrc

で開いたファイルの最後に↓追加。

export PATH=/usr/local/cuda/bin${PATH:+:${PATH}}
export LD_LIBRARY_PATH=/usr/local/cuda/lib64${LD_LIBRARY_PATH:+:${LD_LIBRARY_PATH}}

保存して一旦再起動。

$ export

と打つと正しく設定できたか確認できる。

３．Python3.9をインストール
$ sudo add-apt-repository ppa:deadsnakes/ppa 
$ sudo apt update
$ sudo apt install python3.9 python3.9-dev

ここで、インストラクション★にないがもう１つ追加で以下を実行する必要がある。

$ sudo apt install python3.9-distutils

ここまでで、Python3.9はインストールされるが、python3と打った場合には今まで通りPython3.6.9が使われる状態になる。

$ python3 -V

と打ってみると、3.6.9になっているはず。

４．仮想環境virtualenvをインストール
$ sudo apt install python3-pip
$ sudo -H pip3 install virtualenv
$ virtualenv -p /usr/bin/python3.9 py39

この後、↓のように打つと、プロンプトの先頭に(py39)と表示され、Python3.9が使われる状態になる。

$ source ~/py39/bin/activate

解除したいときは

$ deactivate

と打つ。

$ python3 -V

と打ってみると、3.9.9になっているはず。

この仮想環境は何もインストールされていない状態なので、必要なものをインストールする。

$ python3 -m pip install numpy scipy six wheel
$ sudo apt install g++

　

５．JAXをgitクローン

インストラクション★の通りまず↓を実行した(が、いらなかった)。

$ git clone https://github.com/google/jax

これだと現時点(2022/1/6)での最新バージョン0.2.26を使うことになるが、上記のようにうまく行かなかったため、旧バージョンのJAX0.2.20を使ったらできた。↓からJAX0.2.20をダウンロード、解凍してgitクローンしたjaxフォルダを削除して置き換えた。
Releases · google/jax · GitHub

私はこのやり方で旧バージョンにしたが、だったらたぶん最初から↓でできたと思う(未確認)。

$ git clone https://github.com/google/jax -b jax-v0.2.20 --depth 1

また、このバージョンは適当に選んだもので他は試していない。試していないのでわからないが、もう少し新しいバージョンでもできるかもしれない。


後はビルドするただけなのだが、これが非常に時間がかかる。最初ビルドしたとき、丸2日経っても終わらず、いつまで続くかもわからなかったので強制終了して高速化のために効きそうな対策をいくつか施した。どれが効いたかわからないが、半分以下の時間でできるようになった。

６． 高速化のための対策
  • 電源アダプタを5V2Aのものから5V3Aのものに変える　　時々、System throttled due to overcurrentなどと出ていたので。
  • モニタ、キーボード、マウスをつないでいたが、全部外して、Wifiから小さめの解像度でリモートデスクトップ接続する　　メモリを節約できると思って。
  • 「超入門」に書かれていたパフォーマンス最大化方策
$ sudo nvpmodel -m 0
$ sudo jetson_clocks

　

７． ビルド
$ cd jax
$ python3 build/build.py --enable_cuda

インストラクション★では12時間かかったそうだが、私の場合19時間くらいだった。メモリ4GBと2GBの差か。SDカードよりUSB3.0のSSDの方が速い、というのもあるかもしれない。
途中、分数(の形をした何かの数字)で進捗が表示されるが、分母も大きくなるのでいつまで続くかわからない。途中いくつかメモした。3425/3941→7420/8065→9212/9632→10394/10618といった感じ。参考までに、私の場合、分母は10618が最後だった。最初は速いが9000ぐらいから非常に遅くなる。

終わったら、最後に以下実行。

$ pip3 install dist/*.whl
$ pip3 install -e .

(最後の . (ドット)を見落とさないように注意)
これでできるはず。

ODriveで低価格サーボ

ODrive サーボ制御

大きさは正義(力こそパワー)であるが，個人でロボットを作ろうとする場合や，企業等で個人向けにロボットを作って販売しようとする場合，たいていは小さいものしかできず，(オタクではない)一般人には，おもちゃ扱いされがちである。

なぜ小さいものになってしまうかといえば，大抵は資金の問題が大きい。例えば手足6軸ずつのヒューマノイドを作ろうと思えば，それだけで24軸必要なので，だいたい1軸1万円くらいが個人で出せる限界であろう。そのため個人のロボットでは1軸1万円くらいのラジコンサーボがよく使われる。大きなラジコンサーボもあるが，それだと1軸3万円くらいになる。大きいロボットを個人で作ったり買ったりするのは資金的にきつい。

ところが数年前くらいから，ドローン用のブラシレスDCモータを使って比較的低価格で大きめのロボットを実現している事例が多く見られるようになってきた。

例えば ↓ UCBerkeley  Blue

新しい汎用型協働ロボット「Blue」 値段は破格の5000ドル | Forbes JAPAN（フォーブス ジャパン）

↓Unitree Go1 air

中国製の犬型ロボがもっと安価に。｢Go1 air｣なら30万円でお釣りが来る | ギズモード・ジャパン

 

というような状況から，特に何が作りたいわけでもないのだが，大きめのロボット等をいつでも作れるようしておきたくなり，1軸1万円程度を目安に，ハイパワーのサーボを作れる方策を探ってみることにした。こういうことやってるうちに何かやりたいことが見つかるのを期待している。

モータドライバを選ぶ

調べてみると，ブラシレスDCモータのドライバとして，速度制御だけで良ければラジコン用のESC (Electric Speed Controller)というのが安い。

もう少し深入りして，トルク(電流)のレベルで制御をしたい場合等は，オープンソースで中のソフトまでいじれるODriveやMoteusというのが人気がありそうだった。まずは↓のサイト(以下「参考サイト※」とする)を参考にODriveで一式揃えてみることにした。

BLDC制御実験キットの製作 - Qiita

モータドライバ　ODrive v3.6（の模造品）↓を購入。AliExpressで9194円ja.aliexpress.com

到着したものを見てみると，基板が緑色(本家は黒)なので模造品らしい。本家の写真を使って堂々と販売しているので購入時に模造品だと気づかなかった。エンコーダのコネクタに本家ではあるはずの3.3Vピンがなかったり，細かいところが本家とは違う。本家に還元する意味でも，本家のサイト↓から買うべきだった。

odriverobotics.com

モータを選ぶ

ロボット用途で考えると，一番気になるのはトルクなのだが，ドローン用のBLDCモータの仕様ではトルクが書いていない事が多い。以下の式で計算できるようだ。

BLDCモータの最大トルク計算方法

最大トルク[Nm] = 8.27 / KV値[min^{-1}/V] * 最大電流[A]

8.27という数字は↓の値。
\displaystyle\frac{3}{2\sqrt{3}}\cdot\frac{60}{2\pi}=8.27

ODriveの公式サイトでモータの表がある。最もトルクが大きいのはホバーボード(取手のないセグウェイ)用の物であるが、具体的な型式ではなくジャンルとしてひとまとめにされている。その他で最もトルクが大きいのは「9235-100KV Turnigy Multistar」で4.7Nmとある。マブチの540が最大0.2Nmくらいなのでこれでも結構なトルクである。参考サイト※で選ばれたモータの型式はX8318Sとなっているが，画像等から判断して，これもどうやら9235と同じものらしい。これを選ぼうと思ったが、既に製造中止っぽい。AliExpressで検索すると、見た目やスペックが同じ、たぶん模造品がいろんな会社から出てる。

モータ 8318（の模造品の中古）AliExpressで6320円のもの↓を購入。(価格は変動するし，無くなることも多いが，多くの業者が出品しているので検索すればまだしばらくは出てくるだろう。)

ja.aliexpress.com

↓到着。

f:id:manva:20211108064506j:image

「second hand」(中古)とは書かれていたが、想像以上にボロい。あと，線，短すぎ！

エンコーダを選ぶ

磁気式エンコーダ評価基板 AS5047P-TS_EK_AB（参考サイト※と同じもの）にする。ODrive公式サイトの対応エンコーダの表にも挙がっている。SPI通信で使えば14bit，AB相パルスで使うなら4逓倍で4000パルス/rev。表では，「Supported in SPI AMS mode」と書かれており，Quadrature(AB相パルスのこと)が「ｎ」となっているがAB相パルスでもできた（分解能は落ちるが）。

マルツで2316円（Digi-Keyで2000円だが送料も2000円なのでこちらの方が安い)。

www.marutsu.co.jp

いざ使おうとすると，ICのすぐ横にある巨大なジャンパピンのタワーがめちゃ邪魔。磁石をICの0.5～3mmの距離に配置しなければいけないのに，どうやって使う想定なんだろうか。ハンダを溶かして引き抜こうとしたが結構固く，無理やり引き抜いたらスルーホールも取れてしまった。ニッパで切断したほうが良さそう。

3Dプリンタで作成

支持部品をFusion360で設計し，３DプリンタX-Makerで作成した。

↓　磁気式エンコーダのマグネットホルダ

f:id:manva:20211106103320p:plainf:id:manva:20211106122602p:plain

↓　モータとエンコーダ基板の支持フレーム

f:id:manva:20211106103353p:plain

 

配線。組み立てた様子↓。エンコーダは，まずは参考サイト※のように，AB相パルスで読み取るようにした。セリアのすのこを使うというところまで真似させてもらった。電源は手持ちの第一電波工業の安定化電源を使った。

f:id:manva:20211107132544j:image

動作確認

ODrive公式サイトのGetting Startedの通りにやってみる。

Getting Started | ODrive

USBでODriveをパソコンに繋ぎ，電源を入れたら認識された。
Anacondaはもう入れていたので，WindowsスタートメニューからAnaconda Prompt (Anaconda3)起動
> pip install --upgrade odrive
> odrivetool

→エラー「Coud not open USB device」。
TroubleshootingページのUSB Connectivity Issuesに従って，zadig-2.6.exeをダウンロード，Odriveを起動して，Options - List All DevicesにチェックしてリストからOdriveを選択（ODrive 3.x CDC Interfaceではなく，ODrive 3.x Native Interfaceの方）。WinUSB(v10‥が入っていたが，WinUSB(v6‥にダウングレード(それしか選べないので)
→できた！
> odrv0.vbus_voltage
→15.5V。正しそう。
> odrv0.axis0.motor.config.calibration_current　→10A
> odrv0.axis0.controller.config.vel_limit　→2 [turn/s].
ちょうどよいのでそのまま。
odrv0.axis0.motor.config.pole_pairs　→7　磁石の数数えたら40個。/2で20にする。
> odrv0.axis0.motor.config.pole_pairs=20
odrv0.axis0.motor.config.torque_constant　→0.04   8.27/モータKVにする。KV=100なので，
> odrv0.axis0.motor.config.torque_constant = 0.0827
> odrv0.axis0.motor.config.motor_type=MOTOR_TYPE_GIMBAL
公式のDocのDeepL翻訳「ジンバルモーターを使用する場合、電流フィードバックを使用しないため、current_limとcalibration_currentは実際には「電圧制限」と「校正電圧」を意味します。つまり、10に設定した場合、パラメータ名とは裏腹に10Vを意味します。」
電流制限10Aじゃなく電圧制限10Vになっているらしい。まあいいか。

> odrv0.axis0.encoder.config.cpr = 4000
> odrv0.axis0.requested_state = AXIS_STATE_FULL_CALIBRATION_SEQUENCE
→一瞬動いたがダメ。

> dump_errors(odrv0)
でエラー確認できる。→ MOTOR_ERROR_MODULATION_MAGNITUDE

> odrv0.axis0.motor.config.calibration_current=1
にしたらそれっぽく動いた！しかし今度はエンコーダのエラー
→ENCODER_ERROR_NO_RESPONSE
上記のようにジャンパピン抜くときにスルーホール壊したせいか。もう一つ買っておいたエンコーダ基板に交換。ジャンパは抜くのではなく切断し、ハンダ付けでショートさせる。→Ok!
> odrv0.axis0.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL
としてほんの少し触ったら暴走。発散していたのは速度ループのようで，速度ゲインを下げたらOk。デフォルト0.166だが，0.1だとブルブル振動する。0.05くらいの方がマシになる。
> odrv0.axis0.controller.config.vel_gain=0.05

止め方がわからん。
> odrv0.quit()

でodrivetoolごと終了。

 

まあまあ苦労したが動くようになった。産業用のロボットでは，バックラッシや剛性等，減速機がネックになる場合が多いので，ハイトルクのモータで低減速比にできるのがメリットになると期待したが，それ以前にモータのコギングがひどいので，指令通りのトルクが出てなさそう。少なくとも現時点では，正直，産業用サーボモータと比較できるレベルではなかった。エンコーダをSPI通信にして分解能を上げ，パラメータを調整して，リップル補正とかして，どこまで改善できるか。

 

スモールゲイン定理の必要性を完全に理解した

制御工学

前に書いた記事↓で，
manva.hatenablog.com

スモールゲイン定理(ロバスト制御の話で出てくるバージョン)で，\|M\|_\infty<1が必要条件でもあることについて，自分なりの結論を出していたが，まだちゃんと理解できたという確信がなかったので，しつこく調べていた。

スモールゲイン定理
f:id:manva:20210816132754p:plain
上図で，\DeltaとMは安定でプロパな伝達関数とする。このとき，\|\Delta\|_\infty\leq1をみたす全ての\Deltaに対して，図の閉ループ系が内部安定となるための必要十分条件は\|M\|_\infty<1であること。

解釈の仕方の結論↓

結論

スモールゲイン定理は，\|\Delta\|_\infty\leq1の「全ての」\Deltaに対して内部安定であるための条件なので，\Deltaの位相遅れがどうなっているかはわからない。したがって，全ての周波数で開ループの位相遅れが-180(+360*n) deg になり得る。どの周波数で-180degになっても安定であるためには\|M\|_\infty<1であることが「必要」である。

調べてみて，やはりその解釈で正しいという確信が強まったので根拠を説明する。
確信がなかったのは，Zhou氏らの著書↓の証明を読んでも難しくて理解できなかったためである。
Robust and Optimal Control　9.2節
Essentials of Robust Control　8.2節
上記の結論とZhou氏の証明の関係が見えない。ここを理解したい。

いろいろ文献を探しても，大抵どれも，スモールゲイン定理の必要性(安定であるために\|M\|_\infty<1が必要であること)の証明は「省略する」とか，「(Zhou氏の著書)を参照」としていて，なかなか説明してくれているものがなかった。のだが，井村氏の著書↓にわかりやすい説明があった。

システム制御のための安定論 (システム制御工学シリーズ)

システム制御のための安定論 (システム制御工学シリーズ)

Amazon

Zhou氏の著書の証明と同様の話を，１入出力の場合で説明してくれているのでありがたい。詳細な条件や数学的に正確な話はこの本を見てもらうとして無視して，以下では理解するためのメイン部分のみ私なりに説明する。

スモールゲイン定理の必要性(安定であるために\|M\|_\infty<1が必要であること)を証明するには，対偶をとって，

\|M\|_\infty\geq1であるとすると，\|\Delta\|_\infty\leq1であってもループが不安定になるような\Deltaが存在する

ということを示せば良い。井村氏の著書では，１入出力の場合に限定して，

ある周波数\omega_0のときに|M(j\omega_0)|\geq1であるとすると，|\Delta(j\omega)|\leq1 \quad\forall\omegaであっても1-M(j\omega_0)\Delta(j\omega_0)=0になるような\Deltaが存在する

ということを示している。

1-M(j\omega_0)\Delta(j\omega_0)=0のときにループが不安定になる，というのは「ナイキストの安定定理より明らか」と書かれているが，私レベルにとってはそんなに「明らか」ではないので補足する。
（※ただし，教科書で1+M(j\omega_0)\Delta(j\omega_0)=0と書かれていたところを，たぶんミスと判断したため，勝手に1-M(j\omega_0)\Delta(j\omega_0)=0に書き換えている。ナイキストの安定定理は、普通，開ループをマイナスでフィードバックした場合で考えるので、直接プラスでフィードバックしている上記定義の図だと、不安定であることを言うには、1-M(j\omega_0)\Delta(j\omega_0)=0が言えないといけないはず。Zhou氏の著書の証明では，この条件がdet(I-M(j\omega_0)\Delta(j\omega_0))=0であり，多入力多出力で考えているので，det( )がついているが，やはりマイナスになっている。）
ナイキストの安定定理は，実用的には開ループが安定な場合の簡易版しかほぼ使わないので，そちらで覚えている人が多いだろう。簡易版の考え方で言うと，開ループ伝達関数の軌跡が複素平面で(-1,0)の左側を回らなければ閉ループは安定である。ただし，開ループはマイナスでフィードバックする前提なので，今回の場合，開ループ伝達関数の軌跡は，-M(j\omega)\Delta(j\omega)のようにマイナスで考えなければならない。1-M(j\omega_0)\Delta(j\omega_0)=0は，\omega=\omega_0のときにちょうど開ループが(-1,0)になることを意味している。(-1,0)は位相遅れが-180degの点だ。つまり，ある周波数\omega_0で|M(j\omega_0)|\geq1となる場合，その周波数で開ループの位相遅れが-180degになるような\Deltaを作れる，ということを示してスモールゲイン定理の必要性を証明しているのである。私の解釈した結論とつながった。
(-1,0)の点は，安定かどうかの判定のちょうど境目であり発散はしないが、持続振動になるので，０に収束するのが安定であるとする漸近安定の定義では安定ではない。

後は、一応、例えばどのような\Delta(s)の時に|\Delta(j\omega)|\leq1 \quad\forall\omegaかつ1-M(j\omega_0)\Delta(j\omega_0)=0となるのかを見ておこう。井村氏の著書では，

\displaystyle{\begin{eqnarray} \Delta(s)&=&\frac{g(s)h(s)}{|M(j\omega_0)|^2}\\ g(s)&=&\left(\frac{\omega_0s}{qs^2+\omega_0s+q\omega_0^2}\right)^2\\ h(s)&=&a+\frac{s}{\omega_0}b \end{eqnarray}}
という関数を作っている。ただし，a=\mathrm{Re}[M(j\omega_0)] ，b=\mathrm{Im}[ M(j\omega_0)] とおいた。M(j\omega_0)=a+bj である。

g(s)は、s=j\omega_0のとき，

\displaystyle{\begin{eqnarray} g(j\omega_0)&=&\left(\frac{j\omega_0^2}{-q\omega_0^2+j\omega_0^2+q\omega_0^2}\right)^2=1 \end{eqnarray}}
となる関数になっている。それ以外の周波数\omegaのときは0に近い大きさになるように，係数qは十分大きい値であるとしている。
h(s)は、s=j\omega_0のとき，h(j\omega_0)=M(j\omega_0)となる関数である。
h(s)は1次関数で，g(s)の相対次数は2なので，g(s)h(s)はs=j\omega_0のとき最大となる。
そのときの\Delta(s)の大きさは，
\displaystyle{\begin{eqnarray} \left|\Delta(j\omega_0)\right|=\frac{|g(j\omega_0)h(j\omega_0)|}{|M(j\omega_0)|^2}=\frac{1}{|M(j\omega_0)|}\leq1 \end{eqnarray}}
なので，|\Delta(j\omega)|\leq1 \quad\forall\omegaを満たしている。
では、もう一つの条件、1-M(j\omega_0)\Delta(j\omega_0)=0の方も確認しよう。
\displaystyle{\begin{eqnarray} 1-M(j\omega_0)\Delta(j\omega_0)&=&1-M(j\omega_0)\frac{g(j\omega_0)h(j\omega_0)}{|M(j\omega_0)|^2}\\ &=&1-\frac{M(j\omega_0)^2}{|M(j\omega_0)|^2}\ ? \end{eqnarray}}
おや？0にならない。なぜだ？ここも教科書のミスではないか。
私の解釈通りなら、s=j\omega_0のとき，h(j\omega_0)がM(j\omega_0)の共役となるように
h(s)=a-\frac{s}{\omega_0}b
と置けば，共役でも大きさは同じなので|\Delta(j\omega)|\leq1 \quad\forall\omegaはそのままで，かつ
\displaystyle{\begin{eqnarray} 1-M(j\omega_0)\Delta(j\omega_0)&=&1-(a+bj)\frac{a-bj}{a^2+b^2}=0 \end{eqnarray}}
となる。
ちょっと合わなくて自分の解釈に寄せてしまったところもあるが、不安定になる\Deltaを作れた。Zhou氏の証明もこれの多軸バージョンと考えれば，全てを理解したと言っていいだろう。
f:id:manva:20210922144352p:plain

無料で実践ロバスト制御⑦ mixsynの方が楽だった

Python-Control 制御工学

平田氏の著書「実践ロバスト制御」↓を無料のPython-Controlで勉強する試み⑦。

実践ロバスト制御 (システム制御工学シリーズ)

実践ロバスト制御 (システム制御工学シリーズ)

Amazon

今まで，教科書のMATLABサンプルの通り，hinfsynを使ってH∞制御問題を解いてきたが，mixsynというコマンドもあって，使い方を調べてみたら，混合感度問題を解くならこっちを使った方が楽だった。
前の記事↓で書いたように，
manva.hatenablog.com
hinfsynを使う場合のH∞制御系の設計手順は以下の通り。

hinfsynを使う場合の設計手順

１．制御対象の摂動を定義する
２．乗法的摂動のボードゲイン線図を描く
３．それを覆うように重みW_Tを決める
４． 感度関数に対する重みW_Sを決める
５．一般化プラントGを定義
６． 解く！ hinfsyn


mixsynを使う場合，上記の5番がいらない。

mixsynを使う場合の設計手順

１．制御対象の摂動を定義する
２．乗法的摂動のボードゲイン線図を描く
３．それを覆うように重みW_Tを決める
４． 感度関数に対する重みW_Sを決める
５．解く！ mixsyn

Python-Controlのcontrol.mixsynへの引数は(g,w1,w2,w3)で，gは一般化プラントではなくてノミナルの制御対象を与え，w1に感度関数に対する重みW_S，w3に相補感度関数に対する重みW_Tを与える。
control.mixsyn — Python Control Systems Library dev documentation
基本はMATLABの仕様に合わせているので，使い方はMATLABのドキュメント↓を参照するとよい。
Mixed-sensitivity H∞ synthesis method for robust control loop-shaping design - MATLAB mixsyn - MathWorks 日本
↓の図がわかりやすい。
Mixed-Sensitivity Loop Shaping - MATLAB & Simulink - MathWorks 日本

w2は今回はいらないのだが，

K, CL, info = control.mixsyn(Pn,Ws,None,Wt)

としたら応答が返ってこなかったので，

K, CL, info = control.mixsyn(Pn,Ws,tf(0.001,1),Wt)

のように伝達関数で小さい値を入れておいたらできた。

↓の問題をmixsynで解くと
無料で実践ロバスト制御④ ーH∞制御問題を解くー - manvaのエンジニアリング魂
↓のような感じ。

# 実践ロバスト制御4.2節
import numpy as np
import matplotlib.pyplot as plt
from control.matlab import *
import control

# %% パラメータの定義
m1 = 0.8;
m2 = 0.2;
k1 = 100;
k2 = 300;
c1 = 1;
c2 = 0.3;
Ks = 100;
# %% 運動方程式のM,K,C行列を定義
M = np.matrix([ [ m1,  0 ],
                [  0, m2 ] ]);
C = np.matrix([ [ c1+c2, -c2 ],
                [ -c2,    c2 ] ]);
F = np.matrix([ [ Ks ],
                [  0 ] ]);
K = np.matrix([ [ k1+k2, -k2 ],
                [ -k2,    k2 ] ]);
# %% 状態空間実現
iM = np.linalg.inv(M);
Ap = np.concatenate([
     np.concatenate([np.zeros((2,2)),  np.eye(2)], axis=1),
     np.concatenate([          -iM*K,     -iM*C ], axis=1) ]);
Bp = np.concatenate([ np.zeros((2,1)), iM*F ]);
Cp = [ 0, 1, 0, 0 ];
Dp = 0;
# %% ノミナル制御対象の定義
P_nominal = ss( Ap, Bp, Cp, Dp );
Pn = ss2tf(P_nominal);

#%% 重み関数の定義
s = tf('s');
Wm = 3*s**2/(s**2+18*s+45**2);
Ws = 15/(s + 1.5e-2); #% Ws
Wt = Wm;              #% Wt

#%% H-inf制御器の計算（実行4.1）
K, CL, info = control.mixsyn(Pn,Ws,tf(0.001,1),Wt) #混合感度問題
print( info )

plt.figure(4)
mag1, phase1, om1 = bode( P_nominal, plot=False );
plt.semilogx( om1, 20*np.log10(mag1), '--', label='$P_{nominal}$' );
mag2, phase2, om2 = bode( K, plot=False );
plt.semilogx( om2, 20*np.log10(mag2), label='$K$' );
plt.ylim(-60, 60);
plt.xlim(1e0, 1e2);
plt.legend()
実行結果

f:id:manva:20210825030149p:plain
mixsynでも教科書の図4.8に相当する結果が得られた。

修正混合感度問題の場合

修正混合感度問題の場合は，感度関数をW_S=W_{PS}Pとおけばよい。
↓の問題をmixsynで解くには，
無料で実践ロバスト制御⑤ ー修正混合感度問題ー - manvaのエンジニアリング魂
上記プログラムでmixsynのところを↓のように書き換えるだけでよい。

WpsP = Ws*0.8*Pn;
K, CL, info = control.mixsyn(Pn,WpsP,tf(0.001,1),Wt)
実行結果

f:id:manva:20210825030058p:plain
教科書の図4.14に相当する結果が得られた。

修正混合感度問題 h2synの場合

ついでに，どんなものかよくわかっていないが，h2synも試してみた。こちらはhinfsynと同様の使い方で，一般化プラントを与える。↓の記事で
無料で実践ロバスト制御⑤ ー修正混合感度問題ー - manvaのエンジニアリング魂
移植した「プログラム4.5」のhinfsynのところを，↓のように書き換えるだけでできる。

K = control.h2syn(G,1,1)
実行結果

f:id:manva:20210825033349p:plain
H∞制御とよく似た結果が得られている。

平田氏の教科書のおかげで，H∞制御の混合感度問題を解いて制御器を設計することができるようになった。感謝したい。
この後，教科書６章では，μ設計法の話があるのだが，残念ながらPython-ControlではまだD-Kイタレーションのコマンドdksynなどは実装されていないようなので，このあたりでいったんロバスト制御の勉強を終了することにする。

スモールゲイン定理は「必要」十分条件？

制御工学

スモールゲイン定理は，「十分条件」と書かれている文献も多く，それなら当たり前の話として理解できるのだが，ロバスト制御の文献になると「必要十分条件」と書かれている。これが謎だったので，調べてみた。結局，この記事の最後に書いてある結論に到達するために，調べたことはほとんど役に立たなかったのだが，せっかく調べたので周辺の話も含めて書こうと思う。

なぜそこにこだわるのか

前の記事↓に書いたように，
無料で実践ロバスト制御⑥ 重み関数の決め方 - manvaのエンジニアリング魂
スモールゲイン定理が必要十分条件であるのなら，ロバスト安定条件\|W_TT\|_\infty<1も必要十分条件ということになるのだが，ロバスト安定条件を満たしていないのに発散しない現象があった。「必要」十分条件であるなら，この条件を満たさなければ「不安定」になるはずである。ロバスト制御の限界や思想を理解するために重要なポイントの気がするので，この謎をちゃんと理解しておきたかったためだ。

スモールゲイン定理とは

調べてわかってきたこととして，「スモールゲイン定理」というのが，どうも微妙にバージョン違いがいくつもあるようなのだ。だがだいたい以下の２つのどちらかに分けられる。
まず平井氏の著書↓の説明。

非線形制御 (現代非線形科学シリーズ)

非線形制御 (現代非線形科学シリーズ)

Amazon

定義1
f:id:manva:20210822110337p:plain
上図で，\DeltaとMはシステムの作用素である(非線形でもよい)。このシステムで，\DeltaとMのL_pゲインをそれぞれ\gamma_1，\gamma_2とすると，\gamma_1\gamma_2<1であれば，このシステムはL_p安定である。

こちらはスモールゲイン定理は十分条件として書かれている。

そして，平田氏の著書↓の説明。

実践ロバスト制御 (システム制御工学シリーズ)

実践ロバスト制御 (システム制御工学シリーズ)

Amazon

定義2
f:id:manva:20210816132754p:plain
上図で，\DeltaとMは安定でプロパな伝達関数とする。このとき，\|\Delta\|_\infty\leq1をみたす全ての\Deltaに対して，図の閉ループ系が内部安定となるための必要十分条件は\|M\|_\infty<1であること。

こちらは，「必要十分条件」と書かれている。
ロバスト制御の教科書や論文で参考文献としてよく挙がる，Zhou氏らの著書↓の説明も，書き方は少し違うが，こちらの定義になっている。
Robust and Optimal Control (Feher/Prentice Hall Digital and)　9.2節
Essentials of Robust Control (Prentice Hall Modular Series for Eng)　8.2節(同じ説明)
（上記文献で，\gamma=1とし，等号をどっちに含むかというのを１つに絞り，\in \mathcal{RH}_\inftyというのを「安定でプロパな伝達関数」と読み替えると，定義2になる。）
こちらも必要十分条件として書かれており，必要条件であることの証明もあるので，これがすんなり理解できれば早いのだが，難しくてわからない。

浅井氏の解説↓には上記２つの定義が両方説明されている。
「ノルムに基づくロバスト性解析」
https://www.jstage.jst.go.jp/article/sicejl1962/42/9/42_9_748/_pdf
この解説で，定理3.2が上記定義１，定理5.1が上記定義2に相当している。やはり，定理3.2は十分条件，定理5.1は必要十分条件として書かれている。

定義1と定義2の違いは，以下のような点である。

  • 定義1で「作用素」としていたところを，定義２では線形の伝達関数に限定している
  • 定義1はL_p安定(入出力安定)の条件であるが，定義2は内部安定の条件
  • 定義1では，\DeltaとMそれぞれをL_pゲイン(入出力関係の話)で考えているが，定義2ではH_pノルム(作用素の話)で考えている。
  • 定義1では，pは1から∞まで含むL_pゲインで考えているが，定義2ではH_\inftyノルムの話に限定している
  • 定義2では，\Deltaは複数の伝達関数の集合である

上記の違いのどれかが十分条件であるか必要十分条件であるかの差になっているはず。
入出力安定と内部安定の違いは今回の話とは関係なさそう。H_\inftyノルムの話に限定して考えているところの差か。

定義を調べたのは，前の記事で「ロバスト安定条件を満たしていないのに発散しない」ように見えたのは，「安定」の定義が違うためではないかと思ったからだ。つまり，ロバスト制御の話になると，「安定」ではない，すなわち「不安定」と言っても，「発散する」という意味じゃないのではないかと思ったのだ。上記平井氏の著書でも，「ノルムのとり方によって，同じシステムでも安定性が異なる」「例えば，L_1安定であっても，L_2安定ではないシステム‥も存在する」「入力や出力の大きさの測り方によって一つのシステムが安定とも不安定とも言われる」という記載があり，そういう違いではないかと考えたのだ。しかしそうではないらしく，定義2で「内部安定」と言っているのは，任意の初期値が０に収束するという漸近安定の話で間違いなさそうだ。

問題の整理

定義1でも定義2でも，十分条件であることは理解できる。ボード線図で言えば開ループのゲインが0dBを超えるところがなければ安定，
f:id:manva:20210822152548p:plain
ナイキスト線図で言えば単位円から出るところがなければ安定，
f:id:manva:20210822152632p:plain
という考え方でもわかるし，信号がループをぐるぐる回るうちにだんだん小さくなるイメージもわかる。
わからないのは，定義2の必要条件の方で，下図のような場合，
f:id:manva:20210822152656p:plainf:id:manva:20210822152745p:plain
赤線のところで１より大きいので，開ループのH_\inftyノルムは１を超えるが安定であるはずではないのか。

結局，こういう話なのでは？

上記のイメージは開ループの伝達関数の話であるが，スモールゲイン定理(定義2)では，\DeltaとMの２つに分けていることがポイントなのではないか。いろいろ調べて，考えた結果，結局，以下のような結論にたどり着いた。

結論スモールゲイン定理(定義2)は，\|\Delta\|_\infty\leq1の「全ての」\Deltaに対して内部安定であるための条件なので，\Deltaの位相遅れがどうなっているかはわからない。したがって，全ての周波数で開ループの位相遅れが-180(+360*n) deg になり得る。どの周波数で-180degになっても安定であるためには\|M\|_\infty<1であることが「必要」である。

ということならわかるな。
そういう話なら「定義2では，\Deltaは複数の伝達関数の集合である」という違いのせいか。定義1でも，L_pゲイン\gamma_1\leq1の「全ての」\Deltaに対してL_p安定であるためには\gamma_2<1であることが「必要」な気がする。
前の記事でロバスト安定条件を満たしていないのに発散しなかったのは，ばね定数(の大きさ)を±20%の範囲で振っていただけで，位相を振っていなかったため，全ての\Deltaの範囲を網羅的に確認できていなかったと考えられる。
つまり，制御対象のモデル化誤差部分の位相遅れは，本当はある範囲内でしかないのに，ロバスト安定条件(スモールゲイン定理)では，大きさ(H_\inftyノルム)しか制約していないため，\Deltaが全範囲の位相遅れ誤差を持ち得ることになってしまっており，その結果かなり保守的な条件になっているということだろう。