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

cd odomety
cmake -S . -B build
cmake --build build

Chạy demo:

./build/odomety_demo

Chạy test:

cd build && ctest --output-on-failure
# hoặc
./build/odomety_test

Cài đặt (tùy chọn)

sudo cmake --install build --prefix /path/to/install

Trong project khác:

find_package(odomety REQUIRED)
target_link_libraries(your_target PRIVATE odomety::odom_differential)

Cách dùng

Từ vận tốc bánh

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

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)

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