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
optTraj.m 1.19 KB
Copy Edit Web IDE Raw Blame History
function [tauStar,cStar,u,x] = optTraj(x0,x1,h,B,R)
%OPTTRAJ 此处显示有关此函数的摘要
% 输 出:
% x 最优轨迹,?X3矩阵
% u 控制量,?X3矩阵
% cStar 最优轨迹x的代价,标量
% t 时间,列向量
% 输 入:
% x0初始位置列向量,x1末端位置列向量,h时间步长标量,B是6X3输人矩阵,R是权3X3权重矩阵
% 使用函数tauStarSolve.m、rungeKutta4.m和dmdt.m
% 作 者:鲁 鹏(北京理工大学宇航学院)
% 版 本:1.0
% version 1.0 更新日志:
% x0 = [0 0 0 0 0 0]'; % 初始位置列向量
% x1 = [10 10 10 0 0 0]'; % 末端位置列向量
% h = 0.01; % 时间步长,四阶龙格库塔法等使用
[tauStar, cStar, xBarStar, GStar] = tauStarSolve(x0, x1, h);
dTauStar = GStar\(x1 - xBarStar);
hTauStar = [x1; dTauStar]; % 微分方程的终端状态
% 应为是从tauStar朝0积分,所以步长要是‘-h’
[~, m] = rungeKutta4(@dmdt, tauStar, 0 , hTauStar, -h);
% figure
% plot3(h(:, 1), h(:, 2), h(:, 3))
u = zeros(size(m,1), 3);
K = B / R';
for i = 1 : size(m,1)
u(i, :) = m(i,7:12)*K;
end
% 由于是反向求微分方程,所以要将输出结果上下颠倒
x = flipud(m(:,1:6));
u = flipud(u);
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