買書捐殘盟

2011年6月14日 星期二

採用滑動模式設計,應用於NXT馬達定位控制

根據前述(線性非時變,可變結構滑動模式的概念),茲嘗試建立滑動模式控制應用於NXT馬達定位控制,結果定位效果顯著且迅速。

相關參數簡述如下:
(一)滑動曲面函數S(X)=0.7X1+X2=0
   其中X1=期望值-實際量測值,X2=X1變化率
(二)當S‧X1>0,提供一正向驅動控制量,本例為Ud=2*err;
   當S‧X1<0,提供一正向驅動控制量,本例為Ud=-2*err;
   滑動模式控制量Ueq=-0.5*err;
(三)總驅動控制量U=Ud+Ueq
(四)紀錄X1、X2及馬達回應的encoder轉動值,儲存至NXT主機中(本例指定為DataLog.txt)。

[執行結果]



左圖為定位控制的觀測值,期望值為180度。由圖中可看出定位速度很快。













左圖為x1,x2的平面軌跡圖。
迅速滑向原點。















上述參數可由使用者自行更改,以求較佳控制,筆者僅略以此控制法則說明滑動模式非常適用。

[原始程式]
task main()
{
ResetRotationCount(OUT_A);
long readvalue=0;
long DesirePosition=180;
long err=0,prev_err=0;;
long cerr=0;
float S=0;
float Ud=0,Ueq=0;
int U=0;

byte fileHandle;
short bytesWritten;
DeleteFile("DataLog.txt");
CreateFile("DataLog.txt",10000,fileHandle);
string str_position;
string str_err;
string str_cerr;
string str_log;

while(!ButtonPressed(BTNCENTER,true))
{
str_position=NumToStr(readvalue);
str_err=NumToStr(err);
str_cerr=NumToStr(cerr);
str_log= str_position+" " +str_err+ " " +str_cerr;
WriteLnString(fileHandle,str_log,bytesWritten);

readvalue=MotorRotationCount(OUT_A);
err=DesirePosition-readvalue;
cerr=(err-prev_err);

S=0.7*(err)+(cerr);
prev_err=err;

if(S*err>0)
{
Ud=2*err;
}
if(S*err<0) { Ud=-2*err; } Ueq= -0.5*err; U=Ud+Ueq; if(U>100) U=100;
if(U<-100) U=-100; OnFwd(OUT_A,U); TextOut(0,LCD_LINE1,"degree="); NumOut(6*8,LCD_LINE1,readvalue); Wait(50); Off(OUT_A); ClearScreen(); } CloseFile(fileHandle); }

沒有留言:

張貼留言