Transformación de coordenadas TF de componentes avanzados ROS
1 datos
A partir de este artículo, usamos tres blogs para aprender los cambios de coordenadas de TF, TF es transform y el inglés de la película Transformers es transformer.
Los materiales de referencia de este artículo son los siguientes:
(1) "Práctica de desarrollo de robots ROS" Hu Chunxu, Capítulo 4, Sección 2
(2) Sistema de coordenadas de la mano derecha: sistema de coordenadas xyz y regla de la mano derecha en el robot
(3) Ángulo de Euler y cuaternión:
Explicar en detalle la conversión mutua entre ángulo de Euler y cuaternión
Cuaternión y ángulo de Euler (ángulo RPY) Conversión
entre cuaternión y ángulo de Euler (guiñada, cabeceo, balanceo)
2 texto
2.1 Sistema de coordenadas diestro
En el campo de la robótica y la conducción autónoma, todos los sistemas de coordenadas son sistemas de coordenadas de la mano derecha: estire la mano derecha, sosténgala desde la dirección positiva del eje x hacia la dirección positiva del eje y, y la dirección de el pulgar es la dirección positiva del eje z.
2.2 Ángulos de Euler
En el espacio tridimensional, Roll, Pitch y Yaw son los tres ángulos básicos que describen la rotación de los objetos, también conocidos como ángulos de Euler .
Los ángulos de Euler se utilizan para describir la rotación de un objeto en relación con su sistema de coordenadas original.Tomando un avión como ejemplo, su sistema de coordenadas tridimensional original es: el fuselaje está a lo largo del eje x y la nariz está orientada hacia el positivo. dirección del eje X. De acuerdo con la regla de la mano derecha, el eje Y está en el lado izquierdo del plano y el eje Z está en la parte superior del plano.
(1) Roll (rodar): El ángulo en el que el objeto gira alrededor de su propio eje x.
(2) Pitch (pitch): El ángulo en el que el objeto gira alrededor de su propio eje y.
(3) Yaw (guiñada): El ángulo en el que el objeto gira alrededor de su propio eje z.
Aquí hay un diagrama de grupo que también puede ayudar a entender los ángulos de Euler:
2.3 Ángulos de Euler y cuaterniones
Tanto los ángulos de Euler como los cuaterniones se usan para describir la rotación de la física en el espacio tridimensional. Aquí hay una comparación:
(1) Los ángulos de Euler usan tres ángulos de rotación, que son intuitivos y claros, pero habrá un problema de bloqueo del cardán. Para obtener más información, consulte: Explicación detallada del ángulo de Euler y el cuaternión
(2) El cuaternión describe la rotación de un objeto mediante la combinación de vector de rotación + ángulo de rotación, el vector está representado por x, y, z, y el ángulo está representado por w, es decir, q=(x ,y,z,w), es decir, cuaterniones. Además, x, y y z son números complejos, a saber: q=xi+yj+zk+w, i2 = j2 = k2 = -1.
(3) El cuaternión tiene las ventajas de una alta eficiencia de operación, sin problemas de punto muerto del cardán y una conveniente operación de interpolación. Sin embargo, dado que los cuaterniones no pueden entenderse intuitivamente en un sentido físico, son muy abstractos. Por lo tanto, en circunstancias normales, las personas usan los ángulos de Euler para describir las rotaciones y, cuando se requieren operaciones, se convierten en cuaterniones .
(4) Existe un conjunto de fórmulas de conversión de operaciones matriciales fijas entre cuaterniones y ángulos de Euler (no profundizaré aquí, y es realmente difícil de entender): Conversión de cuaterniones y ángulos de Euler (Yaw, Pitch, Roll)
2.4 Origen del paquete de software TF
Un robot suele tener muchos sistemas de coordenadas: manos, pies, cabeza, articulaciones, etc.
Un vehículo autónomo (también un sistema de robot) suele tener muchos sistemas de coordenadas, como:
(1) Proyección universal transversal de Mercator (UTM, las coordenadas GPS son generalmente convertido a este valor de coordenadas)
(2) sistema de coordenadas lidar: este sistema de coordenadas describe los datos de la nube de puntos lidar
(3) sistema de coordenadas de radar (múltiple): igual que arriba
(4) sistema de coordenadas de cámara (múltiple): igual que arriba
( 5) Sistema de coordenadas de imu:
(6) Sistema de coordenadas de la carrocería: el origen se encuentra en el centro del eje trasero
(7) Sistema de coordenadas horizontales locales: ENU, comúnmente conocido como el sistema de coordenadas mundial
Todos los sensores y GPS de los vehículos autónomos Los datos son todos valores relativos a su propio sistema de coordenadas. Al usarlo, debe convertirse al mismo sistema de coordenadas antes de que pueda usarse. Generalmente, se selecciona el sistema de coordenadas ENU, de lo contrario, serán datos dispersos e inválidos.
Por lo general, la conversión de dos sistemas de coordenadas tridimensionales incluye traslación y rotación. La relación de traslación es relativamente simple, es decir, la relación de movimiento entre los dos orígenes de coordenadas; la relación de rotación es más complicada y se requieren ángulos de Euler y cuaterniones.
La conversión de datos del sistema de coordenadas tridimensionales del sistema de robot es muy complicada, y ros proporciona el paquete TF para resolver este difícil problema, evitando que los desarrolladores realicen manualmente la transformación de coordenadas.
2.5 Tome turtle_tf como ejemplo para aprender el paquete TF
ROS proporciona turtle_tf como muestra para que los desarrolladores aprendan los principios básicos del paquete TF.
(1) Ejecute la muestra turtle_tf
roslaunch turtle_tf turtle_tf_demo.launch
rosrun turtlesim turtle_teleop_key
// 如果rviz无显示，看这个博客：https://www.codestudyblog.com/cs2201ai/40115063750.html
rosrun rviz rviz 'rospack find turtle_tf' /rviz/turtle_rviz.rviz
turtle_tf ha diseñado dos tortugas, el operador usa el teclado para controlar el movimiento de la primera tortuga, y la segunda tortuga hará un seguimiento activo, como se muestra en la siguiente figura. (2) El sistema de coordenadas mundial de los tres sistemas de coordenadas de turtle_tf: el origen se encuentra en la esquina inferior izquierda de la ventana, el eje x comienza desde el origen y mira hacia la derecha; el eje y comienza desde el origen y mira hacia la parte superior; de acuerdo con la regla de la mano derecha, el eje z es perpendicular a la
pantalla sistema de coordenadas de tortuga1: el propio sistema de coordenadas de tortuga1, tortuga1 está a lo largo del eje x, la cabeza mira hacia el cuadrado del eje x, el eje y está en el lado izquierdo de tortuga1, el eje z está en la parte superior de tortuga1 (no no te preocupes por eso) sistema de coordenadas de tortuga2: sistema de coordenadas propio de tortuga2, igual que tortuga1 En rviz, los tres sistemas de coordenadas se ven así (nota: las dos capturas de pantalla no son al mismo tiempo): (3) La lógica del paquete TF sistema de coordenadas de gestión: árbol del sistema de coordenadas TF El paquete TF organizará cada sistema de coordenadas en un árbol de sistema de coordenadas interno. Por ejemplo, en el paquete turtle_tf, el nodo raíz del árbol del sistema de coordenadas es el sistema de coordenadas mundial, y los nodos secundarios izquierdo y derecho son los propios sistemas de coordenadas de las dos tortugas. Cada una de las dos tortugas crea su propia emisora TF para transmitir los datos tf de su propio sistema de coordenadas y el sistema de coordenadas mundial, y el paquete TF puede construir un árbol de sistema de coordenadas en tiempo real. Ejecute el siguiente comando para exportar el árbol del sistema de coordenadas TF de turtle_tf:
// ubuntu20.04执行下面命令会崩溃，解决方式：https://github.com/ros/geometry/pull/193
rosrun tf view_frames
(4) Con el árbol del sistema de coordenadas de TF, puede obtener la relación de traslación y rotación de tortuga1 en relación con el sistema de coordenadas de tortuga2 en tiempo real mediante la creación de un oyente de TF . Ejecute el siguiente comando:
rosrun tf tf_echo turtle2 turtle1
Ejecutar salida:
At time 1688002771.829
// 平移向量，也就是两个坐标原点的平移，即在turtle2坐标系下，turtle1的位置
- Translation: [-0.403, 0.072, 0.000]
// 四元数描述的旋转关系，即在turtle2坐标系下，turtle1的朝向
- Rotation: in Quaternion [0.000, 0.000, -0.089, 0.996]
// 弧度描述的欧拉角，因为是二维平面，只有yaw（绕z轴）角有值
in RPY (radian) [0.000, 0.000, -0.178]
// 角度描述的欧拉角
in RPY (degree) [0.000, 0.000, -10.174]
Usando la relación de cambio de coordenadas obtenida, turtle2 puede seguir a turtle1 en tiempo real.Para la implementación específica, consulte el Capítulo 1 de ROS Efficient Advancement - TF Coordinate Transformation of ROS Advanced Components Part 2 .
2.6 Comprensión de turtlesim::Pose y geometric_msgs/Twist
En la serie ROS Efficient Getting Started , escribimos muchos ejemplos, todos usando mensajes turtlesim::Pose y geometric_msgs/Twist. El nombre del tema del primero es: /tortuga1/pose, que se usa para describir la información de posición de la tortuga1; el nombre del tema del último es: /tortuga1/cmd_vel, que se usa para controlar la operación de la tortuga1.
Como no tenemos el conocimiento del sistema de coordenadas antes, no tenemos una comprensión profunda de los campos de datos de estos dos mensajes.Aquí analizamos en profundidad (1)
turtlesim::Pose es un mensaje que define la posición y orientación de la tortuga en el tipo de sistema de coordenadas mundial. Contiene los cinco campos siguientes:
x，y：是world坐标系下的位置
theta：是world坐标系下，乌龟相对于z轴的旋转；
如果乌龟头朝向world坐标系x轴正向，则theta为0；
逆时针旋转时，theta增加，最大为pi；
顺时针旋转时，theta减小，最小为-pi
linear_velocity：线速度，米每秒
angular_velocity：角速度，弧度每秒
(2) El dominio de geometric_msgs/Twist son dos vectores:
geometry_msgs/Vector3 linear // turtle相对于自身坐标系的线速度向量
float64 x // 沿x轴方向的线速度（m/s），正值为前进，负值为后退
float64 y // 沿y轴方向的线速度（m/s），由于turtle永远沿着自身坐标系x轴朝向正方向，不会侧向运动，因此此值一直为0
float64 z // 沿z轴方向的线速度（m/s），由于turtle位于二维平面内，不会上下运动，因此此值一直为0
geometry_msgs/Vector3 angular // turtle相对于自身坐标系的角速度向量
float64 x // 绕x轴的角速度（以弧度/秒为单位），由于turtle不会绕x轴旋转，因此为零
float64 y // 绕y轴的角速度（以弧度/秒为单位），由于turtle不会绕y轴旋转，因此为零
float64 z // 绕z轴的角速度（以弧度/秒为单位），表示turtlez左转（正值），右转（负值）
// 这条命令是让turtle1顺时针画圆，圆半径就是r = v / w = 2.0 m/s / 1.8 rad/s ~= 1.11 m
rostopic pub /turtlesim1/turtle1/cmd_vel geometry_msgs/Twist -r 1 -- '[2.0, 0.0, 0.0]' '[0.0, 0.0, -1.8]'
3 Resumen
En esta sección, comenzamos con las matemáticas, clasificamos sistemáticamente el conocimiento de los cambios de coordenadas tridimensionales y usamos el turtle_tf que viene con ros para aprender inicialmente el principio del paquete de software TF. Con base en el conocimiento de esta sección, implementaremos manualmente una muestra de turtle_tf en la siguiente sección, dominaremos las llamadas de interfaz de la biblioteca TF y dominaremos los métodos de escritura del emisor TF y el oyente TF, consulte ROS Efficient Advanced Capítulo 1 - TF de ROS Componentes avanzados Transformación de coordenadas II