1.来源
在CUHK学习advanced robotics时做hopping robot仿真时多次使用自己的判断条件终止ODE45方程组,遂整理在此
2.ODE45函数调用
[t,x,te,xe,ie] = ode45(@(t,x) myStanceODEFct(t,x,modelPara,Tau(t,x)), t_span, x0, option);
[t,y,te,ye,ie] = ode45(odefun,tspan,y0,options)
其中 tspan = [t0 tf])求微分方程组 y′=f(t,y) 从 t0 到 tf 的积分,
初始条件为 y0。
解数组 y 中的每一行都与列向量 t 中返回的值相对应。
还使用由 options(使用 odeset 函数创建的参数)定义的积分设置。例如,使用 AbsTol 和 RelTol 选项指定绝对误差容限和相对误差容限,或者使用 Mass 选项提供质量矩阵。
还求 (t,y) 的函数(称为事件函数)在何处为零。在输出中,te 是事件的时间,ye 是事件发生时的解,ie 是触发的事件的索引。
(详情参阅matlab帮助文档)
myStanceODEFct(t,x,modelPara,Tau(t,x))
存储了自己的微分方程组,并且有一个参数是函数,
3.编写自己的微分方程组函数
function dydt = myStanceODEFct(t,x,modelPara,Tau)
% This is the my own differential equation for Stance Phase
% see ODE help for details
% x = [l;theta;dl;dtheta]
%
l = x(1);
theta = x(2);
l_dot = x(3);
theta_dot = x(4);
%% the dynamic equation of stance phase
% during this phase, energy will be insert to the robot in the leg
g = modelPara.g;
m = modelPara.m;
k = modelPara.k;
l_0 = modelPara.l_0;
b = modelPara.b;
M = [m 0;
0 m*l^2];
C = [0 -m*l*theta_dot;
m*l*theta_dot m*l*l_dot];
G =[k*(l-l_0)+m*g*cos(theta);
-m*g*l*sin(theta)];
B = [-b*l_dot;
0];
q_ddot = M \ (Tau+B - G - C*[l_dot;theta_dot]);
%输出的微分方程,分别是 l 导数, theta 导数。 l 二阶导数, theta 二阶导数,构成二元二阶微分方程组
dydt = [l_dot;theta_dot;q_ddot(1);q_ddot(2)];
end
代码核心是输入中的x顺序与初值x0中元素以一对应,x中存放的是带求解变量,y中是各个变量对应的一阶微分,其中输入变量中modelPara,Tau是构建微分方程需要的参数。一般操作是把x数组中元素赋值给y数组,使用y数组构建微分方程,dydt中输出是对应各阶微分的表达式,所以dydt中不一定都是只有一元,但是某一元的高阶微分一定要用前面已经出现的低阶微分表示
4.函数为参数的ODE45微分方程
构建微分方程myStanceODEFct中使用的函数参的定义
Tau = @(t,x)controllerOfStance(t,x,modelPara);
function Tau = controllerOfStance(t,x,modelPara)
% Design the controller in stance phase
%{
input
t --> time
x --> current state
output
tau --> torques tau = [f_leg;0]
%}
global Hcenter
global Vcenter
l = x(1);
theta = x(2);
l_dot = x(3);
theta_dot = x(4);
modelPara.energy = 0.5*modelPara.m*Vcenter^2 + modelPara.m*modelPara.g*Hcenter;
error = modelPara.energy - modelPara.energy_desired;
c=2.0;
if l_dot <0
f_leg = c * error;
else
f_leg = -c * error;
end
Tau = [f_leg;
0];
end
上述函数返回一Tau 力矩,其作为
5.ODE45调用顺序
首先生成ODE45微分方程中使用的函数参数值,
然后在正常调用ODE45 函数
function [t,x,te,xe] = simulationStance(t_span,x0,modelPara)
% this is the simulation for the flight phase
%{
t_span ---> this is for the ode function,Interval of integration
x0 ---> initial position for current phase
modelPara ---> model parameters
t ---> time series for the result
x ---> the results of the differential equation for Flight Phase
te ---> Time of events
xe ---> Solution at time of events
%}
%% the controller of the force in the leg (spring)
Tau = @(t,x)controllerOfStance(t,x,modelPara);
%% using ODE to solve parameters
% get the option from the model_Parameter struct
option = modelPara.ODE_options_stance;
% calculate
[t,x,te,xe] = ode45(@(t,x) myStanceODEFct(t,x,modelPara,Tau(t,x)), t_span, x0, option);
end
6.ODE45的事件函数(Event)
这个是ODE45 event的写法,其通过ODE函数option参数传入,其参数可能只能是自变量t与带求因变量。具体我没调查怎么处理,只是知道在这个event函数中简单添加变量后没有成功使用它,详情待查,ODE event写法查odeset
function [value,isterminal, direction] = EventStance(t,x)
%STANCEEVENT determine the stop condition of simulation when robot stand on
%ground
% input: t --> current time
% x --> current state x=[l;theta;dl;dtheta]
% output: value = 0 --> stop condition
% if the mass hit the ground, terminate the simulation
global T
l = x(1);
theta = x(2);
value(1,1) = l*cos(theta);
value(2,1) = (l-0.2);
value(3,1) = t-T;
isterminal = [1;1;1];
direction = [0;1;1];
end
% 事件检查函数,此时需要做的是过零点检测
% ode45函数自动检查当value=0是否成立
% 如果我们要求检测Y=0的点,设置value=Y
% 这里我们要检测Y=4,那么就设置value=Y-4
% isterminal检测到指定条件时,是否终止ode45函数的运行
% 1表示终止,0表示继续
% 在我们这个问题上,我们只要检测到零点时就停止程序
% direction:value过零点检测的方向
% -1表示由正到负,+1表示由负到正
% 对于我们这个问题,当然是由正到负