基于DWA的沿墙算法

沿墙流程说明文档

基于 wall_follow_base.cpp / wall_follow_base.h 的流程与关键参数说明。

一、整体架构

1.1 状态机

沿墙模块采用状态机，状态定义在 WFBStatusTypedef 中：

状态	枚举名	说明
沿墙	`WFB_FOLLOW`	正常沿右侧墙行驶
异常	`WFB_EXCEPTION`	发生碰撞/跌落，执行旋转恢复
无	`WFB_NONE`	仅作状态名，无对应处理

每帧调用 Process() 时：

先执行异常检测 CheckException()
再根据当前状态执行对应处理：
- WFB_FOLLOW → WallFollowProcess()（沿墙主逻辑）
- WFB_EXCEPTION → ExceptionProcess()（异常恢复）

二、主流程：Process()

scss 复制代码

Process()
  ├── CheckException()           // 碰撞/跌落检测
  │     └── 若异常 → 切到 WFB_EXCEPTION，记录 exception_init_yaw_、exception_rotate_angle_
  └── status_map_[cur_status_](this)
        ├── WFB_FOLLOW   → WallFollowProcess()
        └── WFB_EXCEPTION → ExceptionProcess()

三、异常流程：ExceptionProcess()

目标：发生物理碰撞或跌落时，原地旋转一定角度后回到沿墙。

流程：

以进入异常时的航向 exception_init_yaw_ 为基准，目标航向 = 基准 + exception_rotate_angle_
计算当前航向与目标航向的误差 yaw_err，归一化到 [-π, π]
若 |yaw_err| > 3°：以固定角速度 rotate_speed = 1.0 rad/s 旋转
若 |yaw_err| ≤ 3°：停止旋转，切回 WFB_FOLLOW

参数	类型/取值	目标与作用
`exception_init_yaw_`	float	进入异常时刻的机器人航向，作为旋转基准
`exception_rotate_angle_`	float	恢复时需要转过的角度。由碰撞/跌落类型决定：• 类型 1 → 2π/3 (120°)• 类型 2 → π/2 (90°)• 类型 3 → π/3 (60°)
`rotate_speed`	1.0 rad/s	异常恢复时的固定角速度
角度收敛阈值	3°	认为"对准目标航向"的阈值，小于则退出异常

四、沿墙主流程：WallFollowProcess()

4.1 流程概览

scss 复制代码

WallFollowProcess()
  ├── 1. SplitLidarData()              // 按角度拆分：前方点云 + 右侧点云
  ├── 2. CheckFrontLidarWall()        // PCA 判断前方是否为"一面墙"（直线）
  ├── 3. GetMinDisToObs()             // 前方最近障碍点及距离
  ├── 4. 获取雷达缓速结果，设定 pid_max_v_
  ├── 5. GetAimPoseByLidar()         // 根据右侧墙点云计算目标位姿 aim_pose_by_odo
  ├── 6. 计算机器人到目标位姿的 angle_error、dis_error
  ├── 7. 判断是否绕障 rotate_around_obs
  ├── 8. 判断是否碰撞 is_collision（雷达距离 + 沿墙距离）
  ├── 9. RunController()              // 根据 dis_error、angle_error 计算 vc、wc
  ├── 10. GetFeasibleSpeed()         // 在保证不与障碍冲突的前提下，修正 w（必要时 v）
  └── 11. SetSpeed(vc, wc)           // 下发线速度、角速度

4.2 各步骤说明

步骤 1：SplitLidarData ------ 雷达分区

输入：全局雷达数据（通过 lidar_reg_ 获取，已按线段组织）
输出：
- front_points_from_lidar：前方扇形内的障碍点（世界/里程计坐标系）
- right_points_from_lidar：右侧扇形内的障碍点
- front_black / right_black：前后/右侧是否"黑"（点极少）的布尔

前方扇形判定（角度以度为单位，0° 为车头方向）：

置信度 > lidar_conf_threshold（60）
距离 end_dis_ < predict_react_lenth_
角度：end_deg_ < 90° - front_angle_° 或 end_deg_ > 270° + front_angle_°

右侧扇形判定：

置信度、距离同上
角度：245° < end_deg_ < 270° + front_angle_°

作用：把"正前方"和"右侧墙"分开，供后续避障、沿墙距离控制和绕障判断使用。

步骤 2：CheckFrontLidarWall ------ 前方是否为一堵墙

方法：对前方点云做 PCA，求协方差矩阵的特征值 λ_max、λ_min
判定：若 λ_min / λ_max < 0.01，认为点云近似一条直线，即"一面墙"
输出：is_front_wall（true = 前方是墙）
用途：与 avoid_advance 等逻辑配合，区分"正对墙"与"斜对障碍"，影响后续是否谨慎前进。

步骤 3：GetMinDisToObs ------ 前方最近障碍

输入：当前里程计位姿、前方点云
输出：前方点云中离机器人最近的点（以及该距离）
用途：用于判断 min_dis、是否触发 lidar_slow_down、以及碰撞判定 lidar_collision。

步骤 4：速度上限 pid_max_v_

若 ROBOT_PERCEPTION->GetLidarSlowDownResult() 为真：pid_max_v_ = lidar_slow_down_speed_（雷达要求减速）
否则：pid_max_v_ = config_max_v_
后续 RunController 和速度限制都会使用 pid_max_v_，保证沿墙时不会超过当前允许的最大速度。

步骤 5：GetAimPoseByLidar ------ 沿墙目标位姿

输入：右侧墙点云 right_points_from_lidar
输出：
- aim_pose_by_odo：期望的"沿墙目标位姿"（位置 + 朝向）
- 返回值：当前机身到右侧墙的最近距离（用于调试/逻辑）

逻辑简述：

在右侧点云中找离当前机器人最近的点，得到"最近点"及索引
沿点云序列向前后扩展，得到一段墙的"当前点"和"前一点"，用于拟合墙的方向
墙的方向角：angle_to_point = atan2(cur_point - last_point)
目标位姿 = 墙上的参考点 + 沿墙法向偏移 (CARRIER_RADIUS + along_wall_dis_)，朝向与墙平行

作用：给出"理想沿墙位置与朝向"，供下面 dis_error、angle_error 计算。

步骤 6：误差计算

robot_to_aim_pose = cur_odo_pose - aim_pose_by_odo（机器人相对目标位姿的向量）
angle_error = -robot_to_aim_pose.Phi()（航向误差）
dis_error = -robot_to_aim_pose.y（横向偏差，在目标位姿的局部 y 方向）

这两个误差直接送入 RunController，用于生成 vc、wc。

步骤 7：rotate_around_obs（是否绕障）

满足其一即设为 true：

|angle_error| > 10°
右侧点云非空，且最后一个点相对当前机身的 x 分量 < along_wall_dis_

作用：在 GetFeasibleSpeed 中，若 rotate_around_obs == true，会对角速度 w 做采样，只采用与右侧墙不冲突的 w，实现"贴墙绕障"时的安全转向。

步骤 8：碰撞判定 is_collision

lidar_collision = (min_dis < CARRIER_RADIUS + along_wall_dis_ + offset)，其中 offset = 0.01
is_collision = lidar_collision || line_laser_collision（当前实现中 line_laser_collision 恒为 false）

作用：为 true 时在 GetFeasibleSpeed 中会强制 v=0、w 取固定值，优先保证安全。

步骤 9：RunController ------ 线速度与角速度控制

输入：前馈 vp、wp，以及 x_err=0、y_err=dis_error、yaw_err=angle_error
输出：vc、wc（本帧期望的线速度、角速度）

控制律：

综合误差：error = 10.0 * y_err + yaw_err
角速度：wc = wp + 2.5*error + 0.5*(error - last_err) + 0.001*sum_error（P=2.5, D=0.5, I=0.001）
线速度：vc = vp（先等于前馈）

大航向误差时的减速：

若
- wc 固定为 ±1.5，并清空积分/微分项
- vc 每帧递减 pid_speed_down_step_，且不低于 pid_min_v_
否则：vc 每帧递增 pid_speed_down_step_，且不超过 pid_max_v_

防积分饱和：若两帧间隔 > 500 ms，会重置 run_control_last_err_、run_control_sum_error_ 等，避免长时间累积导致乱转。

步骤 10：GetFeasibleSpeed ------ 可行速度修正

在给定 vc、wc 下，预测短轨迹是否与障碍物过近；若过近则调整 w（必要时 v）。

注意，这里用于预测轨迹的速度v使用ROBOT_PERCEPTION->GetCurSpeed(&int_cur_v, &int_cur_w);获取的实际速度更贴合实际情况

若 is_collision：直接设 v=0，w = max_sampel_w/2，返回
若 rotate_around_obs：
- 在 w ∈ [-2*max_sampel_w, 2*max_sampel_w] 内以 step_w 步进采样
- 对每个 w 用 GetPredictTrajectory 生成轨迹（长度 CARRIER_RADIUS）
- 用 CheckPoseValid 检查轨迹点与 right_points_from_lidar 是否保持至少 along_wall_dis_ - 0.01
- 取第一个"整条轨迹都合法"的 w 作为输出
否则：不修改 v、w，直接返回

作用：在绕障或贴墙转弯时，保证所选角速度不会让预测轨迹侵入障碍，避免刮蹭或碰撞。

步骤 11：SetSpeed

将 vc、wc 乘以 1000 后调用 ROBOT_PERCEPTION->SetSpeed(vc*1000, wc*1000) 下发底层。

五、关键参数汇总

5.1 沿墙与安全距离

参数	默认值	单位	目标与作用
`config_along_wall_dis_` / `along_wall_dis_`	0.02	m	沿墙距离：期望机身与右侧墙的保持距离；用于目标位姿计算、碰撞判定、可行速度检查
`CARRIER_RADIUS`	0.175（机型相关）	m	机身半径：所有"机身+安全裕度"的计算基准（目标位姿偏移、碰撞判定、轨迹有效性）
`predict_react_lenth_`	1.5×CARRIER_RADIUS	m	雷达反应距离：仅考虑该距离内的雷达点做前方/右侧分区和避障，比机身略大以提前反应

5.2 前方/右侧扇形与角度

参数	默认值	单位	目标与作用
`config_shrink_angle`	5	°	角度收缩量：参与 `config_front_angle_` 计算，缩小"前方"扇形，减少误检
`config_front_angle_`	见下	rad	前方扇形半角（几何计算）：`acos((CARRIER_RADIUS + along_wall_dis_) / predict_react_lenth_) + DEG2RAD(config_shrink_angle)`用于 SplitLidarData 中"前方"与"右侧"的角度分界
`front_angle_`	= config_front_angle_	rad	实际使用的前方角度，目前与配置一致
`lidar_conf_threshold`（SplitLidarData 内）	60	-	雷达线段置信度阈值，低于则不计入前方/右侧点云

5.3 速度与 PID 相关

参数	默认值	单位	目标与作用
`config_max_v_`	0.25（cpp 构造）/ 0.3（头文件）	m/s	配置最大线速度：无雷达缓速时的最大前进速度
`pid_max_v_`	0.25	m/s	PID 输出最大线速度：RunController 中 vc 的上限，会随雷达缓速结果降低
`pid_min_v_`	0.05	m/s	PID 输出最小线速度：大航向误差减速时的下限，避免完全停车导致难以转向
`pid_speed_down_step_`	0.02	m/s	速度恢复/减速步长：每帧 vc 的增减量，用于大角度偏差时平滑减速、对准后平滑加速
`lidar_slow_down_speed_`	0.08	m/s	雷达缓速速度：GetLidarSlowDownResult() 为真时 pid_max_v_ 的取值，保证障碍附近慢速

5.4 控制器内部（RunController）

参数/系数	取值	目标与作用
综合误差	`error = 10.0*y_err + yaw_err`	把横向偏差和航向偏差合成一个标量，偏重航向
P	2.5	角速度比例项，快速纠正偏差
D	0.5	角速度微分项，抑制 overshoot
I	0.001	角速度积分项，消除稳态偏差
大航向误差阈值	20°	超过则固定 w=±1.5 并减速，避免大角度时乱转
积分重置间隔	500 ms	超过则清空 last_err、sum_error，防止积分饱和

5.5 轨迹预测与可行速度（GetFeasibleSpeed / GetPredictTrajectory）

参数	取值	单位	目标与作用
预测步长（GetPredictTrajectory）	0.01	m	轨迹采样步长，影响预测精度与计算量
绕障时预测长度	CARRIER_RADIUS	m	绕障时只预测"一机身高"的轨迹，检查该段是否与右侧墙冲突
max_sampel_w	2.0	rad/s	可行 w 的搜索范围半宽
step_w	0.005	rad/s	可行 w 的搜索步长
CheckPoseValid 中的安全裕度	along_wall_dis_ - 0.01	m	预测轨迹上的点与障碍的最小允许距离

5.6 碰撞与异常恢复

参数	取值	目标与作用
碰撞判定偏移	offset = 0.01	m
exception_rotate_angle_	见"异常流程"	rad

5.7 其他

参数	说明
`aim_yaw_`	当前未在流程中写入，仅成员存在
`v_last_`, `w_last_`	上一帧下发的 v、w，用于 RunController 中的加减速与限幅
`run_control_last_err_`, `run_control_sum_error_`, `run_control_last_time_`, `run_control_start_time_`	RunController 的差分与积分、时间戳，用于 PD 与积分重置

六、数据流简图

scss 复制代码

雷达/里程计
    │
    ▼
SplitLidarData ──► front_points, right_points, front_black, right_black
    │
    ├──► CheckFrontLidarWall ──► is_front_wall
    ├──► GetMinDisToObs ──► min_dis_point, min_dis
    ├──► GetAimPoseByLidar(right_points) ──► aim_pose_by_odo, min_dis_to_wall
    │
    ▼
angle_error, dis_error, rotate_around_obs, is_collision
    │
    ▼
RunController(vc, wc, dis_error, angle_error) ──► vc, wc
    │
    ▼
GetFeasibleSpeed(vc, wc, ...) ──► 修正后的 vc, wc
    │
    ▼
SetSpeed(vc*1000, wc*1000)

七、小结

状态机：正常沿墙（WFB_FOLLOW）+ 碰撞/跌落异常恢复（WFB_EXCEPTION）。
沿墙核心：用右侧雷达点云拟合墙，得到目标位姿；用 dis_error、angle_error 做 PID 控制线速度与角速度；用 along_wall_dis_、CARRIER_RADIUS 保证与墙的距离和安全裕度。
安全机制：前方/右侧雷达分区、前方最近障碍距离、碰撞判定、雷达缓速、绕障时角速度采样（GetFeasibleSpeed）、大航向误差时减速与固定角速度。
关键调参：沿墙距离 along_wall_dis_、最大/最小速度 config_max_v_/pid_min_v_、雷达反应距离 predict_react_lenth_、前方角度 config_front_angle_、以及 RunController 的 P/D/I 和大航向误差阈值，共同决定沿墙平滑度和安全性。

以上内容与 wall_follow_base.cpp / wall_follow_base.h 实现保持一致，可直接作为开发与调试的流程与参数参考。