一種具有力控制的六自由度機(jī)械臂標(biāo)準(zhǔn)漢字書寫方法
【專利摘要】本發(fā)明屬于機(jī)械臂控制技術(shù)領(lǐng)域，具體涉及一種具有力控制的六自由度機(jī)械臂標(biāo)準(zhǔn)漢字書寫方法。一種具有力控制的六自由度機(jī)械臂標(biāo)準(zhǔn)漢字書寫方法，包括漢字輪廓信息提取、機(jī)械臂末端軌跡規(guī)劃、阻抗控制方法設(shè)計(jì)、接觸面位置尋找和書寫過(guò)程的力控制：具體包括：基于Windows自帶的TTF字庫(kù)提取漢字的輪廓點(diǎn)位置信息，并把輪廓曲線分為直線和樣條曲線；將提取出的漢字輪廓位置坐標(biāo)轉(zhuǎn)化為機(jī)械臂末端執(zhí)行器笛卡爾空間的坐標(biāo)。本發(fā)明使用六自由度機(jī)械臂完成力控制下的漢字書寫任務(wù)，對(duì)工業(yè)機(jī)械臂具有通用性，可廣泛應(yīng)用于實(shí)踐教學(xué)、科技展覽中，也可以用于工業(yè)領(lǐng)域完成噴涂、雕刻等任務(wù)，可以在脆性物體上書寫，保證了書寫的精確性與安全性。
【專利說(shuō)明】
一種具有力控制的六自由度機(jī)械臂標(biāo)準(zhǔn)漢字書寫方法
技術(shù)領(lǐng)域
[0001] 本發(fā)明屬于機(jī)械臂控制技術(shù)領(lǐng)域，具體涉及一種具有力控制的六自由度機(jī)械臂標(biāo) 準(zhǔn)漢字書寫方法。
【背景技術(shù)】
[0002] 機(jī)械臂目前已經(jīng)廣泛應(yīng)用于航天、醫(yī)療及工業(yè)生產(chǎn)領(lǐng)域，雖然近年來(lái)出現(xiàn)了專門 用于書寫漢字的機(jī)械臂，但大多是模仿名人書寫或進(jìn)行書法繪制，使用小型低自由度的執(zhí) 行機(jī)構(gòu)完成任務(wù)，用于家庭或辦公環(huán)境中，這類書法機(jī)器人特點(diǎn)是低自由度手臂的結(jié)構(gòu)設(shè) 計(jì)或?qū)κ謱憹h字的模仿。工業(yè)機(jī)械臂精度高且速度快，設(shè)計(jì)一種專門針對(duì)典型六自由度機(jī) 械臂的標(biāo)準(zhǔn)漢字書寫系統(tǒng)十分必要，標(biāo)準(zhǔn)漢字的書寫系統(tǒng)可以用于實(shí)踐教學(xué)、科技展覽，也 可以用于工廠中完成噴涂、繪制等任務(wù)。
[0003] 當(dāng)機(jī)械臂漢字書寫系統(tǒng)應(yīng)用于特殊的工業(yè)場(chǎng)合時(shí)，如雕刻、脆性物體或不平滑表 面上的書寫等，機(jī)械臂書寫漢字時(shí)只進(jìn)行位置控制無(wú)法保證寫字的精度和安全性，在書寫 過(guò)程中對(duì)機(jī)械臂加入力約束十分必要，目前沒(méi)有在工業(yè)機(jī)械臂漢字書寫系統(tǒng)中加入力控制 的研究。為了保證寫字時(shí)的精度，不出現(xiàn)寫字時(shí)力過(guò)大或過(guò)小的情況，需要在寫字時(shí)使機(jī)械 臂末端以恒定力在接觸面上書寫。另外，具有力控制的機(jī)械臂在書寫漢字時(shí)不需要手動(dòng)對(duì) 接觸面位置進(jìn)行重復(fù)精確定位，可以直接根據(jù)機(jī)械臂末端的接觸力估計(jì)接觸表面的位置， 更加簡(jiǎn)單方便。
[0004] 本發(fā)明設(shè)計(jì)的系統(tǒng)針對(duì)典型的六自由度工業(yè)機(jī)械臂，具有力控制的標(biāo)準(zhǔn)漢字書寫 系統(tǒng)的設(shè)計(jì)對(duì)工業(yè)機(jī)械臂的研究具有重要的借鑒意義，可直接應(yīng)用于同結(jié)構(gòu)類型的機(jī)械臂 控制及漢字書寫任務(wù)中。

【發(fā)明內(nèi)容】

[0005] 本發(fā)明的目的在于提供一種實(shí)現(xiàn)力控制下典型工業(yè)機(jī)械臂的漢字書寫的具有力 控制的六自由度機(jī)械臂標(biāo)準(zhǔn)漢字書寫方法。
[0006] 本發(fā)明的目的是這樣實(shí)現(xiàn)的：
[0007] -種具有力控制的六自由度機(jī)械臂標(biāo)準(zhǔn)漢字書寫方法，包括漢字輪廓信息提取、 機(jī)械臂末端軌跡規(guī)劃、阻抗控制方法設(shè)計(jì)、接觸面位置尋找和書寫過(guò)程的力控制：
[0008] (1)基于Windows自帶的TTF字庫(kù)提取漢字的輪廓點(diǎn)位置信息，并把輪廓曲線分為 直線和樣條曲線；
[0009] (2)將提取出的漢字輪廓位置坐標(biāo)轉(zhuǎn)化為機(jī)械臂末端執(zhí)行器笛卡爾空間的x和y坐 標(biāo)，分別對(duì)直線和樣條曲線進(jìn)行機(jī)械臂末端的軌跡規(guī)劃；
[0010] (3)設(shè)計(jì)機(jī)械臂的阻抗控制方法，對(duì)機(jī)械臂末端的位置和力同時(shí)進(jìn)行控制；
[0011] (4)設(shè)計(jì)六自由度機(jī)械臂尋找接觸面的策略，計(jì)算接觸面在笛卡爾空間的位置；
[0012] (5)在力控制下書寫漢字；
[0013] 所述的步驟(1)具體為：
[0014] (1 ? 1)基于Windows自帶的TTF字庫(kù)，調(diào)用CFont類的CreateFont ()函數(shù)設(shè)置待書寫 漢字的高度、寬度及字體；
[0015] ( 1 . 2)每個(gè)漢字包含若干條閉合輪廓，將其分為直線和樣條曲線，使用 GetGlyphOutline()函數(shù)提取漢字的輪廓信息；
[0016] (1.3)創(chuàng)建文本文件，將輪廓位置坐標(biāo)存入文本文件中。
[0017]所述的步驟(2)具體為：
[0018] (2.1)讀取文本文件中的輪廓曲線位置坐標(biāo)，將讀取的平面坐標(biāo)值分別轉(zhuǎn)化為機(jī) 械臂末端執(zhí)行器在笛卡爾空間的x和y坐標(biāo)；
[0019] (2.2)對(duì)直線輪廓線進(jìn)行直線軌跡規(guī)劃，通過(guò)歸一化參數(shù)確定起點(diǎn)與終點(diǎn)間中間 點(diǎn)的坐標(biāo)，歸一化參數(shù)采用拋物線過(guò)渡的線性函數(shù)，保證機(jī)械臂末端在書寫漢字時(shí)速度連 續(xù)；
[0020] (2.3)對(duì)樣條輪廓線進(jìn)行樣條軌跡規(guī)劃，通過(guò)拋物線函數(shù)擬合樣條曲線，采用等弦 長(zhǎng)的方法得到插補(bǔ)點(diǎn)的位置坐標(biāo)；
[0021] (2.4)依次將軌跡規(guī)劃后的位置坐標(biāo)作為機(jī)械臂期望的末端位置發(fā)送給控制器。 [0022]所述的步驟(3)具體為：
[0023] (3.1)采用基于位置的控制方法，控制內(nèi)環(huán)為位置控制環(huán)，外環(huán)使用阻抗控制器將 力偏差轉(zhuǎn)化為位置偏差；
[0024] (3.2)使用力傳感器測(cè)量機(jī)械臂末端與環(huán)境的實(shí)際接觸力，將期望接觸力與實(shí)際 接觸力的差值輸入阻抗控制器；
[0025] (3.3)將期望位置與阻抗控制器輸出的位置差同時(shí)輸入到位置控制內(nèi)環(huán)中，控制 機(jī)械臂同時(shí)滿足期望力與期望位置。
[0026]所述的步驟(4)具體為：
[0027] (4.1)將機(jī)械臂末端的期望力設(shè)為安全范圍內(nèi)的某一值Fo,并發(fā)送給阻抗控制器， 并控制其運(yùn)動(dòng)直至滿足期望力要求，此時(shí)機(jī)械臂末端與接觸面接觸；
[0028] (4.2)接觸力到達(dá)Fo后，把期望力置為0N，接觸力減小到0N時(shí)的位置即為接觸面的 位置；
[0029] (4.3)讀取此時(shí)機(jī)械臂的關(guān)節(jié)角，通過(guò)機(jī)械臂運(yùn)動(dòng)學(xué)方程計(jì)算機(jī)械臂末端的位置， 即為接觸面在笛卡爾空間的位置。
[0030] 所述的步驟(5)具體為：
[0031] (5.1)書寫漢字時(shí)的期望力根據(jù)實(shí)際需要設(shè)定在安全范圍內(nèi)，并保證接觸力滿足 精度要求；
[0032] (5.2)將軌跡規(guī)劃后的位置(1，7,0)與接觸面的位置(0,0,2)作為期望位置(^ 7， z)，發(fā)送給位置控制器；
[0033] (5.3)控制機(jī)械臂書寫漢字，同時(shí)滿足期望位置與期望力，并且接觸力保持恒定。
[0034]本發(fā)明的有益效果在于：
[0035]本發(fā)明使用六自由度機(jī)械臂完成力控制下的漢字書寫任務(wù)，對(duì)工業(yè)機(jī)械臂具有通 用性，可廣泛應(yīng)用于實(shí)踐教學(xué)、科技展覽中，也可以用于工業(yè)領(lǐng)域完成噴涂、雕刻等任務(wù)，可 以在脆性物體上書寫，保證了書寫的精確性與安全性。
【附圖說(shuō)明】
[0036] 圖1是本發(fā)明基于六自由度機(jī)械臂的漢字書寫系統(tǒng)的結(jié)構(gòu)框圖；
[0037] 圖2是本發(fā)明中漢字提取與軌跡規(guī)劃流程圖；
[0038] 圖3是本發(fā)明中機(jī)械臂的阻抗控制方法結(jié)構(gòu)圖。
【具體實(shí)施方式】
[0039 ]下面結(jié)合附圖對(duì)本發(fā)明進(jìn)行詳細(xì)說(shuō)明。
[0040] 本發(fā)明提供了一種具有力控制的六自由度機(jī)械臂標(biāo)準(zhǔn)漢字書寫系統(tǒng)。針對(duì)目前很 少有人將工業(yè)機(jī)械臂應(yīng)用在漢字書寫任務(wù)中，但在實(shí)際噴涂、雕刻等任務(wù)中需求廣泛等問(wèn) 題，設(shè)計(jì)了六自由度機(jī)械臂的標(biāo)準(zhǔn)漢字書寫系統(tǒng)；另外針對(duì)工業(yè)機(jī)械臂大多使用位置控制， 機(jī)械臂不具有柔順能力，控制精度低且需要人反復(fù)精確定位等問(wèn)題，設(shè)計(jì)了具有力控制的 漢字書寫系統(tǒng)。首先，基于Windows自帶的TTF矢量字庫(kù)提取漢字的輪廓信息，將漢字的像素 點(diǎn)坐標(biāo)轉(zhuǎn)化為機(jī)械臂笛卡爾空間的位置坐標(biāo)，對(duì)輪廓曲線進(jìn)行直線和樣條曲線軌跡規(guī)劃； 然后，設(shè)計(jì)基于位置的阻抗控制方法，控制機(jī)械臂運(yùn)動(dòng)并尋找接觸面的位置;最后，根據(jù)實(shí) 際需求將合適的期望力輸入阻抗控制器，并計(jì)算得到機(jī)械臂的期望位置，通過(guò)阻抗控制方 法控制機(jī)械臂書寫漢字，同時(shí)滿足期望力與期望位置的要求。本發(fā)明使用六自由度機(jī)械臂 完成力控制下的漢字書寫任務(wù)，對(duì)工業(yè)機(jī)械臂具有通用性，可廣泛應(yīng)用于實(shí)踐教學(xué)、科技展 覽中，也可以用于工業(yè)領(lǐng)域完成噴涂、雕刻等任務(wù)，可以在脆性物體上書寫，保證了書寫的 精確性與安全性。
[0041] 本發(fā)明采用了以下技術(shù)方案：
[0042] 具有力控制的六自由度機(jī)械臂的標(biāo)準(zhǔn)漢字書寫系統(tǒng)，包括漢字輪廓信息提取、機(jī) 械臂末端軌跡規(guī)劃、阻抗控制方法設(shè)計(jì)、接觸面位置尋找和書寫過(guò)程的力控制。其中：
[0043] (1)基于Windows自帶的TTF字庫(kù)提取漢字的輪廓點(diǎn)位置信息，并把輪廓曲線分為 直線和樣條曲線；
[0044] (2)將提取出的漢字輪廓位置坐標(biāo)轉(zhuǎn)化為機(jī)械臂末端執(zhí)行器笛卡爾空間的x和y坐 標(biāo)，分別對(duì)直線和樣條曲線進(jìn)行機(jī)械臂末端的軌跡規(guī)劃，保證漢字的書寫精確度；
[0045] (3)設(shè)計(jì)機(jī)械臂的阻抗控制方法，對(duì)機(jī)械臂末端的位置和力同時(shí)進(jìn)行控制；
[0046] (4)設(shè)計(jì)六自由度機(jī)械臂尋找接觸面的策略，計(jì)算接觸面在笛卡爾空間的位置；
[0047] (5)在力控制下書寫漢字，確保在恒定的力控制下完成漢字書寫任務(wù)。
[0048]在一些實(shí)施方式中，漢字輪廓信息提取具體為：
[0049] (1)基于Windows自帶的TTF字庫(kù)，調(diào)用CFont類的CreateFontO函數(shù)設(shè)置待書寫漢 字的高度、寬度及字體；
[0050] (2)每個(gè)漢字包含若干條閉合輪廓，將其分為直線和樣條曲線，使用 GetGlyph0utline()函數(shù)提取漢字的輪廓信息；
[0051 ] (3)創(chuàng)建文本文件，將輪廓位置坐標(biāo)存入文本文件中。
[0052]在一些實(shí)施方式中，機(jī)械臂末端的軌跡規(guī)劃具體為：
[0053] (1)讀取文本文件中的輪廓曲線位置坐標(biāo)，將讀取的平面坐標(biāo)值分別轉(zhuǎn)化為機(jī)械 臂末端執(zhí)行器在笛卡爾空間的X和y坐標(biāo)；
[0054] (2)對(duì)直線輪廓線進(jìn)行直線軌跡規(guī)劃，通過(guò)歸一化參數(shù)確定起點(diǎn)與終點(diǎn)間中間點(diǎn) 的坐標(biāo)，歸一化參數(shù)采用拋物線過(guò)渡的線性函數(shù)，保證機(jī)械臂末端在書寫漢字時(shí)速度連續(xù)；
[0055] (3)對(duì)樣條輪廓線進(jìn)行樣條軌跡規(guī)劃，通過(guò)拋物線函數(shù)擬合樣條曲線，采用等弦長(zhǎng) 的方法得到插補(bǔ)點(diǎn)的位置坐標(biāo)；
[0056] (4)依次將軌跡規(guī)劃后的位置坐標(biāo)作為機(jī)械臂期望的末端位置發(fā)送給控制器。
[0057]在一些實(shí)施方式中，阻抗控制方法設(shè)計(jì)具體為：
[0058] (1)采用基于位置的控制方法，控制內(nèi)環(huán)為位置控制環(huán)，外環(huán)使用阻抗控制器將力 偏差轉(zhuǎn)化為位置偏差；
[0059] (2)使用力傳感器測(cè)量機(jī)械臂末端與環(huán)境的實(shí)際接觸力，將期望接觸力與實(shí)際接 觸力的差值輸入阻抗控制器；
[0060] (3)將期望位置與阻抗控制器輸出的位置差同時(shí)輸入到位置控制內(nèi)環(huán)中，控制機(jī) 械臂同時(shí)滿足期望力與期望位置。
[0061 ]在一些實(shí)施方式中，計(jì)算接觸面在笛卡爾空間的位置具體為：
[0062] (1)將機(jī)械臂末端的期望力設(shè)為10N并發(fā)送給阻抗控制器，并控制其運(yùn)動(dòng)直至滿足 期望力要求，此時(shí)機(jī)械臂末端與接觸面接觸；
[0063] (2)接觸力到達(dá)10N后，把期望力置為0N，接觸力減小到0N時(shí)的位置即為接觸面的 位置；
[0064] (3)讀取此時(shí)機(jī)械臂的關(guān)節(jié)角，通過(guò)機(jī)械臂運(yùn)動(dòng)學(xué)方程計(jì)算機(jī)械臂末端的位置，即 為接觸面在笛卡爾空間的位置。
[0065]在一些實(shí)施方式中，在力控制下書寫漢字具體為：
[0066] (1)書寫漢字時(shí)的期望力根據(jù)實(shí)際需要設(shè)定在安全范圍內(nèi)，并保證接觸力滿足精 度要求；
[0067] (2)將軌跡規(guī)劃后的位置(x，y，0)與接觸面的位置(0,0，z)作為期望位置(x，y，z)， 發(fā)送給位置控制器；
[0068] (3)控制機(jī)械臂書寫漢字，同時(shí)滿足期望位置與期望力，保證書寫漢字時(shí)字跡清晰 且運(yùn)動(dòng)連續(xù)，并且接觸力保持恒定。
[0069] (1)漢字輪廓信息提取
[0070]基于Windows自帶的TTF矢量字庫(kù)提取漢字的輪廓:每個(gè)TTF字符是由多個(gè)輪廓組 成的，每條輪廓的信息存儲(chǔ)在一個(gè)TTP0LYG0NHEADER和多個(gè)TTP0LYCURVE數(shù)據(jù)結(jié)構(gòu)中，輪廓 信息包括輪廓字節(jié)數(shù)、輪廓類型、輪廓起始點(diǎn)及像素點(diǎn)數(shù)據(jù)，其中輪廓類型分為直線和樣條 曲線。
[0071] 參見(jiàn)圖2,調(diào)用CFont類的CreateFont函數(shù)創(chuàng)建邏輯字體，其中，參數(shù)nHeight和 nWidth分別指定字符的高度和寬度，IpszFace指定字體的字樣名；將創(chuàng)建的字體置于設(shè)備 上下文中，調(diào)用GetGlyphOutline函數(shù)獲得字符的輪廓信息，參數(shù)lpvBuffer指向存放點(diǎn)的 位置信息的緩沖區(qū)；遍歷漢字的所有輪廓線上的像素點(diǎn)，并將提取的P0INTFX類型的位置坐 標(biāo)保存在數(shù)組中；創(chuàng)建文本文件，將像素點(diǎn)的位置坐標(biāo)存入文本文件中，并對(duì)每條輪廓線的 類型及起點(diǎn)與終點(diǎn)進(jìn)行標(biāo)記。
[0072] (1)機(jī)械臂末端軌跡規(guī)劃
[0073]參見(jiàn)圖2,讀取文本文件中的像素點(diǎn)位置坐標(biāo)Q(x，y)，將其轉(zhuǎn)化為機(jī)械臂笛卡爾空 間的位置坐標(biāo)P(X，y)，轉(zhuǎn)換公式為： \P{x) = ? (x) x ff - Q{x) x R + Max{x)
[0074] {
[^(y) = Q](y) x R - Q(y) y~ R + Max(y)
[0075] 其中，Qi(x，y)為起始點(diǎn)的像素坐標(biāo)，Max(x，y)為末端位置的上限位，R為轉(zhuǎn)換系 數(shù)。
[0076] 對(duì)直線輪廓線進(jìn)行直線軌跡規(guī)劃，在輪廓線起點(diǎn)（XI，yi)與終點(diǎn)（X2，y2)間插入多 個(gè)中間點(diǎn)，中間點(diǎn)的位置坐標(biāo)(x，y)為： \x = x, + IAjt
[0077] "
[7 = ^ + AAr
[0078] 其中，（AX，Ay)為位置增量，A是歸一化參數(shù)，采用拋物線過(guò)渡的線性函數(shù)，在起 點(diǎn)與終點(diǎn)的鄰域使用一段拋物線型的緩沖區(qū)，保持加速度不變，使期望軌跡的位置和速度 均保持連續(xù)。
[0079] 假設(shè)機(jī)械臂在直線段中的運(yùn)動(dòng)速度為V，拋物線段部分的加速度為a，則拋物線段 的時(shí)間Tb和位移Lb為：
[0081] 待插補(bǔ)直線的位移L與時(shí)間T為：
[0084]將拋物線段的時(shí)間、位移和加速度歸一化：
[0086]得到歸一化參數(shù)A關(guān)于時(shí)間t的分段離散函數(shù)：
[0088] 其中，t = i/N(i = 0,1，…，N)，N為插值點(diǎn)的個(gè)數(shù);0彡入彡1，當(dāng)位于起點(diǎn)時(shí)，A = 〇;位 于終點(diǎn)時(shí)，X=l。
[0089 ]根據(jù)歸一化參數(shù)計(jì)算直線軌跡規(guī)劃的中間點(diǎn)的位置坐標(biāo)。
[0090] 對(duì)樣條曲線進(jìn)行樣條軌跡規(guī)劃，假設(shè)已知樣條曲線上三個(gè)點(diǎn)的坐標(biāo)，依次設(shè)為A (x，y)，B(x，y)，C(x，y)，插值點(diǎn)的坐標(biāo)為P(x，y)。根據(jù)三切線定理構(gòu)造拋物線，插值點(diǎn)P位于 從A到C的拋物線上，計(jì)算公式為：
[0091] P(t) = (l-t)2A+2t(l-t)B+t2C
[0092] 其中，0<t<l，當(dāng)位于點(diǎn)A時(shí)，t = 0;位于點(diǎn)C時(shí)，t = l。
[0093] 設(shè)點(diǎn)M為線段AC的中點(diǎn)，當(dāng)t變化A t時(shí)，前后兩點(diǎn)P的弦長(zhǎng)A P⑴為：
[0094] Ap(t) = 2At(2tM + AB)
[0095] 采用等弦長(zhǎng)的方法計(jì)算插值點(diǎn)的坐標(biāo)，即AP(t)=S，S為定值，則At的值為：
[0096] At = 5/(2 2tBM + j4B)
[0097]將t = t+A t代入拋物線方程中，得到插值點(diǎn)的坐標(biāo)P(x，y)。
[0098] (2)阻抗控制方法設(shè)計(jì)
[0099] 參見(jiàn)圖3,為了同時(shí)控制接觸力與接觸位置，采用基于位置的阻抗控制方法，由位 置控制內(nèi)環(huán)和阻抗控制外環(huán)組成。使用傳感器檢測(cè)機(jī)械臂與環(huán)境的實(shí)際接觸力，將期望力 與實(shí)際力的偏差輸入阻抗控制器中，轉(zhuǎn)化力控制方向上的位置偏差，將力控制方向上的位 置偏差與期望位置使用機(jī)械臂逆運(yùn)動(dòng)學(xué)算法轉(zhuǎn)化為期望關(guān)節(jié)角，輸入位置控制內(nèi)環(huán)，位置 控制內(nèi)環(huán)中的位置控制器使用PID控制。
[0100] 理想阻抗控制的數(shù)學(xué)模型為：
[0101] Md[_x(t) - + + Kd[x(t) - Jrf(0] = Fit)
[0102] 其中，Md、Bd和Kd分別為阻抗模型的期望慣性矩陣、期望阻尼矩陣和期望剛度矩陣， 為半正定對(duì)角陣;x(t)、和1〇)分別為實(shí)際位置、速度和加速度;Xd(t)、尤⑷和<〇)分 別為期望位置、速度和加速度;E(t)=F(t)-Fd(t)為實(shí)際接觸力與期望接觸力的差值。
[0103] 對(duì)阻抗控制的數(shù)學(xué)模型進(jìn)行拉氏變換，得到期望的阻抗特性：
[0105] 其中，5X(S)=X(S)-Xd(s)為機(jī)械臂實(shí)際位置與期望位置的差值。
[0106] (3)接觸面位置尋找
[0107] 書寫漢字時(shí)的期望位置坐標(biāo)(x，y)已知，需要得到接觸面在笛卡爾空間的位置:將 阻抗控制的期望力置為10N，控制機(jī)械臂運(yùn)動(dòng)，當(dāng)使用力傳感器檢測(cè)到的實(shí)際接觸力滿足要 求后，機(jī)械臂停止運(yùn)動(dòng)，此時(shí)機(jī)械臂末端與接觸面接觸;將期望力置為0N輸入阻抗控制器， 機(jī)械臂向減小接觸力的方向運(yùn)動(dòng)，直到接觸力減小為0N停止運(yùn)動(dòng)，此時(shí)機(jī)械臂末端執(zhí)行器 的位置即為接觸面的位置。
[0108] 通過(guò)編碼器讀取機(jī)械臂的六個(gè)軸的關(guān)節(jié)角（01，02，03，04，0 5，06)，根據(jù)機(jī)械臂的正 運(yùn)動(dòng)學(xué)方程計(jì)算此時(shí)機(jī)械臂末端執(zhí)行器的位置(10,70,2())，則接觸點(diǎn)的位置坐標(biāo)為(1()， 70， Z0) 〇
[0109] (4)力控制下書寫漢字
[0110] 由于不同接觸物體的剛度不同，在機(jī)械臂書寫漢字時(shí)需要的力也不同，根據(jù)實(shí)際 書寫環(huán)境的需求向阻抗控制器輸入合適的期望力，控制機(jī)械臂書寫漢字時(shí)的接觸力滿足期 望要求。
[0111]根據(jù)軌跡規(guī)劃后的位置坐標(biāo)(x，y)與接觸面位置(XQ，yQ，z())得到書寫漢字時(shí)的期 望位置(X，y，ZQ)，控制機(jī)械臂運(yùn)動(dòng)到期望位置后發(fā)送下一個(gè)位置坐標(biāo)，在完成一條輪廓曲 線書寫后回到本條輪廓線的起點(diǎn)處，保證輪廓曲線閉合;完成一條曲線繪制后控制機(jī)械臂 離開(kāi)接觸面一段距離，并運(yùn)動(dòng)到下一條輪廓線的起點(diǎn)位置，然后控制機(jī)械臂與接觸面接觸， 繪制輪廓曲線;依次執(zhí)行，直到完成所有漢字的繪制。
【主權(quán)項(xiàng)】
1. 一種具有力控制的六自由度機(jī)械臂標(biāo)準(zhǔn)漢字書寫方法，包括漢字輪廓信息提取、機(jī) 械臂末端軌跡規(guī)劃、阻抗控制方法設(shè)計(jì)、接觸面位置尋找和書寫過(guò)程的力控制，其特征在 于： (1) 基于Windows自帶的TTF字庫(kù)提取漢字的輪廓點(diǎn)位置信息，并把輪廓曲線分為直線 和樣條曲線； (2) 將提取出的漢字輪廓位置坐標(biāo)轉(zhuǎn)化為機(jī)械臂末端執(zhí)行器笛卡爾空間的X和y坐標(biāo)， 分別對(duì)直線和樣條曲線進(jìn)行機(jī)械臂末端的軌跡規(guī)劃； (3) 設(shè)計(jì)機(jī)械臂的阻抗控制方法，對(duì)機(jī)械臂末端的位置和力同時(shí)進(jìn)行控制； (4) 設(shè)計(jì)六自由度機(jī)械臂尋找接觸面的策略，計(jì)算接觸面在笛卡爾空間的位置； (5) 在力控制下書寫漢字； 所述的步驟(1)具體為： (1 · 1)基于Windows自帶的TTF字庫(kù)，調(diào)用CFont類的CreateFont ()函數(shù)設(shè)置待書寫漢字 的高度、寬度及字體； (1.2) 每個(gè)漢字包含若干條閉合輪廓，將其分為直線和樣條曲線，使用GetGlyphOutl ine()函數(shù)提取漢字的輪廓信息； (1.3) 創(chuàng)建文本文件，將輪廓位置坐標(biāo)存入文本文件中。2. 根據(jù)權(quán)利要求1所述的一種具有力控制的六自由度機(jī)械臂標(biāo)準(zhǔn)漢字書寫方法，其特 征在于： 所述的步驟(2)具體為： (2.1) 讀取文本文件中的輪廓曲線位置坐標(biāo)，將讀取的平面坐標(biāo)值分別轉(zhuǎn)化為機(jī)械臂 末端執(zhí)行器在笛卡爾空間的X和y坐標(biāo)； (2.2) 對(duì)直線輪廓線進(jìn)行直線軌跡規(guī)劃，通過(guò)歸一化參數(shù)確定起點(diǎn)與終點(diǎn)間中間點(diǎn)的 坐標(biāo)，歸一化參數(shù)采用拋物線過(guò)渡的線性函數(shù)，保證機(jī)械臂末端在書寫漢字時(shí)速度連續(xù)； (2.3) 對(duì)樣條輪廓線進(jìn)行樣條軌跡規(guī)劃，通過(guò)拋物線函數(shù)擬合樣條曲線，采用等弦長(zhǎng)的 方法得到插補(bǔ)點(diǎn)的位置坐標(biāo)； (2.4) 依次將軌跡規(guī)劃后的位置坐標(biāo)作為機(jī)械臂期望的末端位置發(fā)送給控制器。3. 根據(jù)權(quán)利要求1所述的一種具有力控制的六自由度機(jī)械臂標(biāo)準(zhǔn)漢字書寫方法，其特 征在于： 所述的步驟(3)具體為： (3.1) 采用基于位置的控制方法，控制內(nèi)環(huán)為位置控制環(huán)，外環(huán)使用阻抗控制器將力偏 差轉(zhuǎn)化為位置偏差； (3.2) 使用力傳感器測(cè)量機(jī)械臂末端與環(huán)境的實(shí)際接觸力，將期望接觸力與實(shí)際接觸 力的差值輸入阻抗控制器； (3.3) 將期望位置與阻抗控制器輸出的位置差同時(shí)輸入到位置控制內(nèi)環(huán)中，控制機(jī)械 臂同時(shí)滿足期望力與期望位置。4. 根據(jù)權(quán)利要求1所述的一種具有力控制的六自由度機(jī)械臂標(biāo)準(zhǔn)漢字書寫方法，其特 征在于： 所述的步驟(4)具體為： (4.1)將機(jī)械臂末端的期望力設(shè)為安全范圍內(nèi)的某一值F〇,并發(fā)送給阻抗控制器，并控 制其運(yùn)動(dòng)直至滿足期望力要求，此時(shí)機(jī)械臂末端與接觸面接觸； (4.2) 接觸力到達(dá)Fo后，把期望力置為0N，接觸力減小到ON時(shí)的位置即為接觸面的位置； (4.3) 讀取此時(shí)機(jī)械臂的關(guān)節(jié)角，通過(guò)機(jī)械臂運(yùn)動(dòng)學(xué)方程計(jì)算機(jī)械臂末端的位置，即為 接觸面在笛卡爾空間的位置。5.根據(jù)權(quán)利要求1所述的一種具有力控制的六自由度機(jī)械臂標(biāo)準(zhǔn)漢字書寫方法，其特 征在于： 所述的步驟(5)具體為： (5.1) 書寫漢字時(shí)的期望力根據(jù)實(shí)際需要設(shè)定在安全范圍內(nèi)，并保證接觸力滿足精度 要求； (5.2) 將軌跡規(guī)劃后的位置(1，7,0)與接觸面的位置(0,0,2)作為期望位置(1，7, 2)，發(fā) 送給位置控制器； (5.3) 控制機(jī)械臂書寫漢字，同時(shí)滿足期望位置與期望力，并且接觸力保持恒定。
【文檔編號(hào)】B25J9/16GK106003033SQ201610423392
【公開(kāi)日】2016年10月12日
【申請(qǐng)日】2016年6月16日
【發(fā)明人】朱齊丹, 謝心如, 夏桂華, 李超, 蔡成濤, 張智, 呂曉龍, 劉志林
【申請(qǐng)人】哈爾濱工程大學(xué)