バネマスダンパ系のPIDシミュレーション

はじめに

前回記事のプログラムを改良してPIDシミュレーションしてみた negizoku.hatenablog.jp

この方程式をちょっと改造してPID制御を組み込む

 \displaystyle
\frac{d}{dt}

\begin{pmatrix}
x  \\v  \\
\end{pmatrix}
=
\begin{pmatrix}
0 & 1\\
-c/m & -k/m \\
\end{pmatrix}

\begin{pmatrix}
x  \\v  \\
\end{pmatrix}

+

\begin{pmatrix}
0\\ -f/m\\
\end{pmatrix}

時間微分すると変位xになる変数\etaを定義する

 \displaystyle
\frac{d \eta}{dt} =  x

運動方程式の中に\etaを組み込むと、次のような常微分方程式となる

 \displaystyle
\frac{d}{dt} 
\begin{pmatrix} \eta\\  x \\ v\\ \end{pmatrix}
=  
\begin{pmatrix}
0 & 1       & 0\\
0 & 0       & 1\\
0 & -c/m & -k/m\\
\end{pmatrix}
\begin{pmatrix} \eta\\  x \\ v\\ \end{pmatrix}
+
\begin{pmatrix}  0\\  0 \\  f(\eta,x,v,t)/m\\ \end{pmatrix}

ここで外力f(\eta,x,v,t)

 \displaystyle
 f(\eta,x,v,t)
=  
\begin{pmatrix}  K_i\\  K_p \\ K_d\\ \end{pmatrix}
\cdot
\left\{
\begin{pmatrix}  \int \left(x- x_{target}\right) dt\\x- x_{target} \\ \frac{d}{dt}\left(x- x_{target} \right) \\ \end{pmatrix}
\right\}
\\=  
\begin{pmatrix}  K_i\\  K_p \\ K_d\\ \end{pmatrix}
\cdot
\left\{
\begin{pmatrix}  \eta\\  x \\ v\\ \end{pmatrix}
-
\begin{pmatrix}  \int  x_{target} dt\\ x_{target} \\ \frac{d}{dt} x_{target}\\ \end{pmatrix}
\right\}

このときx_{target}=\rm{const.}なら

 \displaystyle
 f(\eta,x,v,t)
=  
\begin{pmatrix}  K_i\\  K_p \\ K_d\\ \end{pmatrix}
\cdot
\left\{
\begin{pmatrix}  \eta\\  x \\ v\\ \end{pmatrix}
-
\begin{pmatrix}  t x_{target}\\ x_{target} \\ 0\\ \end{pmatrix}
\right\}

実装

from scipy.integrate import odeint
import matplotlib.pyplot as plt
import numpy as np


class Simulation:
    def __init__(self, m, c, k, x0, v0):
        self.setSystem(m, c, k)
        self.intiVal(x0,v0)

    def setSystem(self, m, c, k):
        self.m=m
        self.c=c
        self.k=k
    
    def intiVal(self, x0, v0):
        self.u0 = [0, x0, v0]

    def exForce(self, t):
        return 0

    def equationOfMotion(self, u, t):
        self.u=u
        f = self.exForce(t)
        A = np.array([
            [0,1,0],
            [0,0,1],
            [0,-k/m, -c/m]])
        b = np.array([0,0,f/m])
        return np.dot(A, u) + b

    def do(self, times):
        u0 = np.copy(self.u0)
        orbit=odeint(self.equationOfMotion, u0, times)
        return (orbit[:,1], orbit[:,2])

class PID(Simulation):
    def __init__(self, m, c, k, x0, v0):
        super().__init__(m, c, k, x0, v0)
    
    def setCoefficients(self, kp, ki, kd):
        self.coefficients = np.array([ki, kp, kd])
    
    def setTarget(self, target):
        self.target = target

    def exForce(self, t):
        errors =  np.array([t*self.target, self.target, 0])-self.u
        return np.dot(self.coefficients, errors)



if __name__ =="__main__":
    m=1
    c=2
    k=0
    x0 = 0
    v0 = 0
    sim = PID(m,c,k,x0,v0)
    sim.setTarget(10)
    sim.setCoefficients(20, 20, 10)
    
    dt = 0.001
    stop = 10
    times=np.arange(0., stop+dt, dt)
    x,v = sim.do(times)

    plt.plot(times,x)
    plt.xlim(0,stop)
    plt.xlabel("t[s]")
    plt.ylabel("x[m]")
    plt.grid()
    plt.show()

結果

f:id:negizoku:20211120205853p:plain

目標値を時間関数にする

'''python from scipy.integrate import odeint from scipy import integrate from scipy.misc import derivative import matplotlib.pyplot as plt import numpy as np from math import sin, pi

class Simulation: def init(self, m, c, k, x0, v0): self.setSystem(m, c, k) self.intiVal(x0,v0)

def setSystem(self, m, c, k):
    self.m=m
    self.c=c
    self.k=k

def intiVal(self, x0, v0):
    self.u0 = [0, x0, v0]

def exForce(self, t):
    return 0

def equationOfMotion(self, u, t):
    self.u=u
    f = self.exForce(t)
    A = np.array([
        [0,1,0],
        [0,0,1],
        [0,-k/m, -c/m]])
    b = np.array([0,0,f/m])
    return np.dot(A, u) + b

def do(self, times):
    u0 = np.copy(self.u0)
    orbit=odeint(self.equationOfMotion, u0, times)
    return (orbit[:,1], orbit[:,2])

class PID(Simulation): def init(self, m, c, k, x0, v0): super().init(m, c, k, x0, v0)

def setCoefficients(self, kp, ki, kd):
    self.coefficients = np.array([ki, kp, kd])

def target(self,t):
    return sin(t/5 *2*pi)

def exForce(self,t):
    dt=0.00001
    u_target=np.array([
        integrate.quad(self.target, 0, t)[0],
        self.target(t),
        (self.target(t+dt)-self.target(t+dt))/2/dt
    ])

    errors =  u_target-self.u
    return np.dot(self.coefficients, errors)

if name =="main": m=1 c=2 k=0 x0 = 0 v0 = 0 sim = PID(m,c,k,x0,v0) sim.setCoefficients(100, 0, 10)

dt = 0.001
stop = 20
times=np.arange(0., stop+dt, dt)
x,v = sim.do(times)

plt.plot(times,x)
plt.xlim(0,stop)
plt.xlabel("t[s]")
plt.ylabel("x[m]")
plt.grid()
plt.show()
# 出力結果
[f:id:negizoku:20211120215456p:plain]

##目標の書き換え
def target(self,t):
    return float(int(t)%2)
[f:id:negizoku:20211120221213p:plain]

SciPyを使ってバネマスダンパ系のシミュレーションをやってみる

理論

scipy.integrate.odeintでは常微分方程式 \frac{d}{dt}\vec{u} =f\left( \vec{u} \right) を解くことができる。 バネマスダンパ系の運動方程式 \frac{d}{dt}\vec{u} =f\left( \vec{u} \right) の形に書き換えれば解を得られる。

バネマスダンパ系の運動方程式は次の通り。ここで Fは外力

 \displaystyle
ma = -cv - kx + F

ここで両辺をmで割る。

 \displaystyle
a = -\frac{c}{m}v -\frac{k}{m}x + \frac{F}{m}

 v= \frac{dx}{dt} であることを考慮すれば、現代制御でおなじみの方程式が導かれる

 \displaystyle
\frac{d}{dt}

\begin{pmatrix}
x  \\v  \\
\end{pmatrix}
=
\begin{pmatrix}
0 & 1\\
-c/m & -k/m \\
\end{pmatrix}

\begin{pmatrix}
x  \\v  \\
\end{pmatrix}

+

\begin{pmatrix}
0\\ -f/m\\
\end{pmatrix}

実装

from scipy.integrate import odeint
import matplotlib.pyplot as plt
import numpy as np

#外力
def exForce(u,t):
    return 0

#運動方程式
def equationOfMotion(u, t, m,c,k):
    f = exForce(u,t)
    A = np.array([
        [0,1],
        [-k/m, -c/m]])
    b = np.array([0,f/m])
    return np.dot(A, u) + b


if __name__ =="__main__":
    #バネマスダンパの係数
    m=1
    c=2
    k=5
    args= (m,c,k)
    
    #初期値
    x0 = 10
    v0 = 0
    u0=[x0,v0]
    
    #計算時間
    dt = 0.001
    stop = 10
    times=np.arange(0., stop+dt, dt)

    #解
    orient = odeint(equationOfMotion, u0, times, args)
    x=orient[:,0]
    v=orient[:,1]

    #グラフ表示
    plt.plot(times,x)
    plt.xlim(0,stop)
    plt.xlabel("t[s]")
    plt.ylabel("x[m]")
    plt.grid()
    plt.show()

出力結果

f:id:negizoku:20211120194827p:plain
mck

外力を変えてみる

ステップ入力

def exForce(u,t):
    if(t>2):
        f=4
    else:
        f=0
    return f

f:id:negizoku:20211120195912p:plain
ステップ応答

正弦波

from math import sin

#外力
def exForce(u,t):
    return 0.5*sin(2*t)

f:id:negizoku:20211120200504p:plain
正弦波応答

読書記録「スポーツ・ルール学への序章」


説明

f:id:negizoku:20210108172604j:plain
表紙

  • 「野球はなぜ9人で行うのか」
  • ラグビーのゴールはなぜ11人なのか」
  • 「テニスの得点はなぜ0, 15, 30, 40, 60なのか」

といった誰もが抱く疑問の答えを探求するのが「スポーツ・ルール学」である。

本書ではまず「日本人の公認ルール神聖視」や「ルール変更の根拠は記録されにくい」といった問題点を指摘する。 その後野球、フットボールがスポーツ化していった経緯について紹介し、ルールの目的について整理した。 そして三章で近代社会のルール観から、アピールの意義と審判の役割拡大についてのべた。 最終章である四章では「スポーツ文化を発展させるために、ルールはどうあるべきか」といった観点から考察をしていった。

※ 公認ルール神聖視とは「ボールに4回触れてから相手に返球してもよいようなルールでやるバレーボールはレベルが低い」といった価値観のことである。そもそもルールは同意の下つくられた幻想にすぎないのだからこのような価値観は受動的だと筆者は批判する。

ロボコンとの関係性について考察

 スポーツルール学的観点からスポーツとロボコンを比較する。

共通点1「共通のルールの基で競い勝利を目的とする」

 ここでの「勝利を目的とする」というのは八百長などを禁止する意味合いが含まれる。スポーツは賭博や賞金など大金が関わることが多いためこれが発生しやすいのである。日本のロボコンは殆ど全て「学生がやるもの」や「趣味の延長線上」として捉えられているせいか、八百長問題などは耳にしない。将来的に職業としてロボコンができるようになれば同様に八百長問題がでてくると考えられる。高専ロボコンにおけるお飾り程度の「アイデアマンシップ」は、スポーツマンシップや非紳士的行為の禁止を参考にしたと思われる。幸か不幸か、この文言が役に立つ日はまだ遠いようである。

 また競技である以上勝敗が存在するため全ての人が同時に同じように楽しむことができないという共通の課題も存在する。ロボコニストの中にはスポーツが下手で落ちこぼれとなってしまった人も多いのではないだろうか。

共通点2「ルール変更の根拠が記録されにくい」

 ルールは競技の水準や安全性、組織の収入増など様々な理由で変更されるがその理由が説明されることは殆どないし、ましてやそれが世間で話題になることは滅多に無い。たいていのルール変更は競技委員会中心で行われる。これはロボコンも同じで、ルール変更の理由が説明されるのは専ら安全対策が理由となる場合のみである。

共通点3「審判の役割が大きい」

 審判が大きな役割を果たすのもスポーツ(特に現代スポーツ)との共通点である。近代のスポーツは決闘などに代表される「法から外れた自己統治の意識」から、審判の役割が小さく選手のアピールや話し合いに依る部分が大きかった。しかし現代になるにつれ、参加者の増加や試合結果の社会的評価や利益との結びつきにより、審判の役割が大きくなっていった。これを参考にしたのか、多くのロボコンでは同様に審判の判断は絶対である。

相違点1「個人の力量を比較するため道具に制限を加える」

 スポーツでは「飛びすぎるボール」や「体の筋肉の凹凸を減らす効果を持つ水着」など記録に有利な道具がが禁止されることがある。これはスポーツが肉体的闘争を重視しているからである。ロボコンではこういったことはあまり起こらない。「強いロボットに優れた技術が載っているのは当然」といった発想があるからかもしれない。

相違点2「文化である」

 スポーツは様々な外圧の影響を受けながらも社会が発展させていったものである。しかしロボコンは生まれて間もないものであるし、外部からの圧力を受けた経験もほとんど無く、社会からの関心もスポーツと比べて小さい。

書籍情報

  • 著者:中村敏雄
  • 発行年:1995/07/10
  • 発行所:大修館書店

OpenCVSharp4で細線化

いろいろプログラムを組もうとしたが、やっぱりライブラリには勝てなかったよ...

using OpenCvSharp;  
using OpenCvSharp.XImgProc;  

namespace thinning  
{  
    class Program  
    {  
        static void Main(string[] args)  
        {  
            Mat src = new Mat(@"D:\OpenCV\curves0.png", ImreadModes.Grayscale);  
            Mat dst = new Mat();  
            CvXImgProc.Thinning(src, dst, ThinningTypes.ZHANGSUEN);  
            Cv2.ImWrite(@"D:\OpenCV\curves1.png", dst);  
        }  
    }  
}  

OpenCVとカメラを使ったフォームアプリケーション

はじめに

カメラで撮影した画像を、OpenCVで処理してからフォームアプリで表示させたかったので作成した。
撮影+画像処理はbackgroundWorker内で行い、timerでbackgroundWorkerを定期的に呼び出すことにした。

ツールのレイアウト

  • TableLayoutPanel
  • PictureBox
  • label
  • backgroundWorker
  • timer
    を配置する

コード

Nugetから「OpenCvSharp4」と「OpenCvSharp4.runtime.win」をインストール
backgroundWorkerやtimerの動作は、「Form1.cs[デザイン]」内の要素をダブルクリックするか、「Form1.Designer.cs」で追加する。

using System;  
using System.Collections.Generic;  
using System.ComponentModel;  
using System.Data;  
using System.Drawing;  
using System.Linq;  
using System.Text;  
using System.Threading.Tasks;  
using System.Windows.Forms;  
using OpenCvSharp;  

namespace cvvideo  
{  
    public partial class Form1 : Form  
    {  
        MyImageProcessing MIP = new MyImageProcessing();  
        private Mat dstImg = new Mat(); //PictureBox表示用のmat  
        private int tickCount = 0; //撮影数をカウントする変数  


        public Form1()  
        {  
            InitializeComponent();  
        }  


        private void Form1_Load(object sender, EventArgs e) //Form起動時の処理  
        {  
            label1.Text = "START";  

            //backgroundworkerの設定  
            backgroundWorker1.DoWork += new DoWorkEventHandler(backgroundWorker1_DoWork);  
            backgroundWorker1.RunWorkerCompleted += new RunWorkerCompletedEventHandler(backgroundWorker1_RunWorkerCompleted);  


            timer1.Enabled = false; //タイマーを止めておく  
            label1.Text = "PictureBoxクリックで撮影開始";  
        }  

        private void backgroundWorker1_DoWork(object sender, DoWorkEventArgs e) //backgroundworkerで行う処理  
        {  
            Mat mat = new Mat();  
            MIP.TakePicture(mat);  
            mat = MIP.ImageProcessing(mat);  
            e.Result = mat; //結果を出力  
        }  

        private void backgroundWorker1_RunWorkerCompleted(object sender, RunWorkerCompletedEventArgs e) //backgroundworkerのDoWork終了時に行う処理  
        {  
            dstImg = (Mat)e.Result; //処理結果を受け取る。  
            updateImg();  
        }  

        private void timer1_Tick(object sender, EventArgs e) //timer起動時に行う処理  
        {  

            if (!backgroundWorker1.IsBusy) //backgroundがビジーでないことを確認  
            {  
                label1.Text = tickCount.ToString() + " tick";  
                tickCount++;  

                backgroundWorker1.RunWorkerAsync(); //backgroundworkerの処理実行  
            }  

        }  

        private void updateImg() //PictureBoxの画像を更新する。  
        {  
            pictureBox1.Image = MIP.MatToBitmap(dstImg);  
        }  


        private void pictureBox1_Click(object sender, EventArgs e)  
        {  
            timer1.Enabled = !timer1.Enabled; //タイマー動作/停止を切り替え  
            if (timer1.Enabled)  
            {  
                label1.Text = "timer enabled";  
            }  
            else  
            {  
                label1.Text = "timer unenabled";  
            }  
        }  


    }  

    public class MyImageProcessing //画像処理クラス  
    {  
        VideoCapture capture = new VideoCapture(0); //撮影用のクラス  

        public Bitmap MatToBitmap(Mat src) //matをbitmapに変換する  
        {  
            return OpenCvSharp.Extensions.BitmapConverter.ToBitmap(src);  
        }  

        public void TakePicture(Mat dst) //カメラを使って撮影する  
        {  

            Mat img = new Mat();  
            capture.Read(img);  

            if (img.Empty()) //撮影失敗  
            {  

            }  
            else //撮影成功  
            {  
                img.CopyTo(dst);  
            }  

        }  

        public Mat ImageProcessing(Mat src) //画像処理  
        {  
            Mat mat = new Mat();  
            src.CopyTo(mat);  
            Cv2.CvtColor(mat, mat, ColorConversionCodes.BGR2GRAY);  
            Cv2.ApplyColorMap(mat, mat, ColormapTypes.Cividis);  

            return mat;  
        }  


    }  

}  

OpenCVSharp4で勾配構造テンソルを使った二値化

OpenCVチュートリアルこの章をOpenCVSharp4仕様に書き換えた。

構造テンソルを使うと、勾配の方向や勾配の一貫性などがわかる。

解説↓

 

negizoku.hatenablog.jp

 

 

プログラム

using OpenCvSharp;  

namespace Anisotropic_image_segmentation_by_a_gradient_structure_tensor {  
    class Program {  
        static void Main(string[] args) {  
            //画像の読込  
            Mat src = new Mat(@"D:\gst_input.jpg", ImreadModes.Grayscale);  

            //cohency(勾配の一貫性)と傾く方向を計算  
            Mat imgCoherency = new Mat();  
            Mat imgOrientation = new Mat();  
            int windowWidth = 52;  
            calcGST(ref src, out imgCoherency, out imgOrientation, windowWidth);  


            //cohencyで二値化  
            Mat imgCoherencyBin = new Mat();  
            double cThresh = 0.43;  
            Cv2.Threshold(imgCoherency, imgCoherencyBin, cThresh, 255, ThresholdTypes.Binary);  

            //傾く方向で二値化  
            Mat imgOrientationBin = new Mat();  
            double lowThresh = 35,  
                highThresh = 114;  
            Cv2.InRange(imgOrientation, new Scalar(lowThresh), new Scalar(highThresh), imgOrientationBin);  


            //cohencyと傾く方向の論理積をとる  
            Mat imgBin = new Mat();  
            imgOrientationBin.ConvertTo(imgOrientationBin, MatType.CV_8UC1);  
            imgCoherencyBin.ConvertTo(imgCoherencyBin, MatType.CV_8UC1);  
            imgBin = imgOrientationBin.Mul(imgCoherencyBin);  


            //表示  
            imgCoherency *= 255;  
            imgOrientation.Normalize(0, 255, NormTypes.MinMax);  
            imgCoherency.ConvertTo(imgCoherency, MatType.CV_8UC1);  
            imgOrientation.ConvertTo(imgOrientation, MatType.CV_8UC1);  

            Cv2.ApplyColorMap(imgCoherency, imgCoherency, ColormapTypes.Viridis);  
            Cv2.ApplyColorMap(imgOrientation, imgOrientation, ColormapTypes.Viridis);  

            Cv2.ImShow("result", src + imgBin);  
            Cv2.ImShow("cohency", imgCoherency);  
            Cv2.ImShow("orientation", imgOrientation);  

            Cv2.WaitKey();  
        }  

        static void calcGST(ref Mat inputImg, out Mat imgCoherency, out Mat imgOrientation, int windowWidth) {  
            //画像をコピー  
            Mat img = new Mat();  
            inputImg.ConvertTo(img, MatType.CV_32FC1);  

            //差分を取る  
            Mat imgDiffX = new Mat(),  
                imgDiffY = new Mat();  

            Cv2.Sobel(img, imgDiffX, MatType.CV_32FC1, 1, 0);  
            Cv2.Sobel(img, imgDiffY, MatType.CV_32FC1, 0, 1);  

            //差分の積を計算  
            Mat imgDiffXX = imgDiffX.Mul(imgDiffX);  
            Mat imgDiffYY = imgDiffY.Mul(imgDiffY);  
            Mat imgDiffXY = imgDiffX.Mul(imgDiffY);  

            //窓関数フィルター  
            Mat J11 = new Mat(),  
                J22 = new Mat(),  
                J12 = new Mat();  

            Cv2.BoxFilter(imgDiffXX, J11, MatType.CV_32FC1, new Size(windowWidth, windowWidth));  
            Cv2.BoxFilter(imgDiffYY, J22, MatType.CV_32FC1, new Size(windowWidth, windowWidth));  
            Cv2.BoxFilter(imgDiffXY, J12, MatType.CV_32FC1, new Size(windowWidth, windowWidth));  

            //固有値計算  
            Mat eigenVal1 = new Mat(),  
                eigenVal2 = new Mat();  
            Mat D = (J11 - J22).Mul(J11 - J22) + 4 * (J12.Mul(J12));  
            Mat rootD = new Mat();  
            Cv2.Sqrt(D, rootD);  

            eigenVal1 = J11 + J22 + rootD;//大きい方  
            eigenVal2 = J11 + J22 - rootD;//小さい方  

            //勾配一貫性(cohency)  
            imgCoherency = new Mat();  
            Cv2.Divide(eigenVal1 - eigenVal2, eigenVal1 + eigenVal2, imgCoherency);  

            //傾く方向を計算  
            imgOrientation = new Mat();  
            Cv2.Phase(J22 - J11, 2 * J12, imgOrientation, true);  
        }  
    }  
}  

結果

入力画像

f:id:negizoku:20200920233143j:plain




出力画像
f:id:negizoku:20200920233155p:plain

ODEのオブジェクトの作り方

  1. オブジェクト用の構造体を作る

    struct MyObject {  
    dBodyID body;  
    };  
    static MyObject sphere;  
  2. main関数で詳しく設定

    sphere.body = dBodyCreate (world);  
    dReal radius = 5.0;   
    dMassSetSphere (&m,DENSITY,radius);  
  3. drawstuffのシミュレーションループ関数内で描画

    const dReal *pos, *radius;  
    pos = dBodyGetPosition(sphere.body);   
    radius   = dBodyGetRotation(sphere.body);  
    dsDrawSphere(pos1,R1,radius);