Làm thế nào để hợp nhất dữ liệu tuyến tính và góc từ các cảm biến

Nhóm của tôi và tôi đang thiết lập một robot ngoài trời có bộ mã hóa, IMU cấp thương mại và cảm biến GPS . Robot có một ổ đĩa cơ bản, vì vậy các bộ mã hóa cung cấp đủ bọ ve từ bánh xe bên trái và bên phải. IMU cho phép gia tốc cuộn, cao độ, ngáp và gia tốc tuyến tính theo x, y và z. Sau này chúng ta có thể thêm các IMU khác, sẽ cung cấp dự phòng, nhưng cũng có thể cung cấp tỷ lệ góc của cuộn, cao độ và ngáp. GPS xuất bản các tọa độ x, y và z toàn cầu.

Biết vị trí và tiêu đề xy của robot sẽ hữu ích cho robot để bản địa hóa và vạch ra môi trường của nó để điều hướng. Vận tốc của robot cũng có thể hữu ích để đưa ra quyết định di chuyển trơn tru. Đó là một robot trên mặt đất, vì vậy chúng tôi không quan tâm quá nhiều đến trục z. Robot cũng có cảm biến nắp và máy ảnh - vì vậy cuộn và cao độ sẽ hữu ích cho việc chuyển đổi dữ liệu nắp và máy ảnh để định hướng tốt hơn.

Tôi đang cố gắng tìm ra cách kết hợp tất cả các số này lại với nhau theo cách tận dụng tối ưu độ chính xác của tất cả các cảm biến. Ngay bây giờ chúng tôi đang sử dụng bộ lọc Kalman để tạo ước tính [x, x-vel, x-accel, y, y-vel, y-accel]với ma trận chuyển tiếp đơn giản:

[[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]]

Bộ lọc ước tính trạng thái độc quyền dựa trên các gia tốc do IMU cung cấp. (IMU không phải là chất lượng tốt nhất; trong khoảng 30 giây, nó sẽ cho thấy robot (ở phần còn lại) trôi cách vị trí ban đầu 20 mét.) Tôi muốn biết cách sử dụng cuộn, cao độ và ngáp từ IMU, và có khả năng cuộn, cường độ và tốc độ ngáp, dữ liệu mã hóa từ các bánh xe và dữ liệu GPS để cải thiện ước tính trạng thái.

Sử dụng một chút toán học, chúng ta có thể sử dụng hai bộ mã hóa để tạo x, y và tiêu đề thông tin về robot, cũng như vận tốc tuyến tính và góc. Các bộ mã hóa rất chính xác, nhưng chúng có thể dễ bị trượt trên một lĩnh vực ngoài trời.

Dường như với tôi có hai bộ dữ liệu riêng biệt ở đây, rất khó hợp nhất:

1. Ước tính của x, x-vel, x-accel, y, y-vel, y-accel
2. Ước tính của cuộn, cao độ, ngáp và tỷ lệ của cuộn, cao độ và ngáp

Mặc dù có sự giao thoa giữa hai bộ này, tôi gặp khó khăn khi suy luận về cách kết hợp chúng lại với nhau. Ví dụ, nếu robot đi với tốc độ không đổi, hướng của robot, được xác định bởi x-vel và y-vel của nó, sẽ giống như ngáp của nó. Mặc dù, nếu robot ở trạng thái nghỉ, ngáp không thể được xác định chính xác bởi vận tốc x và y. Ngoài ra, dữ liệu được cung cấp bởi các bộ mã hóa, được dịch sang vận tốc góc, có thể là một bản cập nhật cho tốc độ ngáp ... nhưng làm thế nào một bản cập nhật cho tỷ lệ ngáp cuối cùng cung cấp ước tính vị trí tốt hơn?

Liệu có ý nghĩa gì khi đặt tất cả 12 số vào cùng một bộ lọc hoặc chúng thường được giữ riêng biệt? Đã có một cách phát triển tốt để đối phó với loại vấn đề này?

Hai điều.

1. Nếu bạn có kế hoạch thực hiện ánh xạ, bạn cần một Thuật toán Bản đồ hóa và Bản đồ hóa đồng thời (SLAM) chính thức. Xem: Bản đồ hóa và Bản đồ hóa đồng thời (SLAM): Phần I Các thuật toán cần thiết . Trong SLAM, ước tính trạng thái robot chỉ là một nửa vấn đề. Làm thế nào để làm điều đó là một câu hỏi lớn hơn có thể được trả lời ở đây.

2. Về nội địa hóa (ước tính trạng thái của robot), đây không phải là công việc cho Bộ lọc Kalman. Sự chuyển đổi từ sang x ( t + 1 $x(t)=[x,y,\dot{x},\dot{y},\theta,\dot{\theta}]$$x(t+1)$không phải là một hàm tuyến tính do gia tốc và vận tốc góc. Do đó, bạn cần xem xét các công cụ ước tính phi tuyến tính cho nhiệm vụ này. Vâng, có những cách tiêu chuẩn để làm điều này. Vâng, chúng có sẵn trong văn học. Có, thông thường tất cả các đầu vào được đưa vào cùng một bộ lọc. Vị trí, vận tốc, định hướng và tốc độ góc của robot được sử dụng làm đầu ra. Và có, tôi sẽ giới thiệu ngắn gọn về các chủ đề phổ biến của họ ở đây. Các phương tiện chính là

   1. bao gồm thiên vị Gyro và IMU trong tiểu bang của bạn hoặc ước tính của bạn sẽ phân kỳ
   2. Một mở rộng Kalman Filter (EKF) thường được sử dụng cho vấn đề này
   3. Việc triển khai có thể bắt nguồn từ đầu và thường không cần phải "tra cứu".
   4. Triển khai tồn tại cho hầu hết các vấn đề nội địa hóa và SLAM, vì vậy đừng làm nhiều việc hơn bạn phải làm. Xem: Hệ điều hành Robot ROS

Bây giờ, để giải thích EKF trong bối cảnh hệ thống của bạn. Chúng tôi có IMU + Gyro, GPS và đo hình. Các robot trong câu hỏi là một ổ đĩa vi sai như đã đề cập. Nhiệm vụ lọc là để có những ước tính tư thế hiện tại của robot x t , đầu vào kiểm soát u t , và các số đo từ mỗi cảm biến, z t , và sản xuất ước tính ở bước tiếp theo thời gian x t + 1 . Chúng tôi sẽ gọi các phép đo IMU là I t , GPS là G t và đo hình, O t .${\hat{x}_t}$$u_t$$z_t$$\hat{x}_{t+1}$$I_t$$G_t$$O_t$

Tôi cho rằng chúng ta đang quan tâm đến việc đánh giá các tư thế robot như . Vấn đề với IMU và Gyros là trôi dạt. Có sự thiên vị không cố định trong các gia tốc mà bạn phải tính đến trong EKF. Điều này được thực hiện (thường) bằng cách đưa sự thiên vị vào trạng thái ước tính. Điều này cho phép bạn ước tính trực tiếp độ lệch ở mỗi bước thời gian. 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}$, cho một vectơ sai lệch .$b$

Tôi đang giả sử:

1. = hai phép đo khoảng cách đại diện cho quãng đường mà các bước đã đi trong một khoảng thời gian nhỏ$O_t$
2. = ba phép đo định hướng alpha , β , θ và số đo ba accelleration ¨ x , ¨ y , ¨ z .$I_t$${\alpha, \beta, \theta}$${\ddot{x},\ddot{y},\ddot{z}}$
3. = vị trí của robot trongkhungtoàn cầu, G x t , G y t .$G_t$$^Gx_t,^Gy_t$

Thông thường, kết quả của các đầu vào điều khiển (tốc độ mong muốn cho từng rãnh) rất khó ánh xạ tới các đầu ra (sự thay đổi trong tư thế của robot). Thay cho , người ta thường sử dụng (xem Thrun , Câu hỏi trắc nghiệm ) để sử dụng phép đo hình học làm "kết quả" của điều khiển. Giả định này hoạt động tốt khi bạn không ở trên bề mặt gần như không ma sát. IMU và GPS có thể giúp khắc phục tình trạng trượt, như chúng ta sẽ thấy.$u$

Vì vậy, nhiệm vụ đầu tiên là để dự đoán trạng thái tiếp theo từ trạng thái hiện . Trong trường hợp robot điều khiển vi sai, dự đoán này có thể được lấy trực tiếp từ tài liệu (xem Trên Động học của Robot di động có bánh xe hoặc cách xử lý ngắn gọn hơn trong bất kỳ sách giáo khoa về robot hiện đại nào), hoặc xuất phát từ đầu như được nêu ở đây: Câu hỏi hình học .$\hat{x}_{t+1} = f(\hat{x}_t,u_t)$

Vì vậy, bây giờ chúng ta có thể dự đoán x t + 1 = f ( x t , O t ) . Đây là bước tuyên truyền hoặc dự đoán. Bạn có thể vận hành một robot bằng cách đơn giản là tuyên truyền. Nếu các giá trị O t là hoàn toàn chính xác, bạn sẽ không bao giờ có một ước tính x mà không chính xác bằng trạng thái thực sự của bạn. Điều này không bao giờ xảy ra trong thực tế.$\hat{x}_{t+1} = f(\hat{x}_t, O_t)$$O_t$$\hat{x}$

Điều này chỉ đưa ra một giá trị dự đoán từ ước tính trước đó và không cho chúng tôi biết độ chính xác của ước tính giảm theo thời gian như thế nào. Vì vậy, để truyền bá độ không đảm bảo, bạn phải sử dụng các phương trình EKF (truyền các độ không đảm bảo ở dạng đóng theo giả định nhiễu Gaussian), bộ lọc hạt (sử dụng phương pháp lấy mẫu) *, UKF (sử dụng phương pháp lấy điểm xấp xỉ độ không đảm bảo), hoặc một trong nhiều biến thể khác.

Trong trường hợp của EKF, chúng tôi tiến hành như sau. Gọi là ma trận hiệp phương sai của trạng thái robot. Chúng tôi tuyến tính hóa hàm f bằng cách sử dụng mở rộng chuỗi Taylor để có được một hệ thống tuyến tính. Một hệ thống tuyến tính có thể được giải quyết dễ dàng bằng Bộ lọc Kalman. Giả sử hiệp phương sai của ước lượng tại thời điểm t là P t và hiệp phương sai giả định của nhiễu trong phép đo được đưa ra dưới dạng ma trận U t (thường là ma trận 2 × 2 chéo , như .1 × I 2 × 2 ). Trong trường hợp của hàm f , chúng ta có được Jacobian$P_t$$f$$t$$P_t$$U_t$$2\times2$$.1\times I_{2\times 2}$$f$ vàFu=∂$F_x=\frac{\partial f}{\partial x}$ , sau đó tuyên truyền sự không chắc chắn là,$F_u=\frac{\partial f}{\partial u}$

Bây giờ chúng ta có thể tuyên truyền ước tính và sự không chắc chắn. Lưu ý sự không chắc chắn sẽ tăng đơn điệu theo thời gian. Điều này được mong đợi. Để khắc phục điều này, những gì thường được thực hiện, là sử dụng và G t để cập nhật trạng thái dự đoán. Đây được gọi là bước đo lường của quá trình lọc, vì các cảm biến cung cấp một phép đo gián tiếp về trạng thái của robot.$I_t$$G_t$

$h_g()$$h_i()$$R$$R_g$$R_i$$h$

$s$$z_s$

In the case of GPS, the measurement $z_g=h_g()$ it is probably just a transformation from latitude and longitude to the local frame of the robot, so the Jacobian $H_g$ will be nearly Identity. $R_g$ is reported directly by the GPS unit in most cases.

In the case of the IMU+Gyro, the function $z_i=h_i()$ is an integration of accelerations, and an additive bias term. One way to handle the IMU is to numerically integrate the accelerations to find a position and velocity estimate at the desired time. If your IMU has a small additive noise term $p_i$ for each acceleration estimate, then you must integrate this noise to find the accuracy of the position estimate. Then the covariance $R_i$ is the integration of all the small additive noise terms, $p_i$. Incorporating the update for the bias is more difficult, and out of my expertise. However, since you are interested in planar motion, you can probably simplify the problem. You'll have to look in literature for this.

Some off-the-top-of-my-head references:

1. Improving the Accuracy of EKF-Based Visual-Inertial Odometry

2. Observability-based Consistent EKF Estimators for Multi-robot Cooperative Localization

3. Adaptive two-stage EKF for INS-GPS loosely coupled system with unknown fault bias

This field is mature enough that google (scholar) could probably find you a working implementation. If you are going to be doing a lot of work in this area, I recommend you pick up a solid textbook. Maybe something like Probablistic Robotics by S. Thrun of Google Car fame. (I've found it a useful reference for those late-night implementations).

*There are several PF-based estimators available in the Robot Operating System (ROS). However, these have been optimized for indoor use. Particle filters deal with the multi-modal PDFs which can result from map-based localization (am I near this door or that door). I believe most outdoor implementations (especially those that can use GPS, at least intermittently) use the Extended Kalman Filter (EKF). I've successfully used the Extended Kalman Filter for an outdoor, ground rover with differential drive.

You can greatly simplify the problem in most common cases:

• A lot of "commercial grade" IMus (e.g. Xsens) have very noisy accelerometers. Don't even bother fusing them to get speed, the odometry is already order of magnitudes better. The only usable data the IMU is going to provide is the pitch and roll, and to some extent the heading (see next point)
• heading from IMUs is not that trustworthy. It uses magetometers, and will show huge drifts (up to 25 degrees over 2m in our case) near ferromagnetic masses, such as the one you can find in building walls. What we did to solve this is to use the IMU heading, but estimate a heading bias.
• If you are outdoors, don't forget that travelling 10m on a 10 degree incline does not lead to the same change in X and Y than travelling 10m on a flat terrain. This is usually accounted for by estimating Z, but I guess it can be estimated differently.
• GPS is also a lying bitch, typically in high-multipath environments. Plus low-grade (and even in some conditions high-grade) GPSes have a tendency to report very wrong standard deviations. We used some simple chi-square tests to check whether a particular GPS measurement should be integrated (i.e. checking that it matches the current filter estimate up to a certain point), which gave us decent results.

The "typical" solution for us is to use odometry + IMU to get an ego-motion estimate and then use GPS to correct X,Y,Z and heading bias.

Here is an EKF implementation that we extensively used. If you need to estimate the IMU's orientation (i.e. if it does not already have a built-in filter), you can also use on of these two filter: UKF and EKF.