人工勢場演算法，用於路徑規劃
main.m程式
%初始化車的引數
xo=[0 0];%起點位置
k=15;%計算引力需要的增益係數
k=0;%初始化
m=5;%計算斥力的增益係數，都是自己設定的。
po=2.5;%障礙影響距離，當障礙和車的距離大於這個距離時，斥力為0，即不受該障礙的影響。也是自己設定。
n=7;%障礙個數
a=0.5;
l=0.2;%步長
j=200;%迴圈迭代次數
%如果不能實現預期目標，可能也與初始的增益係數，po設定的不合適有關。
%end
%給出障礙和目標資訊
xsum=[10 10;1 1.2;3 2.5;4 4.5;3 6;6 2;5.5 5.5;8 8.5];%這個向量是(n+1)*2維，其中[10 10]是目標位置，剩下的都是障礙的位置。
xj=xo;%j=1迴圈初始，將車的起始座標賦給xj
%***************初始化結束，開始主體迴圈******************
for j=1:j %迴圈開始
goal(j,1)=xj(1); %goal是儲存車走過的每個點的座標。剛開始先將起點放進該向量。
goal(j,2)=xj(2);
%呼叫計算角度模組
theta=compute_angle(xj,xsum,n);%theta是計算出來的車和障礙，和目標之間的與x軸之間的夾角，統一規定角度為逆時針方向，用這個模組可以計算出來。
%呼叫計算引力模組
angle=theta(1);%theta（1）是車和目標之間的角度，目標對車是引力。
angle_at=theta(1);%為了後續計算斥力在引力方向的分量賦值給angle_at
[fatx,faty]=compute_attract(xj,xsum,k,angle,0,po,n); %計算出目標對車的引力在x,y方向的兩個分量值。
for i=1:n
angle_re(i)=theta(i+1);%計算斥力用的角度，是個向量，因為有n個障礙，就有n個角度。
end%呼叫計算斥力模組
[frerxx,freryy,fataxx,fatayy]=compute_repulsion(xj,xsum,m,angle_at,angle_re,n,po,a);%計算出斥力在x,y方向的分量陣列。
%計算合力和方向，這有問題，應該是數，每個j迴圈的時候合力的大小應該是乙個唯一的數，不是陣列。應該把斥力的所有分量相加，引力所有分量相加。
fsumyj=faty+freryy+fatayy;%y方向的合力
fsumxj=fatx+frerxx+fataxx;%x方向的合力
position_angle(j)=atan(fsumyj/fsumxj);%合力與x軸方向的夾角向量
%計算車的下一步位置
xnext(1)=xj(1)+l*cos(position_angle(j));
xnext(2)=xj(2)+l*sin(position_angle(j));
%儲存車的每乙個位置在向量中
xj=xnext;
%判斷if ((xj(1)-xsum(1,1))>0)&((xj(2)-xsum(1,2))>0) %是應該完全相等的時候算作到達，還是只是接近就可以？現在按完全相等的時候程式設計。
k=j;%記錄迭代到多少次，到達目標。
break;
%記錄此時的j值
end%如果不符合if的條件，重新返回迴圈，繼續執行。
end%大迴圈結束
k=j;
goal(k,1)=xsum(1,1);%把路徑向量的最後乙個點賦值為目標
goal(k,2)=xsum(1,2);
%***********************************畫出障礙，起點，目標，路徑點*************************
%畫出路徑
x=goal(:,1);
y=goal(:,2);
%路徑向量goal是二維陣列,x,y分別是陣列的x,y元素的集合，是兩個一維陣列。
x=[1 3 4 3 6 5.5 8];%障礙的x座標
y=[1.2 2.5 4.5 6 2 5.5 8.5];
plot(x,y,'o',10,10,'v',0,0,'ms',x,y,'.r');
function y=compute_angle(x,xsum,n)%y是引力，斥力與x軸的角度向量,x是起點座標，xsum是目標和障礙的座標向量,是(n+1)*2矩陣
for i=1:n+1%n是障礙數目
deltax(i)=xsum(i,1)-x(1);
deltay(i)=xsum(i,2)-x(2);
r(i)=sqrt(deltax(i)^2+deltay(i)^2);
if deltax(i)>0
theta=acos(deltax(i)/r(i));
else
theta=pi-acos(deltax(i)/r(i));
endif i==1%表示是目標
angle=theta;
else
angle=theta;
endy(i)=angle;%儲存每個角度在y向量裡面，第乙個元素是與目標的角度，後面都是與障礙的角度
endend
function [yatx,yaty]=compute_attract(x,xsum,k,angle,b,po,n)%輸入引數為當前座標，目標座標，增益常數,分量和力的角度
%把路徑上的臨時點作為每個時刻的xgoal
r=(x(1)-xsum(1,1))^2+(x(2)-xsum(1,2))^2;%路徑點和目標的距離平方
r=sqrt(r);%路徑點和目標的距離
yatx=k*r*cos(angle);%angle=y(1)
yaty=k*r*sin(angle);
end
function [yrerxx,yreryy,yataxx,yatayy]=compute_repulsion(x,xsum,m,angle_at,angle_re,n,po,a)%輸入引數為當前座標，xsum是目標和障礙的座標向量，增益常數,障礙，目標方向的角度
rat=(x(1)-xsum(1,1))^2+(x(2)-xsum(1,2))^2;%路徑點和目標的距離平方
rat=sqrt(rat);%路徑點和目標的距離
for i=1:n
rrei(i)=(x(1)-xsum(i+1,1))^2+(x(2)-xsum(i+1,2))^2;%路徑點和障礙的距離平方
rre(i)=sqrt(rrei(i));%路徑點和障礙的距離儲存在陣列rrei中
r0=(xsum(1,1)-xsum(i+1,1))^2+(xsum(1,2)-xsum(i+1,2))^2;
r0=sqrt(r0);
if rre(i)>po%如果每個障礙和路徑的距離大於障礙影響距離，斥力令為0
yrerx(i)=0;
yrery(i)=0;
yatax(i)=0;
yatay(i)=0;
else
%if r0直接放到matlab執行即可看到效果。
軌跡規劃方法 人工勢場法
人工勢場法是由khatib提出的一種虛擬力法。它的基本思想是將被控物件在空間中的運動虛擬成乙個質點在虛擬力場中的受力運動，即 將機械人在周圍環境中的運動，設計成一種抽象的人造引力場中的運動，目標點對移動機械人產生 引力 障礙物對移動機械人產生 斥力 最後通過求合力來控制移動機械人的運動。應用勢場法規...
壓縮感知OMP演算法 OMP演算法的Matlab版本
omp演算法 omp的函式 s 測量 t 觀測矩陣 n 向量大小 function hat y omp fun s,t,k n size t,2 size size t 觀測矩陣大小 m size 1 測量 hat y zeros 1,n 待重構的譜域 變換域 向量 aug t 增量矩陣 初始值為空...
單源最短路Dijkstra演算法 matlab實現
迪傑斯特拉 dijkstra 演算法是典型最短路徑演算法，用於計算乙個節點到其他節點的最短路徑。它的主要特點是以起始點為中心向外層層擴充套件 廣度優先搜尋思想 直到擴充套件到終點為止。基本思想 通過dijkstra計算圖g中的最短路徑時，需要指定起點s 即從頂點s開始計算 此外，引進兩個集合s和u。...