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
rrtstar.m 15.51 KB
Copy Edit Web IDE Raw Blame History
classdef rrtstar
% RRTSTAR Summary of this class goes here
% Detailed explanation goes here
properties %(Access = private)
A_ = [];
B_ = [];
c_ = [];
R_ = [];
h_ = [];
facet_ = [];
index_ = [];
vertex_ = [];
% best_arrival = [];
% t = sym('t', 'positive'); %符号变量't'为正
% x = sym('x', 'positive'); %used for integration
% t_s = sym('t_s', 'real'); %optimal arrival time
% min_dist = sym('d', 'real');
%
% x0 = [];
% x1 = [];
%
% states_eq = [];
% control_eq = [];
% cost_eq = [];
% distance_eq = [];
timings = [];
% eval_arrival_internal; % 以x0、x1为参数的函数句柄(多项式)
% eval_cost_internal; % 以t_s、x0、x1为参数的代价函数句柄
% eval_states_internal; % 以t、t_s、x0、x1为参数的状态函数句柄
% eval_control_internal; % 以t、t_s、x0、x1为参数的控制量函数句柄
% eval_time_for_distance_internal; % 以d、t_s、x0、x1为参数
%termination conditions (per segment)
max_it = 150; %设置迭代次数
max_time = 0; % ???
max_radius = 1000; % 寻找临近状态节点(node)用的代价半径r
tauStarSolve;
optTraj;
end
methods
function obj = rrtstar(A, B, c, R, h)
if rank(ctrb(A,B)) ~= size(A,1) %用ctrb()判断系统的可控性
disp('System is not controllable - aborting');
return;
end
obj.A_ = A;
obj.B_ = B;
obj.c_ = c;
obj.R_ = R;
obj.h_ = h;
obj.tauStarSolve = @tauStarSolve;
obj.optTraj = @optTraj;
load('433Eros.mat','facet','index','vertex')
obj.index_ = index;
obj.facet_ = facet;
obj.vertex_ = vertex;
% state_dims = size(A,1);
% % 生成符号变量组成的state_dims*1向量
% obj.x0 = sym('x0',[state_dims,1]);
% assume(obj.x0, 'real'); % obj.x0的元素都是实数
% obj.x1 = sym('x1',[state_dims,1]);
% assume(obj.x1, 'real');
% % 以下判断中'var'表示在工作区变量中查找dist_idxs是否存在
% if ~exist('dist_idxs','var') || isempty(dist_idxs)
% dist_idxs = 1:size(B,1); % 生成距离索引
% end
%
% [obj.best_arrival, obj.states_eq, obj.control_eq,...
% obj.cost_eq, obj.distance_eq] = obj.calc_equations(obj.A_,...
% obj.B_,obj.R_,obj.c_,dist_idxs);
%%calculate the factors for the resulting polynomial explicitly
% p1 = [];
% it = 0;
% %just so mupad knows it's a polynomial
% while it < 20 && length(p1) <= 1
% % feval(symengine,F,x1,...,xn)将x1,...,xn作为参数传递给F(MuPAD函数名),
% % coeff(p,x,All)求符号变量表示的多项式的关于变量x的所有系数(包括0系数)
% % simplifyFraction(expr)简化符号有理式
% % obj.best_arrival是代价函数c(τ)对τ的导数
% p1 = feval(symengine,'coeff',...
% simplifyFraction(obj.best_arrival*obj.t^it),...
% obj.t, 'All');
% it = it+1;
% end
% if it > 20
% disp('either the result is not a polynomial or the degree is too high');
% end
% % B = fliplr(A) 返回 A,围绕垂直轴按左右方向翻转其各列。
% p([obj.x0', obj.x1']) = fliplr(p1);
% % matlabFunction(p, 'file', 'arrival_time.m'); %to write to file
% obj.eval_arrival_internal = matlabFunction(p);
% obj.eval_cost_internal = matlabFunction(obj.cost_eq);
% obj.eval_states_internal = matlabFunction(obj.states_eq);
% obj.eval_control_internal = matlabFunction(obj.control_eq);
%%calculate the factors for the resulting polynomial explicitly
% p1 = [];
% it = 0;
% %just so mupad knows it's a polynomial
% while it < 20 && length(p1) <= 1
% p1 = feval(symengine,'coeff',simplifyFraction(...
% obj.distance_eq*obj.t^it), obj.t, 'All');
% it = it+1;
% end
% if it > 20
% disp('either the result is not a polynomial or the degree is too high');
% end
%
% p([obj.min_dist, obj.t_s, obj.x0', obj.x1']) = fliplr(p1);
% obj.eval_time_for_distance_internal = matlabFunction(p);
end
function [time] = evaluate_arrival_time(obj,x0_, x1_, h_)
%arr = subs(obj.best_arrival, obj.x0, x0_);
%arr = subs(arr, obj.x1, x1_);
%time_a = min(eval(solve(arr, 'Real', true))); %symbolical version
% in = num2cell([x0_', x1_']);
% time = roots(obj.eval_arrival_internal(in{:}));
% time = time(imag(time)==0);
% time = min(time(time>=0));
[time, ~] = obj.tauStarSolve(x0_, x1_, h_);
end
function [cost, time] = evaluate_cost(obj, x0_, x1_, h_)
% if ~exist('time','var') || isempty(time)
% time = obj.evaluate_arrival_time(x0_, x1_);
% end
%c = subs(obj.cost_eq, obj.x0, x0_);
%c = subs(c, obj.x1, x1_);
%c = subs(c, obj.t_s, time);
%cost = eval(c);
% in = num2cell([time, x0_', x1_']);
% cost = obj.eval_cost_internal(in{:});
[time, cost, ~, ~] = obj.tauStarSolve(x0_, x1_, h_);
end
function [states, inputs] = evaluate_states_and_inputs(obj,x0_,x1_,h_,B_,R_)
% if ~exist('time','var') || isempty(time)
% time = obj.evaluate_arrival_time(x0_, x1_);
% end
%s = subs(obj.states_eq, obj.x0, x0_);
%s = subs(s, obj.x1, x1_);
%states = subs(s, obj.t_s, time);
%c = subs(obj.control_eq, obj.x0, x0_);
%c = subs(c, obj.x1, x1_);
%inputs = subs(c, obj.t_s, time);
% in = num2cell([time, x0_', x1_']);
% states = @(t)obj.eval_states_internal(t, in{:}); %以t为参数的状态函数句柄
% inputs = @(t)obj.eval_control_internal(t, in{:}); %以t为参数的输入函数句柄
[tauStar, ~, input, state] = obj.optTraj(x0_,x1_,h_,B_,R_);
% states = state'; % 使输出的states每一列是一个时刻是状态,不同列代表不同时刻
% inputs = input';
n = floor(size(input,1)/10);
partI = zeros(n+1, 3);
partS = zeros(n+1, 6);
partI(1,:) = input(1, :);
partS(1,:) = state(1, :);
for i = 2:n
partI(i,:) = input((i-1)*10, :);
partS(i,:) = state((i-1)*10, :);
end
partI(n+1,:) = input(end, :);
partS(n+1,:) = state(end, :);
for i = 1:size(partI,1)
X = state(i,1:3)';
g = Poly_gravity(X,obj.vertex_,obj.facet_,obj.index_);
partI(i,:) = partI(i,:) - g';
end
l = linspace(0,tauStar,n+1);
inputs = @(t) interp1(l,partI,t);
states = @(t) interp1(l,partS,t);
end
function [close_time] = get_time_for_distance_equal(obj, x0_, x1_,...
time, distance)
if isempty(obj.distance_eq)
close_time = obj.evaluate_arrival_time(x0_, x1_, obj.h_);
return;
end
if ~exist('time','var') || isempty(time)
time = obj.evaluate_arrival_time(x0_, x1_, obj.h_);
end
in = num2cell([distance, time, x0_', x1_']);
close_time = roots(obj.eval_time_for_distance_internal(in{:}));
close_time = close_time(imag(close_time)==0);
close_time = min(close_time(close_time>=0));
end
function [obj] = set_termination_conditions(obj, iterations, time)
obj.max_it = iterations;
obj.max_time = time;
end
function [path_states, closest_end_state] = run(obj, sample_free_state,...
is_state_free, is_input_free, start, goal, display, max_distance)
T = [start]; % 放入起始点
costs = [0]; % 初始化代价为0,存放每个节点(node)的代价
times = [0]; % 初始化时间为0,
parents = [-1];
children = {[]};
is_terminal = [false];
cost_to_goal = [inf]; %
time_to_goal = 0; %
goal_cost = inf; % 从起始点到达目标点的代价,初始化为inf
goal_parent = 0; %
goal_time = inf; % 从起始点到达目标点的时间,初始化为inf
it_limit = obj.max_it; % 迭代次数
[cost, time] = evaluate_cost(obj,start, goal, obj.h_); % 求s->g最小代价和最优时间
% 求最优轨迹对应的状态states(t)和u(t)
[states, u] = evaluate_states_and_inputs(obj,start,goal,obj.h_,obj.B_,obj.R_);
if is_state_free(states,[0,time]) && is_input_free(u,[0,time])
disp('goal is reachable from the start node (optimal solution)');
it_limit = 0;
goal_parent = 1;
display(-1, obj, start, -1, goal, cost, 1);
end
display_scratch = -1; % ???
it = 0;
while it < it_limit && goal_time > obj.max_time
it = it + 1;
disp(['iteration ', num2str(it), ' of ', num2str(it_limit),...
'(time:',num2str(goal_time),')']);
sample_ok = false;
tic; % 开始计时
while ~sample_ok
x_i = sample_free_state(); % 采样
min_idx = 1; %
min_cost = inf; % 初始化起点到终点的最小代价
min_time = inf; % 初始化起点到终点的最短时间
queue = 1;
while ~isempty(queue)
node_idx = queue(1);
%switch with stack to perform DFS instead of BFS
queue = queue(2:end);
node = T(:,node_idx);
if is_terminal(node_idx)
continue;
end
[cost, time] = evaluate_cost(obj, node, x_i, obj.h_);
if cost < obj.max_radius && costs(node_idx)+cost <...
min_cost && costs(node_idx)+cost < goal_cost
[states, u] = evaluate_states_and_inputs(obj,node,x_i,obj.h_,obj.B_,obj.R_);
if is_state_free(states,[0,time]) && is_input_free(u,[0,time])
sample_ok = true;
min_idx = node_idx;
min_cost = costs(node_idx)+cost;
min_time = times(node_idx)+time;
continue;
end
end
%if we arrive here it means there is no trajectory from node to new x_i
%however child nodes might be able to form a connection
queue = [queue, children{node_idx}];
end
end
parents = [parents, min_idx];
children{min_idx} = [children{min_idx},it+1];
children{it+1} = [];
costs = [costs, min_cost];
times = [times, min_time];
T = [T,x_i];
is_terminal = [is_terminal, false];
cost_to_goal = [cost_to_goal, inf];
time_to_goal = [time_to_goal, inf];
%update the tree with new shorter paths (the root node is checked
%here even though it's not necessary)
stack(1).index = 1;
stack(1).improvement = 0;
stack(1).time_improvement = 0;
while ~isempty(stack)
node = stack(end);
stack = stack(1:end-1);
state = T(:,node.index);
costs(node.index) = costs(node.index)-node.improvement;
times(node.index) = times(node.index)-node.time_improvement;
if is_terminal(node.index)
if costs(node.index)+cost_to_goal(node.index) < goal_cost
goal_cost = costs(node.index)+cost_to_goal(node.index);
goal_time = times(node.index)+time_to_goal(node.index);
goal_parent = node.index;
end
continue;
end
diff = node.improvement;
time_diff = node.time_improvement;
[cost, time] = evaluate_cost(obj, x_i,state,obj.h_);
if cost < obj.max_radius && costs(end)+cost < costs(node.index)
[states, u] = evaluate_states_and_inputs(obj,x_i,state,obj.h_,obj.B_,obj.R_);
if is_state_free(states,[0,time]) && is_input_free(u,[0,time])
old_cost = costs(node.index);
old_time = times(node.index);
old_parent = parents(node.index);
costs(node.index) = costs(end)+cost;
parents(node.index) = size(parents,2);
ch = children{old_parent};
children{old_parent} = ch(ch~=node.index);
diff = old_cost-costs(node_idx);
time_diff = old_time-times(node_idx);
end
end
for jj=1:length(children{node.index})
stack(end+1).index = children{node.index}(jj);
stack(end).improvement = diff;
stack(end).time_improvement = time_diff;
end
end
[cost, time] = evaluate_cost(obj, x_i, goal, obj.h_);
if costs(end)+cost < goal_cost
[states, u] = evaluate_states_and_inputs(obj, x_i,goal,obj.h_,obj.B_,obj.R_);
if is_state_free(states,[0,time]) && is_input_free(u,[0,time])
goal_cost = costs(end)+cost;
goal_time = times(end)+time;
goal_parent = it+1;
is_terminal(end) = true;
cost_to_goal(end) = cost;
end
end
obj.timings = [obj.timings, toc()];
display_scratch = display(display_scratch, obj, T, parents,...
goal, goal_cost, goal_parent);
if mod(length(obj.timings),50) == 1 && length(obj.timings) > 50
disp(['time for the last 50 iterations: ',...
num2str(sum(obj.timings(end-50:end))),' seconds']);
end
end
next = goal_parent;
path_states = goal;
while next ~= -1
path_states = [T(:,next) ,path_states];
next = parents(next);
end
if ~exist('max_distance','var') || isempty(max_distance)
closest_end_state = goal;
else
src = T(:,goal_parent);
best_t = obj.evaluate_arrival_time(src, goal, obj.h_);
close_t = get_time_for_distance_equal(obj, src, goal, best_t,...
max_distance);
[s, ~] = evaluate_states_and_inputs(obj, src, goal, obj.h_,obj.B_,obj.R_);
closest_end_state = s(close_t);
end
end
function [path, time] = find_path(obj, sample_free_state, is_state_free,...
is_input_free, waypoints, display, max_distance)
time = 0;
path = [waypoints(:,1)];
for ii=2:size(waypoints,2)
figure('name', [num2str(ii-1),' to ', num2str(ii)]);
[path_states, closest_end_state] = obj.run(sample_free_state,...
is_state_free, is_input_free, path(:,end), waypoints(:,ii),...
display, max_distance);
path = [path, path_states(:,2:end)];
if ii ~= size(waypoints,2)
path(:,end) = closest_end_state;
end
end
for ii=2:size(path,2)
time = time + obj.evaluate_arrival_time(path(:,ii-1),path(:,ii),obj.h_);
end
end
end
% methods (Access = protected)
%
% function [tau_star, states, control, cost, sq_distance] = ...
% calc_equations(obj, A, B, R, c, dist_idxs)
% % the solution of the equation 'tau_star = 0' gives the best
% % arrival time. states, control, and cost depend on t_s which
% % is the solution from above
% state_dims = size(A,1);
%
% G = int(expm(A*(obj.t-obj.x))*B/R*B'*expm(A'*(obj.t-obj.x)),...
% obj.x, 0, obj.t);
% x_bar = expm(A*obj.t)*obj.x0+int(expm(A*(obj.t-obj.x))*c,...
% obj.x, 0, obj.t);
%
% d = G\(obj.x1-x_bar);
% tau_star = 1-2*(A*obj.x1+c)'*d-d'*B/R*B'*d; % 代价函数对τ的导数
%
% solution = expm([A, B/R*B';zeros(state_dims), -A']*(obj.t-obj.t_s))...
% *[obj.x1;subs(d,obj.t,obj.t_s)]+ ...
% int(expm([A, B/R*B';zeros(state_dims), -A']*(obj.t-obj.x))...
% *[c; zeros(state_dims,1)],obj.x,obj.t_s,obj.t);
%
% control = R\B'*solution(state_dims+1:2*state_dims,:);
% states = solution(1:state_dims);
%
% if exist('dist_idxs','var')
% sq_distance = sum((obj.x1(dist_idxs)-states(dist_idxs)).^2)...
% -obj.min_dist^2;
% end
%
% cost = int(1+control'*R*control, obj.t, 0, obj.t_s);
%
% end
% end
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