一種基于位置速度控制的遙操作機(jī)器人碰撞預(yù)警方法
【專利摘要】本發(fā)明提供一種基于位置速度控制的遙操作機(jī)器人碰撞預(yù)警方法，其步驟包括：首先將主手控制器視為積分器，通過位置積分計(jì)算出對(duì)機(jī)器人位置的期望值，實(shí)現(xiàn)機(jī)器人任務(wù)速度與手控器位置的有效對(duì)應(yīng)，然后根據(jù)手控器位置信號(hào)主動(dòng)預(yù)測(cè)出未來時(shí)刻機(jī)器人的位置，當(dāng)機(jī)器人與環(huán)境臨近碰撞狀態(tài)時(shí)生成預(yù)警力，最后將反饋力與操作者手動(dòng)控制力進(jìn)行融合，引導(dǎo)操作者控制機(jī)器人避開障礙物，完成作業(yè)過程。本發(fā)明可將人類智能決策與機(jī)器智能相結(jié)合，有效提高機(jī)器人的作業(yè)精度與效率，減輕操作者的作業(yè)負(fù)擔(dān)，降低對(duì)操作者技術(shù)熟練度的依賴，避免系統(tǒng)延時(shí)導(dǎo)致對(duì)機(jī)器人控制的盲目性。
【專利說明】
一種基于位置速度控制的遙操作機(jī)器人碰撞預(yù)警方法
技術(shù)領(lǐng)域
[0001] 本發(fā)明屬于機(jī)器人人機(jī)交互及機(jī)器人遙操作領(lǐng)域，特別涉及一種基于位置速度控 制的遙操作機(jī)器人碰撞預(yù)警方法。
【背景技術(shù)】
[0002] 在危及人身安全的極限環(huán)境或危險(xiǎn)環(huán)境下作業(yè)，例地震災(zāi)后或核事故現(xiàn)場(chǎng)清理救 援、海洋探索、宇宙空間開發(fā)和利用等，機(jī)器人作為代替人類進(jìn)行作業(yè)的工具，發(fā)揮越來越 重要的作用，因而遙操作技術(shù)得到充分發(fā)展與運(yùn)用。但是，由于受機(jī)構(gòu)、控制、人工智能、傳 感和材料技術(shù)等發(fā)展水平的制約，自主作業(yè)機(jī)器人的開發(fā)在短期內(nèi)無法實(shí)現(xiàn)。機(jī)器人控制 中通常所采用的操作方法是直接控制法，而機(jī)器人控制領(lǐng)域的操作性及安全性目前還存在 諸多問題：
[0003] 首先，遙操作系統(tǒng)存在著不可避免的延時(shí)問題，這極易導(dǎo)致操控不當(dāng)而帶來作業(yè) 對(duì)象的損毀?，F(xiàn)有的機(jī)器人控制方法中，機(jī)器人的位置控制方式屬于被動(dòng)方式，即機(jī)器人只 是順應(yīng)跟隨手控器的位置進(jìn)行作業(yè)，操作者并不能主動(dòng)預(yù)測(cè)出機(jī)器人未來某一時(shí)刻的姿 態(tài)。由于數(shù)據(jù)傳輸量以及傳輸距離等問題，遙操作系統(tǒng)存在著不可避免的延時(shí)問題，這將導(dǎo) 致作業(yè)過程中極易操控不當(dāng)，造成無法彌補(bǔ)的損失。其次，在遠(yuǎn)程操作系統(tǒng)中，手控器的種 類繁多，通用性差 [W]。不僅不同領(lǐng)域的手控器結(jié)構(gòu)各不相同，即使同一領(lǐng)域的手控器結(jié)構(gòu) 也存在差異。再次，手控器有效活動(dòng)空間同機(jī)器人的活動(dòng)空間存在差異，這將導(dǎo)致高精度作 業(yè)不易實(shí)現(xiàn)，操作者工作負(fù)擔(dān)增加 [8]。當(dāng)手控器的活動(dòng)空間范圍遠(yuǎn)遠(yuǎn)小于機(jī)器人的活動(dòng)范 圍時(shí)，操作者對(duì)于手控器的控制精度難以掌握，機(jī)器人便難以進(jìn)行高精度作業(yè)。最后，操作 者的技能水平和熟練程度極大地影響著機(jī)器人作業(yè)效率的高低。手控器同機(jī)器人在結(jié)構(gòu)、 尺寸、有效活動(dòng)空間范圍等方面通常存在很大差異，操作者在操控機(jī)器人過程中，特別是精 細(xì)作業(yè)任務(wù)中，往往需要反復(fù)調(diào)整機(jī)器人的姿態(tài)，才可以完成作業(yè)任務(wù)+ 11]，這無疑會(huì)導(dǎo)致 機(jī)器人作業(yè)效率不高。
[0004] [1]Adelstein B.D.,Rosen M.U.J..Design and implementation of a force reflecting manipulandum for manual control research.Advances in Robotics，ASME DSC，1992,5(3):卜12.
[0005] [2]Howe R.D.,Force-reflecting teleoperated hand system for the study of tactile sensing in precision manipulation.Proceedings of the IEEE International Conference on Robotics and Automation，1992,1321-1326.
[0006] [3]Massie T，Salisbury K.The phantom haptic interface:a device for probing virtual objects.ASME Journal of Dynamic Systems，Measurement，and Control，1994，116:295-299.
[0007] [4]Berkelman PJ.,Hollis RL?，Salcudean SE..Interacting with virtual environments using a magnetic levitation haptic interface.Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems，1995，117- 122.
[0008] [5]Ellis RE,Ismaei1 0M，Lipsett MG.Design and evaluation of high-performance haptic interface.Robotica,1996,14:321-327.
[0009] [6]Chiaverini S. .Singularity-robust task-priority redundancy resolution for real-time kinematic control of robot manipulators.IEEE Transaction on Robotic and Automation,1997,13(3):398-410.
[0010] [7]Jin B.D.,Ki-Young,Kwon D.-S..A 6D0F force-reflecting master hand controller using the fivebar parallel mechanism.Journal of Control,Automation and System Engineers,1999,5(3):312-317.
[0011] [8]寧祎.手控器的性能和評(píng)價(jià)方法.機(jī)器人，2000,22(4):282-288
[0012] [9]鄧樂，趙丁選，倪濤等.基于Stewart平臺(tái)的6自由度力反饋手控器.農(nóng)業(yè)機(jī)械學(xué) 報(bào)，2005,36(7):118-121
[0013] [10]倪濤，趙丁選，張紅彥.電液手控器型遙操作機(jī)器人力反饋控制策略.農(nóng)業(yè)機(jī) 械學(xué)報(bào)，2010,41(12): 190-194
[0014] [11]Huang Lingtao,Kawamura Takuya，Hirinao Yamada.Application of a position-force control method in a master-slave teleoperation construction robot system.Applied Mechanics and Materials,2012,229-231：2243-2247.

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

[0015] 本發(fā)明針對(duì)遙操作機(jī)器人控制領(lǐng)域中的操作性及安全性所存在的問題，提供一種 基于位置速度控制的遙操作機(jī)器人碰撞預(yù)警方法，具體步驟如下：
[0016] S1、基于手控器位置速度控制方法，以手控器的位置信號(hào)控制機(jī)器人運(yùn)動(dòng)；
[0017] S2、通過手控器的位置信號(hào)對(duì)遙操作機(jī)器人碰撞預(yù)警進(jìn)行建模，構(gòu)建機(jī)器人接近 目標(biāo)對(duì)象的預(yù)警反力；
[0018] S3、將S1中操作者的操作力矩與S2中的接觸反饋力矩和預(yù)警反饋力矩進(jìn)行融合， 對(duì)手控器和機(jī)器人的運(yùn)動(dòng)進(jìn)行控制，實(shí)現(xiàn)機(jī)器智能與人類智能共同作用下控制機(jī)器人接近 目標(biāo)對(duì)象，完成作業(yè)過程。
[0019] 所述步驟S1包括以下步驟：
[0020] SI. 1、搭建基于搖桿式手控器控制機(jī)器人運(yùn)動(dòng)的系統(tǒng)控制平臺(tái)；
[0021] S1.2、計(jì)算機(jī)采集人手力及手控器位置信號(hào)，傳送數(shù)據(jù)為系統(tǒng)控制手控器的運(yùn)動(dòng) 做準(zhǔn)備；
[0022] S1.3、通過將手控器視作為積分器，根據(jù)公式
[0023] < ~ +qs qm ^ 0 l q：：l=q： f：=〇
[0024] 計(jì)算手控器期望的機(jī)器人關(guān)節(jié)角控制信號(hào)qsd。式中，kms為手控器與機(jī)器人關(guān)節(jié)角 比例因子，t sam為系統(tǒng)的采樣周期，f；：為n時(shí)刻手控器關(guān)節(jié)角，^/+1為t+1+l時(shí)刻手控器期望 的機(jī)器人關(guān)節(jié)角，k ms為手控器按鈕修改比例因子。
[0025] S1.4、為避免積分引起控制量的過飽和及消除積分誤差，輸入信號(hào)為零時(shí)，設(shè)定 為上一采樣時(shí)刻機(jī)器人關(guān)節(jié)角fsH。
[0026] S1.5、按照公式
[0027] xm=JTFm
[0028]計(jì)算在操縱力Fm下手控器的操作力矩
[0029] S1.6、將手控器的運(yùn)動(dòng)簡(jiǎn)化為質(zhì)量-阻尼-彈簧系統(tǒng)，手控器動(dòng)力學(xué)模型表示為公 式
[0030] Mmqm + Bmqm + Kmqm = kxTm ,
[0031] 式中，F(xiàn)m為手控器操縱力，J為手控器的雅可比矩陣，Mm、BdPK m為手控器的慣性系 數(shù)矩陣，阻尼系數(shù)矩陣和彈簧系數(shù)矩陣，h為操作者手動(dòng)控制的權(quán)值系數(shù)。
[0032] 所述步驟S2包括以下步驟：
[0033] S2.1、根據(jù)公式
[0034] q[t + ^
[0035] 預(yù)測(cè)t秒后機(jī)器人關(guān)節(jié)角位置<。式中，咖、辦為手控器及機(jī)器人當(dāng)前時(shí)刻關(guān)節(jié)角。
[0036] S2.2、通過立體視覺技術(shù)獲取作業(yè)場(chǎng)景圖像數(shù)據(jù)并實(shí)時(shí)重構(gòu)作業(yè)對(duì)象環(huán)境。
[0037] S2.3、結(jié)合環(huán)境相機(jī)測(cè)得的背景環(huán)境(或障礙物）的高度信息，及預(yù)測(cè)得到的機(jī)器 人末端執(zhí)行器位置，求解機(jī)器人末端沿速度方向與背景環(huán)境的距離d。
[0038] S2.4、依據(jù)公式
[0039] FtJk'^-d)v d<(!''
[0 d>d0
[0040]計(jì)算機(jī)器人所受的碰撞預(yù)警力Ft，式中，kt為比例因子，do為機(jī)器人末端與障礙物 間的安全距離，^為機(jī)器人末端運(yùn)動(dòng)速度的單位向量。
[0041] S2.5、依據(jù)公式
[0043]計(jì)算機(jī)器人與環(huán)境間的接觸反饋力矩^和預(yù)警反饋力矩式中，F(xiàn)e為機(jī)器人末端 與環(huán)境間的作用力，Mt、Me為力矩向量，R為機(jī)器人末端執(zhí)行器坐標(biāo)系相對(duì)手控器坐標(biāo)系的旋 轉(zhuǎn)矩陣。
[0044] 所述步驟S3包括以下步驟：
[0045] S3.1、按照公式
[0046] +BJmd +Kmqm =kf.m -kjz.+z^
[0047] 設(shè)計(jì)手控器的位置預(yù)測(cè)控制器。式中，1?為預(yù)警反饋力矩的權(quán)值系數(shù)。h為手控器 在操縱桿的操作力矩、Tr為機(jī)器人與環(huán)境的接觸力矩，^為預(yù)警反饋力矩，q md為手控器的運(yùn) 動(dòng)趨勢(shì)。
[0048] S3.2、按照公式
[0049] um - kmd(qmd -qm) + kmp(q,lld -qm)
[0050] 設(shè)計(jì)手控器的控制量um，使手控器跟隨反饋力矩輸出的運(yùn)動(dòng)趨勢(shì)。式中，k_、k md為 手控器的比例增益和微分增益。
[0051] S3.3、依據(jù)公式
[0052] <： qd - q) + {ijd - a J + Ktl {q(i -q,) = F,
[0053]設(shè)計(jì)機(jī)器人阻抗模型，式中，Md、Bd和Kd分別為期望阻抗模型的慣性矩陣、阻尼矩陣 和剛度矩陣，qd和qs分別為機(jī)器人參考關(guān)節(jié)角和實(shí)際關(guān)節(jié)角。
[0054] S3.4、機(jī)器人與環(huán)境間的作用力矩滿足公式
[0055] MAq + B!：IM} + = Fr,
[0056] S3.5、按公式
[0058] 計(jì)算作用力Fe經(jīng)過阻抗模型后，機(jī)器人關(guān)節(jié)控制的軌跡修正量犯（At從.，
[0059] S3.6、依據(jù)公式
[0060] Q〇 = Qd-AQ
[0061 ]計(jì)算經(jīng)過修正的機(jī)器人關(guān)節(jié)控制指令，式中，& = 。
[0062] S3.7、根據(jù)公式
[0063] w.' =々,.+ -么）十夂,,(義一<：/、）
[0064] 設(shè)計(jì)經(jīng)阻抗修正的機(jī)器人控制量Us，實(shí)現(xiàn)機(jī)器人對(duì)手控器的跟隨。
[0065] S3.8、機(jī)器人與環(huán)境(或障礙物)無接觸情況時(shí)，依據(jù)公式
[0066] us =qm +kjqin -qj + ksp(cL, -qj
[0067] 設(shè)計(jì)機(jī)器人的控制量us，實(shí)現(xiàn)機(jī)器人對(duì)手控器的跟隨。式中，ksp、ksd為機(jī)器人控制 器的比例增益和微分增益。
[0068]本發(fā)明相對(duì)于現(xiàn)有技術(shù)具有如下的優(yōu)點(diǎn)及技術(shù)效果：
[0069] 1.提出基于位置積分的位置速度控制策略，采用位置增量式的控制，改變傳統(tǒng)手 控器對(duì)機(jī)器人位置的點(diǎn)對(duì)點(diǎn)操作方式；
[0070] 2.降低作業(yè)效率對(duì)操作者技術(shù)熟練度的依賴；
[0071 ] 3.能夠?qū)C(jī)器人未來時(shí)刻的姿態(tài)進(jìn)行預(yù)測(cè)，在機(jī)器人與周邊環(huán)境發(fā)生碰撞前，生 成防碰撞預(yù)警力，避免由延時(shí)引起的誤操作，改善操作系統(tǒng)的安全性；
[0072] 4.為手控器的開發(fā)提供一種新的思路，為通用型手控器的標(biāo)準(zhǔn)化設(shè)計(jì)奠定基礎(chǔ)。
【附圖說明】
[0073] 圖1為位置速度控制技術(shù)路線；
[0074]圖2為碰撞預(yù)警力建模技術(shù)路線。
【具體實(shí)施方式】
[0075]下面結(jié)合實(shí)施例及附圖對(duì)本發(fā)明作進(jìn)一步詳細(xì)的描述，圖1為位置速度控制技術(shù) 路線，圖2為碰撞預(yù)警力建模技術(shù)路線。
[0076]本發(fā)明根據(jù)基于位置速度控制的遙操作機(jī)器人碰撞預(yù)警方法，包括如下步驟S1、 S2、S3:
[0077] SI、基于手控器位置積分的方法，以手控器的位置信號(hào)控制機(jī)器人運(yùn)動(dòng)，所述步驟 S1 包括步驟 S1.1、S1.2、S1.3、S1.4、S1.5、S1.6:
[0078] SI. 1、搭建基于搖桿式手控器控制機(jī)器人運(yùn)動(dòng)的系統(tǒng)控制平臺(tái)；
[0079] S1.2、計(jì)算機(jī)采集人手力及手控器位置信號(hào)，傳送數(shù)據(jù)為系統(tǒng)控制手控器的運(yùn)動(dòng) 做準(zhǔn)備；
[0080] S1.3、通過將手控器視作為積分器，根據(jù)公式 r t+l
[0081 ] < = q;; :矣〇 Cl) 、 €jx = € =°
[0082] 計(jì)算手控器期望的機(jī)器人關(guān)節(jié)角控制信號(hào)qsd。式中，kms為手控器與機(jī)器人關(guān)節(jié)角 比例因子，t sam為系統(tǒng)的采樣周期，<為1!時(shí)刻手控器關(guān)節(jié)角，if1為t+1+l時(shí)刻手控器期望 的機(jī)器人關(guān)節(jié)角，k ms為手控器按鈕修改比例因子。
[0083] S1.4、為避免積分引起控制量的過飽和及消除積分誤差，輸入信號(hào)^為零時(shí)，設(shè)定 為上一采樣時(shí)刻機(jī)器人關(guān)節(jié)角?：。
[0084] S1.5、按照公式
[0085] xm=JTFm (2)
[0086]計(jì)算在操縱力Fm下手控器的操作力矩
[0087] S1.6、將手控器的運(yùn)動(dòng)簡(jiǎn)化為質(zhì)量-阻尼-彈簧系統(tǒng)，手控器動(dòng)力學(xué)模型表示為公 式
[0088] Mmqm +Bmqm +Kmqm = klTm , (3)
[0089] 式中，F(xiàn)m為手控器操縱力，J為手控器的雅可比矩陣，Mm、BdPK m為手控器的慣性系 數(shù)矩陣，阻尼系數(shù)矩陣和彈簧系數(shù)矩陣，h為操作者手動(dòng)控制的權(quán)值系數(shù)。
[0090] S2、根據(jù)手控器的位置信號(hào)對(duì)遙操作機(jī)器人碰撞預(yù)警進(jìn)行建模，構(gòu)建機(jī)器人接近 目標(biāo)對(duì)象的預(yù)警反力。所述步驟S2包括步驟S2.1、S2.2、S2.3、S2.4、S2.5:
[0091] S2.1、根據(jù)公式
[0092] (f * ^ 0) (4)
[0093] 預(yù)測(cè)t秒后機(jī)器人關(guān)節(jié)角位置<。式中，咖、辦為手控器及機(jī)器人當(dāng)前時(shí)刻關(guān)節(jié)角。
[0094] S2.2、通過立體視覺技術(shù)獲取作業(yè)場(chǎng)景圖像數(shù)據(jù)并實(shí)時(shí)重構(gòu)作業(yè)對(duì)象環(huán)境。
[0095] S2.3、結(jié)合環(huán)境相機(jī)測(cè)得的背景環(huán)境(或障礙物）的高度信息，及預(yù)測(cè)得到的機(jī)器 人末端執(zhí)行器位置，求解機(jī)器人末端沿速度方向與背景環(huán)境的距離d。
[0096] S2.4、依據(jù)公式 123 Ft = \k,{dn-d)v d<d0 (5)
[0 d > clL) 2
[0098]計(jì)算機(jī)器人所受的碰撞預(yù)警力Ft，式中，kt為比例因子，do為機(jī)器人末端與障礙物 間的安全距離，^為機(jī)器人末端運(yùn)動(dòng)速度的單位向量。 3 S2.5、依據(jù)公式 _ n
[0100] =-JT' C6) U」 RFe L艦」
[0101] 計(jì)算機(jī)器人與環(huán)境間的接觸反饋力矩Tr和預(yù)警反饋力矩Tt。式中，F(xiàn)e為機(jī)器人末端 與環(huán)境間的作用力，M t、Me為力矩向量，R為機(jī)器人末端執(zhí)行器坐標(biāo)系相對(duì)手控器坐標(biāo)系的旋 轉(zhuǎn)矩陣。
[0102] S3、將S1中操作者的操作力矩與S2中的接觸反饋力矩和預(yù)警反饋力矩進(jìn)行融合， 對(duì)手控器和機(jī)器人的運(yùn)動(dòng)進(jìn)行控制，實(shí)現(xiàn)機(jī)器智能與人類智能共同作用下控制機(jī)器人接近 目標(biāo)對(duì)象，完成作業(yè)過程。所述步驟S3包括步驟S3.1、S3.2、S3.3、S3.4、S3.5、S3.6、S3.7、 S3.8：
[0103] S3.1、按照公式
[0104] Mmqmd +Bmqmd +Kmqmd -kz{z t+Tr) (7)
[0105] 設(shè)計(jì)手控器的位置預(yù)測(cè)控制器。式中，1?為預(yù)警反饋力矩的權(quán)值系數(shù)。h為手控器 在操縱桿的操作力矩、Tr為機(jī)器人與環(huán)境的接觸力矩，^為預(yù)警反饋力矩，q md為手控器的運(yùn) 動(dòng)趨勢(shì)。
[0106] S3.2、按照公式
[0107] Un = - q,n) + kinr{qiiit! -qj ( 8 )
[0108] 設(shè)計(jì)手控器的控制量Um，使手控器跟隨反饋力矩輸出的運(yùn)動(dòng)趨勢(shì)。式中，k_、k md為 手控器的比例增益和微分增益。
[0109] S3.3、依據(jù)公式
[0110] Mlt (qu，~ cj) 4 Bd (qri -q,) + Kd (a-qj = F (9 )
[0111] 設(shè)計(jì)機(jī)器人阻抗模型，式中，Md、Bd和Kd分別為期望阻抗模型的慣性矩陣、阻尼矩陣 和剛度矩陣，qd和qs分別為機(jī)器人參考關(guān)節(jié)角和實(shí)際關(guān)節(jié)角。
[0112] S3.4、機(jī)器人與環(huán)境間的作用力依照下列公式計(jì)算。
[0113] + Br.Ai] + KtiA(] = F (10)
[0114] S3.5、由阻抗控制理論，按公式
(11)
[0116] 計(jì)算作用力Fe經(jīng)過阻抗模型后，機(jī)器人關(guān)節(jié)控制的軌跡修正量A&( ~c)。
[0117] S3.6、依據(jù)公式
[0118] Q〇 = Qd-AQ (12)
[0119] 計(jì)算經(jīng)過修正的機(jī)器人關(guān)節(jié)控制指令見），式中，込=(也Uw)。
[0120] S3.7、根據(jù)公式
[0121] % > ( 13)
[0122] 設(shè)計(jì)經(jīng)阻抗修正的機(jī)器人控制量Us，實(shí)現(xiàn)機(jī)器人對(duì)手控器的跟隨。
[0123] S3.8、機(jī)器人與環(huán)境(或障礙物)無接觸情況時(shí)，依據(jù)公式
[0124] Us = q!H -I- k^.{qm + /v\,,(<7v/ -fs) (.14》
[0125] 設(shè)計(jì)機(jī)器人的控制量us，實(shí)現(xiàn)機(jī)器人對(duì)手控器的跟隨。式中，ksp、k sd為機(jī)器人控制 器的比例增益和微分增益。
【主權(quán)項(xiàng)】
1. 一種基于位置速度控制的遙操作機(jī)器人碰撞預(yù)警方法，其特征在于，包括W下步驟： 51、 基于手控器位置速度控制方法，W手控器的位置信號(hào)控制機(jī)器人運(yùn)動(dòng)； 52、 通過手控器的位置信號(hào)對(duì)遙操作機(jī)器人碰撞預(yù)警進(jìn)行建模，構(gòu)建機(jī)器人接近目標(biāo) 對(duì)象的預(yù)警反力； 53、 將Sl中操作者的操作力矩與S2中的接觸反饋力矩和預(yù)警反饋力矩進(jìn)行融合，對(duì)手 控器和機(jī)器人的運(yùn)動(dòng)進(jìn)行控制，實(shí)現(xiàn)機(jī)器智能與人類智能共同作用下控制機(jī)器人接近目標(biāo) 對(duì)象，完成作業(yè)過程。2. 根據(jù)權(quán)利要求1所述的基于位置速度控制的遙操作機(jī)器人碰撞預(yù)警方法，其特征在 于，所述步驟Sl包括W下步驟： Sl. 1、搭建基于搖桿式手控器控制機(jī)器人運(yùn)動(dòng)的系統(tǒng)控制平臺(tái)； SI. 2、計(jì)算機(jī)采集人手力及手控器位置信號(hào)，傳送數(shù)據(jù)為系統(tǒng)控制手控器的運(yùn)動(dòng)做準(zhǔn) 備； Sl. 3、通過將手控器視作為積分器，根據(jù)公式計(jì)算手控器期望的機(jī)器人關(guān)節(jié)角控制信號(hào)qsd，式中，kms為手控器與機(jī)器人關(guān)節(jié)角比例 因子，tsam為系統(tǒng)的采樣周期，g:為n時(shí)刻手控器關(guān)節(jié)角，矣為t+1 + l時(shí)刻手控器期望的機(jī) 器人關(guān)節(jié)角，kms為手控器按鈕修改比例因子； SI.4、為避免積分引起控制量的過飽和及消除積分誤差，輸入信號(hào)姑為零時(shí)，設(shè)定妃f 為上一采樣時(shí)刻機(jī)器人關(guān)節(jié)角躬； SI. 5、按照公式 Tm =/Fm 計(jì)算在操縱力Fm下手控器的操作力矩Tm. SI.6、將手控器的運(yùn)動(dòng)簡(jiǎn)化為席畳-陽(yáng)辰-強(qiáng)當(dāng)系統(tǒng).豐梓器動(dòng)力學(xué)模型表示為公式式中，F(xiàn)m為手控器操縱力，J為手控器的雅可比矩陣，Mm、Bm和Km為手控器的慣性系數(shù)矩 陣，阻尼系數(shù)矩陣和彈黃系數(shù)矩陣，kl為操作者手動(dòng)控制的權(quán)值系數(shù)。3. 根據(jù)權(quán)利要求1所述的基于位置速度控制的遙操作機(jī)器人碰撞預(yù)警方法，其特征在 于，所述步驟S2包括W下步驟： 52.1、 根據(jù)公式預(yù)測(cè)t秒后機(jī)器人關(guān)節(jié)角位置妃，式中，qm、qs為手控器及機(jī)器人當(dāng)前時(shí)刻關(guān)節(jié)角； 52.2、 通過立體視覺技術(shù)獲取作業(yè)場(chǎng)景圖像數(shù)據(jù)并實(shí)時(shí)重構(gòu)作業(yè)對(duì)象環(huán)境； 52.3、 結(jié)合環(huán)境相機(jī)測(cè)得的背景環(huán)境或障礙物的高度信息，及預(yù)測(cè)得到的機(jī)器人末端 執(zhí)行器位置，求解機(jī)器人末端沿速度方向與背景環(huán)境的距離d; 52.4、 依據(jù)公式計(jì)算機(jī)器人所受的碰撞預(yù)譽(yù)刀11，AT，kt刃CL W囚于，日O為機(jī)器人末端與障礙物間的 安全距離，：為機(jī)器人末端運(yùn)動(dòng)速度的單位向量； S2.5、依據(jù)公式計(jì)算機(jī)器人與環(huán)境間的接觸反饋力矩Tr和預(yù)警反饋力矩Tt,式中，F(xiàn)e為機(jī)器人末端與環(huán) 境間的作用力，Mt、Me為力矩向量，R為機(jī)器人末端執(zhí)行器坐標(biāo)系相對(duì)手控器坐標(biāo)系的旋轉(zhuǎn)矩 陣。4.根據(jù)權(quán)利要求1所述的基于位置速度控制的遙操作機(jī)器人碰撞預(yù)警方法，其特征在 于，所述步驟S3包括W下步驟： 53.1、 按照公式設(shè)計(jì)手控器的位置預(yù)測(cè)控制器，式中，k2為預(yù)警反饋力矩的權(quán)值系數(shù)，Tm為手控器在操 縱桿的操作力矩、Tr為機(jī)器人與環(huán)境的接觸力矩，Tt為預(yù)警反饋力矩，qmd為手控器的運(yùn)動(dòng)趨 勢(shì)； 53.2、 按照公式設(shè)計(jì)手巧器的巧制量Um,使手巧器跟隨反饋力矩輸出的;is動(dòng)趨勢(shì)，式中，kmp、kmd為手巧 器的比例增益和微分增益； 53.3、 依據(jù)公式設(shè)計(jì)機(jī)器人阻抗模型，式中，Md、ai和Kd分別為期望阻抗模型的慣性矩陣、阻尼矩陣和剛 度矩陣，qd和qs分別為機(jī)器人參考關(guān)節(jié)角和實(shí)際關(guān)節(jié)角； 53.4、 機(jī)器人與環(huán)境間的作用力矩滿足公式 53.5、 按公式計(jì)算作用力Fe經(jīng)過阻抗模型后，機(jī)器人關(guān)節(jié)控制的軌跡修正量A0( A務(wù)，A如，Aa); 53.6、 依據(jù)公式 Qc = Qd-A Q 計(jì)算經(jīng)過修正的機(jī)器人關(guān)節(jié)控制指令53.7、 根據(jù)公式設(shè)計(jì)經(jīng)阻抗修正的機(jī)器人控制量Us，實(shí)現(xiàn)機(jī)器人對(duì)手控器的跟隨； S3.8、機(jī)器人與環(huán)境(或障礙物)無接觸情況時(shí)，依據(jù)公式設(shè)計(jì)機(jī)器人的控制量Us,實(shí)現(xiàn)機(jī)器人對(duì)手控器的跟隨；式中，ksp、ksd為機(jī)器人控制器的 比例增益和微分增益。
【文檔編號(hào)】G05D1/02GK105911995SQ201610429644
【公開日】2016年8月31日
【申請(qǐng)日】2016年6月16日
【發(fā)明人】黃玲濤, 倪濤, 黃海東, 張紅彥
【申請(qǐng)人】吉林大學(xué)