## PengLu / 使用kinodynamic RRTStar算法规划轨迹 .gitee-modal { width: 500px !important; }

Explore and code with more than 6 million developers，Free private repositories ！：）
This repository doesn't specify license. Without author's permission, this code is only for learning and cannot be used for other purposes.
kinodynamic_rrtstar_for_guidance_3d.m 2.50 KB
% 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];

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,...
input_free = @(input, time_range)(is_input_free(input, input_limits,...
time_range));
sample_state = @()(sample_free_states(state_limits, obstacles,...
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);