www.pudn.com > globalpathplanning.rar > main.m, change:2012-03-13,size:4166b


%main 
%障碍和目标,起始位置都已知的路径规划,意图实现从起点可以规划出一条避开障碍到达目标的路径。 
%初始化车的参数 
global total weightx weighty weightz 
global  rpoint 
%定义全局变量,保存权重等参数值 
total=40; 
weightx=[1,-1,0,0,0,0]; 
weighty=[0,0,1,-1,0,0]; 
weightz=[0,0,0,0,1,-1]; 
H=[0,1,0,1,0,6]; 
%初始化起点和终点 
rpoint(1).x=0.2; 
rpoint(1).y =1; 
rpoint(1).z =0.2; 
rpoint(total).x =0.9; 
rpoint(total).y =0; 
rpoint(total).z =5;     
%将路径上的点进行初始化操作 
for i=2:total-1 %循环求解出所有点初始坐标 
	 
    rpoint(i).x = rpoint(1).x + i*(rpoint(total).x -rpoint(1).x )/(total-1); 
    rpoint(i).y = rpoint(1).y + (rpoint(total).y -rpoint(1).y )*(rpoint(i).x -rpoint(1).x )/(rpoint(total).x -rpoint(1).x ); 
    rpoint(i).z = rpoint(1).z + (rpoint(total).z -rpoint(1).z )*(rpoint(i).x -rpoint(1).x )/(rpoint(total).x -rpoint(1).x ); 
	   
end     
%***********************************画出障碍,起点,目标,路径点********************* 
 %画出障碍物和路径 
    boxplot3(0,0,0,1,1,6); 
    axis([-0.5 2 -0.5 2 0 7]);%坐标系范围 
    xlabel('x'); %标记各个轴 
    ylabel('y'); 
    zlabel('z'); 
    title('路径');%命名图示 
    
  %画路径 
          for i=1:total 
               plot3(rpoint(i).x,rpoint(i).y,rpoint(i).z,'rd');       
          end 
 %判断所有点是否在外部,不在则进行优化 
     isrpointInside(1,:)=ones(1,total);%定义标记,判断是否38个路径结点是否都在物体外部,1表在物体内部	 
	for k=1:100 %设置结束条件,比如最大循环次数k 
      
		isAllOutside=1;%定义标记,38个点只要存在一点在内部,则标记为0,初始1假定都在外部 
                       
		for i=2:total-1  %对点P2 to P total判断,若点在物体内部flag[i]标记为1 
             
			 for j=1:6 %判断每个点			 
				  temp=rpoint(i).x*weightx(j)+rpoint(i).y*weighty(j)+rpoint(i).z*weightz(j)+H(j); 
					 if temp<0            %if表达式成立则代表点是在外部的。				  
					   isrpointInside(i)=0;%只要有一个不符合,则在物体外部,设置点Pi对应的标记为0 
					   j=j-1;                                                
                       break  %判断出该点在外部,跳出此次循环 
                     end 
             end 
             j=j+1; 
			 if j>6 
			 %temp都大于0,此中的点是在内部。 
				 isAllOutside=0;%j>6表示j循环正常结束,此节点为内部部点,设置为0 
				 isrpointInside(i)=1;			  
             end    
        end 
         for i = 2:total-1 
			 if isrpointInside(i)==1             %点为内部的点,用公式15优化	 
		                                         %注意if判断必须用==。 
				for coord = 0:2 %此中coord代表选取哪一个坐标轴上的值 
                    optimizerpoint15(rpoint,i,coord);   %这里函数调用的时候直接传递结构体rpoint,i表示第i个点,coord表示x,y,z 
                end 
             else                                %外部点则用公式18优化 
			 %公式18计算			 
				k1 = -0.8*(2*rpoint(i).x -rpoint(i-1).x -rpoint(i+1).x );  
				k2 = -0.8*(2*(rpoint(i).x+0.01*k1/2) -rpoint(i-1).x -rpoint(i+1).x ); 
				k3 = -0.8*(2*(rpoint(i).x+0.01*k2/2) -rpoint(i-1).x -rpoint(i+1).x ); 
				k4 = -0.8*(2*(rpoint(i).x+0.01*k3) -rpoint(i-1).x -rpoint(i+1).x ); 
				rpoint(i).x = rpoint(i).x +0.01*(k1+2*k2+2*k3+k4)/6; 
 
				k1 = -0.8*(2*rpoint(i).y -rpoint(i-1).y -rpoint(i+1).y );  
				k2 = -0.8*(2*(rpoint(i).y+0.01*k1/2) -rpoint(i-1).y -rpoint(i+1).y ); 
				k3 = -0.8*(2*(rpoint(i).y+0.01*k2/2) -rpoint(i-1).y -rpoint(i+1).y ); 
				k4 = -0.8*(2*(rpoint(i).y+0.01*k3) -rpoint(i-1).y -rpoint(i+1).y ); 
				rpoint(i).y = rpoint(i).y +0.01*(k1+2*k2+2*k3+k4)/6; 
 
				k1 = -0.8*(2*rpoint(i).z -rpoint(i-1).z -rpoint(i+1).z );  
				k2 = -0.8*(2*(rpoint(i).z+0.01*k1/2) -rpoint(i-1).z -rpoint(i+1).z ); 
				k3 = -0.8*(2*(rpoint(i).z+0.01*k2/2) -rpoint(i-1).z -rpoint(i+1).z ); 
				k4 = -0.8*(2*(rpoint(i).z+0.01*k3) -rpoint(i-1).z -rpoint(i+1).z ); 
				rpoint(i).z = rpoint(i).z +0.01*(k1+2*k2+2*k3+k4)/6;			  
             end 
			 
         end 
     
    end 
        %%所有点都在外部时 输出结果 
		if isAllOutside==1	 
			for count=1:total-1		 
				plot3(rpoint(count).x,rpoint(count).y,rpoint(count).z,'o');               
            end 
        end