1 Star 2 Fork 3

PengLu / 使用kinodynamic RRTStar算法规划轨迹

Create your Gitee Account
Explore and code with more than 6 million developers,Free private repositories !:)
Sign up
This repository doesn't specify license. Without author's permission, this code is only for learning and cannot be used for other purposes.
Clone or download
rungeKutta4.m 1.16 KB
Copy Edit Web IDE Raw Blame History
% RUNGEKUTTA4 4阶龙格库塔法
% 作 者:hyowinner(MATALB中文论坛用户)
% 创建日期:未 知
% 修 改 人:鲁 鹏,
% 修改日期:2018年7月11日
% 参考网站:http://www.ilovematlab.cn/thread-42808-1-1.html
% 版 本:2.1
% version 2.1 更新日志:添加输出变量的意义和输出形式
% version 2.0 更新日志:将输出结果y转置后输出,输出列向量(同ode45)
function [ x, y ] = rungeKutta4( ufunc, a, b, y0, h )
% 输出:
% x 时间,列向量
% y 求解的状态,每一列为一种状态随不同时间的值
% 参数表顺序依次是微分方程组的函数名称ufunc,时间起点a,
% 时间终点b,初始值向量y0,步长h(参数形式参考了ode45函数)
n = floor((b - a) / h);%求步数
x = ones(n,1);
x(1) = a;%时间起点
y(:,1) = y0;%赋初值,可以是向量,但是要注意维数
for ii = 1:n
x(ii + 1) = x(ii) + h;
k1 = ufunc(x(ii), y(:,ii));
k2 = ufunc(x(ii) + h / 2, y(:,ii) + h * k1 / 2);
k3 = ufunc(x(ii) + h / 2, y(:,ii) + h * k2 / 2);
k4 = ufunc(x(ii) + h, y(:,ii) + h * k3);
y(:,ii+1) = y(:,ii) + h * (k1 + 2 * k2 + 2 * k3 + k4) / 6;
end
y = y';
end

Comment ( 0 )

Sign in for post a comment

Matlab
1
https://gitee.com/olupengo/kinodynamic-RRTStar-for-landing-on-small-body.git
git@gitee.com:olupengo/kinodynamic-RRTStar-for-landing-on-small-body.git
olupengo
kinodynamic-RRTStar-for-landing-on-small-body
使用kinodynamic RRTStar算法规划轨迹
master

Search