First_Dev_Dept/ParkingRobot
10
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

泛化轨迹工厂参数并新增组合运动计划执行器与复合测试

Browse Source
This commit is contained in:
yuxiang.shen
2026-08-07 13:25:47 +08:00
parent 14ca1150e4
commit bc37e71ad0
26 changed files with 874 additions and 92 deletions
Download Patch File Download Diff File Expand all files Collapse all files
data_process/新版控制器轨迹测试处理/直线测试/第一组4m测试/记录.txt
+18
View File
@@ -0,0 +1,18 @@
: * (Exception):DriveTask failed, msg=车辆已在终点零速参考处停稳，但终点精度不满足要求：位置误差=0.063m，航向误差=3.91°。, stack:
at ClumsyCore.DriveTask.Wait() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 205
at MultiWheelC.NewControllerStraight4mTest.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\NewControllerTrackingTests.cs:line 146
at ClumsyLite.ClumsyLiteUI.<>c__DisplayClass19_1.<MainPanelHandler>b__21() in D:\MDCS\Source\Core\Clumsy\ClumsyLite\ClumsyLiteUI.cs:line 592
*p.InnerException * (InvalidOperationException):车辆已在终点零速参考处停稳，但终点精度不满足要求：位置误差=0.063m，航向误差=3.91°。, stack:
at MultiWheelC.TrajectoryTrackingMovement.Get()+MoveNext() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\TrajectoryTrackingMovement.cs:line 237
at ClumsyCore.DriveTask.<>c__DisplayClass9_0.<.ctor>b__2() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 112
当前 Stanley 参数与车辆转向动态不匹配，形成了低频蛇形振荡。
数据很有代表性：
横向误差 RMSE：约 12.3 mm
航向误差 RMSE：约 1.98°
航向误差范围：约 -3.24°～+4.82°
角速度命令共明显换向约5次，不是 Detour 高频噪声
Stanley 目标角与经过 GCP 限速后实际发送角的平均差仅约 0.26°
所以确实是“中心贴线，但车身左右摆”。
data_process/新版控制器轨迹测试处理/直线测试/第三组4m测试/记录.txt
+15
View File
@@ -0,0 +1,15 @@
误差主要集中在轨迹 s=0.7～2.3m：
该区间：
横向RMSE约22.4mm
航向RMSE约1.91°
s>2.5m以后：
横向RMSE约4.58mm
航向RMSE约0.57°
也就是说，第三组不是持续蛇形，而是在中途发生了一次：
车辆向一侧偏移
→ 控制器给出修正
→ 实际角速度响应滞后
→ 修正稍微过头
→ 随后重新稳定
第三组终点成功、后半段精度很好，说明控制器没有结构性错误。纵向 PID 也暂时不用调整。
data_process/新版控制器轨迹测试处理/直线测试/第二组4m测试/记录.txt
+22
View File
@@ -0,0 +1,22 @@
: * (Exception):DriveTask failed, msg=车辆已在终点零速参考处停稳，但终点精度不满足要求：位置误差=0.048m，航向误差=0.08°。, stack:
at ClumsyCore.DriveTask.Wait() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 205
at MultiWheelC.NewControllerStraight4mTest.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\NewControllerTrackingTests.cs:line 146
at ClumsyLite.ClumsyLiteUI.<>c__DisplayClass19_1.<MainPanelHandler>b__21() in D:\MDCS\Source\Core\Clumsy\ClumsyLite\ClumsyLiteUI.cs:line 592
*p.InnerException * (InvalidOperationException):车辆已在终点零速参考处停稳，但终点精度不满足要求：位置误差=0.048m，航向误差=0.08°。, stack:
at MultiWheelC.TrajectoryTrackingMovement.Get()+MoveNext() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\TrajectoryTrackingMovement.cs:line 237
at ClumsyCore.DriveTask.<>c__DisplayClass9_0.<.ctor>b__2() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 112
第二组在终点前的情况是：
距离终点约97mm：
参考速度 0.197m/s
命令速度 0.135m/s
实际速度约0.320m/s
距离终点约12mm：
参考速度 0.053m/s
命令速度已经为0
实际速度仍约0.286m/s
说明控制器已经要求停车，但车辆和 M 层速度斜坡来不及完全降速，最终越过终点约48 mm。
这不是 Stanley 横向控制问题，而是参考速度规划的减速距离不够。
data_process/新版控制器轨迹测试处理/直线测试/第五组4m测试3次/记录.txt
+15
View File
@@ -0,0 +1,15 @@
第一次：
: * (Exception):DriveTask failed, msg=车辆已在终点零速参考处停稳，但终点精度不满足要求：位置误差=0.034m，航向误差=0.43°。, stack:
at ClumsyCore.DriveTask.Wait() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 205
at MultiWheelC.NewControllerStraight4mTest.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\NewControllerTrackingTests.cs:line 146
at ClumsyLite.ClumsyLiteUI.<>c__DisplayClass19_1.<MainPanelHandler>b__21() in D:\MDCS\Source\Core\Clumsy\ClumsyLite\ClumsyLiteUI.cs:line 592
*p.InnerException * (InvalidOperationException):车辆已在终点零速参考处停稳，但终点精度不满足要求：位置误差=0.034m，航向误差=0.43°。, stack:
at MultiWheelC.TrajectoryTrackingMovement.Get()+MoveNext() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\TrajectoryTrackingMovement.cs:line 237
at ClumsyCore.DriveTask.<>c__DisplayClass9_0.<.ctor>b__2() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 112
第二次：
第三次：
data_process/新版控制器轨迹测试处理/直线测试/第四组4m测试3次/记录.txt
+57
View File
@@ -0,0 +1,57 @@
第一次：
: * (Exception):DriveTask failed, msg=车辆已在终点零速参考处停稳，但终点精度不满足要求：位置误差=0.045m，航向误差=0.51°。, stack:
at ClumsyCore.DriveTask.Wait() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 205
at MultiWheelC.NewControllerStraight4mTest.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\NewControllerTrackingTests.cs:line 146
at ClumsyLite.ClumsyLiteUI.<>c__DisplayClass19_1.<MainPanelHandler>b__21() in D:\MDCS\Source\Core\Clumsy\ClumsyLite\ClumsyLiteUI.cs:line 592
*p.InnerException * (InvalidOperationException):车辆已在终点零速参考处停稳，但终点精度不满足要求：位置误差=0.045m，航向误差=0.51°。, stack:
at MultiWheelC.TrajectoryTrackingMovement.Get()+MoveNext() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\TrajectoryTrackingMovement.cs:line 237
at ClumsyCore.DriveTask.<>c__DisplayClass9_0.<.ctor>b__2() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 112
第二次：
: * (Exception):DriveTask failed, msg=车辆已在终点零速参考处停稳，但终点精度不满足要求：位置误差=0.032m，航向误差=0.48°。, stack:
at ClumsyCore.DriveTask.Wait() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 205
at MultiWheelC.NewControllerStraight4mTest.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\NewControllerTrackingTests.cs:line 146
at ClumsyLite.ClumsyLiteUI.<>c__DisplayClass19_1.<MainPanelHandler>b__21() in D:\MDCS\Source\Core\Clumsy\ClumsyLite\ClumsyLiteUI.cs:line 592
*p.InnerException * (InvalidOperationException):车辆已在终点零速参考处停稳，但终点精度不满足要求：位置误差=0.032m，航向误差=0.48°。, stack:
at MultiWheelC.TrajectoryTrackingMovement.Get()+MoveNext() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\TrajectoryTrackingMovement.cs:line 237
at ClumsyCore.DriveTask.<>c__DisplayClass9_0.<.ctor>b__2() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 112
第三次：
: * (Exception):DriveTask failed, msg=车辆已在终点零速参考处停稳，但终点精度不满足要求：位置误差=0.041m，航向误差=0.07°。, stack:
at ClumsyCore.DriveTask.Wait() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 205
at MultiWheelC.NewControllerStraight4mTest.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\NewControllerTrackingTests.cs:line 146
at ClumsyLite.ClumsyLiteUI.<>c__DisplayClass19_1.<MainPanelHandler>b__21() in D:\MDCS\Source\Core\Clumsy\ClumsyLite\ClumsyLiteUI.cs:line 592
*p.InnerException * (InvalidOperationException):车辆已在终点零速参考处停稳，但终点精度不满足要求：位置误差=0.041m，航向误差=0.07°。, stack:
at MultiWheelC.TrajectoryTrackingMovement.Get()+MoveNext() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\TrajectoryTrackingMovement.cs:line 237
at ClumsyCore.DriveTask.<>c__DisplayClass9_0.<.ctor>b__2() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 112
次数 横向 RMSE 航向 RMSE 速度 RMSE 终点误差
第1次 19.70 mm 1.42° 0.0492 m/s 45.2 mm
第2次 6.08 mm 0.45° 0.0515 m/s 31.7 mm
第3次 8.66 mm 0.72° 0.0540 m/s 40.7 mm
第二、三次横向效果不错；第一次数值较差。
第一次变差的主要原因
第一次在：
t ≈ 9.74s
s ≈ 2.22m
出现了一次约 33.8mm 的 Detour 横向位置突变。突变后的位置持续保持在新的坐标基准上，不是单帧尖峰。
控制器随后正常纠偏，但由于车辆转向响应存在滞后，形成了一次明显摆动：
Detour横向位置突然变化
→ Stanley认为车辆偏离约30mm
→ 给出较大转向修正
→ 实际角速度滞后
→ 航向和横向误差产生一次超调
因此第一次不是 Stanley 自己无缘无故发散，而是定位突变触发了欠阻尼响应。