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