做運(yùn)動控制的人都知道電機(jī)啟動和停止這件事遠(yuǎn)沒有“給個脈沖它就轉(zhuǎn)”那么簡單。尤其是步進(jìn)電機(jī)和伺服電機(jī)在面對較大慣量負(fù)載或者高速運(yùn)動時如果速度跳變太猛輕則丟步、過沖重則直接撞限位甚至燒驅(qū)動器。我自己在這個坑里栽過好幾次之后才真正意識到一套可靠的速度規(guī)劃算法有多重要。市面上關(guān)于“S曲線加減速”“梯形加減速”的說法很多但大多數(shù)都是零散的概念很少有人把兩種算法放在同一套源碼框架里講清楚更少有人愿意把中文注釋和實(shí)現(xiàn)原理一起整理出來。最近我整理了一套經(jīng)典的電機(jī)加減速控制源碼伺服和步進(jìn)通用核心就是梯形和S曲線兩套速度規(guī)劃邏輯。這篇文章把它掰開揉碎從數(shù)學(xué)原理到代碼實(shí)現(xiàn)再到我實(shí)測時踩過的坑一次性說清楚。1. 先把加減速這件事講透兩種算法的核心差異1.1 為什么不能直接給電機(jī)一個恒定速度指令很多人第一次接觸電機(jī)控制時會下意識認(rèn)為電機(jī)啟動時直接把目標(biāo)速度發(fā)出去然后用PID慢慢調(diào)節(jié)誤差就可以了。但在開環(huán)步進(jìn)系統(tǒng)或者要求高動態(tài)響應(yīng)的伺服系統(tǒng)里這種思路完全是錯的。直接給滿速指令帶來最直接的后果就是“加速度無窮大”。物理上物體速度不能瞬變必然需要力來推動而力矩是有限的。當(dāng)指令速度瞬間從0跳到比如600rpm電機(jī)實(shí)際能提供的力矩根本拉不動轉(zhuǎn)子和負(fù)載步進(jìn)電機(jī)會直接失步伺服電機(jī)則會觸發(fā)跟隨誤差報警甚至引發(fā)機(jī)械共振。反過來停機(jī)時直接從600rpm跳到0負(fù)載的慣性會帶著電機(jī)軸往前沖這就是過沖和定位超調(diào)。所以任何一臺正經(jīng)的運(yùn)動控制設(shè)備都必須有“速度規(guī)劃”環(huán)節(jié)。它的作用是把速度從0平滑地爬到目標(biāo)值再從目標(biāo)值平滑地降回0。加減速算法本質(zhì)上是根據(jù)物理限制最大加速度、最大加加速度去規(guī)劃一條速度-時間曲線讓電機(jī)在“不丟步、不過沖、不振動”的前提下盡可能快地完成運(yùn)動。1.2 梯形加減速和S曲線加減速到底差在哪梯形加減速是最簡單、最直觀的速度規(guī)劃方式。它的速度曲線形狀像梯形先勻加速再勻速最后勻減速。加速階段速度隨時間線性上升意味著加速度恒定減速階段同理速度線性下降。這個方案代碼量極少計算量也很低非常適合單片機(jī)或者低性能控制器。但梯形算法有一個天然的缺陷加速度在“啟動瞬間”、“加速轉(zhuǎn)勻速瞬間”、“勻速轉(zhuǎn)減速瞬間”、“停機(jī)瞬間”都發(fā)生了突變。加速度突變意味著力的突變表現(xiàn)在機(jī)械上就是一頓一頓的沖擊感負(fù)載慣量大的時候還會激起機(jī)械振動讓設(shè)備發(fā)出那種讓人牙酸的“嗡嗡”聲。S曲線加減速就是為了解決這個問題。它的速度曲線不是直線而是帶圓滑過渡的“S”形——啟動時加速度從0緩慢攀升中間加速度達(dá)到最大值接近目標(biāo)速度時加速度又平滑回落到0。整個過程中加速度是連續(xù)變化的沒有突變。加速度的變化率叫加加速度jerkS曲線本質(zhì)上是“梯形規(guī)劃的jerk限制”。打個直觀的比方開手動擋的車如果起步時瞬間松開離合器車會往前猛沖一下這就是梯形加減速如果緩慢松開離合器讓車平穩(wěn)提速這就是S曲線加減速。沖一下的代價是離合器磨損加劇、車身晃動而平順起步代價是提速時間略長。1.3 這套源碼為什么值得看伺服和步進(jìn)通吃很多網(wǎng)上流傳的加減速代碼只針對步進(jìn)電機(jī)且用的是脈沖頻率的概念而伺服電機(jī)通常用速度指令或者位置指令的規(guī)劃方式兩者混不上。但這套源碼的核心思路是“規(guī)劃每個中斷周期應(yīng)該跑到的目標(biāo)速度”底層不管你是發(fā)脈沖還是走總線都能套用。具體來說不管步進(jìn)還是伺服最終給定的速度指令都是“單位時間內(nèi)的位移量”。步進(jìn)電機(jī)是每秒多少個脈沖伺服電機(jī)是每秒多少毫米或者多少轉(zhuǎn)。源碼只需要把單位統(tǒng)一為“位置計數(shù)”就能同時適配。下面所有的源碼解析都基于這個統(tǒng)一的位置計數(shù)思路適配性非常強(qiáng)。2. 梯形加減速源碼拆解邏輯清晰代碼量少2.1 數(shù)學(xué)模型三段式速度規(guī)劃梯形加減速的數(shù)學(xué)模型很簡單設(shè)目標(biāo)速度是 (V_{max})加速階段用時 (t_a)勻速階段用時 (t_u)減速階段用時 (t_d)。給定最大加速度 (a_{max})則加速段速度(v(t) a_{max} \cdot t)勻速段速度(v(t) V_{max})減速段速度(v(t) V_{max} - a_{max} \cdot (t - t_a - t_u))一段完整的位移由三段面積組成。如果位移不夠長可能連勻速段都沒有剛加速到一半就要減速那就是“三角形速度曲線”。源碼里必須處理這種情況否則運(yùn)動距離短時會出現(xiàn)速度還沒到目標(biāo)值就得急剎的bug。我會在代碼里維護(hù)一個狀態(tài)機(jī)三個狀態(tài)分別是加速、勻速、減速。每個定時器中斷周期比如1ms去更新一次當(dāng)前速度然后輸出給電機(jī)。核心點(diǎn)在于“減速點(diǎn)的判斷”——要提前計算好從當(dāng)前速度減速到0需要多長距離一旦剩余距離等于這個減速距離立即切換到減速狀態(tài)。2.2 源碼核心結(jié)構(gòu)一個狀態(tài)機(jī)加一個速度累加器源碼的主體部分非常精簡我提煉出核心邏輯如下/* 梯形加減速狀態(tài)機(jī) */ typedef enum { ACCEL 0, /* 加速 */ CONST, /* 勻速 */ DECEL /* 減速 */ } RampState; typedef struct { int32_t current_pos; /* 當(dāng)前位置 */ int32_t target_pos; /* 目標(biāo)位置 */ uint32_t current_speed; /* 當(dāng)前速度 */ uint32_t max_speed; /* 最高速度 */ uint32_t accel; /* 加速度 */ uint32_t decel; /* 減速度 */ RampState state; /* 當(dāng)前狀態(tài) */ } RampMotion;每次進(jìn)入中斷代碼執(zhí)行以下邏輯要點(diǎn)根據(jù)當(dāng)前狀態(tài)計算出當(dāng)前周期的目標(biāo)速度。累加速度增量加速時 (current_speed accel \cdot \Delta t)減速時 (current_speed - decel \cdot \Delta t)勻速時不變。位置增量按當(dāng)前速度推進(jìn)(current_pos current_speed \cdot \Delta t)。減速判斷計算以當(dāng)前速度減速到0需要的距離 (S_{brake} \frac{v^2}{2 \cdot decel})當(dāng)剩余距離 (S_{remain} target_pos - current_pos \leq S_{brake}) 時強(qiáng)制切換為減速狀態(tài)。還有一點(diǎn)容易被忽略如果初始目標(biāo)很近可能剛進(jìn)加速狀態(tài)就已經(jīng)滿足減速條件這時必須把目標(biāo)速度直接改成“能剛好跑完剩余距離”的速度也就是三角形曲線的情況源碼中稱之為“單段速度修正”。2.3 參數(shù)計算脈沖速度怎么換算成物理單位如果你用步進(jìn)電機(jī)常見的驅(qū)動器細(xì)分?jǐn)?shù)是比如1600脈沖/圈電機(jī)轉(zhuǎn)速單位用脈沖頻率 (f) 表示那么線速度或角速度換算分別是轉(zhuǎn)速 (n)轉(zhuǎn)/秒 (f / 1600)如果配上絲杠導(dǎo)程 (p)mm/轉(zhuǎn)線速度 (V)mm/s (f \cdot p / 1600)在實(shí)際調(diào)參時最大速度不能只看驅(qū)動器的額定脈沖頻率還要考慮電機(jī)的矩頻特性。步進(jìn)電機(jī)高速時力矩會下跌可能沒到達(dá)碼表上的最高速度就已經(jīng)丟步了。所以我在源碼里特意把最大加速度和最大速度做成了可在線修改的變量方便用上位機(jī)或者撥碼開關(guān)直接在設(shè)備上調(diào)。2.4 梯形算法的中文注釋價值在哪里說實(shí)話網(wǎng)上梯形加減速代碼一抓一大把但多數(shù)注釋都極其敷衍變量名是a、b、c根本不敢把這種代碼放進(jìn)工業(yè)設(shè)備里維護(hù)。這套源碼讓我最舒服的一點(diǎn)是它對“狀態(tài)機(jī)切換”“減速距離判斷”這些關(guān)鍵邏輯都寫了詳細(xì)注釋連“為什么這里要加一個最小速度限制”這種邊緣情況都說明了。比如注釋里會寫“步進(jìn)電機(jī)在極低速時容易出現(xiàn)共振因此實(shí)際命令速度不能低于某個下限值啟動時直接從下限速度開始跳變避免低速抖動區(qū)間?！边@種注釋不是廢話是真的能救命的經(jīng)驗(yàn)。剛?cè)胄械拈_發(fā)者照抄邏輯不會出大錯而想深入理解的人也能順著注釋把整個推導(dǎo)打回原形。3. S曲線加減速源碼拆解從原理到實(shí)現(xiàn)3.1 S曲線的7段式數(shù)學(xué)解析S曲線比梯形復(fù)雜一個層級它不再限制“加速度恒定”而是限制“加速度變化率恒定”也就是加加速度(jerk)恒定。一個完整的S曲線加減速過程通常分成7段加加速段加速度從0勻增至 (a_{max})速度呈二次曲線上升。勻加速段加速度保持 (a_{max}) 不變速度線性上升。減加速段加速度從 (a_{max}) 勻減至0速度繼續(xù)上升但趨勢放緩。勻速段加速度為0速度保持 (V_{max})。加減速段加速度從0勻減至 (-a_{max})速度開始下降。勻減速段加速度保持 (-a_{max})速度線性下降。減減速段加速度從 (-a_{max}) 勻增至0速度平滑降為0。每一段的位移都是通過加速度的積分得到的。如果總位移不夠長這些段會被壓縮甚至某些段直接消失。源碼里用了一個分段規(guī)劃器每次計算時先根據(jù)目標(biāo)距離、最大速度、最大加速度、最大加加速度來確定每段時間然后把整條速度曲線離散成每個中斷周期的速度值。3.2 關(guān)鍵是jerk的選取加加速度到底設(shè)多大很多剛接觸S曲線的人會問“最大加加速度怎么定是不是越大越好”不是。jerk越大加速度變化越猛烈S曲線的平滑優(yōu)勢就越弱jerk太小整個運(yùn)動過程會變得拖沓影響生產(chǎn)效率。合理做法是根據(jù)負(fù)載允許的“沖擊”來反推機(jī)械工程師會給定一個允許的最大沖擊力除以負(fù)載質(zhì)量/慣量換算得到允許的jerk。沒有明確指標(biāo)時我會按經(jīng)驗(yàn)設(shè)置通常讓加減速段時間占整個過程時間的10%30%先用大jerk跑一遍再觀察機(jī)械振動逐步調(diào)小。一個值得注意的細(xì)節(jié)是S曲線算法里不只是7段這么簡單。源碼做了“段數(shù)自適應(yīng)判斷”——如果目標(biāo)距離很短無法完整走完7段會退化成5段、3段甚至全程只有加加速和減減速。這種退化處理很考驗(yàn)邏輯嚴(yán)密性少一段就會導(dǎo)致速度曲線在中途出現(xiàn)加速度跳變讓S曲線退化回梯形那種“沖擊感”。3.3 源碼實(shí)現(xiàn)思路實(shí)時遞推而不是查表我看過很多S曲線實(shí)現(xiàn)是離線算好了一張速度表啟動后直接查表發(fā)速度。這種方案優(yōu)點(diǎn)是計算量小缺點(diǎn)是不夠靈活中途改目標(biāo)速度或暫停再恢復(fù)很麻煩。這套源碼用的思路是“實(shí)時遞推”每個中斷周期里根據(jù)當(dāng)前所處的段用公式更新當(dāng)前速度和位置。核心代碼簡化為如下形式/* S曲線每周期速度遞推 */ float jerk; /* 加加速度 */ float current_accel; /* 當(dāng)前加速度 */ float current_speed; /* 當(dāng)前速度 */ /* 每周期周期T更新 */ current_accel jerk * T; /* 加速度線性變化 */ current_speed current_accel * T; /* 速度隨加速度積分 */ current_pos current_speed * T; /* 位置隨速度積分 */這個遞推看起來簡單難的是段與段之間的切換條件。如果當(dāng)前段是加加速那么切換到勻加速的條件是 (current_accel a_{max})切換到減加速則是到了位移中點(diǎn)或者速度接近目標(biāo)。源碼里用了一組段標(biāo)志位和邊界條件判斷我花了一個下午才把每一段的狀態(tài)遷移圖捋順。這也是整個算法里注釋最多的地方因?yàn)楣饪创a真的很難看懂為什么在某個瞬間會切進(jìn)某一段。3.4 S曲線的計算量問題實(shí)時遞推帶來了靈活性代價是每周期要做很多次浮點(diǎn)運(yùn)算。對一個1ms中斷來說現(xiàn)代MCU做幾十次浮點(diǎn)乘法毫無壓力但如果用很便宜的8位單片機(jī)跑1kHz中斷浮點(diǎn)運(yùn)算就有些吃緊。實(shí)測下來這套源碼在Cortex-M0級別的單片機(jī)上一毫秒中斷里跑完整段S曲線規(guī)劃大概需要十幾微秒占不到1.5%的CPU時間完全夠用。若你用的是極低性能的MCU可以把所有float改成定點(diǎn)數(shù)用Q格式做運(yùn)算速度還能再快一半以上。4. 通用性設(shè)計如何同時適配伺服和步進(jìn)4.1 統(tǒng)一“位置計數(shù)”抽象是核心前提伺服電機(jī)和步進(jìn)電機(jī)的指令方式不同但是它們有一個共同點(diǎn)最終都?xì)w結(jié)為“目標(biāo)位置”“當(dāng)前速度”“加速度”這組參數(shù)。對步進(jìn)電機(jī)位置計數(shù)就是脈沖計數(shù)對伺服電機(jī)位置計數(shù)就是編碼器反饋或者上位機(jī)的位置指令脈沖。這套源碼在最外層定義了一個統(tǒng)一接口void Motion_Plan(RampMotion *motion, uint32_t target_pos, uint32_t speed, uint32_t accel, uint32_t jerk);調(diào)用方只需要告訴規(guī)劃器“走到哪里去、怎么走”具體的電機(jī)驅(qū)動由底層硬件抽象層處理。于是我用同一套規(guī)劃邏輯驅(qū)動過步進(jìn)電機(jī)和伺服電機(jī)只是底層把速度值分別轉(zhuǎn)換為脈沖頻率或者模擬量指令。這個抽象做得好的地方在于上層控制邏輯完全不用關(guān)心電機(jī)類型切換電機(jī)時只改底層適配層。4.2 步進(jìn)電機(jī)適配時的特殊處理防止丟步步進(jìn)電機(jī)是開環(huán)系統(tǒng)如果你按S曲線算出來的加速度超出步進(jìn)電機(jī)的動態(tài)力矩范圍它依然會丟步。所以我在適配層里加了一個“加速度限幅”功能從驅(qū)動器手冊里查出電機(jī)帶負(fù)載時的最大啟動加速度然后把這個值作為S曲線的 (a_{max}) 上限。這樣規(guī)劃器永遠(yuǎn)不會生成超出物理能力的加速度指令。步進(jìn)電機(jī)低速共振也需要處理。很多步進(jìn)電機(jī)在幾百赫茲以內(nèi)的頻段會有明顯共振超過一定速度反而安靜。我的做法是在適配層里保證啟動或停止時速度能跳過共振區(qū)間方法就是前面說的“最小速度限制”。這不是加減速算法本身的事但源碼把這些適配邏輯都揉進(jìn)了底層接口所以上層用起來很清爽。4.3 伺服電機(jī)適配時的特殊處理避免跟隨誤差伺服電機(jī)有編碼器反饋理論上不容易丟步但它對速度的突變更敏感因?yàn)樗俣韧蛔円馕吨恢弥噶畹募铀俣群艽笏欧?qū)動器內(nèi)部的PID可能來不及跟隨產(chǎn)生“跟隨誤差報警”。S曲線天然地緩解了這個問題因?yàn)榧铀俣冗B續(xù)變化速度指令不會出現(xiàn)折線伺服跟隨誤差峰值明顯下降。我在一個負(fù)載轉(zhuǎn)臺上做過對比同一套參數(shù)下梯形算法的跟隨誤差峰值能到幾十個脈沖S曲線直接降到個位數(shù)。還有一點(diǎn)容易被忽略伺服的指令延遲。如果上位機(jī)通過總線周期性地發(fā)送位置增量那么加減速規(guī)劃必須跑在“從站側(cè)的插補(bǔ)周期里”而不是拿到指令后再臨時規(guī)劃。這套源碼提供的是“連續(xù)運(yùn)動規(guī)劃”模式一個運(yùn)行周期里不斷根據(jù)剩余距離更新速度天然適配總線周期的節(jié)奏。4.4 插補(bǔ)周期和速度分辨率的取舍不管是梯形還是S曲線最終精度都取決于中斷周期的分辨率。周期越短速度曲線越平滑但計算開銷越大。常見的工業(yè)場景里控制器插補(bǔ)周期是1ms或者0.5ms步進(jìn)脈沖輸出頻率上限可能有幾百kHz。這時要注意速度分辨率的問題速度值用整數(shù)脈沖頻率表示時低速時每個脈沖帶來的頻率變化比例很大S曲線的平滑性會被量化噪聲破壞。解決方法是在源碼內(nèi)部用“亞脈沖累加”的方式每個周期輸出的實(shí)際脈沖數(shù)是“整數(shù)部分”加“小數(shù)累加部分”小數(shù)累計超過1就補(bǔ)發(fā)一個脈沖。這樣速度雖然在微觀上仍然是離散的但長期平均下來跟理想曲線一致電機(jī)運(yùn)行會順滑很多。這套源碼在速度輸出層做了這個處理這也是它能同時扛住伺服和步進(jìn)的關(guān)鍵原因。5. 實(shí)測中的常見問題與排查經(jīng)驗(yàn)5.1 癥狀一啟動時電機(jī)劇烈抖動甚至反向振動如果你剛把代碼燒進(jìn)去電機(jī)啟動時劇烈抖動先別急著改算法。先檢查兩個地方一是中斷周期是否穩(wěn)定我遇到過定時器預(yù)分頻設(shè)置錯誤導(dǎo)致中斷周期忽長忽短速度遞推結(jié)果時快時慢二是加速度是否設(shè)置過大步進(jìn)電機(jī)最明顯你給一個超出機(jī)械能力的加速度它內(nèi)部齒輪可能直接打滑表現(xiàn)就是抖。排查方法也簡單把最大加速度調(diào)成參數(shù)的1/10如果抖動明顯減少說明是加速度超限。再逐步增大找到臨界點(diǎn)然后取臨界值的80%作為實(shí)際參數(shù)。5.2 癥狀二運(yùn)動結(jié)束時過沖/回程誤差過沖最典型的誘因是“減速判斷過早或過晚”。如果代碼里減速距離計算用的是 (v^2/(2a))但實(shí)際機(jī)械存在反向間隙、絲杠彈性形變、或者負(fù)載慣性力被遺忘那么實(shí)際減速距離會比理論值大于是減速太晚沖到目標(biāo)位置。反之如果減速點(diǎn)太早電機(jī)會在接近目標(biāo)時速度已經(jīng)降到零但又沒到位置最后才龜速爬過去效率極低。我后來加了一個自適應(yīng)的“減速距離修正系數(shù)”第一次運(yùn)行先按理論距離減速如果實(shí)測過沖就把減速距離系數(shù)調(diào)到1.1、1.2再跑一次看趨勢。這個系數(shù)跟負(fù)載慣量、機(jī)械阻尼強(qiáng)相關(guān)所以不能直接抄別人的數(shù)必須每臺機(jī)器單獨(dú)標(biāo)定。5.3 癥狀三S曲線計算到一半卡死或速度變負(fù)這種情況十有八九是段切換的條件寫錯了。最常見的一個邏輯漏洞是在勻加速段還沒達(dá)到最大速度時剩余距離已經(jīng)不夠了此時應(yīng)該直接從當(dāng)前速度進(jìn)入“減加速段”而不是傻等速度到最大值再切。很多初版代碼都栽在這里。排查技巧是打印每個周期的速度值畫出速度曲線肉眼觀察有沒有折點(diǎn)或者速度回退。如果速度曲線在某處出現(xiàn)向下的尖角說明段切換提前進(jìn)入了減速段。我建議調(diào)試時加一個短暫的緩沖數(shù)組記錄最近200個周期的速度值出問題時導(dǎo)出曲線一眼就能定位是哪一段出了問題。5.4 癥狀四伺服驅(qū)動器頻繁報跟隨誤差伺服報跟隨誤差很多情況下不是因?yàn)榧訙p速算法不好而是位置指令太密或者太疏導(dǎo)致速度指令的微分值超過伺服額定參數(shù)。遇到這個問題先不急著改驅(qū)動器的跟隨誤差閾值那是治標(biāo)不治本。先檢查你的速度規(guī)劃是否因?yàn)榉直媛什蛔惝a(chǎn)生了鋸齒狀的速度曲線前面說的亞脈沖累加功能有沒有打開。如果速度曲線已經(jīng)很平滑再檢查指令周期和驅(qū)動器內(nèi)部插補(bǔ)周期是否匹配。我曾經(jīng)遇到一個案例控制器每5ms發(fā)一次位置指令但驅(qū)動器內(nèi)部插補(bǔ)周期是0.5ms導(dǎo)致驅(qū)動器看到的位置指令是階梯狀的每次階梯跳變都相當(dāng)于一次加速度沖擊。我就把S曲線的 (a_{max}) 降低讓每個階梯跳變之間速度變化更小問題才緩解。所以記住一個原則指令周期越粗糙加減速平滑度越要保守。5.5 參數(shù)整定速查表參數(shù)步進(jìn)電機(jī)建議做法伺服電機(jī)建議做法過大會怎樣過小會怎樣最大速度不超過矩頻特性拐點(diǎn)的80%不超過額定轉(zhuǎn)速丟步/報警效率低最大加速度實(shí)測臨界值的80%不超過驅(qū)動器額定加速度沖擊抖動運(yùn)動拖沓最大jerk讓加加速段時間占總加速時間的20%讓加速度曲線呈平滑鐘形沖擊感明顯加速蝸牛爬中斷周期0.5ms~2ms均可匹配伺服指令周期CPU負(fù)載高分辨率低鋸齒明顯減速距離系數(shù)1.0~1.2之間標(biāo)定1.0~1.1之間標(biāo)定位置提前停過沖這張表是我實(shí)測下來的通用經(jīng)驗(yàn)具體數(shù)值必須結(jié)合機(jī)械剛度和負(fù)載慣量調(diào)整。比如負(fù)載慣量很大的轉(zhuǎn)臺jerk就得調(diào)得很小否則加速度變化太快整個臺的鋼結(jié)構(gòu)都會震顫。6. 一點(diǎn)實(shí)用技巧怎么把這份源碼用出最大價值6.1 先跑仿真曲線再上電機(jī)我把源碼拿到手的第一件事不是直接下載到控制器里試而是先寫了一個簡單的上位機(jī)仿真腳本把同樣的遞推邏輯跑一遍畫出速度曲線和位移曲線。整個過程不用電機(jī)就能驗(yàn)證算法正確性尤其是S曲線的七段切換在純數(shù)據(jù)層面就能看出問題。等曲線完全符合預(yù)期我再把算法移植到單片機(jī)里做實(shí)測。仿真這一步還能順手做一件事把每段的切換時刻、速度、位移打印出來為后面整定參數(shù)提供初始值。真實(shí)項(xiàng)目里我經(jīng)常會拿仿真曲線跟實(shí)際示波器抓到的頻率曲線對比差異大的地方就暗示有機(jī)械或者指令周期問題診斷效果非常好。6.2 建議在源碼外層加一個“運(yùn)動參數(shù)包”直接把加減速代碼暴露給業(yè)務(wù)邏輯層用容易把不同電機(jī)的參數(shù)混淆。我給這套源碼做了一個薄封裝層每種運(yùn)動場景定義成一組參數(shù)包比如“快移模式”“定位模式”“模擬追蹤模式”每個模式對應(yīng)不同的 (V_{max}, a_{max}, jerk)。業(yè)務(wù)層只需要調(diào)用對應(yīng)的模式編號不需要手動傳參。這樣一來不用動核心算法只需要調(diào)整參數(shù)包就能讓同一臺設(shè)備適配不同工藝要求。封裝層還有個好處是安全某些易碎負(fù)載的工藝段最大加速度被鎖得死死的就算上位機(jī)誤發(fā)了高速指令底層規(guī)劃器也能把實(shí)際速度拉回到安全范圍。6.3 適合什么樣的人讀這份源碼如果你是剛接觸運(yùn)動控制的新手可以重點(diǎn)把梯形加減速的狀態(tài)機(jī)看明白它包含了很多常見運(yùn)動控制概念狀態(tài)切換、邊界判斷、距離計算。如果是有一定經(jīng)驗(yàn)的開發(fā)者重點(diǎn)就看S曲線的段切換和退化處理這是最體現(xiàn)設(shè)計功底的地方。如果你要同時維護(hù)步進(jìn)和伺服兩套設(shè)備那這套源碼里“統(tǒng)一位置計數(shù)抽象”和“驅(qū)動器適配層”的思路是最值得吸取的。我個人在實(shí)際使用中的體會是算法本身并不難難的是把邊界條件和異常情況都想清楚。這套源碼讓我省下最多時間的不是核心公式而是它把加速過程中途改變目標(biāo)速度、運(yùn)行中急停、最小距離退化這些“臟活”全都處理好了。寫代碼容易把邊界寫對難這份源碼在這些細(xì)節(jié)上的處理是真正能拉開項(xiàng)目進(jìn)度差距的地方。如果后續(xù)你想讓這段代碼更進(jìn)一步我給兩個擴(kuò)展方向一是把S曲線的實(shí)時遞推改成“加減速前瞻”也就是提前多看幾段待執(zhí)行路徑提前規(guī)劃速度曲線這對高速連續(xù)軌跡加工非常有用二是把它改造成實(shí)時操作系統(tǒng)的任務(wù)用高優(yōu)先級線程做速度遞推低優(yōu)先級線程做參數(shù)下發(fā)這樣就能適配更復(fù)雜的多軸聯(lián)動場景。