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
Poly_gravity.m 5.57 KB
Copy Edit Web IDE Raw Blame History
% X = [160000,0,0]'
% 参考文献见EXTERIOR GRAVITATION OF A POLYHEDRON DERIVED AND COMPARED WITH
% HARMONIC AND MASCON GRAVITATION REPRESENTATIONS OF ASTEROID 4769 CASTALIA
function dU= Poly_gravity(X,vertex,facet,index)
%%%%%!!!必须在内部load,所以使用时必须先取消下面一行的注释,然后后面加上小天体参数的文件名(编者按)
% load steinsp
% vertex=vertex_216;
% facet=facet_216;
% for j = 1:3
% for i = 1:size(vertex(:,1))
% vertex(i,j)= vertex(i,j)*1000; %vertex中的数据为千米 换算成米
% end
% end
% for j = 1:3
% for i = 1:size(facet(:,1))
% facet(i,j)= facet(i,j)+1; %facet中数据是距离第一个点的偏差 全部加1 防止出现0点
% end
% end
% dU = [0 0 0]';
dU_face = [0 0 0]';
dU_edge = [0 0 0]';
% ddU = zeros(3,3);
ddU_face = zeros(3,3);
ddU_edge = zeros(3,3);
delta_U = 0;
nv = length(vertex(:,1)); %顶点的数量
n_facet = length(facet(:,1)); %面的数量
ne = n_facet + nv - 2; %多面体边的数量
% r1 = zeros(nf,3);
% r2 = zeros(nf,3);
% r3 = zeros(nf,3);
% rou1 = zeros(nf,3);
% rou2 = zeros(nf,3);
% rou3 = zeros(nf,3);
% omiga = zeros(nf,1);
face_part = 0;
% for i = 1:nf
% offset = facet(i,:);
% r1(i,:) = vertex(offset(1),:); %面上的三个顶点
% r2(i,:) = vertex(offset(2),:);
% r3(i,:) = vertex(offset(3),:);
%
% n(i,:) = cross(r2(i,:)- r1(i,:),r3(i,:) - r2(i,:))/norm(cross(r2(i,:) - r1(i,:),r3(i,:) - r2(i,:))); %计算平面的法向向量 指向外
% rou1(i,:) = r1(i,:)' -X %用于计算omiga
% rou2(i,:) = r2(i,:)' -X;
% rou3(i,:) = r3(i,:)' -X;
% omiga(i) = 2*atan(dot(rou1(i,:) ,cross(rou2(i,:),rou3(i,:)))/...
% (norm(rou1(i,:))*norm(rou2(i,:))*norm(rou3(i,:)) + norm(rou1(i,:))* dot(rou2(i,:),rou3(i,:))...
% + norm(rou2(i,:))* dot(rou3(i,:),rou1(i,:)) +norm(rou3(i,:))* dot(rou1(i,:),rou2(i,:)) ) );
%
% %计算omiga
% face_part = face_part + rou1(i,:)*(n(i,:)'*n(i,:))*rou1(i,:)'*omiga(i,:);
% dU_face = dU_face + n(i,:)'*n(i,:)*rou1(i,:)'*omiga(i);
% ddU_face = ddU_face + n(i,:)'*n(i,:)*omiga(i);
% delta_U = delta_U + omiga(i);
% end
for i = 1:n_facet
offset = facet(i,:);
r1 = vertex(offset(1),:) ; %面上的三个顶点
r2 = vertex(offset(2),:);
r3 = vertex(offset(3),:);
% if (dot(nf,r1) < 0)
% nf=-nf
% end
rou1 = r1' - X ; %用于计算omiga,面的顶点位置 - 当前位置
rou2 = r2' - X;
rou3 = r3' - X;
nf = cross((r2-r1),(r3 - r2))/norm(cross((r2 - r1),(r3 - r2))) ;%计算平面的法向向量 指向外
% nf2 = cross((rou2-rou1),(rou3 - rou2))/norm(cross((rou2 - rou1),(rou3 - rou2))) %计算平面的法向向量 指向外
omiga = 2*atan(dot(rou1 ,cross(rou2,rou3))/...
(norm(rou1)*norm(rou2)*norm(rou3) + norm(rou1)* dot(rou2,rou3)...
+ norm(rou2)* dot(rou3,rou1) +norm(rou3)* dot(rou1,rou2) ) );
%计算omiga
face_part = face_part + rou1'*(nf'*nf)*rou1*omiga;
dU_face = dU_face + nf'*nf*rou1*omiga;
ddU_face = ddU_face + nf'*nf*omiga;
delta_U = delta_U + omiga;
end
%%%%%求出与一个面相互共边的索引 索引的是facet中的列
% h = 1;
% for i = 1:nf
% for j = i:nf
% a = facet(i,:);
% [c, ia, ib] = intersect(a ,facet(j,:));
% if length(c)== 2
% index(h,:) = [i,j,c];
% h = h+1;
% end
%
% end
% end
% 计算好之后 供给仿真的时候调用生成的index
tic
for i =1:ne
face_A = facet(index(i,1),:) ; %共边的两个面 其一
face_B = facet(index(i,2),:) ; %共边的两个面 其二
edge_vertex1 = vertex(index(i,3),:); %共边的两个顶点
edge_vertex2 = vertex(index(i,4),:);
re1 = norm(edge_vertex1'-X) ; %见199页 B.25-27定义
re2 = norm(edge_vertex2'-X);
e12 = norm(edge_vertex1' - edge_vertex2' );
r_A1 = vertex(face_A(1),:) ; %A B 两面的顶点
r_A2 = vertex(face_A(2),:);
r_A3 = vertex(face_A(3),:);
r_B1 = vertex(face_B(1),:);
r_B2 = vertex(face_B(2),:);
r_B3 = vertex(face_B(3),:) ;
nA = cross((r_A2 - r_A1),(r_A3 - r_A2))/norm(cross((r_A2 - r_A1),(r_A3 - r_A2))) ; %计算A/B平面的法向向量 指向外侧
nB = cross((r_B2 - r_B1),(r_B3 - r_B2))/norm(cross((r_B2 - r_B1),(r_B3 - r_B2)));
if (dot(r_A1,nA) < 0)
nA=-nA;
end
if (dot(r_B1,nB) < 0)
nB=-nB;
end
P1P2 = edge_vertex1 - edge_vertex2;
nA12 = cross(nA,P1P2)/norm(cross(nA,P1P2)); %公共边的位于平面内的法向 指向外侧
nB21 = cross(nB,-P1P2)/norm( cross(nB,-P1P2));
%判断nA12是否是指向外侧
% m1 = dot(edge_vertex2-r_A1,nA12)
% m2= dot(edge_vertex2-r_A2,nA12)
% m3= dot(edge_vertex2-r_A3,nA12)
if (dot(edge_vertex2-r_A1,nA12)>=-0.1&&dot(edge_vertex2-r_A2,nA12)>=-0.1&&dot(edge_vertex2-r_A3,nA12)>=-0.1)
nA12 = nA12;
else
nA12 = -nA12;
end
if (dot(edge_vertex1-r_B1,nB21)>=-0.1&&dot(edge_vertex1-r_B2,nB21)>=-0.1&&dot(edge_vertex1-r_B3,nB21)>=-0.1)
nB21 = nB21;
else
nB21 = -nB21;
end
% m4 = dot(edge_vertex1-r_B1,nB21)
% m5 = dot(edge_vertex1-r_B2,nB21)
% m6 = dot(edge_vertex1-r_B3,nB21)
% dot(nA,nA12)
% nA/norm(nA)
% nA12/norm(nA12)
% dot(nB,nB21)
Eab = nA'*nA12 + nB'*nB21;
Le = log((re1 + re2 + e12)/(re1 + re2 - e12));
dU_edge = dU_edge + Eab*(edge_vertex1'-X)*Le;
ddU_edge = ddU_edge + Eab*Le;
end
% toc %%记录时间
G=6.67e-11; % 引力常数
rou = 2670; % 433 eros密度 2670 kg/m^3
dU= -G*rou*dU_edge + G*rou*dU_face;
% ddU = G*rou*ddU_edge - G*rou*ddU_face;

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