레이저 스캔 + 알려진 맵이있는 확장 칼만 필터

10

저는 현재 레이저 스캐너로 포인트 로봇을 위해 확장 된 칼만 필터를 구현해야하는 학교 프로젝트를 진행하고 있습니다. 로봇은 0도 회전 반경으로 회전하고 앞으로 주행 할 수 있습니다. 모든 동작은 부분적으로 선형입니다 (구동, 회전, 구동).

우리가 사용하는 시뮬레이터는 가속을 지원하지 않으며 모든 동작이 즉각적입니다.

또한 현지화해야하는 알려진 맵 (png 이미지)이 있습니다. 레이저 스캔을 시뮬레이션하기 위해 이미지에서 광선 추적을 할 수 있습니다.

내 파트너와 나는 우리가 사용해야 할 모션 및 센서 모델에 대해 약간 혼란스러워합니다.

지금까지 상태를 벡터로 모델링하고 있습니다 $(x,y,\theta)$ .

다음과 같이 업데이트 방정식을 사용하고 있습니다

void kalman::predict(const nav_msgs::Odometry msg){
    this->X[0] += linear * dt * cos( X[2] ); //x
    this->X[1] += linear * dt * sin( X[2] ); //y
    this->X[2] += angular * dt; //theta

    this->F(0,2) = -linear * dt * sin( X[2] ); //t+1 ?
    this->F(1,2) =  linear * dt * cos( X[2] ); //t+1 ?

    P = F * P * F.t() + Q;

    this->linear = msg.twist.twist.linear.x;
    this->angular = msg.twist.twist.angular.z;
    return;
}

우리는 초기화를 잊어 버렸고 P그것이 0이라는 것을 알 때까지 모든 것이 효과가 있다고 생각 했습니다. 아직 시스템에 노이즈가 발생하지 않았기 때문에 전파가 매우 정확했습니다.

모션 모델의 경우 F에 다음 행렬을 사용합니다.

$F = \begin{bmatrix}1 & 0 & -v*\Delta t*sin(\theta) \\ 0 & 1 & v*\Delta t*cos(\theta) \\ 0 & 0 & 1 \end{bmatrix}$

업데이트 공식의 Jacobian으로. 이 올바른지?

센서 모델의 경우 로봇의 유한 차이를 취하여 야 코비안 (H)을 근사화합니다. $x$ , $y$ 과 $\theta$ 지도에서 위치와 광선 추적. 우리는 이것이 효과가 있다고 말한 TA와 이야기했지만 여전히 그럴 것이라고 확신하지 않습니다. 우리의 교수진이 떨어져서 불행히도 물어볼 수 없습니다. 보정 단계 당 3 개의 레이저 측정을 사용하므로 H는 3x3입니다.

P를 초기화하는 방법이있는 다른 문제. 우리는 1,10,100을 시도하고 맵이 50x50 일 때 로봇을 (-90, -70)에 배치합니다.

우리 프로젝트의 코드는 여기에서 찾을 수 있습니다 : https://github.com/en4bz/kalman/blob/master/src/kalman.cpp

모든 조언을 부탁드립니다.

편집하다:

이 시점에서 나는 기본적인 움직임 노이즈로 안정화시키기 위해 필터를 얻었지만 실제 움직임은 없습니다. 로봇이 움직이기 시작하자마자 필터가 아주 빠르게 분기되어 맵에서 나옵니다.

— en4bz
소스

3

레이저 스캔 및 알려진 맵을 기반으로 한 현지화에 EKF를 사용하는 것은 나쁜 생각이며 EKF의 주요 가정 중 하나가 유효하지 않기 때문에 작동하지 않습니다. 측정 모델을 차별화 할 수 없습니다.

Monte Carlo Localization (입자 필터)을 살펴볼 것을 제안합니다. 이렇게하면 측정 모델의 문제를 해결할뿐만 아니라 전역 지역화 (지도 내에서 초기 포즈를 완전히 알 수 없음) 할 수 있습니다.

편집 : 다른 중요한 점을 언급하는 것을 잊어 버려서 죄송합니다.

EKF를 적용하려면 모션 모델 뿐만 아니라 측정 모델에 가우스 노이즈 만 포함될 수 있습니다 . 즉, 측정 모델을 다음과 같이 기록 할 수 있어야합니다. $z_t = h(x_t) + N(0, Q_t)$ . Probabilistic Robotics의 beam_range_finder_model과 같은 약간 더 복잡한 모델을 허용하지 않기 때문에 이는 심각한 한계입니다. 로봇 앞의 동적 객체, 유효하지 않은 (최대) 측정 및 완전히 임의의 판독 값도 고려합니다. 당신은에 대한 주조 광선으로 붙어있을 것입니다 $h(x_t)$ 가우스 노이즈를 추가하고 추가합니다.

— 세브
소스

^^ "레이저 스캔과 알려진지도를 기반으로 한 현지화를 위해 EKF를 사용하는 것은 나쁜 생각입니다." 참조를 제공하십시오. EKF는 가장 성공적인 추정량이며 많은 논문에서 현지화 문제에 EKF를 사용할 것을 제안합니다. 사실, 나는 당신이 말하는 것에 놀랐습니다. EKF의 주요 가정은 모션 및 관측 모델이 선형이며 잡음은 가우시안입니다. "측정 모델이 차별화되지 않는다"는 것이 무슨 의미인지 잘 모르겠습니다.

— CroCo

원래 포스터는 레이트 레이싱을 언급했는데, 이는 확률 론적 로봇 공학에서 "거리 측정기의 빔 모델"과 유사한 측정 모델을 사용하려고 함을 의미합니다. 전시 점프이 측정 모델 (레이저 빔이 거의 장애물을 타격하고없는 상상 : 그것은 단지 미분없는 점프 작은 회전을합니다.)

— sebsch

0

우선, 어떤 종류의 현지화를 사용하고 있는지 언급하지 않았습니다. 알려 지거나 알려지지 않은 서신이 있습니까?

이제 Matlab에서 Jacobian을 얻으려면 다음을 수행하십시오.

>> syms x y a Vtran Vrotat t
>> f1 = x + Vtran*t*cos(a);
>> f2 = y + Vtran*t*sin(a);
>> f3 = a + Vrotat*t;
>> input  = [x y a];
>> output = [f1 f2 f3];
>> F = jacobian(output, input)

결과

F =
[ 1, 0, -Vtran*t*sin(a)]
[ 0, 1,  Vtran*t*cos(a)]
[ 0, 0,               1]

확장 칼만 필터는 시스템이 선형이고 잡음이 가우시안이어야합니다. 여기서 시스템은 동작 및 관측 모델이 선형이어야 함을 의미합니다. 레이저 센서는 로봇 프레임에서 랜드 마크를 향한 범위와 각도를 제공하므로 측정 모델이 선형이 아닙니다. 에 대해 P,이 값이 크면 처음에 EKF 추정기가 나 빠지고 이전 상태 벡터를 사용할 수 없기 때문에 측정 값에 의존합니다. 그러나 로봇이 움직이고 감지하기 시작하면 EKF는 모션 및 측정 모델을 사용하여 로봇의 포즈를 추정하기 때문에 더 나아질 것입니다. 로봇이 랜드 마크를 감지하지 못하면 불확실성이 급격히 증가합니다. 또한 각도에주의해야합니다. C ++에서cos and sin각도는 라디안이어야합니다. 이 문제에 대한 코드에는 아무것도 없습니다. 라디안 단위로 계산하는 동안 각도의 노이즈를도 단위로 가정하면 라디안 단위의 계산이 큰 것으로 간주되는 동안 1 도는 노이즈로 이상한 결과를 얻을 수 있습니다.

— 크로 코
소스