change file readme
This commit is contained in:
576
README.md
576
README.md
@@ -1,217 +1,461 @@
|
||||
# 🎯 Stanley Local Planner
|
||||
# Mission Adapters
|
||||
|
||||
**Stanley Local Planner** là một bộ điều khiển cấp địa phương (Local Planner) sử dụng **Stanley Method** cho robot di động. Bộ điều khiển này được thiết kế để hoạt động trên hai nền tảng:
|
||||
Mission Adapters là một framework quản lý nhiệm vụ (Mission Management Framework) được xây dựng bằng C++ thuần, hướng tới các hệ thống AGV/AMR và tương thích với mô hình nhiệm vụ của VDA5050.
|
||||
|
||||
- 🤖 **ROS Noetic** - Tích hợp vào ROS Navigation Stack
|
||||
- ⚙️ **C++ Standalone** - Thư viện C++ thuần cho các ứng dụng không dùng ROS
|
||||
Framework cung cấp:
|
||||
|
||||
Bộ điều khiển này sử dụng **Bicycle Kinematic Model** và kết hợp hai thành phần điều khiển:
|
||||
- **Heading Error** - Lỗi góc định hướng
|
||||
- **Cross-track Error** - Sai lệch ngang so với đường dẫn
|
||||
* Chuyển đổi Goal hoặc VDA5050 Order thành Mission.
|
||||
* Quản lý hàng đợi Mission.
|
||||
* Xử lý Event bất đồng bộ.
|
||||
* Điều phối Mission theo trạng thái Robot.
|
||||
* Hỗ trợ Pause / Resume / Cancel / Emergency.
|
||||
* Tách biệt Mission Scheduling và Mission Execution.
|
||||
* Không phụ thuộc ROS.
|
||||
|
||||
---
|
||||
|
||||
## 🚀 Đặc điểm chính
|
||||
# Kiến trúc tổng thể
|
||||
|
||||
✅ **Stanley Method kinh điển** - Thuật toán được chứng minh hiệu quả trong tự lái
|
||||
✅ **Bicycle Kinematic Model** - Mô hình chính xác cho xe, xe tải, xe điện
|
||||
✅ **Dual Platform Support** - ROS Noetic + C++ Standalone
|
||||
✅ **Hiệu suất cao** - CPU usage < 5%
|
||||
✅ **Cấu hình đơn giản** - Chỉ cần 3-4 tham số chính
|
||||
✅ **Theo dõi đường chính xác** - Tracking error < 0.1m
|
||||
✅ **Chuyển động mượt mà** - Điều khiển lái góc mức** (smooth steering)
|
||||
|
||||
---
|
||||
|
||||
## 📚 Nguyên lý Stanley Method
|
||||
|
||||
### Bicycle Model
|
||||
|
||||
Mô hình bicycle kinematic mô tả động học của robot:
|
||||
|
||||
```
|
||||
θ (yaw angle)
|
||||
↑
|
||||
| Front Axle (steering angle δ)
|
||||
| /
|
||||
|___/______ Rear Axle (reference point)
|
||||
L (wheelbase)
|
||||
```
|
||||
|
||||
Phương trình:
|
||||
```
|
||||
ẋ = v * cos(θ)
|
||||
ẏ = v * sin(θ)
|
||||
θ̇ = v * tan(δ) / L
|
||||
```
|
||||
|
||||
### Stanley Control Law
|
||||
|
||||
Stanley controller kết hợp hai thành phần lỗi:
|
||||
|
||||
```
|
||||
δ = atan2(k_e * e_y, v) + e_θ
|
||||
|
||||
Trong đó:
|
||||
δ - Góc lái (steering angle)
|
||||
k_e - Cross-track error gain
|
||||
e_y - Sai lệch ngang (cross-track error)
|
||||
v - Vận tốc tiến (forward velocity)
|
||||
e_θ - Sai lệch góc (heading error)
|
||||
```
|
||||
|
||||
**Thành phần 1: Cross-track Error (e_y)**
|
||||
- Khoảng cách ngang từ robot đến đường dẫn
|
||||
- Được cân nhân bằng vận tốc để ổn định (adaptive gain)
|
||||
- Ưu điểm: Quay về đường nhanh chóng
|
||||
|
||||
**Thành phần 2: Heading Error (e_θ)**
|
||||
- Sai lệch góc so với hướng mong muốn của đường dẫn
|
||||
- Đảm bảo robot song song với đường dẫn
|
||||
- Ưu điểm: Tránh dao động, điều khiển ổn định
|
||||
|
||||
---
|
||||
|
||||
## 🛠️ Cài đặt
|
||||
|
||||
### Yêu cầu hệ thống
|
||||
|
||||
**Chung:**
|
||||
- C++17 trở lên
|
||||
- Eigen 3.3+
|
||||
- CMake 3.10+
|
||||
|
||||
### Build từ source
|
||||
|
||||
```bash
|
||||
# Clone repository
|
||||
cd ~/ros_workspace/src
|
||||
git clone link_to_repo
|
||||
cd ..
|
||||
|
||||
# Build với ROS
|
||||
catkin_make
|
||||
|
||||
# Hoặc build standalone C++ library
|
||||
cd src/stanley_local_planner
|
||||
mkdir build && cd build
|
||||
cmake -DBUILD_ROS=OFF ..
|
||||
make
|
||||
sudo make install
|
||||
```text
|
||||
Order / Goal
|
||||
│
|
||||
▼
|
||||
+------------------+
|
||||
| EventProcessor |
|
||||
+------------------+
|
||||
│
|
||||
▼
|
||||
+------------------+
|
||||
| MissionManager |
|
||||
+------------------+
|
||||
│
|
||||
▼
|
||||
+------------------+
|
||||
| MissionExecutor |
|
||||
+------------------+
|
||||
│
|
||||
▼
|
||||
Navigation Stack
|
||||
(MoveBase, MPPI,
|
||||
Pure Pursuit...)
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 🎯 Cấu hình
|
||||
# Thành phần chính
|
||||
|
||||
### Tham số chính
|
||||
## Mission
|
||||
|
||||
Tất cả tham số được cấu hình trong `config/stanley_local_planner_params.yaml`:
|
||||
Đơn vị thực thi cơ bản.
|
||||
|
||||
```yaml
|
||||
# Robot Parameters
|
||||
robot_max_v: 1.0 # Vận tốc tối đa (m/s)
|
||||
robot_max_a: 1.0 # Gia tốc tối đa (m/s²)
|
||||
robot_wheelbase: 0.5 # Khoảng cách trục bánh (m)
|
||||
robot_max_steering_angle: 0.5 # Góc lái tối đa (rad ≈ 28°)
|
||||
robot_min_radius: 0.3 # Bán kính quay tối thiểu (m)
|
||||
```cpp
|
||||
class Mission
|
||||
{
|
||||
public:
|
||||
int sequenceId;
|
||||
|
||||
# Stanley Controller Parameters
|
||||
k_e: 2.0 # Cross-track error gain
|
||||
k_theta: 0.5 # Heading error gain
|
||||
time_step: 0.05 # Chu kỳ điều khiển (s)
|
||||
MissionType type;
|
||||
|
||||
# Trajectory Parameters
|
||||
lookahead_distance: 1.0 # Tầm nhìn trước (m)
|
||||
path_resolution: 0.1 # Độ phân giải đường dẫn (m)
|
||||
int priority;
|
||||
|
||||
# Goal Tolerance
|
||||
xy_goal_tolerance: 0.1 # Sai số vị trí (m)
|
||||
yaw_goal_tolerance: 0.1 # Sai số định hướng (rad)
|
||||
PoseStamped start;
|
||||
|
||||
PoseStamped goal;
|
||||
|
||||
std::vector<Node> nodes;
|
||||
|
||||
std::vector<Edge> edges;
|
||||
|
||||
std::vector<Action> actions;
|
||||
};
|
||||
```
|
||||
|
||||
### Điều chỉnh hiệu suất
|
||||
Mission có thể được tạo từ:
|
||||
|
||||
| Tham số | Tăng | Giảm |
|
||||
|---------|------|------|
|
||||
| `k_e` | Quay nhanh, có thể dao động | Quay chậm, trễ |
|
||||
| `k_theta` | Định hướng ổn định hơn | Có thể dao động |
|
||||
| `lookahead_distance` | Chuyển động mượt, mục tiêu xa | Phản ứng chậm, sai lệch lớn |
|
||||
|
||||
**Khuyến nghị:**
|
||||
- Start với `k_e=2.0, k_theta=0.5`
|
||||
- Nếu quá khúc: giảm `k_e` hoặc tăng `lookahead_distance`
|
||||
- Nếu quá mận: tăng `k_e` hoặc giảm `lookahead_distance`
|
||||
* Goal đơn giản
|
||||
* VDA5050 Order
|
||||
|
||||
---
|
||||
|
||||
## 📊 Hiệu suất
|
||||
## Action
|
||||
|
||||
### Benchmark Results
|
||||
Đại diện cho một hành động tại Node hoặc Edge.
|
||||
|
||||
| Metric | Value | Ghi chú |
|
||||
|--------|-------|--------|
|
||||
| Tracking Error | < 0.08m | Path deviation |
|
||||
| Heading Error | < 0.05 rad | Angular accuracy |
|
||||
| Response Time | < 50ms | Control loop |
|
||||
| CPU Usage | ~2-3% | Single core |
|
||||
| Memory | ~5MB | Runtime |
|
||||
| Max Speed | 1.5 m/s | Tested |
|
||||
```cpp
|
||||
class Action
|
||||
{
|
||||
public:
|
||||
int sequenceId;
|
||||
|
||||
ActionType type;
|
||||
|
||||
robot_protocol_msgs::Action action;
|
||||
};
|
||||
```
|
||||
|
||||
Các Action sẽ được sắp xếp theo `sequenceId`.
|
||||
|
||||
---
|
||||
|
||||
## 📐 Mô hình toán học
|
||||
## GoalAdapter
|
||||
|
||||
### Cross-track Error Calculation
|
||||
Chuyển đổi Goal thành Mission.
|
||||
|
||||
```
|
||||
Cho đường dẫn từ điểm P_i đến P_{i+1}
|
||||
Vị trí robot: R = (x, y)
|
||||
```cpp
|
||||
GoalAdapter adapter;
|
||||
|
||||
1. Tìm hình chiếu vuông góc từ R lên đường P_i → P_{i+1}
|
||||
2. e_y = khoảng cách từ R đến điểm chiếu
|
||||
3. Dấu: dương nếu robot ở bên trái, âm nếu ở bên phải
|
||||
auto missions =
|
||||
adapter.convert(goal);
|
||||
```
|
||||
|
||||
### Heading Error Calculation
|
||||
Kết quả:
|
||||
|
||||
```
|
||||
ψ_desired = atan2(P_{i+1}.y - P_i.y, P_{i+1}.x - P_i.x)
|
||||
ψ_current = θ (yaw angle của robot)
|
||||
e_θ = normalize(ψ_desired - ψ_current) [-π, π]
|
||||
```
|
||||
|
||||
### Steering Angle Computation
|
||||
|
||||
```
|
||||
δ_ff = atan(tan(path_curvature) * wheelbase) // Feedforward
|
||||
δ_fb = atan2(k_e * e_y, v) + k_theta * e_θ // Feedback
|
||||
δ = δ_ff + δ_fb
|
||||
δ = clamp(δ, -max_angle, max_angle)
|
||||
```text
|
||||
Goal
|
||||
└── Mission
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 📚 Tài liệu tham khảo
|
||||
## VDA5050Adapter
|
||||
|
||||
- **Stanley Method Paper**: "Journal of Field Robotics" - Thrun et al. (2006)
|
||||
- **Bicycle Kinematic Model**: Classic control theory
|
||||
- **ROS Navigation**: [http://wiki.ros.org/navigation](http://wiki.ros.org/navigation)
|
||||
- **Eigen Documentation**: [https://eigen.tuxfamily.org/](https://eigen.tuxfamily.org/)
|
||||
Chuyển đổi Order thành danh sách Mission.
|
||||
|
||||
Ví dụ:
|
||||
|
||||
```text
|
||||
N1 ---- N2 ---- N3(Action)
|
||||
|
|
||||
|
|
||||
V
|
||||
Mission A
|
||||
|
||||
N3 ---- N4 ---- N5(Action)
|
||||
|
|
||||
|
|
||||
V
|
||||
Mission B
|
||||
```
|
||||
|
||||
Order sẽ được chia thành nhiều Mission tại các Node chứa Action.
|
||||
|
||||
---
|
||||
|
||||
## 📄 Giấy phép
|
||||
## EventBus
|
||||
|
||||
**MIT License** - Tự do sử dụng, sửa đổi, phân phối
|
||||
Hàng đợi ưu tiên cho các Event.
|
||||
|
||||
```cpp
|
||||
std::priority_queue<
|
||||
Event,
|
||||
std::vector<Event>,
|
||||
EventCompare>;
|
||||
```
|
||||
|
||||
Priority nhỏ hơn sẽ được xử lý trước.
|
||||
|
||||
```cpp
|
||||
EMERGENCY = 0
|
||||
CANCEL = 1
|
||||
RESUME = 2
|
||||
PAUSE = 3
|
||||
NAV_DONE = 4
|
||||
ORDER = 5
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## ✉️ Liên hệ & Support
|
||||
## EventProcessor
|
||||
|
||||
- **Email**: duongtd@phenikaa-x.com
|
||||
Tiếp nhận Event từ bên ngoài.
|
||||
|
||||
Ví dụ:
|
||||
|
||||
```cpp
|
||||
event_processor.orderEvent(order);
|
||||
|
||||
event_processor.pauseEvent();
|
||||
|
||||
event_processor.resumeEvent();
|
||||
|
||||
event_processor.cancelEvent();
|
||||
|
||||
event_processor.emergencyEvent();
|
||||
```
|
||||
|
||||
EventProcessor hoạt động trên một worker thread riêng.
|
||||
|
||||
---
|
||||
|
||||
**Cảm ơn bạn đã sử dụng Stanley Local Planner!** 🚗
|
||||
## MissionManager
|
||||
|
||||
Quản lý trạng thái và hàng đợi Mission.
|
||||
|
||||
Các trạng thái hỗ trợ:
|
||||
|
||||
```cpp
|
||||
IDLE
|
||||
QUEUED
|
||||
RUNNING
|
||||
PAUSED
|
||||
WAITING_ACTION
|
||||
RECOVERY
|
||||
COMPLETED
|
||||
FAILED
|
||||
CANCELLED
|
||||
EMERGENCY
|
||||
```
|
||||
|
||||
Chức năng:
|
||||
|
||||
* Submit Mission
|
||||
* Lấy Mission tiếp theo
|
||||
* Pause
|
||||
* Resume
|
||||
* Cancel
|
||||
* Emergency
|
||||
* Navigation Done
|
||||
* Navigation Failed
|
||||
|
||||
---
|
||||
|
||||
## MissionExecutor
|
||||
|
||||
Thread chuyên lấy Mission từ MissionManager.
|
||||
|
||||
Khi có Mission mới, callback sẽ được gọi.
|
||||
|
||||
```cpp
|
||||
using MissionCallback =
|
||||
std::function<
|
||||
void(
|
||||
const std::shared_ptr<Mission>&
|
||||
)
|
||||
>;
|
||||
```
|
||||
|
||||
Đăng ký callback:
|
||||
|
||||
```cpp
|
||||
mission_executor.setMissionCallback(
|
||||
[&](const std::shared_ptr<Mission>& mission)
|
||||
{
|
||||
executeMission(*mission);
|
||||
});
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
# Luồng hoạt động
|
||||
|
||||
## Goal
|
||||
|
||||
```text
|
||||
Goal
|
||||
│
|
||||
▼
|
||||
GoalAdapter
|
||||
│
|
||||
▼
|
||||
Mission
|
||||
│
|
||||
▼
|
||||
MissionManager
|
||||
│
|
||||
▼
|
||||
MissionExecutor
|
||||
│
|
||||
▼
|
||||
Navigation
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## Order
|
||||
|
||||
```text
|
||||
Order
|
||||
│
|
||||
▼
|
||||
VDA5050Adapter
|
||||
│
|
||||
▼
|
||||
Mission A
|
||||
Mission B
|
||||
Mission C
|
||||
│
|
||||
▼
|
||||
Mission Queue
|
||||
│
|
||||
▼
|
||||
MissionExecutor
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
# Navigation Feedback
|
||||
|
||||
Khi Navigation hoàn thành:
|
||||
|
||||
```cpp
|
||||
event_processor.navDoneEvent();
|
||||
```
|
||||
|
||||
Khi Navigation thất bại:
|
||||
|
||||
```cpp
|
||||
event_processor.navFailedEvent();
|
||||
```
|
||||
|
||||
Ví dụ:
|
||||
|
||||
```cpp
|
||||
auto state =
|
||||
move_base_ptr_->getFeedback()
|
||||
->navigation_state;
|
||||
|
||||
if(state != prev_state_)
|
||||
{
|
||||
if(state ==
|
||||
State::SUCCEEDED)
|
||||
{
|
||||
event_processor.navDoneEvent();
|
||||
}
|
||||
|
||||
if(state ==
|
||||
State::ABORTED)
|
||||
{
|
||||
event_processor.navFailedEvent();
|
||||
}
|
||||
|
||||
prev_state_ = state;
|
||||
}
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
# Ví dụ sử dụng
|
||||
|
||||
## Khởi tạo
|
||||
|
||||
```cpp
|
||||
MissionManager mission_manager;
|
||||
|
||||
EventProcessor event_processor(
|
||||
mission_manager);
|
||||
|
||||
MissionExecutor mission_executor(
|
||||
mission_manager);
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## Start
|
||||
|
||||
```cpp
|
||||
event_processor.start();
|
||||
|
||||
mission_executor.start();
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## Đăng ký callback
|
||||
|
||||
```cpp
|
||||
mission_executor.setMissionCallback(
|
||||
[&](const std::shared_ptr<Mission>& mission)
|
||||
{
|
||||
executeMission(*mission);
|
||||
});
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## Nhận Order
|
||||
|
||||
```cpp
|
||||
robot_protocol_msgs::Order order;
|
||||
|
||||
event_processor.orderEvent(order);
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## Pause
|
||||
|
||||
```cpp
|
||||
event_processor.pauseEvent();
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## Resume
|
||||
|
||||
```cpp
|
||||
event_processor.resumeEvent();
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## Cancel
|
||||
|
||||
```cpp
|
||||
event_processor.cancelEvent();
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## Emergency Stop
|
||||
|
||||
```cpp
|
||||
event_processor.emergencyEvent();
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
# Thread Model
|
||||
|
||||
Framework sử dụng 2 worker thread:
|
||||
|
||||
```text
|
||||
Thread 1
|
||||
└─ EventProcessor
|
||||
|
||||
Thread 2
|
||||
└─ MissionExecutor
|
||||
```
|
||||
|
||||
Navigation Stack hoạt động độc lập.
|
||||
|
||||
```text
|
||||
Main Thread
|
||||
│
|
||||
├─ Navigation
|
||||
│
|
||||
├─ EventProcessor
|
||||
│
|
||||
└─ MissionExecutor
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
# TODO
|
||||
|
||||
Các chức năng dự kiến bổ sung:
|
||||
|
||||
* Action Executor
|
||||
* WAITING_ACTION state
|
||||
* ACTION_DONE event
|
||||
* ACTION_FAILED event
|
||||
* Recovery Framework
|
||||
* Mission Priority Queue
|
||||
* Mission Persistence
|
||||
* VDA5050 Instant Actions
|
||||
* Multi-Robot Fleet Support
|
||||
|
||||
---
|
||||
|
||||
# License
|
||||
|
||||
Internal Project.
|
||||
|
||||
Reference in New Issue
Block a user