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
G_dot.m 2.72 KB
Copy Edit Web IDE Raw Blame History
function dG = G_dot(~,g) %此处参数用小写的g,是因为要将G转化为向量
% G_DOT 该函数由语句matlabFunction(dG,'File','G_dot','Vars',G)自动生成
% DG = G_DOT(T,G11,G21,G31,G41,G51,G61,G12,G22,G32,G42,G52,G62,G13,G23,
% G33,G43,G53,G63,G14,G24,G34,G44,G54,G64,G15,G25,G35,G45,G55,G65,
% G16,G26,G36,G46,G56,G66)
% This function was generated by the Symbolic Math Toolbox version 8.0.
% 02-Jul-2018 11:46:55
% 修改1:添加参数~
% 修改2:将函数参数由g11,g21,...,,g56,g66改为g
% 修改3:将dG表达式中reshap函数的第二个参数由[6,6]改为[36,1]
% 以下被注释两层嵌套for循环程序可以打印出:
% g11 = g(1); ...; g61 = g(6);
% . .
% . .
% . .
% g16 =g(31); ...; g66 = g(36);
% for i = 1:6
% for j = 1:6
% fprintf('g%d%d = G(%d); ',j,i,(i-1)*6+j)
% end
% fprintf('\n')
% end
g11 = g(1); g21 = g(2); g31 = g(3); g41 = g(4); g51 = g(5); g61 = g(6);
g12 = g(7); g22 = g(8); g32 = g(9); g42 =g(10); g52 = g(11); g62 = g(12);
g13 =g(13); g23 =g(14); g43 =g(16); g53 = g(17); g63 = g(18);
g14 =g(19); g24 =g(20); g34 =g(21); g44 =g(22); g54 = g(23); g64 = g(24);
g15 =g(25); g25 =g(26); g35 =g(27); g45 =g(28); g55 = g(29); g65 = g(30);
g16 =g(31); g26 =g(32); g36 =g(33); g46 =g(34); g56 = g(35); g66 = g(36);
% g33 = g(15); %状态变量g33为未出现在动力学方程右端,没有参加下面dG的计算
t2 = g11.*1.098649095807376e-7;
t3 = g12.*1.098649095807376e-7;
t4 = g21.*1.098649095807376e-7;
t5 = g22.*1.098649095807376e-7;
t6 = g55.*6.6291752e-4;
t7 = g45.*6.6291752e-4;
t8 = g54.*6.6291752e-4;
dG = reshape([g14+g41,g24+g51,g34+g61,g44+g51.*6.6291752e-4+t2,...
g41.*(-6.6291752e-4)+g54+t4,g64,g15+g42,g25+g52,g35+g62,g45+...
g52.*6.6291752e-4+t3,g42.*(-6.6291752e-4)+g55+t5,g65,g16+g43,...
g26+g53,g36+g63,g13.*1.098649095807376e-7+g46+g53.*6.6291752e-4,...
g23.*1.098649095807376e-7-g43.*6.6291752e-4+g56,g66,...
g15.*6.6291752e-4+g44+t2,g25.*6.6291752e-4+g54+t4,...
g31.*1.098649095807376e-7+g35.*6.6291752e-4+g64,...
g14.*1.098649095807376e-7+g41.*1.098649095807376e-7+t7+t8+2.0,...
g24.*1.098649095807376e-7-g44.*6.6291752e-4+...
g51.*1.098649095807376e-7+t6,g61.*1.098649095807376e-7+...
g65.*6.6291752e-4,g14.*(-6.6291752e-4)+g45+t3,...
g24.*(-6.6291752e-4)+g55+t5,g32.*1.098649095807376e-7-...
g34.*6.6291752e-4+g65,g15.*1.098649095807376e-7+...
g42.*1.098649095807376e-7-g44.*6.6291752e-4+t6,...
g25.*1.098649095807376e-7+g52.*1.098649095807376e-7-t7-t8+2.0,...
g62.*1.098649095807376e-7-g64.*6.6291752e-4,g46,g56,g66,...
g16.*1.098649095807376e-7+g56.*6.6291752e-4,...
g26.*1.098649095807376e-7-g46.*6.6291752e-4,2.0],[36,1]);

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