多机器人系统的群集编队控制理论仿真程序

%解决局部leader的选取问题 clear;close; clc; age6=150;d6=8.4; %群集得到的结果 x6=[244.7856747470721 253.130711655865 262.2922955776236 271.6509422623131 256.3785403303496 249.1667386695703 271.83545852571643 252.24379333003665 250.3725245266302 308.0583593772397 262.7724419788877 263.4349729238124 274.5541986847488 277.7314508444131 275.1102651193679 255.75162531121703 298.7787889160968 265.599236005111 252.86912032909296 271.8896151302412 280.55589041059625 284.33790866148047 259.14345335845917 265.30150224004325 267.74528639711167 252.7595748633976 289.1182725550136 256.5844930434007 274.7557737308817 300.37913807725965 290.7816478667596 309.91245942786105 293.84852650143034 262.3287610319355 259.79581529357534 292.21848177797267 296.37598824407996 266.0959662234472 283.26290607254623 277.64283267326925 280.5354387657268 274.1590841504434 256.7234594986402 259.3104469817834 268.8355376727542 292.5438693521939 274.8300979348465 268.5415201027222 287.9511738585182 265.4278747834835 261.03054985044236 281.8832170291004 305.95042804577804 286.1296410491125 271.0236001013936 271.38234796037875 301.8634565906747 265.27220648480784 259.4219732643188 315.3562126045825 278.26826513389693 292.28811542506975 277.5253523939619 295.3149606963414 289.55416801213465 311.6221450059872 311.7089323110647 298.8168073644731 291.61199610000995 299.4342192105942 303.70873127122246 304.3890264924477 273.7253542046858 268.26533003381155 259.1031115371239 294.80800273411165 246.2306041672154 310.5577009876413 304.59988930358486 304.14344474119366 309.4496749158866 288.6678722406649 301.2884206211138 297.0119919721869 292.2138265332899 297.9873824617662 317.95790515468076 252.5437942589026 276.86712588375434 298.6390579629453 270.3158990081526 265.91517601513783 301.3804712928776 317.6269902790235 302.5595509745302 269.6717320192432 280.508337547357 319.44851507952984 317.1326726772876 306.2042911483781 306.2864394678386 314.09289722957675 313.07903557981876 262.38663586863703 272.5585955205505 307.7181126703651 275.607367982793 249.10876947570844 280.3669837030216 297.2529206874772 256.0338645063019 268.6582540858236 279.3750503874066 290.791050373661 312.0656336352113 284.95794476218936 288.61453560748566 262.2281558269553 286.552405122585 271.11196207687584 278.84423522388295 286.10033489093786 276.87618379386896 293.92119554070797 274.7747622749425 296.0846415118084 283.56080529585614 311.4586494222058 268.1115492380498 294.3808881506218 283.17914710775165 305.7929220077373 282.5249948395082 283.2609539945369 276.99700713711167 314.65141530680495 280.77170006674237 256.04411249525646 307.1222385671579 300.30658668286344 289.5433336500776 300.8499907564595 295.4665872176631 265.2806413815844 291.4859212857574 286.8490146165545 286.27466010653615 283.44359711366036 288.1755548017719 286.59431054378945]; y6=[8.493873120204684 11.089862339692052 5.953222950347888 -7.748270996654416 5.85598709903542 15.760950771490544 -32.60818027598732 -9.723551085662146 5.980478123866531 -16.0100439249324 -13.31049878603675 -23.37853343101694 -2.951151652432847 -16.8768450287528 -21.700739092467103 -4.360922814077406 -19.08728961634749 -8.175430501823318 0.629538229824125 -17.203403925790017 -12.069107492078066 -33.54842841196007 0.836413533382945 1.150224122068511 -28.06227236208328 -18.53609760810024 -7.385710853448632 -13.72350190530123 -12.259472711152144 -8.486723192723602 -34.08187471670179 -22.499531983771973 -28.596650673384072 -3.727819022874942 -18.61765795553758 -2.648218614188641 11.039392048694483 -17.98072632641396 -17.272597893381207 -7.468510773409324 -2.79050498405483 6.004213556937497 -23.955702335395372 11.048192887485554 -12.669068203237742 7.026936965958825 -27.157536449816952 -3.340594453649198 -28.22205549104953 -33.853306249249506 -29.008541275304783 -27.59533580628729 -9.796393318292271 -12.271398012997766 10.752568752343393 1.409888456387008 12.533744423972031 10.96409982838096 -8.663645504034935 -6.066765731194095 -32.2316167385786 -22.900302555053575 1.708643824600883 2.058175688608619 2.208158036535677 14.906705785497875 -11.019211673787169 6.465149214017038 -12.390111967203493 -31.31604700315194 -3.945264123137996 6.345391392256267 15.558401507949856 5.980876434227644 21.51065339827989 -7.711741863812148 1.348943229895969 5.494669575487571 -27.013782316246743 -20.78441225640761 -4.98949033242125 -17.560493838099074 1.382308596784841 16.70817076992819 14.135838748013757 -3.079166256456069 -11.936010874667613 21.50414926855497 10.55724904692147 -24.795124879114038 36.93671832621386 32.12198286025524 19.86156396970751 15.917731762202461 -14.499846163925161 -22.708353473101823 -22.254765305456065 -1.377227841741548 4.482646056114826 16.821888189510982 23.187553751532896 -17.239665517160777 -0.155723516761466 26.91908053577155 31.100486402372862 10.819337239556912 -37.84220357037362 -4.461734602519548 6.439162442954795 -13.03537507364172 16.268621988235502 26.356171715314403 15.063658859592516 25.79436018084314 20.67823554562867 28.153972021918133 10.643393865627568 16.15316731558341 -2.547528939936058 20.84358264636336 29.92801315495592 6.278857858568267 20.201868209849582 -17.497027985172277 25.53763745321486 22.762437310476415 -7.302762449136374 26.961827953402146 15.942251577908245 35.61406999321874 11.356645530501968 29.744552469055602 18.988471658218277 34.167084437043016 35.92554100553551 9.944542793642102 24.366799113677665 27.117130076975364 0.797918804289678 32.83293519585965 31.826283746198417 26.16801566013371 29.154551194973195 21.310323084209777 19.595159953047762 15.956435295844466 -22.700236086844022 1.767009804019813 38.08740028695663 22.343018092251704]; for i=1:age6 x1=x6(i);y1=y6(i); triangle(x1,y1); hold on end for i=1:age6 %^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ agent=i;%记录当前操作对象 k=1; for j=1:age6 if (norm([x6(i)-x6(j),y6(i)-y6(j)])<=d6) x1(k)=x6(j);y1(k)=y6(j); %plot([x6(i),x6(j)],[y6(i),y6(j)],'r');hold on n_sqe(k)=j; k=k+1; end end age=k-1;k=1; x=zeros(1,age);y=zeros(1,age); for i=1:age x(i)=x1(i);y(i)=y1(i); end A=zeros(age,age);sa=zeros(1,age*(age-1)/2);msi=zeros(1,age);msj=zeros(1,age); for i=1:age; for j=(i+1):age; A(i,j)=norm([x(i)-x(j),y(i)-y(j)]); sa(k)=A(i,j); msi(k)=i; msj(k)=j; k=k+1; end end ind=zeros(1,age*(age-1)/2); [ba,ind]=sort(sa); M=zeros(age*(age-1)/2,2*age); for i=1:age*(age-1)/2 ind1=ind(i); indi=msi(ind1); indj=msj(ind1); M(i,2*indi-1)=x(indi)-x(indj);M(i,2*indi)=y(indi)-y(indj); M(i,2*indj-1)=x(indj)-x(indi);M(i,2*indj)=y(indj)-y(indi); end sqe=zeros(1,2*age-3); sqe(1)=1; MM=0; MM=M(1,1:2*age);kk=1; for i=2:age*(age-1)/2 N=[MM;M(i,1:2*age)]; R=rank(N); if (R==kk+1) MM=N; sqe(kk+1)=i; kk=kk+1; end if (R>=2*age-3) break; end end kt=1; for i=1:2*age-3 iid=sqe(i); id=ind(iid); idi=msi(id); idj=msj(id); %记录min rigid的边 agent_to(agent,kt)=n_sqe(idi); agent_to(agent,kt+1)=n_sqe(idj); kt=kt+2; %记录min rigid的边 end end %判别边是否属于min-weighted rigid [m,n]=size(agent_to);agent_re=agent_to; for i=1:age6 for hvi=1:age6 if (hvi~=i) for hvj=1:2:n if (agent_re(hvi,hvj)~=0) if (norm([x6(agent_re(hvi,hvj))-x6(i),y6(agent_re(hvi,hvj))-y6(i)])<=d6&&norm([x6(agent_re(hvi,hvj+1))-x6(i),y6(agent_re(hvi,hvj+1))-y6(i)])<=d6)%检测边的两端点是否为所检查点的rigid内的点 k_p=0; for jj=1:2:n if ((agent_re(i,jj)==agent_re(hvi,hvj)&&agent_re(i,jj+1)==agent_re(hvi,hvj+1))||(agent_re(i,jj)==agent_re(hvi,hvj+1)&&agent_re(i,jj+