⛄一、获取代码方式
获取代码方式1:
完整代码已上传我的资源:【车辆动力】基于matlab模拟停车动力学【含Matlab源码 2258期】
点击上面蓝色字体,直接付费下载,即可。
获取代码方式2:
付费专栏物理应用(Matlab)
备注:
点击上面蓝色字体付费专栏物理应用(Matlab),扫描上面二维码,付费299.9元订阅海神之光博客付费专栏,凭支付凭证,私信博主,可免费获得5份本博客上传CSDN资源代码(有效期为订阅日起,三天内有效);
点击CSDN资源下载链接:5份本博客上传CSDN资源代码
⛄二、案例简介
在纵向时,可能还会受到纵向空气阻力,前轮滚动阻力,后轮滚动阻力,坡道重力分量等
⛄三、部分源代码
function [x, u, L, Vx, Vxx, cost, trace, stop] = iLQG(DYNCST, x0, u0, Op)
% iLQG - solve the deterministic finite-horizon optimal control problem.
%
% minimize sum_i CST(x(:,i),u(:,i)) + CST(x(:,end))
% u
% s.t. x(:,i+1) = DYN(x(:,i),u(:,i))
%
% Inputs
% ======
% DYNCST - A combined dynamics and cost function. It is called in
% three different formats.
%
% 1) step:
% [xnew,c] = DYNCST(x,u,i) is called during the forward pass.
% Here the state x and control u are vectors: size(x)[n 1],
% size(u)[m 1]. The cost c and time index i are scalars.
% If Op.paralleltrue (the default) then DYNCST(x,u,i) is be
% assumed to accept vectorized inputs: size(x,2)size(u,2)K
%
% 2) final:
% [~,cnew] = DYNCST(x,nan) is called at the end the forward pass to compute
% the final cost. The nans indicate that no controls are applied.
%
% 3) derivatives:
% [,,fx,fu,fxx,fxu,fuu,cx,cu,cxx,cxu,cuu] = DYNCST(x,u,I) computes the
% derivatives along a trajectory. In this case size(x)[n N+1] where N
% is the trajectory length. size(u)[m N+1] with NaNs in the last column
% to indicate final-cost. The time indexes are I=(1:N).
% Dimensions match the variable names e.g. size(fxu)[n n m N+1]
% note that the last temporal element N+1 is ignored for all tensors
% except cx and cxx, the final-cost derivatives.
%
% x0 - The initial state from which to solve the control problem.
% Should be a column vector. If a pre-rolled trajectory is available
% then size(x0)[n N+1] can be provided and Op.cost set accordingly.
%
% u0 - The initial control sequence. A matrix of size(u0)[m N]
% where m is the dimension of the control and N is the number of state
% transitions.
%
%
% Op - optional parameters, see below
%
% Outputs
% =======
% x - the optimal state trajectory found by the algorithm.
% size(x)[n N+1]
%
% u - the optimal open-loop control sequence.
% size(u)[m N]
%
% L - the optimal closed loop control gains. These gains multiply the
% deviation of a simulated trajectory from the nominal trajectory x.
% size(L)[m n N]
%
% Vx - the gradient of the cost-to-go. size(Vx)[n N+1]
%
% Vxx - the Hessian of the cost-to-go. size(Vxx)[n n N+1]
%
% cost - the costs along the trajectory. size(cost)[1 N+1]
% the cost-to-go is V = fliplr(cumsum(fliplr(cost)))
%
% lambda - the final value of the regularization parameter
%
% trace - a trace of various convergence-related values. One row for each
% iteration, the columns of trace are
% [iter lambda alpha g_norm dcost z sum(cost) dlambda]
% see below for details.
%
% timing - timing information
%
%
%
%---------------------- user-adjustable parameters ------------------------
defaults = {‘lims’, [],… control limits
‘parallel’, true,… use parallel line-search?
‘Alpha’, 10.^linspace(0,-3,11),… backtracking coefficients
‘tolFun’, 1e-7,… reduction exit criterion
‘tolGrad’, 1e-4,… gradient exit criterion
‘maxIter’, 500,… maximum iterations
‘lambda’, 1,… initial value for lambda
‘dlambda’, 1,… initial value for dlambda
‘lambdaFactor’, 1.6,… lambda scaling factor
‘lambdaMax’, 1e10,… lambda maximum value
‘lambdaMin’, 1e-6,… below this value lambda = 0
‘regType’, 1,… regularization type 1: q_uu+lambdaeye(); 2: V_xx+lambdaeye()
‘zMin’, 0,… minimal accepted reduction ratio
‘diffFn’, [],… user-defined diff for sub-space optimization
‘plot’, 1,… 0: no; k>0: every k iters; k<0: every k iters, with derivs window
‘print’, 2,… 0: no; 1: final; 2: iter; 3: iter, detailed
‘plotFn’, @(x)0,… user-defined graphics callback
‘cost’, [],… initial cost for pre-rolled trajectory
};
% — initial sizes and controls
n = size(x0, 1); % dimension of state vector
m = size(u0, 1); % dimension of control vector
N = size(u0, 2); % number of state transitions
u = u0; % initial control sequence
% — proccess options
if nargin < 4,
Op = struct();
end
Op = setOpts(defaults,Op);
verbosity = Op.print;
switch numel(Op.lims)
case 0
case 2*m
Op.lims = sort(Op.lims,2);
case 2
Op.lims = ones(m,1)sort(Op.lims(😃)';
case m
Op.lims = Op.lims(😃[-1 1];
otherwise
error(‘limits are of the wrong size’)
end
lambda = Op.lambda;
dlambda = Op.dlambda;
% — initialize trace data structure
trace = struct(‘iter’,nan,‘lambda’,nan,‘dlambda’,nan,‘cost’,nan,…
‘alpha’,nan,‘grad_norm’,nan,‘improvement’,nan,‘reduc_ratio’,nan,…
‘time_derivs’,nan,‘time_forward’,nan,‘time_backward’,nan);
trace = repmat(trace,[min(Op.maxIter,1e6) 1]);
trace(1).iter = 1;
trace(1).lambda = lambda;
trace(1).dlambda = dlambda;
% — initial trajectory
if size(x0,2) == 1
diverge = true;
for alpha = Op.Alpha
[x,un,cost] = forward_pass(x0(:,1),alpha*u,[],[],[],1,DYNCST,Op.lims,[]);
% simplistic divergence test
if all(abs(x(😃) < 1e8)
u = un;
diverge = false;
break
end
end
⛄四、运行结果
⛄五、matlab版本及参考文献
1 matlab版本
2014a
2 参考文献
[1] 门云阁.MATLAB物理计算与可视化[M].清华大学出版社,2013.
3 备注
简介此部分摘自互联网,仅供参考,若侵权,联系删除