数式処理ソフトウェアで立式した運動方程式をSimulinkで実行する

この記事は制御工学アドベントカレンダー15日目の記事です．

制御工学で避けては通れないのが運動方程式の立式です．
プラントモデリングではもちろんのこと，現代制御に代表されるモデルベース制御では制御ロジックを構築するために運動方程式（状態方程式）が必要になります．

運動方程式の立式というと，ほぼラグランジュ法（オイラー＝ラグランジュ方程式）一択のような気がしています．

いまいちこのあたりの用語を掴みきれていないのですが，汎函数と呼ばれる“函数を変数とした函数”を変分（汎函数での微分）することで運動方程式を求めるといった流れです．

たとえば，$f(t) = at$は函数と呼ばれ，$\frac{df(t)}{dt}$という操作が微分と呼ばれることはご存知のとおりです．
それと似たような関係で，$I[f(t)] = Af(t)$を汎函数と呼び，$\frac{dI[f(t)]}{df(t)}$を変分と呼ぶようです．

物体の運動の場合，最も必要エネルギの少ない方向に物体が運動するという原理があるらしく（最小作用の原理？），以下の形式で与えられます．

$$I[ y ] = \int^{b}_{a}f( x, y, y' ) dx$$

特に運動の場合は，右辺$f(x,y,y')$をラグランジアン$L$というエネルギ量で与えるようです．
合っている自信はないですが，以下の式のようなイメージです．（時間微分なのでドットで書いています）

$$I[ x ] = \int^{b}_{a}L( t, x, \dot{x} ) dt$$

これはある時刻$a$から$b$までのエネルギの合計となります．
上の式が最小になるようにすれば良いということで，$I[x]$の変分（微分）がゼロとなるような$x$（停留値）を求めれば良いことになります．

この解（停留値）は決まっていて，これをオイラー＝ラグランジュ方程式と呼びます．
$$\frac{d}{dt}\left( \frac{\partial L}{\partial \dot{x}} \right) - \left( \frac{\partial L}{\partial x} \right) = 0$$

解の形式が決まっているのでシーケンシャルに解けて便利なのですが，非常に計算量が多いです．

前置きが長くなりましたがこれを数式処理ソフトウェアで楽にしようというのが今回のコンセプトです．

---

この記事では数式処理ソフトウェアとして以下を使用しています．

• wxMaxima（Windows版Maxima，無償）
• Symbolic Math Toolbox（MATLAB，有償）

恐らく運動方程式を求めたあとはMATLAB/SimulinkとかPyControlとかでシミュレーションすることになると思うのですが，結論としてはwxMaxima優位といった印象です．

---

今回，2重振り子を例として，Simulinkでのシミュレーションまでやってみようかと思います．

ラグランジュ法では物体の運動エネルギ，位置エネルギの関係を与えてあげることが必要になります．
逆に言うとエネルギまで出せてしまえばあとはソフトウェア任せにできます．

事前にやってみたところ，wxMaximaのインストールから結果確認まで45分程度で終わりました．

最適化函数を使って船を横移動させてみる

この記事は制御工学アドベントカレンダー9日目の記事です．
さいきん，仕事の方で船に関わることが多いので船の動きも面白いよ，と布教してみる．
船，とひとくちに言ってもタンカーなどの超大型船から客船，個人所有のクルーザー，漁船までいろいろ種類があります．
また，推進方法によっても船内にエンジンを積んだ船内機艇，エンジンごと取り外し可能な船外機艇，ジェット推進の艇などの種類があります．

ここではクルーザーなどに多く使われる下図のような船外機艇(Outboard motor)に焦点を当ててみます．

Mercury社のウェブページ
図の船は船外機が2つ取り付けられています（2機掛けと呼びます）が，最大で5つ取り付ける方もいるようです．
250馬力を5機掛け．パワーの面でも価格の面でも恐ろしいですね．

---

その船外機艇のなかでも面白いなあと思ったものがMercury社のSkyhookというJoystick操船システムです．
1分34秒あたりを見るとわかりやすいですが，着岸状態から船がほぼ真横に移動しています．

Mercury Joystick Piloting for Outboards
通常，船外機は何機掛けであろうと同じ方向を向かせて舵を切るのですが，この操船システムではそれぞれの船外機を独立させて反対方向に動かすことで横移動を実現しています．

このSkyhookを再現するため，最適化函数を使って遊んでみようというのがこの記事の趣旨です．
環境はMATLAB/Simulink R2015aです．

C/GMRESによる非線形モデル予測制御1

今回は珍しく2部構成で，本記事ではシステムの立式から評価函数の設定まで，次の記事では具体的な解き方について述べます．

第1部となる本記事ではC/GMRESの核となる部分には言及しないため，そちらについては2部以降を見ていただければと思います．

---

非線形モデル予測制御(Nonlinear Model Predictive Control; NMPC)とは，その名の通り”状態方程式が非線形なシステム”に対するモデル予測制御(Model Predictive Control; MPC)です．

最近流行りの制御則で，車の自動運転やロケットの再着陸システムにも使用されているそうです．

MyEnigmaさん
モデル予測制御(Model Predictive Control:MPC)の応用例

そもそも制約有りのMPCは非線形な制御系ですが，”非線形MPC”といった場合には“制御対象が非線形”な系を指すように思います．

線形のモデル予測制御については別途，記事に起こすつもりです．

通常，線形MPCでは，システムの表現に以下のような線形な状態方程式を使うのに対して，
$$\dot{x}(t) = Ax(t)+Bu(t)$$
非線形MPCでは状態方程式の形式を問わず使用でき，以下のような形式で表現されます．
$$\dot{x}(t)=f(x(t),u(t)) = \left[\begin{array}{c} x_{2}(t) \\ ax_{1}(t)+bx_{2}(t)u_{1}(t) \end{array}\right]$$

これはセミアクティブダンパの状態方程式ですが，式中に$x_{2}(t)u_{1}(t)$という“状態変数と操作量の積”が含まれています．少し考えるとわかりますが，線形の状態方程式に書き直すことはできません．

実際，このシステムを制御する際に単純に線形MPCを適用することはできません．
このような，非線形なシステムの制御を目的としたMPCが非線形MPCです．

非線形MPCの中にも種々ありますが，ここではC/GMRESによる非線形MPCについて取り上げます．

ただ，このシステムは減衰が操作量なのでダンパとして動作させること自体はPID等でも可能です．というよりずっとONにしておけばよいのです．しかし，できるだけ少ない操作量で最大の減衰を得よう，などと考え始めるとちゃんと制御する必要があります．

カルマンフィルタの基礎式を代数とベイズ定理から見る

カルマンフィルタの歴史ももう長いことと思いますが，各所で各人なりにまとめられているあたり，なかなか理解の難しいものなんだなあと感じています．
例に漏れず自分もカルマンフィルタの原理がいまいちピンとこなかったので自分なりにまとめてみました．

カルマンフィルタ(Kalman Filter)はフィルタと名付けられてはいますが，使われ方としてはフィルタよりも現代制御のオブザーバ（Observer; 状態推定器）に近いような気がします．
観測した情報から，状態ベクトルが真値に最も近くなるように推定するものです．

概念は以下のところでわかりやすく説明されています．

モノを作りたくなるブログ

シンプルなモデルとイラストでカルマンフィルタを直感的に理解してみる

ただ，数式として少し省略されているところもあったので，改めてまとめるに至りました．
基本的に，代数とベイズの定理のみから丁寧に導出していこうという感じです．

カルマンゲインの式の導出，予測値の算出，分散の更新は天下り的な部分も多いので，今回はこれらの導出に焦点を当ててみました．
統計的にみて，予測値と実プラントの誤差が最小となるように演繹的に求めていくとカルマンフィルタの式に行き着くあたり，統計学の応用先の代表例になっているだけのことはあるなあ，という感想です．

ここでは，状態空間と観測空間が同一の場合のみ扱います．

4ストロークエンジンの物理モデル

4ストロークのガソリンエンジンの物理モデリングをしてみました．
シミュレーションはMATLAB/Simulink，可視化ツールはProcessingです．

参考元はこのあたり．
双方ともに高知工科大学の磯村先生の文献です．

4 サイクルガソリンエンジンの動特性について

並列計算機によるエンジンのシミュレータ開発

---

以前にも倒立振子とかパーティクルフィルタでのグリッドマップマッチングとか2リンク平面マニピュレータとかのモデリングをやってきたのですが，そのどれとも色が違うなあ，ということで．

例によって，ラグランジュ法で運動方程式を立てます．
このあたりは参考文献の上の方が詳しいかな．

シンプルな外乱オブザーバ

外乱オブザーバ(Disturbance observer; DOB)はフィードバックシステムに外乱が加わるような状況で，比較的簡単に外乱を除去し安定化できる強力なツールですが，ウェブにはあまり記述が無いようだったので書いてみました．

• 外乱オブザーバとは？
• なぜ外乱オブザーバが必要なのか
• 基本原理
• 実際の外乱オブザーバ（ノミナルモデル，ローパスフィルタ）

---

そもそも外乱オブザーバとは？

“外乱”でググってみると，以下のような説明が得られます．

ある通信系に、所定の信号系以外から加わり妨害となる信号。 --- 大辞林

大体内容としては間違いないですが，今回扱うのは”通信系”に限らず，制御システムに対してはたらく，妨害となる信号くらいに留めておきます．

外乱オブザーバとは，このような外乱を除去するために，制御系のコントローラに組み込む補償器のひとつです．

---

なぜ外乱オブザーバが必要なのか

次のような1次遅れ系のフィードバックループを考えてみます．

ここで，$C(s)$はコントローラ，$G(s)$は制御対象です．また，目標値を$r$，出力を$y$で表します．
外乱というと，図の1, 2, 3番の箇所に加わることが想定されますが，本稿では1について記述します．

また，以下の理由で2, 3番を省いています．

2番の部分に入力される外乱は，フィードバックループを構成する主な目的のひとつのため，比較的簡単に除去（補償）できる．
3番については，システム自体は目標値通り動作している（外乱によって我々使用者が目標値どおりに動作しているか確認できないだけの状態）ため．

1番は，コントローラは制御対象を最適に動作させるための操作量を正常に生成できているが，それが外乱によって変えられ制御対象に入力されてしまっている状態です．

つまり，PIDなどのコントローラを適切に設計しても，設計通りの操作量が制御対象に入力されないということです．

今回はこの外乱を除去する外乱オブザーバについて述べていきます．

ここでは，"最適な操作量"は設計時に想定している操作量を指します．
最適レギュレータなどの最適とは根本的に異なる内容ですのであしからず．

多入力多出力系とは

自分も初めて聞いた時に，

多入力多出力系ってなんぞや？入出力ごとに制御系組めばいいんじゃないの？

とおもったので，覚え書き程度にまとめておきます．

多入力多出力系（システム）とは，制御工学において，入力および出力が1つだけではないことを指します．

一体何言ってるかわかんないよね．

ということで制御工学でときおり出てくる，多入力多出系について．

多入力多出力系（Multi Input Multi Output; MIMO; マイモ）系とは，上で述べているように入出力が1つではない制御対象の事を言います．

多入力多出力系は，よく制御が困難であると言われます．

この記事では，多入力多出力系の制御のどこがどう困難であるのかについて，簡単に述べたいと思います．

倒立振子シミュレーション(MATLAB/Simulink)

せっかく個人用（学生用）のMATLABを買ったので，それを使ってみましょう的な記事．
制御系の研究ではお馴染みの倒立振子のシミュレーションをやってみます．

動画はシミュレーションの様子．
実は，ToolBoxにSimscapeというものがありまして，その中のSimMechanicsではMATLAB上で3Dソリッドモデルを作るだけで物理シミュレーションができたりします．

ただし，今回は普通のSimulinkのみで行います（笑）

やっぱり解析的に色々するには，どちらにしろ運動方程式も立てないといけないので．
大体の場合において，制御をしようと思うと，運動方程式のパラメータから
制御系の設計パラメータを求める必要が出てきます．
そんな理由から，運動方程式を立てて，制御パラメータを決めていく方法で今回は制御系の設計を行います．

STM32F4 DISCOVERYでのPID制御

STM32F4 DISCOVERYで簡易なPID制御をやってみました．

PID制御は，制御入力が以下の式で表される制御のことです．

$$u = K_{p}(e(t) + \frac{1}{T_{I}}\int e(t) \ \mathrm{dt} + T_{D}\mathrm{\frac{d}{dt}}e(t)) \\ \because K_{p}: \mathrm{比例ゲイン, const.}, \\ T_{I}: \mathrm{積分時間, const.},\\ T_{D}: \mathrm{微分時間, const.},\\ e(t): \mathrm{誤差}$$
上記はIdeal形式の表記で，Parallelでは以下のようになりますが，本質的には同じです．

$$u = K_{P} \ e(t) +\frac{1}{T_{I}}\int e(t) \ \mathrm{dt}+T_{D}\mathrm{\frac{d}{dt}}e(t)$$

今回はIdeal形式の方で，制御則を組んでいます．
目的としては，ポテンショメータの値をAD変換し，PWMでモータを駆動して，位置サーボをするというものです．

---

まずは，GPIOの設定です．

STM32のシリーズでは，ピンとして用意されているGPIO（PA0, PA1, …）のそれぞれに色々な機能がついています．
このため，何か機能を使いたいときには，使いたい機能を持っているGPIOを設定してから，個別の機能を設定する必要があります．

GPIO（General Purpose IO）には，デジタル（1か0）の入力，出力の他に，それぞれのピンに割り振られた特殊機能を使うことができます．
特殊機能の詳細はベンダーホームページのデザイン・リソースから，UM1472: Discovery kit for STM32F407/417 linesをみるとわかります．

この表を見ると，PA1にはADC_123_IN1が，PA2にはTIM2_CH3があることが分かります．

このため，PA1をADCとして，PA2をPWM（タイマ機能の一部であるため）の出力として利用します．

GPIO No.	Alternate function	details
PA1	ADC1	AD変換器1．ポテンショメータの値を読む用
PA2	TIM2 CH3	タイマ2のチャンネル3．モータへのPWM出力（正回転）

　
このときのGPIOの設定はこちら．

GPIO_InitTypeDef GPIO_InitStructure;

/* GPIO for ADC */
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AN;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_1;
GPIO_Init(GPIOA, &GPIO_InitStructure);

/* GPIO for TIM2 */
GPIO_PinAFConfig(GPIOA, GPIO_PinSource2, GPIO_AF_TIM2);
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2;
GPIO_Init(GPIOA, &GPIO_InitStructure);

まずは，GPIOA全体の有効化として，以下を記述．
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);

次に，GPIOAの1番ピンはADCとして使うので，
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AN;
としてAN（Analog）モードに．

2番ピンはPWM（タイマ）として使うので，
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
としてAF（Alternate function）モードに設定します．

---

次に，ADCの設定．

　
個別にADCの設定をしていきます．

ADC_CommonInitTypeDef ADC_CommonInitStructure;
ADC_InitTypeDef ADC_InitStructure;

RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1, ENABLE);

ADC_StructInit(&ADC_InitStructure);
ADC_DeInit();

ADC_CommonInitStructure.ADC_DMAAccessMode = ADC_DMAAccessMode_Disabled;
ADC_CommonInitStructure.ADC_Mode = ADC_Mode_Independent;
ADC_CommonInitStructure.ADC_Prescaler = ADC_Prescaler_Div4;
ADC_CommonInitStructure.ADC_TwoSamplingDelay = ADC_TwoSamplingDelay_5Cycles;
ADC_CommonInit(&ADC_CommonInitStructure);

ADC_InitStructure.ADC_ContinuousConvMode = DISABLE;
ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right;
ADC_InitStructure.ADC_ExternalTrigConvEdge = ADC_ExternalTrigConvEdge_None;
ADC_InitStructure.ADC_NbrOfConversion = 1;
ADC_InitStructure.ADC_Resolution = ADC_Resolution_12b;
ADC_InitStructure.ADC_ScanConvMode = DISABLE;
ADC_Init(ADC1, &ADC_InitStructure);

ADC_RegularChannelConfig(ADC1, ADC_Channel_1, 1, ADC_SampleTime_15Cycles);
ADC_Cmd(ADC1, ENABLE);

ADC_SoftwareStartConv(ADC1);
while(!ADC_GetFlagStatus(ADC1, ADC_FLAG_EOC));

　
まずは，今回使うADC1の有効化．
RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1, ENABLE);

　
次にADC全般の設定．

ADC_CommonInitStructure.ADC_DMAAccessMode = ADC_DMAAccessMode_Disabled;
ADC_CommonInitStructure.ADC_Mode = ADC_Mode_Independent;
ADC_CommonInitStructure.ADC_Prescaler = ADC_Prescaler_Div4;
ADC_CommonInitStructure.ADC_TwoSamplingDelay = ADC_TwoSamplingDelay_5Cycles;
ADC_CommonInit(&ADC_CommonInitStructure);

今回は，DMAを使わず，個別にADCするので，DMAAccessModeはDisableにしています．

次に，もう少し詳細なADCの設定．

ADC_InitStructure.ADC_ContinuousConvMode = DISABLE;
ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right;
ADC_InitStructure.ADC_ExternalTrigConvEdge = ADC_ExternalTrigConvEdge_None;
ADC_InitStructure.ADC_NbrOfConversion = 1;
ADC_InitStructure.ADC_Resolution = ADC_Resolution_12b;
ADC_InitStructure.ADC_ScanConvMode = DISABLE;
ADC_Init(ADC1, &ADC_InitStructure);

前述のように個別でADCするので，ContinuousConvModeはDisableに．
また，今回はADCはこの1chしか使わないので，NbrOfConversionは1に設定しています．
DataAlignは右揃えに，Resolutionは12bit精度にしています．

　
設定が終わったら，有効化して，

ADC_RegularChannelConfig(ADC1, ADC_Channel_1, 1, ADC_SampleTime_15Cycles);
ADC_Cmd(ADC1, ENABLE);

　
一度だけADC1をテスト動作しています．テスト動作が終わる（ADC_FLAG_EOCが立つ）まで，待っています．

ADC_SoftwareStartConv(ADC1);
while(!ADC_GetFlagStatus(ADC1, ADC_FLAG_EOC));

これでADCの設定は終わり．

---

次はタイマの設定．

　
タイマ個別の設定は以下．

#define PWM_FREQ 20e3
#define PWM_DIV 200

TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;

RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);

/* timer 2 for PWM */
TIM_TimeBaseInitStructure.TIM_ClockDivision = 0;
TIM_TimeBaseInitStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseInitStructure.TIM_Period = (uint32_t)(PWM_DIV - 1); //20kHz, 200div, 0.5percent per div, period = 84MHz / (prescaler + 1) / 20kHz - 1
TIM_TimeBaseInitStructure.TIM_Prescaler = (uint16_t)((float)84e6 / (PWM_FREQ * PWM_DIV) - 1); //desired : 200steps in 20kHz, prescaler = 84 MHz (Core clock) / (20kHz*200step) - 1
TIM_TimeBaseInitStructure.TIM_RepetitionCounter = 0;
TIM_TimeBaseInit(TIM2, &TIM_TimeBaseInitStructure);

/* configuration for PWM */
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
TIM_OCInitStructure.TIM_Pulse = PWM_DIV * 25 / 100; //25% duty
TIM_OC3PreloadConfig(TIM2, TIM_OCPreload_Enable);
TIM_Cmd(TIM2, ENABLE);
TIM_CtrlPWMOutputs(TIM2, ENABLE);
TIM_OC3Init(TIM2, &TIM_OCInitStructure);

上部defineのPWM_FREQはPWMの周波数，PWM_DIVはPWMの分解能を表しています．
PWM_DIVを分解能と呼ぶのは少し正規の表現から外れていますが，便宜上こう呼びます．
PWM_DIVは，PWMのデューティ比を何分割までできるかを表しており，
今回は200分割，つまり0.5%単位でのデューティ比の設定が可能となっています．

PWMの場合，モータドライバの電圧によって同じデューティ比でも実際にモータにかかる電圧が変わってくるので，
こういった設定のほうがいいんじゃないかということで．

たとえば，モータドライバの最大出力電圧が10Vで，
TIM_OCInitStructure.TIM_Pulse = PWM_DIV * 25 / 100;
とすると，出力電圧は10Vの25%である，2.5V相当となります．

次にタイマ2の有効化．
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);

PWMを200分割で使用するための設定．
TIM_TimeBaseInitStructure.TIM_Period = (uint32_t)(PWM_DIV - 1);

プリスケーラの設定．
TIM_TimeBaseInitStructure.TIM_Prescaler = (uint16_t)((float)84e6 / (PWM_FREQ * PWM_DIV) - 1);

タイマ2の設定の反映．
TIM_TimeBaseInit(TIM2, &TIM_TimeBaseInitStructure);

次に，タイマをPWMで使用することや，タイマのピン出力をONにすることなどを設定し，反映しています．

TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
TIM_OCInitStructure.TIM_Pulse = PWM_DIV * 25 / 100; //25% duty
TIM_OC3PreloadConfig(TIM2, TIM_OCPreload_Enable);
TIM_Cmd(TIM2, ENABLE);
TIM_CtrlPWMOutputs(TIM2, ENABLE);
TIM_OC3Init(TIM2, &TIM_OCInitStructure);

途中，TIM_Pulseに値を代入しているのは，ただのテストです．

これで設定は終わり．

---

次は時間割り込みの処理について記述します．

PIDでは積分器および微分器を使うので，PIDの計算が周期的に実行されてほしいです．
このため，PIDが20ms周期で実行されるように，時間割り込みの処理について記述しています．

STM32F407では，周期割り込みが関数として記述されているみたいなので，それを使います．
SysTick_Handlerという関数です．

別スクリプトの設定で，SysTick_Handlerには1ms周期で入るようになっています．

#define STROKE 25.0
#define PID_SMPL_PERIOD 20

void SysTick_Handler(void){
    static float c = 0, u = 0, pos = 0;
    static int i = 0;

    i++;

    /* ADC */
    if(!(i % 20)){
        ADC_SoftwareStartConv(ADC1);
        while(!ADC_GetFlagStatus(ADC1, ADC_FLAG_EOC));
        pos = STROKE * (float)ADC_GetConversionValue(ADC1) / (float)((1<<12) - 1);
    }

    /* PID */
    if(!(i % PID_SMPL_PERIOD)){
        u = PID(c, pos);

        /* saturation */
        if(u > PWM_MAX_VOLTAGE){
            u = PWM_MAX_VOLTAGE;
        }else{
            u = 0;
        }


        TIM_OCInitStructure.TIM_Pulse = (uint32_t)(PWM_DIV * (u / PWM_MAX_VOLTAGE));
        TIM_OC3Init(TIM2, &TIM_OCInitStructure);
    }

    if(i == 10000) i = 0;
}

この関数内では，AD変換は20msごとに実行され，
ポテンショメータの電圧から，実際のストロークに変換されています．

AD変換をスタートし，
ADC_SoftwareStartConv(ADC1);

終了するまで待っています．
while(!ADC_GetFlagStatus(ADC1, ADC_FLAG_EOC));

そのAD変換値ADC_GetConversionValue(ADC1)から，実際の位置posに変換しています．
pos = STROKE * (float)ADC_GetConversionValue(ADC1) / (float)((1<<12) - 1);
ここで，AD変換が12bitなため，その最大値((1<<12) -1)で割っています．

---

PIDの計算は，PID_SMPL_PERIODごとに行われます．
今回はADCの周期と同じ20msごとです．
u = PID(c, pos)

PID()は，目標値cと現在の位置posを引数として与えて，返り値として計算された操作量uを得る関数です．

ここでは，計算された操作量がドライバで出力できるかどうかの判断を行っています．

/* saturation */
if(u > PWM_MAX_VOLTAGE){
    u = PWM_MAX_VOLTAGE;
}else{
    u = 0;
}

あとは，操作量（電圧）からPWMのデューティ比を計算し，
TIM_OCInitStructure.TIM_Pulse = (uint32_t)(PWM_DIV * (u / PWM_MAX_VOLTAGE));

実際の出力に反映させています．
TIM_OC3Init(TIM2, &TIM_OCInitStructure);

---

最後に，PID()関数の中身について．

PID()関数は非常にシンプルで，以下のようになっています．

float PID(float c, float pos){
    static float e = 0, ei = 0, ed = 0, e_pre = 0;

    e = c - pos;
    ei += e * (PID_SMPL_PERIOD * 1e-3);
    ed = (e - e_pre) / (PID_SMPL_PERIOD * 1e-3);
    e_pre = e;

    return Kp * (e + 1 / Ti * ei + Td * ed);
}

ここでcは目標の位置，posは現在の位置です．

まず，現在の偏差$e(t)$を計算し，
e = c - pos;

偏差の積分値$\int{e(t)} \ \mathrm{dt}$，
ei += e * (PID_SMPL_PERIOD * 1e-3);

偏差の微分値$\mathrm{\frac{d}{dt}}e(t)$，を計算します．
ed = (e - e_pre) / (PID_SMPL_PERIOD * 1e-3);

微分計算では，前回の誤差と現在の誤差を使って計算するので，前回の誤差e_preを更新します．
e_pre = e

これらの値を元に，$K_{p}(e(t) + \frac{1}{T_{I}}\int e(t) \ \mathrm{dt} + T_{D}\mathrm{\frac{d}{dt}}e(t))$を計算し，returnします．
return Kp * (e + 1 / Ti * ei + Td * ed);

---

これらの記述で，STM32F4DISCOVERYでPID制御ができます．
以下はこれらを基にしたプログラムリスト(main.c, timer.c, timer.h)です．

このままビルドするとSysTick_Handler()が重複しているとかでエラーが出るので，
stm32f4xx_it.c内のSysTick_Handler()を消してあげてください．

下の方のCall back関数は消すとリンクエラーが出るので，
これを消したい場合にはリンク先の箇所を消せば動くと思います．

今回は正方向の出力しかできないので，両方向にモータを回転させたい場合は，
PWMのチャンネルを1つ追加する必要があると思います．

モータのPID制御2（偏差無し制御，I制御）

一年以上期間が開きましたが，せっかくなんで前回の続きでも．

モーターでPID制御を使う，といった場合，何をしたいかというと，前回も述べたとおり

• 一定の速度で回したい
• ある角度で合わせたい
• 指定した動作をさせたい

がありましたね．前回同様，2つめの「ある角度で合わせたい」という機能に絞ります．

前回は，指定された角度と現在の角度を引き算して，その差の分だけ入力するのがP制御という話をしました（したと思う）．

今回は，PIDの二つ目のI制御（積分制御）について．

モータのPID制御1（サーボ制御の基礎，P制御）

久しぶりの記事があんなのだけでは少し寂しいので，よくあるPID制御について少し．
メカトロ系の研究室に配属されたこともあり，制御の話も少し耳にするので備忘録になれば．

まず，PID制御ってなんじゃ，って話．

Proportional（比例項），Integral（積分項），Differential（微分項）の3要素により
物体を制御する…
なんて言っちゃえばお終いなんですけど．それじゃちょっと...

一般的にモータ制御の目的というと，

• 一定の速度で回したい
• ある角度で合わせたい
• 指定した動作をさせたい

とかですね．今回は2つめの，「ある角度で合わせたい」という機能に絞ってみます． たとえば，現在の角度から90度だけ回したい，とか．

高校生の頃は正直，やっすいギア付きモータしか知らなかったので，
「90度になったらOFFにすればいいじゃん」
とか思ってたんですけど．

実際には慣性力のよく効くモータ（e.g.ギア無しのモータとか）では，ON-OFFだと，
早すぎて回り過ぎ→モータを逆転させる→反対側に行き過ぎる
の繰り返し（振動）みたいになるんですよね．

これを解決するのがPIDなどの制御．（実際にはもっと色々あるけど）

とまあ，前置きはこれくらいで．

PIDってなんじゃ，っていう僕の高校時代くらいの人が読めるレベルで書いてます．