1. عنوان پروژه: كنترل بهينه وضعيت فضاپيماي صلب توسط
چرخ عكس العملي در زواياي بزرگ
استاد: پروفسور پورتاکدوست
دانشجو: شکيب شريفيان ٨۶٢٠٣٩۶۴
پاييز ٨٧
2.
3. فهرست
بخش 1
-1 تعريف پروژه 1 -1
5 DP در LQR -2 روش -1
6 H -3-1 سياست محاسبه
7 H= در 1 ρ با تغيير پارامتر R -4 محاسبه -1
بخش 2
16 N بر حسب مقدار Steady state gain factor رسم نمودارهاي
بخش 3
23 ρ رسم تغييرات بخش كنترلي تابع هزينه در ازاي تغيير ضريب
پيوست
25 كد برنامه 1
27 كد برنامه 2
29 كد برنامه 3
31 كد برنامه 4
33 كد برنامه 5
35 كد برنامه 6
كد برنامه 7 و زير برنامه ها 36
4. -1-1 تعريف پروژه:
در اين پروژه سيستم ما يك فضاپيماست كه با توجه به معادلات 1 سعي مي كنيم تا بـا اسـتفاده از
چرخ هاي عكس العملي(شكل 1) و توليد نرخ مومنتوم زاويه اي در هر لحظه هـم متغييرهـاي حالـت
سيستم را در يك مسير خواسته شده به پيش ببريم و هم بـر روي هزينـه تبـ ديل ايـن متغييرهـاي
حالت كه طبق معادله 2 داده شده نظارت داشته باشيم(هدف كنترل بهينه در اين پروژه).
&
J I J J h h u
( ) ( )
− Ω = − Ω Ω − Ω + Ω −
1 1 1 2 3 2 3 3 2 2 3 1
&
J I J J h h u
( ) ( )
− Ω = − Ω Ω − Ω + Ω −
2 2 2 3 1 3 1 1 3 3 1 2
&
J I J J h h u
( ) ( )
− Ω = − Ω Ω − Ω + Ω −
3 3 3 1 2 1 2 2 1 1 2 3
& &
h u I for i
2
1 = + ρ ∫ + +
f [ ( ) ( ) 2 ( )]
&& & & & &&
t f
T + T = I + 4 ( I − I ) + ( I − I − I )
+ h − h − h − h −
I
&& &
I I I
− − +
&& & && & && &
T T I I I h I I
3 ( ) ( 2 ) ( 2 )
+ = + − + + − − + − − +
&& & & & &&
T T I I I I I I h h h h I
١
١- چرخ عکس العملی - شکل ١
(١)
(٢)
خطي شده معادلات ( 1) به صورت زير است:
(٣)
φ ω φ ω ψ ω ψ φω θ
dx cx x y z y z x wz wz wy wy xy
ψ ω ψ ω θ
xz xz yz
θ ω θ φ ω ψ ω φ ψ ω φ ω ψ
dy cy y x z wy xy yz
+ = + + − + − + − + − −
ψ ω φ ω ψ ω φ ψω θ
dz cz z z x y y x wz wx wy wy yz
2
0
&& &&
I I I
φ φ ω φ
xz xz xz
0 0 0 0
2
0 0
2
0 0
2
0 0
2
0
0
2
0
0 0 0 0 0
2
0
2
( ) ( )
2
− − −
1,2,3
= − Ω =
i i i i
J t u t u t u t dt
3
0
2
2
5. ⎧
⎪ ⎪ ⎪ ⎪ ⎪ ⎪
⎨
x =
x
J x J J x J J J x u x
0 4 ( ) ( )
ω ω ω
= + − + − − + −
x =
x
J x J J x u
0 3 ω
( )
= + − +
x =
x
J x J J J x J J x u x
0 ( ) ( )
= + + − + − + +
x &
=
u
x &
u
٢
براي شروع بايد معادلات را در فضاي حالت بنويسيم، كه در اين مورد متغيرهاي حالت و كنترل را
بفرم زير تعريف مي كنيم:
(۴)
φ
⎫
⎪ ⎪ ⎪ ⎪ ⎪ ⎪
&
φ
θ
&
θ
ψ
⎬
⎪ ⎪ ⎪ ⎪ ⎪ ⎪
⎭
⎧
⎪ ⎪ ⎪ ⎪ ⎪ ⎪
⎨
⎪ ⎪ ⎪ ⎪ ⎪ ⎪
⎩
=
⎫
⎪ ⎪ ⎪ ⎪ ⎪ ⎪
⎬
⎪ ⎪ ⎪ ⎪ ⎪ ⎪
⎭
⎪ ⎪ ⎪ ⎪ ⎪ ⎪
⎩
h
1
2
3
x
1
2
3
4
5
6
7
8
9
h
h
x
x
x
x
x
x
x
x
ψ
&
;
⎫
⎪⎭
⎪⎬
⎧
=
⎪⎩
⎪⎨
⎫
⎪⎭
⎪⎬
u
⎧
⎪⎩
⎪⎨
&
h
1
&
2
3
3
2
3
h
h
u
u
&
1 2
1 2 0
3 4
2
2 4 0
5 6
7 1
=
8 2
9 3
2 3 1 0 2 3 1 6 1 0 9
2 1 5 3 0 7
2
1 3 3 2
2
3 6 0 3 1 2 2 0
x &
=
u
&
&
&
&
&
&
ω ω ω
براي اين منظور بايد تعدادي از پارامترها را بعنوان متغييرهاي حالت در نظـر بگيـريم كـه در اينجـا
بهتر است از هم ان متغييرهاي حالتي كـه در پـروژه محـدوديت دارنـد ، اسـتفاده كنـيم و مـاتريس
كنترلي هم طبق خواسته پروژه براساس روابط زير مي باشند:
(۵)
[ ]
⎤
⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥
⎦
⎡
⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢
⎣
0 0 0
1/ 0 0
0 0 0
0 1/ 0
0 0 0
0 0 −
1/
−
−
=
1 0 0
0 1 0
0 0 1
3
2
1
J
J
J
B
6. A J B J J
⎡
− −
A = ω
J −
J
B = ω
J − J −
J
C = ω
J −
J
D J J J
٣
متغييرهاي حالت نيز طبق روابط زير مي باشند:
(۶)
[ ]
0 1 0 0 0 0 0 0 0
/ 0 0 0 0 ω / 0 0 ω
/
1 0 1 0 1
0 0 0 1 0 0 0 0 0
C J
0 0 / 0 0 0 0 0 0
2
−
0 0 0 0 0 1 0 0 0
0 / 0 0 / 0 / 0 0
0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0
4 ( )
( )
0 2 3 1
3 ( )
( )
ω
= + −
0 3 1 2
( )
2 1
2
0
1 3
2
0
2 3
2
0
3 3 0 3
E J J
D J E J J
A
= −
⎤
⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥
⎦
⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢
⎣
− − −
=
ω
ω
همانگونه ك ه مشاهده مي شود معادلا ت سيستم م ا غير خطي مي باشند، پس دو كار مـي تـوان انجـام
داد:
بصورت غيرخطي Bellman's recursive -1 حل معادلات با روش
LQR -2 خطي كردن معادلات و استفاده از روش
با توجه به تعداد زياد متغييرهاي حالت و كنترلي، حجم بالاي محاسباتي (كـد برنامـه نوشـته شـده ،
ولي زمان اجراي بر نامه بالا مي باشد) به سختي امكان پياده سازي با اين روش وجـود دارد، بنـابراين
به حل معادلات مربوطه مي پـردازيم، بـراي ايـن منظـور LQR با خطي سازي معادلات و استفاده از
ابتدا بايد معادلات 3كـه بـه فـرم فـضاي حالـت در آمـده انـدرا از فـرم پيوسـته بـه گسـسته تبـديل
( كنيم(معادلات 7
7. x k + = x k +
dt x k
x k + = x k − dt + ω J − J x k + ω J − J − J x k + u k −
ω
x k J
x k + = x k +
dt x k
x k + = x k − dt ω
J − J x k +
u k J
x k + = x k +
dt x k
x k + = x k − dt J + J − J x k + J − J x k + u k +
x k J
x k + = x k +
dt u k
x k x k dt u k
f + + Δ + = Σ−
2
1 J t k t u k u k u k
A J B J J
⎡
− −
A = ω
J −
J
B = ω
J − J −
J
C = ω
J −
J
D J J J
٤
(٧)
( 1) ( ) ( ( ))
2
1 1 2
( 1) ( ) ( 4 ( ) ( ) ( ) ( ) ( ) ( )) /
2 2 0
( 1) ( ) ( ( ))
2
3 3 4
2 3 1 0 2 3 1 6 1 0 9 1
( 1) ( ) ((3 ( ) ( ) ( )) / )
4 4 0
( 1) ( ) ( ( ))
5 5 6
1 3 3 2 2
( 1) ( ) (( ( ) ( ) ( ) ( ) ( ) ( )) / )
( 1) ( ) ( ( ))
7 7 1
( + 1) = ( ) +
( ( ))
8 8 2
( 1) ( ) ( ( ))
9 9 3
2 1 5 3 0 7 3
2
6 6 0 3 1 2 2 0
x k + = x k +
dt u k
ω ω ω
( ) ( ( ) ( ) 2 ( )
ρ (٨)
3
2
2
1
0
N
k
=
با مرتب كردن متغييرهاي حالت و كنترلي نهايتـا مـاتريس هـاي حالـت و كنتـرل مـا بفـرم زيـر در
مي آيند:
(٩)
[ ]
1 1 0 0 0 0 0 0 0
/ 1 0 0 0 ω / 0 0 ω
/
1 0 1 0 1
0 0 1 1 0 0 0 0 0
C J
0 0 / 1 0 0 0 0 0
2
−
0 0 0 0 1 1 0 0 0
0 / 0 0 / 1 / 0 0
0 0 0 0 0 0 1 0 0
0 0 0 0 0 0 0 1 0
0 0 0 0 0 0 0 0 1
4 ( )
( )
0 2 3 1
3 ( )
( )
ω
= + −
0 3 1 2
( )
2 1
2
0
1 3
2
0
2 3
2
0
3 3 0 3
E J J
D J E J J
A
= −
⎤
⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥
⎦
⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢
⎣
− − −
=
ω
ω
8. T T
F N − K = − R + B P K − B B P K −
A
P K = A + BF N − K P K − A + BF N − K F N − K RF N − K +
Q
u N K F N K x N K
( ( ) ( ) 1
2
J xT t Hx t x t Qx t u t Ru t dt
٥
:DP در LQR -2-1 روش
از آخرين مرحله به مرحله اول شروع به حركت كرده و مقادير بهينه كنترلي DP در كليه روش هاي
با توجه به هزينه محاسبه مي شوند.
براي حالت خطي است كه به سرعت به جـواب مـي رسـ د و DP نيز يكي از روش هاي LQR قاعده
بجاي يك تاريخچه كلي از همه حالتها و كنترل هاي P(K) و F(N-K) شما با داشتن يك ماتريس
موجود در هر لحظه امكان بدست آوردن هزينه و كنترل مينيمم را داريد.
و P(K) و F(N-K) در صورتي كه معادله حالت ما در حالت گسسته بـه فـرم زيـر باشـد، مقـادير
و تابع هزينه از دسته روابط 10 بدست مي آيند. min(U)
(١٠)
x(k +1) = Ax(k) + Bu(k)
( ) [ ( 1) ] ( 1)
( ) [ ( )] ( 1)[ ( )] ( ) ( )
( − ) = ( − ) ( −
)
( ) ( ) ( )
( ) 1
2
*
,
1
T
J J N K x N K P K x N K
N K N
T T
= − = − −
∗
−
−
از روي تابع هزينه گسسته شده 11 داريم. R و Q ،H با توجه به مقادير ماتريس
(١١)
t f
= + ∫ +
t
T T
f f
0
( ( ) ( ) ( ( ) ( ))
2
1
با دقت روي تابع هزينه متوجه مي شويم كه:
-1 بيشتر هزينه صرف كم كردن فرامين كنترلي است
هزينه و مقادير كنترلي تحت تاثير قرار مي گيرند. H -2 با تعاريف متفاوت از
بيشترين تلاش بـراي رسـيدن بـه دقـت بـالا در مقـدار نهـايي ،H -3 در صورت بالا بردن اثر
متغييرهاي حالت در حالت نهايي است و در مقابل توجه به هزينه اين عمل كمتر خواهد بود.
بيـشترين تـلاش بـراي رسـيدن بـه كمتـرين هزينـه در مقـدار ،R -4 در صورت بالا بردن اثر
متغييرهاي حالت در حالت نهايي است و در مقابل به دقت كمتر توجه خواهد شد.
در نتيجه مقدار نسبي اين ضرايب ماتريسي است كه نشان دهنده توجه مـا بـراي به ينـه كـردن
هزينه كل نسبت به آن پارامتر مي باشد.
9. ٦
:H -3-1 سياست محاسبه
به گونـه اي كـه هـر دو H راه حل ما براي ايجاد يك تعادل، استفاده از يك حلقه براي محاسبه
عامل محدوديت در تلاش فرا مين كنترلي و دقت متغييرهاي حالت در نظر گرفته شـوند، يعنـي
هاي گوناگون تابع هزينه محاسبه شده و تـا زمـاني كـه تمـام شـرايط و محـدوديت هـاي -H با
كوچـك و H فرامين كنترلي بر آورده نشوند، حلقه متوقف نخواهد شد؛ براي اين كـار يكبـار از
بـا مقـادير H نزديك صفر شروع به حركت به مقادير بالا كرده و بلافاصله بعـد از يـافتن اولـين
بدست آيد (كـد برنامـه 1). مقـدار H كوچك به سمت صفر حركت مي كنيم تا تقريب دقيقي از
1)، كـه مقـادير كنترلـي و وضـعيتي مـا در - شكل 2 )H= بدست آمده برابر است با 0.15944
1 ديــده مــي شــوند(كــد برنامــه 3)، در ايــن وضــعيت گرچــه تمــامي - 1 تــا 5 - نمودارهــاي 1
محدوديت هاي كنترلي و متغييرهاي حالت بر آورده مي شوند ولي متوجه مي شـويم كـه در ايـن
به حالت نهايي خواسته شده نمي رسيم، پـس يـا بايـد محـدوديت هـا را ناديـده t = 45s زمان
بازي كرد و يا مدت زم ان كنترلـي در هزينـه را بـالا بـرد، كـه طبـق R و H گرفت و با مقادير
محاسبه انجام داده شده توسط كـد برنامـه ( 2) ايـن اتفـاق بـراي خطـاي كمتـر از 1 درجـه در
خواهد افتاد. t = 315s
1 توليد خواهد شد كه با ت وجه به - 1 تا 15 - نمودارهاي 11 H در صورت صفر قرار دادن مقدار
در صورت تكـرار خـواهيم ديـ د كـه ايـن پديـده قابـل ،Q = دسته روابط ( 10 )، و با توجه به 0
پيش بيني بود، يعني وقتي تنها هزينه مهم ما هزينه كنترلي باشد ، طبيعـي اسـت كـه كمتـرين
(h=0, هزينه كنترلي انجام ندادن هيچ واكنشي خواهد بود.(كد برنامه 3
1 خواهد بـود ، و ايـن افـزايش بـه - 1 تا 10 - رفتار ما طبق نمودار هاي 6 H با زياد كردن مقدار
معناي بالا بردن اهميت مقدار نهايي متغييرهاي حالت در مقابـل هزينـه كنترلـي مـي باشـد، و
همانگونه كه در نمودارها هم مي بينيد، با اين شرط محدوديت هاي كنترلي ما شكـسته خواهنـد
(h=100, شد.(كد برنامه 3
H ١ اجرای کد برنامه ١ برای محاسبه - شکل ٢
10. ٧
:H= در 1 ρ با تغيير پارامتر R -4-1 محاسبه
3)، و حالتي كه بتواند - براي اين منظور از كد برنامه 4 در پيوست استفاده شده است(شكل 1
بدست مي آيد. ρ = . محدوديت هاي مساله را ارضا كند در 0069323
R ١ اجرای کد برنامه ۴ برای محاسبه - شکل ٣
19. ١٦
:N بر حسب مقدار Steady state gain factor -2 رسم نمودارهاي
استفاده شـده،و از آنجـايي N براي اين منظور از كد برنامه 5 و مقادير 20،50،200،500 براي
بدست آمد (طبق مطالب گفته شده )، بنابراين نمي توان انتظـار داشـت H ≠ كه در اين پروژه 0
به يك مقدار ثابت ميل كند، كـه در نمودارهـاي Steady state gain N كه با افزايش مقدار
2 مي توان اين موضوع را مشاهده كرد، در ضمن برخي از متغييرهاي حالت با توجـه - 2-1 تا 12
و دسته روابط ( 10 )، تاثيري در صـدور فرمـان كنترلـي Steady state gain به صفر بودن
ندارند!
به ازای کنترل ١ ،N= در 20 steady state gain ١- مقادير - نمودار ٢
20. ١٧
به ازای کنترل ٢ ،N= در 20 steady state gain ٢- مقادير - نمودار ٢
به ازای کنترل ٣ ،N= در 20 steady state gain ٣- مقادير - نمودار ٢
21. ١٨
به ازای کنترل ١ ،N= در 50 steady state gain ۴- مقادير - نمودار ٢
به ازای کنترل ٢ ،N= در 50 steady state gain ۵- مقادير - نمودار ٢
22. ١٩
به ازای کنترل ٣ ،N= در 50 steady state gain ۶- مقادير - نمودار ٢
به ازای کنترل ١ ،N= در 200 steady state gain ٧- مقادير - نمودار ٢
23. ٢٠
به ازای کنترل ٢ ،N= در 200 steady state gain ٨- مقادير - نمودار ٢
به ازای کنترل ٣ ،N= در 200 steady state gain ٩- مقادير - نمودار ٢
24. ٢١
به ازای کنترل ١ ،N= در 500 steady state gain ١٠ - مقادير - نمودار ٢
به ازای کنترل ٢ ،N= در 500 steady state gain ١١ - مقادير - نمودار ٢
25. ٢٢
به ازای کنترل ٣ ،N= در 500 steady state gain ١٢ - مقادير - نمودار ٢
26. :ρ -3 رسم تغييرات بخش كنترلي تابع هزينه در ازاي تغيير ضريب
براي اين بخش از كد برنامه 6 استفاده شده است،همانگونه كه مشاهد ش د مـي شـود در مقـادير
بسيار زياد بوده و به شدت بخش كنترلي تابع هزينه را كاهش مـي دهـد، ρ پايين تاثير افزايش
0، كمتر شده و به / بيشتر از 1 ρ 3 ديده مي شود، اين اثر با افزايش - اما همانگونه كه در نمودار 2
مقدار ثابت حدود 7 ميل مي كند.
٢٣
٠/ ٠ تا ٠٠٩ / در بازه ٠٠٠١ ρ ١- تغييرات بخش کنترلی تابع ھزينه در ازای تغيير ضريب - نمودار ٣
٠ تا ٢ / در بازه ٠٠٠١ ρ ١- تغييرات بخش کنترلی تابع ھزينه در ازای تغيير ضريب - نمودار ٣
27. ٢٤
-4 نتيجه گيري و پيشنهاد:
و خطي سازي، تمامي خواسته هاي مساله يكجا مرتفع نمي گـردد و بايـد از LQR -1 با روش
نظاير روش هاي بكار رفته در اين پروژه استفاده كرد.
-2 در روش غير خطي نيز بدليل حجم بالاي مح اسبات بايد بسياري از موارد نظير ميان يـابي و
تقسيم بندي بازه ها تضعيف شوند تا بتوان از برنامه اجرا گرفت، كه اين خود از اطم ينان به حـل
مي كاهد.(كد برنامه 7 و زير برنامه ها)
-3 در كد نويسي مي توان با توجه به اثر متغيير حالتي كه باعث عبـور از محـدوده تعيـين شـده
را بعنوان متغيير در نظر گرفت و در اين حالت احتمالا H كنترلر ما مي شود، دراي ههاي ماتريس
هم در مدت 45 ثانيه به متغييرهاي LQR اي پيدا كرد كه در آن حالت با – H بتوان ماتريس
دلخواه دست يافت.
-4 در حل غيرخطي بدليل نياز به يك كنترلر با رفتار آرام و با اين ديد كه در وارد شدن شـتابها
به جسم محدوديت داريم، در هر حالت تنها متغييرهاي حالت نزديك به متغيير حالـت مرحلـه
بعد را در نظر گرفت و با اين كار از حجم محاسبات تا حدي كاست.
39. tx=0:dt:tf;
tu=0:dt:tf-dt;
%calculate cost function
cost_value(ro_iter)=sum(min_j);
figure(1)
plot(ro(ro_iter),cost_value(ro_iter))
hold on
drawnow
end%ro_iteration
٣٦
. پايان كد برنامه 6
كد برنامه 7 و زير برنامه ها:
%+++++++++++ main program DP ++++++++++++
clc
clear
tf=45;%sec
x_prepare(1:3)=[0 0 0];%final omega
x_prepare(4:6)=[-7 -7 -7];%final h
x_prepare(7:10)=[1 0 0 0];%final quaternion
dt=5;%delta t =5s ==>45/5=9step
u=[-1.05:.2625:1.05];%controller signal
x=[-.05:.025:0.05];%interval for omega; state signal
xx=[-7:3.5:7];%interval for h
xxx=[-1, -.7233,-.532,-.4397,-.0223,0, .0223,.4397, .532, .7233,1];
user_wanna=3;%kind of states
kind_control=3;%kind of control
%+flag 1 : produce all kind of states and controlers that user want
to have!
for do_it=1:user_wanna%3 states
generalized_x1(do_it,:)=x;
end
for do_it=1:user_wanna%3 states
generalized_x2(do_it,:)=xx;
end
for do_it=1:user_wanna+1%4 states
generalized_x3(do_it,:)=xxx;
end
% for generalized_x4=1:user_wanna
% generalized_x4(do_it,:)=x;
% end
for do_it=1:kind_control
generalized_u(do_it,:)=u;
end
[i_s1,i_f]=first_end(generalized_x1(1,:),x_prepare(1));
[i_s2,i_f2]=first_end(generalized_x1(2,:),x_prepare(2));
[i_s3,i_f3]=first_end(generalized_x1(3,:),x_prepare(3));
[i_s4,i_f4]=first_end(generalized_x2(1,:),x_prepare(4));
[i_s5,i_f5]=first_end(generalized_x2(2,:),x_prepare(5));
[i_s6,i_f6]=first_end(generalized_x2(3,:),x_prepare(6));
[i_s7,i_f7]=first_end(generalized_x3(1,:),x_prepare(7));
[i_s8,i_f8]=first_end(generalized_x3(2,:),x_prepare(8));
[i_s9,i_f9]=first_end(generalized_x3(3,:),x_prepare(9));
[i_s10,i_f10]=first_end(generalized_x3(4,:),x_prepare(10));
%-flag 1
40. N=tf/dt;%final_stage
[nothing,j_f]=size(u);
j_f2=j_f;
j_f3=j_f2;
% [nothing,i_f]=size(x);
% i_f2=i_f;
% i_f3=i_f2;
% [nothing,i_f4]=size(xx);
% i_f5=i_f4;
% i_f6=i_f5;
% [nothing,i_f7]=size(xxx);
% i_f8=i_f7;
% i_f9=i_f8;
% i_f10=i_f9;
j_star(N+1,1:i_f)=0;
j_stak(1,1:j_f)=0;
for k=N:-1:1%stage counter
٣٧
k
for i10=i_s10:i_f10
for i9=i_s9:i_f9
for i8=i_s8:i_f8
for i7=i_s7:i_f7
for i6=i_s6:i_f6
for i5=i_s5:i_f5
for i4=i_s4:i_f4
for i3=i_s3:i_f3
for i2=i_s2:i_f2
% for i1=1:i_f1
for i=i_s1:i_f%state counter generalized_x1
for j=1:j_f
for j2=1:j_f2
for j3=1:j_f3%controller counter
x_next=state_eq(generalized_u(1,j),generalized_u(2,j2),generalized_u(
3,j3),generalized_x1,generalized_x2,generalized_x3,i,i2,i3,i4,i5,i6,i
7,i8,i9,i10,dt);
%new
[nothing,col2]=find(x==x_next);
[i_s1,i_f]=first_end(x,x_next(1));
[i_s2,i_f2]=first_end(x,x_next(2));
[i_s3,i_f3]=first_end(x,x_next(3));
[i_s4,i_f4]=first_end(xx,x_next(4));
[i_s5,i_f5]=first_end(xx,x_next(5));
[i_s6,i_f6]=first_end(xx,x_next(6));
[i_s7,i_f7]=first_end(xxx,x_next(7));
[i_s8,i_f8]=first_end(xxx,x_next(8));
[i_s9,i_f9]=first_end(xxx,x_next(9));
[i_s10,i_f10]=first_end(xxx,x_next(10));
%new
41. ٣٨
if ((1/10)*(abs(x_next-x_prepare)))<=.05%new check column
10=number of states
% [nothing,colmn]=find(x_next==x_prepare);%redo with
interpolate
% if (~isempty(colmn) && colmn==10)
if k==N
stage='last';
else
stage='nono';
end
if k==N%final stage only
j_c(k,i,j)=cost_function(u(j),u(j2),u(j3),stage,tf,dt);
%
j_c(k,i,j)=cost_function(u(j),u(j2),u(j3),generalized_x,generalized_x
2,generalized_x3,generalized_x4,stage,x_next,i,i2,i3,i4,i5,i6,i7,i8,i
9,i10);
else%final and this stage
j_c(k,i,j)=cost_function(u(j),u(j2),u(j3),stage,tf,dt)+min(j_stak(1,:
));%j_stak(1,find_next_j(x_next,x));
%
j_c(k,i,j)=cost_function(u(j),u(j2),u(j3),generalized_x(:,i),stage,x_
next)+j_stak(1,find_next_j(x_next,x));
end
else%else check colmn
j_c(k,i,j)=1e6;%for diffrence between 0 and not
acceptable amounts
end%if check colmn
end%j3
end%j2
end%j
end%i
% end%i1
end%i2
end%i3
end%i4
end%i5
end%i6
end%i7
end%i8
end%i9
end%i10
for i=1:i_f
[j_star(k,i),u_star_index(i)]=min(j_c(k,i,:));
%++A1 best use: find(j_c(k,i,:)==min(j_c(k,i,:)))
end
j_stak=j_star(k,:);
u_star(k,:)=u(u_star_index);
u_stak=u_star(k,:);
end%k
42. function [i_start,i_end]=first_end(x_length,current_x)
[nothing,col]=find(current_x==x_length);
[nothing,a]=size(x_length);
i_start=col-1;
i_end=col+1;
if i_start<1
٣٩
i_start=1;
end
if i_end>a
i_end=a;
end
end
function min_j_index=find_next_j(x_next,x)
% min_j_index=find_next_j(x_next,x)
%scheme of variable min_j: min_j(stage index,input index)
[nothing,min_j_index]=find(x==x_next);
end
function [c] = cost_function(u1,u2,u3,stage,tf,dt)
% [c] = cost_fcn(u,x,stage,x_last)
ro=.001;
if stage=='last'
c=tf;%(k) last stage cost function
else
c=ro*dt*(u1^2)+(u2^2)+(u3^2);%(1 until k-1) cost function in
stages)
end%if
end
function [c] =
cost_fcn(u1,u2,u3,gen_x1,gen_x2,gen_x3,gen_x4,stage,x_last,i1,i2,i3,i
4,i5,i6,i7,i8,i9,i10)
% [c] = cost_fcn(u,x,stage,x_last)
if stage=='last'
c=(x_last^2)+(2*u^2);%(k) last stage cost function
else
c=2*u^2;%(1 until k-1) cost function in stages
end%if
end
پايان كد برنامه 7 و زير برنامه ها.
