136 lines
3.6 KiB
Markdown
136 lines
3.6 KiB
Markdown
# odomety
|
||
|
||
Thư viện C++ ước lượng odometry cho robot **differential drive** (2 bánh chủ động).
|
||
|
||
Từ vận tốc góc bánh xe, **encoder ticks**, hoặc lệnh `v`/`ω`, thư viện tích phân pose 2D `(x, y, θ)` và vận tốc `(v, ω)` theo thời gian.
|
||
|
||
## Tính năng
|
||
|
||
- Cập nhật từ vận tốc góc bánh trái/phải `[rad/s]`
|
||
- Cập nhật từ cumulative encoder ticks (`Encoder_Measurement`-style)
|
||
- Cập nhật trực tiếp từ lệnh tuyến tính/góc `[m/s]`, `[rad/s]`
|
||
- Tích phân exact (và Runge–Kutta bậc 2 khi gần đi thẳng)
|
||
- Làm mượt vận tốc bằng rolling mean (Boost Accumulators)
|
||
- Demo và unit test kèm theo
|
||
|
||
## Yêu cầu
|
||
|
||
- CMake ≥ 3.14
|
||
- C++17
|
||
- Boost (header: Accumulators, Function)
|
||
|
||
## Cấu trúc
|
||
|
||
```
|
||
odomety/
|
||
├── include/odometry/odom_differential.h
|
||
├── src/odom_differential.cpp
|
||
├── src/main.cpp # demo
|
||
├── test/odom_differential_test.cpp
|
||
└── CMakeLists.txt
|
||
```
|
||
|
||
## Build
|
||
|
||
```bash
|
||
cd odomety
|
||
cmake -S . -B build
|
||
cmake --build build
|
||
```
|
||
|
||
Chạy demo:
|
||
|
||
```bash
|
||
./build/odomety_demo
|
||
```
|
||
|
||
Chạy test:
|
||
|
||
```bash
|
||
cd build && ctest --output-on-failure
|
||
# hoặc
|
||
./build/odomety_test
|
||
```
|
||
|
||
## Cài đặt (tùy chọn)
|
||
|
||
```bash
|
||
sudo cmake --install build --prefix /path/to/install
|
||
```
|
||
|
||
Trong project khác:
|
||
|
||
```cmake
|
||
find_package(odomety REQUIRED)
|
||
target_link_libraries(your_target PRIVATE odomety::odom_differential)
|
||
```
|
||
|
||
## Cách dùng
|
||
|
||
### Từ vận tốc bánh
|
||
|
||
```cpp
|
||
#include <odometry/odom_differential.h>
|
||
|
||
differential::Odometry odom(/*velocity_rolling_window_size=*/10);
|
||
|
||
odom.setWheelParams(
|
||
/*wheel_separation=*/0.5, // [m]
|
||
/*left_wheel_radius=*/0.09, // [m]
|
||
/*right_wheel_radius=*/0.09 // [m]
|
||
);
|
||
|
||
odom.init(/*time_sec=*/t0);
|
||
odom.updateWheel(left_omega, right_omega, t); // [rad/s]
|
||
```
|
||
|
||
### Từ encoder ticks (khuyến nghị khi có đếm xung)
|
||
|
||
```cpp
|
||
odom.setWheelParams(0.5, 0.09, 0.09);
|
||
odom.setEncoderParams(/*ticks_per_revolution=*/4096); // phía bánh, sau gearbox
|
||
odom.init(t0);
|
||
|
||
// Cumulative tick counts (unwrap overflow về int64 monotonic trước khi gọi)
|
||
odom.updateTicks(left_ticks, right_ticks, t); // lần đầu chỉ seed, trả về false
|
||
|
||
double x = odom.getX();
|
||
double y = odom.getY();
|
||
double yaw = odom.getHeading();
|
||
double v = odom.getLinear();
|
||
double w = odom.getAngular();
|
||
```
|
||
|
||
Công thức nội bộ mỗi bước:
|
||
|
||
- `Δs_L/R = 2π · r_L/R · Δn_L/R / N_L/R`
|
||
- `Δs = (Δs_L + Δs_R) / 2`
|
||
- `Δθ = (Δs_R − Δs_L) / b`
|
||
|
||
### Từ lệnh vận tốc (open-loop)
|
||
|
||
```cpp
|
||
odom.updateOdom(linear_mps, angular_rps, t);
|
||
```
|
||
|
||
## API chính
|
||
|
||
| Hàm | Mô tả |
|
||
|---|---|
|
||
| `init(time)` | Reset pose/vận tốc/tick seed và gắn timestamp |
|
||
| `setWheelParams(sep, r_l, r_r)` | Khoảng cách bánh và bán kính |
|
||
| `setEncoderParams(N)` / `setEncoderParams(N_l, N_r)` | CPR phía bánh |
|
||
| `updateWheel(ω_l, ω_r, time)` | Cập nhật từ vận tốc góc bánh |
|
||
| `updateTicks(n_l, n_r, time)` | Cập nhật từ cumulative encoder ticks |
|
||
| `updateOdom(v, ω, time)` | Cập nhật từ lệnh vận tốc |
|
||
| `getX/Y/Heading/Linear/Angular()` | Đọc trạng thái hiện tại |
|
||
|
||
## Ghi chú
|
||
|
||
- `updateWheel` / `updateTicks` trả về `false` nếu `dt < 0.0001` s.
|
||
- `updateTicks`: lần gọi đầu sau `init` chỉ lưu tick trước đó (seed), không tích phân.
|
||
- `updateTicks` trả về `false` nếu chưa gọi `setEncoderParams` (CPR ≤ 0).
|
||
- Tick phải là đếm **phía bánh** (sau gearbox). Overflow counter phần cứng cần unwrap về `int64` monotonic.
|
||
- Đơn vị thời gian là giây (epoch hoặc monotonic, miễn nhất quán).
|
||
- Pose mặc định sau `init`: `(0, 0, 0)`.
|