做六軸機(jī)械臂項(xiàng)目做到中期以后大家問得最多的往往不是“怎么讓舵機(jī)轉(zhuǎn)起來”而是“怎樣讓它按照我想要的軌跡動(dòng)起來”。如果要靠運(yùn)動(dòng)學(xué)逆解去編寫一段手腕翻轉(zhuǎn)、抓取、回位的組合動(dòng)作對(duì)數(shù)學(xué)和代碼能力的要求都不低但如果采用示教軌跡復(fù)現(xiàn)的方式整個(gè)過程就變成“人拖動(dòng)小機(jī)械臂做一遍單片機(jī)記下來再讓執(zhí)行臂重放”工程門檻會(huì)一下子低很多。這一期教程圍繞六自由度機(jī)械臂的示教操作與軌跡復(fù)現(xiàn)展開控制核心使用 STM32F103 單片機(jī)。文章會(huì)先講清楚整體架構(gòu)和硬件接線再給出可落地的 PWM 舵機(jī)驅(qū)動(dòng)、ADC 角度采集、軌跡記錄和回放狀態(tài)機(jī)設(shè)計(jì)思路最后補(bǔ)充常見故障排查和工程注意事項(xiàng)。無論你是在做課程設(shè)計(jì)、畢業(yè)設(shè)計(jì)還是自己組一套六軸機(jī)械臂玩這套思路都可以直接遷移。1. 為什么六軸機(jī)械臂要做“示教操作 軌跡復(fù)現(xiàn)”很多剛開始接觸六軸機(jī)械臂的同學(xué)第一反應(yīng)是讓機(jī)械臂完成“從 A 點(diǎn)抓取放到 B 點(diǎn)”這樣的任務(wù)。要做到這一點(diǎn)常規(guī)思路是給機(jī)械臂建立 DH 參數(shù)模型。標(biāo)定六個(gè)關(guān)節(jié)的零位和運(yùn)動(dòng)范圍。寫出正運(yùn)動(dòng)學(xué)把關(guān)節(jié)角度換算成末端位姿。再寫逆運(yùn)動(dòng)學(xué)把目標(biāo)位姿換算成六個(gè)關(guān)節(jié)角。規(guī)劃軌跡生成每個(gè)時(shí)刻的關(guān)節(jié)位置。最后讓控制器逐點(diǎn)執(zhí)行。這套流程當(dāng)然是最接近工業(yè)機(jī)器人開發(fā)的但放在 DIY 六軸機(jī)械臂上問題也很明顯結(jié)構(gòu)件加工誤差、舵機(jī)安裝偏差、減速箱回差都會(huì)讓理論模型和實(shí)際運(yùn)動(dòng)對(duì)不上。如果每一段動(dòng)作都想靠代碼去“算”出來調(diào)試周期會(huì)非常長。示教操作則繞開了復(fù)雜的運(yùn)動(dòng)學(xué)推導(dǎo)。它的核心思想是由人直接拖動(dòng)一套與執(zhí)行機(jī)械臂結(jié)構(gòu)相似的小機(jī)械臂各關(guān)節(jié)的角度傳感器實(shí)時(shí)檢測(cè)關(guān)節(jié)位置主控把傳感器數(shù)據(jù)轉(zhuǎn)換為舵機(jī) PWM 信號(hào)驅(qū)動(dòng)另一條機(jī)械臂同步跟隨?！巴蟿?dòng)小機(jī)械臂”是人工示教比較常用的一種做法。好處有三個(gè)不需要逆解操作人員無需關(guān)心關(guān)節(jié)角度只要用手把教臂擺成目標(biāo)姿態(tài)即可。動(dòng)作直觀教臂轉(zhuǎn)多少執(zhí)行臂就按預(yù)設(shè)比例轉(zhuǎn)多少效果所見即所得?？梢钥焖佼a(chǎn)生軌跡把示教過程中每個(gè)時(shí)刻的六個(gè)關(guān)節(jié)角度按固定時(shí)間間隔保存下來就能形成一條完整軌跡需要復(fù)現(xiàn)時(shí)單片機(jī)按同樣的時(shí)間間隔把角度點(diǎn)回放出來即可。因此本文要做的事情并不復(fù)雜讀取教臂角度、實(shí)時(shí)驅(qū)動(dòng)執(zhí)行臂、定時(shí)保存軌跡點(diǎn)、按記錄順序回放。這正好是 STM32F103 這類資源不多、但控制能力足夠的 MCU 所擅長的任務(wù)。2. 整體方案與工作流程2.1 系統(tǒng)由哪幾部分組成整套系統(tǒng)可以劃分為四個(gè)部分模塊作用典型器件示教小機(jī)械臂人手拖動(dòng)提供六個(gè)關(guān)節(jié)的目標(biāo)角度六軸串聯(lián)結(jié)構(gòu)各關(guān)節(jié)安裝角度傳感器角度采集電路把機(jī)械角度變成 MCU 可讀的電信號(hào)旋轉(zhuǎn)電位器或 AS5600 等磁編碼器主控板讀取角度、處理按鍵、記錄和回放軌跡STM32F103C8T6 最小系統(tǒng)板執(zhí)行機(jī)械臂按主控給出的 PWM 信號(hào)運(yùn)動(dòng)六個(gè) PWM 數(shù)字舵機(jī) 結(jié)構(gòu)件這里要說明一下很多同學(xué)會(huì)問為什么不讓執(zhí)行臂直接被手拖動(dòng)對(duì)于普通舵機(jī)驅(qū)動(dòng)的機(jī)械臂舵機(jī)內(nèi)部有減速齒輪組通電后反向拖動(dòng)很容易導(dǎo)致舵機(jī)堵轉(zhuǎn)、發(fā)熱甚至燒毀。因此更安全的 DIY 示教方案是采用“主從式”結(jié)構(gòu)人拖動(dòng)的是無動(dòng)力或輕負(fù)載的教臂執(zhí)行臂只在復(fù)現(xiàn)階段承擔(dān)運(yùn)動(dòng)任務(wù)。如果想讓執(zhí)行臂本身支持直接拖動(dòng)示教需要特殊設(shè)計(jì)的電機(jī)抱閘釋放機(jī)構(gòu)和絕對(duì)值角度反饋復(fù)雜度會(huì)高很多。2.2 數(shù)據(jù)流向用一句話概括系統(tǒng)流程人手拖動(dòng)教臂 - 六個(gè)關(guān)節(jié)電位器產(chǎn)生模擬電壓 - STM32F103 的 ADC 采樣 - 轉(zhuǎn)換為舵機(jī)脈寬 - 輸出 PWM 驅(qū)動(dòng)執(zhí)行臂 - 同時(shí)按時(shí)間間隔保存當(dāng)前角度 - 按鍵切換后按保存順序回放在實(shí)時(shí)跟隨模式下STM32F103 每 20ms 左右讀取一次六路 ADC 值并把這組值映射成 PWM 脈寬輸出。這個(gè)循環(huán)不斷重復(fù)看起來就是執(zhí)行臂跟隨教臂同步運(yùn)動(dòng)。在記錄模式下單片機(jī)除了實(shí)時(shí)跟隨還會(huì)把每一次的 PWM 脈寬保存到軌跡數(shù)組中。停止記錄后數(shù)組里保存的就是這段時(shí)間的完整動(dòng)作序列。在回放模式下單片機(jī)從軌跡數(shù)組第一個(gè)點(diǎn)開始每隔固定時(shí)間間隔輸出一組 PWM 脈寬直到全部回放結(jié)束。3. 本期硬件組裝與接線要點(diǎn)3.1 材料清單第二期既然要接入示教與復(fù)現(xiàn)功能硬件上需要增加“教臂”部分。由于材料包的螺絲、鋁件尺寸不一樣下面只給出通用清單類別說明執(zhí)行機(jī)械臂結(jié)構(gòu)底座、腰部、大臂、小臂、腕部等六軸結(jié)構(gòu)件教臂結(jié)構(gòu)件與執(zhí)行臂關(guān)節(jié)數(shù)量一致的小型串聯(lián)臂可以用 3D 打印也可以用型材搭建角度傳感器每個(gè)教臂關(guān)節(jié)一個(gè)旋轉(zhuǎn)電位器共 6 個(gè)舵機(jī)每個(gè)執(zhí)行關(guān)節(jié)一個(gè) PWM 舵機(jī)共 6 個(gè)主控板STM32F103C8T6 最小系統(tǒng)板或類似兼容板電源舵機(jī)獨(dú)立電源例如 2S 鋰電池加穩(wěn)壓模塊5V 主控供電按鍵與指示燈用于切換“記錄 / 回放 / 停止”接線材料杜邦線或者排線、銅柱、扎帶等如果你手里的執(zhí)行臂是“總線舵機(jī)”版本則舵機(jī)接口不再是普通 PWM通信協(xié)議不同本文示例代碼需要對(duì)應(yīng)替換以下內(nèi)容以最適合入門學(xué)習(xí)的 PWM 舵機(jī)為例。3.2 示教臂角度傳感器裝配對(duì)于 DIY 主從示教旋轉(zhuǎn)電位器是最容易獲取的角度傳感器。裝配時(shí)有幾點(diǎn)需要注意電位器要和關(guān)節(jié)旋轉(zhuǎn)軸同心否則轉(zhuǎn)動(dòng)時(shí)會(huì)憋住。每個(gè)關(guān)節(jié)最好有機(jī)械限位避免電位器超過有效電氣轉(zhuǎn)角。電位器兩端分別接 3.3V 和 GND中間抽頭接 STM32 的 ADC 引腳。如果某個(gè)關(guān)節(jié)方向反了可以在軟件里做反向映射不一定要重新改結(jié)構(gòu)。在功能上教臂并不需要很大的力矩關(guān)節(jié)轉(zhuǎn)動(dòng)越順滑越好。如果結(jié)構(gòu)件轉(zhuǎn)動(dòng)不順暢首先檢查軸承或軸套的安裝同軸度。3.3 執(zhí)行臂舵機(jī)與供電六自由度機(jī)械臂的六個(gè)舵機(jī)在啟動(dòng)和換向瞬間電流較大最忌諱的是用 STM32 最小系統(tǒng)板的 USB 口直接給舵機(jī)供電。更合理的供電方式是電池或電源適配器 - 穩(wěn)壓/降壓模塊 - 舵機(jī)電源正負(fù)極 - 舵機(jī)信號(hào)線接 STM32 PWM 引腳還需要特別注意舵機(jī)電源地、STM32 系統(tǒng)板地必須共地。很多“舵機(jī)不轉(zhuǎn)、舵機(jī)亂抖、單片機(jī)復(fù)位”的問題本質(zhì)都是沒有共地。舵機(jī)的信號(hào)線是高電平脈沖信號(hào)STM32 的 PWM 引腳輸出就可以驅(qū)動(dòng)。接線時(shí)不需要把所有舵機(jī)電源并到主控板上正確的電源走線是直接從電源模塊分出來再讓主控板單獨(dú)取電。3.4 引腳規(guī)劃與避免 IO 沖突STM32F103C8T6 的引腳資源有限。如果六個(gè)舵機(jī) PWM 和六路 ADC 都要占用引腳直接隨便接很容易沖突。一個(gè)比較直觀的分配方案如下功能引腳對(duì)應(yīng)外設(shè)舵機(jī) 0~3 PWMPA0、PA1、PA2、PA3TIM2_CH1~CH4舵機(jī) 4、5 PWMPA8、PA9TIM1_CH1、TIM1_CH2教臂 ADC 通道 0~5PA4、PA5、PA6、PA7、PB0、PB1ADC1_IN4~IN9這樣安排后PWM 引腳和 ADC 引腳不重疊六個(gè)舵機(jī)和六路電位器都能接入。如果你手里的 PCB 已經(jīng)定義好其他引腳以你手上的原理圖為準(zhǔn)重點(diǎn)是把引腳配置集中到宏定義或配置文件中方便后續(xù)調(diào)整。4. STM32F103 核心驅(qū)動(dòng)代碼解析4.1 舵機(jī) PWM 控制原理常見的模擬或數(shù)字舵機(jī)控制信號(hào)是周期約 20ms 的脈沖。脈沖寬度不同舵機(jī)輸出的角度不同脈寬典型角度0.5ms0° 附近1.5ms中間位置2.5ms最大角度附近不同廠家舵機(jī)的脈寬范圍有差異有的實(shí)際范圍是 0.5ms~2.5ms有的只有 1ms~2ms。建議先查舵機(jī)數(shù)據(jù)手冊(cè)然后在代碼里把范圍定義成宏方便標(biāo)定。在 STM32CubeMX 中將定時(shí)器時(shí)鐘配置成 72MHz預(yù)分頻器設(shè)為 71則計(jì)數(shù)器頻率為72MHz / 72 1MHz也就是計(jì)數(shù)器每計(jì)數(shù)一次為 1 微秒。如果把自動(dòng)重載值 ARR 設(shè)為 19999則 PWM 周期為(19999 1) * 1us 20000us 20ms這正好符合舵機(jī)的 20ms 周期要求。此時(shí)設(shè)置 CCR 比較值CCR 值脈寬500500us15001500us25002500us因此代碼里不需要寫很復(fù)雜的數(shù)學(xué)公式直接“往 CCR 寄存器寫入微秒值”即可。4.2 六路舵機(jī) PWM 輸出代碼假設(shè)你在 STM32CubeMX 中已經(jīng)生成了工程定時(shí)器句柄分別是htim1和htim2?？梢詫懸粋€(gè)統(tǒng)一的舵機(jī)設(shè)置接口// servo.c #include main.h #include servo.h #define PULSE_MIN_US 500 #define PULSE_MAX_US 2500 void Servo_SetPulse(uint8_t idx, uint16_t pulse_us) { if (pulse_us PULSE_MIN_US) pulse_us PULSE_MIN_US; if (pulse_us PULSE_MAX_US) pulse_us PULSE_MAX_US; switch (idx) { case 0: __HAL_TIM_SET_COMPARE(htim2, TIM_CHANNEL_1, pulse_us); break; case 1: __HAL_TIM_SET_COMPARE(htim2, TIM_CHANNEL_2, pulse_us); break; case 2: __HAL_TIM_SET_COMPARE(htim2, TIM_CHANNEL_3, pulse_us); break; case 3: __HAL_TIM_SET_COMPARE(htim2, TIM_CHANNEL_4, pulse_us); break; case 4: __HAL_TIM_SET_COMPARE(htim1, TIM_CHANNEL_1, pulse_us); break; case 5: __HAL_TIM_SET_COMPARE(htim1, TIM_CHANNEL_2, pulse_us); break; default: break; } } void Servo_InitAll(void) { HAL_TIM_PWM_Start(htim2, TIM_CHANNEL_1); HAL_TIM_PWM_Start(htim2, TIM_CHANNEL_2); HAL_TIM_PWM_Start(htim2, TIM_CHANNEL_3); HAL_TIM_PWM_Start(htim2, TIM_CHANNEL_4); /* TIM1 是高級(jí)定時(shí)器需要使能主輸出 */ __HAL_TIM_MOE_ENABLE(htim1); HAL_TIM_PWM_Start(htim1, TIM_CHANNEL_1); HAL_TIM_PWM_Start(htim1, TIM_CHANNEL_2); for (uint8_t i 0; i 6; i) { Servo_SetPulse(i, 1500); } }代碼里的“1500”表示舵機(jī)先回到 1.5ms 脈寬中位。實(shí)際項(xiàng)目中如果機(jī)械臂結(jié)構(gòu)安裝時(shí)零位并不在舵機(jī)中位這里需要按你的機(jī)械零位修改。4.3 六路 ADC 角度讀取如果教臂使用電位器ST 的 ADC1 可以依次掃描多個(gè)通道。為了減少抖動(dòng)我習(xí)慣對(duì)每個(gè)通道連續(xù)讀幾次然后取平均。下面是一個(gè)輕量實(shí)現(xiàn)// teach_adc.c #include main.h #include teach_adc.h static uint16_t adc_channel_map[6] { ADC_CHANNEL_4, // PA4 ADC_CHANNEL_5, // PA5 ADC_CHANNEL_6, // PA6 ADC_CHANNEL_7, // PA7 ADC_CHANNEL_8, // PB0 ADC_CHANNEL_9 // PB1 }; uint16_t ReadOneChannel(uint16_t channel) { ADC_ChannelConfTypeDef sConfig {0}; uint16_t value 0; sConfig.Channel channel; sConfig.Rank 1; sConfig.SamplingTime ADC_SAMPLETIME_239CYCLES_5; HAL_ADC_ConfigChannel(hadc1, sConfig); HAL_ADC_Start(hadc1); HAL_ADC_PollForConversion(hadc1, 10); value HAL_ADC_GetValue(hadc1); HAL_ADC_Stop(hadc1); return value; } void TeachArm_ReadAll(uint16_t *raw) { uint32_t sum 0; for (uint8_t ch 0; ch 6; ch) { sum 0; for (uint8_t sample 0; sample 5; sample) { sum ReadOneChannel(adc_channel_map[ch]); } raw[ch] sum / 5; } }這里之所以每讀一個(gè)通道都執(zhí)行HAL_ADC_Stop()是為了讓下一次切換通道時(shí)不要沿用上一次的配置。在 6 個(gè)電位器、采樣率要求不高的場(chǎng)景下這種寫法簡(jiǎn)單可靠。5. 示教、記錄與回放軟件設(shè)計(jì)5.1 狀態(tài)機(jī)設(shè)計(jì)機(jī)械臂示教不只是一個(gè)“循環(huán)讀角度”的過程還需要考慮“什么時(shí)候開始記錄”“什么時(shí)候停止”“什么時(shí)候回到軌跡首點(diǎn)”“什么時(shí)候開始回放”。使用狀態(tài)機(jī)會(huì)清晰很多。可以定義這樣幾種模式模式含義MODE_FOLLOW實(shí)時(shí)跟隨教臂運(yùn)動(dòng)但不記錄MODE_RECORD實(shí)時(shí)跟隨同時(shí)按周期保存軌跡點(diǎn)MODE_BACK_TO_START從當(dāng)前姿態(tài)平滑回到軌跡首點(diǎn)MODE_PLAYBACK逐點(diǎn)回放軌跡按鈕的功能可以這樣設(shè)計(jì)按鍵 1進(jìn)入記錄。如果已經(jīng)在記錄則停止記錄并進(jìn)入“待機(jī)”或“跟隨”模式。按鍵 2開始回放。如果采集到的軌跡點(diǎn)數(shù)為 0則按下無效。按鍵 3急?；驈?fù)位所有舵機(jī)輸出中位脈寬。采用這種劃分后主循環(huán)的邏輯會(huì)很清楚。5.2 軌跡數(shù)據(jù)結(jié)構(gòu)使用 STM32F103C8T6 時(shí)RAM 只有約 20KB。如果六個(gè)關(guān)節(jié)都保存為uint16_t每個(gè)軌跡點(diǎn)需要6 * 2 12 字節(jié)如果保存 800 個(gè)點(diǎn)總?cè)萘繛?9600 字節(jié)約占 RAM 的一半。示例中先按這個(gè)規(guī)模設(shè)計(jì)// trajectory.h #define SERVO_NUM 6 #define MAX_TRAJ_POINT 800 typedef struct { uint16_t pulse[SERVO_NUM]; } TrajPoint; extern TrajPoint g_traj[MAX_TRAJ_POINT]; extern volatile uint16_t g_traj_len;當(dāng)然800 個(gè)點(diǎn)通常不夠保存幾分鐘的長軌跡。如果希望斷電保存或保存更長時(shí)間可以外接 SPI Flash、SD 卡或者用串口把數(shù)據(jù)發(fā)送到上位機(jī)保存。這里先把 RAM 方案跑通再考慮擴(kuò)展存儲(chǔ)。5.3 實(shí)時(shí)跟隨與軌跡記錄下面這段代碼是核心循環(huán)邏輯// main.c 中的循環(huán)片段 void MainLoop_Tick(void) { static uint32_t last_record_tick 0; static uint32_t last_follow_tick 0; uint32_t now HAL_GetTick(); if (g_mode MODE_FOLLOW || g_mode MODE_RECORD) { if (now - last_follow_tick 20) { uint16_t raw[6]; TeachArm_ReadAll(raw); for (uint8_t i 0; i 6; i) { g_current_pulse[i] MapRawToPulse(i, raw[i]); Servo_SetPulse(i, g_current_pulse[i]); } last_follow_tick now; } } if (g_mode MODE_RECORD) { if (now - last_record_tick