桃园套袋机器人
履带底盘沿树行跟踪路径、逐树停车；车体到达停车点后，机械臂带着套袋末端伸进树冠，接近被枝叶遮挡的桃子完成套袋。硬枝不能碰，叶片可以推开；纯位置控制一旦接触就会刮伤枝叶、碰落果实。
98.0%
实车套袋成功率（441 / 450）
0%
伤果率，树冠内导纳柔顺接近
≈ 6s
单果套袋耗时
15 × 30
果园实车测试：批次 × 目标幼果
作业流程
- 障碍观测语义点云：硬 / 软 / 桃
- 粗定位Fast Path，失败回退 MTC
- 精定位沿枝轴扇形采样 + IK 筛选
- 最终接近语义风险 ABIT* 规划 + 末端导纳柔顺
- 套袋笛卡尔直插 + IO 控制末端
方法
- 末端导纳控制：刚度与叶面法向对齐的各向异性设计，接触枝叶时柔顺让位
- RGB-D 语义分割构建双通道体素场：树干是不可碰障碍，树叶是可穿过的风险代价，接入 ABIT* 规划
- 粗定位与最终接近都有规划失败时的 MTC 回退
技术难点
- 树冠内作业无法完全避开枝叶，需要在接触中保护树体
- 桃子位姿与枝轴约束下，套袋姿态可行域很窄
- 双相机标定与 map → base_link 坐标统一
我的实现
- 底盘 Frenet MPC 路径跟踪器（C++ / OSQP），替换原 Pure Pursuit 内核
ur10e_peachROS1 主控节点（C++）与车体–机械臂通信协议- 末端导纳控制器设计、数值稳定性分析与实机部署
- 分割全链路：数据采集 → SegFormer-B2 / UNet 训练 → ONNX → ROS1 节点
| 果园实车测试 | 15 批次 × 30 个目标幼果，成功套袋 441 个，成功率 98.0% |
|---|---|
| 作业质量 | 伤果率 0%，单果套袋约 6 s |
| 硬障碍分割 | 像素召回 90.6%，逐图召回 84.4%，ONNX 部署 |
| 语义风险规划 | ABIT* 代价加入语义风险积分；相比把树叶也当障碍，成功率 50%–69% → 100%；较无语义基线沿途风险降低 26% |
| 柔性接触 | 按叶面法向分配刚度矩阵，10 N 死区下稳态接触力降低 55%，同等接触力下沿叶面偏移降至 1/10 |
实机视频待放入：assets/video/orchard_approach.mp4
底盘路径跟踪 · Frenet MPC
- 误差模型横向偏差 / 航向误差线性化
- 曲率前馈未来 2 s 曲率剖面
- QPOSQP，82 变量 / 120 约束
- 实时求解10 Hz 重解 / 50 Hz 控制
- v, ωCAN 下发履带底盘
| 模型 | Frenet 系下以横向偏差、航向误差为状态建立线性化误差模型，未来 2 s（20 步）曲率剖面作前馈 |
|---|---|
| 约束 | 线速度、角速度幅值与 Δu 都作为 QP 内硬约束 |
| 实时性 | Hessian 预构建 + 稀疏约束矩阵热更新 + OSQP 热启动 |
| 终点位置误差 | 与 Pure Pursuit 同路径田测对比：0.120 → 0.055 m（−54%） |
| 跟踪与效率 | 横向误差 p95 −11%，平均车速 +10% |
