Bedienbarkeit von Arm und mobilem Roboter Ellipse mit Python zeichnen

Es gibt ein Konzept der manövrierbaren Ellipse im Roboterarm. Es ist ein Index, um zu bewerten, in welche Richtung es leicht ist, die Handposition in einem bestimmten Winkel zu bewegen.

Dieses Mal, nachdem wir dies mit einem 2-Achsen-Roboterarm gefunden haben, werden wir es auch mit einem mobilen Roboter finden, der ein Mecanum-Rad verwendet.

2-Achs-Roboterarm

Betrachten Sie einen 2-Achsen-Roboterarm wie unten gezeigt. Es ist eine einfache, die oft in der Mechanik verwendet wird.

Zitiert von https://jp.mathworks.com/discovery/inverse-kinematics.html

Vorwärtskinematik

Die Vorwärtskinetik dieses Roboterarms wird durch die folgende Formel ausgedrückt. Nun, es besteht bisher kein Grund zur Erklärung.

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

Bedienbarkeitsellipse

Von hier aus erklären wir die bedienbare Ellipse. Beide Seiten der obigen Vorwärtskinematikgleichung sind zeitlich differenziert. Wenn Sie es vorerst schreiben, kann der folgende relationale Ausdruck zwischen Spitzengeschwindigkeit und Gelenkwinkelgeschwindigkeit abgeleitet werden.

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

Wenn dieser Ausdruck in einer Matrix angezeigt wird, kann er außerdem wie folgt zusammengefasst werden.

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

Weiterhin werden jeder Vektor und jede Matrix gemeinsam wie folgt ausgedrückt.

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

R ist jedoch der Positionsvektor der Spitze und θ ist der Gelenkvektor. J heißt Jacobi-Matrix und ist eine Matrix, die durch Folgendes dargestellt wird. Sie haben vielleicht bisher gelernt.

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

Erstellen Sie mit dieser Jacobi-Matrix J die folgende quadratische Matrix A.

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

Die Quadratwurzel des Matrixausdrucks dieser Matrix A wird als Manipulierbarkeit w bezeichnet.

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

Zusätzlich kann die Operabilitätsellipse aus dem Eigenwert λ dieser Matrix A und dem Eigenvektor v erhalten werden. Der Eigenvektor v repräsentiert die axiale Richtung der Operabilitätsellipse, und die Quadratwurzel des Eigenwerts λ repräsentiert die Größe der Ellipse in Bezug auf die entsprechende axiale Richtung.

Der Code, den Sie tatsächlich wollen

Ich habe es mit Python berechnet. Zeichnung ist 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):
    #Berechnen Sie die quadratische Matrix A.
    A = J * J.T
    #Eindeutiger Wert von A.,Eigenvektor berechnen
    values, vectors = np.linalg.eig(A)
    #print(values, vectors)

    #Eigenwert^(1/2)Erhalten
    Lambda_1 = np.sqrt(values[0])
    Lambda_2 = np.sqrt(values[1])
    #Ermitteln Sie die Größe der Ellipse aus dem Eigenwert
    a = Lambda_2    #Horizontale Achsenlänge
    b = Lambda_1    #Vertikale Achsenlänge
    
    #Holen Sie sich Eigenvektor
    Vector_1 = vectors[:,0]
    Vector_2 = vectors[:,1]
    #Holen Sie sich die Neigung der Ellipse
    #Da A eine symmetrische Matrix ist, sind die Eigenvektoren orthogonal
    #Sie können also einen der Eigenvektoren für die Neigung der Ellipse verwenden
    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):
    #Vorwärtskinematik
    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 )

    #Eine Ellipse zeichnen
    draw_ellipsoid(ax, a*0.5, b*0.5, x2, y2, np.rad2deg(rad))

    #Roboterarm zeichnen
    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)

    #Anpassung der Figur
    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()

Ergebnis

Das Ausführungsergebnis ist wie folgt. Macht es nicht Spaß, eine Spurleiste hinzuzufügen und schleimig zu machen?

Mecanum Rad beweglicher Roboter

Als nächstes betrachten wir eine manövrierbare Ellipse für einen mobilen Roboter. Ich konnte keine Literatur finden, auf die die anderen bedienbaren Ellipsen als der Roboterarm angewendet wurden, aber die Idee ist dieselbe.

Vorwärtskinematik

Der Roboter, der sich mit dem Mecanum-Rad bewegt, ist wie in der folgenden Abbildung dargestellt definiert.

--Mecanum Radrollenwinkel φ

• Vertikale Breite des Flugzeugs a
• Breite des Flugzeugs (Profilbreite) b Es wurde gemacht.

L ist unten von der Figur.

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

Wenn aus der Figur die Umfangsgeschwindigkeiten jedes Rades v1, v2, v3, v4 sind, kann die folgende Beziehung geometrisch abgeleitet werden. (Stellen Sie sich vor, Sie bewegen den Roboter direkt von Hand.)

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

Die Matrixanzeige ist wie folgt.

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

Der Geschwindigkeitsvektor ist jedoch die Geschwindigkeit, die vom Roboterrahmen {R} aus gesehen wird. Bei der eigentlichen Steuerung möchte ich darüber nachdenken, wie ich mich vom Standpunkt des Trägheitsrahmens {I} aus bewegen kann. Der Geschwindigkeitsvektor im Roboterrahmen wird durch die folgende Formel aus dem Geschwindigkeitsvektor im Trägheitsrahmen unter Verwendung der Rotationsmatrix ausgedrückt.

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

Daher wird durch Einsetzen in die vorherige Gleichung die folgende Gleichung erhalten.

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

Sie können sehen, dass es wie die folgende Formel aussieht.

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

Wenn hier die Formel in die folgende Form umgewandelt wird, erscheint die Jacobi-Matrix J (θ).

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

Hier finden wir die bedienbare Ellipse des Mecanum-Radroboters. Da jedoch die mit der Rotation verbundene Bedienbarkeit nicht berücksichtigt wird, wird die verwendete Formel wie folgt geändert.

 \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

Sie benötigen die oben genannte draw_ManipulabilityEllipsoid_RobotArm.py.

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 )

    #Eine Ellipse zeichnen
    draw_ellipsoid(ax, a*3, b*3, 0, 0, np.rad2deg(rad))

    #Roboterzeichnung
    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)

    #Anpassung der Figur
    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()

Ergebnis

Sie benötigen die oben genannte draw_ManipulabilityEllipsoid_RobotArm.py.

θ=20°, φ=30°

Das Ausführungsergebnis ist wie folgt. So wie das

θ=0°, φ=30°

Nun, selbst wenn sich der Roboter dreht, hat dies keinen Einfluss auf die Form der Ellipse. Wenn Sie Theta = 0 setzen, ist dies wie folgt.

θ=0°, φ=45°

Wenn fai = 45 ° ist, wird die Bewegungsfreiheit in x- und y-Richtung gleich und das Ergebnis ist wie folgt.

θ=0°, φ=0°

Wenn fai = 0 ° ist, ist es außerdem wie folgt. Wenn Sie genau hinschauen, befindet sich in der Mitte des Roboters eine vertikale Linie. (Die Farbe des Hauptkörpers wurde aus Gründen der Klarheit aufgehellt.)

Es ist leicht zu verstehen, wenn Sie die folgende Abbildung betrachten. Sie können sofort sehen, dass Sie sich bei fai = 0 nicht in x-Richtung bewegen können.