cart-pole 建模

使用matlab symbolic toolbox进行拉格朗日建模，并转为LTI模型

%% Cart-Pole Lagrangian modeling + symbolic linearization (upright, theta=0) clear; clc; syms x dx ddx real syms th dth ddth real syms M m l g u real % --- Generalized coordinates q = [x; th]; dq = [dx; dth]; ddq = [ddx; ddth]; % --- Kinetic & Potential energy % Cart velocity: dx % Pole mass position: px = x + l*sin(th), pz = l*cos(th) % Velocities: vpx = dx + l*cos(th)*dth; vpz = -l*sin(th)*dth; v2 = vpx^2 + vpz^2; T_cart = 0.5*M*dx^2; T_pole = 0.5*m*v2; T = T_cart + T_pole; % Potential energy (zero at upright th=0 ⇒ V = m*g*l*(1 - cos(th))) V = m*g*l*(1 - cos(th)); % Lagrangian L = T - V; % --- Generalized forces (only x is actuated by force u) Q = [u; 0]; % --- Euler-Lagrange: d/dt(dL/ddq) - dL/dq = Q dLd_dq = jacobian(L, q).'; % ∂L/∂q dLd_ddq = jacobian(L, dq).'; % ∂L/∂dq % Time derivative of dLd_ddq (treat q,dq as functions of time) % Use total derivative: d/dt = (∂/∂q)*dq + (∂/∂dq)*ddq d_dt_dLd_ddq = jacobian(dLd_ddq, q)*dq + jacobian(dLd_ddq, dq)*ddq; EL = d_dt_dLd_ddq - dLd_dq - Q; % = 0 gives equations of motion % Solve for accelerations ddq = [ddx; ddth] sol = solve(EL == 0, ddq); ddx_expr = simplify(sol.ddx); ddth_expr = simplify(sol.ddth); % --- Build nonlinear state-space: X=[x;dx;th;dth] f = [ dx; ddx_expr; dth; ddth_expr ]; g_u = jacobian(f, u); % input channel (should be a 4x1) f0 = subs(f, u, 0); % drift when u=0 % --- Linearize around upright equilibrium: x=0, dx=0, th=0, dth=0, u=0 xeq = [0; 0; 0; 0]; ueq = 0; A = simplify( subs( jacobian(f, [x, dx, th, dth]), ... [x, dx, th, dth, u], [xeq.', ueq] ) ); B = simplify( subs( g_u, [x, dx, th, dth, u], [xeq.', ueq] ) ); % --- Optional: pretty-print A, B disp('A ='); pretty(A); disp('B ='); pretty(B); % --- 验证：将A,B化简到常见形式（象征参数不数值化） A_simplified = simplify(A); B_simplified = simplify(B); % --- 如果想代入具体参数，取消注释以下行： % Mv = 1.0; mv = 0.1; lv = 0.5; gv = 9.81; % A_num = double(subs(A_simplified, {M,m,l,g}, {Mv,mv,lv,gv})); % B_num = double(subs(B_simplified, {M,m,l}, {Mv,mv,lv}));