‫ﮔﺰارش ﭘﺎﻧﺰدﻫﻢ‬

‫ﺻﻤﯿﻢ ﺧﺴﺮوی‬
‫‪ ١٩‬ﺑﻬﻤﻦ ‪١٣٩٢‬‬

‫روﯾﺘﮕﺮ ﻏﯿﺮﺧﻄ‬

‫دﻟﯿﻞ اﺳﺘﻔﺎده از روﯾﺘﮕﺮ‬ ‫‪١‬‬

‫در ﺟﺮاﺣ از دور ﯾ از ﻣﺸ ﻼت اﻧﺪازهﮔﯿﺮی ﻧﯿﺮوی ﻣﺤﯿﻄ ﻣ ﺑﺎﺷﺪ ﮐﻪ ﺑﺮای اﯾﻨﮑﻪ ﮐﺎرﺑﺮ ﺣﺲ ﺧﻮﺑﯽ‬
‫از ﻣﺤﯿﻂ ﺑﺪﺳﺖ آورد ﺣﺘﻤﺎً ﺑﺎﯾﺪ اﻧﺪازهﮔﯿﺮی ﮔﺮدد‪ .‬ﯾ از روﺷﻬﺎی اﻧﺪازهﮔﯿﺮی ﻧﯿﺮو ﺧﻄ ﺳﺎزی ﮐﺮدن‬
‫ﺳﯿﺴﺘﻢ و اﺳﺘﻔﺎده از روﯾﺘﮕﺮﻫﺎی ﺧﻄ ﻣﺜﻞ ﻓﯿﻠﺘﺮ ﮐﺎﻟﻤﻦ ﻣ ﺑﺎﺷﺪ‪ .‬اﻣﺎ از آﻧﺠﺎﯾﯽ ﮐﻪ رﺑﺎت ﯾ ﺳﯿﺴﺘﻢ‬
‫ﻣ ﺎﻧﯿ ﻏﯿﺮﺧﻄ اﺳﺖ و ﺣﺘ در اﯾﻦ ﺣﺎﻟﺖ ﻏﯿﺮﺧﻄ ﺗﻌﯿﯿﻦ دﻗﯿﻖ ﭘﺎراﻣﺘﺮﻫﺎی رﺑﺎت و ﻧﯿﺮوﻫﺎی‬
‫اﺻﻄ ﺎﮐ ﺑﺎﻋﺚ اﯾﺠﺎد ﻧﺎﻣﻌﯿﻨ ﻣ ﮔﺮدد ﻟﺬا ﺧﻄ ﮐﺮدن اﯾﻦ ﺳﯿﺴﺘﻢ ﺑﺎﻋﺚ اﯾﺠﺎد ﻧﺎﻣﻌﯿﻨ زﯾﺎد در‬
‫ﺳﯿﺴﺘﻢ و ﮐﺎﻫﺶ ﻋﻤﻠ ﺮد ﻧﺎﻣﻨﺎﺳﺐ روﯾﺘﮕﺮ ﺧﻮاﻫﺪ ﺷﺪ‪ .‬ﻻزم ﺑﻪ ذﮐﺮ اﺳﺖ ﮐﻪ از آﻧﺠﺎﯾﯽ ﮐﻪ رﺑﺎت در‬
‫اﯾﻦ ﺳﯿﺴﺘﻢ ﺗﻠﻪاﭘﺮاﺗﻮری ﻣ ﺧﻮاﻫﺪ ﯾ ﻣﺴﯿﺮی را دﻧﺒﺎل ﮐﻨﺪ ﭘﺲ ﺧﻄ ﮐﺮدن آن ﺣﻮل ﯾ ﻧﻘﻄﻪ اﺻﻼ‬
‫اﻣﺮی ﻣﻨﻄﻘ ﺑﻪ ﻧﻈﺮ ﻧﻤ رﺳﺪ‪.‬‬

‫روﯾﺘﮕﺮ ﻣﺪ ﻟﻐﺰﺷ‬ ‫‪٢‬‬

‫در ﻣﻘﺎﻟﻪ ]‪ [١‬از ﻓﯿﺪﺑ ﺧﺮوﺟ ﻣﻮﻗﻌﯿﺖ ﻣﻔﺼﻞﻫﺎی رﺑﺎت ﺑﺎ ‪ n‬درﺟﻪ آزادی ﺑﺮای ﺗﺨﻤﯿﻦ ﺳﺮﻋﺖ‬
‫ﻣﻔﺼﻞﻫﺎ و ﻫﻤﭽﻨﯿﻦ ﻧﯿﺮوی اﯾﺠﺎد ﺷﺪه در اﺛﺮ ﺗﻌﺎﻣﻞ رﺑﺎت ﺑﺎ ﻣﺤﯿﻂ ﺑﻪ وﺳﯿﻠﻪی روﯾﺘﮕﺮﻫﺎی ﻣﺪ ﻟﻐﺰﺷ‬
‫ﺑﺮای ﺳﯿﺴﺘﻢﻫﺎی ﻣﺮﺗﺒﻪ دو اﺳﺘﻔﺎده ﺷﺪه اﺳﺖ ﮐﻪ در ﻣﻘﺎﻟﻪ ]‪ [٢‬ﺑﺮای ﺳﯿﺴﺘﻢﻫﺎی ﻣ ﺎﻧﯿ ‪ SISO‬ﺑﯿﺎن‬
‫ﺷﺪه اﺳﺖ‪.‬‬
‫اﮔﺮ ﻣﻌﺎدﻻت دﯾﻨﺎﻣﯿ رﺑﺎت در ﻓﻀﺎی ﮐﺎرﺗﺰﯾﻦ را ﺑﻪ ﺻﻮرت زﯾﺮ در ﻧﻈﺮ ﺑ ﯿﺮﯾﻢ‪:‬‬

‫}‪Ẋi1 = Xi2 i ∈ {m, s‬‬

‫‪−1‬‬
‫= ‪Ẋi2‬‬ ‫) ‪Mi (Xi1 )(−hi (Xi1 , Xi2 ) + Fi + Fj‬‬ ‫}‪j ∈ {h, e‬‬

‫ﮐﻪ در آن ‪ Xi1‬ﻣﻮﻗﻌﯿﺖ و ‪ Xi2‬ﺳﺮﻋﺖ ﻣﺠﺮی ﻧﻬﺎﯾﯽ رﺑﺎت را ﻧﺸﺎن ﻣ دﻫﺪ و ‪ hi‬ﺗﺮم ﻫﺎی ﮐﻮرﯾﻮﻟﯿﺲ‬
‫و ﮔﺮاﻧﺶ را ﻧﺸﺎن ﻣ دﻫﺪ‪ .‬ﻫﻤﭽﻨﯿﻦ ‪ Fi‬ﻧﺸﺎﻧﺪﻫﻨﺪهی ﺳﯿ ﻨﺎل ﮐﻨﺘﺮﻟ و ‪ Fh‬ﻧﯿﺮوی دﺳﺖ اﭘﺮاﺗﻮر و ‪Fe‬‬

‫‪١‬‬
 ‫ﻧﯿﺮوی ﻣﺤﯿﻄ ﻣ ﺑﺎﺷﺪ‪ .‬در اﯾﻦ ﺻﻮرت ﻣﻌﺎدﻟﻪی روﯾﺘﮕﺮ ﺑﻪ ﺻﻮرت زﯾﺮ ﺧﻮاﻫﺪ ﺑﻮد‪:‬‬
‫˙‬
‫‪X̂i1 = Xˆi2 + zi1‬‬ ‫}‪i ∈ {m, s‬‬
‫˙‬
‫‪X̂i2 = Mi−1 (X̂i1 )(−hi (X̂i1 , X̂i2 ) + Fi ) + zi2‬‬

‫ﮐﻪ در آن ‪ zi1‬و ‪ zi2‬ﺗﺮمﻫﺎی اﺻﻼﺣ ﺑﺮای روﯾﺘﮕﺮ ﻣ ﺑﺎﺷﻨﺪ ﮐﻪ ﻫﺮ داراﯾﻪ از اﯾﻦ ﺑﺮدارﻫﺎ ﺑﻪ ﺻﻮرت زﯾﺮ‬
‫ﻣﺤﺎﺳﺒﻪ ﻣ ﮔﺮدد‪:‬‬
‫‪1‬‬
‫) ‪zi1k = λik |Xi1 − X̂i1 | 2 sgn(Xi1 − X̂i1‬‬
‫) ‪zi2k = αik sgn(Xi1 − X̂i1‬‬

‫ﺗﻮﺟﻪ ﮐﻨﯿﺪ ﮐﻪ در ﻣﻌﺎدﻟﻪی روﯾﺘﮕﺮ ﻧﯿﺮوی دﺳﺖ ﮐﺎرﺑﺮ و ﻣﺤﯿﻂ وارد ﻧﻤ ﺷﻮد و اﯾﻦ دو ﺑﻪ ﺻﻮرت زﯾﺮ‬
‫ﺑﺮ اﺳﺎس آﻧﭽﻪ ﮐﻪ در ﻣﻘﺎﻟﻪ ﺑﯿﺎن ﺷﺪه اﺳﺖ‪ ،‬ﺗﻌﺮﯾﻒ ﻣ ﺷﻮﻧﺪ‪:‬‬

‫‪Fˆh = Mm (X̂m )zm2eq‬‬

‫‪F̂e = Ms (X̂s )zs2eq‬‬

‫ﮐﻪ در آن ‪ zm2eq‬و ‪ zs2eq‬ﺗﺮمﻫﺎی اﺻﻼﺣ ﻣﻮرد اﺳﺘﻔﺎده در روﯾﺘﮕﺮ ﻣ ﺑﺎﺷﻨﺪ ﮐﻪ از ﯾ ﻓﯿﻠﺘﺮ ﭘﺎﯾﯿﻦﮔﺬر‬
‫ﻋﺒﻮر ﮐﺮدهاﻧﺪ‪ .‬دﻟﯿﻞ اﯾﻨﮑﻪ از اﯾﻦ راﺑﻄﻪ ﺑﺮای ﺑﺪﺳﺖ آوردن ﻧﯿﺮوی ﻣﺤﯿﻄ اﺳﺘﻔﺎده ﻣ ﮐﻨﺪ اﯾﻦ اﺳﺖ ﮐﻪ‬
‫ﺑﺎ ﻣﻘﺎﯾﺴﻪ ﻣﻌﺎدﻻت ﺣﺎﻟﺖ روﯾﺘﮕﺮ و ﻣﻌﺎدﻻت اﺻﻠ ﺳﯿﺴﻨﻢ ﺑﺎ ﻓﺮض اﯾﻨﮑﻪ ﻣﺘﻐﯿﺮﻫﺎی ﺣﺎﻟﺖ ﺗﺨﻤﯿﻨ‬
‫ﺑﺎ ﻣﺘﻐﯿﺮﻫﺎی ﺣﺎﻟﺖ اﺻﻠ ﺑﺮاﺑﺮ ﺑﺎﺷﻨﺪ ﻣﻼﺣﻈﻪ ﻣ ﮔﺮدد ﮐﻪ ‪:‬‬

‫‪M −1 Fh + δm = zm2‬‬ ‫‪M −1 Fe + δs = zs2‬‬

‫ﮐﻪ در آن ‪ δm‬ﻧﺎﻣﻌﯿﻨ در ﺳﻤﺖ رﺑﺎت راﻫﺒﺮ و ‪ δs‬ﻧﺎﻣﻌﯿﻨ در ﺳﻤﺖ رﺑﺎت رﻫﺮو ﻣ ﺑﺎﺷﺪ ﮐﻪ ﻫﺮ ﻗﺪر ﻣﻘﺪار‬
‫اﯾﻦ ﻧﺎﻣﻌﯿﻨ در ﻣﺪل ﮐﻤﺘﺮ ﺑﺎﺷﺪ ﻣ ﺗﻮان ﺑﻪ ﺗﺨﻤﯿﻦ ﻣﻨﺎﺳﺒﯽ از ﻧﯿﺮوی ﻣﺤﯿﻂ ﯾﺎ دﺳﺖ رﺳﯿﺪ‪ .‬ﺗﻮﺟﻪ ﮐﻨﯿﺪ‬
‫ﮐﻪ ﭘﺎراﻣﺘﺮﻫﺎی درﺟﻪ آزادی در روﯾﺘﮕﺮ ‪ α‬و ‪ λ‬ﻣ ﺑﺎﺷﺪ ﮐﻪ اﯾﻦ دو ﭘﺎراﻣﺘﺮ ﺑﺮ اﺳﺎس ﻧﺎﻣﻌﯿﻨ ﻫﺎی ﻣﻮﺟﻮد‬
‫در ﻣﺪل ﺑﺮ اﺳﺎس آﻧﭽﻪ در ﻣﻘﺎﻟﻪ ]‪ [٢‬ﺑﯿﺎن ﺷﺪه اﺳﺖ‪ ،‬ﺗﻌﯿﯿﻦ ﻣ ﮔﺮدد‪ .‬اﻣﺎ آﻧﭽﻪ ﮐﻪ از ﺷﺒﯿﻪﺳﺎزیﻫﺎ ﺑﺮ‬
‫ﻣ آﯾﺪ ﻫﺮ ﻗﺪر اﯾﻦ دو ﭘﺎراﻣﺘﺮ را ﺑﯿﺸﺘﺮ ﮐﻨﯿﻢ ﺑﺎﻋﺚ ﻣ ﺷﻮد ﺳﺮﯾﻌﺘﺮ ﺣﺎﻟﺖﻫﺎ ﺑﻪ ﻣﻘﺪار واﻗﻌ ﺧﻮد ﻫﻤ ﺮا‬
‫ﺷﻮﻧﺪ و روﯾﺘﮕﺮ ﺳﺮﯾﻌﺘﺮ ﺧﻮاﻫﺪ ﺷﺪ‪.‬‬
‫در اﺑﺘﺪا ﺑﺮای اﯾﻨﮑﻪ ﮐﺎراﯾﯽ اﯾﻦ روﯾﺘﮕﺮ ﻣﻮرد ﻣﺸﺎﻫﺪه ﻗﺮار ﮔﯿﺮد ﻋﻤﻠ ﺮد آن ﺑﺮ روی ﯾ رﺑﺎت دو‬
‫درﺟﻪ آزادی ﻣﺴﻄﺢ ﮐﻪ ﻧﯿﺮوی ﺟﺎذﺑﻪی ﺑﺮ روی ﺗﺎﺛﯿﺮی ﻧﺪارد ﯾﺎ ﺗﻮﺳﻂ ﺟﺒﺮاﻧﺴﺎزی اﺛﺮ آن ﺣﺬف ﺷﺪه‬
‫اﺳﺖ ﻣﻮرد ﺑﺮرﺳ ﻗﺮار ﮔﺮﻓﺖ‪ .‬در ﻣﻘﺎﻟﻪ ]‪ [١‬ﺑﯿﺎن ﺷﺪه اﺳﺖ ﮐﻪ روﯾﺘﮕﺮ ﺑﺮ روی رﺑﺎﺗ ﮐﻪ ﻣﻌﺎدﻻت‬
‫آن در ﻓﻀﺎی ﮐﺎری ﻧﻮﺷﺘﻪ ﺷﺪه اﺳﺖ ﻃﺮاﺣ ﺷﺪه اﺳﺖ‪ .‬ﭘﺲ ﻣﺎ اﺑﺘﺪا ﺑﺮ آن ﺷﺪﯾﻢ ﮐﻪ اﯾﻦ رﺑﺎت را ﺑﺎ‬
‫ﻣﻌﺎدﻻت ﻧﻮﺷﺘﻪ ﺷﺪه در ﻓﻀﺎی ﮐﺎری ﺷﺒﯿﻪﺳﺎزی ﮐﺮده و روﯾﺘﮕﺮ را ﻧﯿﺰ ﻫﻤﺮاه ﺑﺎ ﯾ ﮐﻨﺘﺮﻟﺮ ﺗﻨﺎﺳﺒﯽ‬
‫ﻣﺸﺘﻖﮔﯿﺮ ﺑﻪ آن اﺿﺎﻓﻪ ﮐﻨﯿﻢ اﻣﺎ در اﯾﻦ ﺷﺒﯿﻪﺳﺎزی ﺑﺎ ﻣﺸ ﻼت زﯾﺎدی ﻣﻮاﺟﻪ ﺷﺪﯾﻢ ﮐﻪ ﺑﺎ ﮐﻤ ﺗﻐﯿﯿﺮ و‬
‫زﯾﺎد ﮐﺮدن ﺿﺮاﯾﺐ ﮐﻨﺘﺮﻟﺮ ﺗﺨﻤﯿﻦ ﻣﺘﻐﯿﺮﻫﺎی ﺣﺎﻟﺖ ﻧﺎﭘﺎﯾﺪار ﻣ ﺷﺪ و ﺷﺒﯿﻪﺳﺎزی ﻣﺘﻮﻗﻒ ﻣ ﺷﺪ ﭘﺲ از‬
‫ﺑﺮرﺳ ﻫﺎی اﻧﺠﺎم ﺷﺪه ﺑﻪ ﻧﻈﺮ ﻣﻦ ﻣﺸ ﻞ اﯾﻦ ﻃﺮاﺣ ﺑﺮای ﻣﻌﺎدﻻت رﺑﺎت در ﻓﻀﺎی ﮐﺎری اﯾﻦ اﺳﺖ‬
‫ﮐﻪ وﻗﺘ ﺑﺮای ﺑﺪﺳﺖ آوردن ﻣﺎﺗﺮﯾﺲ ﺟﺮﻣ رﺑﺎت در ﻓﻀﺎی ﮐﺎری از ژاﮐﻮﺑﯿﻦ اﺳﺘﻔﺎده ﻣ ﮐﻨﯿﻢ ﻣﻤ ﻦ‬
‫اﺳﺖ در ﺣﺮﮐﺖ رﺑﺎت ﺑﻪ ﺳﻤﺖ ورودی ﻣﺮﺟﻊ رﺑﺎت دﭼﺎر ﺗﮑﯿﻨﮕ ﺷﻮد و در ﻧﺘﯿﺠﻪ ﻣﺎﺗﺮﯾﺲ ﺟﺮم در‬

‫‪٢‬‬
 ‫ﻓﻀﺎی ﮐﺎری ﻧﯿﺰ ﻣﻌﮑﻮسﭘﺬﯾﺮ ﻧﮕﺮدد‪.‬‬
‫ﺑﺮای رﻓﻊ اﯾﻦ ﻣﺸ ﻞ از ﻣﻌﺎدﻻت رﺑﺎت در ﻓﻀﺎی ﻣﻔﺼﻠ اﺳﺘﻔﺎده ﮐﺮدﯾﻢ و روﯾﺘﮕﺮ را ﺑﺮ اﺳﺎس‬
‫اﯾﻦ ﻣﻌﺎدﻻت ﻧﻮﺷﺘﯿﻢ ﮐﻪ ﻣﺎﺗﺮﯾﺲ ﺟﺮﻣ آن ﻫﻤﻮاره ﻣﺜﺒﺖ ﻣﻌﯿﻦ اﺳﺖ و ﻫﻤﻮاره ﻣﻌﮑﻮسﭘﺬﯾﺮ اﺳﺖ‪.‬‬
‫ﺣﺎل ﻧﺘﺎﯾﺞ اﯾﻦ ﺷﺒﯿﻪﺳﺎزی در ﺣﺎﻟ ﮐﻪ ﺑﺮ روی اﯾﻦ رﺑﺎت دو درﺟﻪ آزادی روﯾﺘﮕﺮ ﻣﺪ ﻟﻐﺰﺷ و ﯾ‬
‫ﮐﻨﺘﺮﻟﺮ ﺗﻨﺎﺳﺒﯽ ﻣﺸﺘﻖﮔﯿﺮ ﻧﯿﺰ ﺑﺎ ﺗﺨﻤﯿﻦ ﻣﺘﻐﯿﺮﻫﺎی ﺣﺎﻟﺖ رﺑﺎت ﻗﺮار داده ﺷﺪه اﺳﺖ‪ .‬در اﯾﻦ ﺷﺒﯿﻪﺳﺎزی‬
‫ﭘﺎراﻣﺘﺮﻫﺎی روﯾﺘﮕﺮ ﺑﻪ ﺻﻮرت زﯾﺮ ﻗﺮار داده ﺷﺪه اﺳﺖ‪:‬‬

‫‪α = 40 ∗ [1‬‬ ‫]‪1‬‬ ‫‪λ = 10 ∗ [1‬‬ ‫]‪1‬‬

‫و در ﺷ ﻞﻫﺎی ‪ ١‬و ‪ ٢‬ﻧﺘﺎﯾﺞ ﺗﺨﻤﯿﻦ ﺑﺮای ﺳﺮﻋﺖ و ﻣﻮﻗﻌﯿﺖ ﻣﻼﺣﻈﻪ ﻣ ﮔﺮدد‪) :‬ﻻزم ﺑﻪ ذﮐﺮ اﺳﺖ ﮐﻪ‬
‫ورودی ﻣﺮﺟﻊ ﺑﻪ ﺻﻮرت ‪ q1 = 1.5‬و ‪ q2 = 0‬داده ﺷﺪه اﺳﺖ‪(.‬‬

‫‪Angle joint of robot‬‬

‫‪2‬‬
‫‪Original‬‬
‫‪Estimate‬‬
‫‪1.5‬‬
‫‪Joint1‬‬

‫‪1‬‬

‫‪0.5‬‬

‫‪0‬‬
‫‪0‬‬ ‫‪1‬‬ ‫‪2‬‬ ‫‪3‬‬ ‫‪4‬‬ ‫‪5‬‬ ‫‪6‬‬ ‫‪7‬‬ ‫‪8‬‬ ‫‪9‬‬ ‫‪10‬‬

‫‪0.15‬‬
‫‪Original‬‬
‫‪0.1‬‬ ‫‪Estimate‬‬
‫‪0.05‬‬

‫‪0‬‬
‫‪Joint2‬‬

‫‪−0.05‬‬

‫‪−0.1‬‬

‫‪−0.15‬‬

‫‪−0.2‬‬
‫‪0‬‬ ‫‪1‬‬ ‫‪2‬‬ ‫‪3‬‬ ‫‪4‬‬ ‫‪5‬‬ ‫‪6‬‬ ‫‪7‬‬ ‫‪8‬‬ ‫‪9‬‬ ‫‪10‬‬
‫‪time‬‬

‫ﺷ ﻞ ‪ :١‬ﻣﻮﻗﻌﯿﺖ ﻣﻔﺼﻞﻫﺎی رﺑﺎت و ﺗﺨﻤﯿﻦ آﻧﻬﺎ ﺗﻮﺳﻂ روﯾﺘﮕﺮ‬

‫ﻫﻤﭽﻨﯿﻦ در ﺷﺒﯿﻪﺳﺎزی ﺻﻮرت ﮔﺮﻓﺘﻪ ‪ ٣٠‬درﺻﺪ ﻧﺎﻣﻌﯿﻨ ﺑﺮای ﺟﺮمﻫﺎ و ‪ ١٠‬درﺻﺪ ﻧﺎﻣﻌﯿﻨ ﺑﺮای‬
‫ﻃﻮل ﻟﯿﻨﮏﻫﺎ در ﻧﻈﺮ ﮔﺮﻓﺘﻪ ﺷﺪه اﺳﺖ‪ .‬در ﺷ ﻞ ‪ ٣‬ﻧﺘﯿﺠﻪی ﺗﺨﻤﯿﻦ ﻧﯿﺮوی ﻣﺤﯿﻂ ﻓﻨﺮی ﺑﺎ ﺛﺎﺑﺖ ﻓﻨﺮی‬
‫‪ ١٠‬در ﻧﻈﺮ ﮔﺮﻓﺘﻪ ﺷﺪه ﺑﺮای رﺑﺎت آﻣﺪه اﺳﺖ‪):‬ﭘﻬﻨﺎی ﺑﺎﻧﺪ ﻓﯿﻠﺘﺮﻫﺎ ‪ ٣٠‬در ﻧﻈﺮ ﮔﺮﻓﺘﻪ ﺷﺪه اﺳﺖ(‬
‫در ﺷ ﻞ ‪۴‬ﺧﻄﺎی ﺗﺨﻤﯿﻦ ﻧﯿﺮو ﻧﺸﺎن داده ﺷﺪه اﺳﺖ‪ ،‬اﯾﻦ ﺧﻄﺎ در ﻟﺤﻈﺎت اوﻟﯿﻪ ﮐﻪ ﻣﺘﻐﯿﺮﻫﺎی ﺣﺎﻟﺖ‬
‫ﺑﻪ ﻣﻘﺪار واﻗﻌ ﺧﻮد ﻫﻤ ﺮا ﻧﺸﺪﻧﺪ زﯾﺎد اﺳﺖ و ﺗﻘﺮﯾﺒﺎً از ﻟﺤﻈﻪی ‪ ٠٬۵‬ﺛﺎﻧﯿﻪ ﺑﻪ ﺑﻌﺪ اﯾﻦ ﻧﯿﺮو ﺑﻪ ﻣﻘﺪار‬
‫واﻗﻌ ﺧﻮد ﻫﻤ ﺮا ﻣ ﮔﺮدد‪ .‬ﺧﻄﺎﻫﺎﯾﯽ ﮐﻪ ﺑﻌﺪ از اﯾﻦ زﻣﺎن ﻣﺸﺎﻫﺪه ﻣ ﺷﻮد ﻧﺎﺷ از ﻧﺎﻣﻌﯿﻨ ﻗﺮار داده‬
‫ﺷﺪه در ﺳﯿﺴﺘﻢ و ﻗﺮار دادن ﻓﯿﻠﺘﺮ ﺑﺮای ‪ ze2eq‬ﻣ ﺑﺎﺷﺪ‪.‬‬

‫‪٣‬‬
 Angle velocity joint of robot
2.5
Original
2 Estimate
1.5

1
Joint1

0.5

−0.5

−1
0 1 2 3 4 5 6 7 8 9 10

0.5
Original
Estimate
0
Joint2

−0.5

−1

−1.5
0 1 2 3 4 5 6 7 8 9 10
time

‫ ﺳﺮﻋﺖ زاوﯾﻪای ﻣﻔﺼﻞﻫﺎی رﺑﺎت و ﺗﺨﻤﯿﻦ آﻧﻬﺎ ﺗﻮﺳﻂ روﯾﺘﮕﺮ‬:٢ ‫ﺷ ﻞ‬

Environment Torque
80
Original
60 Estimate
40
X direction

20

−20

−40

−60
0 1 2 3 4 5 6 7 8 9 10

30
Original
20 Estimate
Y direction

10

−10

−20
0 1 2 3 4 5 6 7 8 9 10
time

‫ ﻧﯿﺮوی ﻣﺤﯿﻂ و ﺗﺨﻤﯿﻦ آن ﺗﻮﺳﻂ روﯾﺘﮕﺮ‬:٣ ‫ﺷ ﻞ‬

۴
 ‫‪Environment Torque error‬‬
‫‪60‬‬

‫‪40‬‬

‫‪20‬‬
‫‪X direction‬‬

‫‪0‬‬

‫‪−20‬‬

‫‪−40‬‬

‫‪−60‬‬

‫‪−80‬‬
‫‪0‬‬ ‫‪1‬‬ ‫‪2‬‬ ‫‪3‬‬ ‫‪4‬‬ ‫‪5‬‬ ‫‪6‬‬ ‫‪7‬‬ ‫‪8‬‬ ‫‪9‬‬ ‫‪10‬‬

‫‪20‬‬

‫‪10‬‬
‫‪Y direction‬‬

‫‪0‬‬

‫‪−10‬‬

‫‪−20‬‬

‫‪−30‬‬
‫‪0‬‬ ‫‪1‬‬ ‫‪2‬‬ ‫‪3‬‬ ‫‪4‬‬ ‫‪5‬‬ ‫‪6‬‬ ‫‪7‬‬ ‫‪8‬‬ ‫‪9‬‬ ‫‪10‬‬
‫‪time‬‬

‫ﺷ ﻞ ‪ :۴‬ﺧﻄﺎی ﺗﺨﻤﯿﻦ ﻧﯿﺮوی ﻣﺤﯿﻂ‬

‫در اداﻣﻪ ﮐﺎر ﺑﺎﯾﺪ اﯾﻦ روﯾﺘﮕﺮ را ﺑﺮ روی رﺑﺎت ﺟﺮاﺣ ﻣﻮرد ﺑﺮرﺳ ﺧﻮد ﺑﺮای ﺗﺨﻤﯿﻦ ﻧﯿﺮوی‬
‫ﻣﺤﯿﻄ و ﺗﺨﻤﯿﻦ ﺳﺮﻋﺖ ﻣﻔﺼﻞﻫﺎی آن ﭘﯿﺎدهﺳﺎزی ﮐﻨﯿﻢ و ﻧﺘﺎﯾﺞ آن را ﺑﺎ ﺳﻨﺴﻮر ﻧﯿﺮوﯾﯽ ﮐﻪ ﺑﺮ روی‬
‫رﺑﺎت ﻗﺮار داده ﺷﺪه اﺳﺖ ﺑﺮرﺳ ﮐﻨﯿﻢ ﮐﻪ در ﺟﻠﺴﻪی ﺑﻌﺪ ﻧﺘﺎﯾﺞ آن آورده ﻣ ﺷﻮد‪.‬‬

‫ﮐﻨﺘﺮﻟﺮ اﺳﺘﻔﺎده ﺷﺪه‬ ‫‪٣‬‬

‫در اداﻣﻪی ﻣﻘﺎﻟﻪ ]‪ [١‬ﭘﺲ از آن ﮐﻪ روﯾﺘﮕﺮی ﺑﺮای ﺗﺨﻤﯿﻦ ﻧﯿﺮوی ﻣﺤﯿﻄ رﺑﺎت رﻫﺮو و ﻧﯿﺮوی دﺳﺖ‬
‫رﺑﺎت راﻫﺒﺮ ﻃﺮاﺣ ﺷﺪ ﺑﻪ ﮐﻨﺘﺮل اﻣﭙﺪاﻧﺲ در ﻫﺮ دو ﺳﻤﺖ ﭘﺮداﺧﺘﻪ اﺳﺖ ﮐﻪ در ﺳﻤﺖ رﺑﺎت راﻫﺒﺮ‬
‫ﻓﻘﻂ ﯾ ﮐﻨﺘﺮل اﻣﭙﺪاﻧﺲ ﺑﺎ ﺧﻄ ﺳﺎزی ﻓﯿﺪﺑ و در ﺳﻤﺖ رﻫﺮو از ﯾ ﮐﻨﺘﺮل اﻣﭙﺪاﻧﺲ ﻣﺒﺘﻨ ﺑﺮ ﻣﺪ‬
‫ﻟﻐﺰﺷ اﺳﺘﻔﺎده ﺷﺪه اﺳﺖ ﮐﻪ ﮐﻨﺘﺮﻟﺮ ﺳﻤﺖ راﻫﺒﺮ ﺑﻪ ﺻﻮرت زﯾﺮ ﻣ ﺑﺎﺷﺪ‪:‬‬

‫‪Fm = Mm (X̂m1 )vm + h(X̂m1 , X̂m2 ) − F̂h‬‬

‫‪−1‬‬
‫‪where vm = M̃m‬‬ ‫) ‪(−B̃m X̂m2 − K̃m X̂m1 + F̂h − F̂ed2‬‬

‫ﮐﻪ در آن ‪ M̃m‬و ‪ B̃m‬و ‪ K̃m‬ﻣﻘﺎدﯾﺮ اﻣﭙﺪاﻧﺲ ﻣﻄﻠﻮب ﻣ ﺑﺎﺷﺪ و در اﯾﻦ ﮐﻨﺘﺮﻟﺮ ﺑﺮای آﻧﮑﻪ ﮐﺎرﺑﺮ ﻧﯿﺮوی‬
‫رﻫﺮو را ﻫﻢ اﺣﺴﺎس ﮐﻨﺪ از ‪ F̂e‬ﮐﻪ در اﻧﺘﻘﺎل از رﻫﺮو ﺑﻪ راﻫﺒﺮ دﭼﺎر ﺗﺎﺧﯿﺮ ﮐﺎﻧﺎل ﺷﺪه اﺳﺖ‪ ،‬اﺳﺘﻔﺎده‬
‫ﺷﺪه اﺳﺖ ﺗﺎ در ﺣﺎﻟﺖ ﻣﺎﻧﺪﮔﺎر ﻣﻌﺎدﻟﻪی اﻣﭙﺪاﻧﺴ ﺑﻪ ﺻﻮرت زﯾﺮ ﺣﺎﺻﻞ ﮔﺮدد‪:‬‬

‫‪M̃m Ẋm2 + B̃m Xm2 − K̃m Xm1 = −Fh + Fed2‬‬

‫‪۵‬‬
 ‫ﺑﺮای ﮐﻨﺘﺮﻟﺮ ﺳﻤﺖ رﻫﺮو از ﻣﺪل اﻣﭙﺪاﻧﺴ زﯾﺮ اﺳﺘﻔﺎده ﺷﺪه اﺳﺖ‪:‬‬
‫(‬ ‫)‬
‫)‪Fe (s‬‬ ‫‪K̃s‬‬
‫= )‪Zs (s‬‬ ‫‪= − M̃s s + B̃s +‬‬ ‫)‪(١‬‬
‫)‪Ve (s‬‬ ‫‪s‬‬

‫‪ er2 = Xs2 − Xs2‬ﮐﻪ در آن ‪ d1‬ﺗﺎﺧﯿﺮ ﻣﻮﺟﻮد در‬ ‫‪d1‬‬

‫‪ er1 = Xs1 − Xm1‬و‬ ‫‪d1‬‬
‫ﺣﺎل ﺑﺎ در ﻧﻈﺮ ﮔﺮﻓﺘﻦ‬
‫ﮐﺎﻧﺎل ﺑﯿﻦ رﺑﺎت راﻫﺒﺮ و رﻫﺮو ﻣ ﺑﺎﺷﺪ‪ ،‬ﻣ ﺗﻮان ﻣﻌﺎدﻟﻪی اﻣﭙﺪاﻧﺴ ﻣﻄﻠﻮب زﯾﺮ را ﺑﺎ اﺳﺘﻔﺎده از ﻋﮑﺲ‬
‫ﺗﺒﺪﯾﻞ ﻻﭘﻼس از ﻣﻌﺎدﻟﻪی ‪ ١‬ﺑﺮای رﺑﺎت رﻫﺮو ﺑﺪﺳﺖ آورد‪:‬‬
‫‪I = M̃s ėr2 + B̃s er2 + K̃s er1 + Fe‬‬
‫ﺣﺎل ﺑﺎ در ﻧﻈﺮ ﮔﺮﻓﺘﻦ ﺳﻄﺢ ﻟﻐﺰﺷ زﯾﺮ‪:‬‬
‫‪∫t‬‬
‫= ̂‪s‬‬ ‫‪M̃s−1 I(τ‬‬
‫) ‪ˆ )d(τ‬‬
‫‪0‬‬

‫ﺳﯿ ﻨﺎل ﮐﻨﺘﺮﻟ ﺑﻪ ﺻﻮرت زﯾﺮ ﺣﺎﺻﻞ ﻣ ﮔﺮدد‪:‬‬

‫‪Fs = − Ms (X̂s1 [M̃s−1 K̃s X̂s1 + M̃ −1 B̃s X̂s2‬‬
‫‪− Ms−1 (X̂s1 )hs (X̂s1 , X̂s2 ) + (M̃m‬‬
‫‪−1‬‬
‫‪K̃m − M̃s−1 K̃s )X̂m1‬‬
‫‪d1‬‬

‫‪−1‬‬
‫‪+ (M̃m‬‬ ‫‪B̃m − M̃s−1 B̃s )X̂m2‬‬
‫‪d1‬‬ ‫‪−1‬‬
‫‪− (M̃m‬‬ ‫‪−1‬‬
‫‪− M̃m‬‬ ‫‪d1‬‬
‫‪(X̂m1‬‬ ‫‪))F̂h‬‬
‫‪−1 dd‬‬
‫‪M̃m‬‬ ‫‪F̂e + M̃s−1 F̂e + zs2eq − zm2eq‬‬
‫‪d1‬‬
‫])̂‪+ Kg sign(s‬‬
‫ﮐﻪ در آن ‪ Kg‬از ﻣﻌﺎدﻟﻪی زﯾﺮ ﺑﺪﺳﺖ ﻣ آﯾﺪ‪:‬‬
‫√‬
‫‪Kg ≥ 2 n (max{αs } + max{αm }) + ϵg‬‬
‫ﮐﻪ اﯾﻦ ﺷﺮط ﺑﺮ اﺳﺎس اﯾﻦ ﺗﻌﺮﯾﻒ ﺷﺪه اﺳﺖ ﮐﻪ ﭘﺎراﻣﺘﺮﻫﺎی درﺟﻪ آزادی در روﯾﺘﮕﺮ ﺑﻪ ﮔﻮﻧﻪای ﺗﻨﻈﯿﻢ‬
‫ﺷﻮد ﮐﻪ ﺗﺨﻤﯿﻦ ﻣﺘﻐﯿﺮﻫﺎی ﺣﺎﻟﺖ در زﻣﺎن ﻣﺤﺪودی ﺑﻪ ﻣﻘﺪار اﺻﻠ آن ﻫﻤ ﺮا ﺷﻮﻧﺪ‪ .‬اﺛﺒﺎت اﯾﻨﮑﻪ اﯾﻦ‬
‫ﮐﻨﺘﺮﻟﺮ ﺑﺎﻋﺚ ﻣ ﺷﻮد ﻣﺴﯿﺮﻫﺎی ﺣﺎﻟﺖ در زﻣﺎن ﻣﺤﺪود ﺑﻪ ﺳﻄﺢ ﻟﻐﺰﺷ ﺑﯿﺎن ﺷﺪه ﺑﺮﺳﻨﺪ در ﻣﺮﺟﻊ‬
‫]‪ [٣‬ﺑﯿﺎن ﺷﺪه اﺳﺖ‪.‬‬
‫در ﻧﻬﺎﯾﺖ ﻃﺮح ﮐﻨﺘﺮﻟ ﺑﯿﺎن ﺷﺪه ﺑﻪ ﺻﻮرت زﯾﺮ ﻣ ﺑﺎﺷﺪ‪:‬‬

‫ﺷ ﻞ ‪ :۵‬ﻃﺮح ﮐﻨﺘﺮﻟ ﻣﻮرد اﺳﺘﻔﺎده‬

‫‪۶‬‬
References
[1] J.M. Daly and D.W.L. Wang. Time-delayed output feedback bilateral
teleoperation with force estimation for n -dof nonlinear manipulators.
Control Systems Technology, IEEE Transactions on, 22(1):299–306, Jan
2014.

[2] J. Davila, L. Fridman, and Arie Levant. Second-order sliding-mode ob-

server for mechanical systems. Automatic Control, IEEE Transactions
on, 50(11):1785–1789, Nov 2005.

[3] John Michael Daly. Output Feedback Bilateral Teleoperation with Force
Estimation in the Presence of Time Delays. PhD thesis, Waterloo Un-
versity, 2010.