센서에서 선형 및 각도 데이터를 융합하는 방법

우리 팀과 나는 상용 등급의 ​​인코더가있는 실외 로봇을 설치하고 있습니다 IMU 및 GPS 센서 있습니다. 로봇에는 기본 탱크 구동 장치가 있으므로 인코더는 왼쪽 및 오른쪽 바퀴에서 틱을 충분히 공급합니다. IMU는 롤, 피치, 요 및 선형 가속을 x, y 및 z로 제공합니다. 나중에 다른 IMU를 추가하여 중복성을 제공 할 수 있지만 롤, 피치 및 요의 각속도를 추가로 제공 할 수도 있습니다. GPS는 전역 x, y 및 z 좌표를 게시합니다.

로봇의 xy 위치와 방향을 알면 로봇이 탐색 할 환경을 현지화하고 매핑하는 데 유용합니다. 로봇의 속도는 부드러운 움직임 결정에 유용 할 수 있습니다. 지면 기반 로봇이므로 z 축에 대해서는 신경 쓰지 않습니다. 로봇에는 라이더 센서와 카메라가 있으므로 롤과 피치는 방향을 개선하기 위해 라이더와 카메라 데이터를 변환하는 데 유용합니다.

모든 센서의 정확도를 최적으로 활용하는 방식으로 이러한 모든 숫자를 통합하는 방법을 알아 내려고 노력 중입니다. 현재 우리는 칼만 필터 를 [x, x-vel, x-accel, y, y-vel, y-accel]사용하여 간단한 전이 행렬 의 추정치를 생성하고 있습니다 .

[[1, dt, .5*dt*dt, 0,  0,        0],
 [0,  1,       dt, 0,  0,        0],
 [0,  0,        1, 0,  0,        0],
 [0,  0,        0, 1, dt, .5*dt*dt],
 [0,  0,        0, 0,  1,       dt],
 [0,  0,        0, 0,  0,        1]]

필터는 IMU에서 제공 한 가속을 기반으로 만 상태를 추정합니다. (IMU는 최고 품질이 아닙니다. 약 30 초 내에 로봇이 초기 위치에서 20 미터 정도 표류하는 로봇을 보여줍니다.) 롤, 피치 및 요를 사용하는 방법을 알고 싶습니다 IMU, 잠재적으로 롤, 피치 및 요 레이트, 휠의 엔코더 데이터 및 GPS 데이터는 상태 추정치를 향상시킵니다.

약간의 수학을 사용하여 두 인코더를 사용하여 선형 및 각 속도뿐만 아니라 로봇에 대한 x, y 및 표제 정보를 생성 할 수 있습니다. 엔코더는 매우 정확하지만 실외에서 미끄러질 수 있습니다.

여기에는 두 가지 별도의 데이터 세트가 있으며, 융합하기가 어렵습니다.

1. x, x-vel, x-accel, y, y-vel, y-accel의 추정치
2. 롤, 피치, 요 및 롤, 피치 및 요의 추정치

이 두 세트 사이에 크로스 오버가 있지만 그것들을 어떻게 조합해야하는지 추론하는 데 어려움을 겪고 있습니다. 예를 들어, 로봇이 일정한 속도로 진행하는 경우 x-vel 및 y-vel에 의해 결정되는 로봇의 방향은 요와 동일합니다. 로봇이 정지 상태 인 경우 x 및 y 속도로 요를 정확하게 결정할 수 없습니다. 또한, 각속도로 변환 된 엔코더가 제공하는 데이터는 요 레이트를 업데이트 할 수 있습니다. 그러나 요 레이트를 업데이트하면 어떻게 더 나은 위치 추정치를 제공 할 수 있습니까?

12 개의 숫자를 모두 같은 필터에 넣는 것이 합리적입니까, 아니면 일반적으로 분리되어 있습니까? 이러한 유형의 문제를 처리하는 잘 개발 된 방법이 있습니까?

두가지.

1. 매핑을 수행하려면 본격적인 SLAM (Simultaneous Localization and Mapping) 알고리즘이 필요합니다. 참조 : SLAM (Simultaneous Localization and Mapping)을 1 부 근본적인 알고리즘 . SLAM에서 로봇 상태 추정은 문제의 절반에 지나지 않습니다. 그렇게하는 방법은 여기서 대답 할 수있는 것보다 더 큰 질문입니다.

2. 현지화 (로봇 상태 추정)와 관련하여 이것은 Kalman Filter의 작업이 아닙니다. 에서 전환 에 대한 X ( t + 1 $x(t)=[x,y,\dot{x},\dot{y},\theta,\dot{\theta}]$$x(t+1)$각가속도와 속도로 인해 선형 함수가 아닙니다. 따라서이 작업을 위해 비선형 추정기를 고려해야합니다. 예, 표준 방법이 있습니다. 예, 문헌으로 제공됩니다. 예, 일반적으로 모든 입력은 동일한 필터에 배치됩니다. 로봇의 위치, 속도, 방향 및 각속도가 출력으로 사용됩니다. 그리고 네, 여기에 공통 주제에 대한 간단한 소개가 있습니다. 주요 테이크 아웃은

   1. 귀하의 주에 자이로 및 IMU 편견을 포함 시키십시오.
   2. 확장 칼만 필터 (EKF)는 일반적으로이 문제에 사용됩니다
   3. 구현은 처음부터 파생 될 수 있으며 일반적으로 "조회"할 필요가 없습니다.
   4. 구현은 대부분의 현지화 및 SLAM 문제에 대해 존재하므로 필요한 것보다 많은 작업을 수행하지 마십시오. 로봇 운영 체제 ROS 참조

이제 시스템의 맥락에서 EKF를 설명하겠습니다. 우리는 IMU + Gyro, GPS 및 odometry를 가지고 있습니다. 문제의 로봇은 언급 된대로 차동 드라이브입니다. 필터링 작업이 로봇의 현재 자세 추정을 수행하는 X t , 제어 입력 U을 t를 , 각 센서로부터 측정 Z t , 및 다음 시간 단계에서의${\hat{x}_t}$$u_t$$z_t$. IMU 측정을It라고하고 GPS를Gt로하고 odometry를O$\hat{x}_{t+1}$$I_t$$G_t$$O_t$ .

로봇 자세를 로 추정하는 데 관심이 있다고 가정 합니다. IMU 및 Gyros의 문제점은 드리프트입니다. EKF에서 고려해야하는 가속에는 비정규적인 편향이 있습니다. 이는 일반적으로 바이어스를 추정 상태로 설정하여 수행됩니다. 이를 통해 각 시간 단계에서 바이어스를 직접 추정 할 수 있습니다. x t = x , y , ˙ x , ˙ y , θ , ˙ θ , $x_t={x,y,\dot{x},\dot{y},\theta,\dot{\theta}}$$x_t={x,y,\dot{x},\dot{y},\theta,\dot{\theta},b}$바이어스 벡터 $b$ .

나는 가정한다 :

1. = 트레드가 약간의 시간 단위로 이동 한 거리를 나타내는 두 개의 거리 측정$O_t$
2. = 3 개의 방향 측정 α , β , θ 및 3 개의 가속도 측정$I_t$${\alpha, \beta, \theta}$ .${\ddot{x},\ddot{y},\ddot{z}}$
3. =에서 로봇의 위치글로벌프레임 G X t , G 용 의 Y t .$G_t$$^Gx_t,^Gy_t$

일반적으로, 제어 입력의 결과 (각 트레드마다 원하는 속도)는 출력 (로봇의 자세 변화)에 매핑하기가 어렵습니다. 대신에 , 그것은 일반적이다 (참조 Thrun , 오도 질문$u$ 제어의 "결과"로 오도를 사용). 이 가정은 마찰이 거의없는 표면에 있지 않을 때 효과적입니다. 앞으로 살펴 보 겠지만 IMU와 GPS는 미끄러짐을 바로 잡는 데 도움이 될 수 있습니다.

첫 번째 작업은 현재 상태에서 다음 상태를 예측하므로 . 차동 드라이브 로봇의 경우,이 예측은 문헌에서 직접 얻을 수 있거나 ( 휠식 모바일 로봇의 운동학 또는 현대 로봇 공학 교과서의 더 간결한 처리 참조) 다음과 같이 처음부터 얻을 수 있습니다.$\hat{x}_{t+1} = f(\hat{x}_t,u_t)$ .

그래서 우리는 지금 예측할 수 X를. 이것이 전파 또는 예측 단계입니다. 간단히 전파하여 로봇을 조작할수있습니다. 값 경우 O t이 완전히 정확, 당신은 추정하지 않아도됩니다 X 정확하게 실제 상태와 동일하지 않습니다. 이것은 실제로 일어나지 않습니다.$\hat{x}_{t+1} = f(\hat{x}_t, O_t)$$O_t$$\hat{x}$

이것은 이전 추정치에서 예측 된 값만 제공하며 추정치의 정확도가 시간에 따라 어떻게 저하되는지는 알려주지 않습니다. 따라서 불확실성을 전파하려면 EKF 방정식 (가우시안 잡음 가정 하에서 닫힌 형태로 불확실성을 전파), 입자 필터 (샘플링 기반 접근 방식 사용) *, UKF (점 단위 사용)를 사용해야합니다. 불확실성의 근사) 또는 다른 많은 변형 중 하나.

EKF의 경우 다음과 같이 진행합니다. 로봇 상태의 공분산 행렬 이라고하자 . Taylor 시스템 확장을 사용하여 함수 f 를 선형화하여 선형 시스템을 얻습니다. 칼만 필터를 사용하면 선형 시스템을 쉽게 해결할 수 있습니다. 시간 t 에서의 추정치의 공분산 이 P t이고 , 측량법에서 잡음의 추정 된 공분산은 행렬 U t (보통 대각선 2 × 2) 로 주어진다고 가정$P_t$$f$$t$$P_t$$U_t$$2\times2$ 와 같은 행렬 . 함수 f 의 경우 Jacobian 을 얻는다$.1\times I_{2\times 2}$$f$ 와Fu=∂$F_x=\frac{\partial f}{\partial x}$ 다음과 같은 불확실성을 전파$F_u=\frac{\partial f}{\partial u}$

이제 추정치와 불확실성을 전파 할 수 있습니다. 불확실성은 시간이 지남에 따라 단조 증가합니다. 이것은 예상됩니다. 이 문제를 해결하려면 일반적으로 수행되는 작업은 및 G $I_t$$G_t$ 를 사용하여 예측 상태를 업데이트해야합니다. 센서가 로봇 상태의 간접 측정을 제공하기 때문에이를 필터링 프로세스의 측정 단계라고합니다.

먼저 각 센서를 사용하여 로봇 상태의 일부 를 GPS, IMU에 대한 일부 함수 및 h i ( ) 로 추정 합니다. 폼 잔존 하거나 혁신 예측 측정 값의 차이이다. 그런 다음 모든 센서에 대해 공분산 행렬 R 의 형태로 각 센서 추정값의 정확도를 추정하십시오 ( 이 경우 R g , R i ). 마지막으로 h 의 Jacobians를 찾아서 다음과 같이 상태 추정값을 업데이트하십시오.$h_g()$$h_i()$$R$$R_g$$R_i$$h$

각 센서 상태를 추정하여 (Z) 들 ( 이후 위키의 항목 )$s$$z_s$

S S = H S * P의 t + 1 * H T S + R S K = P t + 1 * H T S S - 1 의 X t + 1 = X t + 1 - K *

$z_g=h_g()$$H_g$$R_g$

$z_i=h_i()$$p_i$$R_i$$p_i$. 편견에 대한 업데이트를 통합하는 것이 더 어려우며 전문 지식이 부족합니다. 그러나 평면 모션에 관심이 있으므로 문제를 단순화 할 수 있습니다. 이를 위해서는 문헌을 봐야합니다.

내 머리 속의 일부 참고 문헌 :

1. EKF 기반 Visual-Inertial Odometry의 정확도 향상

2. 멀티 로봇 협동 지역화를위한 관찰 기반의 일관된 EKF 추정기

3. 알 수없는 결함 바이어스를 가진 INS-GPS 느슨하게 결합 된 시스템을위한 적응 형 2 단계 EKF

이 필드는 google (scholar)이 작동중인 구현을 찾을 수있을 정도로 성숙합니다. 이 분야에서 많은 일을하려는 경우, 견고한 교과서를 선택하는 것이 좋습니다. Google Car의 S. Thrun 의 Probablistic Robotics 와 같은 것이있을 것 입니다. (저는 늦은 밤 구현에 유용한 참고 자료를 찾았습니다).

* ROS ( Robot Operating System ) 에는 여러 가지 PF 기반 추정기가 있습니다 . 그러나 이들은 실 내용으로 최적화되었습니다. 입자 필터는지도 기반의 현지화 발생할 수있는 멀티 모달 PDF 파일 처리 (나는 근처에 오전 이 문이나 그 문). 나는 대부분의 실외 구현 (특히 GPS를 적어도 간헐적으로 사용할 수있는 구현)이 확장 칼만 필터 (EKF)를 사용한다고 생각 합니다. 차동 드라이브가있는 실외 그라운드 로버에 확장 칼만 필터를 성공적으로 사용했습니다.

가장 일반적인 경우 문제를 크게 단순화 할 수 있습니다.

• 많은 "상업용 등급"IMus (예 : Xsens)에는 잡음이 많은 가속도계가 있습니다. 속도를 얻기 위해 그것들을 융합하지 않아도 odometry는 이미 몇 배나 더 좋습니다. IMU가 제공 할 수있는 유일하게 사용 가능한 데이터는 피치와 롤이며 어느 정도 헤딩입니다 (다음 지점 참조).
• IMU로부터 향하는 것은 그렇게 믿을만한 것이 아닙니다. 그것은 magetometers를 사용하며 건물 벽에서 찾을 수있는 것과 같은 강자성 덩어리 근처에 거대한 드리프트 (우리의 경우 2m 이상에서 최대 25도)를 보여줍니다. 이 문제를 해결하기 위해 IMU 제목을 사용하지만 제목 바이어스를 추정했습니다.
• 실외에있는 경우 10도 경사로 10m를 여행해도 평평한 지형에서 10m를 여행하는 것과 X와 Y의 변화가 동일하지 않습니다. 이것은 일반적으로 Z를 추정하여 설명되지만 다르게 추정 할 수 있다고 생각합니다.
• GPS는 또한 일반적으로 다중 경로 환경에서 거짓말하는 년입니다. 또한 저급 (및 일부 조건에서도 고급) GPS는 매우 잘못된 표준 편차를보고하는 경향이 있습니다. 간단한 카이-제곱 테스트를 사용하여 특정 GPS 측정을 통합해야하는지 (즉, 현재 필터 추정치와 특정 지점까지 일치하는지 확인) 적절한 결과를 얻었습니다.

우리를위한 "일반적인"솔루션은 자력 측정을 위해 odometry + IMU를 사용한 다음 GPS를 사용하여 X, Y, Z 및 헤딩 바이어스를 수정하는 것입니다.

다음은 우리가 광범위하게 사용한 EKF 구현입니다. IMU의 방향을 추정해야하는 경우 (즉, 내장 필터가없는 경우) UKF 및 EKF의 두 필터를 사용할 수도 있습니다 .