introduction
约 2294 字
一、总体概述
ChipSolver 是 RoboCup SSL(小型组)视觉系统中的挑球(Chip Kick)轨迹预测模块。当机器人执行挑球动作时,球会在空中飞行并多次弹跳。SSL-Vision 相机只能输出球的 2D 地面投影坐标,而挑球在空中时这个投影坐标会因透视效应偏离真实位置。ChipSolver 的核心任务是:根据一系列带有透视偏差的 2D 观测数据,反推出踢球速度(含 vz 竖直分量)、踢球位置和多跳轨迹,从而预测球的落点、当前空中位置和飞行时间。
整个系统参考了 TIGERs Mannheim 开源框架 Sumatra 的 Java 实现(ChipKickEstimator.java 等),用 C++ 重写,使用 Eigen 做线性代数、NLopt 做非线性优化。
二、文件逐个解析
1. ballparameters.h — 球物理参数
定义 BallParameters 结构体,包含挑球轨迹建模所需的所有物理常数:
| 参数 | 默认值 | 含义 |
|---|---|---|
ballRadius | 21.5 mm | 球半径 |
accSlide | -3000 mm/s² | 滑动摩擦减速度 |
accRoll | -260 mm/s² | 滚动摩擦减速度 |
chipDampingXYFirstHop | 0.8 | 首跳弹起后 XY 速度保留比 |
chipDampingXYOtherHops | 0.85 | 后续跳弹起后 XY 速度保留比 |
chipDampingZ | 0.47 | Z 方向弹起后速度保留比 |
minHopHeight | 10 mm | 低于此高度认为球不再弹跳 |
maxInterceptableHeight | 150 mm | 机器人可拦截的最大高度 |
所有参数通过 ZParamManager 从 zss.ini 配置文件的 chipball/* 键加载,运行时可调。
2. camcalibration.h — 相机标定
定义 CamCalibration 结构体,核心输出是 cameraWorldPos(相机在世界坐标系中的 3D 位置),这是投影校正的关键。
标定参数详解:
| 参数 | 含义 |
|---|---|
cameraId | 相机编号(0, 1, 2, ...) |
focalLength | 焦距(像素单位),决定成像缩放比 |
distortion | 径向畸变系数 |
principalPointX/Y | 主点坐标(图像中心在像素坐标中的偏移) |
q0, q1, q2, q3 | 旋转四元数,描述相机坐标系相对于世界坐标系的旋转 |
tx, ty, tz | 平移向量,从世界坐标到相机坐标的平移 |
cameraWorldPos | 相机在世界坐标系中的 3D 位置(最终输出) |
相机世界坐标的计算算法:
相机外参模型为:
vcam=R⋅vworld+t
相机本身在相机坐标系中位于原点 (0,0,0),因此:
0=R⋅pworld+t⟹pworld=RT⋅(−t)
代码中从四元数构建旋转矩阵 R:
R=1−2(q22+q32)2(q1q2+q0q3)2(q1q3−q0q2)2(q1q2−q0q3)1−2(q12+q32)2(q2q3+q0q1)2(q1q3+q0q2)2(q2q3−q0q1)1−2(q12+q22)
然后计算 RT⋅(−t):
wx=−(r00tx+r10ty+r20tz)
wy=−(r01tx+r11ty+r21tz)
wz=−(r02tx+r12ty+r22tz)
数据来源: fromProto() 从 SSL-Vision 的 SSL_GeometryCameraCalibration protobuf 消息解析。优先使用 derived_camera_world_* 字段(SSL-Vision 已计算好的世界坐标),否则通过上述公式手动反算。
仿真模式: setDefaultForSim() 设置相机为正对地面的恒等旋转,位于指定 XY 中心上方 4000mm 高处。
3. projection.h — 透视投影校正
ChipProjection 命名空间提供两个核心函数:
projectToGround:模拟 SSL-Vision 的透视投影过程。当球在空中时,相机看到的不是球的真实 XY 坐标,而是球心到相机的连线延伸到地面的交点。
投影公式推导(基于相似三角形):
相机位于 f=(fx,fy,fz),球在 b=(bx,by,bz),地面投影点 g=(gx,gy,0)。 三点共线,参数方程 g=f+λ(b−f),令 z=0:
0=fz+λ(bz−fz)⟹λ=fz−bzfz
代入 x、y:
gx=fx+fz−bzfz(bx−fx),gy=fy+fz−bzfz(by−fy)
projectionOffset:计算投影偏移量 (gx−bx,gy−by),即球在空中时观测位置偏离真实 XY 的量。
4. ichipkicksolver.h — 求解器抽象接口
定义:
KickSolverResult:求解结果,包含踢球 2D 位置、3D 速度(含 vz)、时间戳和求解器名称。IChipKickSolver:抽象基类,所有求解器的统一接口。solve()接收一组BallRecord(球观测),返回KickSolverResult。内部维护各相机 3D 世界坐标。
5. lin3offset.h — 3 参数线性求解器
求解目标:已知踢球位置 pkick,求踢球速度 x=[vx,vy,vz]T
物理模型(第一跳抛物运动):
p(t)=pkick+vkick⋅t+21(0,0,−g)t2
球的 3D 位置经透视投影后得到观测值 g(t)=projectToGround(p(t),f)。
将投影公式代入并整理为线性方程 Ax=b,每个观测贡献 2 行(x 和 y 分量):
A2i=[fzt0(gx−fx)t]
b2i=21g⋅t2(gx−fx)+gxfz−pkick,xfz
A2i+1=[0fzt(gy−fy)t]
b2i+1=21g⋅t2(gy−fy)+gyfz−pkick,yfz
用 Eigen 的 QR 分解 (ColPivHouseholderQR) 求解。
时间偏移搜索:踢球的精确时刻未知,引入偏移 tOffset 加在时间上。通过二分搜索(初始 0.05s,步进每次减半直到精度 $< 10^{-3}$s)来最小化 L1 残差。
后处理:验证 vz>0(确认是挑球)、速度不太小,并用完整的 projectToGround 重新计算 L1 误差。
6. lin5offset.h — 5 参数线性求解器
求解目标:x=[px,py,vx,vy,vz]T(同时估计踢球位置和速度)
适用条件:hasMultipleCameras(records) == true(观测来自多个摄像头),因为单相机数据无法区分踢球位置和速度(欠定)。
矩阵结构扩展为 5 列:
A2i=[fz0fzt0(gx−fx)t]
A2i+1=[0fz0fzt(gy−fy)t]
其余求解流程与 Lin3Offset 完全一致(QR 分解 + 二分搜索时间偏移)。
7. nonlin3direct.h — 非线性优化求解器
求解目标:[vx,vy,vz](与 Lin3 相同)
区别:使用完整的 ChipBallTrajectory 多跳轨迹模型而非仅第一跳抛物线,因此能处理球跨越多次弹跳的数据。
优化算法:NLopt 的 Nelder-Mead 单纯形法(NLOPT_LN_NELDERMEAD),无需梯度。
流程:
- 从 Lin3Offset 获取初始猜测值
setInitialGuess() - 构建
ChipBallTrajectory对象,对每条观测计算模拟位置 - 将模拟位置
projectToGround()后与观测的欧氏距离累加作为目标函数 - 搜索范围:vx,vy∈[−6500,6500],vz∈[0,6500] mm/s
- 最大评估次数 100,相对容差 10−4
8. chipballtrajectory.h — 多跳轨迹模型
建模挑球从踢出到静止的完整轨迹。核心模型:
弹跳阶段(循环直到最大高度 <minHopHeight):
- 每跳飞行时间:tfly=g2vz(g=9810 mm/s²)
- 空中位置(抛物线):z(t)=vzt−21gt2
- 每跳落地后应用阻尼:
vx′=vx⋅dxy,vy′=vy⋅dxy,vz′=∣vz∣⋅dz
其中 dxy 首跳为 0.8、后续为 0.85,dz=0.47
- 最大弹跳高度判断:hmax=2gvz2,当 hmax<10 mm 时退出弹跳
滚动阶段:
- Z 速度归零,XY 按
accRoll(-260 mm/s²)减速 - 停止时间:tstop=−∣vxy∣/aroll
fromState() 静态工厂方法:从当前空中某状态反推踢球初始条件。利用运动学方程:
vz,kick=vz2+2g⋅pz
三、主控逻辑 — chipsolver.h / chipsolver.cpp
Chipsolver 是 Singleton,在独立线程中由 chipsolverEvent 信号驱动。
多阶段求解策略(chipsolver.cpp):
- 阶段 1:始终运行
Lin3Offset(已知踢球位置的 3 参数拟合);如有多相机数据,同时运行Lin5Offset,取 L1 误差更小者 - 阶段 2:当观测时间跨度 > 第一跳飞行时间 g2vz 且记录数 ≥12 时,用线性结果作为初值启动
NonLin3Direct - 非线性结果须满足
errNL < bestLinError * 1.5才被采用
数据流:
SSL-Vision → visionmodule.cpp → CamCalibration.fromProto() (存入 GlobalData)
→ chipsolverEvent.Signal()
→ Chipsolver::dealresult()
├─ setrecord() ← 采集 BallRecord
├─ updateCameraPositions()
├─ collectRecords() + pruneRecords()
├─ computeFitResult()
│ ├─ Lin3Offset.solve()
│ ├─ Lin5Offset.solve()
│ └─ NonLin3Direct.solve()
└─ updatePredictions()
└─ ChipBallTrajectory → 落点/当前位置/飞行时间
↓
ballstate.cpp → vision->ball[0]验证
- 单元测试:TestChipSolver.lua
- 调试可视化:
drawDebugMsg()在场上绘制踢球点(绿色)、第一落点(红色)、第二落点(橙色)、所有弹跳点(黄色)和 L1 误差 / 飞行时间数值
关键配置(zss.ini 的 [chipsolver] 节):
| 参数 | 默认 | 含义 |
|---|---|---|
minRecordNum | 8 | 开始求解最少记录数 |
maxRecordNum | 50 | 记录上限(超过则精简) |
maxFailures | 12 | 连续失败超过此数则重置 |
maxFittingError | 100.0 mm | 平均 L1 误差阈值 |
debug_flag | true | 是否绘制调试信息 |
四、核心算法总结
| 算法 | 数学方法 | 适用场景 | 依赖 |
|---|---|---|---|
| 投影校正 | 相似三角形几何投影 | 所有求解器的基础 | 相机 3D 坐标 |
| Lin3Offset | QR 分解 + 二分时间搜索 | 第一跳,单/多相机,已知踢球位置 | Eigen |
| Lin5Offset | QR 分解 + 二分时间搜索 | 第一跳,多相机,未知踢球位置 | Eigen |
| NonLin3Direct | Nelder-Mead 单纯形优化 | 跨多跳的长时间观测 | NLopt + 多跳轨迹模型 |
| 多跳轨迹模型 | 抛物运动 + 阻尼弹跳 + 滚动摩擦 | 预测完整轨迹 | BallParameters |
| 相机标定 | 四元数→旋转矩阵→逆变换 | 获取相机世界坐标 | SSL-Vision protobuf |
贡献者
更新日志
66dbc-chipsolver StaticGeiBallV2 WBack于