Corrección de distorsión de movimiento de la nube de puntos láser mediante IMU

Corrección de distorsión de movimiento de la nube de puntos láser mediante IMU

1. Principios del pensamiento

En el proceso de posicionamiento y mapeo de SLAM láser, cuando el lidar se mueve lentamente y no tiene una rotación relativamente grande, la distorsión de movimiento de la nube de puntos se puede ignorar y la posición del radar aún se puede estimar mediante el registro ICP durante la comparación cuadro a cuadro. Sin embargo, cuando el rango de movimiento lidar es relativamente grande, se debe considerar la distorsión de movimiento de la nube de puntos láser, y la información de estimación de pose a corto plazo proporcionada por la IMU o el codificador se puede usar para eliminar la distorsión de movimiento de la nube de puntos. Aquí utilizo la IMU para eliminar la distorsión de movimiento de la nube de puntos. La salida integral de información de aceleración de la IMU es muy divergente. Si es un dispositivo IMU peor, la información de aceleración no se utilizará. Aquí, se considera la información de velocidad angular de la IMU. En el tiempo de la nube de puntos de 1 cuadro, la velocidad angular de todas las IMU se integra para obtener el ángulo de rotación de la IMU, y luego se convierte al lidar, y la interpolación lineal esférica del cuaternión de actitud se realiza en la actitud del radar para convertir la nube de puntos correspondiente a la actitud del estado final para eliminar la distorsión del movimiento de la nube de puntos.

2. Proceso de operación

Equipamiento usado:

• Lidar: Velodyne VLP16, frecuencia 10HZ, rotación mecánica, 16 líneas, en dirección vertical $(-15^{\circ },15^{\circ })$, el ángulo entre cada línea es$2^{\circ }$ , hay 1800 puntos láser en el plano horizontal, la resolución promedio$0\_2^{\circ }$。
• IMU: D435i, la frecuencia es de 200 HZ.

proceso específico:

1. Integración de velocidad angular para la IMU : la integración de la velocidad angular se refiere al proceso de preintegración de VINS-Mono, y la frecuencia del lidar es mucho más baja que la frecuencia de la IMU. En un ciclo de datos de una nube de puntos (100 ms), la IMU generará alrededor de $200÷10=20$个，根据公式：
   $$\begin{gathered} a_{}=\frac{{Vaya}_{t_{}}+{Vaya}_{}}{2}\cdot dt\_ \\ dt\_=t-t_{} \\ a=\sum _{yo=0}^{n-}{a}_{} \end{gathered}$$
   donde $a_{}$para $El\; tiempo\; t$ es relativo al tiempo anterior$t_{}$El cambio de ángulo de ${Vaya}_{t_{}},{Vaya}_{}$son la velocidad angular en el momento correspondiente, $n$ Datos de la IMU. De esta forma, el ángulo de rotación de la IMU en un ciclo se puede obtener integrando, acumulando e iterando continuamente.

   Aquí, se adopta el método de cálculo de cuaternión. Cuaternión $q=\left[\begin{array}{c}cos(\_\_\frac{}{2})\\ pecado(\_\_\frac{}{2})\cdot \end{array}\right]=\lfloor \begin{array}{c}w\\ X\\ y\\ \end{array}\rfloor$, para datos IMU adyacentes $\frac{}{2}$Todos tienden a 0, equivalente infinitesimal disponible
   $$\begin{gathered} \underset{perro\to 0}{}\frac{sen(perro}{perro}=1 \\ \underset{perro\to 0}{}cos\left(perro\right)\_\_=1 \end{gathered}$$
   则有：
   $$\begin{gathered} q_{}=q_{t_{}}\cdot q_{dt} \\ {q}_{dt}=(1,\frac{a_{}}{2},\frac{a_{}}{2},\frac{a_{}}{2}) \end{gathered}$$
   donde$q_{}$Actitud cuaternión en el momento, $q_{t_{}}$es el cuaternión de actitud en el momento anterior, $q_{dt}$es el valor de cambio del cuaternión en este período de tiempo, $a_{}$, $a_{}$, $a_{}$es el cambio de ángulo de tres ejes XYZ correspondiente.

   Adjuntar parte del código de ROS+Eigen

   std::vector<sensor_msgs::Imu::ConstPtr> imu_msg_vector;
   std::mutex mutex_lock;
   
   // IMU消息回调存储
   void imu_cb(const sensor_msgs::Imu::ConstPtr &imu_msg)
   {
         
         
       mutex_lock.lock();
       imu_msg_vector.push_back(imu_msg);
       mutex_lock.unlock();
   }
   
   // IMU角度积分
   Eigen::Quaterniond imu_q = Eigen::Quaterniond::Identity(); // R
   Eigen::Vector3d angular_velocity_1;
   angular_velocity_1 << imu_msg_v[0]->angular_velocity.x, imu_msg_v[0]->angular_velocity.y, imu_msg_v[0]->angular_velocity.z;
   double lastest_time = imu_msg_v[0]->header.stamp.toSec();
   for (int i = 1; i < imu_msg_v.size(); i++)
   {
         
         
       double t = imu_msg_v[i]->header.stamp.toSec();
       double dt = t - lastest_time;
       lastest_time = t;
       Eigen::Vector3d angular_velocity_2;
       angular_velocity_2 << imu_msg_v[i]->angular_velocity.x, imu_msg_v[i]->angular_velocity.y, imu_msg_v[i]->angular_velocity.z;
       Eigen::Vector3d aver_angular_vel = (angular_velocity_1 + angular_velocity_2) / 2.0;
       imu_q = imu_q * Eigen::Quaterniond(1, 0.5 * aver_angular_vel(0) * dt, 0.5 * aver_angular_vel(1) * dt, 0.5 * aver_angular_vel(2) * dt);
   }
   

2. Transformación de actitud IMU->radar : El cambio de ángulo de actitud de la IMU medido arriba debe asignarse al sistema de coordenadas LIDAR. La implementación necesita conocer la matriz de transformación de transformación de parámetros externos de lidar e IMU (no se usa el vector de traducción, pero debe usarse la matriz de rotación), $R_{lidar}^{yosoy}$Es la matriz de rotación de lidar a IMU (debe calibrarse con anticipación). Durante el movimiento de lidar e IMU, se puede considerar como $R_{lidar}^{yosoy}$Inmutable. Relacionado:
   $$R_{lida{r}_{}}^{lida{r}_{}}\cdot R_{lidar}^{yosoy}=R_{lidar}^{yosoy}\cdot R_{yosoy{tu}_{}}^{yosoy{tu}_{}}$$
   其中$R_{lida{r}_{}}^{lida{r}_{}}$Es la transformación de actitud desde el estado inicial hasta el estado final del radar, $R_{IMU_0}^{IMU_1} es la transformación de actitud desde el estado $Eslatransformaciónde\; actituddelestadoinicialal\; estadofinaldeIMU(esdecir,elcuaternión\; delángulode\; actitudIMU$ q$$integrado$ $arriba$ $)\; .$Multiplique la fórmula por la inversa al mismo tiempo, podemos obtener:
   $$R_{lida{r}_{}}^{lida{r}_{}}=R_{lidar}^{yosoy}\cdot R_{yosoy{tu}_{}}^{yosoy{tu}_{}}\cdot {R_{lidar}^{yosoy}}^{-1}$$
   código propio:

   Eigen::Matrix3d lidar_imu_R << -1,     -1.22465e-16,            0,
             1.22465e-16,          -1,            0,
              0,            0,           1;     // lidar->IMU 外参数旋转矩阵
   Eigen::Matrix3d lidar_R;
   lidar_R = lidar_imu_R * (imu_q.toRotationMatrix()) * lidar_imu_R.inverse();
   

   El efecto es el siguiente:
   

3. Realice la conversión de interpolación lineal esférica en el cuaternión de actitud del radar : en un ciclo (100 ms), el radar gira en el sentido de las agujas del reloj en la vista superior y el ángulo horizontal es el siguiente:
   

   En un plano horizontal de 360 ​​grados, dividido en 1800 partes, primero calcula el ángulo θ i \theta_i correspondiente a cada nube de puntos$i_{}$:
   $$\begin{gathered} i_{}=arcos\_\_\_\_\_\left(\frac{X_{}}{\sqrt{X_i^{}+y_i^{}}}\right),yosi\_{}_{}<0 \\ {i}_{}=14:00\_-arcos\_\_\_\_\_\left(\frac{X_{}}{\sqrt{X_i^{}+y_i^{}}}\right),yosi\_{}_{}>0 \end{gathered}$$
   再进行插值，注意是将当前时刻的点云转换到末状态雷达姿态上：
   $$q_{sler}=Slerp\left(q_{},q_{};t\right)=q_{}{\left(q_0^{-}{q}_{}\right)}^t=q_{}{\left(q_1^{-}{q}_{}\right)}^{1-t}={\left(q_{}{q}_1^{-}\right)}^{1-t}{q}_{}={\left(q_{}{q}_0^{-}\right)}^t{q}_{}$$
   donde $q_{}$es el cuaternión inicial, $q_{}$es el cuaternión de estado final, $t$ es el tamaño de paso de la interpolación,$t\in [0,1]$。

   El cuaternión $q_{sler}$Convertir a matriz de rotación $R_{sler}$Luego, la forma puede usar la fórmula de transformación de coordenadas: P
   $${PAG}_{}=\lfloor \begin{array}{c}{X}_{}\\ y_{}\\ z_{}\\ \end{array}\rfloor =T_{sler}\cdot {PAG}_{}=\left[\begin{array}{cc}{R}_{sler}& 0\\ & \end{array}\right]\cdot \lfloor \begin{array}{c}{X}_{}\\ y_{}\\ z_{}\\ \end{array}\rfloor$$
   donde ${PAG}_{}$son las coordenadas originales de la nube de puntos, ${PAG}_{}$son las coordenadas de la nube de puntos convertida.

   Adjunte parte del código Eigen:

   pcl::PointCloud<pcl::PointXYZI> cloud_undistorted;
   pcl::copyPointCloud(cloud_in, cloud_undistorted);
   
   for (int i = 0; i < cloud_undistorted.points.size(); i++)
   {
         
         
       double horizon_angle; // 化为角度为单位
       if (cloud_rgb.points[i].y < 0)
       {
         
         
           double theta = acos(cloud_undistorted.points[i].x / sqrt(cloud_undistorted.points[i].x * cloud_undistorted.points[i].x + cloud_undistorted.points[i].y * cloud_undistorted.points[i].y));
           horizon_angle = theta * 180.0 / M_PI;
       }
       else
       {
         
         
           double theta = acos(cloud_undistorted.points[i].x / sqrt(cloud_undistorted.points[i].x * cloud_undistorted.points[i].x + cloud_undistorted.points[i].y * cloud_undistorted.points[i].y));
           horizon_angle = (2 * M_PI - theta) * 180 / M_PI;
       }
       int id = int(horizon_angle / 360.0 * 1800);
       // std::cout << "horizon_angle: " << horizon_angle << ", id: " << id << std::endl;
   
       Eigen::Quaterniond q_slerp = lidar_q0.slerp((1.0 - horizon_angle / 360.0), lidar_q);
       Eigen::Matrix3d R_slerp = q_slerp.toRotationMatrix();
       Eigen::Matrix4d T_lidar_slerp;
       T_lidar_slerp << R_slerp(0, 0), R_slerp(0, 1), R_slerp(0, 2), 0,
           R_slerp(1, 0), R_slerp(1, 1), R_slerp(1, 2), 0,
           R_slerp(2, 0), R_slerp(2, 1), R_slerp(2, 2), 0,
           0, 0, 0, 1;
       Eigen::Vector4d Pi, Pj;
       Pi << cloud_undistorted.points[i].x, cloud_undistorted.points[i].y, cloud_undistorted.points[i].z, 1;
       Pj = T_lidar_slerp.inverse() * Pi;
       cloud_undistorted.points[i].x = Pj(0);
       cloud_undistorted.points[i].y = Pj(1);
       cloud_undistorted.points[i].z = Pj(2);
   }
   

4. Efecto:

   Efecto de procesamiento de nube de puntos de fotograma único:
   
   la nube de puntos blanca en la figura son los datos originales y la nube de puntos verde es el efecto de nube de puntos sin distorsión. De esta manera, el efecto real no se puede ver, y luego hay un ejemplo de construcción de un mapa para ilustrar.

   Entorno de pasillo exterior:
   
   el rojo es la dirección de desplazamiento del lidar portátil. Uso de A-LOAM para el mapeo, sin construcción de nubes de puntos sin distorsión:
   
   después de la eliminación de distorsión, la construcción de nubes de puntos sigue siendo tan estable como un perro viejo en un entorno grande con una gran rotación, el efecto es el siguiente: