医デジ化を推進・展開するにあたってわれわれが独自に開発・蓄積してきた制御に関するコア基盤技術について概説する。人体との安定的な接触動作やなめらか、高速、ならびに高精度なロボット動作制御技術に特徴がある。
（コラム）
一般にシステムが利用価値をもつためには、まず安定(的)であることが必須の要件である。医療ロボットにおいてもこれは同様であり、すくなくとも目標位置（平衡点）周辺領域における局所的な安定性を確保することを考える必要がある※。つぎに（追従）精度や大域的安定性（ロバスト性）を高めることを考えることになる。
※場合によっては、高性能を追求するあまり、一時的（インパルス応答的）に不安定化してしまうことについては許容される可能性も考えられる。しかしながら、その場合であっても、全体的（ある関心時間区間）にならしてみてより高い性能が期待され、なおかつ大局的には安定的である場合に限られるであろう。
安定性の評価指標としては、リアプノフの意味で発散しない（であるゾーン内にとどまる）という意味の安定性、目標位置近傍で目標位置に収束するという意味での局所的漸近（きょくしょてきぜんきん）安定性、目標位置近傍以外の任意の位置を出発点として目標位置に収束するという意味での大域的漸近（たいいきてきぜんきん）安定性などがあげられる。
（参考文献）
上記を踏まえて提案する医療支援システムおよびその制御手法の適用範囲について議論するために，安定性について議論する。
因果律
剛体系の運動では、『力→加速度→速度→位置』の順に変化が伝わる。同様に、電磁気系では力を生じるのに先だって電流や磁場の変化がなければならない。これらは因果（いんが）律として扱うことができる。
電動モータで駆動されるロボットにおいては、上記の剛体系の因果律と電磁気系の因果律とが組み合わさったものとして扱うことができる。
マスタ・スレーブ・システムにおいては基本的に、『マスタの操作入力→スレーブの動作出力』の順に変化が伝わる。スレーブ側の反力呈示については、『スレーブと環境の接触→スレーブ側における反力の発生→マスタ側における医師への反力提示』の順に因果律にもとづいて変化が伝わる。
ベッドに横たわっている人を超音波プローブで診断している状況を考えよう。プローブとベッド（の天板）に挟まれた人体に加わる力のうち、医師がプローブを介して人体（の表面）を押す力と（質点からなる人体各部の）重力は能動的な力（既知力）とよばれる。
重力の影響については、ベッド（の天板）近くになればなるほど、人体各部はより多くの重力に起因する既知力を受けることになり、より圧縮・変形することとなる。
ベッド（の天板）が人体を押し返す力は能動的な力によって人体とベッド（の天板）との拘束条件が発生する力であり、受動的な力である。これは既知力と拘束条件から決定される。このため、未知な力あるいは拘束力とよばれる。ここでいう『未知』とは方程式を解いて求めるべき対象である『未知数』に近い意味で用いられている『未知』である。
このとき人体各部は既知力と拘束力のつり合いによって運動はしない（横隔膜や心臓の運動についてはここでは無視する）が、発生する内力（押しつけ力および人体各部の重力）によって変形はしている。
プローブと患部の間のすきまの微小体積要素には接触面に平行な方向（せん断方向）に超音波ジェルとプローブの押しつけ（応）力（垂直応力）による粘性摩擦（ねんせいまさつ）力も発生している。これについてはニュートンの粘性法則にもとづいて計算されるせん断応力とプローブの運動速度からある程度、その大きさを見積もることが可能であろう。
参考文献
[] 内山 勝，中村仁彦，ロボットモーション，岩波書店，2004.[] https://ja.wikipedia.org/wiki/%E3%83%8B%E3%83%A5%E3%83%BC%E3%83%88%E3%83%B3%E6%B5%81%E4%BD%93
ロボットの運動軌道計画および制御
ロボットの運動軌道計画においては、2階連続微分可能であることを運動軌道の条件として要求することが多い。運動量がmvによって表されることからもわかるように、速度をなめらかに制御することがロボットの運動軌道制御における本質であるからだ。
２階連続微分可能とは、２回微分したものがこれまた、連続な関数になっているという意味である。ロボットの関節角の２回微分は関節角加速度であり、関節角加速度に連続的な指令値を与えて関節角速度をなめらかに制御することができる。
ロボットのモーションコントロールボードは一般にこのような運動軌道制御が可能な仕様になっている。医療ロボットにおいては特に、安全・安心・安定的で思いやりのある動作がシステムに求められている。これを踏まえて運動軌道を計画するにあたってなるべく急激な速度変化（高周波動作）は避けるべきである。
(参考文献)
内山 勝，中村仁彦，ロボットモーション，岩波書店，2004.
Norihiro Koizumi, Shin'ichi Warisawa, Hiroyuki Hashizume, and Mamoru Mitsuishi, "Continuous path controller for the remote ultrasound diagnostic system," IEEE/ASME Trans. on Mechatronics (TMECH), Vol.13, No.2, pp.206-218, 2008, https://doi.org/10.1109/TMECH.2008.918530.
システムの数式モデル化
システムの制御系$C(s)$を効果的に設計するためには，XYZステージ$P(s)$，機構部の振動特性$M(s)$，追従対象位置取得部（超音波診断装置および画像処理装置）$O(s)$の各推定値$\hat{P}(s)$，$\hat{M}(s)$, $\hat{O}(s)$を同定して数式モデルとする必要がある．
（コラム）
一般にシステムが利用価値をもつためには、まず安定(的)であることが必須の要件である。医療ロボットにおいてもこれは同様であり、すくなくとも目標位置（平衡点）周辺領域における局所的な安定性を確保することを考える必要がある※。つぎに（追従）精度や大域的安定性（ロバスト性）を高めることを考えることになる。
※場合によっては、高性能を追求するあまり、一時的（インパルス応答的）に不安定化してしまうことについては許容される可能性も考えられる。しかしながら、その場合であっても、全体的（ある関心時間区間）にならしてみてより高い性能が期待され、なおかつ大局的には安定的である場合に限られるであろう。
安定性の評価指標としては、リアプノフの意味で発散しない（であるゾーン内にとどまる）という意味の安定性、目標位置近傍で目標位置に収束するという意味での局所的漸近（きょくしょてきぜんきん）安定性、目標位置近傍以外の任意の位置を出発点として目標位置に収束するという意味での大域的漸近（たいいきてきぜんきん）安定性などがあげられる。
（参考文献）
上記を踏まえて提案する医療支援システムおよびその制御手法の適用範囲について議論するために，安定性について議論する。
剛体系の運動では、『力→加速度→速度→位置』の順に変化が伝わる。同様に、電磁気系では力を生じるのに先だって電流や磁場の変化がなければならない。これらは因果（いんが）律として扱うことができる。
電動モータで駆動されるロボットにおいては、上記の剛体系の因果律と電磁気系の因果律とが組み合わさったものとして扱うことができる。
マスタ・スレーブ・システムにおいては基本的に、『マスタの操作入力→スレーブの動作出力』の順に変化が伝わる。スレーブ側の反力呈示については、『スレーブと環境の接触→スレーブ側における反力の発生→マスタ側における医師への反力提示』の順に因果律にもとづいて変化が伝わる。
ベッドに横たわっている人を超音波プローブで診断している状況を考えよう。プローブとベッド（の天板）に挟まれた人体に加わる力のうち、医師がプローブを介して人体（の表面）を押す力と（質点からなる人体各部の）重力は能動的な力（既知力）とよばれる。
重力の影響については、ベッド（の天板）近くになればなるほど、人体各部はより多くの重力に起因する既知力を受けることになり、より圧縮・変形することとなる。
ベッド（の天板）が人体を押し返す力は能動的な力によって人体とベッド（の天板）との拘束条件が発生する力であり、受動的な力である。これは既知力と拘束条件から決定される。このため、未知な力あるいは拘束力とよばれる。ここでいう『未知』とは方程式を解いて求めるべき対象である『未知数』に近い意味で用いられている『未知』である。
このとき人体各部は既知力と拘束力のつり合いによって運動はしない（横隔膜や心臓の運動についてはここでは無視する）が、発生する内力（押しつけ力および人体各部の重力）によって変形はしている。
プローブと患部の間のすきまの微小体積要素には接触面に平行な方向（せん断方向）に超音波ジェルとプローブの押しつけ（応）力（垂直応力）による粘性摩擦（ねんせいまさつ）力も発生している。これについてはニュートンの粘性法則にもとづいて計算されるせん断応力とプローブの運動速度からある程度、その大きさを見積もることが可能であろう。
参考文献
[] 内山 勝，中村仁彦，ロボットモーション，岩波書店，2004.
[] https://ja.wikipedia.org/wiki/%E3%83%8B%E3%83%A5%E3%83%BC%E3%83%88%E3%83%B3%E6%B5%81%E4%BD%93
ロボットの運動軌道計画および制御
ロボットの運動軌道計画においては、2階連続微分可能であることを運動軌道の条件として要求することが多い。運動量がmvによって表されることからもわかるように、速度をなめらかに制御することがロボットの運動軌道制御における本質であるからだ。
２階連続微分可能とは、２回微分したものがこれまた、連続な関数になっているという意味である。ロボットの関節角の２回微分は関節角加速度であり、関節角加速度に連続的な指令値を与えて関節角速度をなめらかに制御することができる。
ロボットのモーションコントロールボードは一般にこのような運動軌道制御が可能な仕様になっている。医療ロボットにおいては特に、安全・安心・安定的で思いやりのある動作がシステムに求められている。これを踏まえて運動軌道を計画するにあたってなるべく急激な速度変化（高周波動作）は避けるべきである。
(参考文献)
内山 勝，中村仁彦，ロボットモーション，岩波書店，2004.
Norihiro Koizumi, Shin'ichi Warisawa, Hiroyuki Hashizume, and Mamoru Mitsuishi, "Continuous path controller for the remote ultrasound diagnostic system," IEEE/ASME Trans. on Mechatronics (TMECH), Vol.13, No.2, pp.206-218, 2008, https://doi.org/10.1109/TMECH.2008.918530.
システムの数式モデル化
システムの制御系$C(s)$を効果的に設計するためには，XYZステージ$P(s)$，機構部の振動特性$M(s)$，追従対象位置取得部（超音波診断装置および画像処理装置）$O(s)$の各推定値$\hat{P}(s)$，$\hat{M}(s)$, $\hat{O}(s)$を同定して数式モデルとする必要がある．
システムの制御系$C(s)$を効果的に設計するためには，XYZステージ$P(s)$，機構部の振動特性$M(s)$，追従対象位置取得部（超音波診断装置および画像処理装置）$O(s)$の各推定値$\hat{P}(s)$，$\hat{M}(s)$, $\hat{O}(s)$を同定して数式モデルとする必要がある．
本システムではデジタル制御系として実装するため，系は離散的であるが，制御対象の運動を特徴づける周波数（0.1-0.5Hz程度\cite{AJN2001:D.Cordes}）と比較して，サンプリング周波数が十分に高いと考え，連続時間の伝達関数モデルとして各ブロックを表現する．本節では，このモデル化手法およびその結果について概説する．
離散には値の離散と時間の離散とがある\cite{2012YMori}．超音波画像にもとづくフィードバックシステムにおいてもこれは同様である．
フィードバック制御系
今日，多くのシステムがそうであるように，本システムにおいても，制御対象そのものは連続時間ベースで動作しているが，これを制御するためにデジタル制御装置を利用している．ディジタル制御装置を採用することで，制御系設計変更への対応が容易になる．
Although, the object for the control itself is an analog and continuous system, we apply an digital processing device to control our system, which is the same case as many other systems these days.
It makes easy to cope with the change of the design of the controller by adopting digital control device.
具体的に，本制御系は100 Hz 部分と1 kHz の部分が混在するマルチサンプリングレート系であり，離散系のシステムだが，制御系を構築するにあたって，
Specifically, our system is a digital controller, which has one control part at the sampling rate of 100 Hz and has another control part at the sampling rate of 1 kHz. In order to construct a digital controller for our system, we apply a method which has the following two steps.
まずは，連続時間システムの枠内で学問体系化されている制御系設計論を適用して，連続時間ベースで動作する制御装置のための制御則を求め，
In the first step, we construct a controller for analog system based on the control theory for continuous time systems.
つぎに，その制御則をデジタル制御装置に実装するためサンプリング周期Tを用いてデジタル化するという手法を採用する．本手法により，サンプリング周期が十分に短い場合には，十分な制御性能が期待できる．
In the second step, we digitize the controller by the sampling period $T$ so as to apply the controller to the digital control device. If the sampling period is set short enough, sufficient control performance is expectable by this method.
（注）接近法
ディジタル制御装置に実装する制御則を求めるにあたって，まずは，連続時間システムの枠内で学問体系化されている制御系設計論を適用して，連続時間ベースで動作する制御装置のための制御則を求め，その制御則をディジタル演算装置に実装するためサンプリング周期$T$を用いてディジタル化する．サンプリング周期がきわめて短い場合には，(連続時間系のモデルと離散時間系のモデルは接近するため)十分な制御性能を発揮することが期待できる．
（注）制御ブロック線図において，連続時間システムの伝達関数$G_c(s)$の前後にホールダとサンプラを付加した部分の入出力特性をパルス伝達関数$G_H(z)$で表すと，$G_H(z)$と$G_c(s)$の間に，つぎの関係式が成立する．
高次系の応答と2次系あるいは1次系への近似
多くの高次系システムの応答は，支配的な極である代表根を考慮することで，2次系あるいは1次系の応答に近似できる\cite{1998MATLABniyoru-seigyoriron-no-kiso:KNonami}．
一般に，虚軸に近い極に対応する成分は減衰が小さく，原点に近い極の係数は大きくなるので，$t = 0$ に近い部分を除きこれらが過渡応答の主要な部分を支配する．このような極を代表根と呼ぶ．
（フィードバック制御とコンプライアンス）~~~~~~~
PID制御によるフィードバック制御はソフトウェアによる仮想的なバネのアナロジーとして捉えることもできる。
前後の自動車は仮想的なバネでつながれている。自動運転によって仮想的なバネでつながれた車の群は蛇型ロボットと考えることもできる。
~~~~~~~（フィードバック制御とコンプライアンス）
安定判別の方法
受動性
システムへの入力に対して対象とする系全体のエネルギーが消費・減衰することを安定であることの定義としようとする、観察による経験則にもとづいた考え方。具体的に、摩擦などによってエネルギーは消費・減衰する。
（参考文献）
[1] 坂本登, システムの受動性に関する研究, 1996.
http://ir.nul.nagoya-u.ac.jp/jspui/handle/2237/6437リアプノフ安定性
エネルギーを用いて安定性を議論する。ほかの安定判別法では扱いが困難な非線形なシステムに対しても安定性を議論できるきわめて強力な理論。解軌道に沿って系（システム）のエネルギーが増加しない関数（リアプノフ関数とよばれる）を発見することにより、内部安定性（外部入力がない場合に系が安定であること）を確認しようという観察にもとづく考え方。
単峰（たんほう：peakが一つ）性の等高線をもつ山頂付近からふもとに向けて流れる川はポテンシャルエネルギーと川の軌道に沿ってエネルギーが増加しない関数を発見できるため、リアプノフ安定であるといえる。フィードバック制御系、インピーダンス制御系は入力に対していずれもエネルギーを消散する系なので、本質的にリアプノフ安定である。
（参考文献）
[] 志水清孝, フィードバック制御理論, コロナ社, 2013.
[] 小郷，美多：「システム制御理論入門」, p.79, 実教出版(1981)。
[] 動的システムの安定性と過渡応答
http://power.nagaokaut.ac.jp/convenience/pdffiles/stability.pdf
ボード線図は，ナイキスト線図と同等の情報を有する．制御系設計の際の実用性からいえば，ボード線図が最もよく用いられているといえるだろう．
（参考文献）
[] 志水清孝, フィードバック制御理論, コロナ社, 2013.
[] 小郷，美多：「システム制御理論入門」, p.79, 実教出版(1981)。
[] 動的システムの安定性と過渡応答
http://power.nagaokaut.ac.jp/convenience/pdffiles/stability.pdf
ボード線図は，ナイキスト線図と同等の情報を有する．制御系設計の際の実用性からいえば，ボード線図が最もよく用いられているといえるだろう．
[] http://stlab.ssi.ist.hokudai.ac.jp/yuhyama/lecture/linearsys/st2-2up.pdf
ラウスの安定判別法
特性方程式（１巡伝達関数＝０）をもとに、その係数からラウス表を作成してシステムの安定性を判別する方法。1874年にイギリスの数学者であるエドワード・ラウス（Edward Routh）が提案した。
ラウスの安定判別法
特性方程式（１巡伝達関数＝０）をもとに、その係数からラウス表を作成してシステムの安定性を判別する方法。1874年にイギリスの数学者であるエドワード・ラウス（Edward Routh）が提案した。
フルビッツの安定判別法
1895年にフルビッツは、ラウスの安定判別法とは独立にフルビッツの安定判別法を提案した。 両判別法は数学的には全く等価であることが知られている。フルビッツの安定判別法では特性方程式（１巡伝達関数＝０）をもとに、その係数から作成した主座（しゅざ）行列式を用いてシステムの安定性を判別する。
（参考文献）
ゲイン余有
ボード線図は直観的に見通し良く参照目標値に追従するフィードバック制御の安定性を議論できる利便性から頻回（ひんかい）に用いられている。一般にフィードバック制御ではゲインを大きくすればするほど、位相が遅れれば遅れるほど不安定化につながることが知られている。
ボード線図は直観的に見通し良く参照目標値に追従するフィードバック制御の安定性を議論できる利便性から頻回（ひんかい）に用いられている。一般にフィードバック制御ではゲインを大きくすればするほど、位相が遅れれば遅れるほど不安定化につながることが知られている。
これを踏まえてループゲインG(jω)（位相情報を含む複素数で与えられていることに着目されたい！）の位相遅れが180度（逆相）になる（サッカーやバスケットボールなどでフェイントに引っかかったときのようにダメージが最悪の、泣きっ面に蜂の状態で評価する！）ときのことを想定して、さらにその上でゲインの絶対値が不安定化の指標である1倍(0dB)を超えるまでにどれほどのゲインの余有（よゆう）があるかを表す。
臓器内に埋め込まれた患部に追従するロボティック制御の安定性を考慮したゲインを設計するうえでも非常に有用な指標である。現実的な運用としては、ゲインを実験（経験）的に調整するにあたって、臓器の呼吸（あるいは拍）動を模擬した正弦波的な周期位相（呼吸動の場合は0.25Hz、拍動の場合は1Hz程度）をもつある入力に対して現在設定しているゲインで位相遅れが180度（逆相）になり、フェイントに引っかかる場合を想定して、それでもまだ安定的に追従動作しうるかどうか、ゲインをもうすこし上げる余地があるのかどうか、あるいはもうすこし保守的にゲインを抑えて制御したほうが良いのかを（試行錯誤的に）判断することになる。
位相の遅れが追従にもたらす影響は、正弦波状にジグザグに動くターゲット（獲物）にあわせて、蛇が体軸をおなじく正弦波状にくねらせ、ジグザグ走行しながら追いかけているところを想像してみれば直観的に理解しやすいだろう。すなわち正弦波の振幅がゲインを表す。
このとき、蛇の胴体を体軸にそっていくつかの部分ごとに切り分けて見た場合、蛇の頭の部分は位相の遅れがゼロで追従していたとしても、頭から尾にむけて、位相が徐々に遅れてゆき、追従の位相の遅れが１８０度（逆相）になってしまう胴体の部分では、頭とは逆方向にパーツが動くことになる（頭は追従できていても、尻尾は追従できていない）。
この状態でゲインの絶対値が1倍(0dB)を超えてしまうと上記のパーツ部分では参照目標値（追従ターゲット）との誤差に対して常に誤差を拡大する方向にフィードバックがかかり続けるため、時間とともに誤差が際限なく拡大（不安定化する）してしまうことになる。一般的に、サーボおよび定値制御の安定余有の目安は以下とされている。
（サーボ制御の安定余有の目安）
ゲイン余有：10～20[dB]
位相余有 ：40～60[°]
（定値制御の安定余有の目安）
ゲイン余有：3～10[dB]
位相余有 ：10～20[°]
 |
| 臓器追従制御の安定性評価 |
位相余有
ボード線図は直観的に見通し良く参照目標値に追従するフィードバック制御の安定性を議論できる利便性から頻回（ひんかい）に用いられている。一般にフィードバック制御ではゲインを大きくすればするほど、位相が遅れれば遅れるほど不安定化につながることが知られている。
ボード線図は直観的に見通し良く参照目標値に追従するフィードバック制御の安定性を議論できる利便性から頻回（ひんかい）に用いられている。一般にフィードバック制御ではゲインを大きくすればするほど、位相が遅れれば遅れるほど不安定化につながることが知られている。
これを踏まえてループゲインG(jω)（位相情報を含む複素数で与えられていることに着目されたい！）の絶対値が1倍(0dB)になるときのことを想定して、さらにその上で位相遅れが180度（逆相）を超える（サッカーやバスケットボールなどでフェイントに引っかかったときのようにダメージが最悪の、泣きっ面に蜂の状態で評価する！）までにどれほどの位相の余有（よゆう）があるかを表す。
臓器内に埋め込まれた患部に追従するロボティック制御の安定性を考慮したゲインを設計するうえでも非常に有用な指標である。現実的な運用としては、ゲインを実験（経験）的に調整するにあたって、臓器の呼吸（あるいは拍）動を模擬した正弦波的な周期位相（呼吸動の場合は0.25Hz、拍動の場合は1Hz程度）をもつある入力に対して現在のゲインで位相の追従遅れがどの程度でているのか？位相遅れが180度（逆相）を超えるまでにはまだもうすこし余裕があるのか？を観察しながらゲインをもうすこし上げる余地があるのかどうか、あるいはもうすこし保守的にゲインを抑えて制御したほうが良いのかを（試行錯誤的に）判断することになる。
位相の遅れが追従にもたらす影響は、正弦波状にジグザグに動くターゲット（獲物）にあわせて、蛇が体軸をおなじく正弦波状にくねらせ、ジグザグ走行しながら追いかけているところを想像してみれば直観的に理解しやすいだろう。すなわち正弦波の振幅がゲインを表す。
このとき、蛇の胴体を体軸にそっていくつかの部分ごとに切り分けて見た場合、蛇の頭の部分は位相の遅れがゼロで追従していたとしても、頭から尾にむけて、位相が徐々に遅れてゆき、追従の位相の遅れが１８０度（逆相）になってしまう胴体の部分では、頭とは逆方向にパーツが動くことになる（頭は追従できていても、尻尾は追従できていない）。
この状態でゲインの絶対値が1倍(0dB)を超えてしまうと上記のパーツ部分では参照目標値（追従ターゲット）との誤差に対して常に誤差を拡大する方向にフィードバックがかかり続けるため、時間とともに誤差が際限なく拡大（不安定化する）してしまうことになる。一般的に、サーボおよび定値制御の安定余有の目安は以下とされている。
（サーボ制御の安定余有の目安）
ゲイン余有：10～20[dB]
位相余有 ：40～60[°]
（定値制御の安定余有の目安）
ゲイン余有：3～10[dB]
位相余有 ：10～20[°]
フィードバック制御における摂動への対策
状態方程式の構造は，さまざまなノイズに起因する摂動的なノイズへの対処法に関して，重要な示唆を与えてくれる．
まず，連続系の表現を与える．
\begin{equation}
\begin{displaymath}
\left\{
\begin{array}{l}
\dot{\xi} = A_c \xi + B_c (r_f - y_w)\\
u_p = C_c \xi
\end{array}
\right.
\end{displaymath}
\label{dotxi = A_c \xi + B_c (r_f - y_w)}
\end{equation}
つぎに，上式を離散化すると，
\begin{equation}
\begin{displaymath}
\left\{
\begin{array}{l}
\xi[k+1] = A_c^d \xi[k] + B_c^d (r_f[k] - y_w[k])\\
u_p[k] = C_c^d \xi[k]
\end{array}
\right.
\end{displaymath}
\label{xi[k+1] = A_c^d xi[k] + B_c^d (r_f[k] - y_w[k])}
\end{equation}
現在の位置・速度 $\xi[k]$ に状態遷移行列$A_c^d$をかけた（１サンプリング時間進ませた）ものにゲインをかけて制御入力とする．ここで，$r_f[k]$はターゲットに対する追従目標位置，$y_w[k]$はロボットの位置（ロボット先端部の位置）出力である．
ここで，$A_c^d = e^{AT} \simeq I +TA + \frac{(TA)^2}{2 !} \cdots + \frac{(TA)^n}{n !} $でn次近似でき，サンプリング時間$T$が十分に短ければ，$A_c^d$は単位行列$I$に近づくことがわかる．このことは，サンプリング時間が短いと，$\xi[k]$と$\xi[k+1]$の差が極めて小さくなる（$\xi[k+1] \simeq \xi[k]$）という直観とも一致する．
システムそのものが，非線形的な出力を出す場合には，ロバスト安定化を考慮する必要がある．
本システムにおいても，超音波診断装置を画像処理して得られた3次元位置情報は多分に非線形的な出力を含む．
（参考）確率微分方程式によるアプローチ
begin{equation}
dy(t) = V \cdot dt+ W \cdot
dB(t)
+ W cdot
dBt }
\end{equation}
まず，カルマンフィルタなどのオブザーバにおける確率差分方程式の利用について概観する．
カルマンフィルタはシステム雑音$w(k)$および観測雑音$v(k)$が無視できない場合によく用いられ，つぎの3つの機能を有する．
[機能1] 過去のデータの雑音を取り除くスムージング機能
[機能2] 雑音に乱された情報から真の値を推定するフィルタリング機能
[機能3] 未来の値を予測するフォアキャスティング機能
Q：システム雑音の共分散
R：観測雑音の共分散
Rを大きくみつもる→予測値を信用→なめらか
Rを小さくみつもる→観測値を信用→振動的
Qを小さくみつもる→相対的にRを大きくみつもる→予測値を信用→なめらか
Qを大きくみつもる→相対的にRを小さくみつもる→観測値を信用→振動的
バブルに汚染される前の標準的な最尤軌道を獲得しておき，これを利用して，追従の精度を高める方法が考えられる．
カルマンフィルタは、ノイズを含む軌道をなめらかに整形するためのフィルタだと捉えられれば、トポロジーの問題へとつなげる世界観も可能であろう。
（参考文献）
[1] http://blog.livedoor.jp/k_yon/archives/52309233.html
[2] http://www1.accsnet.ne.jp/~aml00731/kalman.pdf
（参考）
高木 周，「機械系のための数学」 ，数理工学社，全231ページ，2005．の第８章 摂動法
都筑卓司，トポロジー入門、講談社．
（アキレスと亀）~~~~~~~
アキレスと亀の問題がある。アキレスが亀に追いついたときには亀はほんの先をいっており，ずっと追い越せないというパラドックスである。従来型のP制御のみを適用するとこの問題に直面することになる。そこで，患部運動モデルを構築して，将来の亀の位置を予測してこの地点に先回りしようというのがわれわれの提案法のコンセプトである。
~~~~~~~（アキレスと亀）
[1] 長沼伸一郎, 物理数学の直観的方法―理工系で学ぶ数学「難所突破」の特効薬〈普及版〉 (ブルーバックス), 2011.
オブザーバの構築
状態フィードバック制御を行なうためには，系の状態変数（たとえば，位置と速度）のすべてがセンサで計測可能であることが望ましいが，実際には，速度信号を正しく計測することは困難である．そこで，現実的には，センサ信号（出力）から状態変数を推定し，推定した状態変数を用いて状態フィードバック制御を行なうことになる．このための状態推定器をオブザーバという．すなわち，オブザーバとは，大雑把には，システムの入出力情報を用いて，運動（微分／差分）方程式にもとづいて状態変数を逐次的に推定するものと捉えて差し支えないであろう．
まず，オブザーバの一般系について概観する．
体表の運動から腎臓の運動を推定する
ここでは，
状態方程式の構造は，さまざまなノイズに起因する摂動的なノイズへの対処法に関して，重要な示唆を与えてくれる．
まず，連続系の表現を与える．
\begin{equation}
\begin{displaymath}
\left\{
\begin{array}{l}
\dot{\xi} = A_c \xi + B_c (r_f - y_w)\\
u_p = C_c \xi
\end{array}
\right.
\end{displaymath}
\label{dotxi = A_c \xi + B_c (r_f - y_w)}
\end{equation}
つぎに，上式を離散化すると，
\begin{equation}
\begin{displaymath}
\left\{
\begin{array}{l}
\xi[k+1] = A_c^d \xi[k] + B_c^d (r_f[k] - y_w[k])\\
u_p[k] = C_c^d \xi[k]
\end{array}
\right.
\end{displaymath}
\label{xi[k+1] = A_c^d xi[k] + B_c^d (r_f[k] - y_w[k])}
\end{equation}
現在の位置・速度 $\xi[k]$ に状態遷移行列$A_c^d$をかけた（１サンプリング時間進ませた）ものにゲインをかけて制御入力とする．ここで，$r_f[k]$はターゲットに対する追従目標位置，$y_w[k]$はロボットの位置（ロボット先端部の位置）出力である．
ここで，$A_c^d = e^{AT} \simeq I +TA + \frac{(TA)^2}{2 !} \cdots + \frac{(TA)^n}{n !} $でn次近似でき，サンプリング時間$T$が十分に短ければ，$A_c^d$は単位行列$I$に近づくことがわかる．このことは，サンプリング時間が短いと，$\xi[k]$と$\xi[k+1]$の差が極めて小さくなる（$\xi[k+1] \simeq \xi[k]$）という直観とも一致する．
システムそのものが，非線形的な出力を出す場合には，ロバスト安定化を考慮する必要がある．
本システムにおいても，超音波診断装置を画像処理して得られた3次元位置情報は多分に非線形的な出力を含む．
（参考）確率微分方程式によるアプローチ
begin{equation}
dy(t) = V \cdot dt
\end{equation}
まず，カルマンフィルタなどのオブザーバにおける確率差分方程式の利用について概観する．
カルマンフィルタはシステム雑音$w(k)$および観測雑音$v(k)$が無視できない場合によく用いられ，つぎの3つの機能を有する．
[機能1] 過去のデータの雑音を取り除くスムージング機能
[機能2] 雑音に乱された情報から真の値を推定するフィルタリング機能
[機能3] 未来の値を予測するフォアキャスティング機能
Q：システム雑音の共分散
R：観測雑音の共分散
Rを大きくみつもる→予測値を信用→なめらか
Rを小さくみつもる→観測値を信用→振動的
Qを小さくみつもる→相対的にRを大きくみつもる→予測値を信用→なめらか
Qを大きくみつもる→相対的にRを小さくみつもる→観測値を信用→振動的
バブルに汚染される前の標準的な最尤軌道を獲得しておき，これを利用して，追従の精度を高める方法が考えられる．
カルマンフィルタは、ノイズを含む軌道をなめらかに整形するためのフィルタだと捉えられれば、トポロジーの問題へとつなげる世界観も可能であろう。
（参考文献）
[1] http://blog.livedoor.jp/k_yon/archives/52309233.html
[2] http://www1.accsnet.ne.jp/~aml00731/kalman.pdf
（参考）
高木 周，「機械系のための数学」 ，数理工学社，全231ページ，2005．の第８章 摂動法
都筑卓司，トポロジー入門、講談社．
（アキレスと亀）~~~~~~~
アキレスと亀の問題がある。アキレスが亀に追いついたときには亀はほんの先をいっており，ずっと追い越せないというパラドックスである。従来型のP制御のみを適用するとこの問題に直面することになる。そこで，患部運動モデルを構築して，将来の亀の位置を予測してこの地点に先回りしようというのがわれわれの提案法のコンセプトである。
~~~~~~~（アキレスと亀）
[1] 長沼伸一郎, 物理数学の直観的方法―理工系で学ぶ数学「難所突破」の特効薬〈普及版〉 (ブルーバックス), 2011.
オブザーバの構築
状態フィードバック制御を行なうためには，系の状態変数（たとえば，位置と速度）のすべてがセンサで計測可能であることが望ましいが，実際には，速度信号を正しく計測することは困難である．そこで，現実的には，センサ信号（出力）から状態変数を推定し，推定した状態変数を用いて状態フィードバック制御を行なうことになる．このための状態推定器をオブザーバという．すなわち，オブザーバとは，大雑把には，システムの入出力情報を用いて，運動（微分／差分）方程式にもとづいて状態変数を逐次的に推定するものと捉えて差し支えないであろう．
まず，オブザーバの一般系について概観する．
体表の運動から腎臓の運動を推定する
ここでは，
ここで，『腎臓』は主に，ｘおよびｚ方向に運動する．
一方，『体表（おへそ）』はｚ軸方向のみ運動する．これは，
一方，『体表（おへそ）』はｚ軸方向のみ運動する．これは，
ここで，『腎臓』のx,zの運動も『体表（おへそ）』
軌道計画
アルキメデスの螺旋軌道の利用．
PTP制御とCP制御：
PTP (Point to Point)制御：軌道（経路）上の要所要所の点を（人が）与えることで軌道（経路）を生成する方法。点と点の間の軌道はサーボ系（システム）が決定する。Pick and Place作業などでよく用いられる。
CP制御（Continuous Path) 制御：運動の軌道（経路）を（人が）細かく与えることで連続的な軌道（経路）を計画する方法。連続的な経路の生成には直線補間、円弧補間、スプライン補間などが用いられる。PTP制御と比較して多くのメモリが必要になる。アーク溶接や障害物を回避しながらの作業などによく用いられる。
(インピーダンス制御)
[1] Kikuuwe, R. et al, Admittance and Impedance Representations of Friction Based on Implicit Euler Integration, Robotics, IEEE Transactions on, 2006.
http://ieeexplore.ieee.org/xpl/articleDetails.jsp?arnumber=4020366&queryText=Admittance%20and%20Impedance%20representaion%20of%20friction&newsearch=true
Kikuue, et al., studies an admittance and impedance representations for the haptic interfaces.
教示
ダイレクト・ティーチング：ロボットに動作を教示する際に、人間がロボットを直接手に取って教示するなど、人間を介在させる方法。
ティーチング・プレイバック：教示されたときの関節角のデータをもとに教示された動作を復元・再生する方法。
内部モデル原理
内部モデル原理はむだ時間を含まない系では定常偏差をゼロにすることを保証するが，
むだ時間を含む系では話はそれほど簡単にはゆかない．
パデ近似：
時間遅れを近似することができる。
最終値の定理
医療支援システムにおける内部モデル原理の拡張＝かゆいところに手がとどくシステム
＝医師が診たい位置に診たい姿勢からアプローチして適切な効果をおよぼす．
機能関数発生器（医師）と同じ機能関数をシステムの機構・制御・画像処理アルゴリズム上にもつことに対応する．
（注意）アクチュエータを実装する際は，まず，必要最小限のアクチュエータに絞ることを考えられたい．制御の追従誤差の観点から，動作させるモータは少ないほうがよいからだ．また，能動的なアクチュエータの整定時間を考慮すれば，パッシブな機構を採用したほうが有利な場合も多いだろう．
5.7 画像処理装置からXYZステージの制御系への送信データ
画像処理装置からXYZステージの制御系に送信するデータは下記の6点である．本データはLANを介してソケット通信により送受信することを想定している．そのため，送受信のためのデータ量の適正化などについては特に考慮していない．しかしながら，遠隔システムで，通信帯域に制限がある場合などは，特別の考慮が必要であろう\cite．
1. Dx, Dy, Dz：患部と焦点の相対位置（超音波画像により同定）
2. Kp, Kd：PDゲイン
3. Td：時間遅れ情報（画像取込みの際のむだ時間＋画像処理時間）
4. m_est_posX, m_est_posY, m_est_posZ：上記の1のデータのカルマンフィルタによる濾波位置推定値
5. m_est_velX, m_est_velY, m_est_velZ：上記の1のデータのカルマンフィルタによる濾波速度推定値
6. m_corrVal[2]：中央（超音波診断装置のバイプレーン･モードにおける左画面）および側面プローブ（同右画面）の画像相関値
（コラム：止まったエスカレータは歩きにくい）
止まったエスカレータは歩きにくい。これはわれわれの頭のなかにエスカレータのモデルができているからである。すなわち，これまでの経験的学習によってわれわれはエスカレータのモデルを頭のなかに構築してきているのである。
時間遅れ
制御系には，かならず時間遅れが存在する．本システムにおいてもこれは同様である．
1. 画像の取込みの際のむだ時間．ダブルバッファリングであればサンプリング時間遅れる．われわれのシステムでは，たとえば10ms程度．
2. 超音波画像処理の際の演算時間遅れ．われわれのシステムでは，たとえば5ms程度．
3. 伝送時間遅れ．特に，遠隔地への伝送やインターネットを介した伝送などでは，問題が深刻化する．われわれのシステムでは，たとえば1ms程度．
4. ロボットの制御における応答時間遅れ．1次遅れシステムの場合は時定数程度として見積もる．われわれのシステムでは，たとえば1ms程度．
ロボットの制御における応答時間に関するパラメータを下記に示す．
1. 立上がり時間 - 出力が定常値の10%から90%に達するまでの時間.
2. 遅れ時間 - 出力が定常値の50%に達するまでの時間.
3. 整定時間 - 出力が定常値の許容誤差範囲内に達するまでの時間.
4. 行過ぎ時間 - 出力が最大行過ぎ量（定常値または目標値を超えて現れる最初の最大値）に達する時間.
$t$領域で入力に対して出力が$T_d$ s 遅れる入出力関係を有する要素は時間遅れ要素と呼ばれる．時間遅れ要素は非線形要素の一つであり，その取扱いには注意を要する．
時間遅れ要素の位相は周波数に比例して遅れるため，時間遅れ要素がフィードバックループ内に存在すると，制御系の安定性に影響を与える．
時間遅れ要素を含むシステムの周波数応答を求める方法には以下の2つがある．
（方法1） まず，時間遅れ要素以外の伝達要素の周波数伝達関数からゲインと位相を求め，つぎに位相だけを遅らせればよい．しかしながら，この方法には煩雑な場合があるという短所がある．
（方法2） パデ近似を用いると時間遅れ要素を代数的な伝達関数として扱うことができる．ここで，パデ近似の次数を高くすると，モデルが正確になり，近似モデルの位相誤差が小さくなる．他方，次数を高くしすぎるとモデル全体の次数も高くなってしまい，解析も煩雑になってしまうため，解析対象の制御帯域での位相誤差が顕著にならない程度の次数を選定する．
Simulinkの時間遅れ要素（transport delay）をlinmod
機構振動部の同定
最初の１分は，
呼吸の周期は人によって異なり，安静時の正常呼吸は，
呼吸開始時には１２個以上のデータから運動モデルを作成，
（脳内回路の抵抗）~~~~~~~
例えば，英語でのプレゼンであったりとか，CPUとメモリに余裕がない状態になることが多い．普段やりなれないことをやるときなど，おそらく脳内回路の抵抗（インピーダンス）は大きくなり、熱を発する状態になっているのだろう，，，この場合，インピーダンスを下げるためには，回路をよく利用するしかないのだろう，，，やればやるほどやりたくなるなど，のめり込みもこれで説明できそうだ，，，
~~~~~~~（脳内回路の抵抗）
(参考文献)
[1] Anshul Thakral, et al., Surgical Motion Adaptive Robotic Technology (S.M.A.R.T): Taking the Motion out of Physiological Motion, 2016.
http://link.springer.com/chapter/10.1007/3-540-45468-3_38
[] Ginhoux, R., et al., Beating heart tracking in robotic surgery using 500 Hz visual servoing, model predictive control and an adaptive observer, 2004.
http://ieeexplore.ieee.org/stamp/stamp.jsp?arnumber=1307163
周期的な生体運動のモデル表現
神経振動子の位相\phi_iを用いて臓器運動が記述できるとする考え方（仮定）。
[] 青井伸也, 生物の歩行と力学系, 数理科学, vol.626, pp.47-52, 2015.
同次変換行列（hOmogeneous transformAtion mAtrix）
同次変換行列．姿勢変換行列（rotAtion mAtrix）および並進ベクトル（translAtion vector）を要素として内包する。運用にあたって同次変換行列が直交行列ではない（転置しても逆行列にはならない）ことには注意が必要である。
（脳内回路の抵抗）~~~~~~~
例えば，英語でのプレゼンであったりとか，CPUとメモリに余裕がない状態になることが多い．普段やりなれないことをやるときなど，おそらく脳内回路の抵抗（インピーダンス）は大きくなり、熱を発する状態になっているのだろう，，，この場合，インピーダンスを下げるためには，回路をよく利用するしかないのだろう，，，やればやるほどやりたくなるなど，のめり込みもこれで説明できそうだ，，，
~~~~~~~（脳内回路の抵抗）
(参考文献)
[1] Anshul Thakral, et al., Surgical Motion Adaptive Robotic Technology (S.M.A.R.T): Taking the Motion out of Physiological Motion, 2016.
http://link.springer.com/chapter/10.1007/3-540-45468-3_38
[] Ginhoux, R., et al., Beating heart tracking in robotic surgery using 500 Hz visual servoing, model predictive control and an adaptive observer, 2004.
http://ieeexplore.ieee.org/stamp/stamp.jsp?arnumber=1307163
周期的な生体運動のモデル表現
神経振動子の位相\phi_iを用いて臓器運動が記述できるとする考え方（仮定）。
[] 青井伸也, 生物の歩行と力学系, 数理科学, vol.626, pp.47-52, 2015.
同次変換行列（hOmogeneous transformAtion mAtrix）
同次変換行列．姿勢変換行列（rotAtion mAtrix）および並進ベクトル（translAtion vector）を要素として内包する。運用にあたって同次変換行列が直交行列ではない（転置しても逆行列にはならない）ことには注意が必要である。
アルキメデスの螺旋軌道の利用．
PTP制御とCP制御：
PTP (Point to Point)制御：軌道（経路）上の要所要所の点を（人が）与えることで軌道（経路）を生成する方法。点と点の間の軌道はサーボ系（システム）が決定する。Pick and Place作業などでよく用いられる。
CP制御（Continuous Path) 制御：運動の軌道（経路）を（人が）細かく与えることで連続的な軌道（経路）を計画する方法。連続的な経路の生成には直線補間、円弧補間、スプライン補間などが用いられる。PTP制御と比較して多くのメモリが必要になる。アーク溶接や障害物を回避しながらの作業などによく用いられる。
(インピーダンス制御)
[1] Kikuuwe, R. et al, Admittance and Impedance Representations of Friction Based on Implicit Euler Integration, Robotics, IEEE Transactions on, 2006.
http://ieeexplore.ieee.org/xpl/articleDetails.jsp?arnumber=4020366&queryText=Admittance%20and%20Impedance%20representaion%20of%20friction&newsearch=true
Kikuue, et al., studies an admittance and impedance representations for the haptic interfaces.
教示
ダイレクト・ティーチング：ロボットに動作を教示する際に、人間がロボットを直接手に取って教示するなど、人間を介在させる方法。
ティーチング・プレイバック：教示されたときの関節角のデータをもとに教示された動作を復元・再生する方法。
内部モデル原理
内部モデル原理はむだ時間を含まない系では定常偏差をゼロにすることを保証するが，
むだ時間を含む系では話はそれほど簡単にはゆかない．
パデ近似：
時間遅れを近似することができる。
最終値の定理
医療支援システムにおける内部モデル原理の拡張＝かゆいところに手がとどくシステム
＝医師が診たい位置に診たい姿勢からアプローチして適切な効果をおよぼす．
機能関数発生器（医師）と同じ機能関数をシステムの機構・制御・画像処理アルゴリズム上にもつことに対応する．
（注意）アクチュエータを実装する際は，まず，必要最小限のアクチュエータに絞ることを考えられたい．制御の追従誤差の観点から，動作させるモータは少ないほうがよいからだ．また，能動的なアクチュエータの整定時間を考慮すれば，パッシブな機構を採用したほうが有利な場合も多いだろう．
5.7 画像処理装置からXYZステージの制御系への送信データ
画像処理装置からXYZステージの制御系に送信するデータは下記の6点である．本データはLANを介してソケット通信により送受信することを想定している．そのため，送受信のためのデータ量の適正化などについては特に考慮していない．しかしながら，遠隔システムで，通信帯域に制限がある場合などは，特別の考慮が必要であろう\cite．
1. Dx, Dy, Dz：患部と焦点の相対位置（超音波画像により同定）
2. Kp, Kd：PDゲイン
3. Td：時間遅れ情報（画像取込みの際のむだ時間＋画像処理時間）
4. m_est_posX, m_est_posY, m_est_posZ：上記の1のデータのカルマンフィルタによる濾波位置推定値
5. m_est_velX, m_est_velY, m_est_velZ：上記の1のデータのカルマンフィルタによる濾波速度推定値
6. m_corrVal[2]：中央（超音波診断装置のバイプレーン･モードにおける左画面）および側面プローブ（同右画面）の画像相関値
（コラム：止まったエスカレータは歩きにくい）
止まったエスカレータは歩きにくい。これはわれわれの頭のなかにエスカレータのモデルができているからである。すなわち，これまでの経験的学習によってわれわれはエスカレータのモデルを頭のなかに構築してきているのである。
時間遅れ
制御系には，かならず時間遅れが存在する．本システムにおいてもこれは同様である．
 |
| NIUTSにおけるむだ時間 |
1. 画像の取込みの際のむだ時間．ダブルバッファリングであればサンプリング時間遅れる．われわれのシステムでは，たとえば10ms程度．
2. 超音波画像処理の際の演算時間遅れ．われわれのシステムでは，たとえば5ms程度．
3. 伝送時間遅れ．特に，遠隔地への伝送やインターネットを介した伝送などでは，問題が深刻化する．われわれのシステムでは，たとえば1ms程度．
4. ロボットの制御における応答時間遅れ．1次遅れシステムの場合は時定数程度として見積もる．われわれのシステムでは，たとえば1ms程度．
ロボットの制御における応答時間に関するパラメータを下記に示す．
1. 立上がり時間 - 出力が定常値の10%から90%に達するまでの時間.
2. 遅れ時間 - 出力が定常値の50%に達するまでの時間.
3. 整定時間 - 出力が定常値の許容誤差範囲内に達するまでの時間.
4. 行過ぎ時間 - 出力が最大行過ぎ量（定常値または目標値を超えて現れる最初の最大値）に達する時間.
$t$領域で入力に対して出力が$T_d$ s 遅れる入出力関係を有する要素は時間遅れ要素と呼ばれる．時間遅れ要素は非線形要素の一つであり，その取扱いには注意を要する．
時間遅れ要素の位相は周波数に比例して遅れるため，時間遅れ要素がフィードバックループ内に存在すると，制御系の安定性に影響を与える．
時間遅れ要素を含むシステムの周波数応答を求める方法には以下の2つがある．
（方法1） まず，時間遅れ要素以外の伝達要素の周波数伝達関数からゲインと位相を求め，つぎに位相だけを遅らせればよい．しかしながら，この方法には煩雑な場合があるという短所がある．
（方法2） パデ近似を用いると時間遅れ要素を代数的な伝達関数として扱うことができる．ここで，パデ近似の次数を高くすると，モデルが正確になり，近似モデルの位相誤差が小さくなる．他方，次数を高くしすぎるとモデル全体の次数も高くなってしまい，解析も煩雑になってしまうため，解析対象の制御帯域での位相誤差が顕著にならない程度の次数を選定する．
Simulinkの時間遅れ要素（transport delay）をlinmod
機構振動部の同定
In this chapter, we propose a mechanical system identification method including the mechanical oscillation part for the Non-Invasive Ultrasound Theragnostic System (NIUTS). NIUTS tracks and follows the movement in an affected area to be treated (kidney stones, in the present study) by irradiating the area with high-intensity focused ultrasound (HIFU). Blur noise caused by oscillation of the mechanical systems deteriorates the servoing performance. To cope with this problem and enhance the servoing performance, it should be required at first to identify the mechanical oscillation part.
本システムにおいては，超音波画像取得および画像処理の際に発生するむだ時間がフィードバック系におけるメインのむだ時間要素になる．
（例）
たとえばメインのむだ時間が10ms，メインの周波数成分が1Hzだとすると，むだ時間が位相おくれに与える影響は下記で計算できる。
位相遅れ=むだ時間／メインの周波数成分の周期～2π×10ms/1s～2π×10^{-2} rad
位相余裕はこの位相遅れ分だけなくなることになる．
（参考文献）
[1] Keehoon Kim, Çavuşoğlu, M.C., Wan Kyun Chung, Quantitative Comparison of Bilateral Teleoperation Systems Using μ-Synthesis, Robotics, IEEE Transactions on
Year: 2007, Volume: 23, Issue: 4, Pages: 776 - 789, DOI: 10.1109/TRO.2007.900625
http://ieeexplore.ieee.org/xpl/articleDetails.jsp?arnumber=4285849&newsearch=true&queryText=quantitative%20comparison%20%20keehoon
[2] Tanner, N.A., et al., High-frequency acceleration feedback in wave variable telerobotics, Mechatronics, IEEE/ASME Trans., Volume:11 Issue:2, 2006.
http://ieeexplore.ieee.org/xpl/articleDetails.jsp?arnumber=1618669&newsearch=true&queryText=High-Frequency%20acceleration%20feedback
Tanner propose a control method to transmit high frequency acceleration information to the operator independently of the low frequency force feedback.
安定判別法
ボード線図
ボード線図はナイキスト線図と同じ情報を有しており，制御系設計の実用性という観点からは，ボード線図がもっともよく利用されている．ナイキスト線図での(-1,j0)という点はゲインでいえば0DB，位相でいえば-180度を意味する．したがって，ボード線図で位相が-180度のときの0dBとゲイン線図との隔たりがナイキスト線図における(-1,j0)点までの距離に対応しており，これをゲイン余裕とよぶ．すなわち，ナイキスト線図が(-1,j0)点と重なるまでゲインKを何倍にできるか，その値のデシベル表示である．
位相が-180度の点でゲインが0dBより大きければ，ナイキスト線図での(-1,j0)点の外側を意味し，制御系は不安定である．同様に，ゲイン線図と0dBラインとの交点の周波数における位相線図の-180度までの隔たりを位相余裕とよぶ．ゲインが0dBということは，ナイキスト線図では半径が1の単位円を意味しており，ナイキスト線図ではベクトル軌跡と単位円との交点を意味する．この点においてもし位相が180度以上遅れていたらその制御系は不安定である．これはナイキスト線図で単位円とベクトル軌跡の交点が(-1,j0)点より上側であることに対応する．
根軌跡法
根軌跡法は安定判別法の範疇というより，むしろ閉ループ制御系設計法のひとつであると考えるほうが良いだろう．しかしながら，根軌跡法が意味するところは，じつはナイキスト線図やボード線図がいみするところと本質的に同じである．
根軌跡とは，制御系の特性方程式
1+KGH=0
において，ゲインKを0→∞まで変化させたときの特性根の変化を複素平面上にプロットしたものにほかならない．ラウスやフルビッツの安定判別法がゲインKに対して，特性根を直接求めることなく正の実部を有する特性根が存在しないための必要十分条件を与えているのに対して，根軌跡はすべてのKに対して特性根を計算し，その軌跡を複素平面上にプロットしたものである．
この軌跡が複素平面上の右半平面に出た瞬間に制御系は不安定化する．このときのゲインが安定限界値である．
根軌跡の性質
1. 根軌跡は実軸に関して対称である．
2. 根軌跡は一巡伝達関数の極にはじまり，零点または無限遠点でおわる．
3. 根軌跡の分岐の数は一巡伝達関数の極の数，これを特性方程式の次数という，に等しく，極の数と零点の数の差が無限遠点に向かう根軌跡の数である．
以前は，一般の特性方程式に対して，すべての根を計算することは非現実的であったが，近年のCPUパワーの増大により，複雑な特性方程式に対しても根軌跡をたちどころに描けるようになった．
自由システムと自律システム
システムへの入力がないシステムを自由システムという。自由システムが安定であることをシステムが内部安定性を有するという。
システムが時間によって陽に変化しない（システムの構造が時間によって不変な）システムを自律システムという。任意の時刻を時間軸の原点に設定し直すことができる。
ノッチフィルタ
提案する制御手法の有効性を確認するために， [Condition I] ノッチフィルタを適用しない場合（図3）と[Condition II] ノッチフィルタを適用した場合（図4）を比較する実験を行なった．モデルを動作させる方向は体軸方向に相当する並進1軸として，実際のヒトの腎臓動作をモデルに入力した．結果をTable 1に示す．計測時間は30 s とした．Table 1から，ノッチフィルタの利用により追従精度が0.3mm程度向上することが確認される．また，図3,4から，システムの振動がノッチフィルタを適用することにより，抑制されることが確認される．これらの振動は，主に，システムの共振周波数に基づく振動である
We conducted an experiment to confirm the proposed oscillation reduction filter (Notch filter), which is based on the identification of the mechanical oscillation part. [Condition I] Without Notch filter, [Conditon II] With Notch filter (Fig.\ref).
プローブ操作の目的に応じてインピーダンス・パラメータを適正化
カルマンフィルタ
（参考文献）
[1] http://www1.accsnet.ne.jp/~aml00731/kalman.pdf
パーティクル・フィルタ
1. 各パーティクルの次の座標を適切な運動モデル（運動方程式等）にもとづいて予測する。運動モデルのもっとも簡単なものとして等速直線運動が用いられることが多い。
2. 予測値と実測値の誤差（誤差の大小が尤度をあらわす）を評価，これにもとづいて各パーティクルに対する重み（重要度）を更新。
3. ２でもとめた重みにもとづいて，パーティクルをリサンプリング（再度サンプリング）する。得られた分布のたとえば最頻値を予測値とする。
（参考文献）
[1] http://www.hiroict.com/misato/hiro_ict_techs/index9/link:9
隠れマルコフモデル
（例）動画中の追跡対象物の位置データ系列がそのデータ系列の裏側に隠れて推移しているモデル。
（参考文献）
[1] http://www.hiroict.com/misato/hiro_ict_techs/index9/link:9
（参考文献）
[1] 清水年美, 柔軟構造を含む機械システムの運動と振動を同時に制御可能な制御系設計法.
http://www.ccr.gunma-u.ac.jp/4u/Seeds/Pdf/2012/Ibaraki/Vol05_P149_150_I_Mono_Shimizu.pdf
制御目的
（参考文献）
本システムにおいては，超音波画像取得および画像処理の際に発生するむだ時間がフィードバック系におけるメインのむだ時間要素になる．
（例）
たとえばメインのむだ時間が10ms，メインの周波数成分が1Hzだとすると，むだ時間が位相おくれに与える影響は下記で計算できる。
位相遅れ=むだ時間／メインの周波数成分の周期～2π×10ms/1s～2π×10^{-2} rad
位相余裕はこの位相遅れ分だけなくなることになる．
（参考文献）
[1] Keehoon Kim, Çavuşoğlu, M.C., Wan Kyun Chung, Quantitative Comparison of Bilateral Teleoperation Systems Using μ-Synthesis, Robotics, IEEE Transactions on
Year: 2007, Volume: 23, Issue: 4, Pages: 776 - 789, DOI: 10.1109/TRO.2007.900625
http://ieeexplore.ieee.org/xpl/articleDetails.jsp?arnumber=4285849&newsearch=true&queryText=quantitative%20comparison%20%20keehoon
[2] Tanner, N.A., et al., High-frequency acceleration feedback in wave variable telerobotics, Mechatronics, IEEE/ASME Trans., Volume:11 Issue:2, 2006.
http://ieeexplore.ieee.org/xpl/articleDetails.jsp?arnumber=1618669&newsearch=true&queryText=High-Frequency%20acceleration%20feedback
Tanner propose a control method to transmit high frequency acceleration information to the operator independently of the low frequency force feedback.
安定判別法
ボード線図
ボード線図はナイキスト線図と同じ情報を有しており，制御系設計の実用性という観点からは，ボード線図がもっともよく利用されている．ナイキスト線図での(-1,j0)という点はゲインでいえば0DB，位相でいえば-180度を意味する．したがって，ボード線図で位相が-180度のときの0dBとゲイン線図との隔たりがナイキスト線図における(-1,j0)点までの距離に対応しており，これをゲイン余裕とよぶ．すなわち，ナイキスト線図が(-1,j0)点と重なるまでゲインKを何倍にできるか，その値のデシベル表示である．
位相が-180度の点でゲインが0dBより大きければ，ナイキスト線図での(-1,j0)点の外側を意味し，制御系は不安定である．同様に，ゲイン線図と0dBラインとの交点の周波数における位相線図の-180度までの隔たりを位相余裕とよぶ．ゲインが0dBということは，ナイキスト線図では半径が1の単位円を意味しており，ナイキスト線図ではベクトル軌跡と単位円との交点を意味する．この点においてもし位相が180度以上遅れていたらその制御系は不安定である．これはナイキスト線図で単位円とベクトル軌跡の交点が(-1,j0)点より上側であることに対応する．
根軌跡法
根軌跡法は安定判別法の範疇というより，むしろ閉ループ制御系設計法のひとつであると考えるほうが良いだろう．しかしながら，根軌跡法が意味するところは，じつはナイキスト線図やボード線図がいみするところと本質的に同じである．
根軌跡とは，制御系の特性方程式
1+KGH=0
において，ゲインKを0→∞まで変化させたときの特性根の変化を複素平面上にプロットしたものにほかならない．ラウスやフルビッツの安定判別法がゲインKに対して，特性根を直接求めることなく正の実部を有する特性根が存在しないための必要十分条件を与えているのに対して，根軌跡はすべてのKに対して特性根を計算し，その軌跡を複素平面上にプロットしたものである．
この軌跡が複素平面上の右半平面に出た瞬間に制御系は不安定化する．このときのゲインが安定限界値である．
根軌跡の性質
1. 根軌跡は実軸に関して対称である．
2. 根軌跡は一巡伝達関数の極にはじまり，零点または無限遠点でおわる．
3. 根軌跡の分岐の数は一巡伝達関数の極の数，これを特性方程式の次数という，に等しく，極の数と零点の数の差が無限遠点に向かう根軌跡の数である．
以前は，一般の特性方程式に対して，すべての根を計算することは非現実的であったが，近年のCPUパワーの増大により，複雑な特性方程式に対しても根軌跡をたちどころに描けるようになった．
自由システムと自律システム
システムへの入力がないシステムを自由システムという。自由システムが安定であることをシステムが内部安定性を有するという。
システムが時間によって陽に変化しない（システムの構造が時間によって不変な）システムを自律システムという。任意の時刻を時間軸の原点に設定し直すことができる。
ノッチフィルタ
提案する制御手法の有効性を確認するために， [Condition I] ノッチフィルタを適用しない場合（図3）と[Condition II] ノッチフィルタを適用した場合（図4）を比較する実験を行なった．モデルを動作させる方向は体軸方向に相当する並進1軸として，実際のヒトの腎臓動作をモデルに入力した．結果をTable 1に示す．計測時間は30 s とした．Table 1から，ノッチフィルタの利用により追従精度が0.3mm程度向上することが確認される．また，図3,4から，システムの振動がノッチフィルタを適用することにより，抑制されることが確認される．これらの振動は，主に，システムの共振周波数に基づく振動である
We conducted an experiment to confirm the proposed oscillation reduction filter (Notch filter), which is based on the identification of the mechanical oscillation part. [Condition I] Without Notch filter, [Conditon II] With Notch filter (Fig.\ref).
プローブ操作の目的に応じてインピーダンス・パラメータを適正化
カルマンフィルタ
（参考文献）
[1] http://www1.accsnet.ne.jp/~aml00731/kalman.pdf
パーティクル・フィルタ
1. 各パーティクルの次の座標を適切な運動モデル（運動方程式等）にもとづいて予測する。運動モデルのもっとも簡単なものとして等速直線運動が用いられることが多い。
2. 予測値と実測値の誤差（誤差の大小が尤度をあらわす）を評価，これにもとづいて各パーティクルに対する重み（重要度）を更新。
3. ２でもとめた重みにもとづいて，パーティクルをリサンプリング（再度サンプリング）する。得られた分布のたとえば最頻値を予測値とする。
（参考文献）
[1] http://www.hiroict.com/misato/hiro_ict_techs/index9/link:9
隠れマルコフモデル
（例）動画中の追跡対象物の位置データ系列がそのデータ系列の裏側に隠れて推移しているモデル。
（参考文献）
[1] http://www.hiroict.com/misato/hiro_ict_techs/index9/link:9
（参考文献）
[1] 清水年美, 柔軟構造を含む機械システムの運動と振動を同時に制御可能な制御系設計法.
http://www.ccr.gunma-u.ac.jp/4u/Seeds/Pdf/2012/Ibaraki/Vol05_P149_150_I_Mono_Shimizu.pdf
制御目的
制御目的には位置をあわせることと速度をあわせることの2つがあるだろう。一般に，位置と速度を同時に制御することはできない。
 |
| Two control purposes. |
古典制御理論と現代制御理論
両者をつなぐもの： 等速直線運動
連続時間システムと離散時間システム
連続時間システムと離散時間システムが混在する。
サンプラ（連続時間システムを離散時間システムに接続）
1次ホールド（連続時間システムを→離散時間システムに接続）
両者をつなぐもの： 双一次変換
力制御
位置制御ベースの長所として、力制御は既存の位置制御系に組み込みやすい。
短所として、力制御特性が内部ループの帯域に左右され、剛性の高い環境下では、安定化しにくい。位置制御ベースの力制御系は産業用機械などの減速比が大きなシステムに適用しやすい。減速比が大きい場合、動力学補償を考慮しなくてよくなるため、制御ループを単純化することができる。
他方、トルク制御ベース力制御の長所は内部ループに左右されずに力制御特性を付加できることである。短所は、動力学補償を行なうため、制御ループが複雑になってしまう点である。ダイレクト・ドライブ・モータなどを用いた減速比が比較的小さなシステムにおいて有用である。
満員電車のなかで、押されたらその力に応じて奥につめるように、環境から印加される外力に対して柔軟に対応したい場合がよくある。柔軟に対応できることの指標としては力に応じた変位、コンプライアンスc[N/mm]が人体に対する安全・安心・思いやりの環境を実現するうえで本質的に重要であろう。
ロバスト制御と適応制御のプラント自身のもつ曖昧さに対する対応の違い
ロバスト制御
プラントが変化しても（あるいは、あいまいな制御対象に対しても）安定であるように、十分な余裕をもって制御パラメータを設定する。プラントのシステムモデル同定の際の曖昧さが比較的許容されるロバストな制御手法といえる。制御パラメータは制御中に変更できない（一発勝負）。システムは単純に構成できる。サンプリング周波数は十分に短いことが要求される。
小型で軽量なシステム構築を高陸できるフレキシブル・アームは医療・福祉分野においても将来有望である。しかしながらフレキシブルアームの主要な欠点のひとつとして、振動問題が挙げられる。一般にロボットアームの運動方程式には無限の振動モードが存在するが、計算機の都合上これらすべてをモデル化することは一般に困難である。
このため有限次数（N次とする）で切り上げ、特定の周波数領域で制御モデルを構築する（トランケーション）ことになる。しかしながら上記のトランケーションが引き起こす問題点のひとつとして、無視されたN次以降の振動モードから不安定振動（スピルオーバ現象という）が生じ、これが無視できない場合が挙げられる。スピルオーバ現象が無視できない場合にはロバスト制御等を用いて安定化の対策をとる必要が生じる。
適応制御
プラントのシステム同定の際のあいまいさが比較的許容されない。プラントのパラメータを同定、制御パラメータを動的に変化させる。システムは複雑化する。サンプリング周波数は比較的長くても良い。同定結果がうまく現実一致しているうちはうまく機能するが、同定結果が現実と異なる場合に動作が不安定化しやすく、タスクを失敗しやすい。この課題についてはモデルベーストな枠組みのなかでなんらかの対策が必要となる。
力制御
位置制御ベースの長所として、力制御は既存の位置制御系に組み込みやすい。
短所として、力制御特性が内部ループの帯域に左右され、剛性の高い環境下では、安定化しにくい。位置制御ベースの力制御系は産業用機械などの減速比が大きなシステムに適用しやすい。減速比が大きい場合、動力学補償を考慮しなくてよくなるため、制御ループを単純化することができる。
他方、トルク制御ベース力制御の長所は内部ループに左右されずに力制御特性を付加できることである。短所は、動力学補償を行なうため、制御ループが複雑になってしまう点である。ダイレクト・ドライブ・モータなどを用いた減速比が比較的小さなシステムにおいて有用である。
満員電車のなかで、押されたらその力に応じて奥につめるように、環境から印加される外力に対して柔軟に対応したい場合がよくある。柔軟に対応できることの指標としては力に応じた変位、コンプライアンスc[N/mm]が人体に対する安全・安心・思いやりの環境を実現するうえで本質的に重要であろう。
ロバスト制御と適応制御のプラント自身のもつ曖昧さに対する対応の違い
ロバスト制御
プラントが変化しても（あるいは、あいまいな制御対象に対しても）安定であるように、十分な余裕をもって制御パラメータを設定する。プラントのシステムモデル同定の際の曖昧さが比較的許容されるロバストな制御手法といえる。制御パラメータは制御中に変更できない（一発勝負）。システムは単純に構成できる。サンプリング周波数は十分に短いことが要求される。
小型で軽量なシステム構築を高陸できるフレキシブル・アームは医療・福祉分野においても将来有望である。しかしながらフレキシブルアームの主要な欠点のひとつとして、振動問題が挙げられる。一般にロボットアームの運動方程式には無限の振動モードが存在するが、計算機の都合上これらすべてをモデル化することは一般に困難である。
このため有限次数（N次とする）で切り上げ、特定の周波数領域で制御モデルを構築する（トランケーション）ことになる。しかしながら上記のトランケーションが引き起こす問題点のひとつとして、無視されたN次以降の振動モードから不安定振動（スピルオーバ現象という）が生じ、これが無視できない場合が挙げられる。スピルオーバ現象が無視できない場合にはロバスト制御等を用いて安定化の対策をとる必要が生じる。
（参考文献）
[] 歩行者による吊床版橋振動のH∞制御理論によるアクティブ制御
https://www.jstage.jst.go.jp/article/jscej1984/1997/577/1997_577_153/_pdf適応制御
プラントのシステム同定の際のあいまいさが比較的許容されない。プラントのパラメータを同定、制御パラメータを動的に変化させる。システムは複雑化する。サンプリング周波数は比較的長くても良い。同定結果がうまく現実一致しているうちはうまく機能するが、同定結果が現実と異なる場合に動作が不安定化しやすく、タスクを失敗しやすい。この課題についてはモデルベーストな枠組みのなかでなんらかの対策が必要となる。
Riccati方程式と観測・制御問題
Riccatiの方程式は２次の非線形項を含む微分方程式
Riccatiの方程式は２次の非線形項を含む微分方程式
制御に関する医デジ化コア基盤技術
医デジ化を推進・展開するにあたってわれわれが独自に開発・蓄積してきた制御に関するコア基盤技術について概説する。人体との安定的な接触動作やなめらか、高速、ならびに高精度なロボット動作制御技術に特徴がある。

（コラム）
一般にシステムが利用価値をもつためには、まず安定であることが必須の要件である。医療ロボットにおいてもこれは同様であり、すくなくとも目標位置（平衡点）周辺領域における局所的な安定性を確保することを考える必要がある。つぎに（追従）精度や大域的安定性（ロバスト性）を高めることを考えることになる。
安定性の評価指標としては、リアプノフの意味での発散しないという意味での安定性、目標位置に収束するという意味での局所的漸近安定性、大域的漸近安定性などがあげられる。
上記を踏まえて提案する医療支援システムおよびその制御手法の適用範囲について議論するために，安定性について議論する。
因果律
剛体系の運動では、『力→加速度→速度→位置』の順に変化が伝わる。同様に、電磁気系では力を生じるのに先だって電流や磁場の変化がなければならない。これらは因果（いんが）律として扱うことができる。
電動モータで駆動されるロボットにおいては、上記の剛体系の因果律と電磁気系の因果律とが組み合わさったものとして扱うことができる。
マスタ・スレーブ・システムにおいては基本的に、『マスタの操作入力→スレーブの動作出力』の順に変化が伝わる。スレーブ側の反力呈示については、『スレーブと環境の接触→スレーブ側における反力の発生→マスタ側における医師への反力提示』の順に因果律にもとづいて変化が伝わる。
ベッドに横たわっている人を超音波プローブで診断している状況を考えよう。プローブとベッド（の天板）に挟まれた人体に加わる力のうち、医師がプローブを介して人体（の表面）を押す力と（質点からなる人体各部の）重力は能動的な力（既知力）とよばれる。
重力の影響については、ベッド（の天板）近くになればなるほど、人体各部はより多くの重力に起因する既知力を受けることになり、より圧縮・変形することとなる。
ベッド（の天板）が人体を押し返す力は能動的な力によって人体とベッド（の天板）との拘束条件が発生する力であり、受動的な力である。これは既知力と拘束条件から決定される。このため、未知な力あるいは拘束力とよばれる。ここでいう『未知』とは方程式を解いて求めるべき対象である『未知数』に近い意味で用いられている『未知』である。
このとき人体各部は既知力と拘束力のつり合いによって運動はしない（横隔膜や心臓の運動についてはここでは無視する）が、発生する内力（押しつけ力および人体各部の重力）によって変形はしている。
参考文献
[] 内山 勝，中村仁彦，ロボットモーション，岩波書店，2004.
ロボットの運動軌道計画および制御
ロボットの運動軌道計画においては、2階連続微分可能であることを運動軌道の条件として要求することが多い。運動量がmvによって表されることからもわかるように、速度をなめらかに制御することがロボットの運動軌道制御における本質であるからだ。
２階連続微分可能とは、２回微分したものがこれまた、連続な関数になっているという意味である。ロボットの関節角の２回微分は関節角加速度であり、関節角加速度に連続的な指令値を与えて関節角速度をなめらかに制御することができる。
ロボットのモーションコントロールボードは一般にこのような運動軌道制御が可能な仕様になっている。医療ロボットにおいては特に、安全・安心・安定的で思いやりのある動作がシステムに求められている。これを踏まえて運動軌道を計画するにあたってなるべく急激な速度変化（高周波動作）は避けるべきである。
(参考文献)
内山 勝，中村仁彦，ロボットモーション，岩波書店，2004.
Norihiro Koizumi, Shin'ichi Warisawa, Hiroyuki Hashizume, and Mamoru Mitsuishi, "Continuous path controller for the remote ultrasound diagnostic system," IEEE/ASME Trans. on Mechatronics (TMECH), Vol.13, No.2, pp.206-218, 2008, https://doi.org/10.1109/TMECH.2008.918530.
https://www.contec.com/jp/support/basic-knowledge/daq-control/motion-control/
システムの数式モデル化
システムの制御系$C(s)$を効果的に設計するためには，XYZステージ$P(s)$，機構部の振動特性$M(s)$，追従対象位置取得部（超音波診断装置および画像処理装置）$O(s)$の各推定値$\hat{P}(s)$，$\hat{M}(s)$, $\hat{O}(s)$を同定して数式モデルとする必要がある．
本システムではデジタル制御系として実装するため，系は離散的であるが，制御対象の運動を特徴づける周波数（0.1-0.5Hz程度\cite{AJN2001:D.Cordes}）と比較して，サンプリング周波数が十分に高いと考え，連続時間の伝達関数モデルとして各ブロックを表現する．本節では，このモデル化手法およびその結果について概説する．
離散には値の離散と時間の離散とがある\cite{2012YMori}．超音波画像にもとづくフィードバックシステムにおいてもこれは同様である．
フィードバック制御系
今日，多くのシステムがそうであるように，本システムにおいても，制御対象そのものは連続時間ベースで動作しているが，これを制御するためにデジタル制御装置を利用している．ディジタル制御装置を採用することで，制御系設計変更への対応が容易になる．
Although, the object for the control itself is an analog and continuous system, we apply an digital processing device to control our system, which is the same case as many other systems these days.
It makes easy to cope with the change of the design of the controller by adopting digital control device.
具体的に，本制御系は100 Hz 部分と1 kHz の部分が混在するマルチサンプリングレート系であり，離散系のシステムだが，制御系を構築するにあたって，
Specifically, our system is a digital controller, which has one control part at the sampling rate of 100 Hz and has another control part at the sampling rate of 1 kHz. In order to construct a digital controller for our system, we apply a method which has the following two steps.
まずは，連続時間システムの枠内で学問体系化されている制御系設計論を適用して，連続時間ベースで動作する制御装置のための制御則を求め，
In the first step, we construct a controller for analog system based on the control theory for continuous time systems.
つぎに，その制御則をデジタル制御装置に実装するためサンプリング周期Tを用いてデジタル化するという手法を採用する．本手法により，サンプリング周期が十分に短い場合には，十分な制御性能が期待できる．
In the second step, we digitize the controller by the sampling period $T$ so as to apply the controller to the digital control device. If the sampling period is set short enough, sufficient control performance is expectable by this method.
（注）接近法
ディジタル制御装置に実装する制御則を求めるにあたって，まずは，連続時間システムの枠内で学問体系化されている制御系設計論を適用して，連続時間ベースで動作する制御装置のための制御則を求め，その制御則をディジタル演算装置に実装するためサンプリング周期$T$を用いてディジタル化する．サンプリング周期がきわめて短い場合には，(連続時間系のモデルと離散時間系のモデルは接近するため)十分な制御性能を発揮することが期待できる．
（注）制御ブロック線図において，連続時間システムの伝達関数$G_c(s)$の前後にホールダとサンプラを付加した部分の入出力特性をパルス伝達関数$G_H(z)$で表すと，$G_H(z)$と$G_c(s)$の間に，つぎの関係式が成立する．
高次系の応答と2次系あるいは1次系への近似
多くの高次系システムの応答は，支配的な極である代表根を考慮することで，2次系あるいは1次系の応答に近似できる\cite{1998MATLABniyoru-seigyoriron-no-kiso:KNonami}．
一般に，虚軸に近い極に対応する成分は減衰が小さく，原点に近い極の係数は大きくなるので，$t = 0$ に近い部分を除きこれらが過渡応答の主要な部分を支配する．このような極を代表根と呼ぶ．
（フィードバック制御とコンプライアンス）~~~~~~~
PID制御によるフィードバック制御はソフトウェアによる仮想的なバネのアナロジーとして捉えることもできる。
前後の自動車は仮想的なバネでつながれている。自動運転によって仮想的なバネでつながれた車の群は蛇型ロボットと考えることもできる。
~~~~~~~（フィードバック制御とコンプライアンス）
安定判別の方法
受動性
システムへの入力に対して対象とする系全体のエネルギーが減衰することを安定であることの定義としようとする、観察による経験則にもとづいた考え方。
（参考文献）
[1] 坂本登, システムの受動性に関する研究, 1996.
http://ir.nul.nagoya-u.ac.jp/jspui/handle/2237/6437
リアプノフ安定性
エネルギーを用いて安定性を議論する。ほかの安定判別法では扱いが困難な非線形なシステムに対しても安定性を議論できるきわめて強力な理論。解軌道に沿って系（システム）のエネルギーが増加しない関数（リアプノフ関数とよばれる）を発見することにより、内部安定性（外部入力がない場合に系が安定であること）を確認しようという観察にもとづく考え方。
フィードバック制御系、インピーダンス制御系は入力に対していずれもエネルギーを消散する系なので、本質的にリアプノフ安定である。
（参考文献）
[] 志水清孝, フィードバック制御理論, コロナ社, 2013.
[] 小郷，美多：「システム制御理論入門」, p.79, 実教出版(1981)。
[] 動的システムの安定性と過渡応答
http://power.nagaokaut.ac.jp/convenience/pdffiles/stability.pdf
ボード線図は，ナイキスト線図と同等の情報を有する．制御系設計の際の実用性からいえば，ボード線図が最もよく用いられているといえるだろう．
フィードバック制御における摂動への対策
状態方程式の構造は，さまざまなノイズに起因する摂動的なノイズへの対処法に関して，重要な示唆を与えてくれる．
まず，連続系の表現を与える．
\begin{equation}
\begin{displaymath}
\left\{
\begin{array}{l}
\dot{\xi} = A_c \xi + B_c (r_f - y_w)\\
u_p = C_c \xi
\end{array}
\right.
\end{displaymath}
\label{dotxi = A_c \xi + B_c (r_f - y_w)}
\end{equation}
つぎに，上式を離散化すると，
\begin{equation}
\begin{displaymath}
\left\{
\begin{array}{l}
\xi[k+1] = A_c^d \xi[k] + B_c^d (r_f[k] - y_w[k])\\
u_p[k] = C_c^d \xi[k]
\end{array}
\right.
\end{displaymath}
\label{xi[k+1] = A_c^d xi[k] + B_c^d (r_f[k] - y_w[k])}
\end{equation}
現在の位置・速度 $\xi[k]$ に状態遷移行列$A_c^d$をかけた（１サンプリング時間進ませた）ものにゲインをかけて制御入力とする．ここで，$r_f[k]$はターゲットに対する追従目標位置，$y_w[k]$はロボットの位置（ロボット先端部の位置）出力である．
ここで，$A_c^d = e^{AT} \simeq I +TA + \frac{(TA)^2}{2 !} \cdots + \frac{(TA)^n}{n !} $でn次近似でき，サンプリング時間$T$が十分に短ければ，$A_c^d$は単位行列$I$に近づくことがわかる．このことは，サンプリング時間が短いと，$\xi[k]$と$\xi[k+1]$の差が極めて小さくなる（$\xi[k+1] \simeq \xi[k]$）という直観とも一致する．
システムそのものが，非線形的な出力を出す場合には，ロバスト安定化を考慮する必要がある．
本システムにおいても，超音波診断装置を画像処理して得られた3次元位置情報は多分に非線形的な出力を含む．
（参考）確率微分方程式によるアプローチ
begin{equation}
dy(t) = V \cdot dt + W \cdot dB(t)
\label{dyt = V cdot dt + W cdot dBt}
\end{equation}
まず，カルマンフィルタなどのオブザーバにおける確率差分方程式の利用について概観する．
カルマンフィルタはシステム雑音$w(k)$および観測雑音$v(k)$が無視できない場合によく用いられ，つぎの3つの機能を有する．
[機能1] 過去のデータの雑音を取り除くスムージング機能
[機能2] 雑音に乱された情報から真の値を推定するフィルタリング機能
[機能3] 未来の値を予測するフォアキャスティング機能
Q：システム雑音の共分散
R：観測雑音の共分散
Rを大きくみつもる→予測値を信用→なめらか
Rを小さくみつもる→観測値を信用→振動的
Qを小さくみつもる→相対的にRを大きくみつもる→予測値を信用→なめらか
Qを大きくみつもる→相対的にRを小さくみつもる→観測値を信用→振動的
バブルに汚染される前の標準的な最尤軌道を獲得しておき，これを利用して，追従の精度を高める方法が考えられる．
（参考文献）
[1] http://blog.livedoor.jp/k_yon/archives/52309233.html
[2] http://www1.accsnet.ne.jp/~aml00731/kalman.pdf
（参考）
高木 周，「機械系のための数学」 ，数理工学社，全231ページ，2005．の第８章 摂動法
（アキレスと亀）~~~~~~~
アキレスと亀の問題がある。アキレスが亀に追いついたときには亀はほんの先をいっており，ずっと追い越せないというパラドックスである。従来型のP制御のみを適用するとこの問題に直面することになる。そこで，患部運動モデルを構築して，将来の亀の位置を予測してこの地点に先回りしようというのがわれわれの提案法のコンセプトである。
~~~~~~~（アキレスと亀）
[1] 長沼伸一郎, 物理数学の直観的方法―理工系で学ぶ数学「難所突破」の特効薬〈普及版〉 (ブルーバックス), 2011.
オブザーバの構築
状態フィードバック制御を行なうためには，系の状態変数（たとえば，位置と速度）のすべてがセンサで計測可能であることが望ましいが，実際には，速度信号を正しく計測することは困難である．そこで，現実的には，センサ信号（出力）から状態変数を推定し，推定した状態変数を用いて状態フィードバック制御を行なうことになる．このための状態推定器をオブザーバという．すなわち，オブザーバとは，大雑把には，システムの入出力情報を用いて，運動（微分／差分）方程式にもとづいて状態変数を逐次的に推定するものと捉えて差し支えないであろう．
まず，オブザーバの一般系について概観する．
体表の運動から腎臓の運動を推定する
ここでは，超音波画像上で腎臓を見失ったときに，自動で追従状態に復帰することを目的として，『体表（おへそ）の運動（３次元磁気センサの値）』から『腎臓の運動』を推定する方法を提案する．
ここで，『腎臓』は主に，ｘおよびｚ方向に運動する．
一方，『体表（おへそ）』はｚ軸方向のみ運動する．これは，息を吸ったり，吐いたりしたときの自分のおへその動きを考えてみるとz方向には動くけれども，x方向やy方向には動かないことからわかるであろう．
ここで，『腎臓』のx,zの運動も『体表（おへそ）』のz方向の運動も，横隔膜の運動と同期しているため，これらの運動軌道は振幅が定数倍異なるだけで，同じような波形をしていることに着目されたい．これにより，体表の運動から腎臓の運動を推定することが可能になる．
最初の１分は，呼吸が安定するまでにすこし時間がかかることを考慮して，すこし長め（１分間）のデータをとり，呼吸が安定してきたら，すこし短めの４０秒ごとにデータを更新する．
呼吸の周期は人によって異なり，安静時の正常呼吸は，成人の場合，１分間に１２～２０回の頻度，４０秒だと８～１４回程度になる．
呼吸開始時には１２個以上のデータから運動モデルを作成，安定してきたら，８個以上のサンプルをとって，運動モデルを更新すれば，経験的に安定したモデルができる．
（脳内回路の抵抗）~~~~~~~
例えば，英語でのプレゼンであったりとか，CPUとメモリに余裕がない状態になることが多い．普段やりなれないことをやるときなど，おそらく脳内回路の抵抗（インピーダンス）は大きくなり、熱を発する状態になっているのだろう，，，この場合，インピーダンスを下げるためには，回路をよく利用するしかないのだろう，，，やればやるほどやりたくなるなど，のめり込みもこれで説明できそうだ，，，
~~~~~~~（脳内回路の抵抗）
(参考文献)
[1] Anshul Thakral, et al., Surgical Motion Adaptive Robotic Technology (S.M.A.R.T): Taking the Motion out of Physiological Motion, 2016.
http://link.springer.com/chapter/10.1007/3-540-45468-3_38
[] Ginhoux, R., et al., Beating heart tracking in robotic surgery using 500 Hz visual servoing, model predictive control and an adaptive observer, 2004.
http://ieeexplore.ieee.org/stamp/stamp.jsp?arnumber=1307163
周期的な生体運動のモデル表現
神経振動子の位相\phi_iを用いて臓器運動が記述できるとする考え方（仮定）。
[] 青井伸也, 生物の歩行と力学系, 数理科学, vol.626, pp.47-52, 2015.
同次変換行列（hOmogeneous transformAtion mAtrix）
同次変換行列．姿勢変換行列（rotAtion mAtrix）および並進ベクトル（translAtion vector）を要素として内包する。運用にあたって同次変換行列が直交行列ではない（転置しても逆行列にはならない）ことには注意が必要である。
軌道計画
アルキメデスの螺旋軌道の利用．
PTP制御とCP制御：
PTP (Point to Point)制御：軌道（経路）上の要所要所の点を（人が）与えることで軌道（経路）を生成する方法。点と点の間の軌道はサーボ系（システム）が決定する。Pick and Place作業などでよく用いられる。
CP制御（Continuous Path) 制御：運動の軌道（経路）を（人が）細かく与えることで連続的な軌道（経路）を計画する方法。連続的な経路の生成には直線補間、円弧補間、スプライン補間などが用いられる。PTP制御と比較して多くのメモリが必要になる。アーク溶接や障害物を回避しながらの作業などによく用いられる。
(インピーダンス制御)
[1] Kikuuwe, R. et al, Admittance and Impedance Representations of Friction Based on Implicit Euler Integration, Robotics, IEEE Transactions on, 2006.
http://ieeexplore.ieee.org/xpl/articleDetails.jsp?arnumber=4020366&queryText=Admittance%20and%20Impedance%20representaion%20of%20friction&newsearch=true
Kikuue, et al., studies an admittance and impedance representations for the haptic interfaces.
教示
ダイレクト・ティーチング：ロボットに動作を教示する際に、人間がロボットを直接手に取って教示するなど、人間を介在させる方法。
ティーチング・プレイバック：教示されたときの関節角のデータをもとに教示された動作を復元・再生する方法。
内部モデル原理
内部モデル原理はむだ時間を含まない系では定常偏差をゼロにすることを保証するが，
むだ時間を含む系では話はそれほど簡単にはゆかない．
パデ近似：
時間遅れを近似することができる。
最終値の定理
医療支援システムにおける内部モデル原理の拡張＝かゆいところに手がとどくシステム
＝医師が診たい位置に診たい姿勢からアプローチして適切な効果をおよぼす．
機能関数発生器（医師）と同じ機能関数をシステムの機構・制御・画像処理アルゴリズム上にもつことに対応する．
（注意）アクチュエータを実装する際は，まず，必要最小限のアクチュエータに絞ることを考えられたい．制御の追従誤差の観点から，動作させるモータは少ないほうがよいからだ．また，能動的なアクチュエータの整定時間を考慮すれば，パッシブな機構を採用したほうが有利な場合も多いだろう．
5.7 画像処理装置からXYZステージの制御系への送信データ
画像処理装置からXYZステージの制御系に送信するデータは下記の6点である．本データはLANを介してソケット通信により送受信することを想定している．そのため，送受信のためのデータ量の適正化などについては特に考慮していない．しかしながら，遠隔システムで，通信帯域に制限がある場合などは，特別の考慮が必要であろう\cite．
1. Dx, Dy, Dz：患部と焦点の相対位置（超音波画像により同定）
2. Kp, Kd：PDゲイン
3. Td：時間遅れ情報（画像取込みの際のむだ時間＋画像処理時間）
4. m_est_posX, m_est_posY, m_est_posZ：上記の1のデータのカルマンフィルタによる濾波位置推定値
5. m_est_velX, m_est_velY, m_est_velZ：上記の1のデータのカルマンフィルタによる濾波速度推定値
6. m_corrVal[2]：中央（超音波診断装置のバイプレーン･モードにおける左画面）および側面プローブ（同右画面）の画像相関値
（コラム：止まったエスカレータは歩きにくい）
止まったエスカレータは歩きにくい。これはわれわれの頭のなかにエスカレータのモデルができているからである。すなわち，これまでの経験的学習によってわれわれはエスカレータのモデルを頭のなかに構築してきているのである。
時間遅れ
制御系には，かならず時間遅れが存在する．本システムにおいてもこれは同様である．

NIUTSにおけるむだ時間
1. 画像の取込みの際のむだ時間．ダブルバッファリングであればサンプリング時間遅れる．われわれのシステムでは，たとえば10ms程度．
2. 超音波画像処理の際の演算時間遅れ．われわれのシステムでは，たとえば5ms程度．
3. 伝送時間遅れ．特に，遠隔地への伝送やインターネットを介した伝送などでは，問題が深刻化する．われわれのシステムでは，たとえば1ms程度．
4. ロボットの制御における応答時間遅れ．1次遅れシステムの場合は時定数程度として見積もる．われわれのシステムでは，たとえば1ms程度．
ロボットの制御における応答時間に関するパラメータを下記に示す．
1. 立上がり時間 - 出力が定常値の10%から90%に達するまでの時間.
2. 遅れ時間 - 出力が定常値の50%に達するまでの時間.
3. 整定時間 - 出力が定常値の許容誤差範囲内に達するまでの時間.
4. 行過ぎ時間 - 出力が最大行過ぎ量（定常値または目標値を超えて現れる最初の最大値）に達する時間.
$t$領域で入力に対して出力が$T_d$ s 遅れる入出力関係を有する要素は時間遅れ要素と呼ばれる．時間遅れ要素は非線形要素の一つであり，その取扱いには注意を要する．
時間遅れ要素の位相は周波数に比例して遅れるため，時間遅れ要素がフィードバックループ内に存在すると，制御系の安定性に影響を与える．
時間遅れ要素を含むシステムの周波数応答を求める方法には以下の2つがある．
（方法1） まず，時間遅れ要素以外の伝達要素の周波数伝達関数からゲインと位相を求め，つぎに位相だけを遅らせればよい．しかしながら，この方法には煩雑な場合があるという短所がある．
（方法2） パデ近似を用いると時間遅れ要素を代数的な伝達関数として扱うことができる．ここで，パデ近似の次数を高くすると，モデルが正確になり，近似モデルの位相誤差が小さくなる．他方，次数を高くしすぎるとモデル全体の次数も高くなってしまい，解析も煩雑になってしまうため，解析対象の制御帯域での位相誤差が顕著にならない程度の次数を選定する．
Simulinkの時間遅れ要素（transport delay）をlinmod
機構振動部の同定
In this chapter, we propose a mechanical system identification method including the mechanical oscillation part for the Non-Invasive Ultrasound Theragnostic System (NIUTS). NIUTS tracks and follows the movement in an affected area to be treated (kidney stones, in the present study) by irradiating the area with high-intensity focused ultrasound (HIFU). Blur noise caused by oscillation of the mechanical systems deteriorates the servoing performance. To cope with this problem and enhance the servoing performance, it should be required at first to identify the mechanical oscillation part.
本システムにおいては，超音波画像取得および画像処理の際に発生するむだ時間がフィードバック系におけるメインのむだ時間要素になる．
（例）
たとえばメインのむだ時間が10ms，メインの周波数成分が1Hzだとすると，むだ時間が位相おくれに与える影響は下記で計算できる。
位相遅れ=むだ時間／メインの周波数成分の周期～2π×10ms/1s～2π×10^{-2} rad
位相余裕はこの位相遅れ分だけなくなることになる．
（参考文献）
[1] Keehoon Kim, Çavuşoğlu, M.C., Wan Kyun Chung, Quantitative Comparison of Bilateral Teleoperation Systems Using μ-Synthesis, Robotics, IEEE Transactions on
Year: 2007, Volume: 23, Issue: 4, Pages: 776 - 789, DOI: 10.1109/TRO.2007.900625
http://ieeexplore.ieee.org/xpl/articleDetails.jsp?arnumber=4285849&newsearch=true&queryText=quantitative%20comparison%20%20keehoon
[2] Tanner, N.A., et al., High-frequency acceleration feedback in wave variable telerobotics, Mechatronics, IEEE/ASME Trans., Volume:11 Issue:2, 2006.
http://ieeexplore.ieee.org/xpl/articleDetails.jsp?arnumber=1618669&newsearch=true&queryText=High-Frequency%20acceleration%20feedback
Tanner propose a control method to transmit high frequency acceleration information to the operator independently of the low frequency force feedback.
安定判別法
ボード線図
ボード線図はナイキスト線図と同じ情報を有しており，制御系設計の実用性という観点からは，ボード線図がもっともよく利用されている．ナイキスト線図での(-1,j0)という点はゲインでいえば0DB，位相でいえば-180度を意味する．したがって，ボード線図で位相が-180度のときの0dBとゲイン線図との隔たりがナイキスト線図における(-1,j0)点までの距離に対応しており，これをゲイン余裕とよぶ．すなわち，ナイキスト線図が(-1,j0)点と重なるまでゲインKを何倍にできるか，その値のデシベル表示である．
位相が-180度の点でゲインが0dBより大きければ，ナイキスト線図での(-1,j0)点の外側を意味し，制御系は不安定である．同様に，ゲイン線図と0dBラインとの交点の周波数における位相線図の-180度までの隔たりを位相余裕とよぶ．ゲインが0dBということは，ナイキスト線図では半径が1の単位円を意味しており，ナイキスト線図ではベクトル軌跡と単位円との交点を意味する．この点においてもし位相が180度以上遅れていたらその制御系は不安定である．これはナイキスト線図で単位円とベクトル軌跡の交点が(-1,j0)点より上側であることに対応する．
根軌跡法
根軌跡法は安定判別法の範疇というより，むしろ閉ループ制御系設計法のひとつであると考えるほうが良いだろう．しかしながら，根軌跡法が意味するところは，じつはナイキスト線図やボード線図がいみするところと本質的に同じである．
根軌跡とは，制御系の特性方程式
1+KGH=0
において，ゲインKを0→∞まで変化させたときの特性根の変化を複素平面上にプロットしたものにほかならない．ラウスやフルビッツの安定判別法がゲインKに対して，特性根を直接求めることなく正の実部を有する特性根が存在しないための必要十分条件を与えているのに対して，根軌跡はすべてのKに対して特性根を計算し，その軌跡を複素平面上にプロットしたものである．
この軌跡が複素平面上の右半平面に出た瞬間に制御系は不安定化する．このときのゲインが安定限界値である．
根軌跡の性質
1. 根軌跡は実軸に関して対称である．
2. 根軌跡は一巡伝達関数の極にはじまり，零点または無限遠点でおわる．
3. 根軌跡の分岐の数は一巡伝達関数の極の数，これを特性方程式の次数という，に等しく，極の数と零点の数の差が無限遠点に向かう根軌跡の数である．
以前は，一般の特性方程式に対して，すべての根を計算することは非現実的であったが，近年のCPUパワーの増大により，複雑な特性方程式に対しても根軌跡をたちどころに描けるようになった．
自由システムと自律システム
システムへの入力がないシステムを自由システムという。自由システムが安定であることをシステムが内部安定性を有するという。
システムが時間によって陽に変化しない（システムの構造が時間によって不変な）システムを自律システムという。任意の時刻を時間軸の原点に設定し直すことができる。
ノッチフィルタ
提案する制御手法の有効性を確認するために， [Condition I] ノッチフィルタを適用しない場合（図3）と[Condition II] ノッチフィルタを適用した場合（図4）を比較する実験を行なった．モデルを動作させる方向は体軸方向に相当する並進1軸として，実際のヒトの腎臓動作をモデルに入力した．結果をTable 1に示す．計測時間は30 s とした．Table 1から，ノッチフィルタの利用により追従精度が0.3mm程度向上することが確認される．また，図3,4から，システムの振動がノッチフィルタを適用することにより，抑制されることが確認される．これらの振動は，主に，システムの共振周波数に基づく振動である
We conducted an experiment to confirm the proposed oscillation reduction filter (Notch filter), which is based on the identification of the mechanical oscillation part. [Condition I] Without Notch filter, [Conditon II] With Notch filter (Fig.\ref).
プローブ操作の目的に応じてインピーダンス・パラメータを適正化
制御目的
制御目的には位置をあわせることと速度をあわせることの2つがあるだろう。一般に，位置と速度を同時に制御することはできない。

Two control purposes.
古典制御理論と現代制御理論
両者をつなぐもの： 等速直線運動
連続時間システムと離散時間システム
連続時間システムと離散時間システムが混在する。
サンプラ（連続時間システムを離散時間システムに接続）
1次ホールド（連続時間システムを→離散時間システムに接続）
両者をつなぐもの： 双一次変換
力制御
位置制御ベースの長所として、力制御は既存の位置制御系に組み込みやすい。
短所として、力制御特性が内部ループの帯域に左右され、剛性の高い環境下では、安定化しにくい。位置制御ベースの力制御系は産業用機械などの減速比が大きなシステムに適用しやすい。減速比が大きい場合、動力学補償を考慮しなくてよくなるため、制御ループを単純化することができる。
他方、トルク制御ベース力制御の長所は内部ループに左右されずに力制御特性を付加できることである。短所は、動力学補償を行なうため、制御ループが複雑になってしまう点である。ダイレクト・ドライブ・モータなどを用いた減速比が比較的小さなシステムにおいて有用である。
満員電車のなかで、押されたらその力に応じて奥につめるように、環境から印加される外力に対して柔軟に対応したい場合がよくある。柔軟に対応できることの指標としては力に応じた変位、コンプライアンスc[N/mm]が人体に対する安全・安心・思いやりの環境を実現するうえで本質的に重要であろう。
ロバスト制御と適応制御のプラント自身のもつ曖昧さに対する対応の違い
ロバスト制御
プラントが変化しても（あるいは、あいまいな制御対象に対しても）安定であるように、十分な余裕をもって制御パラメータを設定する。プラントのシステムモデル同定の際の曖昧さが比較的許容されるロバストな制御手法といえる。制御パラメータは制御中に変更できない（一発勝負）。システムは単純に構成できる。サンプリング周波数は十分に短いことが要求される。
小型で軽量なシステム構築を高陸できるフレキシブル・アームは医療・福祉分野においても将来有望である。しかしながらフレキシブルアームの主要な欠点のひとつとして、振動問題が挙げられる。一般にロボットアームの運動方程式には無限の振動モードが存在するが、計算機の都合上これらすべてをモデル化することは一般に困難である。
このため有限次数（N次とする）で切り上げ、特定の周波数領域で制御モデルを構築する（トランケーション）ことになる。しかしながら上記のトランケーションが引き起こす問題点のひとつとして、無視されたN次以降の振動モードから不安定振動（スピルオーバ現象という）が生じ、これが無視できない場合が挙げられる。スピルオーバ現象が無視できない場合にはロバスト制御等を用いて安定化の対策をとる必要が生じる。
（参考文献）
歩行者による吊床版橋振動のH∞制御理論によるアクティブ制御
https://www.jstage.jst.go.jp/article/jscej1984/1997/577/1997_577_153/_pdf
佐伯正美，制御工学－古典制御からロバスト制御へー，朝倉書店．
適応制御
プラントのシステム同定の際のあいまいさが比較的許容されない。プラントのパラメータを同定、制御パラメータを動的に変化させる。システムは複雑化する。サンプリング周波数は比較的長くても良い。同定結果がうまく現実一致しているうちはうまく機能するが、同定結果が現実と異なる場合に動作が不安定化しやすく、タスクを失敗しやすい。この課題についてはモデルベーストな枠組みのなかでなんらかの対策が必要となる。
代数的Riccati方程式と観測・制御問題
カルマンフィルタ（最適観測）問題と最適制御問題
現代制御理論において本質的な最適レギュレータやカルマンフィルタ（最適オブザーバ）においてはRiccatiの方程式がキーとなる方程式として現れる。これはロバスト制御理論をはじめ、さまざまな制御の問題においても同様である。
Riccatiの方程式は２次の非線形項を含む微分方程式であるが、特殊解がみつかれば１階の微分方程式に帰着でき、人間にとって直観的に見通しのよい線形代数的な演算で解を求めることができる（ある点まわりの直線の最適勾配探索の問題に帰着できる）。『Riccati』は『リ～・カッティ』と発音するのが原音に近い。Riccatiの方程式はヤコポ・リッカチ（Jacopo Riccati）が1720年頃に研究した。余談だが、お笑い芸人のなかやまきんに君の『アーノルド・シュワルツェネッガー』の発音が参考になる。
カルマンフィルタの理論は観測値から推定される状態量はシステム雑音や観測雑音に由来する確率的な誤差を分布として包含している、言わば『あるばらつき（分布）を誤差として包含する状態量』であると捉える世界観である。すなわち、見えたり見えなかったりするひとつひとつの小魚（観測値）の影に翻弄（ほんろう）されるのではなく、その本質たる魚影群の中心（観測値の分布の中心）を大局的に追いかけることで、ばらつきを内包する画像追跡の観測結果に対して運動方程式を考慮した安定的な状態推定値にもとづく追従を実現しようとするものである。これにより、物理的なロボティック追従をより、安全・安心・安定的で思いやりのあるものにすることができる！
カルマンフィルタは状態方程式と同様に行列・ベクトルを係数としてもつ一次方程式系として捉えることができる。このとき、上記の一次方程式系はシステムのダイナミクスを差分方程式の形で表現している。カルマンフィルタではこのモデルを用いて、一（数）サンプル時刻先の値を予測するとともに、観測値とのずれを逐次的に修正してゆくことができる、言わば『（ダイナミクス・モデルにもとづくシミュレーション・）予測と観測値とのずれを逐次（ちくじ）的に修正する式』と捉える世界観が重要である。
カルマンフィルタ（観測可能性）や最適レギュレータ（制御可能性）など、観測と制御とは双対（そうつい）の関係として捉えることができる。すなわち、観測と制御に関する議論はまったく平行に（対にして）進めることができる。
カルマンフィルタリング問題とは、時系列データ（y[i], i=1,…, k）にもとづいて、状態（x[k]）の平均二乗誤差（MSE：Mean Square Error）の最小値を与える推定値、すなわち最小平均二乗誤差推定値（MMSE：Minimum Mean Square Error）を見つける問題である。
カルマンフィルタの大きな特長は、あらたな観測値が入るたびに漸化式を用いて逐次的に状態推定値を時間更新できることである。出力予測誤差はその二つ名をイノベーション過程（innovation process）といい、現時刻の観測値に含まれている、『予測でどうにかなる範囲を超えた最新の情報』をあらわす。イノベーション過程は1960年代半ばにT.Kailathによって命名された。
Riccati方程式は最適観測出力行列（H）→最適制御入力行列（G）を求めるうえでキーとなる方程式である。カルマンフィルタや最適レギュレータの理論はさらにロバスト制御理論へと発展してゆく。
P=YX^{-1}。
すなわち、観測および制御の両分野にまたがって重要な方程式である。この式の本質は（直線Y=PXの）最適な勾配Pの探索にある！
状態方程式において、（A, b）が可制御で、（c, A）が可観測であれば、Riccati方程式の正定値解が存在し、定常カルマンフィルタが漸近（ぜんきん）安定であることが知られている。実際には、可制御の条件は可安定、可観測の条件は可検出にゆるめてよい。
有本-Potterの方法：
有本、PotterはRiccati方程式の解法をRiccati方程式の係数行列によって構成される行列Aの固有値問題へと帰着した。
行列Aが安定（反安定）であるとは、その行列の固有値（の住み処）が複素平面左（右）半面に存在していることをいう。
事前の期待値に観測値（見えたり見えなかったりする魚影）の（そこに魚が潜んでいるであろう）観測期待値からのずれを観測値の分布（観測される魚影群の分布）モデルを更新するとともにこれを考慮して加えることにより、事後の期待値に反映する。
毎回の観測値の観測期待値からのずれを踏まえてカルマンゲインを更新することにより、正規誤差分布モデルのウェスト部分をそのつど引き締め（ゆるめ）ている。日々のウエスト部分の増減に対するベルトの穴の調整と同様である。
定常過程のみならず、非定常過程に対しても適用可能である点がカルマンフィルタの特長のひとつである。これは、カルマンフィルタのアルゴリズムが逐次処理的であることによる。ここで、一般の非定常時系列に対してカルマンフィルタを適用するにあたっては、時刻kにおけるシステム雑音の分散および観測雑音の分散が時変である可能性について十分に考慮するべきである。
可制御と可観測、可安定と可検出
安定的に患部を抽出・追従・モニタリングできることが医療支援システムに求められている。このとき、観測値が発散せず、安定的に計測できることはシステムの制御においてきわめて重要である。また、安定的な制御ができれば安定的な医用画像の獲得、観測値の計測にも資する（観測と制御の双対性！）。
（参考文献）
足立修一, 丸田一郎, カルマンフィルタの基礎, 東京電機大学出版局, 2012.
パーティクル・フィルタ
1. 各パーティクルの次の座標を適切な運動モデル（運動方程式等）にもとづいて予測する。運動モデルのもっとも簡単なものとして等速直線運動が用いられることが多い。
2. 予測値と実測値の誤差（誤差の大小が尤度をあらわす）を評価，これにもとづいて各パーティクルに対する重み（重要度）を更新。
3. ２でもとめた重みにもとづいて，パーティクルをリサンプリング（再度サンプリング）する。得られた分布のたとえば最頻値を予測値とする。
（参考文献）
[1] http://www.hiroict.com/misato/hiro_ict_techs/index9/link:9
隠れマルコフモデル
（例）動画中の追跡対象物の位置データ系列がそのデータ系列の裏側に隠れて推移しているモデル。
ブラウン運動（Brownian motion）の数学的に厳密なモデルはウィーナー過程（Wiener process）と呼ばれる。
（参考文献）
[1] http://www.hiroict.com/misato/hiro_ict_techs/index9/link:9
（参考文献）
[1] 清水年美, 柔軟構造を含む機械システムの運動と振動を同時に制御可能な制御系設計法.
http://www.ccr.gunma-u.ac.jp/4u/Seeds/Pdf/2012/Ibaraki/Vol05_P149_150_I_Mono_Shimizu.pdf
正規分布
半径Σ（２次元の場合はσ）の球と等価な体積（２次元の場合は面積）を有するひし体（２次元の場合はひし形）（l1ノルム空間における超球）の直径で規格化、exponentialの中身はひし形の面積にマイナスをつけたもの。Me-DigIT Diamond（ひし体） Fujiが観れるのはまれである！
行列の分解
コレスキー分解
正定値対称行列→平方根（下三角）行列
UD分解
特異値（Singular Value）分解
システムの制御可能性と観測可能性
（アクチュエータによる）制御入力システム・ゲインとその自由システム・ゲインによる更新
および
（センサによる）観測出力システム・ゲインとその自由システム・ゲインによる更新
がいずれも縮退していない。
（参考文献）
制御におけるRiccati方程式
H∞制御はゴッドフレイ・ハロルド・ハーディ（Godfrey Harold Hardy, 1877年2月7日 - 1947年12月1日）が提案したハーディ空間をその土俵とする。
ハーディ空間
リプシッツ連続の条件
リプシッツ連続（Lipschitz continuity）の条件は呼吸・拍動等に応じて能動的に運動する臓器の抽出・追従・モニタリングを行なううえでも有用である。直観的には、リプシッツ連続函数は変化の速さが制限されるときにその最大値Lを与える。即ち、適当な有限値の実数Lが存在して、その函数のグラフ上の任意の二点を結ぶ直線の傾きの絶対値はその実数を超えないと考えることができる。この上界をその函数の「リプシッツ定数L」（あるいは一様連続度）と呼ぶ。たとえば、臓器の運動は瞬間最大速度L（腎臓の場合は20mm/sec程度）が制限されているため、このLを評価・同定することで関心探索領域（±L_x, ±L_y）の大きさを制限することができ、効率的な臓器の抽出・追従・モニタリングが可能になる。
リプシッツ連続（Lipschitz continuity）の条件は呼吸・拍動等に応じて能動的に運動する臓器の抽出・追従・モニタリングを行なううえでも有用である。直観的には、リプシッツ連続函数は変化の速さが制限されるときにその最大値Lを与える。即ち、適当な有限値の実数Lが存在して、その函数のグラフ上の任意の二点を結ぶ直線の傾きの絶対値はその実数を超えないと考えることができる。この上界をその函数の「リプシッツ定数L」（あるいは一様連続度）と呼ぶ。たとえば、臓器の運動は瞬間最大速度L（腎臓の場合は20mm/sec程度）が制限されているため、このLを評価・同定することで関心探索領域（±L_x, ±L_y）の大きさを制限することができ、効率的な臓器の抽出・追従・モニタリングが可能になる。
（参考文献）
[] 原岡喜重，はじめての解析学，講談社．
ルジャャンドル変換（Legendre Transformation）
運動ポテンシャルエネルギー（L:ラグランジュアン）はルジャンドル変換を用いて全エネルギー（H:ハミルトニアン）に変換することができる。運動ポテンシャルエネルギーの概念はラグランジュの運動方程式を導出するうえできわめて重要であるのでしっかり・くっきり・はっきりとそのアウトラインを理解しておきたい（ここでいうポテンシャルとは潜在的な運動仕事能力といったくらいの意味合いである）。ラグランジュの運動方程式においては『ポテンシャルエネルギーの減少分が慣性力の増加分に寄与している』というモノの観方・考え方・捉え方・発想の仕方・世界観が本質的に重要であろう。
 |
| ポテンシャルエネルギーの減少分による慣性力向上 |
基底の運動と中身（座標上の関数）の運動は逆方向、逆（再生）行列には随伴行列（転置行列）が関与
ビデオカメラを右に移動すると、中身の映像は逆方向に移動する。すなわち、基底の運動と中身（座標上の関数）の運動は逆方向となる。これは超音波プローブと超音波画像においても同様で、超音波プローブをある方向に並進あるいは回転運動すると、超音波画像内の像（中身）は逆方向に並進あるいは回転運動する。
われわれの体動補償型ロボティック超音波診断・治療基盤システムにおいては、このことを活用して、臓器内に埋め込まれた患部（ターゲット）の運動をキャンセルしている。具体的に、ターゲットが右に動けば、これをキャンセルするために超音波プローブを同じように右に動かし、時計回りに回転した際には、これをキャンセルするために超音波プローブを同じように時計回りに回転運動してやればよい。
ここで、直交（ユニタリ）行列を典型に逆（再生）行列にはその背後に転置行列（随伴行列A^T）が関与することに留意（りゅうい）されたい。
 |
| 基底の運動と中身（座標上の関数）の運動は逆方向、逆（再生）行列には随伴行列（転置行列）が関与している。 |
DCモータの制御
DCモータは単にモータの両端の電流を計測するだけでモータに印加しているトルクをモニタリングすることができる。この情報を用いれば、負荷トルクについてもある程度推定することができる（たとえばプローブと患部の間に印加されている接触力など）。
また、DCモータは、モータの両端の電圧を上げ下げして調整するだけで、どんな負荷トルクであっても任意の回転数でモータを回すことができる。具体的に角速度を検出するセンサと併用（へいよう）すれば制御量として電圧を上げ下げすることによって角速度（回転速度）についても簡便に制御することができる。
（参考文献）
２次遅れ系
通常のフィードバック系は１次遅れ系であるが、システムのムダ時間や通信の時間遅れが存在するため、２次遅れ系として扱うことが適切である。
（参考文献）
佐伯正美，制御工学－古典制御からロバスト制御へー，朝倉書店．
感度関数と相補感度関数によるフィードバック系の評価
（参照目標値入力に対する）感度関数と相補感度関数によってフィードバック系の性能を評価することができる。感度関数と相補感度関数は足し合わせると１になる性格のものなので、これらを同時に小さくすることはできない。上記を踏まえて、感度関数を低周波域において小さく、相補感度関数をノイズが支配的となる高周波域において小さく設計することが望ましい。
臓器運動に追従するロボティック超音波においても同様で、臓器運動の高周波域ではどうしてもノイズ成分が支配的となるため、これを無視してローパス（低周波域通過）フィルタを用いて、臓器運動の低周波域のみに特化して追従する戦略が有効である。また、こうすることによってロボットの振動がもたらす超音波画像のブレを抑制することができる。
