Erste Schritte mit ROS für Jetson Nano – Roboterkoordinatentransformation

---

Vorwort

ROS bietet Entwicklern viele hochintegrierte Entwicklungstools wie RVIZ und Gazebo. rviz ist ein dreidimensionales Visualisierungstool, das Informationen wie Bilder, Modelle, Pfade usw. anzeigen kann, aber die Voraussetzung ist, dass diese Daten in Form von Themen und Parametern veröffentlicht wurden. Was rviz tut, ist, diese Daten zu abonnieren und vervollständigen Sie das visuelle Rendering. Dieser Blogger verlässt sich hauptsächlich auf die 3D-Plattform von rviz, um Koordinatentransformation und Roboter-3D-Modellierung zu erlernen

---

1. Das Prinzip der Raumkoordinatentransformation

1. Posenbeschreibung

Bevor wir die Posenbeschreibung lernen, müssen wir zunächst die Natur der Matrix verstehen. Wenn wir die Matrix mit der Funktion verknüpfen und sie als Abbildungsänderung verstehen, besteht das Wesen der Matrix darin, eine Raumtransformation des Basisvektors zu beschreiben, und die Determinante drückt genau die Fläche aus, die durch die Transformation des Basisvektors eingeschlossen wird

$i$ und$j$ ist das einfachste Basispaar zur Bildung einer zweidimensionalen Ebene, und die Matrix konzentriert sich auf die lineare Transformation dieser beiden Basisvektorpaare, eine Funktion ähnlich einer Funktion a ⃗ = f ( b ⃗ ) \vec{a}
$$\vec{A}=f\left(\vec{B}\right)$$

比如, $\left[\begin{array}{c}ich\\ \end{array}\right]$ Diese Basis geht durch$\left[\begin{array}{cc}3& 2\\ -& \end{array}\right]$ Kartierung,$i$ wird zu$(3,-2)$ Dieser neue Basisvektor,$j$ wird zu$(2,1)$ Mit diesem neuen Basisvektor ändert sich die gesamte zweidimensionale Ebene wie folgt:

Wir setzen die geänderten beiden Basisvektoren wie folgt in die Matrix ein, sodass diese Matrix$\left[\begin{array}{cc}3& 2\\ -& \end{array}\right]$ stellt eine Transformationsabbildung auf einen zweidimensionalen Raum dar, und die beiden Spalten sind die transformierten$i$ und$j$。

Sprechen wir über Pose, was für Position und Körperhaltung steht. Jeder starre Körper im Raumkoordinatensystem $In\; OXYZ$ können Position und Lage verwendet werden, um den Positionsstatus genau und eindeutig darzustellen. Für das kartesische Koordinatensystem {A} ist jeder Punkt$Die\; Position\; von\; p$ kann als 3x1-Spaltenvektor${}^{A}P$array
$${}^{A}P=\left[\begin{array}{c}{P}_{}\\ P_{}\\ P_{}\end{array}\right]$$

Angenommen, das Hauptkoordinatensystem ist $A$，Für uns$X_{}、Y_{}、Z_{}$um ein anderes Koordinatensystem $Die\; Einheitsvektoren\; der\; drei\; Achsen\; von\; B$ , dann das Koordinatensystem$B$ ist relativ zum Koordinatensystem$Ein$ Randzyklus mit einem definierten$${}_B^{}R={(}^A{X_{}}^A{Y_{}}^A{Z}_{})=\left[\begin{array}{ccc}{X}_{}\bullet X_{}& Y_{}\bullet X_{}& Z_{}\bullet X_{}\\ X_{}\bullet Y_{}& Y_{}\bullet Y_{}& Y_{}\bullet Y_{}\\ X_{}\bullet Z_{}& Y_{}\bullet Z_{}& Z_{}\bullet Z_{}\end{array}\right]$$

Gemäß dem Wesen der Matrix, die wir gerade verstanden haben, ist $X_{}$Nach der Abbildung durch die Matrix wird er zum Einheitsvektor $X_{}$在$X_{}、Y_{}、Z_{}$Die Projektion auf die drei Basisvektoren ist $X_{}$；$Y_{}$Nach der Abbildung durch die Matrix wird er zu einem Einheitsvektor $Y_{}$在$X_{}、Y_{}、Z_{}$Die Projektion auf die drei Basisvektoren ist $Y_{}$；$Z_{}$Nach der Abbildung durch die Matrix wird er zum Einheitsvektor $Z_{}$在$X_{}、Y_{}、Z_{}$Die Projektion auf die drei Basisvektoren ist $Z_{}$, diese Rotationsmatrix ist eigentlich das Koordinatensystem $Die\; Raumtransformation\; von\; A$ bildet das Koordinatensystem$B$

Mit $Die\; Lage\; des\; B-$ Koordinatensystems, jetzt brauchen wir einen Positionsvektor${}^A{P}_{}$um B{B} zu bestimmen$B-$ Koordinatensystem relativ zu$Die\; relative\; Position\; des\; A-$ Koordinatensystems kann schließlich$Der\; Lageausdruck\; des\; B-$ Koordinatensystems ist
$$B={}_B^{}R{,}^A{P}_{}$$

Nachdem wir die Posenausdrücke der beiden Koordinatensysteme im dreidimensionalen Raum gelernt haben, diskutieren wir das Koordinatensystem A {A}Wie liegt ein Vektor in$A$ im Koordinatensystem B{B}Ausgedrückt in$B$ diskutieren wir zunächst den Fall, in dem die Ursprünge der beiden Koordinatensysteme zusammenfallen, unter der Annahme, dass das Koordinatensystem A {A}Es gibt einen VektorPP$in\; A$$P$ ist nach dem Abbildungsprinzip der Matrix der Ortsvektor$P$ liegt im KoordinatensystemA{A}Der Ausdruck in$A$$P$ im KoordinatensystemB{B}Der Ausdruck in$B$$B$ wird auf das Koordinatensystem$A$
$${}^{A}P{=}^B{P}_B^{}R$$

Wenn die Ursprünge der beiden Koordinatensysteme nicht übereinstimmen, müssen wir auch den Positionsvektor ${}^A{P}_{}$,von ${}^A{P}_{}$liegt im Koordinatensystem $A$ wird als Referenz angegeben, sodass keine Zuordnung des Koordinatensystems erforderlich ist. Fügen Sie einfach Vektoren direkt hinzu

$${}^{A}P{=}^B{P}_B^{}R{+}^A{P}_{}$$

Als nächstes stellen wir einen 4x4 vorhomogene TransformationsmatrixUm die obige Formel zu ersetzen, ist dieser Ausdruck klarer und prägnanter
$$\left[\begin{array}{c}{}^{A}P\\ \end{array}\right]=\left[\begin{array}{cc}{}_B^{}R& {}^A{P}_{}\\ [\begin{array}{ccc}& & \end{array}& \end{array}\right]\left[\begin{array}{c}{}^{B}P\\ \end{array}\right]$$

2. Euler-Winkel und Quaternionen

Stellen Sie sich ein Flugzeug mit einem Koordinatensystem B{B} im Schwerpunkt des Flugzeugs vor$B$ , das Koordinatensystem$B$ um seinZZDrehung$der\; Z-$$\alpha -$ Winkel und dann YYum ihn herumDrehung$der\; Y-$$\beta -$ Winkel und schließlich XXum ihn herumDrehung$der\; X-$$\gamma -$ Winkel, dieser Satz von drei Rotationswinkeln wird Euler-Winkel genannt. Es gibt drei Euler-Winkel, nämlich Gier (Gierwinkel), Nick (Nickwinkel) und Roll (Rollwinkel), die den Drehwinkel um die Z/Y/X-Achse darstellen

Gemäß dem oben beschriebenen Verfahren zur Bildung von Euler-Winkeln können wir den mathematischen Ausdruck jeder Drehung
$$R_{}\left(a\right)=\left[\begin{array}{ccc}cos\left(\alpha \right)& -sin\left(a\right)& 0\\ sin\left(a\right)& cos\left(\alpha \right)& 0\\ & & \end{array}\right]$$
$$R_{}\left(b\right)=\left[\begin{array}{ccc}weil\left(b\right)& 0& sin\left(b\right)\\ 0& 1& 0\\ -sin(b& & cos(b\end{array}\right]$$
$$R_{}\left(c\right)=\left[\begin{array}{ccc}1& 0& 0\\ 0& weil\left(c\right)& -sin\left(c\right)\\ & sin(c& cos(c\end{array}\right]$$
Schließlich integrieren wir das Produkt der drei Rotationsmatrizen, um das Koordinatensystem B {B} zu erhaltenDas neue Koordinatensystem B ′ {B^{'}}, das$von\; B$ nach einer Reihe von Drehungen$B^{{}^{\prime }}$ verwenden wir eine Rotationsmatrix${}_{B^{{}^{\text{'}}}}^{}R$ , um das Koordinatensystem$B$ zum Koordinatensystem$B^{{}^{\text{'}}}$ Kartenbeziehung
$${}_{B^{{}^{\text{'}}}}^{}{R}_{Z-Y-}(ein,b,c)=\left[\begin{array}{ccc}cos\left(a\right)cos\left(b\right)& cos\left(a\right)sin\left(b\right)sin\left(c\right)-sin\left(a\right)cos\left(c\right)& cos\left(a\right)sin\left(b\right)cos\left(c\right)+sin\left(a\right)sin\left(c\right)\\ sin\left(a\right)cos\left(b\right)& sin\left(a\right)sin\left(b\right)sin\left(c\right)+cos\left(a\right)cos\left(c\right)& sin\left(a\right)sin\left(b\right)cos\left(c\right)-cos\left(a\right)sin\left(c\right)\\ -sin(b& cos\left(b\right)sin(c& cos\left(b\right)cos(c\end{array}\right]$$

Quaternionen ähneln komplexen Zahlen. Quaternionen gelten eigentlich für Basen $(ein,\overrightarrow{ich},\vec{J},\vec{k})$ ist eine lineare Kombination eines Skalars und eines 3x1-Vektors. Wir verwenden komplexe Zahlen als Quaternion-Analogie. Der Skalar der Quaternion ist der Realteil der komplexen Zahl und der Vektor der Quaternion ist der Imaginärteil der komplexen Zahl . Multiplikation von

Quaternionen Befolgen Sie die folgenden Eigenschaften: Die Multiplikation mit sich selbst ist -1. Befolgen Sie wie bei anderen Multiplikations- und Kreuzmultiplikationseigenschaften auch die Rechte-Hand-Regel

Wir definieren $q$ ist eine Einheitsquaternion, die ausgedrückt werden kann als$Q=w+xi+yj+zk$ , gib$q$ ist eine Einheitsquaternion

Quaternion q \mathbf{q}Die Rotationsmatrix R von$q$$R_{}$kann ausgedrückt werden als

2. Python in ROS implementiert die Koordinatentransformation

1. Nachrichtenträger koordinieren

Die Datenträgernachricht im Abonnementveröffentlichungsmodell stellt eine wichtige Implementierungsverbindung dar. Die häufig verwendete Koordinatendatennachrichtenträgernachricht bei der Realisierung der Koordinatentransformation umfasst geometry_msgs/TransformStamped und Geometry_msgs/PointStamped

Im Folgenden sind einige Daten aufgeführt, die von der Nachricht „geometry_msgs/TransformStamped“ zurückgegeben werden, und wir können die Posenbeziehung zwischen den beiden Koordinatensystemen ermitteln. Im Allgemeinen wählen wir hier das Weltkoordinatensystem „world“ als übergeordnetes Koordinatensystem.

std_msgs/Header header                     #头信息
  uint32 seq                                #|-- 序列号
  time stamp                                #|-- 时间戳
  string frame_id                            #|-- 坐标 ID
string child_frame_id                    #子坐标系的 id
geometry_msgs/Transform transform        #坐标信息
  geometry_msgs/Vector3 translation        #偏移量
    float64 x                                #|-- X 方向的偏移量
    float64 y                                #|-- Y 方向的偏移量
    float64 z                                #|-- Z 方向上的偏移量
  geometry_msgs/Quaternion rotation        #四元数
    float64 x                                
    float64 y                                
    float64 z                                
    float64 w

Im Folgenden finden Sie einige Daten, die von der Nachricht „geometry_msgs/PointStamped“ zurückgegeben werden. Wir können die Position des Punktes im Koordinatensystem bestimmen, zu dem er gehört

std_msgs/Header header                    #头信息
  uint32 seq                                #|-- 序号
  time stamp                                #|-- 时间戳
  string frame_id                            #|-- 所属坐标系的 id
geometry_msgs/Point point                #点坐标
  float64 x                                    #|-- x y z 坐标
  float64 y
  float64 z

Mit diesen Nachrichtendaten können wir einige unterhaltsame und leistungsstarke Funktionen implementieren, z. B. die Schildkrötenverfolgung in der Simulationsumgebung

2. Umsetzung des Programms, dem die Schildkröte folgt

Der Kern der folgenden Implementierung der Schildkröte besteht darin, dass beide Schildkröten A und B Koordinateninformationen relativ zum Weltkoordinatensystem veröffentlichen und diese Informationen dann abonnieren und die Informationen von A relativ zum Koordinatensystem von B konvertieren und erhalten müssen Generieren Sie schließlich Informationen zur Lineargeschwindigkeit und Winkelgeschwindigkeit und steuern Sie die Schildkröte BSports.

Wählen Sie zunächst im src-Verzeichnis des Arbeitsbereichs die Option „Catkin-Paket erstellen“, um ein neues Funktionspaket zu erstellen und hinzuzufügentf2, tf2_ros, tf2_geometry_msgs, roscpp rospy std_msgsWarten Sie auf das abhängige Paket, erstellen Sie dann einen neuen Skriptordner zum Speichern von Python-Dateien und einen Startordner zum Speichern von Startdateien

Zuerst rufen wir den Dienst von ROS auf, um die zweite kleine Schildkröte zu generieren. Um die zweite Schildkröte zu erstellen, müssen wir rosservice verwenden und das Thema ist /spawn

#! /usr/bin/env python

#1.导包
import rospy
from turtlesim.srv import Spawn, SpawnRequest, SpawnResponse

if __name__ == "__main__":
    # 2.初始化 ros 节点
    rospy.init_node("turtle_spawn_p")
    # 3.创建服务客户端
    client = rospy.ServiceProxy("/spawn",Spawn)
    # 4.等待服务启动
    client.wait_for_service()
    # 5.创建请求数据
    req = SpawnRequest()
    req.x = 1.0
    req.y = 1.0
    req.theta = 0
    req.name = "turtle2"
    # 6.发送请求并处理响应
    try:
        response = client.call(req)
        rospy.loginfo("乌龟创建成功，名字是:%s",response.name)
    except Exception as e:
        rospy.loginfo("服务调用失败....")

Dann müssen wir den Veröffentlichungsknoten für das Nachrichtenthema koordinieren und das Thema $/Schildkrötenname/Pose$ , um die Pose der Schildkröte zu erhalten, sie in eine Koordinatennachricht relativ zum Weltkoordinatensystem aufzulösen und$sie\; dann\; zu\; veröffentlichen$

#! /usr/bin/env python

# 1.导包
import rospy
import sys
from turtlesim.msg import Pose
from geometry_msgs.msg import TransformStamped
import tf2_ros
import tf_conversions

turtle_name = ""

def doPose(pose):
    # rospy.loginfo("x = %.2f",pose.x)
    #1.创建坐标系广播器
    broadcaster = tf2_ros.TransformBroadcaster()
    #2.将 pose 信息转换成 TransFormStamped
    tfs = TransformStamped()
    tfs.header.frame_id = "world"
    tfs.header.stamp = rospy.Time.now()

    tfs.child_frame_id = turtle_name
    tfs.transform.translation.x = pose.x
    tfs.transform.translation.y = pose.y
    tfs.transform.translation.z = 0.0

    qtn = tf_conversions.transformations.quaternion_from_euler(0, 0, pose.theta)
    tfs.transform.rotation.x = qtn[0]
    tfs.transform.rotation.y = qtn[1]
    tfs.transform.rotation.z = qtn[2]
    tfs.transform.rotation.w = qtn[3]

    #3.广播器发布 tfs
    broadcaster.sendTransform(tfs)


if __name__ == "__main__":
    # 2.初始化 ros 节点
    rospy.init_node("sub_tfs_p")
    # 3.解析传入的命名空间
    rospy.loginfo("-------------------------------%d",len(sys.argv))
    if len(sys.argv) < 2:
        rospy.loginfo("请传入参数:乌龟的命名空间")
    else:
        turtle_name = sys.argv[1]
    rospy.loginfo("///乌龟:%s",turtle_name)

    rospy.Subscriber(turtle_name + "/pose",Pose,doPose)
    #     4.创建订阅对象
    #     5.回调函数处理订阅的 pose 信息
    #         5-1.创建 TF 广播器
    #         5-2.将 pose 信息转换成 TransFormStamped
    #         5-3.发布
    #     6.spin
    rospy.spin()

Schließlich müssen wir einen Abonnementknoten für das Koordinatennachrichtenthema schreiben, indem wir die TF-Koordinatenübertragungsinformationen von Turtle1 und Turtle2 abonnieren, die neuesten TF-Koordinateninformationen suchen und konvertieren, Turtle1 in Koordinaten relativ zu Turtle2 umwandeln und dann die Lineargeschwindigkeit und den Winkel berechnen Geschwindigkeit und veröffentlichen Sie sie im Thema „Schildkrötenbewegungssteuerung“.

#! /usr/bin/env python

# 1.导包
import rospy
import tf2_ros
from geometry_msgs.msg import TransformStamped, Twist
import math

if __name__ == "__main__":
    # 2.初始化 ros 节点
    rospy.init_node("sub_tfs_p")
    # 3.创建 TF 订阅对象
    buffer = tf2_ros.Buffer()
    listener = tf2_ros.TransformListener(buffer)
    # 4.处理订阅到的 TF
    rate = rospy.Rate(10)
    # 创建速度发布对象
    pub = rospy.Publisher("/turtle2/cmd_vel",Twist,queue_size=1000)
    while not rospy.is_shutdown():

        rate.sleep()
        try:
            #def lookup_transform(self, target_frame, source_frame, time, timeout=rospy.Duration(0.0)):
            trans = buffer.lookup_transform("turtle2","turtle1",rospy.Time(0))
            # rospy.loginfo("相对坐标:(%.2f,%.2f,%.2f)",
            #             trans.transform.translation.x,
            #             trans.transform.translation.y,
            #             trans.transform.translation.z
            #             )   
            # 根据转变后的坐标计算出速度和角速度信息
            twist = Twist()
            # 间距 = x^2 + y^2  然后开方
            twist.linear.x = 0.5 * math.sqrt(math.pow(trans.transform.translation.x,2) + math.pow(trans.transform.translation.y,2))
            twist.angular.z = 4 * math.atan2(trans.transform.translation.y, trans.transform.translation.x)

            pub.publish(twist)

        except Exception as e:
            rospy.logwarn("警告:%s",e)

Fügen Sie im Terminal ausführbare Berechtigungen für Python-Dateien hinzu

chmod +777 *.py

Konfigurieren Sie die Datei CMakeLists.txt gemäß der Methode im letzten Hinweis. Sie kompilieren drei zu deklarierende .py-Dateien und prüfen dann mit catkin_make:build, ob nach dem Kompilieren ein Fehler vorliegt. Wenn kein Fehler vorliegt, können Sie Folgendes schreiben Startdatei.

<launch>
    <node pkg="turtlesim" type="turtlesim_node" name="turtle1" output="screen" />
    <node pkg="turtlesim" type="turtle_teleop_key" name="key_control" output="screen"/>

    <node pkg="xxx" type="xxx.py" name="turtle_spawn" output="screen"/>

    <node pkg="xxx" type="xxx.py" name="tf_pub1" args="turtle1" output="screen"/>
    <node pkg="xxx" type="xxx.py" name="tf_pub2" args="turtle2" output="screen"/>
    <node pkg="xxx" type="xxx.py" name="tf_sub" output="screen"/>
</launch>

Klicken Sie nach dem Schreiben der Startdatei mit der rechten Maustaste auf die Startdatei, um das Terminal zu öffnen, und führen Sie die Startdatei aus, um die Auswirkungen der folgenden Schildkröte zu sehen

roslaunch xxx(功能包) xxx.launch（launch文件）

Zusammenfassen

Das Obige sind die Studiennotizen zur Roboterkoordinatentransformation. In diesem Hinweis werden kurz die theoretischen Grundlagen der Roboterkoordinatentransformation und der Implementierungsprozess der TF-Koordinatentransformation in ROS vorgestellt. Die Bedeutung der Roboterkoordinatentransformation besteht darin, dass sie dafür sorgen kann, dass der Roboter in verschiedenen Koordinatensystemen eine gleichbleibende Bewegungsgenauigkeit beibehält. Denn in verschiedenen Koordinatensystemen ändern sich die Flugbahn- und Positionsinformationen des Roboters. Beispielsweise kann die Bewegung des Roboters im Basiskoordinatensystem eine lineare Bewegung sein, die Bewegung im Werkzeugkoordinatensystem kann jedoch eine Drehbewegung sein. Wenn die Koordinatentransformation nicht durchgeführt wird, weichen die Flugbahn- und Positionsinformationen des Roboters ab, was dazu führt, dass der Roboter die Aufgabe nicht genau ausführen kann.