用于调整出最优参数
This commit is contained in:
4
.idea/claudeCodeTabState.xml
generated
4
.idea/claudeCodeTabState.xml
generated
@@ -7,9 +7,9 @@
|
|||||||
<value>
|
<value>
|
||||||
<TabSessionState>
|
<TabSessionState>
|
||||||
<option name="provider" value="claude" />
|
<option name="provider" value="claude" />
|
||||||
<option name="sessionId" value="dd69d44e-8774-4d83-adda-209d86f8bb78" />
|
<option name="sessionId" value="36e0672c-c35d-4e77-8799-d9ea097d6209" />
|
||||||
<option name="cwd" value="$PROJECT_DIR$" />
|
<option name="cwd" value="$PROJECT_DIR$" />
|
||||||
<option name="model" value="claude-opus-4-6" />
|
<option name="model" value="claude-opus-4-6[1m]" />
|
||||||
<option name="permissionMode" value="plan" />
|
<option name="permissionMode" value="plan" />
|
||||||
<option name="reasoningEffort" value="medium" />
|
<option name="reasoningEffort" value="medium" />
|
||||||
</TabSessionState>
|
</TabSessionState>
|
||||||
|
|||||||
96
.idea/editor.xml
generated
96
.idea/editor.xml
generated
@@ -249,101 +249,5 @@
|
|||||||
<option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=IfStdIsConstantEvaluatedCanBeReplaced/@EntryIndexedValue" value="SUGGESTION" type="string" />
|
<option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=IfStdIsConstantEvaluatedCanBeReplaced/@EntryIndexedValue" value="SUGGESTION" type="string" />
|
||||||
<option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=StdIsConstantEvaluatedWillAlwaysEvaluateToConstant/@EntryIndexedValue" value="WARNING" type="string" />
|
<option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=StdIsConstantEvaluatedWillAlwaysEvaluateToConstant/@EntryIndexedValue" value="WARNING" type="string" />
|
||||||
<option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=StringLiteralTypo/@EntryIndexedValue" value="DO_NOT_SHOW" type="string" />
|
<option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=StringLiteralTypo/@EntryIndexedValue" value="DO_NOT_SHOW" type="string" />
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppClangFormat/EnableClangFormatSupport/@EntryValue" value="false" type="bool" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/ALIGN_MULTILINE_ARGUMENT/@EntryValue" value="true" type="bool" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/ALIGN_MULTILINE_BINARY_EXPRESSIONS_CHAIN/@EntryValue" value="true" type="bool" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/ALIGN_MULTILINE_CALLS_CHAIN/@EntryValue" value="false" type="bool" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/ALIGN_MULTILINE_EXPRESSION/@EntryValue" value="false" type="bool" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/ALIGN_MULTILINE_EXTENDS_LIST/@EntryValue" value="true" type="bool" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/ALIGN_MULTILINE_FOR_STMT/@EntryValue" value="true" type="bool" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/ALIGN_MULTILINE_PARAMETER/@EntryValue" value="true" type="bool" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/ALIGN_MULTILINE_TYPE_ARGUMENT/@EntryValue" value="false" type="bool" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/ALIGN_MULTILINE_TYPE_PARAMETER/@EntryValue" value="false" type="bool" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/ALIGN_MULTIPLE_DECLARATION/@EntryValue" value="false" type="bool" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/ALIGN_TERNARY/@EntryValue" value="ALIGN_ALL" type="string" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/ANONYMOUS_METHOD_DECLARATION_BRACES/@EntryValue" value="END_OF_LINE" type="string" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/BLANK_LINES_AROUND_CLASS_DEFINITION/@EntryValue" value="1" type="int" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/BLANK_LINES_AROUND_DECLARATIONS/@EntryValue" value="0" type="int" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/BLANK_LINES_AROUND_FUNCTION_DECLARATION/@EntryValue" value="1" type="int" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/BLANK_LINES_AROUND_FUNCTION_DEFINITION/@EntryValue" value="1" type="int" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/BREAK_TEMPLATE_DECLARATION/@EntryValue" value="LINE_BREAK" type="string" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/CASE_BLOCK_BRACES/@EntryValue" value="END_OF_LINE" type="string" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/CONTINUOUS_LINE_INDENT/@EntryValue" value="Double" type="string" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/INDENT_ACCESS_SPECIFIERS_FROM_CLASS/@EntryValue" value="false" type="bool" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/INDENT_CASE_FROM_SWITCH/@EntryValue" value="true" type="bool" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/INDENT_CLASS_MEMBERS_FROM_ACCESS_SPECIFIERS/@EntryValue" value="true" type="bool" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/INDENT_COMMENT/@EntryValue" value="true" type="bool" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/INDENT_SIZE/@EntryValue" value="4" type="int" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/INDENT_STYLE/@EntryValue" value="Space" type="string" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/INITIALIZER_BRACES/@EntryValue" value="END_OF_LINE_NO_SPACE" type="string" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/INT_ALIGN_EQ/@EntryValue" value="false" type="bool" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/INVOCABLE_DECLARATION_BRACES/@EntryValue" value="END_OF_LINE" type="string" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/KEEP_BLANK_LINES_IN_CODE/@EntryValue" value="2" type="int" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/KEEP_BLANK_LINES_IN_DECLARATIONS/@EntryValue" value="2" type="int" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/KEEP_USER_LINEBREAKS/@EntryValue" value="true" type="bool" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/LINE_BREAK_AFTER_COLON_IN_MEMBER_INITIALIZER_LISTS/@EntryValue" value="ON_SINGLE_LINE" type="string" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/MEMBER_INITIALIZER_LIST_STYLE/@EntryValue" value="DO_NOT_CHANGE" type="string" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/NAMESPACE_DECLARATION_BRACES/@EntryValue" value="END_OF_LINE" type="string" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/NAMESPACE_INDENTATION/@EntryValue" value="All" type="string" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/OTHER_BRACES/@EntryValue" value="END_OF_LINE" type="string" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/PLACE_CATCH_ON_NEW_LINE/@EntryValue" value="false" type="bool" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/PLACE_ELSE_ON_NEW_LINE/@EntryValue" value="false" type="bool" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/PLACE_NAMESPACE_DEFINITIONS_ON_SAME_LINE/@EntryValue" value="false" type="bool" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/PLACE_WHILE_ON_NEW_LINE/@EntryValue" value="false" type="bool" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SIMPLE_BLOCK_STYLE/@EntryValue" value="DO_NOT_CHANGE" type="string" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_AFTER_CAST_EXPRESSION_PARENTHESES/@EntryValue" value="true" type="bool" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_AFTER_COLON_IN_BITFIELD_DECLARATOR/@EntryValue" value="true" type="bool" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_AFTER_COMMA_IN_TEMPLATE_ARGS/@EntryValue" value="true" type="bool" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_AFTER_COMMA_IN_TEMPLATE_PARAMS/@EntryValue" value="true" type="bool" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_AFTER_EXTENDS_COLON/@EntryValue" value="true" type="bool" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_AFTER_FOR_COLON/@EntryValue" value="true" type="bool" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_AFTER_FOR_SEMICOLON/@EntryValue" value="true" type="bool" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_AFTER_PTR_IN_DATA_MEMBER/@EntryValue" value="false" type="bool" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_AFTER_PTR_IN_DATA_MEMBERS/@EntryValue" value="false" type="bool" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_AFTER_PTR_IN_METHOD/@EntryValue" value="false" type="bool" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_AFTER_PTR_IN_NESTED_DECLARATOR/@EntryValue" value="false" type="bool" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_AFTER_REF_IN_DATA_MEMBER/@EntryValue" value="false" type="bool" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_AFTER_REF_IN_DATA_MEMBERS/@EntryValue" value="false" type="bool" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_AFTER_REF_IN_METHOD/@EntryValue" value="false" type="bool" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_AFTER_UNARY_OPERATOR/@EntryValue" value="false" type="bool" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_BEFORE_COLON_IN_BITFIELD_DECLARATOR/@EntryValue" value="false" type="bool" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_BEFORE_EXTENDS_COLON/@EntryValue" value="true" type="bool" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_BEFORE_FOR_COLON/@EntryValue" value="false" type="bool" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_BEFORE_FOR_SEMICOLON/@EntryValue" value="false" type="bool" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_BEFORE_PTR_IN_ABSTRACT_DECL/@EntryValue" value="true" type="bool" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_BEFORE_PTR_IN_DATA_MEMBER/@EntryValue" value="true" type="bool" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_BEFORE_PTR_IN_DATA_MEMBERS/@EntryValue" value="true" type="bool" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_BEFORE_PTR_IN_METHOD/@EntryValue" value="true" type="bool" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_BEFORE_REF_IN_ABSTRACT_DECL/@EntryValue" value="true" type="bool" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_BEFORE_REF_IN_DATA_MEMBER/@EntryValue" value="true" type="bool" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_BEFORE_REF_IN_DATA_MEMBERS/@EntryValue" value="true" type="bool" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_BEFORE_REF_IN_METHOD/@EntryValue" value="true" type="bool" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_BEFORE_TEMPLATE_ARGS/@EntryValue" value="false" type="bool" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_BEFORE_TEMPLATE_PARAMS/@EntryValue" value="false" type="bool" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_BETWEEN_CLOSING_ANGLE_BRACKETS_IN_TEMPLATE_ARGS/@EntryValue" value="true" type="bool" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_WITHIN_ARRAY_ACCESS_BRACKETS/@EntryValue" value="false" type="bool" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_WITHIN_CAST_EXPRESSION_PARENTHESES/@EntryValue" value="false" type="bool" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_WITHIN_DECLARATION_PARENTHESES/@EntryValue" value="false" type="bool" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_WITHIN_EMPTY_BLOCKS/@EntryValue" value="false" type="bool" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_WITHIN_EMPTY_INITIALIZER_BRACES/@EntryValue" value="false" type="bool" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_WITHIN_EMPTY_METHOD_PARENTHESES/@EntryValue" value="false" type="bool" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_WITHIN_EMPTY_TEMPLATE_PARAMS/@EntryValue" value="false" type="bool" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_WITHIN_INITIALIZER_BRACES/@EntryValue" value="false" type="bool" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_WITHIN_TEMPLATE_ARGS/@EntryValue" value="false" type="bool" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_WITHIN_TEMPLATE_PARAMS/@EntryValue" value="false" type="bool" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPECIAL_ELSE_IF_TREATMENT/@EntryValue" value="true" type="bool" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/TAB_WIDTH/@EntryValue" value="4" type="int" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/TYPE_DECLARATION_BRACES/@EntryValue" value="END_OF_LINE" type="string" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/WRAP_AFTER_BINARY_OPSIGN/@EntryValue" value="true" type="bool" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/WRAP_AFTER_DECLARATION_LPAR/@EntryValue" value="false" type="bool" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/WRAP_AFTER_INVOCATION_LPAR/@EntryValue" value="false" type="bool" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/WRAP_ARGUMENTS_STYLE/@EntryValue" value="WRAP_IF_LONG" type="string" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/WRAP_BEFORE_DECLARATION_LPAR/@EntryValue" value="false" type="bool" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/WRAP_BEFORE_DECLARATION_RPAR/@EntryValue" value="false" type="bool" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/WRAP_BEFORE_INVOCATION_LPAR/@EntryValue" value="false" type="bool" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/WRAP_BEFORE_INVOCATION_RPAR/@EntryValue" value="false" type="bool" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/WRAP_BEFORE_TERNARY_OPSIGNS/@EntryValue" value="true" type="bool" />
|
|
||||||
<option name="/Default/CodeStyle/CodeFormatting/CppFormatting/WRAP_PARAMETERS_STYLE/@EntryValue" value="WRAP_IF_LONG" type="string" />
|
|
||||||
<option name="/Default/CodeStyle/EditorConfig/EnableClangFormatSupport/@EntryValue" value="false" type="bool" />
|
|
||||||
</component>
|
</component>
|
||||||
</project>
|
</project>
|
||||||
@@ -62,29 +62,13 @@ void CorridorCtrl_Compute(const CorridorState_t *state,
|
|||||||
w_cmd = -(s_cfg.kd_theta * imu_wz);
|
w_cmd = -(s_cfg.kd_theta * imu_wz);
|
||||||
} else {
|
} else {
|
||||||
/*
|
/*
|
||||||
* 简化原则：
|
* 横向偏差始终使用 EKF 滤波输出 state->e_y，
|
||||||
* - IMU 只管航向 (e_th + wz)
|
* EKF 内部已做分侧 inset 补偿、马氏距离异常检测和协方差加权，
|
||||||
* - 左右激光只管居中
|
* 控制器不应越级用原始观测差值覆盖。
|
||||||
*
|
|
||||||
* 只要左右两侧都完整有效，就直接用左右平均距离差做横向误差：
|
|
||||||
* e_y_direct = (d_right - d_left)/2
|
|
||||||
* 车在正中时 d_left ~= d_right，因此误差应接近 0。
|
|
||||||
*
|
|
||||||
* 若当前帧双侧不完整，再退回滤波器给出的 e_y。
|
|
||||||
*/
|
*/
|
||||||
float e_y_ctrl = state->e_y;
|
float e_y_ctrl = state->e_y;
|
||||||
bool left_ok = ((obs->valid_mask & (1U << 0)) != 0U) &&
|
|
||||||
((obs->valid_mask & (1U << 1)) != 0U);
|
|
||||||
bool right_ok = ((obs->valid_mask & (1U << 2)) != 0U) &&
|
|
||||||
((obs->valid_mask & (1U << 3)) != 0U);
|
|
||||||
|
|
||||||
if (left_ok && right_ok) {
|
/* 正常控制: IMU 管航向, EKF 管居中 */
|
||||||
float d_left = (obs->d_lf + obs->d_lr) * 0.5f;
|
|
||||||
float d_right = (obs->d_rf + obs->d_rr) * 0.5f;
|
|
||||||
e_y_ctrl = 0.5f * (d_right - d_left);
|
|
||||||
}
|
|
||||||
|
|
||||||
/* 正常控制: IMU 管航向, 左右激光管居中 */
|
|
||||||
w_cmd = -(s_cfg.kp_theta * state->e_th
|
w_cmd = -(s_cfg.kp_theta * state->e_th
|
||||||
+ s_cfg.kd_theta * imu_wz
|
+ s_cfg.kd_theta * imu_wz
|
||||||
+ s_cfg.kp_y * e_y_ctrl);
|
+ s_cfg.kp_y * e_y_ctrl);
|
||||||
|
|||||||
142
横向偏移问题代码分析.md
Normal file
142
横向偏移问题代码分析.md
Normal file
@@ -0,0 +1,142 @@
|
|||||||
|
# 垄沟横向偏移问题 — 代码层面分析
|
||||||
|
|
||||||
|
> 基于 `问题诊断报告.md` 的二次审查，对根因优先级提出不同判断。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 现象回顾
|
||||||
|
|
||||||
|
- 车头笔直（IMU 航向控制正常）
|
||||||
|
- 车身稳定偏向一侧墙壁，无法居中
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 代码审查发现
|
||||||
|
|
||||||
|
### 发现 1：EKF 的 `side_sensor_inset` fallback 从未实现（主因）
|
||||||
|
|
||||||
|
**文件：** `App/est/corridor_ekf.c:451-454`
|
||||||
|
|
||||||
|
```c
|
||||||
|
float d_center_left = (W - Rw) / 2.0f + s_cfg.left_sensor_inset; // = 0.10 + 0.0 = 0.10
|
||||||
|
float d_center_right = (W - Rw) / 2.0f + s_cfg.right_sensor_inset; // = 0.10 + 0.0 = 0.10
|
||||||
|
```
|
||||||
|
|
||||||
|
**文件：** `App/robot_params.h:96-113`
|
||||||
|
|
||||||
|
```c
|
||||||
|
#define PARAM_VL53_SIDE_INSET (-(0.018f)) // 已实测！但从未被 EKF 使用
|
||||||
|
#define PARAM_VL53_LEFT_INSET 0.0f // 未标定
|
||||||
|
#define PARAM_VL53_RIGHT_INSET 0.0f // 未标定
|
||||||
|
```
|
||||||
|
|
||||||
|
**问题：**
|
||||||
|
|
||||||
|
`robot_params.h` 注释写着 `side_sensor_inset` 是"未分侧设置时的默认值"，但 `corridor_ekf.c` 中**没有任何 fallback 逻辑**——它直接用 `left_sensor_inset`（0.0f）和 `right_sensor_inset`（0.0f），完全忽略了已经实测好的 `-0.018f`。
|
||||||
|
|
||||||
|
**影响：** EKF 观测方程在双侧有效时退化为：
|
||||||
|
|
||||||
|
```
|
||||||
|
z_ey = (d_right_avg - d_left_avg) / 2
|
||||||
|
```
|
||||||
|
|
||||||
|
没有任何安装偏置补偿。如果传感器安装面距车体边缘实际有 18mm 的偏差（`side_sensor_inset = -0.018`），或者左右不对称，EKF **本身就会算出带系统性偏置的 `e_y`**。
|
||||||
|
|
||||||
|
### 发现 2：控制器 bypass 绕过 EKF（阻塞性 bug）
|
||||||
|
|
||||||
|
**文件：** `App/nav/corridor_ctrl.c:75-85`
|
||||||
|
|
||||||
|
```c
|
||||||
|
float e_y_ctrl = state->e_y;
|
||||||
|
if (left_ok && right_ok) {
|
||||||
|
float d_left = (obs->d_lf + obs->d_lr) * 0.5f;
|
||||||
|
float d_right = (obs->d_rf + obs->d_rr) * 0.5f;
|
||||||
|
e_y_ctrl = 0.5f * (d_right - d_left); // 覆盖 EKF 结果
|
||||||
|
}
|
||||||
|
```
|
||||||
|
|
||||||
|
**问题：**
|
||||||
|
|
||||||
|
当双侧激光都有效时，控制器丢弃 `state->e_y`，改用原始 `(d_right - d_left)/2`。这个公式**不含任何 inset 补偿**。
|
||||||
|
|
||||||
|
**关键推论：** 当前 `left/right_sensor_inset` 都是 0.0f 时，EKF 的 `z_ey` 和 bypass 的公式**完全等价**：
|
||||||
|
|
||||||
|
```
|
||||||
|
EKF: z_ey = [(W-Rw)/2 + 0 - d_left] + [d_right - (W-Rw)/2 - 0] / 2 = (d_right - d_left) / 2
|
||||||
|
Bypass: e_y = (d_right - d_left) / 2
|
||||||
|
```
|
||||||
|
|
||||||
|
所以**当前状态下 bypass 不是偏移的直接制造者**——EKF 输出与 bypass 结果几乎相同。但 bypass 的架构问题在于：即使将来标定了 inset，控制器依然用不上——它直接跳过了 EKF 的补偿结果。
|
||||||
|
|
||||||
|
### 发现 3：无其他符号/算法错误
|
||||||
|
|
||||||
|
- 控制律符号正确：`e_y > 0`（偏左）→ `w_cmd < 0`（右转回中）
|
||||||
|
- EKF 预测/更新、Joseph 协方差更新、马氏距离检验均无明显问题
|
||||||
|
- IMU 航向观测通道独立且正确
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 与原诊断报告的分歧
|
||||||
|
|
||||||
|
| 因素 | 原报告评级 | 本次评级 | 理由 |
|
||||||
|
|------|-----------|---------|------|
|
||||||
|
| 控制器 bypass | **主因** | 阻塞性 bug | 当前 inset=0 时 bypass 与 EKF 输出等价，不是偏移的直接来源；但堵死了标定修复的路 |
|
||||||
|
| inset 未生效 | 叠加因素 | **主因** | `-0.018f` 已实测但 EKF 从未使用；left/right=0 导致观测方程无偏置补偿 |
|
||||||
|
|
||||||
|
**核心观点：** 原报告说"删 bypass 就一劳永逸"，但如果只删 bypass 而不修 inset，EKF 自身的 `state->e_y` 同样带偏——问题不会改善。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 修复建议
|
||||||
|
|
||||||
|
### 修复 1：EKF 添加 inset fallback（立刻见效）
|
||||||
|
|
||||||
|
**文件：** `App/est/corridor_ekf.c` 第 451-454 行附近
|
||||||
|
|
||||||
|
```c
|
||||||
|
// 修改前：
|
||||||
|
float d_center_left = (W - Rw) / 2.0f + s_cfg.left_sensor_inset;
|
||||||
|
float d_center_right = (W - Rw) / 2.0f + s_cfg.right_sensor_inset;
|
||||||
|
|
||||||
|
// 修改后：
|
||||||
|
float l_inset = (s_cfg.left_sensor_inset != 0.0f) ? s_cfg.left_sensor_inset : s_cfg.side_sensor_inset;
|
||||||
|
float r_inset = (s_cfg.right_sensor_inset != 0.0f) ? s_cfg.right_sensor_inset : s_cfg.side_sensor_inset;
|
||||||
|
float d_center_left = (W - Rw) / 2.0f + l_inset;
|
||||||
|
float d_center_right = (W - Rw) / 2.0f + r_inset;
|
||||||
|
```
|
||||||
|
|
||||||
|
这样 `-0.018f` 会立即作为两侧默认值生效，消除统一偏置。
|
||||||
|
|
||||||
|
### 修复 2：删除控制器 bypass（必须配合修复 1）
|
||||||
|
|
||||||
|
**文件：** `App/nav/corridor_ctrl.c` 第 75-85 行
|
||||||
|
|
||||||
|
删除 `if (left_ok && right_ok) { ... }` 分支，始终使用 `state->e_y`。
|
||||||
|
|
||||||
|
### 修复 3：实车标定分侧 inset（消除左右不对称）
|
||||||
|
|
||||||
|
1. 将车精确放在垄沟正中央（卷尺确认）
|
||||||
|
2. 记录左右 VL53 前后平均读数
|
||||||
|
3. 计算：
|
||||||
|
- `left_inset = 0.10 - 实测左侧平均读数`
|
||||||
|
- `right_inset = 0.10 - 实测右侧平均读数`
|
||||||
|
4. 填入 `PARAM_VL53_LEFT_INSET` 和 `PARAM_VL53_RIGHT_INSET`
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 验证方法
|
||||||
|
|
||||||
|
### 快速验证（不需要上车）
|
||||||
|
|
||||||
|
在调试日志中对比以下两个值：
|
||||||
|
- `state->e_y`（EKF 输出）
|
||||||
|
- `(d_right - d_left) / 2`（bypass 计算）
|
||||||
|
|
||||||
|
如果两者几乎相同，证实 bypass 不是偏移的直接来源，inset 缺失才是。
|
||||||
|
|
||||||
|
### 上车验证
|
||||||
|
|
||||||
|
修复 1+2 后：
|
||||||
|
1. 车应从垄沟正中启动后保持居中
|
||||||
|
2. 手推偏后应能自动回中
|
||||||
|
3. `wall_escape` 不应频繁触发
|
||||||
107
问题诊断报告.md
Normal file
107
问题诊断报告.md
Normal file
@@ -0,0 +1,107 @@
|
|||||||
|
# 垄沟行走无法居中/持续侧墙问题诊断报告
|
||||||
|
|
||||||
|
## 现象描述
|
||||||
|
|
||||||
|
- 垄沟内可以**笔直向前走**，说明航向控制（IMU）正常。
|
||||||
|
- 但**无法在垄沟正中行驶**，车身稳定偏向一侧墙壁。
|
||||||
|
- 表现为：持续侧墙行驶，无法回中。
|
||||||
|
|
||||||
|
> **初步判断**：不是 IMU 精度问题（你确认 IMU 精度很高），而是**横向居中控制存在系统性偏差**。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 根因分析
|
||||||
|
|
||||||
|
### 1. 核心 Bug：`corridor_ctrl.c` 控制器绕过 EKF 滤波结果
|
||||||
|
|
||||||
|
**文件：** `App/nav/corridor_ctrl.c:75-85`
|
||||||
|
|
||||||
|
```c
|
||||||
|
float e_y_ctrl = state->e_y;
|
||||||
|
bool left_ok = /* 左前+左后有效 */;
|
||||||
|
bool right_ok = /* 右前+右后有效 */;
|
||||||
|
|
||||||
|
if (left_ok && right_ok) {
|
||||||
|
float d_left = (obs->d_lf + obs->d_lr) * 0.5f;
|
||||||
|
float d_right = (obs->d_rf + obs->d_rr) * 0.5f;
|
||||||
|
e_y_ctrl = 0.5f * (d_right - d_left); // <-- 直接覆盖 EKF 结果！
|
||||||
|
}
|
||||||
|
```
|
||||||
|
|
||||||
|
**问题：**
|
||||||
|
|
||||||
|
当双侧激光都有效时，控制器**丢弃了 EKF 输出**的 `state->e_y`，改用原始的 `(d_right - d_left) / 2` 作为横向偏差。
|
||||||
|
|
||||||
|
这个做法隐含一个致命假设：**"车在正中时，左右激光读数相等"**。
|
||||||
|
|
||||||
|
但实际情况是：
|
||||||
|
- 左右 VL53L0X 传感器安装**不可能绝对对称**，哪怕差 2~3mm，在 40cm 垄沟 + 20cm 车体 = 每边仅 10cm 的裕量下，就是 **2~3% 的系统误差**。
|
||||||
|
- 控制器拿到这个带偏差的 `e_y_ctrl`，认为"偏左"的位置实际上是正中，于是稳定地把车锁在偏右的位置行驶。
|
||||||
|
|
||||||
|
**为什么车头能走直但回不了中？**
|
||||||
|
|
||||||
|
- `kp_theta * e_th` + `kd_theta * imu_wz` 这组负责**航向**，与激光无关 → 车头笔直。
|
||||||
|
- `kp_y * e_y_ctrl` 这组负责**横向回中**，但 `e_y_ctrl` 本身带系统性偏置 → 回中目标点本身就是歪的。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
### 2. 叠加因素：传感器内缩参数 `left/right_sensor_inset` 未标定
|
||||||
|
|
||||||
|
**文件：** `App/robot_params.h:107,113`
|
||||||
|
|
||||||
|
```c
|
||||||
|
#define PARAM_VL53_LEFT_INSET 0.0f // 注释：当前为 0.0 表示未标定
|
||||||
|
#define PARAM_VL53_RIGHT_INSET 0.0f // 注释：当前为 0.0 表示未标定
|
||||||
|
```
|
||||||
|
|
||||||
|
虽然控制器 bug 是直接原因，但 `left/right_sensor_inset` 未标定也会影响：
|
||||||
|
|
||||||
|
- **EKF 层面：** `corridor_ekf.c:453-454` 用 `d_center_left = (W - Rw)/2 + left_sensor_inset` 计算期望居中读数。`inset = 0` 时，EKF 本身对单侧退化场景的计算也存在偏差。
|
||||||
|
- **单侧退化时恶性循环：** 车因 bug 偏向一侧 → 近墙激光进入饱和/退化区（< 3cm）→ 系统退化为单侧观测 → 未标定的 `inset` 让 EKF 进一步算错位置 → 偏差放大 → 更贴墙。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 诊断结论
|
||||||
|
|
||||||
|
| 因素 | 影响程度 | 说明 |
|
||||||
|
|------|---------|------|
|
||||||
|
| **控制器绕过 EKF（Bug）** | **主因** | 双侧有效时直接丢弃滤波结果，用原始差值驱动回中，引入不可消除的安装不对称误差 |
|
||||||
|
| **left/right_sensor_inset 未标定** | 叠加 | 即使修复 bug，单侧退化场景下 EKF 输出仍不精确；标定后整体鲁棒性更好 |
|
||||||
|
| IMU 精度 | 无关 | 航向控制正常，IMU 工作良好 |
|
||||||
|
| 激光硬件 | 无关 | 传感器能正常输出读数，问题在于怎么用读数 |
|
||||||
|
|
||||||
|
**总体结论：这是软件/参数问题，不是硬件问题。**
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 修复建议
|
||||||
|
|
||||||
|
### 方案 A：改代码（推荐，一劳永逸）
|
||||||
|
|
||||||
|
删除 `App/nav/corridor_ctrl.c` 中第 `75-85` 行的 `if (left_ok && right_ok)` 分支，**始终使用 `state->e_y`** 作为横向偏差输入控制器。
|
||||||
|
|
||||||
|
理由：
|
||||||
|
- EKF 已经综合了 `corridor_width`、`robot_width`、`left/right_sensor_inset` 做了完整几何补偿。
|
||||||
|
- EKF 还做了马氏距离异常检测和协方差加权，噪声滤波远好于控制器层直接算差值。
|
||||||
|
- 控制器不应该"越级"跳过 EKF 做原始观测融合。
|
||||||
|
|
||||||
|
### 方案 B：标定参数（也必须做）
|
||||||
|
|
||||||
|
按 `robot_params.h` 注释中的方法：
|
||||||
|
1. 用卷尺将车**精确放在垄沟正中央**。
|
||||||
|
2. 记录左右 VL53 前后平均读数。
|
||||||
|
3. 计算：
|
||||||
|
- `left_inset = 0.10 - 实测左侧平均读数`
|
||||||
|
- `right_inset = 0.10 - 实测右侧平均读数`
|
||||||
|
4. 填入 `PARAM_VL53_LEFT_INSET` 和 `PARAM_VL53_RIGHT_INSET`。
|
||||||
|
|
||||||
|
> 标定完成后，即使不删代码，控制器的原始差值 `d_right - d_left` 也会更接近真实居中偏差（因为 EKF 标定后输出的 `state->e_y` 更准确，但 bug 依然意味着双侧有效时 EKF 被绕过）。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 验证方法
|
||||||
|
|
||||||
|
修复后上垄沟测试，预期表现：
|
||||||
|
1. **笔直 + 居中**：车头不偏，车身稳定在垄沟正中间。
|
||||||
|
2. 故意用手把车推向一侧，松手后应自动回中。
|
||||||
|
3. 近墙时近墙脱离保护（`wall_escape`）不应频繁触发——因为车本就在中间，不需要"救"。
|
||||||
Reference in New Issue
Block a user