ビーム上の球のバランスを取るコードを配備する
はじめに
天秤の水平ビーム（梁）に乗せた球は不安定である．重力の影響で球はビームの端から端へ転がり，天秤も水平位置には留まらない．しかしモータを使うと，ビームの向きと球の位置が制御できる．
球の位置はIR（赤外線）距離センサで測定する．ビームはサーボモータで制御する．モータが必要とする動作電圧は電圧変換器で保たれる．球の希望の位置はポテンショメータを使って指定する．Arduino Unoで実行する制御器はポテンショメータとIRセンサの値を読み取り，サーボモータに適切な信号を送る．
まず系のモデルを計算することから始める．その後，IRセンサとサーボモータの校正を行う．PID制御器，極配置制御器，積分制御器の3つの制御器を設計し，応答を比較する．
部品
Makeblockサーボモータ[リンク]
シャフトコネクタ[リンク]
Makeblockサーボモータのブラケットペア[リンク]
Makeblock Beam0824-320ペア[リンク]
Makeblock Beam0824-192ペア[リンク]
Makeblock Beam0824-160ペア[リンク]
Makeblock Beam0824-144 [リンク]
Makeblock Beam0824-096ペア[リンク]
Makeblock Bracket 3x3ペア[リンク]
M4x22mmねじ7個[リンク]，M4x14mmねじ30個[リンク]，M4x8mmねじ5個[リンク]，M4ナット14個[リンク]，4x7x2のプラスチックリング3個[リンク]
モータフランジとビームの3Dプリントのメスコネクタ[リンク]
モータフランジとビームの3Dプリントオスコネクタ[リンク]
3Dプリントのビームエンド2個[リンク]
IR距離センサ[リンク]，3ピンメスケーブル[リンク]，ハウジング[リンク]
Arduino Uno [リンク]
ポテンショメータ[リンク]
ProtoShield [リンク]
オスピンヘッダ[リンク]
USBケーブル[リンク]
可変DC/DC電圧変換器[リンク]
電源アダプタ[リンク]
各種スペーサ[リンク]
重石またはクランプ
長方形粘着マウントテープ[リンク]
ピンポン玉
組立て
0824-096ビーム2つと0824-160ビーム2つをM4x14ねじ4個で留め，ベースを作る（動作中にカタカタ音がしそうな場合は，底に長方形粘着テープを貼り付ける）．
3x3のブラケットをM4x14ねじ2個とナット2個を使って0824-160ビームそれぞれに取り付ける．
0824-192ビームをM4x22ねじ2個とナット2個を使って0824-160ビームそれぞれに取り付ける．
0824-144ビームをM4x14ねじ4個で0824-192ビーム2つの上に取り付ける．
モータブラケットを0824-144ビームにM4x14ねじ2個とナット2個で取り付ける．
モータ，モータフランジ，シャフトコネクタ，モータフランジとビームのメスコネクタを，M4x22ねじ1個（中央）とM4x14ねじ2個で取り付ける．
組み立てたモータをM4x8 mmねじ3個とプラスチックリング3つでモータブラケットに取り付ける．
3ピンのメスケーブルの片方の端をIR距離センサに，もう片方をハウジングに挿してから，M4x8ねじ2個を使ってセンサをビームエンドの一つに取り付ける．
0824-320ビームの片方は，目盛を付けて長さが測れるようにする．
ビームエンドをM4x14ねじ8個で0824-320ビームに取り付ける．
フランジとビームのオスコネクタを，目盛が付いていない方の0824-320ビームにM14x22ねじ2個とナット2個で取り付ける．
ブラケットをIRセンサが付いている側の0824-320ビームにM14x22ねじ2個とナット2個で取り付ける．
ビームに取り付けたオスコネクタを，組み立てたモータ側部品のメスコネクタに差し込み，重石を使って固定する．
ProtoShieldボードまたはブレッドボードを使って電気接続を配線する．電圧変換器を接続する前に，出力電圧(VOUT)が6Vに設定されていることを確認する．
スペーサを使ってArduinoを持ち上げ，その上にProtoShieldボードを載せる．
モータとセンサをProtoShieldボードのそれぞれ適切な場所に接続する．
モデル化
前から見たピンポン球とビーム．
サーボモータに指定された角度を制御入力とする．モータの動力学を無視すると，ビームの質量も無視できる．
ラグランジアン(Lagrangian)を運動エネルギーとポテンシャルエネルギーの差として計算する．
ℒ = ((1/2 )m r'[t]^2 + (1/2)J θ'[t]^2) - (-m g r [t] Sin[θ[t]])ピンポン球は中空球であり，すべることなく転がると仮定する．
asumps = {J -> (2/3)m R^2, θ'[t] -> (r'[t]/R) };制御入力uの線形化フィードバックを使う．これは系を線形化する．
linF = θ[t] -> ArcSin[u[t]];仮定と線形化フィードバックを加えたラグランジアン．
ℒ = ℒ /. asumps /. linF運動方程式．
eq = D[D[ℒ, r'[t]], t] - D[ℒ, r[t]] == 0入力u，出力rの線形StateSpaceModel（状態空間モデル）．
StateSpaceModel[eq, r[t], u[t], r[t], t]
ballAndBeam = % /. g -> 9.8センサの校正
IRセンサの値を読み取り，それをシリアルチャンネルに出力するコードを埋め込む．
port = "/dev/tty.usbmodem141411";ℳ = MicrocontrollerEmbedCode[StateSpaceModel[{{}, {}, {}, {{1}}}, SamplingPeriod -> 0.5, SystemsModelLabels -> None], <|"Target" -> "ArduinoUno", "Inputs" -> "A0" -> "Analog", "Outputs" -> "Serial"|>, port]マイクロコントローラへの接続を開く．
dv = DeviceOpen["Serial", port]入ってくるデータを解釈する関数．
parseData[{ℳ["Serial"]["StartByte"], val__}] := ToExpression@FromCharacterCode@#& /@ {{val}}
parseData[___] := {}入ってくるデータを読み取るタスク．
y = {};
task = SessionSubmit@ScheduledTask[
If[DeviceExecute[dv, "SerialReadyQ"],
y = Join[y, parseData@DeviceReadBuffer[dv, "ReadTerminator" -> ℳ["Serial"]["EndByte"]]];
DeviceExecute[dv, "ReadFlush"];
],
0.5
];ピンポン球をビームのさまざまな位置に置き，センサの読取り値を記録する．
センサデータ．
sensorData = IconizedObject[«sensor data»];sensorDataPlot = ListPlot[sensorData, PlotTheme -> "Detailed", PlotStyle -> PointSize[Medium], FrameLabel -> {"voltage (volts)", "r (m)"}, ImageSize -> Medium]十分なデータ点を取得したら，タスクを停止し接続を閉じる．
TaskRemove[task];
DeviceClose[dv];非線形モデルをデータにフィットする．
sensorFit = NonlinearModelFit[sensorData, Subscript[c, 0] + Subscript[c, 1]v + Subscript[c, 2]v^2 + Subscript[c, 3]v^3 + Subscript[c, 4]v^4, {Subscript[c, 0], Subscript[c, 1], Subscript[c, 2], Subscript[c, 3], Subscript[c, 4]}, v]データとモデルを比較する．
Show[sensorDataPlot, Plot[sensorFit[v], {v, 0, 3}]]モデルをNonlinearStateSpaceModel（非線形状態空間モデル）として再構築する．
sensor = NonlinearStateSpaceModel[ {{}, sensorFit[v]}, {}, v, SamplingPeriod -> T]サーボの校正
モータの制御にArduinoサーボライブラリを使用する．
servoLib = FileNameJoin[{$RootDirectory, "Applications", "Arduino.app", "Contents", "Java", "libraries", "Servo", "src"}]サーボ出力チャンネル．
servoOut = <|"Type" -> "ExternalLibrary", "IncludeFiles" -> "<Servo.h>", "Utilities" -> "Servo myservo", "Initializations" -> "myservo.attach(2)", "Loop" -> {"double angle", "myservo.write(angle)"}|>;ライブラリで指定されたサーボの「角度」は 0から180までの値を取る．これはモデルで使われている θ と同じではない．θ と「角度」のマッピングを特定する必要がある．
これを行うために，特定の「角度」の値のサーボ出力のモデルを作成する
sys = With[{angle = 81}, NonlinearStateSpaceModel[{{}, angle}, {}, u, SamplingPeriod -> 1]]これをマイクロコントローラに埋め込むと，ビームはそれに従って向きを変える．
MicrocontrollerEmbedCode[sys, <|"Target" -> "ArduinoUno", "Outputs" -> servoOut, "Timer" -> 0|>, port, <|"Libraries" -> servoLib|>]結果の θ を計測して記録する．これを繰り返していくつかのデータ点を得る．
servoData = IconizedObject[«servo data»];servoDataPlot = ListPlot[servoData, PlotTheme -> "Detailed", PlotStyle -> PointSize[Medium], FrameLabel -> {"θ (radians)", "angle (degrees)"}, ImageSize -> Medium, PlotRange -> {All, {40, 130}}]非線形モデルをデータにフィットする．
servoFit = LinearModelFit[servoData, θ, θ]データとモデルを比較する．
servoPlot = Show[servoDataPlot, Plot[servoFit[θ], {θ, -40 °, 25 °}]]モデルをNonlinearStateSpaceModelとして再構築する．
servo = NonlinearStateSpaceModel[ {{}, servoFit[θ]}, {}, θ, SamplingPeriod -> T]θ が大きいと，ビームがテーブルにぶつかる可能性がある．また，制御器が大きい θ 値からビームを戻すときに，ピンポン球がビームから落ちる可能性がある．これを防ぐために，サーボが均衡値 θ ==0からどれだけ回転できるのかについてクランプを設ける．
pts = {-0.4, 0.4};
{lLim, uLim} = servoFit /@ Reverse[pts];
ln[{th_, a_}] := Line[{{-0.8, a}, {th, a}, {th, 40}}]Show[servoPlot, Epilog -> {Orange, Dashed, Thick, ln[{pts[[1]], uLim}], ln[{pts[[2]], lLim}]}]クランプのモデルを作成する．
servoClamp = NonlinearStateSpaceModel[{{}, With[{lLim = lLim, uLim = uLim}, Which[angle ≤ lLim, lLim, angle ≥ uLim, uLim, True, angle]]}, {}, angle, SamplingPeriod -> T]その他の構成要素
必要となる他の構成要素のモデルを作成する．
基準位置はポテンショメータで設定され，これは0から5Vまでである．これを- 6 cmから6 cmにマップするモデルを作成する．
potFit = LinearModelFit[{{0, -0.06}, {5, 0.06}}, v, v]Plot[potFit[v], {v, 0, 5}, Axes -> False, Frame -> True, FrameLabel -> {"voltage (volts)", "rSubscriptBox[\ , ref](m)"}, ImageSize -> Medium, GridLines -> Automatic]モデルをNonlinearStateSpaceModelとして再構築する．
pot = NonlinearStateSpaceModel[{{}, {potFit[v]}}, {}, v, SamplingPeriod -> T]基準位置と実際の位置を比較する比較器のモデル．
comp = StateSpaceModel[{{}, {}, {}, {{1, -1}}}, SamplingPeriod -> T]シミュレーション用に，連続時間比較器も必要である．
compC = StateSpaceModel[{{}, {}, {}, {{1, -1}}}]AVR-Libcライブラリの逆正弦関数asinは，引数が正しい範囲でない場合エラーを返す．ArcSinの計算をうまく扱うモデル．
aSin = NonlinearStateSpaceModel[{{}, Which[u <= -0.99, -(π/2), u ≥ 0.99, (π/2), True, ArcSin[u]]}, {}, u, SamplingPeriod -> T]逆正弦の引数が正しい範囲であることを確認する別のモデル．
sinClamp = NonlinearStateSpaceModel[{{}, Which[u <= -0.99, -0.99, u ≥ 0.99, 0.99, True, u]}, {}, u, SamplingPeriod -> T]制御器コードのためのマイクロコントローラの指定．
μc = <|"Target" -> "ArduinoUno", "Inputs" -> {"A1" -> "Analog", "A0" -> "Analog"}, "Outputs" -> servoOut, "Timer" -> 0|>;マイクロコントローラ，ポート，ライブラリの指定．
embedSpecs = Sequence[μc, port, <|"Libraries" -> servoLib|>];サンプリング期間．
T = 0.05;PID制御器の設計
フィルタした微分部分を持つPID制御器を設計する．
pidData = PIDTune[ballAndBeam, "PIFD", "PIDData"]閉ループ系．
csysPID = pidData["ReferenceOutput"]閉ループ系のシミュレーションを行う．
Plot[Evaluate@OutputResponse[csysPID, 0.03, {t, 0, 10}], {t, 0, 10}, PlotRange -> All]シミュレーションから，安定するのに少し時間がかかることが分かる．系の固有値を見ると，虚軸に近いものがあることが分かる．
Eigenvalues[Normal[csysPID][[1]]]フィードバック制御器．
pidc = pidData["Feedback"]フィードフォワードフィルタ．
pidData["Feedforward"]PID制御器を離散化する．
pid = ToDiscreteTimeModel[pidc, T]PID制御器の配備
PID制御器の全容．
controllerPID = SystemsConnectionsModel[{pot, comp, pid, aSin, servo, servoClamp, sensor}, {{1, 1}{2, 1}, {2, 1}{3, 1}, {3, 1}{4, 1}, {4, 1}{5, 1}, {5, 1}{6, 1}, {7, 1}{2, 2}}, {{1, 1}, {7, 1}}, {{6, 1}}]パッケージをロードする．
Needs["MicrocontrollerKit`"]制御器を埋め込む．
MicrocontrollerEmbedCode[controllerPID, embedSpecs]極配置制御器の設計
極を左半平面に移動させるのに極配置を使用する．これには状態フィードバックすべてを使い，位置rだけが計測されるため，推定器も必要である．また，制御器が基準信号を追跡するためにフィードフォワード項も必要である．
StateFeedbackGainsを使ってフィードバックゲインを計算する．
poles = {-3 + I, -3 - I};
𝓀 = StateFeedbackGains[ballAndBeam, poles]推定器ゲインEstimatorGainsを計算する．
estimPoles = {-8, -10};
ℓ = EstimatorGains[ballAndBeam, estimPoles]状態推定器．
estim = SystemsModelExtract[StateOutputEstimator[ballAndBeam, ℓ], All, {1, 2}]フィードバック制御器．
fbc = SystemsModelSeriesConnect[estim, StateSpaceModel[{{}, {}, {}, 𝓀}]]フィードフォワード制御器．
{a, b, c, d} = Normal[ballAndBeam];
𝓃 = Inverse[ArrayFlatten[{{a, b}, {c, d}}]].SparseArray[{{3, 1} -> 1}];ffc = StateSpaceModel[{{}, {}, {}, 𝓃[[{3}]] + 𝓀.𝓃[[1 ;; 2]]}]制御器とプラントの構造．
閉ループ系．
csysPP = SystemsModelMerge[SystemsConnectionsModel[{ffc, compC, fbc, ballAndBeam}, {{1, 1}{2, 1}, {2, 1}{4, 1}, {2, 1}{3, 1}, {4, 1}{3, 2}, {3, 1}{2, 2}}, {{1, 1}}, {{4, 1}}]]閉ループ系の固有値．
Eigenvalues[First[Normal[csysPP]]]設計された場所にあることを確認する．
Complement[%, Join[poles, estimPoles], SameTest -> (Chop[#1 - #2] == 0 &)]Plot[Evaluate@OutputResponse[csysPP, 0.03, {t, 0, 3}], {t, 0, 3}, PlotRange -> All]制御器を離散化する．
{ff, fb} = ToDiscreteTimeModel[#, T]& /@ {ffc, fbc}極配置の配備
極配置制御器の全容．
controllerPP = SystemsConnectionsModel[{pot, ff, comp, aSin, servo, servoClamp, sinClamp, fb, sensor}, {{1, 1}{2, 1}, {2, 1}{3, 1}, {3, 1}{4, 1}, {4, 1}{5, 1}, {5, 1}{6, 1}, {3, 1}{7, 1}, {7, 1}{8, 1}, {9, 1}{8, 2}, {8, 1}{3, 2}}, {{1, 1}, {9, 1}}, {{6, 1}}]制御器を埋め込む．
MicrocontrollerEmbedCode[controllerPP, embedSpecs]積分制御器の設計
状態方程式を誤差の積分値で拡張する．
ballAndBeamInt = StateSpaceModel[{ArrayFlatten[{{0, -c}, {0, a}}],
ArrayFlatten[{{0}, {b}}]}]誤差に大きくペナルティを与えるオプションの最適ゲイン一式．
κ = LQRegulatorGains[ballAndBeamInt, {DiagonalMatrix[{20, 350, 10}], {{10}}}]閉ループ系の固有値．
eig = Eigenvalues[Normal[SystemsModelStateFeedbackConnect[ballAndBeamInt, κ]][[1]]]フィードフォワード制御器．
ffcInt = SystemsModelSeriesConnect[StateSpaceModel[{{}, {}, {}, -κ[[All, 1 ;; 1]]}], TransferFunctionModel[(1/s), s]]フィードバック制御器．
fbcInt = SystemsModelSeriesConnect[estim, StateSpaceModel[{{}, {}, {}, κ[[All, 2 ;; 3]]}]]制御器とプラントの構造．
閉ループ系．
csysInt = SystemsModelMerge[SystemsConnectionsModel[{compC, ffcInt, compC, fbcInt, ballAndBeam}, {{1, 1}{2, 1}, {2, 1}{3, 1}, {3, 1}{5, 1}, {3, 1}{4, 1}, {5, 1}{4, 2}, {4, 1}{3, 2}, {5, 1}{1, 2}}, {{1, 1}}, {{5, 1}}]]固有値を計算する．
Eigenvalues[First[Normal[csysInt]]]Complement[%, Join[eig, estimPoles], SameTest -> (Chop[#1 - #2] == 0 &)]Plot[Evaluate@OutputResponse[csysInt, 0.03, {t, 0, 12}], {t, 0, 12}, PlotRange -> All]制御器を離散化する．
{ffInt, fbInt} = ToDiscreteTimeModel[#, T]& /@ {ffcInt, fbcInt}積分制御器の配備
積分制御器の全容．
controllerInt = SystemsConnectionsModel[{pot, comp, ffInt, comp, aSin, servo, servoClamp, sinClamp, fbInt, sensor}, {{1, 1}{2, 1}, {2, 1}{3, 1}, {3, 1}{4, 1}, {4, 1}{5, 1}, {5, 1}{6, 1}, {6, 1}{7, 1}, {4, 1}{8, 1}, {8, 1}{9, 1}, {10, 1}{9, 2}, {9, 1}{4, 2}, {10, 1}{2, 2}}, {{1, 1}, {10, 1}}, {{7, 1}}]MicrocontrollerEmbedCode[controllerInt, embedSpecs]結論
3つの制御器の線形閉ループ系のステップ応答のシミュレーションにより，極配置制御器の性能が他よりよいことが分かる．
Table[OutputResponse[sys, 0.01, {t, 0, 10}], {sys, {csysPID, csysPP, csysInt}}];
Plot[%, {t, 0, 10}, PlotTheme -> "Detailed", PlotRange -> All, PlotLegends -> {"PID", "pole placement", "integral"}, ImageSize -> Medium]PID制御器は予想通り，二重積分系ではあまりよい結果が得られなかった．実際の物理的な系では，整定時間が長く，オーバーシュートが大きいという特徴がある．定常状態応答はよい結果が得られた．
極配置制御器ではよい結果が得られたが，別の設置で異なるIRセンサとモータを使うと性能が非常に低かった．
全体として，過渡応答，定常状態応答，外乱応答については積分制御器が最も良い結果を出した．これはセンサとモータを変更してもよい性能を維持でき，十分に堅固であった．
発展
- 外乱応答のシミュレーションを行う．
- 他のPID調整則を試す．
- 他のフィードバックゲイン，推定器ゲインを試す．
- 非線形ArcSin要素を取り除いたときの違いを調べる．
- アンチワインドアップ制御を使う．