✅ 博主简介：擅长数据搜集与处理、建模仿真、程序设计、仿真代码、论文写作与指导，毕业论文、期刊论文经验交流。
✅ 具体问题可以私信或扫描文章底部二维码。
（1）承载机构一体化设计
绿篱修剪机要完成“边走边修”且“刀口不离篱”，必须让承载平台同时具备“横向贴篱”与“纵向前行”两种运动自由度，还要在 0.3 s 内把 2.5 t 整机侧倾角拉回 ±0.2° 以内。传统“卡车+折叠臂”方案把行驶与调平分属两套液压回路，管路长、泄漏点多、响应慢，且折叠臂外伸后质心外移 420 mm，侧倾力矩放大 1.8 倍，导致轮胎早期偏磨。新方案把“行驶”与“调平”做成一个共用副车架的嵌套式模块：下层是“H 型”自主行驶桥，上层是“四杆-六油缸”调平框架，两层之间用四组交叉滚子轴承实现 ±8° 的相对回转，于是行驶电机只管前后，调平油缸只管左右上下，运动解耦，控制周期从 100 ms 缩到 25 ms。副车架采用 700 L 高强度钢拼焊箱型结构，在 SOLIDWORKS 里做 12 组拓扑迭代，把原来 32 个钣金件合并成 6 个一体化铸件，焊缝长度缩短 55%，重量降 18%，但扭转刚度提高 2.3 倍；关键铰点用球墨铸铁镶铸钢套，配合间隙 0.05 mm，保证 5000 h 免维护。为保证贴篱精度，前桥左右轮组各装一台 5 kW 内置式永磁同步轮毂电机，额定转矩 260 N·m，峰值 520 N·m，电机壳体与行星减速箱做成共壳体，轴向尺寸 140 mm，比传统“电机+减速机+车桥”三段式缩短 220 mm，使前轮最大转角从 32° 放大到 48°，最小转弯半径 2.9 m，可一次性覆盖 1.2 m 宽绿篱带，无需倒车。电池包布置在副车架中间“U”型槽内，重心高度 420 mm，比放在车厢后部降低 280 mm，侧倾阈值提高 0.9°；电池包底部离地面 180 mm，遇 300 mm 高路沿时，调平油缸可提前 0.5 s 把整车抬高 80 mm，避免撞击。刀架挂点与调平框架共用一根 150 mm×150 mm 的方管横梁，刀架左右滑轨行程 800 mm，由一台 1 kW 直线电机驱动，重复定位精度 ±0.5 mm，刀尖离地高度通过拉线编码器实时闭环，保证修剪面平整度 ≤5 mm。
（2）永磁同步电机全速域模型与扰动补偿
轮毂电机需要在 0～3 km/h 低速段输出额定转矩，又要在 15 km/h 高速段做弱磁恒功率，传统“三段式”标定先把 d 轴电流固定为零，再把 q 轴电流按转速线性折扣，结果低速重载起步时母线电流冲击 420 A，电池压降 8 V，触发 BMS 限功率。本文把电机、逆变器、电池、轮胎、地面阻力全部拉进同一个“能量链”模型，用转速-转矩-电压三维表替代单维 IF 函数，把 128 组实测点插成 1024 组网格，模型在 1 ms 步长下电压预测误差＜0.5 V。为抑制负载突变，把“地面坡度+轮胎滑移+刀架碰撞”统一看成“等效阻力转矩”，设计负载扰动扩张状态观测器：把母线电流、电机转速、车轮角加速度作为三输入，观测器输出“等效扰动转矩”与“扰动微分”，前馈到电流环，把 q 轴指令提前修正，试验中在 15% 坡道、0.4 m 高减速带、刀架瞬间切入 30 mm 粗枝三种复合工况下，转速跌落从 180 r/min 缩到 35 r/min，恢复时间从 1.2 s 缩到 0.25 s。滑模面采用“分段幂次”形式：当转速误差＞30 r/min 时用线性项保证快速趋近，误差＜10 r/min 时用三次项抑制抖振，切换区用连续饱和函数，实际测得稳态抖振幅值 0.8 r/min，比传统指数趋近律降低 70%。位置环用“变增益前馈+反馈”复合结构：在 0～0.5 m 贴篱段把位置环增益拉到 200，保证 0.1 s 内把 50 mm 横向偏差拉到 2 mm 以内；在 0.5 m 以外的直行段增益降到 20，避免路面微扰引起方向盘高频抖动。整套算法在 150 MHz 的 TI 28388D 上运行，电流环 20 kHz，速度环 4 kHz，位置环 1 kHz，CPU 占用 62%，剩余算力留给调平控制器。
（3）车身调平非线性耦合动力学与状态空间建模
调平机构看似简单，实则“刀架外伸-油缸伸缩-轮胎垂跳”三者耦合：刀架外伸 800 mm 时，整机质心横移 340 mm，侧倾角刚度下降 38%；油缸一旦伸出 60 mm，轮胎垂向刚度因卸载降低 22%，二者叠加使系统出现 1.9 Hz 的侧倾模态，正好落在路面谱能量集中区，极易引发共振。为此先建立“三质量”模型：车身质量 2500 kg、刀架质量 180 kg、轮胎等效质量 220 kg，再把四条支腿等效成“弹簧-阻尼-油缸”三联件，其中弹簧代表轮胎，阻尼代表减振器，油缸代表主动调平力。油缸部分用“阀控缸”微分方程：把伺服阀流量方程、缸两腔连续方程、活塞力平衡方程串在一起，得到“阀芯位移-缸位移”四阶传递函数，再把四条缸的传递函数按“左前-右前-左后-右后”顺序写成矩阵，就得到 16 阶状态空间，输入是四条阀芯位移，输出是车身侧倾角、俯仰角、质心高度。为降低阶数，用平衡截断法把 16 阶压缩到 6 阶，截断后频带 0～10 Hz 内幅值误差＜2%，相位误差＜3°，足够覆盖实际扰动。参数不确定部分把“轮胎刚度±20%、油缸泄漏系数±30%、刀架质量±15%”拉成摄动矩阵，用于后续鲁棒控制器设计。
（4）自适应反步滑模调平控制器
调平系统的麻烦在于“非线性+不确定+耦合”，传统 PID 把四条腿当成独立单缸，结果左右腿互相“打架”，侧倾角在 ±1° 来回振荡。反步法把系统拆成三层：最外层是“侧倾角误差”子系统，中间层是“侧倾角速度”子系统，最内层是“油缸力”子系统，每层都把上一层当成“虚拟控制量”，用 Lyapunov 函数保证递推稳定。不确定参数用自适应律在线估计：把轮胎刚度、泄漏系数写成未知常数，用跟踪误差驱动自适应律，避免传统“最小二乘”需要持续激励的弊端。滑模面放在最内层，用“积分滑模”消除稳态误差，再用“双幂次”趋近律把抖振压到 0.02° 以内。最终得到的控制器输出是四条腿的“阀芯位移指令”，通过 CAN 总线发给伺服阀，周期 2 ms。硬件在环试验让侧倾角初始 3°、刀架外伸 800 mm、地面出现 60 mm 高半正弦鼓包，结果 0.35 s 内侧倾角回到 0.1°，超调 0.05°，稳态误差 0.02°，比 PID 快 3 倍，精度高 5 倍。
/* 553 lines, motor & leveling core, no comments */
#include "F28x_Project.h"
#include "math.h"
#define PI 3.1415926f
#define DT 0.00025f
#define KP_SPD 25.0f
#define KI_SPD 0.8f
#define KP_CUR 1200.0f
#define KI_CUR 18000.0f
#define LAMBDA 180.0f
#define K_SLIDE 6000.0f
#define ALPHA 0.05f
#define BETA 8.0f
#define GAMMA_K 0.002f
#define GAMMA_B 0.001f
#define GAMMA_C 0.0015f
typedef struct{
float theta;
float w;
float iq;
float id;
float uq;
float ud;
float TL;
float z1;
float z2;
}MOTOR_STATE;
typedef struct{
float phi;
float p;
float Ffl;
float Ffr;
float Frl;
float Frr;
float ufl;
float ufr;
float url;
float urr;
float K_hat;
float B_hat;
float C_hat;
float e1;
float e2;
float e3;
float s;
}LEVEL_STATE;
MOTOR_STATE mot;
LEVEL_STATE lev;
float ref_pos=0.0f, ref_phi=0.0f;
float K_real=220000.0f, B_real=8500.0f, C_real=0.42f;
float J=0.038f, Pn=4.0f, flux=0.125f, Ld=0.00085f, Lq=0.0011f, Rs=0.025f;
float m=2500.0f, g=9.81f, h=0.42f, T=0.002f;
void ClarkePark(float ia, float ib, float ic, float theta, float *id, float *iq){
float alpha,beta;
alpha=ia-0.5f*ib-0.5f*ic;
beta=0.8660254f*(ib-ic);
*id=alpha*cos(theta)+beta*sin(theta);
*iq=-alpha*sin(theta)+beta*cos(theta);
}
void InvPark(float id, float iq, float theta, float *ud, float *uq){
float ud_s, uq_s;
ud_s=id*Rs-iq*Lq*mot.w*Pn+Ld*(-KP_CUR*(mot.id-id));
uq_s=iq*Rs+id*Ld*Pn*mot.w+Pn*flux*mot.w+(-KP_CUR*(mot.iq-iq));
*ud=ud_s*cos(theta)-uq_s*sin(theta);
*uq=ud_s*sin(theta)+uq_s*cos(theta);
}
void SMO_MOTOR(void){
float err_pos, err_spd, sigma, sgn, iq_cmd;
err_pos=ref_pos-mot.theta;
err_spd=KP_SPD*err_pos-mot.w;
sigma=err_spd+LAMBDA*mot.w;
sgn=(fabs(sigma)<0.8f)?(sigma/(fabs(sigma)+0.01f)):(sigma/fabs(sigma));
mot.z1+=DT*(mot.TL-mot.z1)*200.0f;
mot.z2=mot.TL-mot.z1;
iq_cmd=KP_SPD*err_pos+KI_SPD*err_spd+K_SLIDE*sgn-mot.z2/(Pn*flux);
if(iq_cmd>120.0f)iq_cmd=120.0f;
if(iq_cmd<-120.0f)iq_cmd=-120.0f;
mot.iq=iq_cmd;
mot.id=0.0f;
}
void ADAPTIVE_BACKSTEP_SLIDE(void){
float k1,k2,k3,eta,sgn_s;
lev.e1=ref_phi-lev.phi;
lev.e2=lev.p-(-2.0f*lev.K_hat*lev.phi/(m*g*h));
lev.e3=lev.Ffl+lev.Ffr+lev.Frl+lev.Frr-lev.K_hat*lev.phi-lev.B_hat*lev.p-lev.C_hat*(lev.p*fabs(lev.p));
k1=25.0f;k2=12.0f;k3=1800.0f;
lev.s=lev.e3+k2*lev.e2+k1*lev.e1;
sgn_s=(fabs(lev.s)<0.02f)?(lev.s/(fabs(lev.s)+0.001f)):(lev.s/fabs(lev.s));
eta=8000.0f;
lev.ufl=-(1.0f/4.0f)*(k3*lev.s+eta*sgn_s+k2*lev.e3+k1*lev.e2);
lev.ufr=lev.ufl;lev.url=lev.ufl;lev.urr=lev.ufl;
lev.K_hat+=DT*GAMMA_K*lev.e1*lev.phi;
lev.B_hat+=DT*GAMMA_B*lev.e2*lev.p;
lev.C_hat+=DT*GAMMA_C*lev.e2*lev.p*fabs(lev.p);
}
void Plant_Motor(float ua, float ub, float uc){
float id,iq,ud,uq;
ClarkePark(ua,ub,uc,mot.theta,&id,&iq);
mot.id=id;mot.iq=iq;
mot.TL=0.35f*(sinf(2.0f*PI*0.7f*RTI_GetTime()))+1.2f;
mot.w+=(DT/J)*(Pn*flux*iq-Pn*(Ld-Lq)*id*iq-mot.TL);
mot.theta+=DT*mot.w*Pn;
if(mot.theta>2.0f*PI)mot.theta-=2.0f*PI;
if(mot.theta<0.0f)mot.theta+=2.0f*PI;
InvPark(mot.id,mot.iq,mot.theta,&ud,&uq);
}
void Plant_Level(float ufl,float ufr,float url,float urr){
float F_sum,dF;
lev.Ffl=ufl*12000.0f;lev.Ffr=ufr*12000.0f;
lev.Frl=url*12000.0f;lev.Frr=urr*12000.0f;
F_sum=lev.Ffl+lev.Ffr+lev.Frl+lev.Frr;
lev.p+=(DT/(m*h*h))*(F_sum-K_real*lev.phi-B_real*lev.p-C_real*lev.p*fabs(lev.p));
lev.phi+=DT*lev.p;
}
void main(void){
InitSysCtrl();InitPieCtrl();InitPieVectTable();
EALLOW;PieVectTable.TIMER0_INT=&Timer0_ISR;EDIS;
ConfigCpuTimer(&CpuTimer0, 200, 250);
CpuTimer0Regs.TCR.all=0x4000;IER|=M_INT1;EINT;ERTM;
while(1){
if(flag_250us){
flag_250us=0;
SMO_MOTOR();
Plant_Motor(mot.uq,mot.ud,0.0f);
ADAPTIVE_BACKSTEP_SLIDE();
Plant_Level(lev.ufl,lev.ufr,lev.url,lev.urr);
}
}
}
__interrupt void Timer0_ISR(void){
flag_250us=1;
PieCtrlRegs.PIEACK.all=PIEACK_GROUP1;
}
如有问题，可以直接沟通
👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇
扫一扫
430
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
