[학습 요약] openvins의 IMU 데이터 시뮬레이션

이 기사에서는 제어 궤적과 SPline 보간을 기반으로 하는 openvins의 IMU 시뮬레이션 원리와 코드를 소개하고 IMU 출력을 계산합니다.

참고

Open-vins의 시뮬레이션에 대한 설명: https://docs.openvins.com/simulation.html
Open-vins 논문: https://pgeneva.com/downloads/papers/Geneva2020ICRA.pdf
Open-vins 논문에서 사용되는 SE3 보간: https://link.springer.com/article/10.1007/s11263-015-0811-3
위 논문에서 언급한 BSpline의 행렬 표현: https://xiaoxingchen.github.io/2020/03/02/bspline_in_so3 / General_matrix_representation_for_bsplines.pdf

Open-Vin의 부품 구조 시뮬레이션

open-vins의 파일 디렉터리에는 다음 파일이 시뮬레이션과 관련되어 있습니다.

ov_core/
	src/
		sim/
			BsplineSE3.cpp
			BsplineSE3.h
ov_msckf/
	src/
		sim/
			Simulator.cpp
			Simulator.h
		test_sim_meas.cpp
ov_data/
	sim/
		xxx.txt

그 중 test_sim_meas.cpp시뮬레이션 프로그램의 입구로 클래스라고 불리는 주요 기능이 있는데, Simualtoropenvins의 모든 컨텐츠는 이 코어를 기반으로 해야 하며 ov_core특정 보간 부분에서는 BsplineSE3이 클래스를 사용합니다. 로드된 궤적 데이터 형식은 ov_data 경로의 파일을 참조하십시오.

test_sim_meas.cpp
내부의 핵심 기능은 get_next_imu(time_imu, wm, am)클래스 내 궤적 데이터를 기반으로 특정 순간의 imu 출력을 계산하는 것입니다.

get_next_imu
클래스 에는 Simulator두 가지 주요 내용이 있습니다. Bspline 보간법을 사용하여 가속도 및 각속도를 얻은 spline->get_acceleration다음 랜덤 워크 및 측정 노이즈를 추가하는 것입니다.

get_acceleration
카테고리 별로 분류되어 있으며 BsplineSE3, 주요 내용은 다음과 같습니다.

1. 궤적의 제어점을 가져옵니다.find_bounding_control_points()
2. De Boor-Cox 행렬 계수 계산
3. 현재 순간의 포즈를 얻기 위한 궤적 보간
4. 포즈를 가속도 및 각속도로 변환(월드 시스템)
5. 월드 시스템의 가속도와 각속도를 IMU 시스템으로 변환

1의 경우 궤적의 현재 시점 전후의 두 포즈(즉,pose0,1,2,3)를 꺼냅니다. 보간이 필요한 현재 순간은 1과 2 사이입니다.

원리 설명

De Boor-Cox 행렬 계수 및 궤적 보간

우선, openvins 논문에 제시된 보간 공식은 다음과 같습니다.
이 공식은 매우 간단하여 무엇을 하는지 알 수 없습니다. 간단히 말하면, 시간 u(ts)에서의 포즈 T를 계산하기 위해 이 포즈를 S 시스템에서 G(월드) 시스템으로 전달하므로 이때 시간의 A 0 , A의 합( i-1) 1 , A 2 A_0, A_1,A_2 를 사용해야 합니다.${ㅏ}_{},{ㅏ}_{},{ㅏ}_{}$이 4개의 행렬이 계산됩니다. 방정식 (29)와 (30)은 $A$ 의 표현

이것이 계산되는 이유를 이해하려면 참조 [38]:
센서 융합 및 롤링 셔터 카메라에 대한 스플라인 기반 궤적 표현을 읽어야 합니다.

SE3 기반 Bspline 보간
이 글의 동기는 다중 센서 융합 시 여러 센서가 동시에 데이터를 얻을 수 없기 때문에 Bspline 보간이 필요하므로 SE3 보간을 제안한다.

구체적으로 보간 방법에는 (1)과 (2)의 두 가지 표현이 있습니다. 여기서 $B$ 와$\widetilde{비}$ 는 두 가지 다른 표현의 기초이지만 본질적으로 동일합니다. 여기서는 두 번째 표현이 사용됩니다.

SE3에 대한 이 표현의 공식은 다음과 같습니다.
여기서 기호 표현은 openvins의 기호 표현과 다릅니다. 여기서 s는 보간 순간을 나타내고 w는 세계 시스템을 나타냅니다. ${오}_{}$위의 $로그(티\_\_{}_{w,나는-1}^{-},{티}_{여,})$ , 그리고 이전 포즈 사이의$세3$。

이때, 위의 식(3)에서 남는 변수는 ${\widetilde{비}}_{0,}\left(t\right)$ , 이에 대한 계산 방법은 Qin2000(General Matrix Representations for B-Splines) 논문에 나와 있습니다. 이 논문은 너무 복잡해서 이해하기가 어렵습니다. 간단하게 요약하자면 BSpline은 행렬로 표현된다는 점입니다. 여기서는 건너뛰세요.

목표는 IMU에 데이터를 제공하는 것이므로 가속도가 연속적이기를 바라므로 궤적 곡선은 C2 연속(즉, 2차 미분 연속)을 채택합니다. C2 연속은 4개의 제어점, 즉 2개의 제어점만 필요합니다. 앞쪽에 2개, 뒤쪽에 2개. 이때 행렬은 4x4이다. uu위 수식에서$u는$ 이전 제어점을 기준으로 보간 증분입니다. 동시에 여기에는 B~\~B가 주어진다.$비$의 1차 및 2차 도함수입니다$\stackrel{.}{}$

다음으로, 논문에서는 궤적 보간 결과와 보간의 1차 도함수(속도) 및 2차 도함수(가속도) 표현을 제공합니다. 다음 $A$ 는 openvins와 일치합니다.

포즈로부터 IMU 시스템의 가속도와 각속도를 계산합니다.

위의 계산 후에 우리는 궤적이 위치한 좌표계(즉, 월드 시스템)를 기준으로 궤적, 속도 및 가속도만 구하고 다음으로 IMU 시스템을 기준으로 해야 합니다. 이 모델은 상대적으로 간단합니다.

중력을 추가한 후 가속도가 IMU 시스템으로 전달되고,
각속도는 회전 행렬을 통해 미분된 다음 IMU 시스템으로 전달되어야 합니다.

강령과 원칙의 대응

openvins의 BsplineSE3에서 가속도 및 각속도를 얻기 위한 코드를 직접 복사합니다.

//~ 输入：待插值的时间戳；
/*~ 输出：IMU的加速度、角速度，以及一些姿态量；

~*/
bool BsplineSE3::get_acceleration(double timestamp, Eigen::Matrix3d &R_GtoI, Eigen::Vector3d &p_IinG, 
	Eigen::Vector3d &w_IinI, Eigen::Vector3d &v_IinG, Eigen::Vector3d &alpha_IinI, Eigen::Vector3d &a_IinG) {
    
    

  // Get the bounding poses for the desired timestamp
  double t0, t1, t2, t3;
  Eigen::Matrix4d pose0, pose1, pose2, pose3;
  bool success = find_bounding_control_points(timestamp, control_points, t0, pose0, t1, pose1, t2, pose2, t3, pose3);

  // Return failure if we can't get bounding poses
  if (!success) {
    
    
    alpha_IinI.setZero();
    a_IinG.setZero();
    return false;
  }
  //~ 这里就是计算上述B矩阵，B的一阶导，二阶导矩阵；
  // Our De Boor-Cox matrix scalars
  double DT = (t2 - t1);
  double u = (timestamp - t1) / DT;
  double b0 = 1.0 / 6.0 * (5 + 3 * u - 3 * u * u + u * u * u);
  double b1 = 1.0 / 6.0 * (1 + 3 * u + 3 * u * u - 2 * u * u * u);
  double b2 = 1.0 / 6.0 * (u * u * u);
  double b0dot = 1.0 / (6.0 * DT) * (3 - 6 * u + 3 * u * u);
  double b1dot = 1.0 / (6.0 * DT) * (3 + 6 * u - 6 * u * u);
  double b2dot = 1.0 / (6.0 * DT) * (3 * u * u);
  double b0dotdot = 1.0 / (6.0 * DT * DT) * (-6 + 6 * u);
  double b1dotdot = 1.0 / (6.0 * DT * DT) * (6 - 12 * u);
  double b2dotdot = 1.0 / (6.0 * DT * DT) * (6 * u);

  // Cache some values we use alot
  Eigen::Matrix<double, 6, 1> omega_10 = log_se3(Inv_se3(pose0) * pose1);
  Eigen::Matrix<double, 6, 1> omega_21 = log_se3(Inv_se3(pose1) * pose2);
  Eigen::Matrix<double, 6, 1> omega_32 = log_se3(Inv_se3(pose2) * pose3);
  Eigen::Matrix4d omega_10_hat = hat_se3(omega_10);
  Eigen::Matrix4d omega_21_hat = hat_se3(omega_21);
  Eigen::Matrix4d omega_32_hat = hat_se3(omega_32);

	// 这里计算了A0~A2，以及一阶导、二阶导
  // Calculate interpolated poses
  Eigen::Matrix4d A0 = exp_se3(b0 * omega_10);
  Eigen::Matrix4d A1 = exp_se3(b1 * omega_21);
  Eigen::Matrix4d A2 = exp_se3(b2 * omega_32);
  Eigen::Matrix4d A0dot = b0dot * omega_10_hat * A0;
  Eigen::Matrix4d A1dot = b1dot * omega_21_hat * A1;
  Eigen::Matrix4d A2dot = b2dot * omega_32_hat * A2;
  Eigen::Matrix4d A0dotdot = b0dot * omega_10_hat * A0dot + b0dotdot * omega_10_hat * A0;
  Eigen::Matrix4d A1dotdot = b1dot * omega_21_hat * A1dot + b1dotdot * omega_21_hat * A1;
  Eigen::Matrix4d A2dotdot = b2dot * omega_32_hat * A2dot + b2dotdot * omega_32_hat * A2;

  // Get the interpolated pose
  Eigen::Matrix4d pose_interp = pose0 * A0 * A1 * A2;
  R_GtoI = pose_interp.block(0, 0, 3, 3).transpose();
  p_IinG = pose_interp.block(0, 3, 3, 1);

  // Get the interpolated velocities
  // NOTE: Rdot = R*skew(omega) => R^T*Rdot = skew(omega) 
  //~ 式(5)，计算角速度插值，角速度转回IMU系
  Eigen::Matrix4d vel_interp = pose0 * (A0dot * A1 * A2 + A0 * A1dot * A2 + A0 * A1 * A2dot);
  w_IinI = vee(pose_interp.block(0, 0, 3, 3).transpose() * vel_interp.block(0, 0, 3, 3));
  v_IinG = vel_interp.block(0, 3, 3, 1);

  // Finally get the interpolated velocities
  // NOTE: Rdot = R*skew(omega)
  // NOTE: Rdotdot = Rdot*skew(omega) + R*skew(alpha) => R^T*(Rdotdot-Rdot*skew(omega))=skew(alpha)
  //~ 式(6)，计算速度插值，加速度转回IMU系
  Eigen::Matrix4d acc_interp = pose0 * (A0dotdot * A1 * A2 + A0 * A1dotdot * A2 + A0 * A1 * A2dotdot + 2 * A0dot * A1dot * A2 + 2 * A0 * A1dot * A2dot + 2 * A0dot * A1 * A2dot);   
  Eigen::Matrix3d omegaskew = pose_interp.block(0, 0, 3, 3).transpose() * vel_interp.block(0, 0, 3, 3);
  alpha_IinI = vee(pose_interp.block(0, 0, 3, 3).transpose() * (acc_interp.block(0, 0, 3, 3) - vel_interp.block(0, 0, 3, 3) * omegaskew));		
  a_IinG = acc_interp.block(0, 3, 3, 1);
  return true;
}