حفظ تعادل دینامیکی ربات بازودار دوچرخ دیفرانسیلی با کمبود عملگر ‏توسط چرخ عکس العملی

(1)

لماک یشهوژپ هلاقم رد خیرات ی تفا 5 / 01 / 10

ذپ خیرات ی شز 4 / 2 / 12

هئارا رد اس ی ت 01 / 4 /

12 12-91صص8 هرامش00هرود ،0012 نابآ

لداعت ظفح یکیمانید

خرچود رادوزاب تابر یلیسنارفید

دوبمک اب

سکع خرچ طسوت رگلمع یلمعلا

یمیرلا اضردیس ،1

یلعدیس نایوسوم ربکا

2

*

1 - ؼاٌقضاو سقضا هً٘اىه ٖؾسٌْه ،

ُاگكًاز

ِجاَذ ،ٖؾََ يٗسلاط٘هً

ىاطْت

2 - زاتؾا ،هً٘اىه ٖؾسٌْه ُاگكًاز

ِجاَذ ىاطْت ،ٖؾََ يٗسلاط٘هً

* ،ىاطْت قٍسٌن ٖتؿپ 43344 19991

، moosavian@kntu.ac.ir

هذیکچ - معً٘اىه هٗ ،ِلامه يٗا ضز سٗسج

ٕظاؾضاسٗاپ

ٕاطت خطچٍز تاتض

ٍظات ٕاضاز تاتض .تؾا ُسق ِئاضا

خطچٍز ٕاّ

هٗ ٕاضاز ِىًآ ل٘لز ِت ،

ضز لاؼفط٘غ لهفه خطچ ؼاوت لحه

ِٗاپ ٕاّ

زَذ نطحته ؼَىؼه لٍسًاپ هٗ سًٌاوّ

ٖه قات

ٌ ،س لزاؼت ظفح ٕاطت ضاسٗاپ ٍ لطتٌو ِت ظاً٘

ظاؾ ِت ٕ

تاتض ًَِگ يٗا لزاؼت لطتٌو ـٍض .سًضاز ٖى٘هاٌٗز تضَن ظا ُزافتؾا ات ،اّ

ِت توطح زضٍآ ضز ظا يتفطگ هوو ٍ تاتض ى يٗا ظا لناح ٖؾطٌٗا ضٍاتكگ

ٖه توطح تٗزٍسحه ـٍض يٗا .سقات

ٖٗاّ

ىَچ سٗسق ٖگتؿتاٍ

لزاؼت حُؾ ِت ي٘هظ

ّ ٍ اّضًَاه ماجًا ٖٗاًاَت مسػ يٌ٘چو ٕ

ٍظات ضز ىَىؾ تلاح

ِٗاپ اض زضاز ِو

ِتًَ ِت زَذ تحت اض ىآ زطىلوػ تلز ىاع٘ه ٍ نؿج ٖٗاجتاج ضز تاتض ٖٗاًاَت ط٘ثأت

ٖه ضاطل سّز . ضز معً٘اىه

ُسق ِئاضا تْج ،

ىزَوً فطَطت يٌ٘چ

ٖهٗامً

ؽىػ خطچ هٗ ظا ، يٗا ِت .تؾا ُسق ُزافتؾا ٖلوؼلا

تضَن ؽىػ ظا لزاؼت ظفح تْج ِو لاوػا ظا ٖقاً لوؼلا

ضٍاتكگ ضَتَه خطچ يٗا ِت

ُزافتؾا

ٖه زَق ات .

ٕط٘گضاىت ل٘لز ِت ،ـٍض يٗا

ِػَوجهطٗظ يٗا زطىلوػ للامتؾا اٗ توطح ظا

ِٗاپ ىَىؾ ي٘ح

،ٕظاؾضاسٗاپ ضز

ىاٍظات ٍ تاتض ِػَوجه زطىلوػ تاتض

ه٘

ُزطىً زاجٗا ٖللاذا ٍ

عً٘

حُؾ ِت اض لزاؼت ٖگتؿتاٍ

توطح فصح لهاو ضََ ِت

ُزطو تؾا .

لسه

ٖى٘هاٌٗز

ِػَوجه يٗا

ِ٘ثق تْج ٕظاؾ

زطىلوػ ، جاطرتؾا ٖل٘لحت لسه هٗ تضَن ِت

ِحن ٍ ٕضاصگ

ُسق تؾا ٖلطتٌو نتؿ٘ؾ .تؾا ُزاف

ُسق يٗا ٍٕض طت

ِػَوجه

ِ٘ثق فلتره ٕضاو تلااح ضز عً٘

ىآ جٗاتً ٍ ُسق ٕظاؾ زط٘گ ٖه ضاطل ثحت زضَه

.

ذیلک

‌ :ناگژاو ،تاتض

ٖل٘ؿًاطفٗز ،

لهفه ؽىػ خطچ ،لاؼفط٘غ

ٖلوؼلا .

Dynamic balancing of an under-actuated differential two wheeledmanipulatorby a reaction wheel

S.R. Larimi¹, S.A.A. Moosavian^2*

1-M.Sc., Dept. of Mech. Eng., K.N. Toosi Univ., Tehran, Iran.

2-Prof., Dept. of Mech. Eng., K.N. Toosi Univ., Tehran, Iran.

* P.O.B. 19991 43344 Tehran, moosavian@kntu.ac.ir

Abstract-In this article, a new stabilizing mechanism for a two wheel robot is proposed. Such systems, due to inherent instability, require dynamic stabilization. The conventional method for stabilizing these robots is moving the base back and forth, to use its inertia effects. Therefore, such strategies drastically depend on the ground surface, besides the robot is not able to reconfigure its manipulator to do any desired task. These limitations reduce the capability of the robot to manipulate objects, and to perform accurate tasks. In order to omit these restrictions, in the developed mechanism, a reaction wheel is used. The proposed mechanism exploits the inertia moment of reaction wheel to stabilize motion of the robot. Therefore, since there is no interaction between the reaction wheel and the ground surface, by using this mechanism there would be no concern about the surface that the robot moves on that. Also, manipulator of the robot can track the given trajectories, without considering stability limitations. In order to show the performance of proposed mechanism, a verified dynamics model of the robot is used and the control algorithm with various initial conditions is simulated.

Keywords: Two-Wheeled Robot, Non-Holonomic constraint, Under-Actuated Joint, Reaction Wheel.

[ Downloaded from mme.modares.ac.ir on 2022-10-31 ]

(2)

1 -

‌ همذقم

‌

تاتض ظا ٖؼ٘ؾٍ فقَپ ٍ ٖٗاجتاج ت٘لتال ل٘لز ِت نطحته ٕاّ

ُزطتؿگ ٕاّزطتضاو ظا ،ٕضاو ٕاًف ىاٌَػ ِت ٍ٘حه ضز ٕا

ٖگسًظ ٍ تؼٌن

ٖه ضازضَذطت سٌقات

ٖه ِو ًَِوً ىاٌَػ ِت ىاَت

تاتض ِت تاتض ،ٖگًاذ ٖتاهسذ ٕاّ

تؿق ٕاّ

ضز فو طگَقٍ

لسه ،تاجًاذضاو ُضاقا اّطگقٍاو ٍ ُسًطپ ٕاّ

زطو . ظا ًَِگ يٗا

تاتض ٖٗاجتاج لوػ ِو ُزَت نطحته ِٗاپ فرت هٗ ٕاضاز ،اّ

ٖه ُسْػطت اض تاتض تاتض ظا ٖكرت ي٘ت يٗا ضز .سًط٘گ

ٕاّ

نطحته

ٕاطت ِت ،ىاكتوطح ي٘ح ٖولااچ ٍ تػطؾ فٗاعفا

َٕحً

ٖه ٖحاطَ

تضَن ِت ىاكلزاؼت ظفح ٍ ٕضاسٗاپ ِو سًَق

ٍذطت ت ٖى٘هاٌٗز ٍ أ

زَق ي٘ه 1]

ٖٗاًاَت تاتض ضاو يٗا ات .[

زاؼت ظفح ضز ٕطتْت زَذ ل

ضز زَجَه تلاىكه ظا ٍ ُزطو اس٘پ

سًاه سّاَذ ضٍز ٖى٘تاتؾا لزاؼت 2]

تاتض ظا .[ ٕضاسٗاپ ات ٖٗاّ

ٖه ،ٖى٘هاٌٗز تاتض ِت ىاَت

تاتض ،اوً ىاؿًا ،ُسًطپ ٕاّ

تاتض ٍ خطچٍز ٕاّ

ُط٘غ زطت ماً

3]

، 4 .[ تاتض ًَِگ يٗا ِچطگا ،اّ

ىاكً زَذ ظا ىاكت٘لاؼف ماجًا ي٘ح ٖهٍامه ضا٘ؿت ضاتفض

ٖه ىاكلزاؼت ٖتحاض ِت سٌتف٘ت ضاو ظا ِىٌٗا ىحه ِت ٖلٍ ،سٌّز

ٖه ىَگغاٍ ٍ ُزضَذ نّ طت ٕاطت ًلاَنا تلػ ي٘وّ ِت .سًَق

ِ٘ىت ،ىاكًَ٘گغاٍ ظا ٕط٘گَلج

ُاگ ٖٗاّ

يٗا ظا ٕط٘گَلج ٕاطت

ٍٕض طت ُسٗسپ

ًآ

ٖه ِ٘ثؼت اْ

زَق 5] ىاوّ .[ ىا٘ت ِو ضََ

سق ،

ت ِو تؾا ِتؾز يٗا ظا ًَِوً هٗ عً٘ خطچ ٍز تاتض ظفح ِ

لزاؼت ٖى٘هاٌٗز زضاز ظاً٘

فٗاضآ ل٘لز ِت خطچ ٍز تاتض .

خطچ نطحته فرت ٕاّ

لاؼف ط٘غ لهفه هٗ ٕاضاز ،ىآ ظاؾ

تحت اض تاتض لزاؼت ًاهاوت ِو ُزَت ط٘ثأت

ٖه ضاطل سّز 6] ضز .[

نتؿ٘ؾ ظا ،ٖلنافه يٌ٘چ لطتٌو تْج ،لاؼفط٘غ لهفه ات ٖٗاّ

ظا ُزافتؾا ٍ اًْآ ٍٕض طت زَجَه ٕاّطگلوػ ٍ لنافه ٌاثتضا ُزافتؾا ءاعجا نّ ِت ِتؿتاٍ ه٘هاٌٗز

ٖه زَق 7] - 9 تاتض ضز .[

عً٘ خطچ ٍز سٗات

اض لاؼفط٘غ لهفه ،ٖلوػ يٌ٘چ ظا ُزافتؾا ات

لطتٌو طو ظفح اض تاتض لو لزاؼت ِج٘تً ضز ٍ ُز زطو

ضز ًلاَنا .

خطچ لطتٌو ظا ُزافتؾا ات خطچ ٍز تاتض لامتًا ٍ نطحته ٕاّ

ؽىػ ظا يتفاٗ ُطْت ٍ ةمػ ٍ َلج توؾ ِت تاتض ظا لناح لوؼلا

لاؼفط٘غ لهفه ن٘ظٌت ٍ لطتٌو ِت ،تاتض ٍٕض طت ٖتوطح يٌ٘چ

ٖه ه ،ـٍض يٗا ضز .سًظازطپ ىاى

ُضاَوّ تاتض يٗا ِت ِتؿتاٍ

ِت تثؿً ٕطتك٘ت ت٘وّا ،تاتض لزاؼت ظفح ٍ ُزَت لهفه لاح يٗا ىاهعوّ لطتٌو ٍ تقاز سّاَذ تاتض لو توطح ت

اّ

ُاطوّ ِت لزاؼت ظفح

، تػطؾ لاات لوػ تاتض لو ٖولااچ ٍ

، زَت سّاَذ ضاَقز ضا٘ؿت 10]

. [ تاتض نْه ٕاّزطتضاو ظا ٖىٗ

ٕاّ

ه اًْآ ٍؾَت ماؿجا ٖٗاجتاج ،نطحته

ٖ لوػ يٗا ِو سقات

ُػٍٗ ٖگتؿتاٍ

زضاز تاتض ٖٗاًْ ىاىه ِت ٕا 11]

ىاوّ اها .[ ضََ

ٍ لاؼفط٘غ لهفه يتقاز تلػ ِت ،ضَتعه تاتض ،تكگ ىا٘ت ِو خطچ ظا ُزافتؾا زَذ نطحته ٕاّ

ٕاطت چّ٘ ِت ،لزاؼت ظفح

ٍ ِتقاسً اض لمتؿه ضََ ِت ٖٗاًْ ىاىه لطتٌو ىاىها ىاٌَػ لاؼفط٘غ لهفه ت٘ؼلَه ٍ لزاؼت ِت ٖٗاجتاج ٖگتؿتاٍ ُضاَوّ

زضاز زَجٍ ىآ ضز 12]

، 13 ي٘وّ .[ خطچ ظا ُزافتؾا ،ضََ

ٕاّ

نطحته فرت لزاؼت ٖگتؿتاٍ ،لزاؼت ظفح ٕاطت ،تاتض ظاؾ

ىآ حُؾ عًَ ٍ ٍ٘حه ِت تاتض

ٖه زَجٍ ِت اض زضٍآ

ىاممحه ِو

ضازاٍ اض

ُزطو فّاو تْج يٗا

ت٘ؼُل مسػ ،ٖگتؿتاٍ

طّ ٕاّ

چ ع٘چاً سٌ

،

ٕضاتذاؾ ٕاّطتهاضاپ ضز ِچ

ٕضاتذاؾط٘غ ِچ ٍ ،

اض

لسه ضز زَذ ٖلطتٌو ٕاّ

لاوػا سٌٌو ٖگتؿتاٍ يٗا اها .

تٗزٍسحه ٖٗاًاَت مسػ ىَچوّ ٖٗاّ

ح ضز تاتض لزاؼت ظف

ناىُنا ات ٖحَُؾ

ٖه زَجٍ ِت اض ي٘ٗاپ تاتض ٖٗآضاو ِو زضٍآ

ٖه فّاو ن٘متؿه ضََ ِت اض يٗا .سّز

ِلأؿه خطچ ٍز تاتض ضز

تاتض ِت تثؿً زضاز لزاؼت مركه تلاح هٗ ضز اٌْت ِو ٕاّ

ٖه ضاسٗاپ ُزٍسحه هٗ ضز ِو اوً ىاؿًا اض ىاكلزاؼت ٍ سٌقات

ٖه سٌٌو ظفح سًٌاَت

، گًضطپ ضا٘ؿت طت

تؾا 14] .[

ٕاطت ظفح

ًَِگ ِت ٖتؿٗات خطچ ٍز تاتض ،لزاؼت ضاطل ٕا

ضا٘ؼه كثَ ِو زط٘گت

ٕضٍاتكگ طفن ُِمً

ظا لناح ضٍاتكگ ،1

طفن طتاطت ٖجضاذ ٍ ٖؾطٌٗا ،ٖلمث ٕاٍّطً٘ سٌٗآطت زَق

15] .[

لاح يٗا ِت ىس٘ؾض ٕاطت ت

اّ

ٖه ، تاتض لمث عوطه لحه ٖتؿٗات

زَوً ِثؾاحه ٍذ طت تضَن ِت اض

ُ ٍ ِٗاپ توطح ِت ِجَت ات ٍ

ٖجضاذ ٕاٍّطً٘ ظا لناح ضٍاتكگ يٌ٘چوّ

اس٘پ اض ىآ ، زطو

.

تاتض ًَِگ يٗا تلازاؼه ٖگس٘چ٘پ تلػ ِت ٍ اّ

ٕاطت

ُزاؾ لاات ٕزاظآ تاجضز ات ضازٍظات خطچ ٍز تاتض ،تلازاؼه ٕظاؾ ،

ُزاؾ نطحته ًِاگٍز ؼَىؼه لٍسًاپ هٗ ِت ٕظاؾ

تؾا ُسق .

فٍّػپ كثَ ضاو يٗا

ّ

ُزاؾ تْج ُسهآ لوػ ِت ٕا ٍ ٕظاؾ

سهآضاو ضا٘ؿت ٖتاثؾاحه تا٘لوػ طتك٘ت تػطؾ

ٖه سقات 16]

، 17 .[

لسه ِت تثؿً ِتفاٗ امتضا معً٘اىه هٗ ِئاضا ات ،ِلامه يٗا ضز تاجضز ِت ٕزاظآ ِجضز هٗ فٗاعفا ٍ مًََلَّط٘غ خطچ ٍز تاتض لهفه لطتٌو ٍ ظفح تْج ًافطن ىآ ظا ُزافتؾا ٍ تاتض ٕزاظآ ٖؼؾ ،لاؼفط٘غ

ُسق تلاىكه قَف

،

َق فصح

ً س ات ،لسه يٗا ضز .

1. ZMP

(3)

ؽىػ خطچ هٗ ظا ُزافتؾا ةهً ٍ ٖلوؼلا

ٖلنا ًِست ٍٕض طت ىآ

ٍ ىآ ٍٕض طت ضٍاتكگ لاوػا ٍ تاتض ظا ٕط٘گضاىت

ط٘ثأت ٕطٗصپ

ؽىػ ظا لاؼفط٘غ لهفه ؽىػ خطچ ظا ٖقاً لوؼلا

ِت ،ٖلوؼلا

ٖه ِتذازطپ ىآ لزاؼت ظفح ٍ لاؼفط٘غ لهفه لطتٌو ضز .زَق

اسج ل٘لز ِت ـٍض يٗا ىزطو

نطحته ِٗاپ توطح لطتٌو فرت

آ ٕضاسٗاپ ي٘هات ظا تاتض لهفه زَجٍ ظا لناح ٖگس٘چ٘پ ،ى

ِت ِتؿتاٍ ٍ ُس٘چ٘پ ضاتذاؾ ضز ه٘هًََلَّط٘غ س٘ل ٍ لاؼفط٘غ ىت ٕاّطلطتٌو ضز تاتض ٖى٘هاٌٗز نّ

ِتفضضا فّاو ضا٘ؿت

ٖه ستاٗ

18] ٖگتؿتاٍ ،ٖلوؼلا ؽىػ خطچ ظا ُزافتؾا يٌ٘چوّ .[

ٍ٘حه حُؾ ِت تثؿً تاتض لزاؼت اض

فصح لهاو ضََ ِت طو

ُز

ضزال تاتض ٍ تؾا

عً٘ ي٘ٗاپ ضا٘ؿت ناىُنا ات ٖحَُؾ ضز ٖتح

ظفح اض زَذ لزاؼت سٌو

.

هوو ِت ،ِتفاٗ امتضا ضاتذاؾ ظا ٖجاطرتؾا ٖى٘هاٌٗز لسه ت ػًاطگلا تلازاؼه ِ

ِحن ٍ ُسهآ تؾز ظا ُزافتؾا ات ىآ ٕضاصگ

مطً

ضاعفا آ زا عه

ُسق ماجًا ظا ُزافتؾا ات ُسق ىا٘ت لسه .تؾا

فلتره تلااح ضز ،ُسق ِئاضا معً٘اىه ُسق ٖؾضطت ٕضاو ٍ ِ٘لٍا

جٗاتً ٍ

ِ٘ثق فرت ضز اًْآ يٗا ماوت ضز .تؾا ِتكگ ىا٘ت ٕظاؾ

لاح ت اّ

خطچ ِت ظاً٘ ىٍست ِتؿًاَت ٖتَذ ِت تاتض ، ِٗاپ ٕاّ

لو ىزَوً اجتاج ،ىآ غثَ ِت ٍ نطحته

ِػَوجه ٍ َلج ِت

ةمػ ؽىػ ظا ُزافتؾا ات اٌْت ِىلت ، ِت ٖلاوػا ضٍاتكگ لوؼلا

ؽىػ خطچ لزاؼت ٍ ٕضاسٗاپ ،ٖلوؼلا

زَذ ظفح اض طو

ظا ٍ ُز

ًَٖگغاٍ

ِػَوجه ٕط٘گَلج

ٌو س .

2 -

‌ لذم هحص‌و‌رادوساب‌خزچود‌تابر‌یساس

‌

‌یراذگ

نآ

‌ لسه ِت فرت يٗا ضز ُاطوّ ِت خطچٍز ضازٍظات تاتض هٗ ٕظاؾ

ؽىػ خطچ معً٘اىه تذازطپ ىآ ٕضاسٗاپ لطتٌو تْج ٖلوؼلا

ِ

تؾا ُسق لىق كثَ ،ضَوصه تاتض ِت ِجَت ات .

1 ِٗاپ فرت ،

ٖه خطچ ٍز ٕاضاز تاتض يٗا نطحته ِت اعجه تضَن ِت ِو سقات

ي٘هات ضَتَه ٍز يٗا ظا اض زَذ توطح ٍ سٌتؿّ لهته ضَتَه ٍز

ٖه ِت ثػات ِو تاتض ِٗاپ ضز زَجَه ضاتذاؾ ِت ِجَت ات .سٌٌو

ٖه لاؼفط٘غ لهفه هٗ ىسهآ زَجٍ

لزاؼت ىآ ِج٘تً ضز ٍ زَق

تحت اض تاتض ط٘ثأت

ٖه ضاطل تضَن ِت سٗات ىآ ٕضاسٗاپ ،سّز

ت ٖى٘هاٌٗز أ

ي٘ه زَق خطچ ىاهس٘چ ضاتذاؾ يٌ٘چوّ . س٘ل ،اّ

ٖه ُاطوّ ِت تاتض ٕاطت عً٘ اض ه٘هًََلَّط٘غ هٗ زَجٍ .زضٍآ

ت ،نطحته فرت ٍٕض طت لهته ٖىٌ٘ل ِؾ ٍٕظات ُظاجا ىآ ِ

ٖه ال ات سّز .سقات ِتقاز اض نؿج ٖٗاجتاج ت٘لت لسه يٗا ضز اها

ٖه ،نطحته ِٗاپ فرت ضاتذاؾ ِت ِجَت ات ،ٍظات ٖلنا ًِست ىاَت

ٍظات ،تضَن يٗا ضز ِو زطو وطف ٍظات لٍا هٌ٘ل ىاٌَػ ِت عً٘ اض

ٖه طگلوػ ِؾ ُاطوّ ِت ٕزاظآ ِجضز ضاْچ ات هٌ٘ل ِؾ ٕاضاز -

اض تاتض ٍ سقات

ٖه لٗسثت طگلوػ زَثوو ات معً٘اىه هٗ ِت .سٌو

ؽىػ خطچ هٗ ظا ُزافتؾا طَاذ ي٘وّ ِت هٗ ىزٍعفا ٍ ٖلوؼلا

لهفه لطتٌو ٕاطت ًافطن ،تاتض يٗا ٍٕض طتك٘ت ٕزاظآ ِجضز فٗاعفا نّ ٍ تاتض ٕضاسٗاپ نّ ِو ُزَت ٍظات لٍا لاؼفط٘غ

ٖه ُاطوّ ِت اض تاتض ٖٗاجتاج ت٘لتال متؿه خطچ يٗا .زضٍآ

ًاو٘

ٍٕظات هٌ٘ل ي٘لٍا ىاوّ اٗ ٖلنا ًِست ِت طگلوػ هٗ ٍؾَت

ٖه لهته تاتض خطچ ٍز تاتض لسه ضز زَجَه ٕاّطتهاضاپ .سقات

لىق ضز 2 لٍسج ضز ٍ ُزاز فٗاوً

1 .تؾا ُسق فٗطؼت ،

‌

ش لک

‌ 1 خطچ ُاطوّ ِت ضازٍظات خطچ ٍز تاتض ٕسؼت ِؾ حطَ

ؽىػ

ٖلوؼلا

‌ ‌

لکش 2 خطچ ٍز تاتض ٖلو حطَ

ؽىػ خطچ ُاطوّ ِت ضازٍظات

ٖلوؼلا

(4)

لوذج

‌ 1 تاتض ضاتذاؾ ضز ِتفض ضاىت ٕاّطتهاضاپ

تاح٘يَت اّطتهاضاپ

غجطه ٖؾطٌٗا ُاگتؾز O_XYZ

ًِست ِت لهته ُاگتؾز o1 _x1y1z1

خطچ ىاضٍز ٍِٗاظ ىاع٘ه q_w1 اّ

q_w2ٍ

ٖلنا ًِست فاطحًا ٍِٗاظ ىاع٘ه q1

ِت تثؿً تاتض ٕاٍّظات ٍِٗاظ ىاع٘ه طگٗسىٗ

q_i(2,3,4)

ٖلوؼلا ؽىػ خطچ ىاضٍز ىاع٘ه q_r

خطچ عاؼق

ِٗاپ ٕاّ

r

خطچٍز ي٘ت ِلناف l

هٌ٘ل لََ

ٍظات ٕاّ

l_i(1,2,3)

2 - 1 -

‌ لذم تابر‌یوساب‌یساس

‌

ىاوّ

ِو ضََ

سق ُضاقا اض ىآ ٍٕظات ،تاتض ضاتذاؾ ِت ِجَت ات

ٖه طظً ضز طگلوػ ِؾ ٍ ٕزاظآ ِجضز ضاْچ هٌ٘ل ِؾ ات ىاَت

:تقاز نّ٘اَذ ن٘متؿه ه٘تاوٌ٘ؾ ٕاطت تضَن يٗا ضز .تفطگ ( 1 ( ) )

ِو ات يٌ٘چوّ .تؾا ٖٗاًف يٗعتضاو ت٘ؼلَه ضازطت ،

هٌ٘ل ماوت باتق ٍ تػطؾ ٕاطت ضازطت يٗا كتكه ظا ُزافتؾا ٕاّ

:تقاز نّ٘اَذ ٍظات ( 2 ̇ ( ) ̇ )

( 3 ̈ ( ) ̈ ̇( ) ̇ )

اًْآ ضز ِو ,

ؽٗطتاه

ٖه ي٘تَواغ ٍ سقات

ٍ m

ٕزاظآ تاجضز ة٘تطت ِت n ٕضاو ٕاًف ٕاّسؼت زاسؼت ٍ

ٖه قات

ٌ .س

ٕاطت

ٖه ،تاتض ٖى٘هاٌٗز لسه جاطرتؾا ػًاطگلا ـٍض ظا ىاَت

ُزافتؾا طو :نٗضاز تضَن يٗا ضز .ز

( 4 )

ىآ ضز ِو ٍ ػًاطگلا غتات L

ٍ T ٕغطًا غتاَت ة٘تطت ِت V

ٖه ل٘ؿًاتپ ٍ ٖكثٌج لَهطف ِت ِجَت ات .سٌقات

( 5

ٖه ) ىاَت

( ُِتاض تضَن ِت اض تاتض ٖى٘هاٌٗز تلازاؼه 6

:زضٍآ تؾست )

( 5 )

̇

( 6 ( ) ̈ ( ̇) ̇ ( ) )

ضازطت ٕاّطتهاضاپ ىآ ضز ِو

ٕ :

ؽٗطتاه يٌ٘چوّ ٍ سٌتؿّ

ٕاّ

G ,H ,M ة٘تطت ِت

ؽٗطتاه

ٖؾطٌٗا :ٕاّ

لاغَ٘فٗطتًاؾ ٍ ؽ٘لَٗطو ٕاٍّطً٘ ،1

ٍ2

ِتشاج ؽٗطتاه

ٖه3

.سٌقات

س٘ل ،لاات ٕزاظآ تاجضز زَجٍ ل٘لز ِت ،ضازٍظات خطچ ٍز تاتض ٕاضاز ،ىآ ضز طگلوػ زَثوو ت٘ناذ يٌ٘چوّ ٍ ه٘هًََلَّط٘غ

ُس٘چ٘پ تلازاؼه

ٖه ٕا سقات 18] ـٍض هٗ ،تلػ ي٘وّ ِت .[

ُزاؾ ٕاطت سهآضاو ُسق ُزاؾ لسه ظا ُزافتؾا تلازاؼه ٕظاؾ

ًِاگٍز ؼَىؼه لٍسًاپ زَت سّاَذ4

19] - 21 ات تضَن يٗا ضز .[

لىق ِت ِجَت 3

لٍسج ٍ 2

ٖه ، وطف ىاَت طو

مٍز هٌ٘ل ِو ز

طذآ هٌ٘ل ِؾ لزاؼه ،ًِاگٍز ؼَىؼه لٍسًاپ لسه ضز زَجَه زَجَه ٍٕظات .سقات تاتض ٍٕض طت

لکش 3 ؼَىؼه لٍسًاپ ِت ضازٍظات خطچ ٍز تاتض ُسق ُزاؾ لسه

ًِاگٍز لوذج 2

ُسق ُزاؾ لسه ضز ِتفض ضاىت ٕاّطتهاضاپ

تاح٘يَت اّطتهاضاپ

خطچ ىاضٍز ٍِٗاظ ىاع٘ه

ٖلنا ًِست فاطحًا ٍِٗاظ ىاع٘ه

ٖلوؼلا ؽىػ خطچ ىاضٍز ىاع٘ه ٍِٗاظ ىاع٘ه ؼَىؼه لٍسًاپ مٍز ٕظاجه هٌ٘ل فاطحًا

ٕظاجه ؼَىؼه لٍسًاپ لٍا ٍٕظات لََ

ٕظاجه ؼَىؼه لٍسًاپ مٍز ٍٕظات لََ

مٍز ٍٕظات ٕاطت لزاؼه مطج خطچ ضَحه ات ٖلنا ًِست مطج عوطه اّ

يٗعتضاو تاهتره ِثؾاحه ٕاطت

ٍ لََ يٌ٘چوّ ٍ

( ( ٍِٗاظ ،) ( مطج ٍ )

نٗضاز لزاؼه مٍز هٌ٘ل ) 22]

:[

1. The Inertia Matrix

2. The Centrifugal and Coriolis Force Matrix 3. The Gravity Matrix

4. Double Inverted Pendulum Model

(5)

( 7

∑ )

∑

( 8

∑ )

∑

( 9 √( ) ( ) )

( 10 ( )

)

( 11 ∑ )

ِثؾاحه ات طلٍٗا غتات

- ( ػًاطگلا مطت ظا ،)L

ٍ ٖكثٌج ٕغطًا ٕاّ

ٕضازطت تضَن ِت عً٘ تاتض ٖتوطح تلازاؼه ،نتؿ٘ؾ ل٘ؿًاتپ لىق ِت ( ) ̈ ( ̇ ) ( ) ،

ٕضازطت ٕاّطتهاضاپ ىآ ضز ِو زَت سٌّاَذ :

طوش ِت مظلا .سٌتؿّ

تلازاؼه يٗا ضز ،تؾا ٔجضز ىاٌَػ ِت ،

ٖه زَجَه لاؼفط٘غ ٕزاظآ ىآ ٔتًَ ِت ِو سقات

طتاطت ُضاَوّ ̂

ٖه طفن سقات

ِٗاضز يٌ٘چوّ ، ؽٗطتاه ٕاّ

ٕاّ

تضاثػ ة٘تطت ِت

:ظا سًا

( )

( ( ( )) ) ( )

( )

̇ ( ( ̇ ( ) ( ( )) ̇ ( )) ̇ ( )

̇ ( )) ̇ ( ̇ ( ) ̇ ( ) ( )) ̇ ( ̇ ( ) ( ) ̇ ( )) ̇ ( ̇ ( ) ( ) ̇ ( ) ( ( )))

̇

̇ ̇ ( ) ( ) ̇ ( ) ( ( )) ̇

( ) ( ) ( )

( ) 2

- 2 -

‌ هحص یجازختسا‌یکیمانید‌لذم‌یراذگ

‌

يٗا ل٘لز ِت لٍسًاپ ظا ِتفاٗ امتضا لسه هٗ ُسق ىا٘ت لسه ِو

ٖه ًِاگٍز ؼَىؼه سقات

، تلازاؼه ٍ ُزَت تٍافته ىآ ٖى٘هاٌٗز

تلازاؼه يٗا ٖتؿٗات

ِحن ٍ ٖؾضطت زضَه ط٘گ ضاطل ٕضاصگ

ً .س اصل

ق هوو ِت

ِ٘ث مطً ات ٕظاؾ ضاعفا

عهازآ ضز تاتض ٖى٘هاٌٗز ضاتفض

ؽىػ خطچ ضًَح ،ٖلوؼلا

ٖه ضاطل ِؿٗامه ٍ ٖؾضطت زضَه زط٘گ

.

مطً

ضاعفا عهازآ مطً هٗ

ل٘لحت ٕاطت طثتؼه ضاعفا

ٖى٘هاٌٗز ٕاّ

نتؿ٘ؾ

ٖه اّ

خطچ توطح طتاطت ضز نتؿ٘ؾ ضاتفض طگا اصل .سقات

ؽىػ ُسق ِتقًَ تلازاؼه ات ،ىآ ِت ُسق زضاٍ ضٍاتكگ ٍ ٖلوؼلا

مطً يٗا ل٘لحت ٍؾَت نتؿ٘ؾ ضاتفض ِت تثؿً لثل فرت ضز ضاعفا

ٖه ،سقات ٖىٗ

ضز ٍ سق طثذ ات تلازاؼه يٗا تحن ظا ىاَت

فرت

ٗز لسه ىاٌَػ ِت ىآ ظا ٕسؼت ٕاّ

تاتض ح٘حن ٖى٘هاٌ

ُزافتؾا ،ضَوصه طو

.ز

ًِاگٍز ؼَىؼه لٍسًاپ تاتض ظا ُسق ٖحاطَ ٕسؼت ِؾ لسه ؽىػ خطچ ات ءاعجا ٕاّطتهاضاپ ٕزسػ طٗزامه يٌ٘چوّ ٍ ٖلوؼلا

لىق ضز ة٘تطت ِت ىآ 4

لٍسج ٍ 3 .تؾا ُسق ُزضٍآ

ِ٘لٍا ٍٗاطق ضز ،ٕزسػ ٍ ٖل٘لحت لسه ٍز يٗا ىزاز ضاطل ات يٗسٌچ ،ىاؿىٗ

طوش ِت مظلا .سق سّاَذ ٖؾضطت ،طٗظ تلاح

مطً ظا ٕزسػ جٗاتً ٍ تلازاؼه ظا ٖل٘لحت جٗاتً ،تؾا عهازآ ضاعفا

.تؾا ُسق جاطرتؾا

‌

لکش

‌ 4

ِ٘ثق لسه خطچ ات ًِاگٍز ؼَىؼه لٍسًاپ ظا ُسق ٕظاؾ

ؽىػ مطً ضز ٖلوؼلا ضاعفا

عهازآ

x y z

(6)

ؽىػ خطچ ِت ،تلاح ي٘لٍا ضز ،ٖلوؼلا

تضَن ِت ٕضٍاتكگ

ٖه زضاٍ ،سحاٍ ِتطي ِت تثؿً ٖلنا ًِست ٍِٗاظ تاط٘٘غت .زَق

( نئال ضَحه ؽىػ خطچ ىاضٍز ىاع٘ه ٍ )

( ،ٖلوؼلا ٍ )

لىق ضز اض ىآ ٕاُذ سنضز ىاع٘ه يٌ٘چوّ

ٕاّ

5 ات 7 ُزضٍآ

.تؾا ُسق

‌

لوذج

‌ 3 ٍ ٖىٗع٘ف ٕاّطتهاضاپ ٕزسػ طٗزامه

ضز اًْآ ٕظاجه لسه

ؽىػ خطچ ات ًِاگٍز ؼَىؼه لٍسًاپ

ِ٘ثق ضز ٖلوؼلا

ٕظاؾ

ٕزسػ طٗزامه اّطتهاضاپ تاح٘يَت

5 (m)

0/ لٍا هٌ٘ل لََ

25 (m) /

0 لٍا هٌ٘ل لمث عوطه لحه

5 (m) /

0 مٍز هٌ٘ل لََ

(

∙s m/rad

∙ ) N 2 /

0 qe لهفه ضز زَجَه ناىُنا ةٗطي

s( m/rad∙ N∙ 2) /

0 q2 لهفه ضز زَجَه ناىُنا ةٗطي

(kg) 3 /

0 اّخطچ مطج

(kg) 45 /

0 لٍا هٌ٘ل مطج

(kg) 6 /

1 ٖلوؼلاؽىػ خطچ مطج

(kg) 3 /

0 مٍز هٌ٘ل لزاؼه مطج

( m² kg∙ 012) /

0 z1 ضَحه ِت تثؿً لٍا هٌ٘ل ٖؾطٌٗا ىاوه (

m²

∙ ) kg 074 /

0 ِت تثؿً ٖلوؼلاؽىػ خطچ ٖؾطٌٗا ىاوه

ضَحه z1

لکش

‌ 5 تاتض ٖلنا ًِست ٍِٗاظ تاط٘٘غت ضاسمه

‌

لکش

‌ 6 ؽىػ خطچ ىاضٍز ىاع٘ه تاتض ٖلوؼلا

‌

لکش

‌ 7 سنضز ٕاطت ٕزسػ ٍ ٖل٘لحت جٗاتً ٍز ظا لناح ٕاُذ

تاتض ٖلنا ًِست ٍِٗاظ تاط٘٘غت ىاع٘ه ؽىػ خطچ ِت ،لاح

ٖه زضاٍ ط٘غته ضٍاتكگ هٗ ٖلوؼلا زَق

.

ؽىػ خطچ ِت ٖلاوػا ضٍاتكگ طٗزامه ٌٖحٌه تلاح يٗا ضز -

تاط٘٘غت ىاع٘ه يٌ٘چوّ ٍ ٖلوؼلا

ٍِٗاظ ِت تثؿً ٖلنا ًِست

تؾا ُسق ُزاز فٗاوً طٗظ ضز ة٘تطت ِت نئال ضَحه ىق(

ل ٕاّ

8 ات 10 ).

‌

لکش

‌ 8 ؽىػ خطچ ِت ُسق زضاٍ ضٍاتكگ

ٖلوؼلا

‌

لکش

‌ 9 تاتض ٖلنا ًِست ٍِٗاظ تاط٘٘غت ىاع٘ه

0 5 10 15 20

-350 -300 -250 -200 -150 -100 -50 0

0 5 10 15 20

-100 -50 0 50 100 150

0 5 10 15 20

-0.03 -0.02 -0.01 0 0.01 0.02 0.03

0 1 2 3 4 5

-0.5 0 0.5

0 1 2 3 4 5

-1200 -1000 -800 -600 -400 -200 0

ٕزسػ جٗاتً

ٖل٘لحت جٗاتً

ىاهظ (s)

ٍِٗاظ (deg)

اُذ (%)ضٍاتكگ (N.m)

ىاهظ (s)

ٍِٗاظ (deg)

ٕزسػ جٗاتً

ىاهظ (s)

ٕزسػ جٗاتً

ٍِٗاظ (deg)

ىاهظ (s)

(7)

‌

لکش

‌ 11 ىاع٘ه ٕاطت ٕزسػ ٍ ٖل٘لحت جٗاتً ظا لناح ٕاُذ

تاتض ٖلنا ًِست ٍِٗاظ تاط٘٘غت ىاوّ

ِ٘ثق ٕزسػ طٗزامه ،ُسق ُسّاكه ِو ضََ

ات ٕظاؾ

مطً ظا ُزافتؾا ضاعفا

عهازآ تلازاؼه ٍؾَت ٖجاطرتؾا طٗزامه ات

ضاىت ِتاكه نّ ات ٖلَثل لتال سح ات ،ُسق ِتفطگ

ٖه اصل .سٌقات

فرت ضز ُسق ِتقًَ تلازاؼه ظا ُزافتؾا ىاىها ِ٘ىت ات ٖتآ ٕاّ

ِ٘ثق يٗا طت

ٖه تضَن ٕظاؾ .زطٗصپ

ِ٘ثق ضز

ٕظاؾ ًافطن ،فسّ ِىٌٗا ل٘لز ِت ،ُسهآ لوػ ِت ٕاّ

ٖجاطرتؾا ٖى٘هاٌٗز لسه ٖؾضطت ٕاطت

ىاع٘ه ٍ طظً زضَه تاتض

ٖجٍطذ كتاُت لسه ٍز ٕاّ

ل ،ُزَت نّ ات ٕظاؾ ظا ٖتٗزٍسحه اص

لٍسًاپ ،تلػ ي٘وّ ِت .تؾا ُسكً لاوػا ي٘هظ َٕؾ

ٖه اّ

-

ًاَت

ٍ سٌقات ِتقاز لهاو ىاضٍز سٌ

ىاوّ

ٌٖحٌه ظا ِو ضََ

اّ

طت ىاؾًَ ٕضاسمه ظا ؽپ لٍسًاپ لٍا تلاح ضز ،تؾا مركه ل٘لز ِت مٍز تلاح ضز ٍ ِتفطگ ضاطل ـضاسٗاپ لزاؼت ُِمً ٍٕض ؽىػ خطچ ِت ضٍاتكگ لاوػا ؼلا

لَح ضٍز ِؾ زٍسح تاتض ،ٖلو

ِ٘ىت ُِمً

ٖه فّاگ .سًظ

ِ٘ثق ٍز ظا لناح ٕاُذ ىاع٘ه لَهذ ضز ىاٗاق عً٘ ٕظاؾ

ِو تؾا طوش

ٌٖحٌه ِت ِجَت ات لىق ٕاّ

7 لىق ٍ 10 يٗا ،

ٖه طظً ِت ضََ

ٍثتطه ٖلنا ًِست ٍِٗاظ ىاع٘ه ِت اُذ ِو سؾض

ٖه سّاَذ ٕزٍسحه ضاسمه ،نو طٗزامه ضز ٍ سقات تقاز

ىَچ ٍ

ِ٘ثق ضز

ٕظاؾ

ُظات ضز اٗاٍظ يٗا ،ٖلنا ٕاّ

ٕ 90 ات ِجضز + 90

-

ُزٍسحه ضز عً٘ لسه ظا لناح ٕاُذ اصل ،سًضاز ضاطل ِجضز ٕ

.تفطگ سّاَذ ضاطل ٖهركه 2 - 3 -

‌ لذم

‌رد‌نآ‌تیعقوم‌یسرزب‌و‌کزحتم‌هیاپ‌یساس

تکزح‌هحفص

‌

ضزال تاتض ،نطحته ِٗاپ ضز ه٘هًََلَّط٘غ س٘ل زَجٍ ل٘لز ِت ضَحه تْج ضز ط٘ؿه ىزَو٘پ ِت

ٖوً

لىق( سقات 11

.) غجطه هٗ ِت تثؿً تاتض ٖٗاًْ ىاىه ِثؾاحه تْج يٌ٘چوّ

طّ ضز ًٖاىه ٕاّطتهاضاپ تاط٘٘غت غوج ِت ظاً٘ اٗ ُضاَوّ ،تتاث ظا ُزافتؾا ات اٗ ٍ ِتقاز ٍذطت تضَن ِت ٍ ِظحل

ِثؾاحه ٕاّطگ

ٍ طَٗهت ـظازطپ ىَچوّ ٖجضاذ ُط٘غ

نٗضاز 23] - 24 .[ ضز

لاطگتًا ـٍض اض تاتض ت٘ؼلَه سٗات ،ٍذ طت تضَن ِت ٕط٘گ

ضز

ِظحل طّ

ىآ تاط٘٘غت ٍ ُزَوً ِثؾاحه

، لاوػا ٍ باؿح ُضاَوّ

زَق لىق ِت ِجَت ات تضَن يٗا ضز . 11

ىزضٍآ تؾست ٕاطت ،

ُاگتؾز عوطه ىاىه ٍ ت٘ؼلَه [x1,y1,z1]

ٖلنا ًٔست ِت لهته

ُاگتؾز ُٖذ تػطؾ ٍ ًٖاضٍز تػطؾ ،غجطه ِحفن ضز تاتض [x1,y1,z1] :نٗضاز تاتض خطچ ٍز ًٖاضٍز ٕاّطتهاضاپ ةؿح طت

( 12 ( ̇ ̇ ) )

̇

( 13 ( ̇ ̇ ) )

ٕاّطتهاضاپ

1X⁰

1 ٍ Z⁰ ٍ لاطگتًا ظا ُزافتؾا ات طّ ضز ٕط٘گ

ِت ِظحل

ٖه لاهحتؾا لتال طٗظ ـٍض :سٌقات

( 14 )

( ) _{( )} _{( )}

( 15 )

( ) _{( )} _{( )} _{( )}

( 16 )

( ) _{( )} _{( )} _{( )}

لکش

‌ 11 ط٘غ خطچٍز تاتض نطحته ِٗاپ طَٗهت ه٘هًََلَّ

3 -

‌ تابر‌لزتنک

‌

ِو اجًآ ظا ؽىػ خطچ ٍؾَت تاتض ٕضاسٗاپ

ٖلوؼلا ماجًا

ٖه

َق ز

خطچ توطح ظا لمتؿه ٍ

ٖه ِٗاپ ٕاّ

ٖه ،سقات لسه ىاَت

فرت ٍز ِت ط٘ؿه فٗاو٘پ ٍ لزاؼت ظفح ٕاطت اض تاتض ٖلطتٌو ن٘ؿمت اعجه طو

ىآ ِت ِهازا ضز ِو ز

ٖه ِتذازطپ زَق

.

0 1 2 3 4 5

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7

اُذ(%)

ىاهظ (s)

(8)

3 - 1 -

‌ یساسراذیاپ

‌

لاؼفط٘غ لهفه زَجٍ ل٘لز ِت خطچ ٍز تاتض ضز

خطچ طٗظ ،اّ

ي٘وّ ِت ٍ ِتقاسً ٖى٘تاتؾا تضَن ِت اض لزاؼت ظفح ٖٗاًاَت لزاؼت ٍ ٕضاسٗاپ ٍذطت ٍ ٖى٘هاٌٗز ضََ ِت ٖتؿٗات ل٘لز زَذ

ظفح اض سٌو ت٘لاؼف ِلوج ظا . يٗا ضز ُسق ِتفطٗصپ تضَن ٕاّ

ضا٘ؼه ظا ُزافتؾا ،عَيَه

ىآ لطتٌو ٍ

ٖه تاتض ٍؾَت ضا٘ؼه .سقات

مً

ٕضٍاتكگ طفن ُِ

ُزٍسحه ، ِو ٕا

ضز ٖجضاذ ٕاٍّطً٘ ٍ ٖؾطٌٗا ،ٖلمث ٕاٍّطً٘ ظا لناح ضٍاتكگ

ٖه طفن ىآ

ٖه ىا٘ت ٍ ُزَوً ِثؾاحه اض سٌقات سٌو

ضز تاتض ِو

اض زَذ لزاؼت ٖت٘لاؼف چّ٘ لاوػا ىٍست ِتؿًاَت ُزٍسحه يٗا ظفح سٌو يٗا ضز ِو

ِلأؿه طظً زضَه ٓزٍسحه ،لاذ ٍ

بَلُه

،

ٖه ُِمً هٗ اٌْت .سقات

ٖه وطف ،تؾا طوش ِت مظلا -

مَلؼه بَلُه ُِمً ىزضٍآ تؾست ٕاطت ضَوصه ٕاٍّطً٘ زَق

ٖه .سٌقات لىق كتاُه ،ٕضا٘ؼه يٌ٘چ ظا ُزافتؾا ات 12

ضاسمه ،

ت٘ؼلَه ٍ ؽپؾ ٍ ُزَوً ِثؾاحه اض تاتض ِػَوجه مطج عوطه

تلاح تاتض بَلُه

، ت ضا٘ؼه يٗا ظا ُزافتؾا ات ِ

تؾز

ٖه سٗآ .

( 17

)

∑

ضا٘ؼه ٍؾَت مطج عوطه ةؾاٌه لحه ِثؾاحه ظا ؽپ طفن ُِمً

ٕضٍاتكگ ؽىػ ظا ُزافتؾا ات ، ِت ضٍاتكگ لاوػا ظا لناح لوؼلا

ؽىػ خطچ ت٘ؼلَه ِت ،مطج عوطه ٖلؼف ت٘ؼلَه ،ٖلوؼلا

ٖه ُزاز لامتًا ُسهآ تؾست بَلُه ٖلطتٌو ماطگاٗز نَلت .زَق

تْج ،تاتض لىق ضز لزاؼت ظفح

13 .تؾا ُسق ُزضٍآ ،

‌ ‌

لکش

‌ 12

ِػَوجه مطج عوطه خطچ ُاطوّ ِت ضازٍظات خطچ ٍز تاتض

ؽىػ

ٖلوؼلا

‌ ‌

لکش

‌ 13

ٖلطتٌو ماطگاٗز نَلت ظاؾضاسٗاپ فرت

‌

ٖه ٖؼؾ ـٍض يٗا ات لاح ماوت ضز تاتض ،زَق

ت اّ

ٍ ٕضاسٗاپ

ظفح اض فلزاؼت سٌو

.

3 - 2 -

‌ ینامس‌تکزح‌زیسم‌شیامیپ‌لزتنک

‌

ضز تْج ٖلطتٌو لسه ىا٘ت ِت فرت يٗا بَلُه ط٘ؿه فٗاو٘پ

،تاتض ٍؾَت

لىق كثَ . 14

تلاح ٍ ت٘ؼلَه ،

تتاث غجطه ِت تثؿً تاتض ىاهظ ضز

ماn تضَن ِت ،

، ( )

ٍ ( )

يٗا وطف ات .تؾا ُسق ُزاز فٗاوً ، ( )

ِو

ُِمً

ضز ،سقات مركه ىاهظ ىآ ضز ٖحاطَ بَلُه ُِمً ،K يٗا

ٖه ٖلطتٌو لسه تضَن ٍ ِلناف ىاع٘ه ِثؾاحه ات سًاَت

ات تاتض تلاح ٍ ت٘ؼلَه ي٘ت لناٍ ٖيطف ٍذ تْج يٌ٘چوّ

ِئاضا اض مظلا تاضَتؾز تاتض نطحته فرت ِت ،بَلُه ُِمً

سّز 25] لىق ضز .[ 15

يٗا ٖلطتٌو نتٗضَگلا ماطگاٗز نَلت ،

.تؾا ُسهآ ضز فٗاوً ِت فرت

‌

لکش

‌ 14 ٕاّطتهاضاپ ٍ تاتض نطحته ِٗاپ لطتٌو تْج ظاً٘ زضَه

ًٖاهظ توطح ط٘ؿه

PID (KP,KI,KD)

خطچ ٍز تاتض

ٕظاجه لسه ضا٘ؼه

ZMP

+ -

ٖجضاذ ٕاٍّطً٘

K

ُاطوّ ِت ضازٍظات ؽىػ خطچ

ٖلوؼلا

ًِاگٍز ؼَىؼه لٍسًاپ

(9)

لکش

‌ 15

‌ تاتض نطحته ِٗاپ ٖلطتٌو نتٗضَگلا

لىق كثَ

15 تْج فاطحًا ٍِٗاظ ظا لناح ٕاُذ ،

توطح ( تاتض eϕ

نَلت هٗ ٍؾَت ) ه٘ؾلاو طلطتٌو

ٖثؾاٌت(

-

كتكه لاطگتًا ٍ ط٘گ )ط٘گ

، خطچ ِت اض ٖل٘ؿًاطفٗز ضَتؾز هٗ

اّ

ٖه لاوػا سٌو

ٖه توطح ي٘ح تاتض فذطچ ثػات ِو زَق

.

ِلناف ِثؾاحه ات يٌ٘چوّ

ُسق ٖحاطَ ُِمً ات تاتض ت٘ؼلَه

نَلت هٗ ظا ُزافتؾا ات ٍٕطك٘پ تػطؾ ىاع٘ه ،ِظحل ىآ ضز

ٖثؾاٌت( ه٘ؾلاو طلطتٌو -

كتكه )ط٘گ لاوػا ٍ ي٘٘ؼت

ٖه زَق .

4 -

‌ هیبش یساس

‌ دزکلمع

‌

ات ضازٍظات خطچٍز تاتض ظا ُسهآ تؾست لسه ٕزسػ ٕاّطتهاضاپ ؽىػ خطچ ةٗاطي ٕزسػ طٗزامه يٌ٘چوّ ٍ ٖلوؼلا

ُطْت

ضز ة٘تطت ِت ُسق ىا٘ت ٖلطتٌو ٕػتاطتؾا ِت ِجَت ات ٖلطتٌو سج لٍ

ٕاّ

4

، 5 ٍ 6

ُسق ُزضٍآ .سًا

ظا ُزافتؾا ات ،ٖلطتٌو طٗزامه

ٖؼؾ

ٍ

ُسق ي٘٘ؼت ٖتؾز ـٍض ِت اُذ .سًا

لوذج

‌ 4 ِت ٌَتطه ٕزسػ ضاسمه ٍ ٖىٗع٘ف ٕاّطتهاضاپ تاتض لسه

ضز

ِ٘ثق

ٕظاؾ

ٕزسػ طٗزامه اّطتهاضاپ تاح٘يَت

26 (m) /

0 لٍا هٌ٘ل لََ

003 (m) /

0 لٍا هٌ٘ل مطج عوطه لحه

25 (m) /

0 مٍز هٌ٘ل لزاؼه مطج يتفطگ ضاطل لحه

)مٍز ٕظاجه هٌ٘ل لََ(

s( m/rad∙ N∙ 2) /

0 مٍز لهفه ناىُنا ةٗطي

(

∙s m/rad

∙ ) N 2 /

0 ٖلوؼلاؽىػ خطچلهفه ناىُنا ةٗطي

(kg) 15 /

0 اّخطچ مطج

(kg) 6 /

0 لٍا هٌ٘ل مطج

(kg) 35 /

0 ٖلوؼلا ؽىػ خطچ مطج

(kg) 3 /

0 مٍز ٕظاجه هٌ٘ل لزاؼه مطج

( m²

∙ ) kg 007 /

0 لٍا هٌ٘ل ٖؾطٌٗا ىاوه

( m² kg∙ 001) /

0 ٖلوؼلا ؽىػ خطچ ٖؾطٌٗا ىاوه

لوذج

‌ 5 ط٘ؿه لطتٌو فرت ةٗاطي طٗزامه

ِ٘ثق ضز

ٕظاؾ

ٕزسػ طٗزامه ط٘ؿه لطتٌو ةٗاطي

5 5 / 0

1 / 0

1 5 / 0

‌

لوذج

‌ 6

ِ٘ثق ضز ظاؾضاسٗاپ فرت ٖلطتٌو ةٗاطي ٕزسػ طٗزامه

ٕظاؾ

ةٗاطي ظاؾضاسٗاپ

ٕزسػ طٗزامه 916 / 3

1 0624 / 0

فّاو ٍ لاات لوػ تػطؾ ،طٗزامه يٗا ي٘٘ؼت ضز ىزَت ًٖاؾًَ

سه ،نتؿ٘ؾ ٕزٍضٍ ٕاطت عاثقا سح ٕاّطتل٘ف ضاٌو ضز باَج .تؾا ُسق ِتفطگ طظً

ِهازا ضز

ِ٘ثق ِت تؿت ٍ تاتض لسه ٕظاؾ

فلتره ٍٗاطق ِؾ ضز ىآ

.

4 - 1 -

‌ لوا‌تلاح

‌

ِ٘لٍا ٍٗاطق هٗ ات تاتض ِو تؾا ُسق وطف تلاح يٗا ضز ٕاٍّظات ٍ ُزطو ضاو ِت عٍطق مركه ىاهظ لََ ضز اض زَذ

ٖه ط٘٘غت لٍسج ضز تاتض ِ٘لٍا ٍٗاطق ٕزسػ طٗزامه .سّز

7 ،

وطف ٕضََ اٍّظات تاط٘٘غت َُحً يٌ٘چوّ .تؾا ُسق ُزضٍآ لٍسًاپ لسه مٍز هٌ٘ل ضز اًْآ لزاؼه ضاسمه ِو تؾا ُسق لىق ٌٖحٌه تضَن ِت ًِاگٍز ؼَىؼه 16

.سقات

ِت اض زَذ ءاعجا تلاح تاتض ،تسه لََ ضز ٍذطت تضَن

طفن ُِمً ضا٘ؼه كثَ اض بَلُه ُِمً تاط٘٘غت ىاع٘ه ٍ ُسّاكه خطچ ِت ٖلاوػا ضٍاتكگ ظا ُزافتؾا ات ٍ ُزطو ِثؾاحه ٕضٍاتكگ ؽىػ

ٖه ط٘٘غت فتَلُه تلاح ِت اض تاتض ٍٗاطق ،ٖلوؼلا .سّز

لوذج

‌ 7 تاتض ُسق ُزاؾ لسه ٕاٗاٍظ ِ٘لٍا طٗزامه

‌ طٗزامه

)ٍِٗاظ( اّطتهاضاپ تاح٘يَت

30

- نئال ضَحه ِت تثؿً ٖلنا ًِست ٍِٗاظ

60

- ِت تثؿً مٍز ٕظاجه هٌ٘ل ٕاتؾاض ٍِٗاظ لٍا هٌ٘ل ٕاتؾاض

لىق ضز ،ٕضٍاتكگ طفن ُِمً ضا٘ؼه ِت ِجَت ات 17

ىاع٘ه ،

ىآ ضاسمه يٌ٘چوّ ٍ ٖلنا ًِست ٍِٗاظ بَلُه لحه تاط٘٘غت

ٖه فٗاوً اض ِظحل طّ ضز تاتض ٍٕض .سّز

تلازاؼه توطح

( )

( ́ ́ )

(10)

لىق 18 خطچ ٍؾَت ُسق ِتفطگ ماجًا ىاضٍز ىاع٘ه ،

ؽىػ طّ ضز فلزاؼت ظفح ٍ تاتض ت٘ؼيٍ لطتٌو تْج ٖلوؼلا

ٖه ىاكً اض ِظحل لىق يٌ٘چوّ .سّز

19 تاتض ٖلو ت٘ؼيٍ ،

.تؾا ُزاز فٗاوً ىاهظ لََ ضز لزاؼت ظفح تْج اض

لکش

‌ 16 مٍز هٌ٘ل ٍِٗاظ تاط٘٘غت ٌٖحٌه ؼَىؼه لٍسًاپ ٍٕظات

ًِاگٍز

لکش

‌ 17 ىآ بَلُه ُِمً ٍ تاتض ٖلنا ًِست ٍِٗاظ تاط٘٘غت ٌٖحٌه

لکش

‌ 18 ؽىػ خطچ ًٖاضٍز تاط٘٘غت ٌٖحٌه

ٖلوؼلا

4 - 2 -

‌ مود‌تلاح

‌

ٖه وطف تلاح يٗا ضز تاتض زَق

ن٘متؿه ط٘ؿه هٗ ضز ات ٍرلا

لثل تلاح سًٌاوّ تاتض ِ٘لٍا ٍٗاطق .سٌو توطح ط٘غته باتق

ٖه ِت تاتض ٖلنا ًِست ٍِٗاظ ،باتق تاط٘٘غت ٌٖحٌه .سقات خطچ ىاضٍز ىاع٘ه ٍ ىآ ٓسق ِثؾاحه بَلُه ضاسمه ُاطوّ

ؽىػ ِت توطح ي٘ح ،ىآ ِت ٖلاوػا ضٍاتكگ يٌ٘چوّ ،ٖلوؼلا

لىق ضز ة٘تطت ٕاّ

20 ت ا 23 .تؾا ُسق ُزضٍآ

‌

لکش

‌ 19 طَٗهت زَذ تلاح حلانا ٍ ىاهظ لََ ضز تاتض ي٘ح

ٕظاؾضاسٗاپ

لکش

‌ 21 تاتض ُٖذ باتق تاط٘٘غت ٌٖحٌه

‌

لکش

‌ 21 ُِمً حلانا ٍ تاتض ٖلنا ًِست ٍِٗاظ تاط٘٘غت ٌٖحٌه

ىآ بَلُه

ٕاطت زَجَه ٍٗاطق ظا نتؿ٘ؾ تلاح ىس٘ؾض

ضا٘ؼه كثَ ٖؼ٘ثَ ضاسٗاپ تلاح ِت

ٌٖحٌه ِت ِجَت ات لىق ٕاّ

21 ٍ 22 ىسًاَذ ظا ؽپ تاتض ،

يٗا ٍ ِتذازطپ بَلُه ضاسمه حلانا ِت ،طلطتٌو ِت ٕزٍضٍ طٗزامه طفن ُِمً ضا٘ؼه ؼاؾا طت ِو تؾا ُزاز ط٘٘غت ٕضََ اض ضاسمه

0 10 20 30 40 50 60

60 70 80 90 100 110 120

0 10 20 30 40 50 60

-60 -40 -20 0 20 40

0 10 20 30 40 50 60

-100 0 100 200 300 400 500

0 10 20 30 40 50 60

-2 -1 0 1 2

0 10 20 30 40 50 60

-60 -40 -20 0 20 40

t= 0s t= 7/5^{s t=}22/5^{s t=}37/5^{s t=}60^s

ٕزسػ ضاسمه بَلُه ضاسمه

ُٖذ باتق (m/s2 ) ٍِٗاظ (deg)

ىاهظ (s)

ٍِٗاظ (deg)ٍِٗاظ (deg)

ىاهظ (s)

ٕزسػ ضاسمه بَلُه ضاسمه ىاهظ

(s)

ىاهظ (s)

ٍِٗاظ (deg)