泛化轨迹工厂参数并新增组合运动计划执行器与复合测试
This commit is contained in:
@@ -0,0 +1,10 @@
|
||||
: * (Exception):DriveTask failed, msg=车辆已在终点零速参考处停稳，但终点精度不满足要求：位置误差=0.033m，航向误差=0.19°。, stack:
|
||||
at ClumsyCore.DriveTask.Wait() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 205
|
||||
at MultiWheelC.CompositeStopTurnGoTest.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\CompositeMotionPlanTests.cs:line 180
|
||||
at ClumsyLite.ClumsyLiteUI.<>c__DisplayClass19_1.<MainPanelHandler>b__21() in D:\MDCS\Source\Core\Clumsy\ClumsyLite\ClumsyLiteUI.cs:line 592
|
||||
|
||||
*p.InnerException * (InvalidOperationException):车辆已在终点零速参考处停稳，但终点精度不满足要求：位置误差=0.033m，航向误差=0.19°。, stack:
|
||||
at MultiWheelC.TrajectoryTrackingMovement.Get()+MoveNext() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\TrajectoryTrackingMovement.cs:line 244
|
||||
at MultiWheelC.MotionPlanExecutor.Get()+MoveNext() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\MotionPlanExecutor.cs:line 140
|
||||
at ClumsyCore.DriveTask.<>c__DisplayClass9_0.<.ctor>b__2() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 112
|
||||
|
||||
@@ -0,0 +1,19 @@
|
||||
: * (Exception):DriveTask failed, msg=车辆已在终点零速参考处停稳，但终点精度不满足要求：位置误差=0.328m，航向误差=8.10°。, stack:
|
||||
at ClumsyCore.DriveTask.Wait() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 205
|
||||
at MultiWheelC.NewControllerStraightSemicircleStraightTest.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\NewControllerTrackingTests.cs:line 449
|
||||
at ClumsyLite.ClumsyLiteUI.<>c__DisplayClass19_1.<MainPanelHandler>b__21() in D:\MDCS\Source\Core\Clumsy\ClumsyLite\ClumsyLiteUI.cs:line 592
|
||||
|
||||
*p.InnerException * (InvalidOperationException):车辆已在终点零速参考处停稳，但终点精度不满足要求：位置误差=0.328m，航向误差=8.10°。, 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
|
||||
|
||||
|
||||
|
||||
最主要的问题：曲率不连续
|
||||
目前轨迹几何是：
|
||||
直线：曲率 0
|
||||
→ 瞬间进入半径2m圆弧：曲率 0.5 1/m
|
||||
→ 瞬间离开圆弧：曲率重新变成0
|
||||
位置和航向是连续的，因此轨迹不会断开；但是曲率不连续。
|
||||
|
||||
主要问题是“直线与圆弧的曲率瞬间跳变”，而你的 GCP 转角又被限制为每秒最多变化 10°，车辆无法瞬间进入或退出半径2m的圆弧。随后 Stanley 反馈不断补偿，形成明显振荡；速度偏高又进一步放大了问题。
|
||||
@@ -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°
|
||||
所以确实是“中心贴线，但车身左右摆”。
|
||||
@@ -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 也暂时不用调整。
|
||||
@@ -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 横向控制问题，而是参考速度规划的减速距离不够。
|
||||
@@ -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
|
||||
|
||||
|
||||
第二次：
|
||||
|
||||
第三次：
|
||||
|
||||
@@ -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 自己无缘无故发散，而是定位突变触发了欠阻尼响应。
|
||||
|
||||
|
||||
|
||||
Binary file not shown.
Reference in New Issue
Block a user