Link de referencia:
[Conducción autónoma] Algoritmo de detección de colisiones
Un método común es el teorema de separación de hiperplanos, que se utiliza para detectar si existe una intersección de conjuntos convexos en el espacio euclidiano de dimensión N. Para obtener una definición estricta, consulte Wikipedia . Su caso bidimensional se llama Teorema del eje de separación, que se describe brevemente de la siguiente manera:
La condición necesaria y suficiente para la disjunción de dos conjuntos planos convexos es que exista una determinada recta (es decir, el eje de separación), de modo que las proyecciones de los dos planos sobre la recta no se corten, y se pueda escribir una proposición negativa de manera similar Obviamente, para probar la intersección necesitamos agotar todas las rectas, lo cual es imposible. Afortunadamente, para polígonos convexos, solo necesitamos verificar si la línea en la que se encuentra su borde es una línea de separación (no un eje de separación). (Puede pensar en cómo implementar polígonos y círculos cóncavos)
En el ámbito de la conducción autónoma, los obstáculos generalmente se representan mediante rectángulos planos (se utilizarán polígonos planos convexos para mayor precisión), y para dos rectángulos, solo necesitamos atravesar 4 lados (dos rectángulos adyacentes por cada lado). , siempre que uno de los cuatro lados sea el eje de separación, los dos rectángulos no se intersecan . Por el contrario, si los dos rectángulos se cruzan, los cuatro lados no son el eje de separación ( compare la mitad de la suma de la longitud de proyección del vehículo del ego y el contorno objetivo en el eje de separación, y el tamaño de la distancia de proyección, el la distancia de proyección es más pequeña que la proyección y la superposición hasOverLap ), es decir, sus proyecciones en él coinciden.
Para representar eficientemente un rectángulo plano, generalmente lo definimos así:
struct Box2d {
float center_x_;
float center_y_;
float length_;
float width_;
float half_length_;
float half_width_;
float heading_;
float cos_heading_;
float sin_heading_;
float min_x_;
float max_x_;
float min_y_;
float max_y_;
struct Vec2d corners_[4];
};Principio de proyección: Proyecte los dos lados adyacentes del vehículo y los dos lados adyacentes del obstáculo
Solo necesitamos comparar la mitad de la suma de la longitud de proyección del vehículo del ego y el contorno objetivo en el eje de separación, y el tamaño de la distancia de proyección, para determinar si las proyecciones de los dos se cruzan. Obviamente, la longitud proyectada del ego car en el eje de separación es length_.
El siguiente código es el cálculo de proyección en los dos lados adyacentes del Vehículo correspondiente
std::abs(shift_x * cos_heading_ + shift_y * sin_heading_) <=
std::abs(dx3 * cos_heading_ + dy3 * sin_heading_) +
std::abs(dx4 * cos_heading_ + dy4 * sin_heading_) +
half_length_ &&
std::abs(shift_x * sin_heading_ - shift_y * cos_heading_) <=
std::abs(dx3 * sin_heading_ - dy3 * cos_heading_) +
std::abs(dx4 * sin_heading_ - dy4 * cos_heading_) +
half_width_
Es decir, algún punto del vehículo no se encuentra dentro de la casilla naranja punteada correspondiente al obstáculo.
El siguiente código es el cálculo de proyección en los dos bordes adyacentes correspondientes al Obstáculo
std::abs(shift_x * box.cos_heading() + shift_y * box.sin_heading()) <=
std::abs(dx1 * box.cos_heading() + dy1 * box.sin_heading()) +
std::abs(dx2 * box.cos_heading() + dy2 * box.sin_heading()) +
box.half_length() &&
std::abs(shift_x * box.sin_heading() - shift_y * box.cos_heading()) <=
std::abs(dx1 * box.sin_heading() - dy1 * box.cos_heading()) +
std::abs(dx2 * box.sin_heading() - dy2 * box.cos_heading()) +
box.half_width()
Box2d::HasOverlap(const Box2d &box)
bool Box2d::HasOverlap(const Box2d &box) const {
if (box.max_x() < min_x() || box.min_x() > max_x() || box.max_y() < min_y() ||
box.min_y() > max_y()) {
return false;
}
const double shift_x = box.center_x() - center_.x();
const double shift_y = box.center_y() - center_.y();
const double dx1 = cos_heading_ * half_length_;
const double dy1 = sin_heading_ * half_length_;
const double dx2 = sin_heading_ * half_width_;
const double dy2 = -cos_heading_ * half_width_;
const double dx3 = box.cos_heading() * box.half_length();
const double dy3 = box.sin_heading() * box.half_length();
const double dx4 = box.sin_heading() * box.half_width();
const double dy4 = -box.cos_heading() * box.half_width();
return std::abs(shift_x * cos_heading_ + shift_y * sin_heading_) <=
std::abs(dx3 * cos_heading_ + dy3 * sin_heading_) +
std::abs(dx4 * cos_heading_ + dy4 * sin_heading_) +
half_length_ &&
std::abs(shift_x * sin_heading_ - shift_y * cos_heading_) <=
std::abs(dx3 * sin_heading_ - dy3 * cos_heading_) +
std::abs(dx4 * sin_heading_ - dy4 * cos_heading_) +
half_width_ &&
std::abs(shift_x * box.cos_heading() + shift_y * box.sin_heading()) <=
std::abs(dx1 * box.cos_heading() + dy1 * box.sin_heading()) +
std::abs(dx2 * box.cos_heading() + dy2 * box.sin_heading()) +
box.half_length() &&
std::abs(shift_x * box.sin_heading() - shift_y * box.cos_heading()) <=
std::abs(dx1 * box.sin_heading() - dy1 * box.cos_heading()) +
std::abs(dx2 * box.sin_heading() - dy2 * box.cos_heading()) +
box.half_width();
}