基于磁導航的直立輪式機器人控制系統設計

摘要：能夠自主循跡的輪式移動機器人在巡檢、搬運、配送等領域已經廣泛應用，但是在狹窄空間的特殊場景中難以靈活轉向。針對這一問題，文章以S9KEAZ128AMLK單片機為核心控制器，設計了一種基于磁導航的智能行駛直立輪式機器人。單片機通過傾斜角及角速度的互補擬合獲取姿態數據，由LC并聯諧振電路對導航磁場進行選頻獲取路徑信息，將直立、轉向和轉速3個閉環控制信號共同作用于左右輪電機，使輪式機器人在保持直立姿勢的同時，能夠根據道路偏移量改變方向和轉速。

關鍵詞：KEA128；磁導航；直立控制；輪式機器人

中圖分類號：TP242

文獻標志碼：A

0 引言

輪式機器人作為移動機器人家族中的重要角色，具有高度靈活性和機動性，在日常巡檢、搬運、配送等領域中廣泛應用。直立輪式機器人因同時具備直立平衡的穩定性和輪式移動的高效性，被應用于一些狹窄空間內需要靈活轉向的場景^［1］。

本文采用S9KEAZ128AMLK單片機，設計了一種基于磁導航的智能行駛直立輪式機器人。硬件系統包括電源管理、單片機最小系統、信號采集、電機驅動、人機交互接口等功能電路。軟件系統根據分周期定時中斷的編程思路，在不同中斷周期內循環執行路徑識別、直立控制、轉向控制、轉速調節、疊加輸出等任務，通過兩輪的電機速度變化實現了機器人的直立保持、方向循跡和轉速調節^［2］。

1 系統控制原理

直立輪式機器人的主要功能包括磁導航識別、直立姿態控制和轉向轉速調節^［3］。

道路中心鋪設一根20 kHz/100mA交流導線，導線周圍便形成了20 kHz的交變磁場。根據電磁感應定律，該交變磁場中的線圈會產生感應電動勢，其大小和通過線圈回路磁通量的變化率成正比。利用這一原理，路徑信息采集采用LC并聯諧振電路對導航磁場進行識別，根據選頻公式可以確定電感和電容的具體值：

四路電感在道路中均勻排布，通過對四路信號的放大、檢波及計算，獲取當前位置與道路中心線的偏差值，作用于兩側車輪電機的差速控制，以實現機身轉向。

直立輪式機器人類似一個倒立單擺模型，當出現傾斜一定角度后，雙輪朝機身傾斜方向加速運動，獲得保持直立平衡的恢復力，圖1是車輪倒立擺力學分析。

當機身向右傾斜θ角時，車輪需要向右以a大小的加速度運動，即可獲得恢復力：

F=mgsinθ-macosθ≈mgθ-mk₁θ（2）

式（2）將θ傾斜角視作較小值進行了線性化處理，k₁ 是加速度 a與傾斜角θ的比例系數，要使得恢復力的方向與傾斜方面相反，需要k₁gt;g。為了使直立盡快恢復，消除系統振蕩，還需要在恢復力中適當添加控制阻尼：

F=mgθ-mk₁θ-mk₂ω（3）

式（3）中，ω 是傾斜角速度，k₂ 是阻尼力的加速度系數，車輪從傾斜恢復到直立平衡狀態的運動加速度大小是：

a=k₁θ+k₂ω（4）

式（4）中，參數 k₁決定了機身是否能夠穩定到垂直平衡位置，須大于重力加速度，k₂決定了恢復到垂直位置的阻尼系數，可以有效抑制機身振蕩。在傾斜角反饋控制中，為了計算直立恢復加速度a，與角度成比例的分量使用比例P控制，與角速度成比例的分量使用微分D控制。為了準確獲取傾斜角，姿態采集使用加速度計和陀螺儀。加速度計的傾角測量與軸上的重力分量有關，機身擺動時測量不準確；陀螺儀能夠直接精確測量傾斜角速度，經積分計算后即可獲得傾斜角度，但是，長期積分運算會形生累積誤差。圖2是直立控制結構圖，系統將傾角和角速度通過互補濾波的方法進行修正，控制電機驅動電壓，使得機身產生同向加速度以恢復平衡。

2 硬件電路設計

硬件電路的整體結構如圖3所示，主要包括電源管理、單片機最小系統、電磁信號采集、橋式電機驅動、姿態信息采集、速度檢測接口、人機交互接口等功能電路模塊。

2.1 電源管理

系統由一塊7.2 V/2000mAh的蓄電池供電，包括12 V、5 V、3.3 V 3個電壓等級。其中，12 V升壓電路采用開關型穩壓芯片MC34063，用于電機驅動；5 V和3.3 V均使用了低壓差、低噪聲的TPS73系列高精度開關穩壓芯片，電池電壓先由TPS7350穩壓至5 V，用于單片機和各信號采集傳感器；5 V經TPS7333穩壓至3.3 V，用于人機交互和藍牙調試，如圖4所示。

2.2 單片機最小系統

S9KEAZ128AMLK是NXP公司基于ARM Cortex-M0+ 內核的32位單片機，具有128 KB Flash ROM和16 KB RAM，體積小、功耗低、運算快且片內外設資源豐富，如8通道PWM，8通道AD轉換，2通道定時器，SCI、I²C、SPI等串行接口以及多達50個I/O端口。KEA128單片機最小系統的設計包括電源電路、復位電路、時鐘電路和下載電路^［4］。

2.3 電磁信號采集

磁導航信號的采集與處理由選頻、放大和檢波三部分電路構成，如圖5所示。在20 kHz交變導航磁場中放置10mH電感和6.8 nF電容構成的LC并聯諧振電路采集電磁信號，由于信號幅值較小、容易衰減，電路采用可調增益的LM386集成運算放大器進行信號放大，放大后的正弦交流信號經二極管檢波、電容濾波后，取樣電壓被送至單片機的AD口進行模數轉換。

2.4 電機驅動

直立、轉向和調速三大控制都是通過電機驅動左右輪完成的，電機驅動是直立輪式機器人輸出控制的核心模塊，采用了經典的可逆雙橋型“H橋”電路。單電機驅動電路如圖6所示，由4個分立的N溝道MOSFET IRLR7843配合專用驅動芯片IR2104搭建而成，電樞回路總電阻小、輸出驅動電流大、調速范圍寬。單片機通過輸出不同占空比的PWM信號，能夠控制左右輪電機的正反轉和調速。

姿態數據采集電路選用六軸運動處理芯片MPU6050，免除了組合加速度計與陀螺儀時間軸之差的問題，減少了封裝空間。硬件系統中還添加了編碼器、顯示屏和藍牙模塊的接口電路，主要用于左右輪的速度采集和調試運行中的人機交互。

3 軟件系統設計

軟件系統主要包括主程序和中斷服務子程序的設計，主程序主要完成各模塊的初始化以及道路信號、速度值和傾斜角等狀態參數的顯示。

數據采集和直立、速度、方向三閉環控制均在定時中斷程序中完成。由于周期性控制任務較多，為保證信號采集和輸出控制的及時性，程序設計采用大小周期的設計思路。首先定時中斷時間設置為1ms，5次中斷為一個大循環控制周期，5次中斷小周期分別對應完成5項不同的控制任務，即信號采集、直立控制、速度控制、方向控制、PWM合成，于是每項任務的循環周期都是5ms的整數倍，不僅程序設計結構清晰、邏輯緊密，而且信號反饋速度快、控制精度高，具體流程如圖7所示。

PIT1周期，單片機的AD通道采集4組磁導航傳感器的取樣電壓，為了消除抖動，每路通道連續采集10次，經過均值濾波后再進行歸一化處理，保證路徑識別不受導航信號源的影響，然后經過差比和計算偏差量，以表示機身偏離道路中心的多少。

PIT2周期，系統根據傾斜角大小進行直立控制，單片機由I²C總線讀取MPU6050中陀螺儀和加速度計的實時值后，通過一階互補濾波算法根據不同權重擬合出傾斜角，再由PD算法計算出直立控制輸出信號。

PIT3周期，程序根據道路偏差量由位置式PID得到方向控制輸出信號，調節左右輪的差速來直行或轉彎，實現方向控制。為了使方向控制更加精準，比例環節采用分段式P參數，即偏移量在不同區間對應

不同的比例系數，可以減少小角度轉向時的方向矯正幅度。

PIT4周期，程序每隔固定周期對左右輪的編碼器進行正交脈沖計數，獲取車輪的實時速度。由于速度控制精度要求不高，采集可以緩慢一些，程序通過設定20次軟件計數的方式，每隔100ms進行一次速度的增量式PID調節，輸出速度控制信號。

PIT5周期，程序將前面直立、轉向和轉速控制的輸出信號按左右輪分別進行疊加和限幅，合成4個作用于橋式驅動電路的PWM信號，通過調節占空比大小分別改變左右電機的轉速，同時實現直立、速度和方向的三閉環控制。

為了方便調試，機身安裝了藍牙發送模塊，PC端外接藍牙接收模塊，程序通過串行中斷發送加速度計值和陀螺儀值，PC端使用虛擬示波器軟件工具監測姿態數據波形的變化，顯示互補擬合后的傾斜角波形，從圖8可以看出，擬合后的傾斜角度平滑連續、跟蹤性好，能夠有效地緩解電機振動以及機身行駛時產生的抖動，使控制更加穩定。

如圖9所示，磁導航信號的采集和轉向、轉速控制相關狀態參數能夠通過機載OLED屏幕實時監測，主界面顯示參數包括電磁信號、姿態信息、傾斜角度、道路中心偏差量、左右輪電機PWM占空比大小等，主要用于控制參數的調試。

4 結語

以 KEA128單片機為核心，基于磁導航循跡的兩輪直立輪式機器人控制系統利用陀螺儀與加速度計的輸出值進行互補融合，獲取機身的姿態數據后控制左右輪電機的加速度，以完成直立控制；采用電磁感應電路識別道路信息，通過電機差速實現轉向控制；利用編碼器檢測行駛速度，控制電機完成速度調節。經過軟硬件測試，兩輪直立機器人能夠實現自動循跡行駛，具有較好的穩定性。

參考文獻

［1］李全民，余華柱，楊杰勇.基于S9KEA128AMLK的智能電磁循跡小車設計與實現［J］.機電工程技術，2022（11）：153-158.

［2］王全，陳學海，楊光友，等.田間輪式機器人的導航控制系統設計［J］.農機化研究，2024（8）：115-119.

［3］葉身村，周慶，于振浩.基于KEA128的電磁直立智能車控制系統設計［J］.汽車實用技術，2018（21）：27-28.

［4］梁家耀，吳子勇，周文駿，等.基于MM32的兩輪平衡車控制系統設計［J］.無線互聯科技，2023（17）：48-50.

（編輯 王永超）

Design of control system for upright wheeled robot based on magnetic navigation

Luo" Jiayi

（Ma’anshan Technical College， Ma’anshan 243031， China）

Abstract：Wheeled mobile robots capable of autonomous trajectory have been widely used in inspection， handling， distribution and other fields， but it is difficult to steer flexibly in special scenarios in narrow spaces. To address this problem， the article designs an intelligent traveling upright wheeled robot based on magnetic navigation with S9KEAZ128AMLK microcontroller as the core controller. The microcontroller obtains attitude data through complementary fitting of tilt angle and angular velocity， and the LC parallel resonant circuit selects the frequency of the navigation magnetic field to obtain the path information， and three closed-loop control signals of upright， steering and rotational speed are applied to the left and right wheel motors， so that the wheeled robot maintains the upright posture while changing direction and rotational speed according to the road offset.

Key words：KEA128; magnetic navigation; upright control; wheeled robots