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
kinodynamic_rrtstar_for_guidance_3d.m 2.50 KB
Copy Edit Web IDE Raw Blame History
% kinodynamic_rrtstar_for_guidance_3d.m文献Heuristic Guidance Techniques
% for the Exploration of Small Celestial Bodies小天体着陆部分的仿真主程序。
% 使用外部函数:ball_is_state_free.m, is_input_free.m,
% ball_sample_free_states.m, ball_plot_field.m
% 作 者:Dustin J. Webb and Jur van den Berg(参考文献的作者)
% 创建日期:未 知
% 修 改 人:鲁 鹏
% 修改日期:2018年6月3日
% 参考文献:Kinodynamic RRT*: Optimal Motion Planning for Systems with
% Linear Differential Constraints
start = [-20000, 0, 10000, 0, 0, 0];
goal = [-7014, 0, 5189, 0, 0, 0];
radius = 100; % 机器人和障碍物最近距离小于radius时,视为发生碰撞
state_limits = ...
[-21000,-7014; % x limit
-1000, 1000; % y limit
5189,11000; % z limit
-100, 100; % vx limit
-100, 100; % vy limit
-100, 100]; % vz limit
input_limits = ...
[-500, 500;
-500, 500;
-500, 500];
state_dims = size(start,2); % 系统的维数
input_dims = 3; % 控制量的维数
omega = 3.31182e-4; % 433Eros绕z轴的旋转角速度
A=zeros(state_dims);
A(1,4) = 1; A(2,5) = 1; A(3,6) = 1;
A(4,1) = omega^2; A(4,5) = 2*omega; A(5,2) = omega^2; A(5,4) = -2*omega;
B=zeros(state_dims, input_dims);
B(4,1) = 1; B(5,2) = 1; B(6,3) = 1;
c = zeros(state_dims,1);
% c(4,1) = -0.0020;
% c(5,1) = -0.0020;
% c(6,1) = -0.0020; %433Eros表面引力在10^(-3)这个量级
R = diag([1/2,1/2,1/2]); %
h = 0.1; % 积分时间步长
obstacles = [-21000,-1000,11000; %Free Space的边界点坐标(每行为一个点的坐标)
-21000,-1000, 5189;
-7014, -1000, 5189;
-7014, -1000,11000;
-21000, 1000,11000;
-21000, 1000, 5189;
-7014, 1000, 5189;
-7014, 1000,11000];
% disp('calculating closed form solution.');
rrt = rrtstar(A,B,c,R,h);
% disp('starting algorithm.');
% @用于创建匿名函数的函数句柄 fhandle = @(x,y) x.^2 + y.^2;
state_free = @(state, time_range)(is_state_free(state,state_limits,...
obstacles, radius, time_range));
input_free = @(input, time_range)(is_input_free(input, input_limits,...
time_range));
sample_state = @()(sample_free_states(state_limits, obstacles,...
radius));
display = @(scratch, obj, tree, parents, goal, goal_cost, goal_parent)...
(plot_field(scratch, obj, tree, parents, goal, goal_cost,...
goal_parent));
[T, parents] = rrt.run(sample_state, state_free, input_free, start',...
goal', display);

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