Bộ lọc Kalman mở rộng với Laser Scan + Bản đồ đã biết

Tôi hiện đang làm việc trong một dự án cho trường học, nơi tôi cần triển khai Bộ lọc Kalman mở rộng cho một robot điểm bằng máy quét laser. Robot có thể xoay với bán kính quay 0 độ và lái về phía trước. Tất cả các chuyển động là piecewise tuyến tính (ổ đĩa, xoay, ổ đĩa).

Trình giả lập chúng tôi đang sử dụng không hỗ trợ tăng tốc, tất cả chuyển động là tức thời.

Chúng tôi cũng có một bản đồ đã biết (hình ảnh png) mà chúng tôi cần bản địa hóa. Chúng tôi có thể chiếu tia trong hình ảnh để mô phỏng quét laser.

Đối tác của tôi và tôi hơi bối rối về các mô hình chuyển động và cảm biến mà chúng tôi sẽ cần sử dụng.

Cho đến nay chúng ta đang mô hình hóa trạng thái như một vectơ .$(x,y,\theta)$

Chúng tôi đang sử dụng các phương trình cập nhật như sau

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;
}

Chúng tôi nghĩ rằng chúng tôi đã làm mọi thứ cho đến khi chúng tôi nhận thấy rằng chúng tôi quên khởi tạo Pvà nó bằng không, có nghĩa là không có sự điều chỉnh nào xảy ra. Rõ ràng sự lan truyền của chúng tôi rất chính xác vì chúng tôi chưa đưa tiếng ồn vào hệ thống.

Đối với mô hình chuyển động, chúng tôi đang sử dụng ma trận sau cho F:

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

Là Jacobian của công thức cập nhật của chúng tôi. Điều này có đúng không?

Đối với mô hình cảm biến, chúng tôi đang tính xấp xỉ Jacobian (H) bằng cách lấy sự khác biệt hữu hạn của các vị trí robot , và và dò tia trong bản đồ. Chúng tôi đã nói chuyện với TA, người nói rằng việc này sẽ hiệu quả nhưng tôi vẫn không chắc là nó sẽ hoạt động. Prof của chúng tôi đi vắng nên chúng tôi không thể hỏi anh ấy một cách đáng tiếc. Chúng tôi đang sử dụng 3 phép đo laser cho mỗi bước hiệu chỉnh nên H là 3x3.$x$$y$$\theta$

Vấn đề khác khi có cách khởi tạo P. Chúng tôi đã thử 1.10.100 và tất cả họ đều đặt robot bên ngoài bản đồ ở (-90, -70) khi bản đồ chỉ có 50x50.

Mã cho dự án của chúng tôi có thể được tìm thấy ở đây: https://github.com/en4bz/kalman/blob/master/src/kalman.cpp

Bất cứ lời khuyên nào cũng đươc đánh giá cao.

BIÊN TẬP:

Tại thời điểm này, tôi đã nhận được bộ lọc để ổn định với tiếng ồn chuyển động cơ bản nhưng không có chuyển động thực tế. Ngay khi robot bắt đầu di chuyển bộ lọc sẽ chuyển hướng khá nhanh và thoát khỏi bản đồ.

• Sử dụng EKF để ​​bản địa hóa dựa trên quét laser và bản đồ đã biết là một ý tưởng tồi và sẽ không hoạt động vì một trong những giả định chính của EKF là không hợp lệ: Mô hình đo lường không khác biệt.

Tôi sẽ đề nghị xem xét nội địa hóa Monte Carlo (Bộ lọc hạt). Điều này sẽ không chỉ giải quyết vấn đề với mô hình đo lường của bạn, mà còn cho phép nội địa hóa toàn cầu (tư thế ban đầu trong bản đồ hoàn toàn không xác định).

Chỉnh sửa: Tôi xin lỗi tôi đã quên đề cập đến một điểm quan trọng khác:

• Nếu bạn muốn áp dụng EKF, mô hình đo lường cũng như mô hình chuyển động của bạn chỉ có thể bao gồm nhiễu Gaussian . Điều này có nghĩa là bạn cần có thể viết ra mô hình đo lường của mình dưới dạng . Đây là một hạn chế nghiêm trọng, vì nó không cho phép một mô hình phức tạp hơn một chút như Beam_range_finder_model trong Xác suất Robotics, cũng xem xét các vật thể động trước robot, các phép đo (tối đa) không hợp lệ và đọc hoàn toàn ngẫu nhiên. Bạn sẽ bị mắc kẹt với các tia đúc cho phần và thêm nhiễu Gaussian.$z_t = h(x_t) + N(0, Q_t)$$h(x_t)$

Trước hết, bạn không đề cập gì về loại nội địa hóa mà bạn đang sử dụng. Là nó với các tương ứng được biết hoặc chưa biết?

Bây giờ để có được Jacobian trong Matlab, bạn có thể làm như sau

>> 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)

Kết quả

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

Bộ lọc Kalman mở rộng yêu cầu hệ thống phải tuyến tính và nhiễu phải là Gaussian. Hệ thống ở đây có nghĩa là các mô hình chuyển động và quan sát phải tuyến tính. Các cảm biến laser cho phạm vi và góc tới một mốc từ khung của robot, do đó mô hình đo không phải là tuyến tính. Về P, nếu bạn cho rằng nó lớn thì công cụ ước tính EKF của bạn sẽ kém ngay từ đầu và nó phụ thuộc vào các phép đo vì vectơ trạng thái trước đó không có sẵn. Tuy nhiên, một khi robot của bạn bắt đầu di chuyển và cảm nhận, EKF sẽ trở nên tốt hơn vì nó sử dụng các mô hình chuyển động và đo lường để ước tính tư thế của robot. Nếu robot của bạn không cảm nhận được bất kỳ cột mốc nào, sự không chắc chắn sẽ tăng mạnh. Ngoài ra, bạn cần cẩn thận về góc độ. Trong C ++,cos and sin, góc nên được tính bằng radian. Không có gì trong mã của bạn về vấn đề này. Nếu bạn giả sử độ nhiễu của góc theo độ trong khi tính toán của bạn theo radian, thì bạn có thể nhận được kết quả kỳ lạ vì một độ là nhiễu trong khi các phép tính theo radian được coi là rất lớn.