概覽

這篇博客記錄了我參加逐際動力創學營2025期間的技術總結。

傳送門：逐際動力開源運控 tron1-rl-isaacgym：https://github.com/limxdynamics/tron1-rl-isaacgym

項目基于legged_gym框架進行開發，主要對envs（環境）和algorithm（算法）模塊進行了修改。其中，algorithm部分基于rsl-rl改造，envs模塊則采用了一個環境對應一個機器人任務配置的設計方案。本文將重點介紹點足式機器人的相關內容。下面是框架的整體流程圖。

基礎框架解讀

線速度估計

觀測結構

與legged_gym不同的是，在compute_group_observations函數中，移除了環境觀測obs_buf的線速度信息。這是因為直接獲取線速度存在較大難度且噪聲干擾嚴重，通常需要通過SLAM或狀態估計技術才能準確獲得。該框架采用狀態估計方法，通過一個MLP網絡來估計線速度。網絡的輸入是連續n幀的觀測數據，輸出為3維線速度估計值。Actor網絡的最終輸入由三部分組成：torch.cat((est, obs, commands))，即估計線速度、當前幀觀測數據和速度跟蹤指令。為了計算損失函數，在critic觀測數據的最前面加入了從仿真器獲取的線速度真值base_lin_vel。

• Actor 觀測量 obs_buf 組成

順序	張量	維度	縮放因子	含義
1	base_ang_vel	3	obs_scales.ang_vel	機身角速度（rad/s）
2	projected_gravity	3	—	重力在機身坐標系下的投影
3	(dof_pos - default_dof_pos)	6	obs_scales.dof_pos	關節位置偏差
4	dof_vel	6	obs_scales.dof_vel	關節速度
5	actions	6	—	上一步動作（關節目標）
6	clock_inputs_sin.view(num_envs, 1)	1	—	步態時鐘輸入：sin 分量
7	clock_inputs_cos.view(num_envs, 1)	1	—	步態時鐘輸入：cos 分量
8	gaits	4	—	當前步態參數（頻率、相位偏移、占空比等）

• Critic 觀測量 critic_obs_buf 組成

順序	張量	維度	縮放因子	含義
1	base_lin_vel	3	obs_scales.lin_vel	機身線速度（m/s）
2	obs_buf	30	—	上表中的 Actor 觀測量

在機器人任務中，關節速度、位置和步態信息的時序演變隱含著機器人的構型及正逆運動學（IK）特征。以往我們在使用觀測值堆疊或LSTM/GRU等時序模型時，往往不會過多關注最終輸出向量的物理含義（特別是在游戲類決策環境中），但機器人任務為此提供了良好的切入點。

仿真實驗

如圖所示，仿真效果相當理想。

點足式步態設計

我們整合了步態時鐘與步態參數指標的觀測數據，各參數定義如下：

• 步頻 $f$
• 相位偏移量 $\delta$
• 占空比 $d$：一周期中有 $d$ 的比例處于支撐
• 抬腿高度（似乎沒用到）

步態相位與接觸狀態建模

對應框架中的（_step_contact_targets函數）

這一過程可理解為：通過調整正弦/余弦函數的形態，使其與期望的接觸狀態隨時間變化的規律相匹配，從而實現跟蹤目標。

i. 步態相位（時鐘）更新

$${?}_{t+1}=({?}_t+\Delta t?f)\mathrm{mod}1$$

離散時間里用頻率 $f$（Hz）推進相位；取模把相位限制在 $[0,1)$ 的單位圓上。等價的連續形式是 $\dot{?}=f$。這樣能穩定地產生嚴格周期的時鐘信號。

ii. 左右腳相位關系

$${?}_L=(?)\mathrm{mod}1,{?}_R=(?+\delta +1)\mathrm{mod}1$$

典型取值：小跑/對角步常用 $\delta \approx 0.5$。

iii. 足部狀態判定
$$\text{state}({?}_i)=\{\begin{array}{ll}\text{stance（支撐）},& {?}_i<d\\ \text{swing（擺動）},& {?}_i\ge d\end{array}$$

走路常用 $d>0.5$（著地更久），跑步常用 $d<0.5$。

iv. 歸一化相位（支撐/擺動映射到固定半周期）
$${?}^{\prime }(?)=?\begin{array}{ll}{?}_{\text{stance}}^{\prime }=\frac{?}{d}?0.5,& ?<d\\ {?}_{\text{swing}}^{\prime }=0.5+\frac{??d}{1?d}?0.5,& ?\ge d\end{array}$$

把原相位區間 $[0,d)$（支撐）線性壓縮到 $[0,0.5)$；把 $[d,1)$（擺動）線性壓縮到 $[0.5,1)$。

v. 平滑過渡與周期窗函數
把“是否接觸”從硬 $0/1$ 變成連續平滑的概率/強度，既能做模仿/獎勵加權，也能作為控制器的軟目標，減少硬切換帶來的震蕩。

$${\Phi }_{\kappa }(x)=\frac{1}{2}\left[1+\mathrm{erf}\left(\frac{x}{\kappa \sqrt{2}}\right)\right]$$
$$S({?}^{\prime })={\Phi }_{\kappa }({?}^{\prime })[1?{\Phi }_{\kappa }({?}^{\prime }?0.5)]+{\Phi }_{\kappa }({?}^{\prime }?1)[1?{\Phi }_{\kappa }({?}^{\prime }?1.5)]$$

• ${\Phi }_{\kappa }$ 是平滑階躍（正態 CDF），$\kappa$ 控制邊界軟硬：$\kappa \downarrow$ 接近硬開關；$\kappa \uparrow$ 過渡更柔和，利于穩定訓練/控制，如圖所示
  

• 第一項近似選擇 ${?}^{\prime }\in (0,0.5)$ 的矩形窗，但上下沿平滑可微；

• 第二項是周期補償（wrap-around insurance），防止 ${?}^{\prime }$ 接近 $1$ 時窗函數斷裂。若已保證 ${?}^{\prime }\in [0,1)$，該項通常接近 0，可省略。

步態接觸獎勵

對應框架中的（_reward_tracking_contacts_shaped_force/vel函數）

我們前面定義了接觸狀態的時間窗口，然后可以對不同接觸狀態進行觀測，包括接觸力和接觸點速度。這兩種物理量采用相同的獎勵結構進行約束。

1. $X_i=F_i$（接觸力的模長）
2. $X_i=V_i$（接觸點的速度模長，通常取水平速度）

獎勵函數結構

• 正向獎勵模式（鼓勵目標行為）
  $$R^{+}=\sum _{i=1}^{N_{\text{feet}}}{M}_i?\exp \left(?\frac{X_i^2}{{\sigma }_X^2}\right)$$

• 懲罰模式（抑制錯誤行為）

$$R^{?}=\sum _{i=1}^{N_{\text{feet}}}{M}_i?\left(1?\exp \left(?\frac{X_i^2}{{\sigma }_X^2}\right)\right)$$

其中：

• $N_{\text{feet}}$：機器人足的數量
• $X_i$：被監控的物理量（力或速度）
• ${\sigma }_X$：尺度系數，控制敏感度
• $M_i$：接觸狀態掩碼項
  • 對接觸力獎勵：$M_i=1?S_i$（只關心騰空相的腳）
  • 對接觸速度獎勵：$M_i=S_i$（只關心支撐相的腳）

物理意義

• 接觸力獎勵：

  • 騰空相腳（$S_i\approx 0$）→ $M_i\approx 1$，接觸力 $F_i$ 越小，獎勵越高；
  • 支撐相腳（$S_i\approx 1$）→ $M_i\approx 0$，不參與計算。
    → 鼓勵離地腳真正離地、輕盈抬起。

• 接觸速度獎勵：

  • 支撐相腳（$S_i\approx 1$）→ $M_i\approx 1$，速度 $V_i$ 越小，獎勵越高；
  • 騰空相腳（$S_i\approx 0$）→ $M_i\approx 0$，不參與計算。
    → 鼓勵支撐腳穩固踩地，不滑動。

動作延遲

在將環境建模為MDP（馬爾可夫決策過程）時，本質上忽略了真實環境中的異步特性，如通信延遲、執行延遲和網絡推理延遲等關鍵因素。實際上，環境狀態的動態演變不僅由step函數觸發。為此，需要通過sim-to-sim方法在MuJoCo環境中進行二次驗證。

為提高訓練魯棒性，可采用隨機動作延遲技術（域隨機化）。具體實現是，構建一個先進先出隊列來執行目標位置序列。

我的改進

Point-goal Locomotion

該部分主要借鑒了eth-rsl團隊在《Advanced skills by learning locomotion and local navigation end-to-end》中的工作。傳統方法通常將導航問題分解為路徑規劃、路徑跟隨和運動控制等子任務，這要求機器人具備精確跟蹤指令速度的運動控制能力，但在復雜機器人構型和復雜地形環境下往往難以實現。

為此，《Advanced skills》提出采用端到端的策略訓練方法來解決整個導航問題：不同于持續跟蹤預設路徑，機器人只需在規定時間內到達目標位置即可。任務完成度僅在回合結束時進行評估，這使得策略不必追求最快到達目標，而可以自主選擇行進路線和步態。這種訓練方式拓展了解空間，使機器人能夠掌握更復雜的運動行為。

觀測修改

將原本的目標速度、角速度改為一個機器人坐標系下的目標點即可。目標點被隨機設定在距離機器人初始位置2到8米之間的范圍內。

獎勵修改

移除原本的速度跟蹤的獎勵函數包括_reward_tracking_lin_vel函數以及_reward_tracking_ang_vel函數，增加下面的點跟蹤獎勵函數。

i. 導航任務獎勵（ _reward_navigation_task 函數）

$$r_{\text{task}}=?\begin{array}{ll}\frac{1}{T_r}?\frac{1}{1+\Vert x_b?x_b^{?}{\Vert }^2},& t>T?T_r\\ 0,& \text{otherwise}\end{array}$$

$x_b$ 表示當前機身位置（$x,y$），$x_b^{?}$ 表示目標位置，$T$ 為最大回合時長，$T_r$ 為獎勵時間窗長度，$t$ 為當前回合已運行時間。該獎勵在末尾 $T_r$ 時間窗內生效，值隨與目標的距離減小而增大，最大可達 $1/T_r$。為了讓策略學會在到達目標后穩定停住，而不是“最后一跳”沖過去，$T_r$ 需要設置得足夠長。

ii. 導航輔助獎勵（ _reward_navigation_bias 函數）

$$r_{\text{bias}}=\frac{{\dot{x}}_b?(x_b^{?}?x_b)}{\Vert {\dot{x}}_b\Vert \Vert x_b^{?}?x_b\Vert }$$

${\dot{x}}_b$ 表示當前機身線速度，$x_b^{?}?x_b$ 表示指向目標的向量。該獎勵通過計算速度方向與目標方向的余弦相似度來引導策略，僅在速度與目標方向的范數均大于閾值時計算，以避免除零問題。

實際實現僅考慮機器人沿x軸正方向的速度，使其掌握面向目標點前進的運動模式。

當 mean(r_task) 達到 bias_removal_threshold * max_task_reward（論文默認 50%）時，該獎勵會被移除，從而在訓練早期引導機器人朝目標方向探索，達到一定主任務表現后不再束縛最終策略。

iii. 停滯懲罰（ _reward_stalling_penalty函數）

$$r_{\text{stall}}=\{\begin{array}{ll}?1,& \Vert {\dot{x}}_b\Vert <0.1\text{m/s}\text{and}\Vert x_b?x_b^{?}\Vert >0.5\text{m}\\ 0,& \text{otherwise}\end{array}$$

該邏輯會檢查機器人速度是否小于 0.1 m/s 且離目標距離大于 0.5 m，若滿足條件則給予 -1 懲罰。這樣可以防止策略利用 PPO 的折扣因子在遠離目標時長時間停滯，將負懲罰推遲到回合末，從而抑制“最后一刻沖刺”的行為。

預訓練地形編碼器

我們整合了Local planner與Locomotion policy，相比單一Locomotion policy需要更全面的高程信息支持。為此，我將原框架9×13分辨率（0.1m網格）擴展為64×64網格，覆蓋6.4m×6.4m的局部地形范圍，顯著提升了導航規劃能力。

為高效處理這些高程數據，我參考了《Terrain-attentive learning for efficient 6-DoF kinodynamic modeling on vertically challenging terrain》論文中提出的TAL方法，采用基于Sliced Wasserstein AutoEncoder的核心模型，通過自監督學習預先訓練地形特征編碼器。

Sliced Wasserstein AutoEncoder

SWAE 屬于一種改進的變分自編碼器（VAE）結構，其核心思想是：
通過編碼器將輸入映射到潛在空間，再用解碼器重構，同時用 切片 Wasserstein 距離（SWD）來約束潛在分布接近先驗分布。

i. 編碼過程

• 輸入為二維圖像數據
• 通過多層卷積運算提取特征，并逐步降低空間分辨率、增加通道數
• 將最終的高維特征展平成向量
• 用線性映射得到潛在表示向量 $z\in {\mathbb{R}}^d$

ii. 解碼過程

• 將潛在向量 $z$ 通過線性映射還原為卷積特征圖的形狀
• 通過多層反卷積逐步上采樣，恢復空間分辨率
• 輸出層使用雙曲正切函數，將像素值約束在 [-1, 1] 區間

iii. 切片 Wasserstein 距離（SWD）正則化

• 目標：使潛在分布 $q(z)$ 接近先驗分布 $p(z)$（通常是標準正態分布）
• 方法：
  1. 在潛在空間隨機采樣多個方向向量 {uk}k=1K\{u_k\}_{k=1}^K{uk?}k=1K?，并歸一化
  2. 將樣本投影到這些方向：
     $${\pi }_k(z)=z?u_k$$
  3. 對投影結果排序，并與先驗分布投影結果一一對應
  4. 計算排序后兩組投影的 p-范數距離
  5. 對所有方向的距離取平均，得到 SWD 值：
     $$\text{SWD}=\frac{1}{K}\sum _{k=1}^K\Vert \text{sort}({\pi }_k(z))?\text{sort}({\pi }_k(z^{\prime })){\Vert }_p$$
     其中 $z^{\prime }～p(z)$

iv. 損失函數
$$\mathcal{L}={\mathcal{L}}_{\text{重構誤差}}+{\lambda }_{\text{SWD}}?\text{SWD}$$
其中重構誤差可以是 $L_1$ 或 $L_2$ 范數。

模型訓練與結果

如圖所示，保存框架中地形課程的全部地形數據，基于原框架中_get_heights和_init_height_points函數的高程點采樣邏輯構建訓練數據集。

如圖所示，模型在重構中能夠較好地捕捉大尺度結構，尤其是高程分布和主要形狀，但在細節還原上表現不足，高頻紋理和銳利邊緣往往被平滑化，這是卷積自編碼器常見的問題。SWD 正則化在一定程度上促使潛在空間分布更加光滑，從而提升整體結構一致性，但也可能犧牲部分精細還原能力。對于噪聲較多的樣本（如 #1、#5），模型更傾向于生成較為“干凈”的版本，而非精確還原原始紋理。

參考材料

• 逐際動力開源運控 tron1-rl-isaacgym：https://github.com/limxdynamics/tron1-rl-isaacgym

• Advanced skills by learning locomotion and local navigation end-to-end (iros 2022)

• Terrain-attentive learning for efficient 6-DoF kinodynamic modeling on vertically challenging terrain (iros 2024)