前言
你在餐厅见过自动送餐机器人吗？
- 它怎么知道要去哪个桌子？
- 遇到人挡路怎么办？
- 多个订单怎么安排？
- 怎么不撞墙、不迷路？
今天从原理到代码，彻底搞懂送餐机器人的核心技术。
系统架构总览
┌─────────────────────────────────────────────────────────────────────────────┐
│ 送餐机器人系统架构 │
├─────────────────────────────────────────────────────────────────────────────┤
│ │
│ ┌─────────────────────────────────────────────────────────────────────┐ │
│ │ 云端/后台系统 │ │
│ │ ┌─────────────┐ ┌─────────────┐ ┌─────────────┐ │ │
│ │ │ 订单系统 │ │ 地图管理 │ │ 调度中心 │ │ │
│ │ │ (POS对接) │ │ │ │ (多机协调) │ │ │
│ │ └──────┬──────┘ └──────┬──────┘ └──────┬──────┘ │ │
│ └─────────┼────────────────┼────────────────┼─────────────────────────┘ │
│ │ │ │ │
│ └────────────────┼────────────────┘ │
│ │ WiFi/4G │
│ ▼ │
│ ┌─────────────────────────────────────────────────────────────────────┐ │
│ │ 机器人本体 │ │
│ │ │ │
│ │ ┌───────────────────────────────────────────────────────────┐ │ │
│ │ │ 决策层 │ │ │
│ │ │ ┌─────────────┐ ┌─────────────┐ ┌─────────────┐ │ │ │
│ │ │ │ 任务管理 │ │ 行为决策 │ │ 异常处理 │ │ │ │
│ │ │ └──────┬──────┘ └──────┬──────┘ └──────┬──────┘ │ │ │
│ │ └─────────┼────────────────┼────────────────┼───────────────┘ │ │
│ │ │ │ │ │ │
│ │ ┌─────────┼────────────────┼────────────────┼───────────────┐ │ │
│ │ │ ▼ ▼ ▼ │ │ │
│ │ │ 规划层 │ │ │
│ │ │ ┌─────────────┐ ┌─────────────┐ ┌─────────────┐ │ │ │
│ │ │ │ 全局规划 │ │ 局部规划 │ │ 速度规划 │ │ │ │
│ │ │ │ (A*/D*) │ │ (DWA/TEB) │ │ │ │ │ │
│ │ │ └──────┬──────┘ └──────┬──────┘ └──────┬──────┘ │ │ │
│ │ └─────────┼────────────────┼────────────────┼───────────────┘ │ │
│ │ │ │ │ │ │
│ │ ┌─────────┼────────────────┼────────────────┼───────────────┐ │ │
│ │ │ ▼ ▼ ▼ │ │ │
│ │ │ 感知层 │ │ │
│ │ │ ┌─────────────┐ ┌─────────────┐ ┌─────────────┐ │ │ │
│ │ │ │ SLAM定位 │ │ 障碍物检测 │ │ 语义识别 │ │ │ │
│ │ │ └──────┬──────┘ └──────┬──────┘ └──────┬──────┘ │ │ │
│ │ └─────────┼────────────────┼────────────────┼───────────────┘ │ │
│ │ │ │ │ │ │
│ │ ┌─────────┼────────────────┼────────────────┼───────────────┐ │ │
│ │ │ ▼ ▼ ▼ │ │ │
│ │ │ 硬件层 │ │ │
│ │ │ ┌────────┐ ┌────────┐ ┌────────┐ ┌────────┐ ┌────────┐ │ │ │
│ │ │ │ 激光雷达│ │ 深度相机│ │ IMU │ │ 编码器 │ │ 电机 │ │ │ │
│ │ │ └────────┘ └────────┘ └────────┘ └────────┘ └────────┘ │ │ │
│ │ └───────────────────────────────────────────────────────────┘ │ │
│ │ │ │
│ └─────────────────────────────────────────────────────────────────────┘ │
│ │
└─────────────────────────────────────────────────────────────────────────────┘一、定位与建图（SLAM）
┌─────────────────────────────────────────────────────────────────────────────┐
│ SLAM：机器人怎么知道自己在哪？ │
├─────────────────────────────────────────────────────────────────────────────┤
│ │
│ 【问题】 │
│ ───────────────────────────────────────── │
│ │
│ 机器人需要同时解决两个问题: │
│ 1. 我在哪？ (定位 Localization) │
│ 2. 周围是什么样？ (建图 Mapping) │
│ │
│ 鸡生蛋问题: 定位需要地图，建图需要定位 │
│ 解决方案: SLAM (同时定位与建图) │
│ │
│ 【激光SLAM流程】 │
│ ───────────────────────────────────────── │
│ │
│ 激光雷达扫描 ──► 点云数据 ──► 特征提取 ──► 扫描匹配 │
│ │ │
│ ▼ │
│ 位姿估计 ◄──── 里程计融合 │
│ │ │
│ ▼ │
│ 回环检测 ──► 地图优化 │
│ │ │
│ ▼ │
│ 栅格地图输出 │
│ │
│ 【栅格地图】 │
│ ───────────────────────────────────────── │
│ │
│ ┌───────────────────────────────────────┐ │
│ │ ■ ■ ■ ■ ■ ■ ■ ■ ■ ■ ■ ■ ■ ■ ■ ■ ■ │ ■ = 障碍物 (100) │
│ │ ■ □ □ □ □ □ □ □ □ □ □ □ □ □ □ □ ■ │ □ = 可通行 (0) │
│ │ ■ □ □ □ ■ ■ □ □ □ □ □ ■ ■ □ □ □ ■ │ ░ = 未知 (-1) │
│ │ ■ □ □ □ ■ ■ □ □ □ □ □ ■ ■ □ □ □ ■ │ │
│ │ ■ □ □ □ □ □ □ □ ★ □ □ □ □ □ □ □ ■ │ ★ = 机器人位置 │
│ │ ■ □ □ □ □ □ □ □ □ □ □ □ □ □ □ □ ■ │ │
│ │ ■ □ □ ■ ■ ■ □ □ □ □ □ ■ ■ ■ □ □ ■ │ 每个格子代表实际的 │
│ │ ■ □ □ □ □ □ □ □ □ □ □ □ □ □ □ □ ■ │ 5cm x 5cm 区域 │
│ │ ■ ■ ■ ■ ■ ■ ■ ■ ■ ■ ■ ■ ■ ■ ■ ■ ■ │ │
│ └───────────────────────────────────────┘ │
│ │
│ 餐厅场景下还需要标注: │
│ - 桌子位置 (Table 1, Table 2, ...) │
│ - 取餐点 │
│ - 充电桩位置 │
│ - 禁行区域 (厨房内部等) │
│ │
└─────────────────────────────────────────────────────────────────────────────┘SLAM定位代码
cpp
/**
* 简化的粒子滤波定位 (AMCL原理)
*/
#include <vector>
#include <random>
#include <cmath>
struct Pose2D {
double x, y, theta;
};
struct Particle {
Pose2D pose;
double weight;
};
struct LaserScan {
std::vector<double> ranges; // 距离数据
double angle_min;
double angle_increment;
int num_readings;
};
class ParticleFilter {
public:
ParticleFilter(int num_particles = 500)
: num_particles_(num_particles), gen_(std::random_device{}()) {
particles_.resize(num_particles);
}
/**
* 初始化粒子
*/
void initialize(double x, double y, double theta,
double xy_std = 0.5, double theta_std = 0.2) {
std::normal_distribution<> xy_dist(0, xy_std);
std::normal_distribution<> theta_dist(0, theta_std);
for (auto& p : particles_) {
p.pose.x = x + xy_dist(gen_);
p.pose.y = y + xy_dist(gen_);
p.pose.theta = theta + theta_dist(gen_);
p.weight = 1.0 / num_particles_;
}
}
/**
* 运动更新 (预测步骤)
*/
void motionUpdate(double delta_x, double delta_y, double delta_theta) {
// 运动噪声
double trans = std::sqrt(delta_x * delta_x + delta_y * delta_y);
double rot1 = std::atan2(delta_y, delta_x);
double rot2 = delta_theta - rot1;
std::normal_distribution<> trans_noise(0, 0.1 * trans + 0.01);
std::normal_distribution<> rot_noise(0, 0.1 * std::abs(delta_theta) + 0.02);
for (auto& p : particles_) {
double noisy_rot1 = rot1 + rot_noise(gen_);
double noisy_trans = trans + trans_noise(gen_);
double noisy_rot2 = rot2 + rot_noise(gen_);
p.pose.x += noisy_trans * std::cos(p.pose.theta + noisy_rot1);
p.pose.y += noisy_trans * std::sin(p.pose.theta + noisy_rot1);
p.pose.theta += noisy_rot1 + noisy_rot2;
// 归一化角度
while (p.pose.theta > M_PI) p.pose.theta -= 2 * M_PI;
while (p.pose.theta < -M_PI) p.pose.theta += 2 * M_PI;
}
}
/**
* 观测更新 (使用激光雷达)
*/
void sensorUpdate(const LaserScan& scan, const OccupancyGrid& map) {
double total_weight = 0;
for (auto& p : particles_) {
// 计算该粒子位置下，激光扫描的似然
p.weight = computeLikelihood(p.pose, scan, map);
total_weight += p.weight;
}
// 归一化权重
if (total_weight > 0) {
for (auto& p : particles_) {
p.weight /= total_weight;
}
}
}
/**
* 计算激光扫描的似然
*/
double computeLikelihood(const Pose2D& pose, const LaserScan& scan,
const OccupancyGrid& map) {
double likelihood = 1.0;
// 每隔几个点采样计算
int step = 10;
for (int i = 0; i < scan.num_readings; i += step) {
double angle = scan.angle_min + i * scan.angle_increment;
double range = scan.ranges[i];
if (range < 0.1 || range > 10.0) continue; // 无效数据
// 计算激光端点在地图坐标系中的位置
double world_angle = pose.theta + angle;
double hit_x = pose.x + range * std::cos(world_angle);
double hit_y = pose.y + range * std::sin(world_angle);
// 查询地图，计算最近障碍物距离
double dist = map.distanceToNearestObstacle(hit_x, hit_y);
// 高斯似然
double sigma = 0.2; // 传感器噪声
double prob = std::exp(-dist * dist / (2 * sigma * sigma));
likelihood *= prob;
}
return likelihood;
}
/**
* 重采样
*/
void resample() {
std::vector<Particle> new_particles;
new_particles.reserve(num_particles_);
// 低方差重采样
std::uniform_real_distribution<> dist(0, 1.0 / num_particles_);
double r = dist(gen_);
double c = particles_[0].weight;
int i = 0;
for (int m = 0; m < num_particles_; ++m) {
double u = r + m * (1.0 / num_particles_);
while (u > c && i < num_particles_ - 1) {
i++;
c += particles_[i].weight;
}
new_particles.push_back(particles_[i]);
new_particles.back().weight = 1.0 / num_particles_;
}
particles_ = std::move(new_particles);
}
/**
* 获取估计位姿 (加权平均)
*/
Pose2D getEstimatedPose() const {
Pose2D pose = {0, 0, 0};
double sin_sum = 0, cos_sum = 0;
for (const auto& p : particles_) {
pose.x += p.weight * p.pose.x;
pose.y += p.weight * p.pose.y;
sin_sum += p.weight * std::sin(p.pose.theta);
cos_sum += p.weight * std::cos(p.pose.theta);
}
pose.theta = std::atan2(sin_sum, cos_sum);
return pose;
}
private:
int num_particles_;
std::vector<Particle> particles_;
std::mt19937 gen_;
};二、全局路径规划
┌─────────────────────────────────────────────────────────────────────────────┐
│ 全局路径规划：从A到B怎么走？ │
├─────────────────────────────────────────────────────────────────────────────┤
│ │
│ 【常用算法对比】 │
│ ───────────────────────────────────────── │
│ │
│ ┌─────────────┬─────────────┬─────────────┬─────────────────────────┐ │
│ │ 算法 │ 时间复杂度 │ 最优性 │ 特点 │ │
│ ├─────────────┼─────────────┼─────────────┼─────────────────────────┤ │
│ │ Dijkstra │ O(V²) │ ✅ 最优 │ 无启发，慢 │ │
│ │ A* │ O(V log V) │ ✅ 最优 │ 有启发，快，最常用 │ │
│ │ D* Lite │ 增量更新 │ ✅ 最优 │ 动态环境，可重规划 │ │
│ │ RRT │ 概率完备 │ ❌ 次优 │ 高维空间 │ │
│ │ RRT* │ 渐近最优 │ ✅ 渐近最优 │ 高维空间，质量更好 │ │
│ └─────────────┴─────────────┴─────────────┴─────────────────────────┘ │
│ │
│ 餐厅场景推荐: A* (静态环境) 或 D* Lite (需要动态避障) │
│ │
│ 【A*算法可视化】 │
│ ───────────────────────────────────────── │
│ │
│ ┌─────────────────────────────────────────────────────────┐ │
│ │ ■ ■ ■ ■ ■ ■ ■ ■ ■ ■ ■ ■ ■ ■ ■ ■ ■ ■ ■ ■ │ │
│ │ ■ S · · · · · · · · · · ■ · · · · · · ■ │ │
│ │ ■ · · · · · · · · · · · ■ · · · · · · ■ │ │
│ │ ■ · · ■ ■ ■ ■ · · · · · ■ · · · · · · ■ │ │
│ │ ■ · · · · · ■ · · · · · · · · ■ ■ · · ■ │ │
│ │ ■ · · · · · ■ · · · · · · · · ■ ■ · · ■ │ │
│ │ ■ · · · · · · · · · · · · · · · · · G ■ │ │
│ │ ■ ■ ■ ■ ■ ■ ■ ■ ■ ■ ■ ■ ■ ■ ■ ■ ■ ■ ■ ■ │ │
│ └─────────────────────────────────────────────────────────┘ │
│ │
│ S = 起点 (Start) │
│ G = 终点 (Goal) │
│ ■ = 障碍物 │
│ · = 可通行 │
│ ★ = 最终路径 │
│ │
│ f(n) = g(n) + h(n) │
│ - g(n): 从起点到n的实际代价 │
│ - h(n): 从n到终点的估计代价 (启发函数) │
│ - f(n): 总估计代价 │
│ │
└─────────────────────────────────────────────────────────────────────────────┘A*算法实现
cpp
/**
* A*路径规划算法
*/
#include <vector>
#include <queue>
#include <unordered_set>
#include <unordered_map>
#include <cmath>
#include <algorithm>
struct GridCell {
int x, y;
bool operator==(const GridCell& other) const {
return x == other.x && y == other.y;
}
};
// 哈希函数
struct GridCellHash {
size_t operator()(const GridCell& cell) const {
return std::hash<int>()(cell.x) ^ (std::hash<int>()(cell.y) << 16);
}
};
struct AStarNode {
GridCell cell;
double g_cost; // 从起点到当前节点的代价
double f_cost; // g + h
GridCell parent;
bool operator>(const AStarNode& other) const {
return f_cost > other.f_cost;
}
};
class OccupancyGrid {
public:
OccupancyGrid(int width, int height, double resolution)
: width_(width), height_(height), resolution_(resolution) {
data_.resize(width * height, 0);
}
bool isOccupied(int x, int y) const {
if (x < 0 || x >= width_ || y < 0 || y >= height_) {
return true; // 边界外视为障碍
}
return data_[y * width_ + x] > 50;
}
void setOccupied(int x, int y, int value) {
if (x >= 0 && x < width_ && y >= 0 && y < height_) {
data_[y * width_ + x] = value;
}
}
// 世界坐标转栅格坐标
GridCell worldToGrid(double wx, double wy) const {
return {
static_cast<int>((wx - origin_x_) / resolution_),
static_cast<int>((wy - origin_y_) / resolution_)
};
}
// 栅格坐标转世界坐标
void gridToWorld(int gx, int gy, double& wx, double& wy) const {
wx = origin_x_ + (gx + 0.5) * resolution_;
wy = origin_y_ + (gy + 0.5) * resolution_;
}
double distanceToNearestObstacle(double x, double y) const {
GridCell cell = worldToGrid(x, y);
// 简化实现：搜索周围区域
double min_dist = 1e9;
int search_radius = 20;
for (int dx = -search_radius; dx <= search_radius; ++dx) {
for (int dy = -search_radius; dy <= search_radius; ++dy) {
if (isOccupied(cell.x + dx, cell.y + dy)) {
double dist = std::sqrt(dx * dx + dy * dy) * resolution_;
min_dist = std::min(min_dist, dist);
}
}
}
return min_dist;
}
int width_, height_;
double resolution_;
double origin_x_ = 0, origin_y_ = 0;
std::vector<int> data_;
};
class AStarPlanner {
public:
AStarPlanner(const OccupancyGrid& map) : map_(map) {}
/**
* 规划路径
*/
std::vector<GridCell> planPath(GridCell start, GridCell goal) {
// 优先队列 (最小堆)
std::priority_queue<AStarNode, std::vector<AStarNode>, std::greater<AStarNode>> open_list;
// 已访问节点
std::unordered_set<GridCell, GridCellHash> closed_set;
// 记录每个节点的信息
std::unordered_map<GridCell, AStarNode, GridCellHash> node_info;
// 初始化起点
AStarNode start_node;
start_node.cell = start;
start_node.g_cost = 0;
start_node.f_cost = heuristic(start, goal);
start_node.parent = start;
open_list.push(start_node);
node_info[start] = start_node;
// 8个方向的移动
const int dx[] = {-1, 0, 1, -1, 1, -1, 0, 1};
const int dy[] = {-1, -1, -1, 0, 0, 1, 1, 1};
const double cost[] = {1.414, 1.0, 1.414, 1.0, 1.0, 1.414, 1.0, 1.414};
while (!open_list.empty()) {
AStarNode current = open_list.top();
open_list.pop();
// 到达目标
if (current.cell == goal) {
return reconstructPath(node_info, start, goal);
}
// 已经处理过
if (closed_set.count(current.cell)) {
continue;
}
closed_set.insert(current.cell);
// 扩展邻居
for (int i = 0; i < 8; ++i) {
GridCell neighbor = {current.cell.x + dx[i], current.cell.y + dy[i]};
// 检查是否可通行
if (map_.isOccupied(neighbor.x, neighbor.y)) {
continue;
}
// 对角线移动时检查是否会穿过障碍物
if (i == 0 || i == 2 || i == 5 || i == 7) {
if (map_.isOccupied(current.cell.x + dx[i], current.cell.y) ||
map_.isOccupied(current.cell.x, current.cell.y + dy[i])) {
continue;
}
}
if (closed_set.count(neighbor)) {
continue;
}
double tentative_g = current.g_cost + cost[i];
// 检查是否是更好的路径
auto it = node_info.find(neighbor);
if (it == node_info.end() || tentative_g < it->second.g_cost) {
AStarNode neighbor_node;
neighbor_node.cell = neighbor;
neighbor_node.g_cost = tentative_g;
neighbor_node.f_cost = tentative_g + heuristic(neighbor, goal);
neighbor_node.parent = current.cell;
node_info[neighbor] = neighbor_node;
open_list.push(neighbor_node);
}
}
}
// 无法找到路径
return {};
}
private:
/**
* 启发函数 (欧几里得距离)
*/
double heuristic(const GridCell& a, const GridCell& b) {
double dx = a.x - b.x;
double dy = a.y - b.y;
return std::sqrt(dx * dx + dy * dy);
}
/**
* 重建路径
*/
std::vector<GridCell> reconstructPath(
const std::unordered_map<GridCell, AStarNode, GridCellHash>& node_info,
const GridCell& start, const GridCell& goal) {
std::vector<GridCell> path;
GridCell current = goal;
while (!(current == start)) {
path.push_back(current);
current = node_info.at(current).parent;
}
path.push_back(start);
std::reverse(path.begin(), path.end());
return path;
}
const OccupancyGrid& map_;
};
/**
* 路径平滑
*/
class PathSmoother {
public:
/**
* 简化路径 (去除共线点)
*/
static std::vector<GridCell> simplify(const std::vector<GridCell>& path) {
if (path.size() < 3) return path;
std::vector<GridCell> simplified;
simplified.push_back(path[0]);
for (size_t i = 1; i < path.size() - 1; ++i) {
// 检查是否共线
int dx1 = path[i].x - path[i-1].x;
int dy1 = path[i].y - path[i-1].y;
int dx2 = path[i+1].x - path[i].x;
int dy2 = path[i+1].y - path[i].y;
if (dx1 != dx2 || dy1 != dy2) {
simplified.push_back(path[i]);
}
}
simplified.push_back(path.back());
return simplified;
}
/**
* B样条平滑
*/
static std::vector<Pose2D> smoothBSpline(const std::vector<GridCell>& path,
const OccupancyGrid& map,
int interpolation_points = 10) {
std::vector<Pose2D> smooth_path;
if (path.size() < 2) return smooth_path;
for (size_t i = 0; i < path.size() - 1; ++i) {
double x1, y1, x2, y2;
map.gridToWorld(path[i].x, path[i].y, x1, y1);
map.gridToWorld(path[i+1].x, path[i+1].y, x2, y2);
for (int j = 0; j < interpolation_points; ++j) {
double t = static_cast<double>(j) / interpolation_points;
Pose2D pose;
pose.x = x1 + t * (x2 - x1);
pose.y = y1 + t * (y2 - y1);
pose.theta = std::atan2(y2 - y1, x2 - x1);
smooth_path.push_back(pose);
}
}
// 添加终点
double x, y;
map.gridToWorld(path.back().x, path.back().y, x, y);
smooth_path.push_back({x, y, smooth_path.back().theta});
return smooth_path;
}
};三、局部路径规划与避障
┌─────────────────────────────────────────────────────────────────────────────┐
│ 局部规划：遇到障碍物怎么办？ │
├─────────────────────────────────────────────────────────────────────────────┤
│ │
│ 全局路径是预先规划的，但实际运行中会遇到: │
│ - 突然出现的行人 │
│ - 移动的椅子 │
│ - 其他机器人 │
│ │
│ 需要局部规划器实时调整 │
│ │
│ 【DWA (Dynamic Window Approach)】 │
│ ───────────────────────────────────────── │
│ │
│ 核心思想: 在速度空间中采样，评估每个速度的好坏 │
│ │
│ ┌─────────────────────────────────────────────────────────────────────┐ │
│ │ │ │
│ │ 速度空间 (v, ω) │ │
│ │ │ │
│ │ ω (角速度) │ │
│ │ ▲ │ │
│ │ │ ┌─────────────┐ │ │
│ │ │ │ 动态窗口 │ 受限于: │ │
│ │ │ │ (可达范围) │ - 最大加速度 │ │
│ │ │ │ ★ │ - 最大速度 │ │
│ │ │ │ 当前速度 │ - 避障约束 │ │
│ │ │ └─────────────┘ │ │
│ │ │ │ │
│ │ ───────┼──────────────────────► v (线速度) │ │
│ │ │ │ │
│ │ │ │
│ └─────────────────────────────────────────────────────────────────────┘ │
│ │
│ 评价函数: │
│ G(v,ω) = α·heading(v,ω) + β·dist(v,ω) + γ·velocity(v,ω) │
│ │
│ - heading: 朝向目标的程度 │
│ - dist: 与最近障碍物的距离 │
│ - velocity: 速度大小 (鼓励快速移动) │
│ │
└─────────────────────────────────────────────────────────────────────────────┘DWA局部规划器
cpp
/**
* DWA局部路径规划器
*/
#include <vector>
#include <cmath>
#include <limits>
struct RobotState {
double x, y, theta; // 位姿
double v, omega; // 线速度、角速度
};
struct Velocity {
double v, omega;
};
struct DWAConfig {
// 速度限制
double max_speed = 0.5; // 最大线速度 m/s
double min_speed = 0.0; // 最小线速度
double max_yaw_rate = 1.0; // 最大角速度 rad/s
// 加速度限制
double max_accel = 0.5; // 最大线加速度 m/s²
double max_yaw_accel = 1.5; // 最大角加速度 rad/s²
// 采样分辨率
double v_resolution = 0.05; // 线速度分辨率
double yaw_rate_resolution = 0.1; // 角速度分辨率
// 预测参数
double predict_time = 2.0; // 预测时间 s
double dt = 0.1; // 时间步长
// 评价函数权重
double heading_weight = 1.0;
double dist_weight = 0.5;
double velocity_weight = 0.3;
double path_weight = 0.5; // 跟踪全局路径
// 机器人参数
double robot_radius = 0.3; // 机器人半径
};
class DWAPlanner {
public:
DWAPlanner(const DWAConfig& config) : config_(config) {}
/**
* 计算最优速度
*/
Velocity computeVelocity(const RobotState& state,
const Pose2D& goal,
const std::vector<Pose2D>& global_path,
const std::vector<std::pair<double, double>>& obstacles) {
// 1. 计算动态窗口
auto [v_min, v_max, omega_min, omega_max] = calculateDynamicWindow(state);
// 2. 在动态窗口内采样
Velocity best_velocity = {0, 0};
double best_score = -std::numeric_limits<double>::infinity();
for (double v = v_min; v <= v_max; v += config_.v_resolution) {
for (double omega = omega_min; omega <= omega_max;
omega += config_.yaw_rate_resolution) {
// 3. 预测轨迹
auto trajectory = predictTrajectory(state, v, omega);
// 4. 检查碰撞
if (checkCollision(trajectory, obstacles)) {
continue;
}
// 5. 评估轨迹
double score = evaluateTrajectory(trajectory, goal, global_path, obstacles);
if (score > best_score) {
best_score = score;
best_velocity = {v, omega};
}
}
}
return best_velocity;
}
private:
/**
* 计算动态窗口 (可达速度范围)
*/
std::tuple<double, double, double, double>
calculateDynamicWindow(const RobotState& state) {
// 速度限制
double v_min = config_.min_speed;
double v_max = config_.max_speed;
double omega_min = -config_.max_yaw_rate;
double omega_max = config_.max_yaw_rate;
// 加速度限制 (一个控制周期内可达的速度)
double v_min_dyn = state.v - config_.max_accel * config_.dt;
double v_max_dyn = state.v + config_.max_accel * config_.dt;
double omega_min_dyn = state.omega - config_.max_yaw_accel * config_.dt;
double omega_max_dyn = state.omega + config_.max_yaw_accel * config_.dt;
// 取交集
v_min = std::max(v_min, v_min_dyn);
v_max = std::min(v_max, v_max_dyn);
omega_min = std::max(omega_min, omega_min_dyn);
omega_max = std::min(omega_max, omega_max_dyn);
return {v_min, v_max, omega_min, omega_max};
}
/**
* 预测轨迹
*/
std::vector<RobotState> predictTrajectory(const RobotState& state,
double v, double omega) {
std::vector<RobotState> trajectory;
RobotState s = state;
s.v = v;
s.omega = omega;
double time = 0;
while (time <= config_.predict_time) {
trajectory.push_back(s);
s = motionModel(s, v, omega, config_.dt);
time += config_.dt;
}
return trajectory;
}
/**
* 运动模型
*/
RobotState motionModel(const RobotState& state,
double v, double omega, double dt) {
RobotState new_state = state;
if (std::abs(omega) < 1e-6) {
// 直线运动
new_state.x += v * std::cos(state.theta) * dt;
new_state.y += v * std::sin(state.theta) * dt;
} else {
// 圆弧运动
double r = v / omega;
new_state.x += r * (std::sin(state.theta + omega * dt) - std::sin(state.theta));
new_state.y += r * (std::cos(state.theta) - std::cos(state.theta + omega * dt));
new_state.theta += omega * dt;
}
new_state.v = v;
new_state.omega = omega;
return new_state;
}
/**
* 碰撞检测
*/
bool checkCollision(const std::vector<RobotState>& trajectory,
const std::vector<std::pair<double, double>>& obstacles) {
for (const auto& state : trajectory) {
for (const auto& [ox, oy] : obstacles) {
double dist = std::sqrt(std::pow(state.x - ox, 2) +
std::pow(state.y - oy, 2));
if (dist <= config_.robot_radius + 0.1) { // 安全余量
return true;
}
}
}
return false;
}
/**
* 评估轨迹
*/
double evaluateTrajectory(const std::vector<RobotState>& trajectory,
const Pose2D& goal,
const std::vector<Pose2D>& global_path,
const std::vector<std::pair<double, double>>& obstacles) {
if (trajectory.empty()) return -std::numeric_limits<double>::infinity();
const RobotState& final_state = trajectory.back();
// 1. 朝向目标
double goal_angle = std::atan2(goal.y - final_state.y, goal.x - final_state.x);
double angle_diff = std::abs(normalizeAngle(goal_angle - final_state.theta));
double heading_score = M_PI - angle_diff;
// 2. 与障碍物距离
double min_dist = std::numeric_limits<double>::infinity();
for (const auto& state : trajectory) {
for (const auto& [ox, oy] : obstacles) {
double dist = std::sqrt(std::pow(state.x - ox, 2) +
std::pow(state.y - oy, 2));
min_dist = std::min(min_dist, dist);
}
}
double dist_score = std::min(min_dist, 3.0); // 上限3米
// 3. 速度 (鼓励快速移动)
double velocity_score = trajectory[0].v;
// 4. 跟踪全局路径
double path_score = 0;
if (!global_path.empty()) {
double min_path_dist = std::numeric_limits<double>::infinity();
for (const auto& path_point : global_path) {
double dist = std::sqrt(std::pow(final_state.x - path_point.x, 2) +
std::pow(final_state.y - path_point.y, 2));
min_path_dist = std::min(min_path_dist, dist);
}
path_score = 1.0 / (1.0 + min_path_dist);
}
// 综合评分
double score = config_.heading_weight * heading_score +
config_.dist_weight * dist_score +
config_.velocity_weight * velocity_score +
config_.path_weight * path_score;
return score;
}
double normalizeAngle(double angle) {
while (angle > M_PI) angle -= 2 * M_PI;
while (angle < -M_PI) angle += 2 * M_PI;
return angle;
}
DWAConfig config_;
};四、任务调度系统
┌─────────────────────────────────────────────────────────────────────────────┐
│ 任务调度：怎么安排送餐顺序？ │
├─────────────────────────────────────────────────────────────────────────────┤
│ │
│ 【问题场景】 │
│ ───────────────────────────────────────── │
│ │
│ 餐厅有多个订单同时待送: │
│ - 订单1: 3号桌，出餐时间 10:00 │
│ - 订单2: 7号桌，出餐时间 10:02 │
│ - 订单3: 5号桌，出餐时间 10:01 │
│ │
│ 机器人一次最多装3个餐，怎么安排最高效？ │
│ │
│ 【任务状态机】 │
│ ───────────────────────────────────────── │
│ │
│ ┌──────────┐ 新任务 ┌──────────┐ │
│ │ IDLE │─────────────►│ PENDING │ │
│ │ 空闲 │ │ 等待分配 │ │
│ └──────────┘ └────┬─────┘ │
│ ▲ │ 分配给机器人 │
│ │ ▼ │
│ │ ┌──────────┐ │
│ │ │ ASSIGNED │ │
│ │ │ 已分配 │ │
│ │ └────┬─────┘ │
│ │ │ 开始执行 │
│ │ ▼ │
│ │ ┌──────────┐ ┌──────────┐ ┌──────────┐ │
│ │ │ PICKING │─►│DELIVERING│─►│ ARRIVED │ │
│ │ │ 取餐中 │ │ 送餐中 │ │ 已到达 │ │
│ │ └──────────┘ └──────────┘ └────┬─────┘ │
│ │ │ 确认送达 │
│ │ ▼ │
│ │ ┌──────────┐ │
│ └───────────────────────────────│COMPLETED │ │
│ 返回待命 │ 已完成 │ │
│ └──────────┘ │
│ │
│ 【调度策略】 │
│ ───────────────────────────────────────── │
│ │
│ 1. FIFO: 先来先服务 │
│ 2. 最短路径优先: 送最近的桌子 │
│ 3. 批量配送: 合并顺路订单 │
│ 4. 优先级调度: VIP客户优先 │
│ 5. 负载均衡: 多机器人均匀分配 │
│ │
└─────────────────────────────────────────────────────────────────────────────┘任务调度系统实现
cpp
/**
* 送餐任务调度系统
*/
#include <vector>
#include <queue>
#include <map>
#include <memory>
#include <chrono>
#include <algorithm>
#include <functional>
using TimePoint = std::chrono::steady_clock::time_point;
// 任务状态
enum class TaskState {
PENDING, // 等待分配
ASSIGNED, // 已分配给机器人
PICKING, // 取餐中
DELIVERING, // 送餐中
ARRIVED, // 已到达
COMPLETED, // 已完成
CANCELLED // 已取消
};
// 任务优先级
enum class TaskPriority {
LOW = 0,
NORMAL = 1,
HIGH = 2,
URGENT = 3
};
// 送餐任务
struct DeliveryTask {
int task_id;
int table_id; // 目标桌号
int pickup_point; // 取餐点
TaskState state;
TaskPriority priority;
TimePoint created_time;
TimePoint deadline; // 最晚送达时间
std::string order_info; // 订单信息
int assigned_robot; // 分配的机器人ID
// 位置信息
double table_x, table_y;
double pickup_x, pickup_y;
};
// 机器人状态
enum class RobotState {
IDLE, // 空闲
MOVING, // 移动中
PICKING, // 取餐中
DELIVERING, // 送餐中
CHARGING, // 充电中
ERROR // 故障
};
struct Robot {
int robot_id;
RobotState state;
double x, y, theta; // 当前位置
int current_task; // 当前任务ID
std::vector<int> task_queue;// 任务队列 (批量配送)
double battery_level; // 电量百分比
int max_capacity; // 最大载餐数
int current_load; // 当前载餐数
};
// 位置点
struct Location {
int id;
std::string name;
double x, y;
int type; // 0=桌子, 1=取餐点, 2=充电桩
};
/**
* 任务调度器
*/
class TaskScheduler {
public:
TaskScheduler() {
next_task_id_ = 1;
}
/**
* 创建新任务
*/
int createTask(int table_id, int pickup_point, TaskPriority priority = TaskPriority::NORMAL) {
DeliveryTask task;
task.task_id = next_task_id_++;
task.table_id = table_id;
task.pickup_point = pickup_point;
task.state = TaskState::PENDING;
task.priority = priority;
task.created_time = std::chrono::steady_clock::now();
task.deadline = task.created_time + std::chrono::minutes(10); // 10分钟内送达
task.assigned_robot = -1;
// 获取位置坐标
if (locations_.count(table_id)) {
task.table_x = locations_[table_id].x;
task.table_y = locations_[table_id].y;
}
if (locations_.count(pickup_point)) {
task.pickup_x = locations_[pickup_point].x;
task.pickup_y = locations_[pickup_point].y;
}
tasks_[task.task_id] = task;
pending_tasks_.push_back(task.task_id);
printf("[调度] 创建任务 #%d: 从取餐点%d送到%d号桌\n",
task.task_id, pickup_point, table_id);
return task.task_id;
}
/**
* 分配任务给机器人
*/
void assignTask(int task_id, int robot_id) {
if (!tasks_.count(task_id) || !robots_.count(robot_id)) {
return;
}
auto& task = tasks_[task_id];
auto& robot = robots_[robot_id];
task.state = TaskState::ASSIGNED;
task.assigned_robot = robot_id;
robot.task_queue.push_back(task_id);
// 从待分配列表移除
pending_tasks_.erase(
std::remove(pending_tasks_.begin(), pending_tasks_.end(), task_id),
pending_tasks_.end());
printf("[调度] 任务 #%d 分配给机器人 #%d\n", task_id, robot_id);
}
/**
* 调度算法：选择最优机器人
*/
int selectBestRobot(int task_id) {
if (!tasks_.count(task_id)) return -1;
const auto& task = tasks_[task_id];
int best_robot = -1;
double best_score = -1e9;
for (auto& [robot_id, robot] : robots_) {
// 跳过不可用的机器人
if (robot.state == RobotState::CHARGING ||
robot.state == RobotState::ERROR) {
continue;
}
// 检查容量
if (robot.current_load >= robot.max_capacity) {
continue;
}
// 检查电量
if (robot.battery_level < 20) {
continue;
}
// 计算分数
double score = calculateRobotScore(robot, task);
if (score > best_score) {
best_score = score;
best_robot = robot_id;
}
}
return best_robot;
}
/**
* 批量配送优化：TSP问题
*/
std::vector<int> optimizeDeliveryOrder(const std::vector<int>& task_ids,
const Robot& robot) {
if (task_ids.size() <= 1) return task_ids;
// 简化版TSP：最近邻算法
std::vector<int> order;
std::vector<bool> visited(task_ids.size(), false);
double current_x = robot.x;
double current_y = robot.y;
for (size_t i = 0; i < task_ids.size(); ++i) {
double min_dist = 1e9;
int nearest = -1;
for (size_t j = 0; j < task_ids.size(); ++j) {
if (visited[j]) continue;
const auto& task = tasks_[task_ids[j]];
double dist = std::sqrt(std::pow(task.table_x - current_x, 2) +
std::pow(task.table_y - current_y, 2));
if (dist < min_dist) {
min_dist = dist;
nearest = j;
}
}
if (nearest >= 0) {
visited[nearest] = true;
order.push_back(task_ids[nearest]);
const auto& task = tasks_[task_ids[nearest]];
current_x = task.table_x;
current_y = task.table_y;
}
}
return order;
}
/**
* 主调度循环
*/
void schedulingLoop() {
// 1. 处理待分配任务
for (int task_id : pending_tasks_) {
int robot_id = selectBestRobot(task_id);
if (robot_id >= 0) {
assignTask(task_id, robot_id);
}
}
// 2. 优化各机器人的配送顺序
for (auto& [robot_id, robot] : robots_) {
if (robot.task_queue.size() > 1) {
robot.task_queue = optimizeDeliveryOrder(robot.task_queue, robot);
}
}
// 3. 检查超时任务
auto now = std::chrono::steady_clock::now();
for (auto& [task_id, task] : tasks_) {
if (task.state != TaskState::COMPLETED &&
task.state != TaskState::CANCELLED &&
now > task.deadline) {
printf("[警告] 任务 #%d 即将超时!\n", task_id);
// 提高优先级
task.priority = TaskPriority::URGENT;
}
}
// 4. 检查需要充电的机器人
for (auto& [robot_id, robot] : robots_) {
if (robot.battery_level < 15 && robot.state == RobotState::IDLE) {
printf("[调度] 机器人 #%d 电量低，安排充电\n", robot_id);
robot.state = RobotState::CHARGING;
// 发送充电命令
}
}
}
/**
* 添加位置点
*/
void addLocation(int id, const std::string& name, double x, double y, int type) {
locations_[id] = {id, name, x, y, type};
}
/**
* 添加机器人
*/
void addRobot(int robot_id, int max_capacity = 3) {
Robot robot;
robot.robot_id = robot_id;
robot.state = RobotState::IDLE;
robot.x = 0;
robot.y = 0;
robot.theta = 0;
robot.current_task = -1;
robot.battery_level = 100;
robot.max_capacity = max_capacity;
robot.current_load = 0;
robots_[robot_id] = robot;
}
/**
* 更新任务状态
*/
void updateTaskState(int task_id, TaskState new_state) {
if (tasks_.count(task_id)) {
tasks_[task_id].state = new_state;
printf("[任务] #%d 状态更新: %s\n", task_id, stateToString(new_state).c_str());
}
}
/**
* 更新机器人位置
*/
void updateRobotPose(int robot_id, double x, double y, double theta) {
if (robots_.count(robot_id)) {
robots_[robot_id].x = x;
robots_[robot_id].y = y;
robots_[robot_id].theta = theta;
}
}
private:
/**
* 计算机器人对任务的适合度分数
*/
double calculateRobotScore(const Robot& robot, const DeliveryTask& task) {
double score = 0;
// 1. 距离分数 (越近越好)
double dist_to_pickup = std::sqrt(
std::pow(robot.x - task.pickup_x, 2) +
std::pow(robot.y - task.pickup_y, 2));
score += 10.0 / (1.0 + dist_to_pickup);
// 2. 空闲状态加分
if (robot.state == RobotState::IDLE) {
score += 5;
}
// 3. 当前负载少加分
score += (robot.max_capacity - robot.current_load) * 2;
// 4. 电量充足加分
score += robot.battery_level / 20.0;
// 5. 如果机器人正在送餐且顺路
if (robot.state == RobotState::DELIVERING && !robot.task_queue.empty()) {
// 检查是否顺路
int current_task_id = robot.task_queue[0];
const auto& current_task = tasks_[current_task_id];
// 如果新任务的桌子在当前送餐路径附近
double detour = calculateDetour(robot, current_task, task);
if (detour < 5.0) { // 绕路小于5米
score += 8; // 大幅加分
}
}
return score;
}
/**
* 计算绕路距离
*/
double calculateDetour(const Robot& robot,
const DeliveryTask& current_task,
const DeliveryTask& new_task) {
// 原路线长度
double original = std::sqrt(
std::pow(robot.x - current_task.table_x, 2) +
std::pow(robot.y - current_task.table_y, 2));
// 新路线长度 (先送新任务再送原任务)
double new_route = std::sqrt(
std::pow(robot.x - new_task.table_x, 2) +
std::pow(robot.y - new_task.table_y, 2)) +
std::sqrt(
std::pow(new_task.table_x - current_task.table_x, 2) +
std::pow(new_task.table_y - current_task.table_y, 2));
return new_route - original;
}
std::string stateToString(TaskState state) {
switch (state) {
case TaskState::PENDING: return "待分配";
case TaskState::ASSIGNED: return "已分配";
case TaskState::PICKING: return "取餐中";
case TaskState::DELIVERING: return "送餐中";
case TaskState::ARRIVED: return "已到达";
case TaskState::COMPLETED: return "已完成";
case TaskState::CANCELLED: return "已取消";
default: return "未知";
}
}
int next_task_id_;
std::map<int, DeliveryTask> tasks_;
std::map<int, Robot> robots_;
std::map<int, Location> locations_;
std::vector<int> pending_tasks_;
};五、语义识别与人机交互
┌─────────────────────────────────────────────────────────────────────────────┐
│ 语义识别与交互 │
├─────────────────────────────────────────────────────────────────────────────┤
│ │
│ 【识别什么？】 │
│ ───────────────────────────────────────── │
│ │
│ 1. 桌号识别 │
│ - 二维码/ArUco标签 │
│ - 桌牌文字识别 │
│ - 预存位置匹配 │
│ │
│ 2. 人员检测 │
│ - 行人检测 (避障) │
│ - 工作人员识别 │
│ - 手势识别 (召唤/让路) │
│ │
│ 3. 场景理解 │
│ - 门/电梯识别 │
│ - 楼梯/台阶检测 │
│ - 动态障碍物分类 │
│ │
│ 【ArUco标签识别】 │
│ ───────────────────────────────────────── │
│ │
│ ┌───────────────┐ │
│ │ ■ □ ■ □ ■ │ │
│ │ □ ■ □ ■ □ │ 每个桌子放一个ArUco标签 │
│ │ ■ □ ■ □ ■ │ 编码了桌号信息 │
│ │ □ ■ □ ■ □ │ 可以精确定位(厘米级) │
│ │ ■ □ ■ □ ■ │ │
│ └───────────────┘ │
│ │
│ 【人机交互】 │
│ ───────────────────────────────────────── │
│ │
│ 输入: │
│ - 触摸屏 │
│ - 语音指令 │
│ - 手势 │
│ - 遥控器 │
│ │
│ 输出: │
│ - 语音播报 ("您的餐品到了") │
│ - 显示屏信息 │
│ - 灯光提示 │
│ - 声音提示 │
│ │
└─────────────────────────────────────────────────────────────────────────────┘ArUco标签识别
cpp
/**
* ArUco标签识别
*/
#include <opencv2/opencv.hpp>
#include <opencv2/aruco.hpp>
struct MarkerInfo {
int id;
std::vector<cv::Point2f> corners;
cv::Vec3d rvec, tvec; // 旋转向量和平移向量
double distance; // 距离
double angle; // 相对角度
};
class ArucoDetector {
public:
ArucoDetector() {
// 使用预定义的字典
dictionary_ = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_50);
parameters_ = cv::aruco::DetectorParameters::create();
// 相机内参 (需要标定获取)
camera_matrix_ = (cv::Mat_<double>(3, 3) <<
500, 0, 320,
0, 500, 240,
0, 0, 1);
dist_coeffs_ = cv::Mat::zeros(5, 1, CV_64F);
// 标签实际尺寸 (米)
marker_size_ = 0.1; // 10cm
}
/**
* 检测图像中的ArUco标签
*/
std::vector<MarkerInfo> detect(const cv::Mat& image) {
std::vector<MarkerInfo> results;
std::vector<int> ids;
std::vector<std::vector<cv::Point2f>> corners;
std::vector<std::vector<cv::Point2f>> rejected;
// 检测标签
cv::aruco::detectMarkers(image, dictionary_, corners, ids,
parameters_, rejected);
if (ids.empty()) {
return results;
}
// 估计位姿
std::vector<cv::Vec3d> rvecs, tvecs;
cv::aruco::estimatePoseSingleMarkers(corners, marker_size_,
camera_matrix_, dist_coeffs_,
rvecs, tvecs);
for (size_t i = 0; i < ids.size(); ++i) {
MarkerInfo info;
info.id = ids[i];
info.corners = corners[i];
info.rvec = rvecs[i];
info.tvec = tvecs[i];
// 计算距离
info.distance = cv::norm(tvecs[i]);
// 计算相对角度
info.angle = std::atan2(tvecs[i][0], tvecs[i][2]);
results.push_back(info);
}
return results;
}
/**
* 绘制检测结果
*/
void drawDetection(cv::Mat& image, const std::vector<MarkerInfo>& markers) {
for (const auto& marker : markers) {
// 绘制标签边框
std::vector<std::vector<cv::Point2f>> corners = {marker.corners};
cv::aruco::drawDetectedMarkers(image, corners);
// 绘制坐标轴
cv::aruco::drawAxis(image, camera_matrix_, dist_coeffs_,
marker.rvec, marker.tvec, marker_size_ * 0.5);
// 显示信息
cv::Point2f center = (marker.corners[0] + marker.corners[2]) / 2;
std::string text = "ID:" + std::to_string(marker.id) +
" D:" + std::to_string(int(marker.distance * 100)) + "cm";
cv::putText(image, text, center, cv::FONT_HERSHEY_SIMPLEX,
0.5, cv::Scalar(0, 255, 0), 2);
}
}
/**
* 根据标签ID获取对应的桌号
*/
int getTableFromMarker(int marker_id) {
// 标签ID到桌号的映射
static std::map<int, int> marker_to_table = {
{1, 1}, {2, 2}, {3, 3}, {4, 4}, {5, 5},
{6, 6}, {7, 7}, {8, 8}, {9, 9}, {10, 10}
};
auto it = marker_to_table.find(marker_id);
return (it != marker_to_table.end()) ? it->second : -1;
}
private:
cv::Ptr<cv::aruco::Dictionary> dictionary_;
cv::Ptr<cv::aruco::DetectorParameters> parameters_;
cv::Mat camera_matrix_;
cv::Mat dist_coeffs_;
double marker_size_;
};六、多机器人协调
┌─────────────────────────────────────────────────────────────────────────────┐
│ 多机器人协调 │
├─────────────────────────────────────────────────────────────────────────────┤
│ │
│ 当餐厅有多个机器人时，需要解决: │
│ │
│ 【问题1: 路径冲突】 │
│ ───────────────────────────────────────── │
│ │
│ ┌────────────────────────────────────────┐ │
│ │ │ │
│ │ 🤖A ──────────► │ │
│ │ 交叉点 ◄────────── 🤖B │ │
│ │ × │ │
│ │ │ │
│ └────────────────────────────────────────┘ │
│ │
│ 解决方案: │
│ - 时间窗预约: 每个路径段有时间窗，避免同时占用 │
│ - 优先级: 高优先级机器人先通过 │
│ - 等待/绕行: 检测到冲突时重新规划 │
│ │
│ 【问题2: 死锁】 │
│ ───────────────────────────────────────── │
│ │
│ ┌─────────────────────┐ │
│ │ 🤖A ◄───── 🤖B │ 两个机器人互相等待 │
│ │ │ │ │ │
│ │ └──────────┘ │ │
│ └─────────────────────┘ │
│ │
│ 解决方案: │
│ - 死锁检测: 构建等待图，检测环 │
│ - 死锁预防: 资源编号，按顺序申请 │
│ - 死锁恢复: 让一个机器人后退/绕行 │
│ │
│ 【问题3: 任务分配】 │
│ ───────────────────────────────────────── │
│ │
│ 多个机器人如何分配任务？ │
│ - 拍卖算法: 机器人竞标任务 │
│ - 匈牙利算法: 最优分配 │
│ - 市场机制: 动态调整 │
│ │
└─────────────────────────────────────────────────────────────────────────────┘多机器人协调代码
cpp
/**
* 多机器人协调系统
*/
#include <vector>
#include <map>
#include <set>
#include <queue>
struct PathSegment {
int from_node;
int to_node;
double start_time;
double end_time;
int robot_id;
};
class MultiRobotCoordinator {
public:
/**
* 检测路径冲突
*/
bool checkConflict(const PathSegment& seg1, const PathSegment& seg2) {
// 检查是否在同一路段
if ((seg1.from_node == seg2.from_node && seg1.to_node == seg2.to_node) ||
(seg1.from_node == seg2.to_node && seg1.to_node == seg2.from_node)) {
// 检查时间是否重叠
return !(seg1.end_time < seg2.start_time || seg2.end_time < seg1.start_time);
}
// 检查节点冲突 (同时到达同一节点)
if (seg1.to_node == seg2.to_node) {
double arrival_diff = std::abs(seg1.end_time - seg2.end_time);
return arrival_diff < 2.0; // 2秒内到达视为冲突
}
return false;
}
/**
* 解决冲突：调整时间窗
*/
void resolveConflict(PathSegment& seg1, PathSegment& seg2) {
// 优先级高的先通过
if (getPriority(seg1.robot_id) >= getPriority(seg2.robot_id)) {
// seg2延迟
double delay = seg1.end_time - seg2.start_time + 2.0;
seg2.start_time += delay;
seg2.end_time += delay;
} else {
// seg1延迟
double delay = seg2.end_time - seg1.start_time + 2.0;
seg1.start_time += delay;
seg1.end_time += delay;
}
}
/**
* 死锁检测
*/
bool detectDeadlock(const std::map<int, int>& wait_for) {
// wait_for: robot_id -> waiting_for_robot_id
std::set<int> visited;
for (auto& [robot_id, _] : wait_for) {
std::set<int> path;
int current = robot_id;
while (wait_for.count(current) && !path.count(current)) {
path.insert(current);
current = wait_for.at(current);
}
if (path.count(current)) {
// 发现环，存在死锁
printf("[警告] 检测到死锁: 机器人 %d\n", current);
return true;
}
}
return false;
}
/**
* 匈牙利算法：最优任务分配
*/
std::vector<std::pair<int, int>> hungarianAssignment(
const std::vector<std::vector<double>>& cost_matrix) {
int n = cost_matrix.size();
if (n == 0) return {};
int m = cost_matrix[0].size();
// 简化版：贪心匹配
std::vector<std::pair<int, int>> assignment;
std::vector<bool> robot_assigned(n, false);
std::vector<bool> task_assigned(m, false);
// 创建成本优先队列
struct Assignment {
int robot, task;
double cost;
bool operator>(const Assignment& other) const {
return cost > other.cost;
}
};
std::priority_queue<Assignment, std::vector<Assignment>, std::greater<Assignment>> pq;
for (int i = 0; i < n; ++i) {
for (int j = 0; j < m; ++j) {
pq.push({i, j, cost_matrix[i][j]});
}
}
while (!pq.empty() && assignment.size() < std::min(n, m)) {
auto [robot, task, cost] = pq.top();
pq.pop();
if (!robot_assigned[robot] && !task_assigned[task]) {
assignment.push_back({robot, task});
robot_assigned[robot] = true;
task_assigned[task] = true;
}
}
return assignment;
}
/**
* 避让策略：计算避让点
*/
std::pair<double, double> calculateYieldPoint(
int robot_id,
double rx, double ry, // 当前位置
double ox, double oy, // 另一机器人位置
double gx, double gy) { // 目标位置
// 计算垂直于目标方向的偏移
double dx = gx - rx;
double dy = gy - ry;
double len = std::sqrt(dx * dx + dy * dy);
if (len < 0.1) return {rx, ry};
// 归一化
dx /= len;
dy /= len;
// 垂直方向
double px = -dy;
double py = dx;
// 选择远离另一机器人的方向
double side = px * (rx - ox) + py * (ry - oy);
if (side < 0) {
px = -px;
py = -py;
}
// 偏移1米
return {rx + px * 1.0, ry + py * 1.0};
}
private:
int getPriority(int robot_id) {
// 可以根据任务紧急程度、电量等返回优先级
return robot_id; // 简化：ID小的优先
}
std::map<int, std::vector<PathSegment>> robot_paths_;
};完整系统集成
cpp
/**
* 送餐机器人主控制器
*/
#include <thread>
#include <atomic>
#include <mutex>
class DeliveryRobotController {
public:
DeliveryRobotController(int robot_id) : robot_id_(robot_id) {
running_ = false;
}
void initialize() {
// 初始化各模块
printf("[Robot %d] 初始化中...\n", robot_id_);
// 加载地图
loadMap("restaurant_map.yaml");
// 初始化定位
localization_.initialize(0, 0, 0); // 初始位置
// 初始化规划器
global_planner_ = std::make_unique<AStarPlanner>(map_);
local_planner_ = std::make_unique<DWAPlanner>(DWAConfig());
// 连接后台调度系统
connectToScheduler();
printf("[Robot %d] 初始化完成\n", robot_id_);
}
void start() {
running_ = true;
// 启动各处理线程
localization_thread_ = std::thread(&DeliveryRobotController::localizationLoop, this);
planning_thread_ = std::thread(&DeliveryRobotController::planningLoop, this);
control_thread_ = std::thread(&DeliveryRobotController::controlLoop, this);
printf("[Robot %d] 启动完成，等待任务...\n", robot_id_);
}
void stop() {
running_ = false;
if (localization_thread_.joinable()) localization_thread_.join();
if (planning_thread_.joinable()) planning_thread_.join();
if (control_thread_.joinable()) control_thread_.join();
}
/**
* 接收新任务
*/
void onNewTask(const DeliveryTask& task) {
std::lock_guard<std::mutex> lock(task_mutex_);
current_task_ = task;
has_task_ = true;
printf("[Robot %d] 收到任务: 送到 %d 号桌\n", robot_id_, task.table_id);
// 规划全局路径
planGlobalPath();
}
private:
/**
* 定位循环
*/
void localizationLoop() {
while (running_) {
// 1. 获取传感器数据
auto laser_scan = getLaserScan();
auto odom = getOdometry();
// 2. 运动更新
localization_.motionUpdate(odom.dx, odom.dy, odom.dtheta);
// 3. 观测更新
localization_.sensorUpdate(laser_scan, map_);
// 4. 重采样
localization_.resample();
// 5. 获取位姿估计
{
std::lock_guard<std::mutex> lock(pose_mutex_);
current_pose_ = localization_.getEstimatedPose();
}
std::this_thread::sleep_for(std::chrono::milliseconds(50)); // 20Hz
}
}
/**
* 规划循环
*/
void planningLoop() {
while (running_) {
if (!has_task_) {
std::this_thread::sleep_for(std::chrono::milliseconds(100));
continue;
}
// 获取当前位姿
Pose2D pose;
{
std::lock_guard<std::mutex> lock(pose_mutex_);
pose = current_pose_;
}
// 获取目标
Pose2D goal;
{
std::lock_guard<std::mutex> lock(task_mutex_);
goal.x = current_task_.table_x;
goal.y = current_task_.table_y;
}
// 检查是否到达
double dist = std::sqrt(std::pow(pose.x - goal.x, 2) +
std::pow(pose.y - goal.y, 2));
if (dist < 0.5) {
onArrived();
continue;
}
// 获取障碍物
auto obstacles = detectObstacles();
// 局部规划
Velocity cmd = local_planner_->computeVelocity(
{pose.x, pose.y, pose.theta, current_velocity_.v, current_velocity_.omega},
goal,
global_path_,
obstacles);
// 发送速度命令
{
std::lock_guard<std::mutex> lock(cmd_mutex_);
cmd_velocity_ = cmd;
}
std::this_thread::sleep_for(std::chrono::milliseconds(100)); // 10Hz
}
}
/**
* 控制循环
*/
void controlLoop() {
while (running_) {
Velocity cmd;
{
std::lock_guard<std::mutex> lock(cmd_mutex_);
cmd = cmd_velocity_;
}
// 发送到电机
sendVelocityCommand(cmd.v, cmd.omega);
std::this_thread::sleep_for(std::chrono::milliseconds(20)); // 50Hz
}
}
/**
* 规划全局路径
*/
void planGlobalPath() {
Pose2D pose;
{
std::lock_guard<std::mutex> lock(pose_mutex_);
pose = current_pose_;
}
DeliveryTask task;
{
std::lock_guard<std::mutex> lock(task_mutex_);
task = current_task_;
}
GridCell start = map_.worldToGrid(pose.x, pose.y);
GridCell goal = map_.worldToGrid(task.table_x, task.table_y);
auto grid_path = global_planner_->planPath(start, goal);
// 平滑路径
global_path_ = PathSmoother::smoothBSpline(grid_path, map_);
printf("[Robot %d] 规划路径: %zu 个路点\n", robot_id_, global_path_.size());
}
/**
* 到达目标
*/
void onArrived() {
printf("[Robot %d] 到达目标桌位\n", robot_id_);
// 播放语音
playVoice("您的餐品到了，请取餐");
// 等待取餐确认
waitForConfirmation();
// 通知后台
notifyTaskCompleted();
has_task_ = false;
}
// 辅助函数 (实际实现需要对接硬件)
void loadMap(const std::string& path) {
// 加载地图文件
map_ = OccupancyGrid(200, 200, 0.05); // 10m x 10m
}
LaserScan getLaserScan() { return {}; }
struct Odom { double dx, dy, dtheta; };
Odom getOdometry() { return {0, 0, 0}; }
std::vector<std::pair<double, double>> detectObstacles() { return {}; }
void sendVelocityCommand(double v, double omega) {}
void connectToScheduler() {}
void playVoice(const std::string& text) {}
void waitForConfirmation() {}
void notifyTaskCompleted() {}
int robot_id_;
std::atomic<bool> running_;
OccupancyGrid map_;
ParticleFilter localization_;
std::unique_ptr<AStarPlanner> global_planner_;
std::unique_ptr<DWAPlanner> local_planner_;
Pose2D current_pose_;
std::mutex pose_mutex_;
DeliveryTask current_task_;
bool has_task_ = false;
std::mutex task_mutex_;
std::vector<Pose2D> global_path_;
Velocity current_velocity_;
Velocity cmd_velocity_;
std::mutex cmd_mutex_;
std::thread localization_thread_;
std::thread planning_thread_;
std::thread control_thread_;
};
int main() {
DeliveryRobotController robot(1);
robot.initialize();
robot.start();
// 模拟收到任务
DeliveryTask task;
task.task_id = 1;
task.table_id = 5;
task.table_x = 8.0;
task.table_y = 6.0;
robot.onNewTask(task);
// 运行一段时间
std::this_thread::sleep_for(std::chrono::seconds(60));
robot.stop();
return 0;
}总结
┌─────────────────────────────────────────────────────────────────────────────┐
│ 送餐机器人技术栈总结 │
├─────────────────────────────────────────────────────────────────────────────┤
│ │
│ 【核心技术模块】 │
│ ───────────────────────────────────────── │
│ │
│ 1. 定位 (SLAM/AMCL) │
│ - 激光雷达 + 粒子滤波 │
│ - 精度: 厘米级 │
│ │
│ 2. 全局路径规划 (A*/D*) │
│ - 基于栅格地图 │
│ - 考虑静态障碍物 │
│ │
│ 3. 局部路径规划 (DWA/TEB) │
│ - 实时避障 │
│ - 速度空间采样 │
│ │
│ 4. 任务调度 │
│ - 多任务排序 (TSP) │
│ - 优先级管理 │
│ - 批量配送优化 │
│ │
│ 5. 语义识别 │
│ - ArUco标签识别 │
│ - 行人检测 │
│ │
│ 6. 多机协调 │
│ - 路径冲突检测 │
│ - 死锁预防 │
│ - 任务分配优化 │
│ │
│ 【常用开源框架】 │
│ ───────────────────────────────────────── │
│ │
│ - ROS/ROS2: 机器人操作系统 │
│ - Navigation2: 导航栈 │
│ - SLAM Toolbox: SLAM算法 │
│ - OpenCV: 视觉处理 │
│ - PCL: 点云处理 │
│ │
│ 【硬件配置参考】 │
│ ───────────────────────────────────────── │
│ │
│ - 激光雷达: 思岚A1/A2, Hokuyo │
│ - 深度相机: Intel RealSense, Orbbec │
│ - 主控: Jetson Nano/Xavier, RK3588 │
│ - 底盘: 差速驱动, 麦克纳姆轮 │
│ - 电池: 24V锂电池, 续航4-8小时 │
│ │
└─────────────────────────────────────────────────────────────────────────────┘一句话总结：
送餐机器人 = SLAM定位 + A*全局规划 + DWA局部避障 + 任务调度 + 多机协调。技术成熟，ROS有现成框架，核心难点在于复杂环境的鲁棒性和多机协调效率。
参考资料：
- ROS Navigation2文档
- 《概率机器人》(Probabilistic Robotics)
- SLAM Toolbox
- 普渡机器人技术方案