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