[PYTHON] Jumeau numérique avec robot mobile de roue

introduction

Des robots à roues (opposés au type à deux roues) équipés de Microbit et d'Arduino sont en vente comme matériel de programmation pour enfants. (Par exemple, un kit voiture servo utilisant un servomoteur à 360 degrés Déplacer un servomoteur à 360 degrés avec Microbit) Des exemples de programmes sont fournis en tant que matériel pédagogique, tels que divers mouvements du robot, un fonctionnement sans fil et un fonctionnement autonome avec des capteurs connectés. Pour aller plus loin, il y a quelque chose qui relie le cyberespace (espace numérique) et l'espace physique (espace réel) à l'aide de données, comme simuler ce mouvement à l'avance et reproduire la trajectoire à partir des données réellement déplacées (jumeau numérique). J'ai pensé que ce serait intéressant. Par conséquent, j'ai fait une simulation qui reproduit la trajectoire à partir des informations du moteur ON / OFF du robot mobile. La récupération des informations ON / OFF pour le moteur (ou le servomoteur) est une tâche future, et cette partie n'est pas incluse cette fois.

Robot de déplacement de roue

Pour le robot mobile de roue, le site suivant sera utile, mais l'entrée est la vitesse des roues gauche et droite, et la longueur entre les roues est la valeur à définir. La formule est la suivante.

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 \\

Figure: Coordonnées et paramètres d'un robot mobile (partiellement modifié la figure dans l'article référencé)

Référence: Principe du robot mobile à roues [Mécanique mobile / robots et bases du contrôle - Traitement de l'information de l'Université Tohoku Gakuin ...](http://www.mech.tohoku-gakuin.ac.jp/rde/contents/sendai/mechatro/archive/RM Seminar_No20_s8.pdf ) Calculons le mouvement d'un robot à roues (type opposé à deux roues)

Simulation par Python

Code référencé Two wheel motion model sample

Dans cette simulation, les vitesses des roues gauche et droite sont constantes et seules les informations ON / OFF sont fournies.

Environnement du programme

Win 10 Pro 64bit Anaconda python 3.7

Code programme

"""
Robot à deux roues opposé
Simulation de trajectoire

Paramètres d'entrée
Longueur entre deux roues: bande de roulement[m]
Vitesse de roue droite, gauche: vr, vl [m/s]

production
Vitesse du centre de gravité du véhicule: ver
Vitesse d'angle de braquage: oméga[rad/s]

Sortie finale
Angle d'aspect: thetat
x,coordonnée y: xt ,yt


Code référencé
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
point de changement:
Développer le format de matrice dans des expressions ordinaires
Changement de la vitesse de roue angulaire à la vitesse de roue
Modifier l'entrée de simulation

animation
Créer une animation d'un graphe (Matplotlib) avec Python (Artist Animation)
https://water2litter.net/rum/post/python_matplotlib_animation_ArtistAnimation/

[Calcul scientifique / technique par Python]Trajectoire de l'animation du mouvement parabolique(locus)Dessiner avec, 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
    #Pour dessiner des graphiques d'animation
    fig = plt.figure()
    ims = [] 
    #Stockage des données calculées (coordonnées)
    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)

        #Paramètres du tracé
        plt.grid(True)
        plt.axis("equal")
        plt.xlabel("X")
        plt.ylabel("Y")
        
        #Si seulement la position au temps t
        # im=plt.plot(xt,yt,'o', color='red',markersize=10, linewidth = 2)
        
        #Créez deux images, la position au temps t et la trajectoire menant au temps 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)
        
    #Création d'animation
    anim = ArtistAnimation(fig, ims, interval=100, blit=True,repeat=False) 
    plt.show()
    # plt.pause(10)

if __name__ == '__main__':
    #Données de vitesse par seconde
    #Allumer/Réglez sur OFF et maintenez la vitesse constante. Rotation avant: 1, rotation arrière:-1, arrêt: 0
    #(1,1): Avance, (0,1): Virage à droite, (1,0): Sens antihoraire,(-1,1) or (1,-1): Rotation in situ
    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)

Nous avons reçu des conseils de Kioto sur le code du programme. Merci beaucoup.

résultat

Résultat lorsque input_list1 est lu comme des données

Résumé

Les jumeaux numériques (cyberespace et espace physique) peuvent être connectés via des données. Les données de l'espace physique sont le pont, et dans le cyberespace, la simulation par les données ou les données par la simulation est le pont. Je voudrais passer de l'enseignement de la programmation à l'enseignement de l'utilisation des données.