選択は逆振り子の問題にかかった。 ビデオの結果：
数学モデル
私は運動方程式の導出を引用しませんが、これはまだ研究所の第三コースです。 結論に興味がある人のために、記事の最後にリンクがあり、ここで詳細に説明されています。
システムを次の形式で表します。
振り子は、長さlの無重量ロッドの端に取り付けられた質量m pです。 モーターがシャフトのもう一方の端に取り付けられ、最大モーメントM kを発生し 、それを質量m wおよび半径rのホイールに伝達します。
制御タスクは、振り子を垂直位置に安定させ、ホイールを初期位置に戻すことです。
逆振り子を記述する運動方程式は、次のように表すことができます。
それらはかなり不快に見えますが、ロボット自体はそれらについて何も知らず、制御は線形化されたモデルを使用します。
コントロールの合成
PIDコントローラーを持っている人がうらやましい。 係数の調整に数時間を費やしましたが、それでも価値ある結果を達成することができませんでした。 監督者は、線形2次レギュレータ（ wiki ）を使用するようアドバイスしました。 このコントローラーは、PIDコントローラーとは対照的に、各座標の誤差と係数の積にすぎません。 微分と積分の離散的な類似物はありません。 ただし、それを計算するには、システムモデルと、Riccati方程式（Matlab）を解く能力が必要です。
matlabでは、コントローラーの計算は一連のコマンドです：
A=[0 1.0 0 0;0 0 -140 0;0 0 0 1.0;0 0 28 0] B=[0;212.85;0;-19.15] Q=[5 0 0 0;0 5 0 0;0 0 1 0;0 0 0 1] R=1500 [K,S,e]=lqr(A,B,Q,R)
ここで、行列AとBは、実際のロボットの値が置換された線形化モデルの対応する行列です。
Qマトリックスは、原点から逸脱するためにシステムをどれだけ微調整する必要があるかを決定します。この場合、座標には速度が含まれることに注意してください。
マトリックスRは、制御によるエネルギーの無駄のためにシステムをどれだけ細かくする必要があるかを決定します。
変数Kには、コントローラーの係数が含まれます。
シミュレーション
Matlabのsimulinkでは、記事の最後にある数学モデルのあるリポジトリへのリンクが必要な場合、システムを簡単にエミュレートできます。 ここではグラフのみを提供します。
振子振れ角：
ホイールたわみ角：
エンジントルク：
鉄の実現
ロボット自体のフレームは12mmと14mmのアルミニウムプロファイルで、互いに入ります。 リベットで接続されています。 電子機器は、T字の形でグラスファイバーに取り付けられます。モーターもグラスファイバーアダプターを介して取り付けられます。
最初は、これらのモーターを使用しようとしました。
トルクは2.2kg * cmまたは0.2Nmです。 シミュレーションに基づいて、さらに多くが必要なため、他のモーターが選択されました。
メーカー参照
最大トルク14kg * cmまたは1.4Nm。 最大5Aの電流を消費するため、arduinoドライバーに人気のあるL293Dはここでは動作しません。
角度と角速度を決定するために、ジャイロスコープと加速度計であるIMUが使用されます。 L3GジャイロスコープとLSM303磁力計を備えた加速度計を搭載したボードが横になっていた。 類似のボードはたくさんありますが、センサー値を取得するためのコードは引用しません。 ただし、ジャイロスコープが常に離れているため、センサーの読み取り値をフィルターで除外する必要があります。また、ロボットが角度を変えずに動き始めると、加速度計はノイズが多く、横になります。
それらは異なるフィルターを使用しますが、最も一般的なのはカルマンフィルターとRCフィルター（相補フィルター）です。 私はこのようなコードを使用しています：
float lastCompTime=0; float filterAngle=1.50; float dt=0.005; float comp_filter(float newAngle, float newRate) { dt=(millis()-lastCompTime)/1000.0; float filterTerm0; float filterTerm1; float filterTerm2; float timeConstant; timeConstant=0.5; filterTerm0 = (newAngle - filterAngle) * timeConstant * timeConstant; filterTerm2 += filterTerm0 * dt; filterTerm1 = filterTerm2 + ((newAngle - filterAngle) * 2 * timeConstant) + newRate; filterAngle = (filterTerm1 * dt) + filterAngle; lastCompTime=millis(); return filterAngle; }
これは完全には機能しませんが、このタスクには十分です。
次のセンサーは、モーターの直交エンコーダーです。 2つの出力で矩形パルスを生成します。
これらは、割り込みまたはループ内の値を読み取ることで考慮することができます。 Arduinoプレイグラウンドには、コード例が記載されたすばらしい記事があります。
車輪の角速度を取得するために残っています。 ここで、学校の公式は、移動距離/滞在時間の助けになります。
#define ToPhiRad(x) ((x)*0.00280357142) timer_old = timer; timer=millis(); G_Dt = (timer-timer_old)/1000.0; dPhi=(ToPhiRad(encoder0Pos)-lastPhi)/G_Dt;
ToPhiRadはエンコーダーのティック数をホイールの角に変換し、エンコーダーは1回転あたり約2240ティックを生成します。 角度を取得するには、目盛りに2 Piを乗算し、ホイールが完全に回転したときに目盛りで割る必要があります。
センサーの測定値はLQRコントローラーに送られます
float K1=0.1,K2=0.29,K3=6.5,K4=1.12; long getLQRSpeed(float phi,float dphi,float angle,float dangle){ return constrain((phi*K1+dphi*K2+K3*angle+dangle*K4)*285,-400,400); }
係数はMatlabから取得されますが、安定性のために最初の2つの係数を修正しました。
私のドライバー、またはそのライブラリーは、-400から400の値を取ります。400では、12Vモーター、つまり モーターは最大トルク（1.4Nm）を発生します。 400を1.4で割ると、LQRを与えるNmから、ドライバーが理解できる値への変換係数が得られます。
ロボットをある時点で安定させるだけではあまり面白くないため、BTモジュールHC-05が追加されました。 モジュールは、マイクロコントローラーのシリアルポートに接続されています。 3.3Vで動作し、arduinoは5Vで動作するため、分圧器を介してモジュールの受信入力を接続する必要があります。 接続図は次のとおりです。
サイクル中、マイクロコントローラーはモジュールの文字をポーリングします：
float phiDif=0.f; float factorDif=0.f; float getPhiAdding(float dif){ // - if(dif<200 && dif>-200){return 0.f;} float ret = dif*0.08; return ret; } float getFactorAdding(float dif){// if(dif<200 && dif>-200){return 0.f;} float ret = dif/500*20; return ret; } //======== if (Serial.available()){ BluetoothData=Serial.read(); if(BluetoothData=='w'){ phiDif=200; } else if(BluetoothData=='s'){ phiDif=-200; } else if(BluetoothData=='a'){ factorDif=200; } else if(BluetoothData=='d'){ factorDif=-200; } else if(BluetoothData=='c'){ factorDif=0; phiDif=0; } }
最終的に、センサーの読み取り値はレギュレーターに送られ、その制御とユーザーの影響はモーターに送られます。
encoder0Pos+=getPhiAdding(phiDif); lastPhi=ToPhiRad(encoder0Pos); spd=getLQRSpeed(ToPhiRad(encoder0Pos),dPhi,balanceAt-angle,gyroRate[coordY]); float factorL=getFactorAdding(factorDif); md.setSpeeds(spd-factorL,spd+factorL);
50ミリ秒ごとに、ロボットの遠隔測定角度が送信されます。
if(millis()%50==0){ Serial.println(angle); }
無線制御を追加
アンドロイド用の電話から制御します。
アプリケーションを起動するとき、接続先を選択するようユーザーに要求します。bt-moduleはすでに電話（標準コード1234）とペアリングされている必要があります。
BluetoothAdapter bluetooth; String []boundedItems; protected static final int RECIEVE_MESSAGE = 1; @Override protected void onCreate(Bundle savedInstanceState) { //... bluetooth = BluetoothAdapter.getDefaultAdapter(); if(bluetooth != null){ if (!bluetooth.isEnabled()) { bluetooth.enable(); } } Set<BluetoothDevice> bounded=bluetooth.getBondedDevices(); boundedItems=new String[bounded.size()]; int i=0; for (BluetoothDevice bluetoothDevice : bounded) { boundedItems[i++]=bluetoothDevice.getName(); } showListDialog(); //... } public void showListDialog(){ AlertDialog.Builder builder = new AlertDialog.Builder(this); builder.setTitle("Pick a device"); builder.setItems(boundedItems, new DialogInterface.OnClickListener() { public void onClick(DialogInterface dialog, int item) { connectTo(item); } }); AlertDialog alert = builder.create(); alert.show(); }
デバイスを選択したら、それに接続します。
private static final UUID MY_UUID = UUID.fromString("00001101-0000-1000-8000-00805F9B34FB"); BluetoothSocket btSocket; public void connectTo(int id){ Set<BluetoothDevice> bounded=bluetooth.getBondedDevices(); for (BluetoothDevice bluetoothDevice : bounded) { if(bluetoothDevice.getName().equalsIgnoreCase(boundedItems[id])){ try { btSocket=bluetoothDevice.createRfcommSocketToServiceRecord(MY_UUID); btSocket.connect(); ct=new ConnectionThread(btSocket); ct.start(); } catch (IOException e) { e.printStackTrace(); try { btSocket.close(); } catch (IOException e1) { e1.printStackTrace(); } showListDialog(); } return; } } }
接続後、フローが開始され、アプリケーションとロボット間の通信が行われます。
private class ConnectionThread extends Thread{ private final InputStream mmInStream; private final BufferedReader br; private final OutputStream mmOutStream; public ConnectionThread(BluetoothSocket socket) throws IOException { mmInStream = socket.getInputStream(); br=new BufferedReader(new InputStreamReader(mmInStream)); mmOutStream = socket.getOutputStream(); } public void run() { while (true) { try { String line=br.readLine(); h.obtainMessage(RECIEVE_MESSAGE, line).sendToTarget(); } catch (IOException e) { e.printStackTrace(); } } } public void sendCmd(char cmd){ try{ mmOutStream.write(cmd); }catch (IOException e) { e.printStackTrace(); } } }
スレッドは、次のように定義されているハンドラーを介してアプリケーションのメインスレッドにメッセージを送信します。
h = new Handler() { public void handleMessage(android.os.Message msg) { switch (msg.what) { case RECIEVE_MESSAGE: String line=(String)msg.obj; try{ float a=Float.parseFloat(line.trim()); balancerView.setAngle((float) (a-Math.PI/2.f)); }catch (Exception e) { } break; } }; };
balancerViewはSurfaceViewクラスの子孫であり、ロボットの現在の位置を表示します。
彼の再描画メソッドは次のとおりです。
public void draw(Canvas canvas) { Paint paint=new Paint(); paint.setStrokeWidth(3); canvas.save(); canvas.rotate((float) (angle*180.f/Math.PI), cx, cy); paint.setColor(Color.BLACK); canvas.drawRect(cx-15, cy-150, cx+15, cy, paint); paint.setColor(Color.WHITE); canvas.drawRect(cx-12, cy-147, cx+12, cy-3, paint); paint.setColor(Color.BLACK); canvas.drawCircle(cx, cy, 30, paint); paint.setColor(Color.WHITE); canvas.drawCircle(cx, cy, 25, paint); canvas.restore(); }
onTouchイベントが発生するとコマンドがロボットに送信されるため、ボタンを押したままロボットを制御できます。
@Override public boolean onTouch(View v, MotionEvent me) { if(me.getAction()==MotionEvent.ACTION_UP){ ct.sendCmd('c'); return false; } if(v==wB){ ct.sendCmd('w'); }else if(v==aB){ ct.sendCmd('a'); }else if(v==sB){ ct.sendCmd('s'); }else if(v==dB){ ct.sendCmd('d'); } return false; }
おわりに
建物全体で最も楽しいことは、数学的モデルが物理的な実装に同意したことです。 鉄片自体の構造は複雑ではありませんが、正しいモーターの選択、ロボットの高さ、上からの負荷の質量、および制御の統合は非常に興味深いタスクです。
約束どおり、車輪上の倒立振子の運動方程式の導出 ： 方程式の導出と構築に関する少し
githubリポジトリ