[PYTHON] Digitaler Zwilling mit Radbewegungsroboter

Einführung

Mit Microbit und Arduino ausgestattete Radbewegungsroboter (gegenüber dem Zweirad) werden als Programmiermaterial für Kinder angeboten. (Zum Beispiel ein Servo-Car-Kit mit einem 360-Grad-Servomotor Bewegen eines 360-Grad-Servomotors mit Microbit) Beispielprogramme werden als Unterrichtsmaterial bereitgestellt, z. B. verschiedene Bewegungen des Roboters, drahtloser Betrieb und autonomer Betrieb mit angebrachten Sensoren. Wenn Sie noch einen Schritt weiter gehen, gibt es etwas, das den Cyberraum (digitaler Raum) und den physischen Raum (realer Raum) mithilfe von Daten verbindet, z. B. diese Bewegung im Voraus simulieren und die Flugbahn aus den tatsächlich bewegten Daten reproduzieren (digitaler Zwilling). Ich dachte es wäre interessant. Daher habe ich eine Simulation erstellt, die die Flugbahn aus den Informationen des Motors EIN / AUS des mobilen Roboters reproduziert. Das Abrufen von EIN / AUS-Informationen für den Motor (oder Servomotor) ist eine zukünftige Aufgabe, und dieser Teil ist diesmal nicht enthalten.

Radbewegungsroboter

Für den Radbewegungsroboter ist die folgende Seite hilfreich, aber die Eingabe ist die Geschwindigkeit des linken und rechten Rads und die Länge zwischen den Rädern ist der einzustellende Wert. Die Formel lautet wie folgt.

v: velocity \\

	\omega: angular \ velocity \\

	 x, y: position\\
	 \theta: directon　\ angle\\
	 t_d: tread

v _k = (v_r +v_l)/2 \\
\omega _k = (v_r +v_l)/t_d \\

x_{k+1} = x_k + \cos \theta _k\cdot\Delta K v_k \\
y_{k+1} = y_k + \sin \theta _k\cdot\Delta K v_k \\
\theta _{k+1} = \theta _k + \Delta K \omega _k \\

Abbildung: Koordinaten und Parameter eines Radbewegungsroboters (teilweise geändert die Abbildung im Artikel, auf den verwiesen wird)

Referenz: Radbewegungsroboterprinzip [Bewegliche Mechanik / Roboter und die Grundlagen der Informationsverarbeitung der Tohoku Gakuin University ...](http://www.mech.tohoku-gakuin.ac.jp/rde/contents/sendai/mechatro/archive/RM Seminar_No20_s8.pdf ) Berechnen wir die Bewegung eines Radroboters (gegenüber dem Zweiradtyp)

Simulation von Python

Referenzierter Code Two wheel motion model sample

In dieser Simulation sind die Geschwindigkeiten des linken und rechten Rads konstant und es werden nur EIN / AUS-Informationen bereitgestellt.

Programmumgebung

Win 10 Pro 64bit Anaconda python 3.7

Programmcode

"""
Gegenüber einem zweirädrigen Roboter
Flugbahnsimulation

Eingabeparameter
Länge zwischen zwei Rädern: Profil[m]
Raddrehzahl rechts, links: vr, vl [m/s]

Ausgabe
Geschwindigkeit des Fahrzeugschwerpunkts: ver
Drehwinkelgeschwindigkeit: Omega[rad/s]

Endgültige Ausgabe
Aspektwinkel: thetat
x,y-Koordinate: xt ,yt


Referenzierter Code
Two wheel motion model sample
https://www.eureka-moments-blog.com/entry/2020/04/05/180844#%E5%AF%BE%E5%90%912%E8%BC%AA%E5%9E%8B%E3%83%AD%E3%83%9C%E3%83%83%E3%83%88
Wechselpunkt:
Erweitern Sie das Matrixformat in gewöhnliche Ausdrücke
Wechsel von Winkelradgeschwindigkeit zu Radgeschwindigkeit
Simulationseingabe ändern

Animation
Erstellen Sie mit Python (Artist Animation) eine Animation eines Diagramms (Matplotlib).
https://water2litter.net/rum/post/python_matplotlib_animation_ArtistAnimation/

[Wissenschaftlich-technische Berechnung von Python]Flugbahn der parabolischen Bewegungsanimation(locus)Malen mit, matplotlib
https://qiita.com/sci_Haru/items/278b6a50c4e9f4c07dcf

"""

import numpy as np
from math import cos, sin, pi
import math
import matplotlib.pyplot as plt
from matplotlib.animation import ArtistAnimation

def twe_wheel_fuc(v, state, delta, factor=1, td=2):
    """
    Equation of state
    Args:
        v (tuple or list): velocity of each wheel unit m/s,(right velocity,left velocity)
        state (list):　state [x, y, thita] , x, y 
        delta (float): update time unit s
        factor (float):velocity factor Defaults to 1
        td (float): tread length between wheel unit m Defaults to 2.

    Returns:
        [list]: next state
    """
    # vr: right wheel velocity, vl:left wheel velocity
    # vel: Center of gravity velocity
    # omega: Rotation angular velocity
    vr = v[0]*factor
    vl = v[1]*factor
    vel = (vr + vl)/2
    omega = (vr - vl)/(td)
    # state[2]: theta
    x_ = vel*delta*cos(state[2]+omega*delta/2)
    y_ = vel*delta*sin(state[2]+omega*delta/2)
    # x_ = vel*delta*cos(state[2])
    # y_ = vel*delta*sin(state[2])
    xt = state[0] + x_
    yt = state[1] + y_
    thetat = state[2]+omega*delta
    update_state = [xt, yt, thetat]
    return update_state


def simulation_twewheel(data,ini_state=[0,0,0],factor=1,td=6.36):
    """
    data: list On/OFF data
    
    """
    # simulation
    #Zum Zeichnen von Animationsgraphen
    fig = plt.figure()
    ims = [] 
    #Speicherung berechneter Daten (Koordinaten)
    st_x = []
    st_y = []
    st_theta = []
    st_vec = ini_state

    for i in data:   
        st_vec = twe_wheel_fuc(i, st_vec, delta=1,factor=factor,td=td)
        xt, yt, thetat = st_vec
        print("State:",st_vec)
        print("Direction angle: ",math.degrees(thetat))
        st_x.append(xt)
        st_y.append(yt)
        st_theta.append(thetat)

        #Einstellungen für Plot
        plt.grid(True)
        plt.axis("equal")
        plt.xlabel("X")
        plt.ylabel("Y")
        
        #Wenn nur die Position zum Zeitpunkt t
        # im=plt.plot(xt,yt,'o', color='red',markersize=10, linewidth = 2)
        
        #Erstellen Sie zwei Bilder, die Position zum Zeitpunkt t und die Flugbahn zum Zeitpunkt t
        plt.annotate('', xy=(xt+cos(thetat),yt+sin(thetat)), xytext=(xt,yt),
                    arrowprops=dict(shrink=0, width=1, headwidth=2, 
                    headlength=10, connectionstyle='arc3',
                    facecolor='blue', edgecolor='blue'))
        im=plt.plot(xt,yt,'o',st_x,st_y, '--', color='red',markersize=10, linewidth = 2)
                    
        ims.append(im)
        
    #Animationserstellung
    anim = ArtistAnimation(fig, ims, interval=100, blit=True,repeat=False) 
    plt.show()
    # plt.pause(10)

if __name__ == '__main__':
    #Geschwindigkeitsdaten pro Sekunde
    #Einschalten/Auf AUS stellen und die Geschwindigkeit konstant halten. Vorwärtsdrehung: 1, Rückwärtsdrehung:-1, Stopp: 0
    #(1,1): Voraus, (0,1): Rechts abbiegen, (1,0): Gegen den Uhrzeigersinn,(-1,1) or (1,-1): In-situ-Rotation
    input_lists =[(1,1),(1,1),(1,1),(1,1),(1,1),(1,1),
                  (0,1),(0,1),(0,1),(0,1),(0,1),(0,1),
                  (1,1),(1,1),(1,1),(1,1),(1,1),(1,1),
                  (1,0),(1,0),(1,0),(1,0),(1,0),(1,0),
                  (1,1),(1,1),(1,1),(1,1),(1,1),(1,1),
                  (1,-1),(1,-1),(1,1),(1,1),
                  (1,-1),(1,-1),(1,1),(1,1),
                  (1,0),(1,0),(1,0),(0,1),(0,1),(0,1),
                  (1,0),(1,0),(1,0),(0,1),(0,1),(0,1),
                  (1,1),(1,1),]
    
    input_lists2 =[(1,1),(1,1),(1,1),(1,1),(1,1),(1,1),]
    
    simulation_twewheel(data=input_lists,ini_state=[0,0,0],factor=1,td=6.36)

Wir wurden von Kioto bezüglich des Programmcodes beraten. Vielen Dank.

Ergebnis

Ergebnis, wenn input_list1 als Daten gelesen wird

Zusammenfassung

Digitale Zwillinge (Cyberraum und physischer Raum) können über Daten verbunden werden. Daten aus dem physischen Raum sind die Brücke, und im Cyberraum ist die Simulation durch die Daten oder die Daten durch die Simulation die Brücke. Ich möchte von der Programmierausbildung zur Datenverwendungserziehung expandieren.