feat: 初始化 Go2 导航代码与配置

2026-10-14 11:08:44 +08:00
parent 8e16ecceba
commit 9dab37e8c8
1258 changed files with 1936673 additions and 0 deletions
--- a/unitree_slam/config/planner_config/pose_ctrl_param.yaml
+++ b/unitree_slam/config/planner_config/pose_ctrl_param.yaml
@@ -0,0 +1,311 @@
+B2:
+  pid:
+    ctrl_rate: 50 #hz
+
+    # x方向
+    PID_x_Limmax: 0.2
+    PID_x_Vmax: 1.8
+    PID_x_KP: 2.5
+    PID_x_KI: 0.1
+    PID_x_KD: 0.0
+
+    # y方向
+    PID_y_Limmax: 0.2
+    PID_y_Vmax: 0.7
+    PID_y_KP: 3.5
+    PID_y_KI: 0.1
+    PID_y_KD: 0.0
+    # yaw
+    PID_yaw_Limmax: 0.2
+    PID_yaw_Vmax: 0.8
+    PID_yaw_KP: 2.5
+    PID_yaw_KI: 0.1
+    PID_yaw_KD: 0.0
+
+    # about threshold
+    # x方向
+    threshold_x: 0.02
+    # y方向
+    threshold_y: 0.02
+    # yaw
+    threshold_yaw: 0.02
+
+  ctrl:
+    CurvatureSamplingInterval: 0.2                               # 曲率离散点采样间隔   pid 巡迹使用
+    # Planner 参数设置
+    Planner/pub_rate: 40.0                                     # 节点发布频率
+    Planner/forwardDis: 0.60 # 0.6                                     # 搜索的最佳追踪点到当前位置的最远距离
+    Planner/transform_tolerance: 0.3                           # 是否到达目标附近的容忍度
+
+    accurateThresholdRotate_x: 0.1
+    accurateThresholdRotate_y: 0.1
+    accurateThresholdRotate_yaw: 0.15
+
+    accurateThresholdAdjust_x: 0.08
+    accurateThresholdAdjust_y: 0.08
+    accurateThresholdAdjust_yaw: 0.1
+
+    accurateThresholdKeeping_x: 0.05
+    accurateThresholdKeeping_y: 0.05
+    accurateThresholdKeeping_yaw: 0.15
+
+
+B2_W:
+  pid:
+    ctrl_rate: 50 #hz
+
+    # x方向
+    PID_x_Limmax: 0.2
+    PID_x_Vmax: 2.5
+    PID_x_KP: 2.25
+    PID_x_KI: 0.1
+    PID_x_KD: 0.0
+
+    # y方向
+    PID_y_Limmax: 0.2
+    PID_y_Vmax: 0.5
+    PID_y_KP: 2.5
+    PID_y_KI: 0.1
+    PID_y_KD: 0.0
+    # yaw
+    PID_yaw_Limmax: 0.2
+    PID_yaw_Vmax: 1.0
+    PID_yaw_KP: 2.5
+    PID_yaw_KI: 0.1
+    PID_yaw_KD: 0.0
+
+    # about threshold
+    # x方向
+    threshold_x: 0.02
+    # y方向
+    threshold_y: 0.02
+    # yaw
+    threshold_yaw: 0.02
+
+  ctrl:
+    CurvatureSamplingInterval: 0.2                               # 曲率离散点采样间隔   pid 巡迹使用
+    # Planner 参数设置
+    Planner/pub_rate: 40.0                                     # 节点发布频率
+    Planner/forwardDis: 1.0 # 0.6                                     # 搜索的最佳追踪点到当前位置的最远距离
+    Planner/transform_tolerance: 0.3                           # 是否到达目标附近的容忍度
+
+    accurateThresholdRotate_x: 0.2
+    accurateThresholdRotate_y: 0.2
+    accurateThresholdRotate_yaw: 0.25
+
+    accurateThresholdAdjust_x: 0.1
+    accurateThresholdAdjust_y: 0.1
+    accurateThresholdAdjust_yaw: 0.1
+
+    accurateThresholdKeeping_x: 0.05
+    accurateThresholdKeeping_y: 0.05
+    accurateThresholdKeeping_yaw: 0.15
+
+G1:
+  pid:
+    # about pid
+    ctrl_rate: 50 #hz
+
+    # x方向
+    PID_x_Limmax: 0.2
+    PID_x_Vmax: 0.8
+    PID_x_KP: 1.8
+    PID_x_KI: 0.0
+    PID_x_KD: 0.0
+
+    # y方向
+    PID_y_Limmax: 0.2
+    PID_y_Vmax: 0.8
+    PID_y_KP: 1.5
+    PID_y_KI: 0.1
+    PID_y_KD: 0.0
+    # yaw
+    PID_yaw_Limmax: 0.2
+    PID_yaw_Vmax: 0.85
+    PID_yaw_KP: 2.0
+    PID_yaw_KI: 0.1
+    PID_yaw_KD: 0.1
+
+    # about threshold
+    # x方向
+    threshold_x: 0.02
+    # y方向
+    threshold_y: 0.02
+    # yaw
+    threshold_yaw: 0.02
+
+  ctrl:
+    CurvatureSamplingInterval: 0.2                             # 曲率离散点采样间隔   pid 巡迹使用
+    # Planner 参数设置
+    Planner/pub_rate: 40.0                                     # 节点发布频率
+    Planner/forwardDis: 0.6 # 0.8                              # 搜索的最佳追踪点到当前位置的最远距离
+    Planner/transform_tolerance: 0.2                           # 是否到达目标附近的容忍度
+
+    accurateThresholdRotate_x: 0.2
+    accurateThresholdRotate_y: 0.2
+    accurateThresholdRotate_yaw: 0.2
+
+    accurateThresholdAdjust_x: 0.25
+    accurateThresholdAdjust_y: 0.25
+    accurateThresholdAdjust_yaw: 0.25
+
+    accurateThresholdKeeping_x: 0.15
+    accurateThresholdKeeping_y: 0.15
+    accurateThresholdKeeping_yaw: 0.15
+
+
+Go2:
+  pid:
+    ctrl_rate: 50 #hz
+
+    # x方向
+    PID_x_Limmax: 0.2
+    PID_x_Vmax: 1.8
+    PID_x_KP: 2.5
+    PID_x_KI: 0.1
+    PID_x_KD: 0.0
+
+    # y方向
+    PID_y_Limmax: 0.2
+    PID_y_Vmax: 0.7
+    PID_y_KP: 3.5
+    PID_y_KI: 0.1
+    PID_y_KD: 0.0
+    # yaw
+    PID_yaw_Limmax: 0.2
+    PID_yaw_Vmax: 1.8
+    PID_yaw_KP: 8.0
+    PID_yaw_KI: 0.5
+    PID_yaw_KD: 0.0
+
+    # about threshold
+    # x方向
+    threshold_x: 0.02
+    # y方向
+    threshold_y: 0.02
+    # yaw
+    threshold_yaw: 0.02
+
+  ctrl:
+    CurvatureSamplingInterval: 0.2                               # 曲率离散点采样间隔   pid 巡迹使用
+    # Planner 参数设置
+    Planner/pub_rate: 40.0                                     # 节点发布频率
+    Planner/forwardDis: 1.0 # 0.6                                     # 搜索的最佳追踪点到当前位置的最远距离
+    Planner/transform_tolerance: 0.3                           # 是否到达目标附近的容忍度
+
+    accurateThresholdRotate_x: 0.1
+    accurateThresholdRotate_y: 0.1
+    accurateThresholdRotate_yaw: 0.15
+
+    accurateThresholdAdjust_x: 0.1
+    accurateThresholdAdjust_y: 0.1
+    accurateThresholdAdjust_yaw: 0.15
+
+    accurateThresholdKeeping_x: 0.15
+    accurateThresholdKeeping_y: 0.15
+    accurateThresholdKeeping_yaw: 0.15
+
+
+Go2_W:
+  pid:
+    ctrl_rate: 50 #hz
+
+    # x方向
+    PID_x_Limmax: 0.2
+    PID_x_Vmax: 1.8
+    PID_x_KP: 2.5
+    PID_x_KI: 0.1
+    PID_x_KD: 0.0
+
+    # y方向
+    PID_y_Limmax: 0.2
+    PID_y_Vmax: 0.7
+    PID_y_KP: 3.5
+    PID_y_KI: 0.1
+    PID_y_KD: 0.0
+    # yaw
+    PID_yaw_Limmax: 0.2
+    PID_yaw_Vmax: 0.8
+    PID_yaw_KP: 2.0
+    PID_yaw_KI: 0.1
+    PID_yaw_KD: 0.0
+
+    # about threshold
+    # x方向
+    threshold_x: 0.02
+    # y方向
+    threshold_y: 0.02
+    # yaw
+    threshold_yaw: 0.02
+
+  ctrl:
+    CurvatureSamplingInterval: 0.2                               # 曲率离散点采样间隔   pid 巡迹使用
+    # Planner 参数设置
+    Planner/pub_rate: 40.0                                     # 节点发布频率
+    Planner/forwardDis: 1.0 # 0.6                                     # 搜索的最佳追踪点到当前位置的最远距离
+    Planner/transform_tolerance: 0.3                           # 是否到达目标附近的容忍度
+
+    accurateThresholdRotate_x: 0.1
+    accurateThresholdRotate_y: 0.1
+    accurateThresholdRotate_yaw: 0.15
+
+    accurateThresholdAdjust_x: 0.1
+    accurateThresholdAdjust_y: 0.1
+    accurateThresholdAdjust_yaw: 0.15
+
+    accurateThresholdKeeping_x: 0.15
+    accurateThresholdKeeping_y: 0.15
+    accurateThresholdKeeping_yaw: 0.15
+
+
+H1:
+  pid:
+    ctrl_rate: 50 #hz
+
+    # x方向
+    PID_x_Limmax: 0.2
+    PID_x_Vmax: 1.8
+    PID_x_KP: 2.5
+    PID_x_KI: 0.1
+    PID_x_KD: 1.1
+
+    # y方向
+    PID_y_Limmax: 0.2
+    PID_y_Vmax: 0.7
+    PID_y_KP: 3.5
+    PID_y_KI: 0.1
+    PID_y_KD: 1.1
+    # yaw
+    PID_yaw_Limmax: 0.2
+    PID_yaw_Vmax: 0.8
+    PID_yaw_KP: 2.0
+    PID_yaw_KI: 0.1
+    PID_yaw_KD: 0.1
+
+    # about threshold
+    # x方向
+    threshold_x: 0.02
+    # y方向
+    threshold_y: 0.02
+    # yaw
+    threshold_yaw: 0.02
+
+  ctrl:
+    CurvatureSamplingInterval: 0.2                               # 曲率离散点采样间隔   pid 巡迹使用
+    # Planner 参数设置
+    Planner/pub_rate: 40.0                                     # 节点发布频率
+    Planner/forwardDis: 1.0 # 0.6                                     # 搜索的最佳追踪点到当前位置的最远距离
+    Planner/transform_tolerance: 0.3                           # 是否到达目标附近的容忍度
+
+    accurateThresholdRotate_x: 0.1
+    accurateThresholdRotate_y: 0.1
+    accurateThresholdRotate_yaw: 0.15
+
+    accurateThresholdAdjust_x: 0.15
+    accurateThresholdAdjust_y: 0.15
+    accurateThresholdAdjust_yaw: 0.15
+
+    accurateThresholdKeeping_x: 0.05
+    accurateThresholdKeeping_y: 0.05
+    accurateThresholdKeeping_yaw: 0.15
+