Opérabilité du bras et du robot mobile Dessinez une ellipse avec python

Il existe un concept d'ellipse maniable dans le bras du robot. C'est un indice pour évaluer dans quelle direction il est facile de déplacer la position de la main selon un certain angle.

Cette fois, après avoir trouvé cela avec un bras de robot à 2 axes, nous le trouverons également avec un robot mobile utilisant une roue Mecanum.

Bras de robot 2 axes

Considérez un bras de robot à 2 axes comme illustré ci-dessous. C'est un modèle simple qui est souvent utilisé en mécanique.

Extrait de https://jp.mathworks.com/discovery/inverse-kinematics.html

Cinématique avant

La cinétique avant de ce bras de robot est exprimée par la formule suivante. Eh bien, il n'y a pas besoin d'expliquer pour l'instant.

\left\{\begin{array}{l}
x=L_{1} \cos \left(\theta_{1}\right)+L_{2} \cos \left(\theta_{1}+\theta_{2}\right) \\
y=L_{1} \sin \left(\theta_{1}\right)+L_{2} \sin \left(\theta_{1}+\theta_{2}\right)
\end{array}\right.

Ellipse d'opérabilité

De là, nous expliquerons l'ellipse opérable. Les deux côtés de l'équation cinématique avant ci-dessus sont différenciés dans le temps. Si vous l'écrivez pour le moment, l'expression relationnelle suivante entre la vitesse de pointe et la vitesse d'angle d'articulation peut être dérivée.

\frac{dx}{dt}=
-L_{1}\dot{\theta_1} \sin \left(\theta_{1}\right)
-L_{2}\dot{\theta_1} \sin \left(\theta_{1}+\theta_{2}\right)
-L_{2}\dot{\theta_2} \sin \left(\theta_{1}+\theta_{2}\right) \\
\frac{dy}{dt}=
L_{1}{\theta_1} \cos \left(\theta_{1}\right)
+L_{2}{\theta_1} \cos \left(\theta_{1}+\theta_{2}\right)
+L_{2}{\theta_2} \cos \left(\theta_{1}+\theta_{2}\right)

De plus, lorsque cette expression est affichée dans une matrice, elle peut être résumée comme suit.

\left[
\begin{array}{ll}
\dot{x}\\
\dot{y}
\end{array}
\right]

=\left[\begin{array}{ll}
-L_{1} \sin \left(\theta_{1}\right)-L_{2} \sin \left(\theta_{1}+\theta_{2}\right) & -L_{2} \sin \left(\theta_{1}+\theta_{2}\right) \\
L_{1} \cos \left(\theta_{1}\right)+L_{2} \cos \left(\theta_{1}+\theta_{2}\right) & L_{2} \cos \left(\theta_{1}+\theta_{2}\right)
\end{array}\right]

\left[
\begin{array}{ll}
\dot{\theta_{1}}\\
\dot{\theta_{2}}
\end{array}
\right]

En outre, chaque vecteur et matrice sont exprimés collectivement comme suit.

\dot{\boldsymbol{r}}=\boldsymbol{J}(\theta) \dot{\boldsymbol{\theta}}

Cependant, r est le vecteur de position de la pointe et θ est le vecteur de joint. De plus, J est appelé matrice jacobienne et est une matrice représentée par ce qui suit. Vous avez peut-être appris jusqu'à présent.

\begin{aligned}
\boldsymbol{J(\theta)} &=\left[\begin{array}{cc}
\frac{\partial x}{\partial \theta_{1}} & \frac{\partial x}{\partial \theta_{2}} \\
\frac{\partial y}{\partial \theta_{1}} & \frac{\partial y}{\partial \theta_{2}}
\end{array}\right] \\
&=\left[\begin{array}{ll}
-L_{1} \sin \left(\theta_{1}\right)-L_{2} \sin \left(\theta_{1}+\theta_{2}\right) & -L_{2} \sin \left(\theta_{1}+\theta_{2}\right) \\
L_{1} \cos \left(\theta_{1}\right)+L_{2} \cos \left(\theta_{1}+\theta_{2}\right) & L_{2} \cos \left(\theta_{1}+\theta_{2}\right)
\end{array}\right]
\end{aligned}

En utilisant cette matrice Jacobi J, créez la matrice carrée A.

\boldsymbol{A}=\boldsymbol{J(\theta)} \boldsymbol{J(\theta)}^{T}

La racine carrée de l'expression matricielle de cette matrice A est appelée la manipulabilité w.

w=\sqrt{\operatorname{det} \boldsymbol{A}}

De plus, l'ellipse d'opérabilité peut être obtenue à partir de la valeur propre λ de cette matrice A et du vecteur propre v. Le vecteur propre v représente la direction axiale de l'ellipse de manipulation et la racine carrée de la valeur propre λ représente la taille de l'ellipse autour de la direction axiale correspondante.

Le code que vous voulez réellement

Je l'ai calculé avec python. Le dessin est matplotlib

draw_ManipulabilityEllipsoid_RobotArm.py

import numpy as np
import matplotlib.pyplot as plt
import matplotlib.patches as pat

fig, ax = plt.subplots(figsize=(10, 8))

def draw_ellipsoid(ax, a, b, x=0, y=0, deg=0):
    ellips = pat.Ellipse(xy = (x, y), width=a, height=b, alpha = 0.6, angle=deg, color="red", label="")
    ax.add_patch(ellips)

def calculate_Ellipsoid_from_Jacobian(J):
    #Calculer la matrice carrée A
    A = J * J.T
    #Valeur unique de A,Calculer le vecteur propre
    values, vectors = np.linalg.eig(A)
    #print(values, vectors)

    #valeur propre^(1/2)Avoir
    Lambda_1 = np.sqrt(values[0])
    Lambda_2 = np.sqrt(values[1])
    #Obtenir la taille de l'ellipse à partir de la valeur propre
    a = Lambda_2    #Longueur de l'axe horizontal
    b = Lambda_1    #Longueur de l'axe vertical
    
    #Obtenez eigenvector
    Vector_1 = vectors[:,0]
    Vector_2 = vectors[:,1]
    #Obtenez l'inclinaison de l'ellipse
    #Puisque A est une matrice symétrique, les vecteurs propres sont orthogonaux
    #Vous pouvez donc utiliser l'un ou l'autre des vecteurs propres pour l'inclinaison de l'ellipse
    rad = -np.arctan2(Vector_1[0],Vector_1[1])
    rad = np.arctan2(Vector_2[1],Vector_2[0])

    return a, b, rad

def get_robotarm_pose(L1, L2, theta1, theta2):
    #Cinématique avant
    X1 = L1*np.cos(theta1)
    Y1 = L1*np.sin(theta1)
    X2 = L1*np.cos(theta1)+L2*np.cos(theta1+theta2)
    Y2 = L1*np.sin(theta1)+L2*np.sin(theta1+theta2)
    return X1, Y1, X2, Y2

def get_Jacobian_robotarm_kinematics(L1, L2, theta1, theta2):
    # Jacobian Matrix
    J = np.matrix([[-L1*np.sin(theta1)-L2*np.sin(theta1+theta2), -L2*np.sin(theta1+theta2)], 
                    [L1*np.cos(theta1)+L2*np.cos(theta1+theta2), L2*np.cos(theta1+theta2)] ])

    return J

def draw_ellipsoid_from_robotarm(L1, L2, theta1, theta2):

    J = get_Jacobian_robotarm_kinematics(L1, L2, theta1, theta2)
    x1,y1,x2,y2 = get_robotarm_pose(L1, L2, theta1, theta2)
    a, b, rad = calculate_Ellipsoid_from_Jacobian( J )

    #Dessiner une ellipse
    draw_ellipsoid(ax, a*0.5, b*0.5, x2, y2, np.rad2deg(rad))

    #Dessin du bras du robot
    ax.plot([0, x1], [0, y1], 'k', linewidth = 10.0, zorder=1)
    ax.plot([x1, x2], [y1, y2], 'k', linewidth = 10.0, zorder=1)
    c0 = pat.Circle(xy=(0, 0), radius=.1, ec='w', fill=True, zorder=2)
    c1 = pat.Circle(xy=(x1, y1), radius=.1, ec='w', fill=True, zorder=2)
    ax.add_patch(c0)
    ax.add_patch(c1)

    #Ajustement de la figure
    ax.set_xlabel('x')
    ax.set_ylabel('y')
    ax.set_aspect('equal')
    ax.grid()
    plt.xlim([-2,2])
    plt.ylim([-0.5,2])

if __name__ == "__main__":
    L1 = 1.0
    L2 = 1.0
    theta1 = np.deg2rad(30)
    theta2 = np.deg2rad(40)
    draw_ellipsoid_from_robotarm(L1, L2, theta1, theta2)
    plt.show()

résultat

Le résultat de l'exécution est le suivant. N'est-ce pas amusant d'ajouter une barre de suivi et de la rendre gluante?

Robot de déplacement de roue Mecanum

Ensuite, considérons une ellipse maniable pour un robot mobile. Je n'ai trouvé aucune littérature à laquelle les ellipses utilisables autres que le bras du robot étaient appliquées, mais l'idée est la même.

Cinématique avant

Le robot qui se déplace par la roue Mecanum est défini comme illustré dans la figure ci-dessous.

• Angle de rouleau de roue mécanique φ -Largeur verticale de l'aéronef a --Largeur de l'aéronef (largeur de bande de roulement) b C'était fabriqué.

L est en dessous de la figure.

L = \sqrt { a ^ { 2 } + b ^ { 2 } } \cos \alpha \\
\alpha = \varphi - \tan ^ { - 1 } \frac { a } { b }

D'après la figure, si les vitesses périphériques de chaque roue sont v1, v2, v3, v4, la relation suivante peut être dérivée géométriquement. (Imaginez déplacer le robot directement à la main.)

\begin{aligned} v _ { 1 } & = - \dot { x } \sin \varphi + \dot { y } \cos \varphi + L \dot { \theta } \\ v _ { 2 } & = - \dot { x } \sin \varphi - \dot { y } \cos \varphi + L \dot { \theta } \\ v _ { 3 } & = \dot { x } \sin \varphi - \dot { y } \cos \varphi + L \dot { \theta } \\ v _ { 4 } & = \dot { x } \sin \varphi + \dot { y } \cos \varphi + L \dot { \theta } \end{aligned}

L'affichage matriciel est le suivant.

 \begin{bmatrix} { v _ { 1 } } \\ { v _ { 2 } } \\ { v _ { 3 } } \\ { v _ { 4 } } \end{bmatrix} = \begin{bmatrix} { - \sin \varphi } & { \cos \varphi } & { L } \\ { - \sin \varphi } & { - \cos \varphi } & { L } \\ { \sin \varphi } & { - \cos \varphi } & { L } \\ { \sin \varphi } & { \cos \varphi } & { L } \end{bmatrix} \begin{bmatrix}{ \dot { x } } \\ { \dot { y } } \\ { \dot { \theta } } \end{bmatrix}

Cependant, le vecteur vitesse est la vitesse vue depuis le châssis du robot {R}. En contrôlant réellement, j'aimerais réfléchir à la manière de me déplacer du point de vue du cadre inertiel {I}. Le vecteur de vitesse dans le cadre du robot est exprimé par la formule suivante à partir du vecteur de vitesse dans le cadre d'inertie en utilisant la matrice de rotation.

\begin{bmatrix}{ \dot { x } } \\ { \dot { y } } \\ { \dot { \theta } }\end{bmatrix}  =  \begin{bmatrix} { \cos \theta } & { \sin \theta } & { 0 } \\ { - \sin \theta } & { \cos \theta } & { 0 } \\ { 0 } & { 0 } & { 1 } \end{bmatrix}   \begin{bmatrix} { \dot { x } _ { I } } \\ { \dot { y } _ { I } } \\ { \dot { \theta } _ { I } } \end{bmatrix} 

Par conséquent, en remplaçant dans l'équation précédente, l'équation suivante est obtenue.

 \begin{bmatrix} { v _ { 1 } } \\ { v _ { 2 } } \\ { v _ { 3 } } \\ { v _ { 4 } } \end{bmatrix} = \begin{bmatrix} { - \sin \varphi } & { \cos \varphi } & { L } \\ { - \sin \varphi } & { - \cos \varphi } & { L } \\ { \sin \varphi } & { - \cos \varphi } & { L } \\ { \sin \varphi } & { \cos \varphi } & { L } \end{bmatrix} \begin{bmatrix} { \cos \theta } & { \sin \theta } & { 0 } \\ { - \sin \theta } & { \cos \theta } & { 0 } \\ { 0 } & { 0 } & { 1 } \end{bmatrix}  \begin{bmatrix}{ \dot { x } _ { l } } \\ { \dot { y } _ { I } } \\ { \dot { \theta } _ { I } } \end{bmatrix}

Vous pouvez voir que cela ressemble à la formule suivante.

\boldsymbol{\dot{ \theta }} = \boldsymbol{X(\theta)} \boldsymbol{\dot{ x }}

Ici, lorsque la formule est transformée en la forme suivante, la matrice de Jacobi J (θ) apparaît.

\boldsymbol{\dot{ x }} = \boldsymbol{J(\theta)} \boldsymbol{\dot{ \theta }}

Ici, nous trouverons l'ellipse fonctionnelle du robot à roue Mecanum. Cependant, comme l'opérabilité liée à la rotation n'est pas prise en considération, la formule utilisée est modifiée comme suit.

 \begin{bmatrix} { v _ { 1 } } \\ { v _ { 2 } } \\ { v _ { 3 } } \\ { v _ { 4 } } \end{bmatrix} = \begin{bmatrix} { - \sin \varphi } & { \cos \varphi }  \\ { - \sin \varphi } & { - \cos \varphi } \\ { \sin \varphi } & { - \cos \varphi }  \\ { \sin \varphi } & { \cos \varphi } \end{bmatrix} \begin{bmatrix} { \cos \theta } & { \sin \theta } \\ { - \sin \theta } & { \cos \theta }  \end{bmatrix}  \begin{bmatrix}{ \dot { x } _ { l } } \\ { \dot { y } _ { I } } \end{bmatrix}

code

Vous avez besoin du draw_ManipulabilityEllipsoid_RobotArm.py ci-dessus.

draw_ManipulabilityEllipsoid_Mecanum.py

import numpy as np
import matplotlib.pyplot as plt
import matplotlib.patches as pat
import matplotlib.transforms as transforms
from draw_ManipulabilityEllipsoid_RobotArm import *

def get_Jacobian_MecanumRobot_kinematics(theta, fai):
    # Jacobian Matrix
    A = np.matrix([ [-np.sin(fai),np.cos(fai)], [-np.sin(fai),-np.cos(fai)], [np.sin(fai),-np.cos(fai)],[np.sin(fai),np.cos(fai)] ])
    R = np.matrix([ [np.cos(theta),np.sin(theta)], [-np.sin(theta),np.cos(theta)] ])
    J = np.linalg.inv(R) * np.linalg.pinv(A);
    return J

def draw_ellipsoid_from_MacanumRobot(theta, fai):

    J = get_Jacobian_MecanumRobot_kinematics(theta, fai)
    a, b, rad = calculate_Ellipsoid_from_Jacobian( J )

    #Dessiner une ellipse
    draw_ellipsoid(ax, a*3, b*3, 0, 0, np.rad2deg(rad))

    #Dessin de robot
    a = 1.0
    b = 1.0
    wheel_width = 0.3
    wheel_diameter = 0.6
    wheel_pos_list = [(b/2,a/2), (-b/2,a/2), (-b/2,-a/2), (b/2,-a/2)]
    ts = ax.transData
    tr = transforms.Affine2D().rotate_deg_around(0, 0, np.rad2deg(theta)) 
    t = tr + ts
    for pos in wheel_pos_list:
        wheel = pat.Rectangle(xy=(pos[0]-wheel_width/2,pos[1]-wheel_diameter/2),width=wheel_width,height=wheel_diameter,transform=t,color="k")
        ax.add_patch(wheel)
    robot = pat.Rectangle(xy=(-b/2+wheel_width/2,-a/2),width=b-wheel_width,height=a,transform=t,color="b")
    ax.add_patch(robot)

    #Ajustement de la figure
    ax.set_xlabel('x')
    ax.set_ylabel('y')
    ax.set_aspect('equal')
    ax.grid()
    plt.xlim([-1.5,1.5])
    plt.ylim([-1.5,1.5])

if __name__ == "__main__":
    theta = np.deg2rad(20)
    fai = np.deg2rad(30)
    draw_ellipsoid_from_MacanumRobot(theta, fai)
    plt.show()

résultat

Vous avez besoin du draw_ManipulabilityEllipsoid_RobotArm.py ci-dessus.

θ=20°, φ=30°

Le résultat de l'exécution est le suivant. Comme ça

θ=0°, φ=30°

Eh bien, même si le robot tourne, cela n'affecte pas la forme de l'ellipse. Si vous définissez thêta = 0, ce sera comme suit.

θ=0°, φ=45°

De plus, si fai = 45 °, la facilité de mouvement dans les directions x et y devient égale, et le résultat est le suivant.

θ=0°, φ=0°

D'ailleurs, si fai = 0 °, ce sera comme suit. Si vous regardez de près, il y a une ligne verticale au centre du robot. (La couleur du corps principal a été éclaircie pour plus de clarté.)

Il est facile à comprendre si vous considérez la figure suivante. Vous pouvez immédiatement voir que si fai = 0, vous ne pouvez pas vous déplacer dans la direction x.