多变量微分方程组带事件控制的ODE45函数写法

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);
1

[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))
1

存储了自己的微分方程组，并且有一个参数是函数，

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
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32

代码核心是输入中的x顺序与初值x0中元素以一对应，x中存放的是带求解变量，y中是各个变量对应的一阶微分，其中输入变量中modelPara,Tau是构建微分方程需要的参数。一般操作是把x数组中元素赋值给y数组，使用y数组构建微分方程，dydt中输出是对应各阶微分的表达式，所以dydt中不一定都是只有一元，但是某一元的高阶微分一定要用前面已经出现的低阶微分表示

4.函数为参数的ODE45微分方程

构建微分方程myStanceODEFct中使用的函数参的定义

Tau = @(t,x)controllerOfStance(t,x,modelPara);
1

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
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31

上述函数返回一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
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25

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


1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24

% 事件检查函数，此时需要做的是过零点检测
% ode45函数自动检查当value=0是否成立
% 如果我们要求检测Y=0的点，设置value=Y
% 这里我们要检测Y=4，那么就设置value=Y-4

% isterminal检测到指定条件时，是否终止ode45函数的运行
% 1表示终止，0表示继续
% 在我们这个问题上，我们只要检测到零点时就停止程序

% direction：value过零点检测的方向
% -1表示由正到负，+1表示由负到正
% 对于我们这个问题，当然是由正到负