velocity_smoother（速度限幅與加減速平滑）

1.velocity_smoother（速度限幅與加減速平滑）

1.1.實車先看：平滑哪一路速度、反饋來自哪裡

當前velocity_smoother處理controller_server輸出的速度指令，逐軸限目標速度、限速度變化，再處理死區。OPEN_LOOP使用上一輪內部指令作當前速度，不用里程計實測速度；scale_velocities=False仍會限速和限加減速。 本節點處理Twist，不修改規劃路徑，也不查詢代價地圖判斷碰撞。

普通路径跟踪，两种navigation_launch分支相同：
    controller_server／OmniPID
        → cmd_vel_controller
        → velocity_smoother
            ├─ 最新目标速度先夹到各轴min／max范围
            ├─ OPEN_LOOP：以内部last_cmd作为当前速度
            ├─ 按加／减速参数约束本轮变化
            └─ 内部记录 → 应用死区 → 发布
        → cmd_vel_nav2_result
        → fake_vel_transform：换平移轴，再加cmd_spin
        → cmd_vel → 串口／执行端

恢复行为：
    组件分支behavior_server → cmd_vel_nav2_result，绕过平滑器
    独立分支behavior_server → namespace/cmd_vel，另有接线问题

當前launch把平滑器內部cmd_vel訂閱重對映為cmd_vel_controller，把cmd_vel_smoothed釋出重對映為cmd_vel_nav2_result；不是直接套用教程中的cmd_vel_nav→cmd_vel名稱。恢復與下游自旋的限速條件另見behavior_server和fake_vel_transform。

本次安裝的nav2_velocity_smoother和nav2_util均為1.1.20。VelocitySmoother與OdomSmoother安裝標頭檔案匹配該版本，平滑器庫存在；本頁依據對應官方原始碼及當前工程接線說明，不把後續版本的TwistStamped或其他引數直接套入。平滑器原始碼、里程計視窗原始碼

1.2.引數

下面原樣保留髮送的12項引數。原始碼與安裝副本的reality段一致；simulation段只有use_sim_time=True不同。本次只寫文件，未改工程配置或執行引數。

velocity_smoother:
  ros__parameters:
    use_sim_time: False
    smoothing_frequency: 20.0
    scale_velocities: False
    feedback: "OPEN_LOOP"
    max_velocity: [2.5, 2.5, 3.0]
    min_velocity: [-2.5, -2.5, -3.0]
    max_accel: [4.5, 4.5, 5.0]
    max_decel: [-4.5, -4.5, -5.0]
    odom_topic: "odometry"
    odom_duration: 0.1
    deadband_velocity: [0.0, 0.0, 0.0]
    velocity_timeout: 1.0

所有三元素陣列順序都是**[x，y，theta]**，對應Twist的linear.x、linear.y、angular.z。前兩項是平移，第三項是繞z軸的旋轉；theta這一項不是以rad為單位的目標角度。

1.2.1.ros__parameters（時鐘、頻率與反饋方式）

1.2.1.1.參數列

1.2.1.2.引數詳解

1.2.1.2.1.smoothing_frequency（定時器、步長與上游控制頻率）

Active階段用wall timer觸發smootherTimer。當前1000／20=50 ms；定時器週期轉換為整數毫秒，其他頻率可能有截斷，例如33 Hz會建立30 ms週期，而變化上限仍按配置33計算。因此不能把任何設定值都視為精確實測頻率。定時器建立

每輪允許變化按引數除以smoothing_frequency計算，沒有根據上次實際回撥時間測dt。負載導致週期延遲時，真實每秒的變化過程會受影響；這也是指令平滑約束，不代表實測機械加速度已被限制到同樣數值。

當前controller_server也為20 Hz，所以兩者名義頻率相同；提高平滑頻率可在兩條控制指令之間繼續生成過渡輸出，但不會提高規劃／路徑跟蹤本身的計算頻率。兩定時迴圈也沒有自動鎖定相位，不能保證每輪輸出都對應剛收到的一條新控制指令。控制器頻率

例如提高到100 Hz而保持加速度不變，名義線速度每輪上限從0.225降為0.045 m/s，角速度從0.25降為0.05 rad/s；理想每秒仍對應4.5 m/s²和5 rad/s²。這是引數計算，未改變當前頻率或測量輸出。

1.2.1.2.2.OPEN_LOOP（內部命令反饋，不等於機器人實測速度）

OPEN_LOOP把上一輪last_cmd作為本輪current速度，再向最新目標靠近。當前死區為0時，它等於上一輪發布的三個有效分量；若以後啟用非零死區，last_cmd記錄在死區置零之前，就不一定等於實際釋出值。反饋選擇與記錄順序

這種模式預設車輛能跟上此前指令，不能從中知道打滑、卡住、驅動限流或真實速度滯後，也不會因收到odometry自動切成實測閉環。當前啟動讀取odom_topic和odom_duration，但不建立OdomSmoother，不用它們參與current速度。

CLOSED_LOOP改用視窗內里程計Twist均值；這裡的“閉環”是平滑器以實測反饋為限變化的起點，並非額外建立一個車輪速度PID。反饋頻率低或一直為0時，每輪可能都從同一個反饋速度重新算，不能像開環那樣自然累計已發命令。

1.2.1.2.3.scale_velocities（縮放速度變化量，不是關閉限幅）

先把目標各軸夾到min／max範圍，再計算dv=目標−current。False令eta=1，各軸分別夾緊自己的dv；True從受約束軸中找更嚴格的統一比例eta，先縮放全部dv，再按各軸自己的約束夾緊。eta與逐軸約束

按當前頻率、從零起步、目標已在速度範圍內且死區為0，舉例：

前兩項單位m/s，第三項rad/s。False能讓小需求軸先達到目標，但首輪平移方向不按原請求的10∶1比例；True在此例協調變化比例，代價是其他軸也更慢接近。這是原始碼公式示例，不是車輛軌跡測試。

current不為0時，同比的是從current到目標的變化向量，不是直接把整個目標速度乘同一個數。先行的各軸絕對速度夾緊、後續逐軸夾緊和死區也可能改變最終比例，不能把True理解為任何情況下都保證原始速度方向不變。

1.2.2.速度與加減速陣列（每軸範圍、帶符號下界）

1.2.2.1.參數列

1.2.2.2.引數詳解

1.2.2.2.1.max／min_velocity（x、y獨立夾緊，不是合速度上限）

當前目標限制為vx、vy各在[-2.5,2.5] m/s，wz在[-3,3] rad/s。例如目標(vx,vy)=(2.5,2.5)沒有超出任何一軸，平移模長卻為sqrt(2.5²+2.5²)≈3.54 m/s。僅本平滑器的這組陣列不能推出模長≤2.5。目標夾緊

當前OmniPID另有標量平移速度範圍和計算過程；普通跟蹤實際能達到多少要看上游輸出。這裡的3.54只是本陣列允許的目標組合，不是證明當前PID或實車已經輸出該速度。min_velocity中的-2.5允許負向運動，沒有把0.1 m/s之類的小速度抬到2.5 m/s。PID速度約束

速度範圍在平滑輸入的座標軸上施加。當前平移指令按gimbal_yaw_fake約定，fake_vel_transform後再換到實際gimbal_yaw；平面純旋轉換軸保持平移模長，但不能保證轉換後每個分量仍滿足同一軸上界。角速度又會加cmd_spin，最終值不由本陣列統一夾緊。

原始碼夾的是目標，沒有在加減速計算後再做一次最終速度夾緊。當前開環、固定邊界且內部初值在範圍內時，會逐步靠近範圍內目標；若改閉環且反饋本身超界，或執行中縮小邊界，過渡輸出仍可能暫時超出新界。例如current.vx=3、目標夾到2.5、20 Hz減速步長0.225時，本輪得2.775 m/s。此例說明實現順序，未執行閉環或改變限值。

1.2.2.2.2.max_accel與max_decel（符號、加速判據與反向）

每軸依據下麵條件選擇變化區間：

若 abs(v_target) >= abs(v_current)，且 v_current × v_target >= 0：
    使用加速：Δv ∈ [-max_accel/f，+max_accel/f]
否则：
    使用减速：Δv ∈ [max_decel/f，-max_decel/f]

v_next = v_current + clamp(eta × (v_target-v_current)，Δv下界，Δv上界)

f為配置的smoothing_frequency。負方向增速仍使用正max_accel來定義變化幅值；正方向減速則使用負max_decel，不是按“正vx用accel、負vx用decel”區分。跨零反向的這一輪先按減速區間處理，之後是否改用加速由下一輪current和目標重新判斷。逐軸公式

當前名義50 ms一輪：線速度最多變0.225 m/s，角速度最多變0.25 rad/s。從零到單軸2.5 m/s至少需要12次名義更新；從零到3 rad/s也需要12次，約0.6 s週期尺度。上游目標持續變化、其他軸統一縮放、定時器相位及實際驅動反饋會改變具體過程；這裡不是實測起步時間。

max_decel從-4.5改成-2，雖然數值提高，幅值卻減小，減速更慢；改成-6則幅值更大。應按車輛制動能力和任務響應調整，不能隨手去掉負號。當前OmniPID還按0.9 m/s²限制標量速度變化，它與本逐軸4.5 m/s²不是同一變數；最終命令受串聯過程共同影響，恢復旁路也不自動受到兩者的相同約束。

1.2.3.里程計反饋（當前OPEN_LOOP不參與估計）

1.2.3.1.參數列

1.2.3.2.引數詳解

1.2.3.2.1.odom_duration（訊息視窗中的樣本平均）

CLOSED_LOOP才在配置時建立OdomSmoother，直接累加Odometry.twist.twist，再除以保留的訊息數。新訊息到來時按“最新訊息時間戳−最早訊息時間戳>odom_duration”移除舊訊息，因此視窗以訊息stamp衡量，且邊界等於0.1 s時仍保留。不是連續時間積分平均，也不按協方差加權。視窗與均值

例如時間戳嚴格等間隔20 ms且穩定收包時，0.1 s視窗名義可保留6個樣本（含兩端）；實際時間戳抖動會改變數量，不是固定五幀。odom_duration=0.1也不表示等待0.1 s收滿才允許輸出。

視窗只在收到新里程計時更新；getTwist返回快取，沒有按now檢查其是否過期。里程計斷流不由velocity_timeout檢測，後者只檢測速度指令斷流；因此切換閉環前必須另行核對反饋新鮮度。當前開環不受這條里程計快取分支影響。快取讀取

1.2.3.2.2.當前odometry不能未經核對就直接作為閉環真值

當前釋出者sensor_scan_generation給出的是gimbal_yaw位姿及其差分Twist，平移差分在odom軸上計算，時間分母使用回撥steady_clock，還存在首幀處理問題；它不等於已經驗證的底盤速度測量。平面條件下odom軸可能與gimbal_yaw_fake輸入軸近似一致，也不能因此跳過時間、原點和速度物件核對。上游速度實現與問題

尤其angular.z可能包含實際雲臺獨立yaw運動，而平滑器的導航旋轉分量與下游附加底盤自旋又是不同來源。OdomSmoother不會根據child_frame_id自動換系或分離這些角運動；切成CLOSED_LOOP前，應明確反饋的是哪個實體、哪個參考點及哪一套軸下的速度。當前保持開環不意味著這些上游問題已經被修復。

1.2.4.死區與輸入超時（低速置零、超時後減速）

1.2.4.1.參數列

1.2.4.2.引數詳解

1.2.4.2.1.deadband_velocity（比較速度絕對值，不是相鄰訊息差）

原始碼先計算受加減速約束的結果並寫入last_cmd，再對釋出結果做abs(v)<deadband置零。當前全0時，不會因為“速度變化很小”而忽略訊息，也不改變任意非零小速度；等於閾值的分量不被置零。死區處理順序

若以後把x死區設0.3 m/s，按當前開環步長、從零朝1 m/s起步：第一輪內部0.225而釋出0，第二輪內部0.45而釋出0.45。這會使釋出值從0直接跳到0.45，不能據內部0.225步長宣稱死區後的每輪輸出仍嚴格遵守同一變化上限。此為順序計算例，未改當前全0死區。

所以死區適合在確認驅動最低可執行速度、微動現象和到點需求後再設定；它不會增加定位精度，也不是控制器的min_x／min_y_velocity_threshold。控制器那些門檻處理輸入里程計，當前這個門檻處理平滑後的釋出速度。

1.2.4.2.2.velocity_timeout（先等待門檻，再把零當目標平滑）

收到有效Twist時儲存目標並把last_command_time更新為接收時的now；NaN或Inf訊息被拒絕，不重新整理這個計時。Twist本身沒有時間戳，因此這裡測的是接收間隔，不是原始指令生成時刻或網路總延遲。輸入回撥

沒有收到過任何指令時，定時回撥直接返回，不持續釋出零。收到指令後，即使暫時沒有新訊息，也繼續按最新目標平滑；當ROS時間差嚴格>1 s，在某次回撥把command改為零，再按當前減速引數生成輸出。顯式收到零指令則立即把它作為新目標，不先等待這1秒。超時分支

例如超時觸發時內部單軸速度為2.5 m/s，當前開環、死區0、20 Hz和減速幅值4.5下，需要12次名義減速更新才能歸零，約0.6 s週期尺度。加上前面的1 s門檻與定時相位，不能寫成“失去輸入1秒內必定停住”；物理停車還受執行端影響。本例是離散公式換算，不是斷流實測。

超時後內部last_cmd已精確為零或stopped=True時，函式直接返回，不再持續釋出零。當前開環能逐步到達精確零，隨後停止釋出；閉環或非零死區情形仍應結合內部狀態與真實反饋判斷，不能把停止釋出當作車已靜止的測量。

本超時使用ROS時鐘，平滑定時器卻使用牆鍾。use_sim_time=True且/clock暫停時，回撥可能仍執行、按最新目標繼續生成指令，而1 s超時差不再增長。恢復旁路也不受這條輸入超時統一管理；平滑器發零後，fake_vel_transform仍可能加上保留的cmd_spin。

1.2.5.動態引數、配置邊界與驗證範圍

1.2.5.1.當前1.1.20哪些值會動態進入成員

本節點在activate安裝回調，deactivate移除；本版不能把“ROS引數設定成功”一律理解成所有內部設定已聯動更新。動態引數實現

此外，max_accel／max_decel動態回撥檢查符號後，即使判為失敗也仍有賦值成員的程式碼，批次設定還可能先更新前面的成員再遇到後項失敗。不能依賴“設定失敗”證明內部完全未變；本次沒有執行這種非法設定實驗。符號檢查與賦值

啟動配置要保持三元素陣列、加速非負、減速非正、各軸min≤max及有效頻率／時間；當前這些數值符合原始碼的基本符號和陣列要求，但是否匹配實際驅動能力仍需實物資料。平滑器沒有jerk約束，沒有統一的車體合速度限幅，也沒有代價地圖碰撞檢查；機械能力與碰撞處理不能只靠這組引數證明。

1.2.5.2.本次核對與實物觀察

• 已核對： nav2_velocity_smoother／nav2_util版本、兩份安裝標頭檔案與庫；reality／simulation引數及安裝副本；兩種navigation_launch的控制器和平滑器重對映、恢復旁路；上游odometry生成和下游速度換系／自旋。
• 原始碼能確認： 十二項引數的使用條件、三軸順序與單位；目標夾緊→變化約束→內部記錄→死區釋出的順序；開環和閉環視窗區別；超時零目標降速、ROS與牆鐘差異；動態引數覆蓋及未覆蓋分支。
• 實物觀察時重點： 同時對照cmd_vel_controller、cmd_vel_nav2_result及最終cmd_vel，觀察起步、減速、橫移／方向切換和驅動實際響應；確認附加自旋、恢復旁路和控制指令斷流各自的處理結果。若保持開環，先不以odom_duration作為除錯輸出平滑的入口。
• 本次範圍： 僅更新文件和目錄，未改變執行引數或工程程式碼，未釋出速度、未啟動導航／恢復、未進行運動或斷流測試；所有數值例子均為原始碼公式計算。