工学男子の日常

モノづくりが好きな男子の日記です。

倒立振子を線形二次レギュレーター(LQR)で”簡単に”立たせる話

 

kogakudanshi.hatenablog.jp

前回でシミュレーターができたのでやっと制御できます。
自分で問題を作って自分で解く、虚無ですね。

 

振り子を立たせるだけでなく、台車位置もゼロに保つようにしてみたいと思います。入力は台車にはたらく力とします。つまり制御対象の状態変数が複数あるのに対して入力は1つの劣駆動システムになります。

運動方程式の線形化と状態方程式・観測方程式

求めた運動方程式

\left \lbrace \begin{array}{l}(M+m)\ddot{x}+\frac{lm}{2}\cos{\theta}\ddot{\theta}-\frac{lm}{2}\sin{\theta}\dot{\theta}^2=F\\ \cos{\theta}\ddot{x}+\frac{2l}{3}\ddot{\theta}-g\sin{\theta}=0\end{array}\right.

棒の傾きが微小の時\theta\rightarrow0を仮定して線形化します。

\ddot{x}=-\frac{3mg}{4M+m}\theta+\frac{4}{4M+m}F

\ddot{\theta}=\frac{6\left(M+m\right)g}{\left(4M+m\right)l}\theta-\frac{6}{\left(4M+m\right)l}F

状態変数を定義します。

x=\begin{bmatrix} x \\ \theta \\ \dot{x} \\ \dot{\theta}\end{bmatrix}

上の線形化した式を用いて状態方程式を作ります。

 

\dot{x}=Ax+Bu

また観測方程式は以下のようになります。シミュレーターなので全ての状態変数が観測できるものとします。

これでシステムが把握できたので制御を組んでいきます。

線形二次レギュレーター

LQRでは以下の評価関数を最小化するゲインKを求めます。

J=\int_0^\infty(x^T(t)Qx(t)+u^T(t)Ru(t))dt

Qは状態に関する重み行列、Rは入力に関する重み行列です。ざっくりとしたイメージでいうと状態変数xの各成分が0から離れていて、その時間が長いほどx^T(t)Qx(t)の値が大きくなります。また入力uが0から離れていて、その時間が長いほどu^T(t)Ru(t)の値が大きくなります。

また対角行列であるQの各行が状態変数の各成分の重みになっているので、優先して収束させたい状態変数のと同じ行の対角成分を大きくすることで過渡特性をチューニングすることができます。またQに比べてRの値を大きくすると多少収束に時間がかかってもいいので入力を減らせ、という省エネ命令になるわけです。

 

入力は以下のように状態変数にゲインをかけた値になります。

u=-Kx

つまりこのKをうまく決めてやるとJがより小さくなる、つまり目標値から外れている時間が短くなり、またより少ない入力でそれを達成できるわけです。

最適ゲインの算出

それじゃあKはどのように決めたらいいのか……というふうに話が進むわけですが「簡単に」と銘打っているように、ここでは自分では求めません。当たり前です。

 

具体的にはmatlabにおんぶに抱っこします。

m = 1;
M = 4;
l = 4;
g = 9.8;

A = [0 0 1 0; 0 0 0 1; 0 -3*m*g/(4*M+m) 0 0; 0 6*(M+m)*g/(4*M+m)/l 0 0]
B = [0; 0; 4/(4*M+m); -6/(4*M+m)/l]
Q = eye(4)
R = 1

lqr(A,B,Q,R)

はい出ました。

簡単ですね。

 

4つの数に左から順にx,\theta,\dot{x},\dot{\theta}をかけた値の合計を求め、-1をかけた値を制御入力Fとします。前回作ったシミュレーターに制御入力を加えた結果が以下です。

これに対して先程のmatlabのシステムに同じ初期値を与えたときの応答が以下です。

上から一番目と二番目、x,\thetaのグラフはほぼ一致していて、どちらも15秒前後で静定していることがわかると思います。一方で\dot{\theta}はまだ誤差が残っています。もう少し動かすとゼロにはなりますが、制御モデルが線形化されているのに対してシミュレーターが非線形であるためと考えられます。

より高速にxをゼロにするためQの1行1列目を1から5にしてみます。

気持ち収束が早くなった気がします。その代わり移動中の棒の傾きは大きくなってしまいました。

次にQ単位行列に戻して、制御量を少なくするためRを1から10にしてみます。

台車の加減速がゆっくりになっているのがわかると思います。制御量が少なくなってエコ&動かしているアクチュエータには優しくなりましたが、静定時間は伸びてしまいました。

 

以上のように最適制御は一つのQ,Rに対しては計算によってパラメーターが一意に決まりますが、理想とする動きに近づけるためにはチューニングが必要です。最適であることが保証されているからこそ、すべての性能がトレードオフになっています。

このような難しさもありますが、重み行列をとりあえず単位行列にしておけば動くし多入力、劣駆動システム対応の制御法としては一番簡単だと思います。

 

ここまでご覧いただきありがとうございました。
記事中に間違いなどありましたらお気軽にコメントで教えていただけると助かります。

Processingで倒立振子のシミュレーターを作る話(オイラー法・ルンゲ=クッタ法)

 

kogakudanshi.hatenablog.jp

前回求めた倒立振子運動方程式を組み込んだシミュレーターを作ります。

\ddot{x}=\frac{4F-3mg\sin{\theta}\cos{\theta}+2lm\sin{\theta}\dot{\theta}^2}{4\left(M+m\right)-3m\cos ^2\theta}

\ddot{\theta}=\frac{6\left(M+m\right)g\sin{\theta}-3\cos{\theta}\left(2F+lm\sin{\theta}\dot{\theta}^2\right)}{l\left(4\left(M+m\right)-3m\cos ^2\theta\right)}


育ちが悪くてMatlabがちゃんと使えないのでProcessingで可視化します。

オイラー

シンプルなやつです。
x(t+dt)=x(t)+\dot{x}(t,y)\times dt

みたいなのをそのまま実装します。

float x = -20;
float x_ = 5;
float t = PI / 180 * 20;
float t_ = -PI / 180 * 51;



final float m = 2;
final float M = 1;
final float l = 4;
final float g = 9.8;


float fx(float x,float t,float x_,float t_,float F){
  return (4*F-3*m*g*sin(t)*cos(t)+2*l*m*sin(t)*t_*t_)/(4*(M+m)-3*m*cos(t)*cos(t));
}


float ft(float x,float t,float x_,float t_,float F){
  return (6*(M+m)*g*sin(t)-3*cos(t)*(l*m*sin(t)*t_*t_+2*F))/(l*(4*(M+m)-3*m*cos(t)*cos(t)));
}


void update(float F,float dt){
  float x__ = fx(x,t,x_,t_,F);
  float t__ = ft(x,t,x_,t_,F);
  x_ += x__ * dt;
  t_ += t__ * dt;
  x += x_ * dt;
  t += t_ * dt;
  if(t > 2*PI)t -= 2*PI;
  if(t < -2*PI)t += 2*PI;
}


void setup(){
  size(1280,720,P2D);
}


void draw(){
  background(255);
  pushMatrix();
  translate(1280 / 2, 600);
 
  strokeWeight(1);
  stroke(0,0,0);
  line(-1280/2,0,1280/2,0);
 
  final int N = 100;
  for(int i = 0;i < N;i++)update(0,1.0/60.0/N);
 
  stroke(0,0,0);
  strokeWeight(3);
  rect(10*x-30,-20,60,40);
 
  stroke(255,0,0);
  strokeWeight(6);
  line(10*x,0,10*x + 400*sin(t),-400*cos(t));
 
  popMatrix();
}

コードは上のようになります。\dot{x}をx_(アンダーバー1つ)、\ddot{x}をx__(アンダーバー2つ)のように表記しています。グローバル変数とローカル変数が同名なのは助走付きでぶん殴られても文句が言えないくらい良くないのですが、書き写す時便利なのでこういう書き方をしました。
update関数をfor文で回しているのはフレームレートに対してシステムの固有振動数が高いためdtを短くするためです。分割数Nは適当に決めましたが、処理速度とfloatの精度が許す限り大きい方が良いと思います。

ルンゲ=クッタ法

計算がめんどくさくて精度が良いやつです。

k_1=\dot{x}(t,y(t))
k_2=\dot{x}(t+\frac{dt}{2},y(t)+\frac{dt}{2}k_1)
k_3=\dot{x}(t+\frac{dt}{2},y(t)+\frac{dt}{2}k_2)
k_4=\dot{x}(t+dt,y(t)+dt k_1)

x(t+dt)=x(t)+\frac{k_1+2k_2+2k_3+k4}{6}dt

これを実装するのですが、今回のケースではyがベクトルでありまた微分がそれ自体の関数になっています。まぁ解決法はあるんでしょうが数弱の自分には手のつけようがなかったので内部的にはオイラー法を使いました。(二階微分にルンゲクッタ適用するだけでもそれなりに精度向上するやろ!!のお気持ち)

さきほどのコードのupdate関数を以下に書き換えます。

void update(float F,float dt){
  float kx1 = fx(x,t,x_,t_,F);
  float kt1 = ft(x,t,x_,t_,F);
  float kx2 = fx(x+x_*dt/2, t+t_*dt/2, x_+kx1*dt/2,t_+kt1*dt/2, F);
  float kt2 = ft(x+x_*dt/2, t+t_*dt/2, x_+kx1*dt/2,t_+kt1*dt/2, F);
  float kx3 = fx(x+x_*dt/2, t+t_*dt/2, x_+kx2*dt/2,t_+kt2*dt/2, F);
  float kt3 = ft(x+x_*dt/2, t+t_*dt/2, x_+kx2*dt/2,t_+kt2*dt/2, F);
  float kx4 = fx(x+x_*dt, t+t_*dt, x_+kx3*dt,t_+kt3*dt, F);
  float kt4 = ft(x+x_*dt, t+t_*dt, x_+kx3*dt,t_+kt3*dt, F);
 
  x_ += (kx1 + 2*kx2 + 2*kx3 + kx4) * dt / 6;
  t_ += (kt1 + 2*kt2 + 2*kt3 + kt4) * dt / 6;
 
  x += x_ * dt;
  t += t_ * dt;
  if(t > 2*PI)t -= 2*PI;
  if(t < -2*PI)t += 2*PI;
}

(合ってんのかなこれで……?)

比較

青がオイラー法、赤がルンゲ=クッタ法です。


誤差が出やすい初期値(頂点で低速になる)を設定したとはいえ、回転方向が反転するほど差があるとは思いませんでした。

とはいえまともな制御が入っていれば多少の誤差はフィードバックで吸収されるはずなので、そこまで気にしなくていいようにも思います正直。

 

これでシミュレーターができたので次回はLQRを適用して立たせたいと思います。
ご覧いただきありがとうございました。

倒立振子の運動方程式を求めた話

倒立振子の制御がやってみたいのでシミュレーターを作ります。

 

運動方程式をインターネッツの海から拾ってきて組み込んだんですが、

地面近くで加速したり、
なにもないところで跳ね返ったり、
外力がないのに左右に振動しだしたり、

↑正しくない物理法則

 

と振り子君が我々の宇宙における常識的な挙動から外れてエネルギー保存則に違反しまくるため自分で導出することにしました。やはり人から与えられたものは信用してはいけませんね。この世界で信じられるのは自分だけです。

 

座標系の定義

 

「神は『座標あれ』と言われた。すると座標があった。」(創世記 1章3節)

 

M:台車の質量

m:一様な棒の質量

l:一様な棒の長さ

g:重力加速度

x:台車の位置

\theta:棒の傾き

F:台車を引く力

ごく普通の座標定義です。棒の半分の長さをlとしたり棒の慣性モーメントを別に定義するやり方もあるらしいですが、シミュレーターを作るだけなので一様な棒です。

運動エネルギーと位置エネルギーの計算

台車の運動エネルギー

K_M=\frac{1}{2}M\dot{x}^2

棒の重心の速度

v_m=\sqrt{\left(\dot{x}+\frac{l}{2}\cos{\theta}\dot{\theta}\right)^2+\left(\frac{l}{2}\sin{\theta}\dot{\theta}\right)^2} =\sqrt{\dot{x}^2+l\cos{\theta}\dot{x}\dot{\theta}+\frac{l^2}{4}\dot{\theta}^2}

一様な棒の重心まわりの慣性モーメント

I_m=\frac{1}{12}ml^2

棒の運動エネルギー

K_m=\frac{1}{2}mv_m^2+\frac{1}{2}I_m\dot{\theta}^2=\frac{1}{2}m\left(\dot{x}^2+l\cos{\theta}\dot{x}\dot{\theta}+\frac{l^2}{4}\dot{\theta}^2\right)+\frac{1}{2}\left(\frac{1}{12}ml^2\right)\dot{\theta}^2

系の運動エネルギー

K=K_M+K_m

位置エネルギーは棒の部分のみです。
系の位置エネルギー

U=\frac{mgl}{2}\cos{\theta}

 

ラグランジュ方程式

ラグランジアンL=K-Uが求まったのでラグランジュ方程式を解いていきます。

 

まずxについて

\frac{\partial L}{\partial \dot{x}}=M\dot{x}+\frac{1}{2}m\left(2\dot{x}+l\cos{\theta}\dot{\theta}\right)\\ \frac{\partial}{\partial t}\left(\frac{\partial L}{\partial \dot{x}}\right)=(M+m)\ddot{x}+\frac{lm}{2}\cos{\theta}\ddot{\theta}-\frac{lm}{2}\sin{\theta}\dot{\theta}^2

\frac{\partial L}{\partial x}=0

これを代入すると

\frac{\partial}{\partial t}\left(\frac{\partial L}{\partial \dot{x}}\right)-\frac{\partial L}{\partial x}=(M+m)\ddot{x}+\frac{lm}{2}\cos{\theta}\ddot{\theta}-\frac{lm}{2}\sin{\theta}\dot{\theta}^2=F

 

次に\thetaについて

\frac{\partial L}{\partial \dot{\theta}}=\frac{1}{2}m\left(l\cos{\theta}\dot{x}+\frac{l^2}{2}\dot{\theta}\right)+\frac{1}{2}\left(\frac{1}{12}ml^2\right)2\dot{\theta}^2=\frac{lm}{2}\cos{\theta}\dot{x}+\frac{l^2m}{3}\dot{\theta}\\ \frac{\partial}{\partial t}\left(\frac{\partial L}{\partial \dot{\theta}}\right)=\frac{lm}{2}\cos{\theta}\ddot{x}-\frac{lm}{2}\sin{\theta}\dot{x}\dot{\theta}+\frac{l^2m}{3}\ddot{\theta}

\frac{\partial L}{\partial \theta}=\frac{1}{2}m\left(-l\sin{\theta}\dot{x}\dot{\theta}\right)+\frac{mlg}{2}\sin{\theta}

これを代入すると

\frac{\partial}{\partial t}\left(\frac{\partial L}{\partial \dot{\theta}}\right)-\frac{\partial L}{\partial \theta}=\frac{lm}{2}\cos{\theta}\ddot{x}-\frac{lm}{2}\sin{\theta}\dot{x}\dot{\theta}+\frac{l^2m}{3}\ddot{\theta}+\frac{ml}{2}\sin{\theta}\dot{x}\dot{\theta}-\frac{mlg}{2}\sin{\theta}=0

\frac{lm}{2}\cos{\theta}\ddot{x}+\frac{l^2m}{3}\ddot{\theta}-\frac{mlg}{2}\sin{\theta}=0

両辺を\frac{ml}{2}で割って

\cos{\theta}\ddot{x}+\frac{2l}{3}\ddot{\theta}-g\sin{\theta}=0

 

式変形

答えとしては以下のようになります。

\left \lbrace \begin{array}{l}(M+m)\ddot{x}+\frac{lm}{2}\cos{\theta}\ddot{\theta}-\frac{lm}{2}\sin{\theta}\dot{\theta}^2=F\\ \cos{\theta}\ddot{x}+\frac{2l}{3}\ddot{\theta}-g\sin{\theta}=0\end{array}\right.

ただプログラマーからすると「連立方程式渡されても…」って感じですよね。
なのでそれぞれについて解いた形にします。

\ddot{x}=\frac{4F-3mg\sin{\theta}\cos{\theta}+2lm\sin{\theta}\dot{\theta}^2}{4\left(M+m\right)-3m\cos ^2\theta}

\ddot{\theta}=\frac{6\left(M+m\right)g\sin{\theta}-3\cos{\theta}\left(2F+lm\sin{\theta}\dot{\theta}^2\right)}{l\left(4\left(M+m\right)-3m\cos ^2\theta\right)}

「3とかあって汚い」とか言わないでください。そうなっちゃったんだもん。

 

\thetaが十分小さいときは以下のように線形化できます。

\ddot{x}=-\frac{3mg}{4M+m}\theta+\frac{4}{4M+m}F

\ddot{\theta}=\frac{6\left(M+m\right)g}{\left(4M+m\right)l}\theta-\frac{6}{\left(4M+m\right)l}F

 

導出した運動方程式を組み込んだシミュレーターの動作はこちら。

↑正しい物理法則

これこれ、こいうのでいいんだよ。

 

とはいえグラフだけ見てても気づかないことは多いので見せ方は大事だと思います。「正しいっぽい」「正しくないっぽい」という感覚は意外と大切ですね。
余談ですがこの記事数式が多いので書くのがクッソ大変でした。

 

次回は今回導出した運動方程式と作ったシミュレーターでLQRをやってみます。

Arduino IDEからSTM32のDSPを使ってみた話

近頃はヘテロジニアスコンピューティング、ハードウェアアクセラレータとか、プログラマブルうんたらとか、各種のエンコーダデコーダFPGAGPGPU等々まさにアクセラレータ花盛りですね。ムーアの法則の限界が見えてきた昨今、消費電力をケチりつつ性能を上げるには汎用プロセッサより専用回路のほうが都合がいいということらしいです。

 

 

ところでマイコンに慣れてきたらDSPという言葉はよく耳にすると思います。デジタルシグナルプロセッサ、まぁ早い話が音声とかのデータをソフトで処理すると実時間に間に合わないてんで、浮動小数点演算とか積和演算やらをハードウェアで実装しちまえというわけですね。

 

通常ならCPUとレジスタでコネコネして浮動小数点演算するところ、横に載ってるブラックボックスにデータを投げ込むと、よくわからんけど答えがお出しされるという代物です。

 

 

先述のとおりの近頃のアクセラレータ流行りにのって、FPGAやらを物色していたんですがHDLを勉強するのが面倒まずは基本に立ち返って身近なマイコンにのっているDSPをやってみようと思ったわけです。

 

STM32duino

話は変わりますが、STM32duino皆さん使ってますか?自分は以前までCubeIDE、HAL、C言語でしこしこ書いていたわけですが、ここのところのSTM32duinoは凄いです。

 

記憶が正しければ最初はSTM32F1xxとSTM32F4xxにしか対応してなかったと思うんですが、STMicroelectronicsが正式にサポートするようになってから爆速で対応ボードが増え続けて2.6.0現在540種類というお化けライブラリになっております。

 

しかもこれ偉いのが、どっかのマイコンみたいにソフトウェアPWMでお茶を濁すようなことをせず、各種ペリフェラルがハードで実装されています。純正Arduinoでは制約が多いServoライブラリがタイマ1個で12本までPWMが出せるようになっていたり、公式IDEでいじれるところは基本いじれるようになっているなど、親切かつ気合の入った作りになっています。

さらにLoRaやBLEまで統合し始めて、お前はどこに向かっているんだと言う感じです。

 

その流れでARMのDSPライブラリであるCMSIS-DSPが追加されているのに気づいたので、Arduino IDEで試してみることにしました。

CMSIS-DSP

曰く

CMSIS-DSPライブラリは、ArmがさまざまなCortex-Mプロセッサーコア用に最適化したリッチなDSP機能のコレクションです。CMSIS – Arm®

だそうです。

 

DSPの元来の用途である

  • 各種フィルタ
  • FFT

のほか

  • 高速化算術関数
  • 行列演算
  • 三相モータ制御に使う計算関数
  • 姿勢制御に便利なQuaternion
  • PID

そのほか組込やさんには馴染みが薄そうな

などの関数も含まれています。

 

検証

STM32のなかでCortex-M4とCortex-M7のチップにはDSPが載っています。

家にはCortex-M3とM0+とXtensa、RISC-Vマイコンとかしかなかったので部室に転がってたNucleo-32 STM32G431KBを使いました。

#include "CMSIS_DSP.h"

特に追加の設定などなくインクルードするだけで使えます。

#include "CMSIS_DSP.h"

void setup() {
  Serial.begin(115200);
}

void loop() {
  Serial.println("N,sum(nomal),time(nomal)[us],sum(dsp),time(dsp)[us],");
  for(int n = 10;n <= 10000000;n*=10){
    Serial.print(n);Serial.print(',');  
    float x,dx,sum;
    unsigned long start_time,end_time;
   
    x = 0,dx = PI/n,sum = 0;
    start_time = micros();
    for(int i = 0;i < n;i++){
      sum += sin(x) * dx;
      x += dx;
    }
    end_time = micros();
    Serial.print(sum,6);Serial.print(',');
    Serial.print(end_time - start_time);Serial.print(',');
   
    x = 0,dx = PI/n,sum = 0;
    start_time = micros();
    for(int i = 0;i < n;i++){
      sum += arm_sin_f32(x) * dx;
      x += dx;
    }
    end_time = micros();
    Serial.print(sum,6);Serial.print(',');
    Serial.print(end_time - start_time);Serial.print(',');
   
    Serial.println();
  }
  delay(3000);
}

上記コードで分割数ごとにsinを0~πまで積分した結果を求め、時間を計測しました。

(最適化オプションによる違いはほとんど有りませんでした)

結果

N sum(nomal) time(nomal)[us] sum(dsp) time(dsp)[us]
10 1.983523 13 1.983499 5
100 1.999835 89 1.99981 40
1000 1.999974 836 1.99995 324
10000 1.999972 8176 1.999948 3087
100000 1.997334 81354 1.997307 30628
1000000 2.017668 812448 2.017648 306029
10000000 2.000000 8125558 2.000000 3060007

DSP有りの場合6割ほど高速化されているのがわかると思います。

さらにN=10程度の少ない回数でも同じ割合で早くなっています。

副次的効果としてDSP有り関数で書いた場合、math.hで書いた場合に比べ2KBほどバイナリサイズが小さくなっていました。算術関数を多用するプログラムでFLASHがカツカツのときには使えるかもしれません。

 

以上になります。簡単なのでぜひ皆さんもSTM32duinoでDSP試してみてください。

本当はマイクをADCに繋いでDSPでフィルタ処理、ADCに出力とかやってみたかったんですがマイクが見つからなかったです。見つけたらやってみようと思います。

ご覧いただきありがとうございました。

ヘッドライトをライジングαに交換した話(レビュー)

どうも、久しぶりにCDKに乗ったら出先で故障して、夜中の1時に後輩に迎えに来てもらった「だん」です。

ちなみにCDKは後日別の後輩に頼んで家までトランポしてもらいました。

ここのところ先輩としての株がロシア国債もかくやというレベルで下がりっぱなしです。

 

というわけで今回はVTRのヘッドライト交換です。

数年ぶりに長距離ツーリングを敢行して旅する楽しさを思い出し、VTRツーリング仕様化への第一歩として豆電球みたいな明るさしかないヘッドライトをアップグレードしようと思い立ったわけです。

交換に際して交換前のバルブを確認したところ、繰り返しの熱による影響か、フィラメントが歪んでいたのでちょうどいいタイミングとも言えます。

 

取付作業

写真撮り忘れました。

 

 

まぁこんな作業いくらでもネットにあると思うのでそちらを参照してください。

開けたことがない車両だとレンズ上部の爪を外すのが面倒かもしれません。

撮影のために何度かバルブの取付・取り外しを行ったのですが、バルブによっては端子が固くてなかなか抜けねーです。気合で引っこ抜いてください。

 

これまでついていたのはこちらです。

……ってコレ四輪用じゃねーーーか!!

購入時のままなので4年間気づかず乗ってました(意外と大丈夫なのかもしれない)。

スペックはH4 3800K 12V 60/55Wです。ルーメンは記載がありませんでした。

 

交換したのはこちらです。スフィアライト RIZINGアルファです。

スペックはH4 6000K 12V 12W 明るさは1800ルーメンです。

これを選んだ理由は、近所のお城の駐車場で集まって話してるVT250F乗りのおじ…お兄さんが激推ししてたからです。

 

比較

まず日中の見た目についてです。色温度のかなり低い暖色のバルブから一気に白色へ変化したため違いは大きいです。

昔懐かしい豆電球カラーから一気に現行車みたいなツラになりました。

C125みたいな絶妙な違和感を感じますね。

 

続いて本命の夜間の視野比較です。一眼レフのマニュアルモードを用いて同じ設定で撮影しました。

個人的にはbefore側もっと暗い感じになるかと思っていたのですが、明るさに関しては視野中央では十分仕事をしていますね。一方で視野全体の明るさ、特に手前と左右に関してはスフィアライトが圧倒的です。これは配光の巧拙も関係していると思われます。

体感では特に十数m先の左右が明るくなったため、かなり視野が広がったように感じました。これは走行中はさらに顕著に感じられます。

また色が白色に近づいたため、見やすさという点でも性能アップです。

 

感想

明るくなって大満足です。視界が明瞭になってストレスフリーなツーリングが楽しめそうです。

ただ色味が変わって青成分も強めになったので、見続けたときの目の疲れなどあれば追記します。

 

これに引き続いてスクリーンが発注済みで、夏までにVTRツーリング仕様化計画を完遂するつもりです。

(CDKも直さないとなぁ…)

 

以上になります。ご覧いただきありがとうございました

ESP32C3でPWMを出力する方法(サーボモータ用に)

最近卒論から目を背けつつESP32C3を使って遊んでます。

性能に比してお値段が異常に安いです。

しかも秋月で手に入る。

akizukidenshi.com

外部アンテナの開発ボードもこの安さです。

akizukidenshi.com

超小型でバッテリー駆動可能、リモートIDに必要なBluetooth LE Coded PHY/Long Range対応ということで(自力での登録が可能化は別として)飛びもの搭載を考えている人も多いと思います。

 

自分もとりあえずということでBLEの飛距離テストを極寒の堤防で実施しました。

 

メインストリームのESP32無印やS3と違ってRISC-Vが1コアのシンプル構成、低消費電力、USBシリアル内蔵と素性がよくハマるポイントは少ないと思うのですが、サーボモータを動かそうとして少々困ったので記事にしました。

(ちなみに他の落とし穴としてはBLEのみ対応で通常のBluetoothに対応していない点があります(1敗))

 

というわけで、XIAO ESP32C3でPWMを出してみます。

サーボモータを想定して50Hz、500~2400μsのパルスです。

 

一応ライブラリもあるよ

ESP32Servo.hというライブラリがありまして、ArduinoIDEのライブラリマネージャーからもダウンロードできます。

github.com

本家のServo.hとほとんど同じ使い勝手で使えるのですが、なぜか2ピンまでしか制御できません。サーボが2個までなら便利だと思います。

とりあえずデーターシートを読む

https://www.espressif.com/sites/default/files/documentation/esp32-c3_technical_reference_manual_en.pdf

曰く

– LED PWM controller, with up to 6 channels

PWMは最大6チャンネルまでらしいです。なぜか本マイコンではPWMはLED用ということになってます。

 

曰く

Four independent timers that support division by fractions (31.2)

4つの分周可能な独立したタイマーがあるらしいです。

 

少し戻って曰く

The accuracy of duty cycle can be up to 18 bits. (3.4.7)

若干分かりづらいんですが分周比の分母は最大18bitらしいです。

分周に関してはかなり変な仕様になってます(31.3.2.2参照)。

 

曰く

Maximum PWM resolution: 14 bits (31.2)

PWMの解像度は最大14bitらしいです。

 

わかりやすい図があったので引用します。

分周の仕様が奇っ怪で謎なんですが結論からいうと、APBクロック80MHzを使った場合、最大40MHz、最低3Hz(←なぜ?)での出力が可能でした。

 

またTable 10より

LED PWM ledc_ls_sig_out0~5 Any GPIO pins

GPIOならどのピンからでもPWMが出力可能です(実際XIAOでもTx、Rxを除くD0~D5,D8~D10でできました)。

 

というわけでPWMは6チャンネル、どのGPIOからでも出力可能ですがタイマが4つなので周波数は4種類ということがわかりました。

SDKを読む

続いてArduinoIDEのpackageからESP32のSDKを読みます。

PWMの設定に使う関数はesp32-hal-ledc.hのledcSetup()です。

ソースファイルに以下の記述がありました。

/*
 * LEDC Chan to Group/Channel/Timer Mapping
** ledc: 0  => Group: 0, Channel: 0, Timer: 0
** ledc: 1  => Group: 0, Channel: 1, Timer: 0
** ledc: 2  => Group: 0, Channel: 2, Timer: 1
** ledc: 3  => Group: 0, Channel: 3, Timer: 1
** ledc: 4  => Group: 0, Channel: 4, Timer: 2
** ledc: 5  => Group: 0, Channel: 5, Timer: 2
** ledc: 6  => Group: 0, Channel: 6, Timer: 3
** ledc: 7  => Group: 0, Channel: 7, Timer: 3
** ledc: 8  => Group: 1, Channel: 0, Timer: 0
** ledc: 9  => Group: 1, Channel: 1, Timer: 0
** ledc: 10 => Group: 1, Channel: 2, Timer: 1
** ledc: 11 => Group: 1, Channel: 3, Timer: 1
** ledc: 12 => Group: 1, Channel: 4, Timer: 2
** ledc: 13 => Group: 1, Channel: 5, Timer: 2
** ledc: 14 => Group: 1, Channel: 6, Timer: 3
** ledc: 15 => Group: 1, Channel: 7, Timer: 3
*/

周波数を個別に設定したい場合はTImerが重ならないようにする必要があるため、一つ飛ばしで使えばいいことがわかります。

(実際は何種類も周波数が必要になるということはないと思いますが)

PWM出力する

まず1チャンネルのときです。

#define LEDC_CHANNEL 0
#define LEDC_PIN D0
#define LEDC_FREQUENCY 50
#define LEDC_RESOLUTION 14
#define LEDC_TIM_CLOCK 80000000

void setPulseWidthMicroseconds(uint32_t us){
    ledcWrite(LEDC_CHANNEL,(1<<LEDC_RESOLUTION)*us*LEDC_FREQUENCY/1000000);
}

void setup() {
  ledcSetup(LEDC_CHANNEL,LEDC_FREQUENCY,LEDC_RESOLUTION);
  ledcAttachPin(LEDC_PIN,LEDC_CHANNEL);
}

void loop() {
  setPulseWidthMicroseconds(500);
  delay(1000);
  setPulseWidthMicroseconds(2400);
  delay(1000);
}

サーボ用にパルス幅指定になってます。

タイマ設定はたぶんレジスタを直接叩いてもできるのですが、先述のややこしい分周設定があるのでledcSetup()でよしなにしてもらったほうがいいと思います。

 

複数チャンネルの場合は

#define LEDC_RESOLUTION 14

void setup() {
  ledcSetup(0,100,LEDC_RESOLUTION);
  ledcSetup(1,100,LEDC_RESOLUTION);//周波数ch0と共通
  ledcSetup(2,300,LEDC_RESOLUTION);
  ledcSetup(3,300,LEDC_RESOLUTION);//周波数ch2と共通
  ledcSetup(4,500,LEDC_RESOLUTION);
  ledcSetup(5,500,LEDC_RESOLUTION);//周波数ch4と共通
 
  ledcAttachPin(D0,0);
  ledcAttachPin(D1,1);
  ledcAttachPin(D2,2);
  ledcAttachPin(D8,3);
  ledcAttachPin(D9,4);
  ledcAttachPin(D10,5);

  ledcWrite(0,100);
  ledcWrite(1,200);
  ledcWrite(2,400);
  ledcWrite(3,800);
  ledcWrite(4,1600);
  ledcWrite(5,3200);
}

void loop() {
}

ほとんど同じですね。

ただピンとチャンネルで動かない組み合わせがあったので、データーシートをもうちょっと読み込む必要があるかもしれません。

 

以上になります。

小型軽量かつ内蔵アンテナでもそこそこ飛び、無線なしマイコンとしても優秀なチップなのでいろいろ使えそうです。なにより安いし、在庫があります。ESPシリーズの特徴(?)だった大消費電力もないので基板自作もし易いと思います(欲をいうとSDIOがあったら最強だった)。

 

ポールタイプの外部アンテナも買ってみたので、そのうちBLE飛距離更新を目指して実験するかもしれません。

3次元の主応力とその方向を求めてみた

どうも。最近ものづくりできてないせいで研究で書いたソースをネタにブログを書くハメになっているだんです。

今回は3次元各軸の垂直応力、せん断応力から主応力とその方向(方向余弦)を求めるプログラムを書いたので紹介します。研究の内容には全く関係ないしコードは全部オリジナルなので好きに使ってください。

 

こちらを参考にさせていただきました。

構造力学メモ(その1) 3次元の主応力とミーゼス応力|Kou|note

 

求め方

基本的には3×3行列の固有値固有ベクトルを求める操作になります。

応力テンソルを以下のように表します。

このとき以下になる固有値λが主応力で

以下のようになる固有ベクトルxが主応力の方向余弦です

ただし参考にさせていただいたサイトによると偏差応力を使った方が簡単らしいので平均応力σ_mを垂直応力から引いた偏差応力テンソルs固有値を求めます。

このとき

|s-λI|=-λ^3+J_2λ+J_3=0

となります。J_2J_3は応力の不変量というやつで

J_2=s_{xy}s_{yx}+s_{yz}s_{zy}+s_{zx}s_{xz}-s_{xx}s_{yy}-s_{yy}s_{zz}-s_{zz}s_{xx}

J_3=s_{xx}s_{yy}s_{zz}+s_{xy}s_{yz}s_{zx}+s_{xz}s_{zy}s_{yx}-s_{yz}s_{zy}s_{xx}-s_{zx}s_{xz}s_{yy}-s_{xy}s_{yx}s_{zz}

で求められます(前述のサイト様が超参考になりました)。

三次方程式の解はいくつか求める方法があるらしいですが、コンピューターに解かせるのでビエトの解を使います。これは

x^3-Ax-B=0

の解が

θ=\frac{1}{3} \arccos{(\frac{3\sqrt{3}B}{2\sqrt{A^3}})}として

x=\frac{2}{3}\sqrt{3A}\cos{θ},\frac{2}{3}\sqrt{3A}\cos{(θ+240°)},\frac{2}{3}\sqrt{3A}\cos{(θ+120°)}

となるというものです。

これで求めた偏差応力テンソル固有値λ_1,λ_2,λ_3に平均応力を足して応力テンソル固有値つまり主応力を得ます。

σ_1=σ_m+λ_1\\σ_2=σ_m+λ_2\\σ_3=σ_m+λ_3

 

続いて各固有値に対応する固有ベクトルを求めます。

これもいくつか方法がありますがここではガウス・ザイデル法を用いました。

ガウス=ザイデル法 - Wikipedia

ざっくりいうとA\vec{x}=\vec{b}を解くための方法です。こんな簡単な操作で解が求まるの素晴らしいですね。さすがガウス先生です(?)

ここでは固有値を求めたいので定義式どおりA=σ-σ_nI,b=\vec{0}として3回計算します。(2回計算した時点で直交するベクトルを求めた方が早い気もする)(誤差だよ誤差)

注意点として最後に正規化しないと単位ベクトルになりません。

プログラム

C++で書きました。というか例によって最初C#で書いてブログ用に書き直しました。

他でも使えそうなガウス・ザイデル法のところだけ関数化してあります。引数の配列とdimを書き換えればn×n行列で解けるはずです(たぶん)。

 

#include "iostream"
#include "cmath"

void GaussSeidelMethod(double A[3][3],double b[3],double x[3]){
    const int dim = 3;

    double x_[dim];
    for(int i = 0;i < dim;i++)x_[i] = 1;
    
    for (int i = 0; i < 1000; i++){
        double _x[dim];
        for(int j = 0;j < dim;j++){
            _x[j] = b[j];
            for(int k = 0;k < j;k++){
                _x[j] -= A[j][k]*_x[k];
            }
            for(int k = j + 1;k < dim;k++){
                _x[j] -= A[j][k]*x_[k];
            }
            _x[j] /= A[j][j];
        }
        
        bool stop = true;
        double th = 0.000001;
        for(int j = 0;j < dim;j++){
            if(std::abs(_x[j] - x_[j]) > th){
                stop = false;
                break;
            }
        }

        if(stop){
            for(int j = 0;j < dim;j++){
                x[j] = _x[j];
            }
        }else{
            for(int j = 0;j < dim;j++){
                x_[j] = _x[j];
            }
        }
    }
}

int main(int argc, char* argv[])
{
    double sx;
    double sy;
    double sz;
    double txy;
    double tzx;
    double tyz;

    std::cin >> sx >> sy >> sz >> txy >> tyz >> tzx;

    double sm = (sx + sy + sz) / 3.0;         //平均応力
    double s[3][3] = {                        //偏差応力テンソル
        { sx - sm, txy, tzx},
        { txy, sy - sm, tyz},
        { tzx, tyz, sz - sm},
    };
    
    //第2不変量
    double J2 = -(s[0][0] * s[1][1] + s[1][1] * s[2][2] + s[2][2] * s[0][0])
                +(s[0][1] * s[1][0] + s[2][1] * s[1][2] + s[2][0] * s[0][2]);

    //第3不変量
    double J3 = s[0][0] * s[1][1] * s[2][2] 
              + s[0][1] * s[1][2] * s[2][0] 
              + s[0][2] * s[2][1] * s[1][0] 
              - s[1][2] * s[2][1] * s[0][0] 
              - s[2][0] * s[0][2] * s[1][1] 
              - s[0][1] * s[1][0] * s[2][2];

    double alpha = acos(3.0 * sqrt(3.0) * J3 / 2.0 / sqrt(J2 * J2 * J2)) / 3.0;

    double sn[3] = {
        2.0 / 3.0 * sqrt(3.0 * J2) * cos(alpha) + sm,
        2.0 / 3.0 * sqrt(3.0 * J2) * cos(alpha + 240.0 / 180.0 * M_PI) + sm,
        2.0 / 3.0 * sqrt(3.0 * J2) * cos(alpha + 120.0 / 180.0 * M_PI) + sm
    };

    std::cout << sn[0] << "\t" << sn[1] << "\t" << sn[2] << std::endl;

    double cosx[3];
    double cosy[3];
    double cosz[3];

    for (int i = 0; i < 3; i++)
    {
        
        double A[3][3] = {
            { sx - sn[i], txy, tzx},
            { txy, sy - sn[i], tyz},
            { tzx, tyz, sz - sn[i]},
        };

        double b[3] = {0,0,0};
        double x[3];
        GaussSeidelMethod(A,b,x);
        double abs = sqrt(x[0]*x[0]+x[1]*x[1]+x[2]*x[2]);

        cosx[i] = x[0] / abs;
        cosy[i] = x[1] / abs;
        cosz[i] = x[2] / abs;
    }

    std::cout << cosx[0] << "\t" << cosx[1] << "\t" << cosx[2] << std::endl;
    std::cout << cosy[0] << "\t" << cosy[1] << "\t" << cosy[2] << std::endl;
    std::cout << cosz[0] << "\t" << cosz[1] << "\t" << cosz[2] << std::endl;
    return 0;
}

ただしガウス・ザイデル法の結果でNaNが返ってくることが稀にあったので計算途中にも正規化した方がいいのかもしれません。ちなみに最初ヤコビ法で書いていたのですがなぜか結果が振動し始めたのでガウス・ザイデル法にしました。小数点以下6桁で打ち切っていますが反復回数5~6回くらいで求まっているようです。

 

閲覧いただきありがとうございました。