目前程序沒有完全調(diào)好需要再改進會在從5cm回到-5cm路上停下一、程序總體功能k230pid.c實現(xiàn)了一套基于 K230 視覺檢測、MSPM0 單片機控制和步進電機執(zhí)行的小球位置閉環(huán)控制系統(tǒng)。系統(tǒng)的基本控制鏈路為K230視覺識別 ↓ 獲得小球相對目標點的位置誤差 ↓ MSPM0通過串口接收誤差 ↓ 狀態(tài)觀測器估計小球位置和速度 ↓ 位置—速度串級控制器計算目標傾斜量 ↓ 步進電機改變導(dǎo)軌/圓管傾角 ↓ 小球在重力作用下移動 ↓ K230再次檢測形成閉環(huán)從文件名看它是一個 PID 程序但從當前實際執(zhí)行路徑看它已經(jīng)發(fā)展成一個組合控制系統(tǒng)包含定時中斷閉環(huán)控制K230視覺誤差接收α-β狀態(tài)觀測器位置—速度串級控制兩階段目標切換識別小球靜止/運動狀態(tài)判斷靜摩擦與卡滯補償步進電機位置限制分段運動與斜率限制數(shù)據(jù)丟失保護最終到位保持文件中還保留了一套傳統(tǒng) PID 和軌跡控制算法但目前定時中斷實際調(diào)用的是BallCascade_Control()而不是傳統(tǒng)的BallTrajectory_Control()。二、核心參數(shù)及含義2.1基礎(chǔ)控制參數(shù)#define BALL_ERROR_DEADBAND 7誤差死區(qū)避免誤差和視覺噪聲導(dǎo)致電機頻繁動作#define BALL_STEPPER_MIN_HZ 650#define BALL_STEPPER_MAX_HZ 2000步進電機最低和最高脈沖頻率其中最大值用于初始化PID輸出范圍在當前串級控制實際使用固定的4200這兩個參數(shù)主要服務(wù)于舊pid控制路徑#define BALL_DATA_TIMEOUT_TICKS 50Uk230數(shù)據(jù)超時門限為50個控制周期超時后就停止電機#define BALL_MOTOR_DIRECTION 1用于適配機械安裝方向為1控制方向不變?yōu)?電機運動方向取反。#define BALL_Y_POSITION_LIMIT 800步進電機相對于啟動零點允許的累計位置范圍(-800~800)#define BALL_OPUT_PER_STEP 50.0f舊PID輸出到步進電機步數(shù)的比例系數(shù)例如:PID輸出為500時對應(yīng)10步。該參數(shù)只在舊函數(shù)BallStepper_SetOutput()和BallStepper_Shake()中使用當前串級控制直接輸出目標電機位置。#define BALL_CONTROL_PERIOD_S 0.01f控制周期為0.01s真正配置取決于定時器周期2.2串級控制參數(shù)#define BALL_POSITION_KP 3.5f位置環(huán)比例系數(shù) 根據(jù)位置誤差生成目標速度v -kp*△x(負號表示偏移某一側(cè)死后,目標速度指向相反方向)#define BALL_TARGET_SPEED_MAX 200.0f目標速度限幅(-200~200)#define BALL_SPEED_KP 2.6f速度環(huán)比例系數(shù)根據(jù)速度誤差計算電機目標位置(目標傾斜角度)#define BALL_CASCADE_POS_LIMIT 800串級輸出限幅#define BALL_CASCADE_DELTA_MAX 28單周期最大調(diào)整步數(shù)優(yōu)點:避免導(dǎo)軌角度突變降低機械沖擊減少步進電機丟步風(fēng)險讓控制輸出更加平滑#define BALL_CASCADE_SPEED_HZ 4200U串級控制實際使用的最大限制頻率三、全局變量與嵌入式變量修飾1.volatile變量表示這些變量可能被中斷或其他執(zhí)行上下文異步修改編譯器每次訪問時都應(yīng)重新從內(nèi)訓(xùn)讀取不能隨意緩存或優(yōu)化掉但是只能防止編譯器錯誤優(yōu)化并以保證多字節(jié)變量的原子性不等價于線程安全或者中斷安全。變量含義Ball_ErrorXK230發(fā)送的小球位置誤差Ball_PID_Output控制器輸出或速度誤差調(diào)試值Ball_NoDataTicks未收到新數(shù)據(jù)的周期數(shù)Ball_PID_Active是否已經(jīng)收到有效數(shù)據(jù)并進入控制狀態(tài)Ball_PID_Enabled控制功能是否允許運行Ball_Y_Position軟件記錄的電機累計位置Ball_AdjustSteps最近一次調(diào)整的步數(shù)Ball_IsMoving小球是否被判斷為正在運動Ball_TaskStage當前任務(wù)階段1或2Ball_ErrorSpeed誤差接近速度的濾波估計Ball_Y_TargetPosition電機目標位置Ball_ControlPhase控制階段Ball_StepCommandCount已發(fā)送的步進運動命令數(shù)Ball_StuckTiltBoost克服靜摩擦的附加傾斜量Ball_FinalHold是否已進入最終保持狀態(tài)Ball_PositionEstimate觀測器估計的小球位置Ball_VelocityEstimate觀測器估計的小球速度Ball_TargetVelocity位置環(huán)產(chǎn)生的目標速度四、小球運動狀態(tài)識別static void Ball_UpdateMotionState(int32_t error) { int32_t change; if (!s_errorSampleValid) { s_lastBallError error; s_errorSampleValid 1U; return; } change error - s_lastBallError; if (change 0) change -change; s_lastBallError error; //判斷小球是否在運動閾值原本為4 if (change 3) //運動 { s_stillConfirmCount 0U; if (s_moveConfirmCount 3U) s_moveConfirmCount; if (s_moveConfirmCount 3U) Ball_IsMoving 1U; } else //靜止 { s_moveConfirmCount 0U; if (s_stillConfirmCount 4U) s_stillConfirmCount; if (s_stillConfirmCount 4U) Ball_IsMoving 0U; } }判斷規(guī)則若連續(xù)3次滿足 \(\Delta e\geq4\)認為小球正在運動若連續(xù)4次滿足 \(\Delta e4\)認為小球靜止。這種方法不是單幀立即切換狀態(tài)而是使用連續(xù)確認計數(shù)。其優(yōu)點是避免視覺坐標抖動導(dǎo)致狀態(tài)在“運動”和“靜止”之間頻繁跳變。這是一種數(shù)字去抖、時間濾波或滯回確認思想。這里需要區(qū)分閾值4是空間變化閾值連續(xù)3次和4次是時間確認閾值二者結(jié)合增強了狀態(tài)判斷穩(wěn)定性。五、電機執(zhí)行static void BallStepper_SetOutput(float output, int32_t maxSteps, uint16_t speedHz) { int32_t adjustSteps; //等待上一次運動完成再執(zhí)行下一次 if (!Servo_Y_IsDone()) { Ball_AdjustSteps 0; return; } adjustSteps (int32_t)(output / BALL_OUTPUT_PER_STEP); if (adjustSteps 0 output ! 0.0f) adjustSteps (output 0.0f) ? 1 : -1; if (adjustSteps maxSteps) adjustSteps maxSteps; if (adjustSteps -maxSteps) adjustSteps -maxSteps; if (BALL_MOTOR_DIRECTION 0) adjustSteps -adjustSteps; if (Ball_Y_Position adjustSteps BALL_Y_POSITION_LIMIT) adjustSteps BALL_Y_POSITION_LIMIT - Ball_Y_Position; else if (Ball_Y_Position adjustSteps -BALL_Y_POSITION_LIMIT) adjustSteps -BALL_Y_POSITION_LIMIT - Ball_Y_Position; Ball_AdjustSteps adjustSteps; if (adjustSteps 0) return; Servo_Y_SetSpeed(speedHz); Servo_Y_Stepper_Run(adjustSteps); Ball_Y_Position adjustSteps; }1.非阻塞異步執(zhí)行不在中斷等待電機2. PID輸出到步數(shù)的轉(zhuǎn)換adjustSteps (int32_t)(output / BALL_OUTPUT_PER_STEP);3.位置式電機控制/*位置式電機控制 不是每次執(zhí)行完整目標控制逐段靠近目標位置 使用位置目標 分段跟蹤的結(jié)構(gòu) */ static void BallStepper_MoveToPosition(int32_t targetPosition, int32_t maxSegmentSteps, uint16_t speedHz) { int32_t moveSteps; //限幅 if (targetPosition BALL_Y_POSITION_LIMIT) targetPosition BALL_Y_POSITION_LIMIT; if (targetPosition -BALL_Y_POSITION_LIMIT) targetPosition -BALL_Y_POSITION_LIMIT; Ball_Y_TargetPosition targetPosition; //等待上一次電機動作完成異步非阻塞 if (!Servo_Y_IsDone()) return; moveSteps targetPosition - Ball_Y_Position; //誤差機制 //限幅 if (moveSteps maxSegmentSteps) moveSteps maxSegmentSteps; if (moveSteps -maxSegmentSteps) moveSteps -maxSegmentSteps; if (moveSteps 0) { Ball_AdjustSteps 0; return; } Servo_Y_SetSpeed(speedHz); Servo_Y_Stepper_Run(moveSteps); if (!Servo_Y_IsDone()) { Ball_Y_Position moveSteps; Ball_AdjustSteps moveSteps; Ball_StepCommandCount; } }4.軟件限位目標位置和運動結(jié)果都會被限制在[-800,800]這屬于嵌入式系統(tǒng)中的安全保護設(shè)計。但是Ball_Y_Position是通過程序累計步數(shù)得到的開環(huán)位置不是編碼器反饋。如果發(fā)生以下情況軟件位置可能與真實位置不一致步進電機丟步電機堵轉(zhuǎn)機械滑動系統(tǒng)啟動時機構(gòu)不在真實零位電源異常導(dǎo)致狀態(tài)丟失。因此嚴格來說這不是絕對位置而是“從啟動零點開始累計的估計位置”。更可靠的系統(tǒng)可以增加零位限位開關(guān)上電回零編碼器反饋電機堵轉(zhuǎn)檢測。六、α-β狀態(tài)觀測器K230發(fā)送的是離散且可能存在噪聲的位置誤差系統(tǒng)還需要知道小球速度。直接用相鄰位置相減會產(chǎn)生較大噪聲因此程序設(shè)計了α-β觀測器。1.觀測器狀態(tài)分別對應(yīng)小球位置估計Ball_PositionEstimate小球速度估計Ball_VelocityEstimate2.預(yù)測過程每次定時中斷先執(zhí)行預(yù)測公式對應(yīng)代碼static void BallObserver_Predict(void) { if (s_observerInited) { Ball_PositionEstimate Ball_VelocityEstimate * BALL_CONTROL_PERIOD_S; } if (s_observerFrameTicks 65535U) s_observerFrameTicks; }這使用了勻速運動模型假設(shè)再很短的10ms周期內(nèi)小球速度基本不變。即使在這一周期沒有收到k230新數(shù)據(jù)觀測器仍可根據(jù)上一次速度繼續(xù)預(yù)測位置。3.校正過程收到視覺誤差后計算殘差其中r:測量與預(yù)測偏差預(yù)測位置zk230發(fā)送來的位置誤差校正α參數(shù)作用α越大位置估計越相信當前視覺測量響應(yīng)快但是噪聲更容易進入估計結(jié)果α越小位置更加平滑抗噪聲能力強但是會增加滯后性。β參數(shù)作用β控制殘差對速度估計的修正強度β越大速度估計靈敏但容易抖動。β越小速度平滑但跟蹤加速度變化較慢。4.不等間隔數(shù)據(jù)處理代碼沒有假設(shè)k230每個控制周期都一定發(fā)送數(shù)據(jù)而是通過參數(shù)s_observerFrameTicks記錄兩次視覺幀之間經(jīng)歷了多少個N10ms的周期參數(shù)含義Ts?控制周期MCU微控制器控制環(huán)的固定運行周期。文中明確提到“MCU控制環(huán)可以運行在100Hz”因此Ts1/1000.01Ts?1/1000.01 秒即10ms。這是系統(tǒng)最基礎(chǔ)的時間基準。(定時器N或 NticksNticks?兩次成功接收到的視覺幀之間所經(jīng)歷的 TsTs? 控制周期的個數(shù)。它是一個整數(shù)計數(shù)器例如如果隔了5個周期才來一幀則 N5N5。T實際視覺幀間隔兩次視覺幀之間真實的物理時間間隔。它通過公式 TN×TsTN×Ts? 計算得出。關(guān)于取值限制 0.01s≤T≤0.20s0.01s≤T≤0.20s 的含義下限0.01s防止 N0N0即同一周期內(nèi)重復(fù)處理同一幀數(shù)據(jù)確保至少經(jīng)過一個完整控制周期才更新一次視覺數(shù)據(jù)。上限0.20s設(shè)定一個超時閾值即最多等待20個控制周期。如果超過0.2秒還沒收到新幀系統(tǒng)會認為視覺數(shù)據(jù)已過期或丟幀嚴重從而停止使用該數(shù)據(jù)避免用“過時”的信息去控制當前的電機或執(zhí)行器。總結(jié)這個機制的本質(zhì)是用固定高頻的時鐘節(jié)拍Ts?去“丈量”不規(guī)則到達的視覺數(shù)據(jù)將非周期的視覺輸入轉(zhuǎn)化為帶有時間戳T的可用信息從而實現(xiàn)多速率系統(tǒng)的協(xié)同。速度限幅速度估計限制為-500 V 500用于防止視覺誤識別、坐標跳變或丟幀后恢復(fù)產(chǎn)生異常大的速度估計從而導(dǎo)致控制器突然輸出極端值。七、位置速度串級控制鋼珠控制核心static void BallCascade_Control(void)static void BallCascade_Control(void) { float positionError; //位置相對目標誤差 float velocityError; //速度相對誤差 float targetPositionFloat; //目標位置浮點 float absPosition; //絕對位置誤差值 float absVelocity; //絕對速度誤差值 int32_t targetPosition; //目標位置 if (!s_observerInited) return; //目標位置定義為0陰惻觀測到位置本身則為相對目標的誤差 positionError Ball_PositionEstimate; absPosition (positionError 0.0f) ? positionError : -positionError; absVelocity (Ball_VelocityEstimate 0.0f) ? Ball_VelocityEstimate : -Ball_VelocityEstimate; if (Ball_TaskStage 2U absPosition 7.0f absVelocity BALL_SETTLE_SPEED) { Ball_FinalHold 1U; } if (Ball_FinalHold) { Ball_ControlPhase 3U; Ball_TargetVelocity 0.0f; Ball_PID_Output 0.0f; BallStepper_MoveToPosition(0, BALL_CASCADE_DELTA_MAX, BALL_CASCADE_SPEED_HZ); return; } //目標速度-3.5*相對目標位置誤差限幅200 Ball_TargetVelocity Ball_ClampFloat( -BALL_POSITION_KP * positionError, -BALL_TARGET_SPEED_MAX, BALL_TARGET_SPEED_MAX); //速度誤差 //Ball_VelocityEstimate在β觀測器中得到 velocityError Ball_TargetVelocity - Ball_VelocityEstimate; targetPositionFloat BALL_SPEED_KP * velocityError; /* A stuck ball needs extra tilt, but the boost disappears after it moves. */ if (absPosition 12.0f absVelocity 10.0f) { if (s_stuckControlFrames 1000U) s_stuckControlFrames; Ball_StuckTiltBoost 120 (int32_t)(s_stuckControlFrames / 6U) * 35; if (Ball_StuckTiltBoost 500) Ball_StuckTiltBoost 500; targetPositionFloat (targetPositionFloat 0.0f) ? (float)Ball_StuckTiltBoost : -(float)Ball_StuckTiltBoost; } else { s_stuckControlFrames 0U; Ball_StuckTiltBoost 0; } targetPositionFloat Ball_ClampFloat( targetPositionFloat, -(float)BALL_CASCADE_POS_LIMIT, (float)BALL_CASCADE_POS_LIMIT); targetPosition (int32_t)targetPositionFloat; if (BALL_MOTOR_DIRECTION 0) targetPosition -targetPosition; Ball_PID_Output velocityError; if ((positionError 0.0f Ball_VelocityEstimate 0.0f) || (positionError 0.0f Ball_VelocityEstimate 0.0f)) Ball_ControlPhase 2U; else Ball_ControlPhase 0U; BallStepper_MoveToPosition(targetPosition, BALL_CASCADE_DELTA_MAX, BALL_CASCADE_SPEED_HZ); }串級控制分為內(nèi)外環(huán)1.外環(huán)位置誤差生成目標速度程序吧目標位置定義位0因此觀測到的位子本身就是相對目標的誤差如果小球離目標較遠目標速度較大接近目標后目標速度自然減小。與直接用位置誤差控制傾角相比這種方式明確規(guī)定了“希望小球以多塊的速度接近目標”。2.內(nèi)環(huán)速度誤差產(chǎn)生目標傾角因為電機位置與導(dǎo)軌傾角近似相關(guān)所以目標電機位置實際上代表目標傾斜程度。內(nèi)環(huán)會根據(jù)實際速度調(diào)整傾角小球運動太慢增加傾角小球運動太快減小傾角小球已有較大沖向目標的速度可能輸出反向傾角進行制動。3.串級控制的優(yōu)勢相較于單一位置PID它具有以下優(yōu)點能顯式控制小球接近目標的速度更容易提前減速降低沖過目標點的概率能利用速度反饋增加系統(tǒng)阻尼對小球這種具有慣性的對象更合適。該控制器嚴格來說是位置外環(huán)P控制器 速度內(nèi)環(huán)P控制器當前實際路徑并不是傳統(tǒng)的PID控制器。八、控制階段判斷變量Ball_Controlphase用于表示當前控制階段0正常驅(qū)動/加速階段1舊軌跡控制中的水平滑行階段2反向制動或速度方向異常階段3最終到位保持階段判斷條件if ((positionError 0 velocity 0) ||(positionError 0 velocity 0))表示位置誤差和速度方向相反即小球正在向目標點運動。不過代碼把這一狀態(tài)名為階段2更準確的說它代表“正在接近目標/存在制動需求”并不一定意味著此刻已經(jīng)執(zhí)行反向制動。九、最終到位保持待優(yōu)化滿足條件Ball_TaskStage 2absPosition 7 //絕對位置absVelocity 8 //絕對速度系統(tǒng)進入Ball_FinalHold 1;優(yōu)點這比只判斷位置更加合理。如果判斷位置小球可能只是高速經(jīng)過目標點并未真正的穩(wěn)定。加入速度條件后要求小球不僅靠近目標而且運動速度足夠小。進入最終保持狀態(tài)后將目標速度設(shè)置為鋼球PID控制系統(tǒng)輸出設(shè)置為制動電機調(diào)整位置并且實時更新邏輯位置。代碼如下Ball_TargetVelocity 0;Ball_PID_Output 0;BallStepper_MoveToPosition(0, ...);系統(tǒng)將導(dǎo)軌逐步恢復(fù)到標定水平位置。需要注意Ball_FinalHold是鎖存狀態(tài)。一旦置1本次任務(wù)中不會因為誤差再次增大而自動退出只能通過停止、重新使能或階段切換清除。這能避免在目標附近反復(fù)切換但也可能使小球受擾動后無法自動重新捕獲目標。這里需要優(yōu)化十、靜摩擦與卡滯補償導(dǎo)軌上存在靜摩擦導(dǎo)致啟動或者低俗運動時候小球出現(xiàn)卡滯現(xiàn)象。判斷條件absPosition 12absVelocity 10小球明顯未到達目標但是出現(xiàn)停止或者低速時候觸發(fā)觸發(fā)后逐漸增加在static void BallCascade_Control(void))Ball_StuckTiltBoostif (absPosition 12.0f absVelocity 10.0f) { if (s_stuckControlFrames 1000U) s_stuckControlFrames; Ball_StuckTiltBoost 120 (int32_t)(s_stuckControlFrames / 6U) * 35; if (Ball_StuckTiltBoost 500) Ball_StuckTiltBoost 500; targetPositionFloat (targetPositionFloat 0.0f) ? (float)Ball_StuckTiltBoost : -(float)Ball_StuckTiltBoost; } else { s_stuckControlFrames 0U; Ball_StuckTiltBoost 0; } targetPositionFloat Ball_ClampFloat( targetPositionFloat, -(float)BALL_CASCADE_POS_LIMIT, (float)BALL_CASCADE_POS_LIMIT); targetPosition (int32_t)targetPositionFloat; if (BALL_MOTOR_DIRECTION 0) targetPosition -targetPosition; Ball_PID_Output velocityError; if ((positionError 0.0f Ball_VelocityEstimate 0.0f) || (positionError 0.0f Ball_VelocityEstimate 0.0f)) Ball_ControlPhase 2U; else Ball_ControlPhase 0U;基本表示小球開始運動后就將補償清0具體代碼s_stuckControlFrames 0;Ball_StuckTiltBoost 0;這屬于自適應(yīng)靜摩擦補償或漸增式啟動力矩補償。它和積分控制有一定相似性二者都會在誤差長期不能消除時逐漸增強控制量。但該方法只在“誤差較大且速度很低”時工作比普通積分更加有針對性也不容易在高速運動時繼續(xù)累積十一、兩段軌跡識別0-5cm;5cm--5cm)使用變量Ball_TaskStage表示兩個任務(wù)階段通過誤差變化推斷目標是否發(fā)生變化判斷條件具體代碼Ball_TaskStage 1 //當前在第一階段previousAbs 420 //上一幀誤差處于一定范圍內(nèi)currentAbs 120 //當前兩幀誤差發(fā)生明顯跳變jump 30 //相鄰兩幀誤差發(fā)生明顯跳變條件滿足后切換為Ball_TaskStage 2;同時清除舊階段狀態(tài)清空最終保持標志清空速度估計相關(guān)變量清空制動閾值清空防卡滯補償清空PID歷史誤差和積分重置觀測器。這是典型的狀態(tài)機和模式切換設(shè)計。切換時重置觀測器非常重要因為目標點變化會讓“誤差坐標”瞬間改變。如果把該跳變當成小球真實運動就會估計出一個極大的虛假速度。十二、誤差接近速度的低通濾波舊軌跡通過以下計算公式接近速度對應(yīng)代碼approachSpeed previousAbs - currentAbs;Ball_ErrorSpeed Ball_ErrorSpeed * 0.7f approachSpeed * 0.3f;含義大于0絕對值誤差正在減少小球接近目標。小于0絕對值誤差正在增大小球遠離目標。這是一階指數(shù)低通濾波器可以降低視覺坐標噪聲對速度計算的影響。當前串級控制主要使用α-β觀測器中的Ball_VelocityEstimateBall_ErrorSpeed主要服務(wù)于保留的舊軌跡控制。十三、傳統(tǒng)軌跡控制算法目前雖然沒有被定時中斷調(diào)用BallTrajectory_Control()仍包含大量值得學(xué)習(xí)的控制思想。它把小球控制過程劃分為以下階段1.加速驅(qū)動階段根據(jù)絕對誤差大小選擇不同傾斜量。誤差越大傾斜步數(shù)越大誤差越小傾斜步數(shù)越小。這屬于分段非線性控制或增益調(diào)度。2.水平滑行階段當小球接近目標且具有一定速度時BallStepper_MoveToPosition(0, ...);讓導(dǎo)軌恢復(fù)到水平讓小球依舊保持慣性滑行。3.反向制動階段當小球更接近目標且速度仍較大時向相反方向傾斜導(dǎo)軌以抵消小球動量。這是一種基于狀態(tài)機的預(yù)測制動思想4.最終保持階段誤差小于等于7時把導(dǎo)軌回復(fù)水平并停止繼續(xù)調(diào)整。這套方法不完全依賴數(shù)字模型而是把人的控制經(jīng)驗寫成分段規(guī)則屬于經(jīng)驗控制或混合控制十四、PID增益調(diào)度static void BallPID_SetProfile(int32_t error, int32_t *maxSteps, uint16_t *speedHz) { int32_t absError (error 0) ? error : -error; /* Faster pulses improve response; smaller step segments limit tube angle. */ if(absError 260) { step.Kp 26.0f; step.Ki 0.0f; step.Kd 25.0f; step.OutMax 1800.0f; step.OutMin -1600.0f; *maxSteps 3; *speedHz 3700U; } else if(absError 230) { step.Kp 23.0f; step.Ki 0.0f; step.Kd 23.0f; step.OutMax 1600.0f; step.OutMin -1600.0f; *maxSteps 3; *speedHz 3500U; } else if (absError 200) { step.Kp 23.0f; step.Ki 0.0f; step.Kd 23.0f; step.OutMax 1600.0f; step.OutMin -1600.0f; *maxSteps 3; *speedHz 3200U; } else if (absError 180) { step.Kp 21.0f; step.Ki 0.0f; step.Kd 27.0f; step.OutMax 800.0f; step.OutMin -800.0f; *maxSteps 1; *speedHz 2700U; } else if (absError 160) { step.Kp 19.0f; step.Ki 5.0f; step.Kd 26.0f; step.OutMax 900.0f; step.OutMin -900.0f; step.IntMax 150;step.IntMin -150; *maxSteps 1; *speedHz 2600U; }else if (absError 130) { step.Kp 20.0f; step.Ki 6.0f; step.Kd 26.0f; step.OutMax 800.0f; step.OutMin -800.0f; *maxSteps 1; *speedHz 2600U; }else if (absError 100) { step.Kp 20.0f; step.Ki 6.0f; step.Kd 25.0f; step.OutMax 900.0f; step.OutMin -900.0f; *maxSteps 1; *speedHz 2500U; } else if (absError 80) { step.Kp 17.0f; step.Ki 2.0f; step.Kd 22.0f; step.OutMax 1000.0f; step.OutMin -1000.0f; *maxSteps 2; *speedHz 2800U; } else if (absError 60) { step.Kp 17.0f; step.Ki 3.0f; step.Kd 22.0f; step.OutMax 900.0f; step.OutMin -900.0f; *maxSteps 3; *speedHz 2800U; } else if (absError 40) { step.Kp 16.0f; step.Ki 3.0f; step.Kd 22.0f; step.OutMax 800.0f; step.OutMin -800.0f; *maxSteps 2; *speedHz 2800U; } else if (absError 30) { step.Kp 16.0f; step.Ki 2.0f; step.Kd 21.0f; step.OutMax 700.0f; step.OutMin -700.0f; *maxSteps 2; *speedHz 2800U; } else { step.Kp 14.0f; step.Ki 2.0f; step.Kd 18.0f; step.OutMax 600.0f; step.OutMin -600.0f; *maxSteps 2; *speedHz 2600U; } }根據(jù)絕對誤差的不同區(qū)間動態(tài)修改KpKiKdPID輸出限幅單次最大步數(shù)電機脈沖頻率這種方法叫做增益調(diào)度?；舅枷胧蔷嚯x目標較遠時強調(diào)快速響應(yīng)接近目標時降低輸出并增強阻尼某些區(qū)間關(guān)閉積分防止大誤差下積分累積不同誤差區(qū)間采用不同執(zhí)行速度。不過該函數(shù)當前沒有在中斷執(zhí)行路徑中被調(diào)用所以它是保留算法不是當前實際運行參數(shù)。另一個值得注意的問題是某些分支修改了IntMax和IntMin其他分支沒有重新賦值。這意味著積分限幅可能保留上一次分支的值形成隱含狀態(tài)。如果重新啟用該算法最好在每個分支中都完整設(shè)置相關(guān)參數(shù)。十五、抖動脫困、回平和反向制動輔助函數(shù)以下函數(shù)目前也沒有被實際主控制路徑調(diào)用BallStepper_Shake()通過正向、反向和再次正向的周期動作讓機構(gòu)振動以克服小球卡滯。這是振動脫困策略。BallStepper_Level()每次最多向零位移動4步讓導(dǎo)軌緩慢回平。BallStepper_ReverseBrake()根據(jù)當前電機傾斜方向執(zhí)行反向運動且制動步數(shù)限制在624步。這些函數(shù)體現(xiàn)了代碼從早期PID控制逐步演化到軌跡控制、再演化到串級控制的過程。十六、初始化、使能和停止流程1.初始化void K230PID_Init(void)void K230PID_Init(void) { UART2_K230_Init(); StepMotor_Init(); K230PID_Stop(); Ball_NoDataTicks 0U; Ball_PID_Active 0U; Ball_PID_Enabled 1U; NVIC_ClearPendingIRQ(TIMER_1_INST_INT_IRQN); NVIC_EnableIRQ(TIMER_1_INST_INT_IRQN); DL_TimerA_startCounter(TIMER_1_INST); }依次完成初始化K230串口初始化步進電機清除控制器歷史狀態(tài)清除無數(shù)據(jù)計數(shù)設(shè)置控制使能清除定時器中斷掛起標志開啟定時器中斷啟動定時器。說明整個控制器是由周期定時器驅(qū)動的。2.使能void K230PID_Enable(void)void K230PID_Enable(void) { Ball_PID_Enabled 1U; Ball_PID_Active 0U; Ball_NoDataTicks 0U; Ball_Y_Position 0; K230PID_Stop(); }執(zhí)行設(shè)置使能標志控制狀態(tài)暫時設(shè)為未激活清空超時計數(shù)把當前電機軟件位置設(shè)為零調(diào)用停止函數(shù)清空歷史狀態(tài)。需要注意設(shè)置Ball_Y_Position0只是把當前機械位置當作軟件零點并沒有真正執(zhí)行機械回零。3.停止void K230PID_Stop(void)void K230PID_Stop(void) { Servo_Y_Stop(); step.Error0 0.0f; step.Error1 0.0f; step.ErrorInt 0.0f; step.Out 0.0f; Ball_PID_Output 0.0f; Ball_AdjustSteps 0; Ball_IsMoving 0U; s_errorSampleValid 0U; s_moveConfirmCount 0U; s_stillConfirmCount 0U; s_shakePhase 0U; Ball_TaskStage 1U; Ball_ErrorSpeed 0.0f; Ball_BrakeThreshold 0; Ball_Y_TargetPosition 0; Ball_ControlPhase 0U; Ball_StuckTiltBoost 0; Ball_FinalHold 0U; s_stuckControlFrames 0U; s_trajectorySampleValid 0U; BallObserver_Reset(); }停止時會立即停止步進電機清空PID誤差和積分清空輸出清空運動狀態(tài)判斷回到第一任務(wù)階段清除速度、制動和防卡滯狀態(tài)清除軌跡采樣狀態(tài)重置α-β觀測器。Ball_PID_Enabled清零所以它更接近“停止當前控制并等待新的有效視覺數(shù)據(jù)”而不是徹底禁用模塊。十七、定時器中斷控制流程核心控制位于void TIMER_1_INST_IRQHandler(void)void TIMER_1_INST_IRQHandler(void) { if (DL_TimerA_getPendingInterrupt(TIMER_1_INST) DL_TIMER_IIDX_ZERO) { int32_t error; uint8_t hasNewFrame 0U; if (!Ball_PID_Enabled) { DL_TimerA_clearInterruptStatus( TIMER_1_INST, DL_TIMERA_INTERRUPT_ZERO_EVENT); return; } BallObserver_Predict(); if (K230_GetFrame(error) K230_FRAME_ERROR) { hasNewFrame 1U; Ball_PID_Active 1U; Ball_NoDataTicks 0U; Ball_ErrorX error; if (BallTrajectory_Update(error)) { BallObserver_Reset(); } Ball_UpdateMotionState(error); BallObserver_Correct(error); } if (!hasNewFrame) { if (!Ball_PID_Active) { DL_TimerA_clearInterruptStatus( TIMER_1_INST, DL_TIMERA_INTERRUPT_ZERO_EVENT); return; } if (Ball_NoDataTicks 65535U) Ball_NoDataTicks; if (Ball_NoDataTicks BALL_DATA_TIMEOUT_TICKS) { K230PID_Stop(); Ball_PID_Active 0U; } } if (Ball_PID_Active) BallCascade_Control(); DL_TimerA_clearInterruptStatus(TIMER_1_INST, DL_TIMERA_INTERRUPT_ZERO_EVENT); } }定時器執(zhí)行順序如下1.判斷是否使能如果沒有使能不執(zhí)行控制。if (!Ball_PID_Enabled)return;2.觀測器預(yù)測無論有沒有新視覺數(shù)據(jù)都先根據(jù)當前速度預(yù)測下一時刻位置。BallObserver_Predict();3.讀取k230數(shù)據(jù)if (K230_GetFrame(error) K230_FRAME_ERROR)收到誤差幀后標記有新數(shù)據(jù)激活控制器清零超時計數(shù)保存誤差判斷任務(wù)階段是否切換更新運動/靜止狀態(tài)用視覺測量校正觀測器。4.處理丟幀如果本周期沒有收到新數(shù)據(jù)若系統(tǒng)從未收到過有效數(shù)據(jù)直接返回若已經(jīng)激活則增加無數(shù)據(jù)計數(shù)達到50次后停止控制。5.執(zhí)行串級控制即使某一周期沒有新視覺幀只要沒有達到超時門限仍會根據(jù)觀測器預(yù)測結(jié)果繼續(xù)控制。if (Ball_PID_Active)BallCascade_Control();6.清除中斷標志保證下一個定時周期能夠正常進入中斷。十八、數(shù)據(jù)丟失保護該程序區(qū)分了兩個狀態(tài)Ball_PID_Enabled模塊是否允許運行Ball_PID_Active是否已經(jīng)收到過有效誤差幀。系統(tǒng)啟動后不會在沒有視覺數(shù)據(jù)的情況下盲目控制。只有收到第一幀有效誤差后才進入活動狀態(tài)?；顒雍笕绻R時丟一兩幀觀測器繼續(xù)預(yù)測控制器仍然工作避免偶發(fā)丟幀造成電機頻繁啟停。如果連續(xù)約0.5秒沒有數(shù)據(jù)K230PID_Stop(); Ball_PID_Active 0;系統(tǒng)停止電機并重置控制狀態(tài)。這是通信看門狗或數(shù)據(jù)有效性監(jiān)控機制。十九、該程序體現(xiàn)的主要知識點可將本文件中的知識點概括為閉環(huán)反饋控制通過視覺誤差持續(xù)修正執(zhí)行器輸出。位置式PID包含比例、積分、微分、積分限幅和輸出限幅。串級控制位置外環(huán)生成目標速度速度內(nèi)環(huán)生成目標傾角。狀態(tài)觀測器使用α-β觀測器融合位置測量并估計小球速度。預(yù)測—校正結(jié)構(gòu)無數(shù)據(jù)時預(yù)測有新數(shù)據(jù)時利用殘差校正。多速率系統(tǒng)MCU控制頻率和K230視覺幀率可以不同。有限狀態(tài)機使用任務(wù)階段、控制階段、最終保持狀態(tài)組織控制邏輯。數(shù)字濾波使用指數(shù)低通濾波處理誤差變化速度。防抖判斷連續(xù)多幀確認小球是運動還是靜止。增益調(diào)度根據(jù)誤差區(qū)間改變PID參數(shù)和執(zhí)行器參數(shù)。非線性分段控制不同誤差區(qū)間使用不同傾斜量和制動力度。靜摩擦補償小球卡滯時逐漸增加傾角。反向制動通過反向傾斜消除小球動量。執(zhí)行器飽和限制目標位置、速度估計、輸出和單次步數(shù)?？刂屏孔兓氏拗泼恐芷谧疃嗾{(diào)整一定步數(shù)降低機械沖擊。軟件限位防止電機累計位置超過安全范圍。非阻塞電機控制控制器只在電機完成上一段動作后發(fā)送下一段命令。通信超時保護長時間無視覺數(shù)據(jù)時自動停止。中斷驅(qū)動控制使用硬件定時器保證近似固定的控制周期。嵌入式變量管理使用volatile處理主程序與中斷共享變量使用static封裝內(nèi)部狀態(tài)。二十、程序優(yōu)點該程序的主要優(yōu)點包括控制鏈路完整形成視覺閉環(huán)使用速度估計優(yōu)于只根據(jù)位置誤差控制串級結(jié)構(gòu)適合具有慣性的小球系統(tǒng)考慮了視覺噪聲、丟幀和幀率不穩(wěn)定有位置、速度、輸出和增量等多層限幅具有卡滯補償適應(yīng)真實機械摩擦使用分段步進控制降低機械沖擊目標切換時重置觀測器避免虛假速度最終到位同時判斷位置和速度保留了大量調(diào)試變量便于串口、OLED或調(diào)試器觀察。二十一、存在的問題和改進方向1 .文件名與實際算法不完全一致雖然文件名為k230pid.c當前實際控制函數(shù)是串級P控制不是傳統(tǒng)完整PID。建議在報告中寫成系統(tǒng)早期使用位置式PID當前版本以位置—速度串級比例控制為主并保留原PID與軌跡控制代碼。2. 存在未調(diào)用函數(shù)和未使用宏當前未被實際路徑調(diào)用的函數(shù)包括BallStepper_SetOutput()BallStepper_Shake()BallStepper_Level()BallStepper_ReverseBrake()BallTrajectory_Control()BallPID_SetProfile()未實際使用的宏包括BALL_ERROR_DEADBANDBALL_STEPPER_MIN_HZ這會增加代碼閱讀難度?？梢詣h除舊代碼或者通過編譯選項明確選擇控制模式。3. 注釋存在亂碼文件中部分中文注釋因編碼不一致出現(xiàn)亂碼可能是UTF-8、GBK和編輯器設(shè)置不一致造成的。建議統(tǒng)一為UTF-8編碼。4. 中斷函數(shù)工作量較大中斷中包含串口幀讀取浮點觀測器運算控制算法電機命令處理。在資源有限的MCU上中斷時間過長可能影響其他實時任務(wù)。更合理的結(jié)構(gòu)是定時中斷只設(shè)置控制標志主循環(huán)或?qū)崟r任務(wù)執(zhí)行復(fù)雜控制計算串口中斷只接收并緩存數(shù)據(jù)。如果當前100Hz周期內(nèi)運算時間充足也可以繼續(xù)使用但最好通過GPIO翻轉(zhuǎn)或定時器測量中斷執(zhí)行時間。5 .軟件電機位置可能漂移Ball_Y_Position是命令累計值不是真實反饋位置。建議增加上電回零限位開關(guān)編碼器定期零點校準。6. 任務(wù)切換采用誤差跳變推斷通過誤差變化判斷K230是否切換目標可能受到誤檢、丟幀和坐標突變影響。更可靠的方法是讓K230數(shù)據(jù)幀直接攜帶當前目標編號當前任務(wù)階段幀序號校驗信息。MCU收到明確階段字段后再切換狀態(tài)。7. 最終保持狀態(tài)不能自動退出Ball_FinalHold一旦置位就持續(xù)讓導(dǎo)軌回平。如果小球后來受到擾動離開目標系統(tǒng)不會重新控制?？梢栽黾油顺鰲l件例如持續(xù)若干幀后清除Ball_FinalHold形成帶滯回的重新捕獲機制。8. 串級參數(shù)缺少單位說明代碼中的位置、速度和步數(shù)單位沒有明確標注。例如K230誤差是像素還是毫米速度單位是像素/秒還是毫米/秒電機步數(shù)和導(dǎo)軌角度之間的換算關(guān)系目標點±5cm如何換算為圖像坐標。報告中最好明確寫出當前控制參數(shù)屬于實驗整定參數(shù)其數(shù)值與視覺坐標尺度、攝像頭安裝、步進電機細分數(shù)及機械傳動比有關(guān)。9. 觀測模型只采用勻速模型α-β觀測器假設(shè)短周期內(nèi)速度保持不變但真實小球受到傾角和摩擦影響存在加速度。后續(xù)可以考慮α-β-γ觀測器卡爾曼濾波將導(dǎo)軌傾角作為模型輸入建立小球動力學(xué)模型。10. 參數(shù)散布在代碼中大量經(jīng)驗參數(shù)直接寫在條件分支中例如7、12、10、120、500等。建議集中定義為宏或配置結(jié)構(gòu)體便于參數(shù)說明在線調(diào)參不同機械平臺復(fù)用記錄實驗版本。二十二、報告結(jié)論的總結(jié)k230pid.c構(gòu)建了一套以K230視覺識別為傳感器、MSPM0為控制器、步進電機為執(zhí)行器的小球位置閉環(huán)控制系統(tǒng)。系統(tǒng)以10ms定時周期運行通過串口接收小球相對目標點的位置誤差并利用α-β狀態(tài)觀測器對位置進行濾波、對速度進行估計。在實際控制路徑中程序采用位置外環(huán)和速度內(nèi)環(huán)組成的串級控制結(jié)構(gòu)位置環(huán)根據(jù)位置偏差產(chǎn)生目標速度速度環(huán)再根據(jù)目標速度與估計速度之差生成導(dǎo)軌的目標傾斜位置。為了適應(yīng)真實機械系統(tǒng)程序還設(shè)計了目標速度限幅、電機位置限位、單周期最大步數(shù)限制、視覺數(shù)據(jù)超時保護、運動狀態(tài)多幀確認、靜摩擦補償和最終穩(wěn)定判斷等機制。此外程序能夠通過視覺誤差的跳變識別兩階段目標切換并在切換時清除舊控制狀態(tài)及重置觀測器避免目標坐標變化被錯誤解釋為小球高速運動。該程序體現(xiàn)了PID控制、串級控制、狀態(tài)觀測、數(shù)字濾波、狀態(tài)機、執(zhí)行器飽和、通信看門狗和嵌入式中斷編程等多方面知識。其主要特點不是單純依靠一個固定參數(shù)PID而是通過觀測器、串級反饋及多種經(jīng)驗補償共同提高系統(tǒng)在視覺噪聲、靜摩擦、慣性和通信丟幀條件下的穩(wěn)定性。