Este artículo resume los problemas relacionados con la conversión del sistema de coordenadas que encuentran los blogueros en su trabajo, así como la implementación de la programación en lenguaje C.
Directorio de artículos
1 escenario de problema
Para el desarrollo de algoritmos ADAS, me he encontrado con muchos escenarios que requieren la conversión del sistema de coordenadas en mi trabajo. Por ejemplo, un automóvil tiene muchos sensores, incluidas cámaras, radares de ondas milimétricas y lidar. Después de detectar la información del entorno externo al mismo tiempo, dado que los diferentes sensores se basan en su propio sistema de coordenadas, la información del objetivo debe convertirse al mismo sistema de coordenadas (generalmente el punto central del eje trasero del automóvil) antes de la fusión de datos .
Por ejemplo, el sistema de coordenadas XOY de la siguiente figura se basa en el punto central del eje trasero del vehículo como origen, la dirección de la parte delantera del vehículo es la dirección positiva del eje X y la dirección vertical de la carrocería del vehículo a la izquierda es la dirección positiva del eje Y. X'O'Y' es el sistema de coordenadas de la cámara de vista frontal, con la posición de la cámara como origen de coordenadas y la dirección del eje de coordenadas es la misma que XY.
El sistema de coordenadas X'O'Y' traduce una distancia hacia adelante en relación con el sistema de coordenadas XOY, y hay una distancia correspondiente a la izquierda y a la derecha. Sabiendo que las coordenadas de un punto P en el sistema de coordenadas X'O'Y' son (x', y'), y las coordenadas del punto O' en el sistema de coordenadas XOY (xo', yo'), puedes pasar las coordenadas Trasladar para calcular las coordenadas del punto P en el sistema de coordenadas XOY. Este es un ejemplo de traducción del sistema de coordenadas.
Para otro ejemplo, cuando el vehículo está en movimiento, si el volante se gira en ángulo, el automóvil realizará un movimiento circular. En un momento determinado t0, después del tiempo Δt, el vehículo no solo tiene una traslación posicional, sino que también tiene una rotación.
En combinación con la figura anterior, por ejemplo, en el momento t0, las coordenadas de un punto de destino en el sistema de coordenadas XOY son (x, y), y en el momento t1, el vehículo se mueve a la posición del punto anterior, y el frente del vehículo se enfrenta a un ángulo a la izquierda. En este momento, la posición relativa entre el punto objetivo y el automóvil ha cambiado, y es necesario volver a calcular sus coordenadas en el sistema de coordenadas X'O'Y'. Utilizando la distancia y el ángulo del sistema de coordenadas X'O'Y' relativo al sistema de coordenadas XOY, se pueden obtener las coordenadas (x', y') del punto objetivo en el sistema de coordenadas X'O'Y'. Este ejemplo incluye traslación y rotación del sistema de coordenadas.
2 Derivación de fórmulas
El problema del escenario anterior se resume así: Las coordenadas del punto conocido P en el sistema de coordenadas XOY son (px,py), y las coordenadas del origen O' del sistema de coordenadas X'O'Y' en el sistema de coordenadas XOY son (ox',oy '), encuentre las coordenadas del punto P en el sistema de coordenadas X'O'Y'. Aquí el sistema de coordenadas adopta el sistema de mano derecha, es decir, X va a la derecha y Y sube. El ángulo de rotación se define como un ángulo positivo en sentido antihorario para su posterior derivación.
Los dos sistemas de coordenadas se transforman mediante traslación y rotación. Para simplificar la derivación, primero haga un sistema de coordenadas intermedio X''O'Y'' basado en el punto O' y complete primero la rotación.
El problema aquí se convierte en derivar primero la relación entre (px', py') y (px'', py''), y luego derivar la relación entre (px'', py'') y (px, py) .
2.1 Derivación del sistema de coordenadas giratorio
Primero, haz líneas verticales PA⊥O'Y', PB⊥O'X', PC⊥O'Y'', PD⊥O'X'' a través del punto P, entonces es fácil saber ∠BPD = θ, como se muestra en la siguiente figura Mostrar.
Luego hacer DE⊥O'X' a través del punto D, como sigue:
De esta manera, px' se puede deducir:
px ′ = ∣ O ′ E ∣ + ∣ EB ∣ = ∣ O ′ D ∣ ⋅ cos θ + ∣ PD ∣ ⋅ pecado θ = px ′ ′ ⋅ cos θ + py ′ ′ ⋅ pecado θ px'\;=\;\vert O'E\vert\;+\vert\;EB\vert\;=\; \vert O'D\vert\cdot\cos\theta\;+\;\vert PD\vert\cdot\sin\theta\;=\;px''\cdot\cos\theta\;\;+\; py' '\cdot\sin\theta\;px _′=∣ O′ mi∣+∣EB ∣=∣ O′ re∣⋅porquei+∣ P D ∣⋅pecadoi=px _"⋅porquei+py _"⋅pecadoi
Luego deduce py', haz DF⊥PF a través del punto D (PF es la línea de extensión de PB), entonces puedes deducir py':
py ′ = ∣ PF ∣ − ∣ BF ∣ = ∣ PD ∣ ⋅ cos θ − ∣ OD ∣ ⋅ pecado θ = py ′ ′ ⋅ cos θ − px ′ ′ ⋅ pecado θ py'\;=\;\vert PF\vert\;-\;\vert\;BF\vert\;=\; \ vert PD\vert\cdot\cos\theta\;-\;\vert OD\vert\cdot\sin\theta\;=\;py''\cdot\cos\theta\;\;-\;px' '\cdot\sin\theta\;py _′=∣ PF ∣−∣BF ∣=∣ P D ∣⋅porquei−∣ O D ∣⋅pecadoi=py _"⋅porquei−px _"⋅pecadoi
Escriba los resultados de las dos derivaciones anteriores juntas, que se usarán en los siguientes capítulos:
px ′ = px ′ ′ ⋅ cos θ + py ′ ′ ⋅ sin θ py ′ = − px ′ ′ ⋅ sin θ + py ′ ′ ⋅ porque θ px'\;=\;px''\cdot\cos\theta\;\;+\;py''\cdot\sin\theta\;\\\; py'\;=- \ ;px''\cdot\sin\theta\;+\;py''\cdot\cos\theta\;\;px _′=px _"⋅porquei+py _"⋅pecadoipy _′=−px _"⋅pecadoi+py _"⋅porquei
Aquí, también se puede usar la transformación de matriz, y esta conclusión se puede sacar en un solo paso. El punto P es equivalente a girar el ángulo θ en el sentido de las agujas del reloj alrededor del origen, por lo que la relación de transformación se puede obtener a través de la fórmula de la matriz de rotación.
[ px ′ py ′ ] = [ porque θ sin θ − sin θ porque θ ] [ px ′ ′ py ′ ′ ] \begin{bmatrix}px'\\py'\end{bmatrix}\;=\ ;\begin{bmatrix}\cos\theta&\sin\theta\\-\sin\theta&\cos\theta\end{bmatrix}\begin{bmatrix}px''\\py''\end{bmatrix}[px _′py _′′]=[porquei−pecadoipecadoiporquei][px _"py _"]
2.2 Derivación del sistema de coordenadas de traslación
El proceso de derivación de la traducción es mucho más simple, como se muestra en la figura a continuación,
es simple suma y resta:
px ′ ′ = px − o ′ xpy ′ ′ = py − o ′ y px''\;=\;px\;-\;o'x\\py''\;=\;py\;-\; oyepx _"=px _−ox _py _"=py _−oy _
2.3 Fórmula completa
Sustituyendo las fórmulas de las dos secciones anteriores en la fórmula completa:
px ′ = ( px − o ′ x ) ⋅ cos θ + ( py − o ′ y ) ⋅ sin θ py ′ = − ( px − o ′ x ) ⋅ pecado θ + ( py − o ′ y ) ⋅ cos θ px'\;=\;(px\;-\;o'x)\cdot\cos\theta\;\;+\;(py\ ;-\;o'y)\cdot\sin\theta\;\\\;py'\;=-\;(px\;-\;o'x)\cdot\sin\theta\;+\; (py\;-\;o'y)\cdot\cos\theta\;\;px _′=( p x−o′ x)⋅porquei+( p y−o′ y)⋅pecadoipy _′=−( p x−o′ x)⋅pecadoi+( p y−o′ y)⋅porquei
Escriba un programa en lenguaje C basado en esta fórmula más adelante
Programación en lenguaje 3C
Primero, analice los requisitos de este programa. La entrada son las coordenadas de un punto en el antiguo sistema de coordenadas y la posición y el ángulo de rotación del nuevo sistema de coordenadas en relación con el antiguo sistema de coordenadas. La salida son las coordenadas del punto en el nuevo sistema de coordenadas.
Aquí, el blogger diseña una función simple para implementar este algoritmo.
#include <stdio.h>
#include <math.h>
#define PI 3.1415926F
typedef struct Point_Tag
{
float X;
float Y;
} Point_Type;//点结构体
typedef struct CoordinatePosition_Tag
{
float deltaX;
float deltaY;
float theta;
} CoordinatePosition_Type;//新坐标系相对旧坐标系位置结构体
//坐标系变换函数
void coordinate_transformation(const Point_Type* const point_old_system_sp,
const CoordinatePosition_Type* const coordinate_position_sp,
Point_Type* const point_new_system_sp)
{
point_new_system_sp->X = (point_old_system_sp->X - coordinate_position_sp->deltaX) * cosf(coordinate_position_sp->theta) +
(point_old_system_sp->Y - coordinate_position_sp->deltaY) * sinf(coordinate_position_sp->theta);
point_new_system_sp->Y = -(point_old_system_sp->X - coordinate_position_sp->deltaX) * sinf(coordinate_position_sp->theta) +
(point_old_system_sp->Y - coordinate_position_sp->deltaY) * cosf(coordinate_position_sp->theta);
}
//main函数中简单测试
int main()
{
Point_Type point_old_system;
CoordinatePosition_Type coordinate_position;
Point_Type point_new_system;
point_old_system.X = 2.0F;
point_old_system.Y = 2.0F;
coordinate_position.deltaX = 0.0F;
coordinate_position.deltaY = 0.0F;
coordinate_position.theta = (45.0F/180.0F*PI);
coordinate_transformation(&point_old_system,&coordinate_position,&point_new_system);
printf("NEW_X = %f,NEW_Y = %f", point_new_system.X,point_new_system.Y);
}
La implementación del código es relativamente simple, es decir, la fórmula se traduce a código C. Se pasan tres parámetros en la función de función, los primeros dos punteros se utilizan para ingresar la posición del punto en el sistema de coordenadas anterior y la posición y el ángulo del nuevo sistema de coordenadas en el sistema de coordenadas anterior. El tercer puntero se utiliza para obtener el resultado de salida.