一種基于位置速度控制的遙操作機器人碰撞預警方法
【專利摘要】本發明提供一種基于位置速度控制的遙操作機器人碰撞預警方法，其步驟包括：首先將主手控制器視為積分器，通過位置積分計算出對機器人位置的期望值，實現機器人任務速度與手控器位置的有效對應，然后根據手控器位置信號主動預測出未來時刻機器人的位置，當機器人與環境臨近碰撞狀態時生成預警力，最后將反饋力與操作者手動控制力進行融合，引導操作者控制機器人避開障礙物，完成作業過程。本發明可將人類智能決策與機器智能相結合，有效提高機器人的作業精度與效率，減輕操作者的作業負擔，降低對操作者技術熟練度的依賴，避免系統延時導致對機器人控制的盲目性。
【專利說明】
一種基于位置速度控制的遙操作機器人碰撞預警方法
技術領域
[0001] 本發明屬于機器人人機交互及機器人遙操作領域，特別涉及一種基于位置速度控 制的遙操作機器人碰撞預警方法。
【背景技術】
[0002] 在危及人身安全的極限環境或危險環境下作業，例地震災后或核事故現場清理救 援、海洋探索、宇宙空間開發和利用等，機器人作為代替人類進行作業的工具，發揮越來越 重要的作用，因而遙操作技術得到充分發展與運用。但是，由于受機構、控制、人工智能、傳 感和材料技術等發展水平的制約，自主作業機器人的開發在短期內無法實現。機器人控制 中通常所采用的操作方法是直接控制法，而機器人控制領域的操作性及安全性目前還存在 諸多問題：
[0003] 首先，遙操作系統存在著不可避免的延時問題，這極易導致操控不當而帶來作業 對象的損毀。現有的機器人控制方法中，機器人的位置控制方式屬于被動方式，即機器人只 是順應跟隨手控器的位置進行作業，操作者并不能主動預測出機器人未來某一時刻的姿 態。由于數據傳輸量以及傳輸距離等問題，遙操作系統存在著不可避免的延時問題，這將導 致作業過程中極易操控不當，造成無法彌補的損失。其次，在遠程操作系統中，手控器的種 類繁多，通用性差 [W]。不僅不同領域的手控器結構各不相同，即使同一領域的手控器結構 也存在差異。再次，手控器有效活動空間同機器人的活動空間存在差異，這將導致高精度作 業不易實現，操作者工作負擔增加 [8]。當手控器的活動空間范圍遠遠小于機器人的活動范 圍時，操作者對于手控器的控制精度難以掌握，機器人便難以進行高精度作業。最后，操作 者的技能水平和熟練程度極大地影響著機器人作業效率的高低。手控器同機器人在結構、 尺寸、有效活動空間范圍等方面通常存在很大差異，操作者在操控機器人過程中，特別是精 細作業任務中，往往需要反復調整機器人的姿態，才可以完成作業任務+ 11]，這無疑會導致 機器人作業效率不高。
[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]寧祎.手控器的性能和評價方法.機器人，2000,22(4):282-288
[0012] [9]鄧樂，趙丁選，倪濤等.基于Stewart平臺的6自由度力反饋手控器.農業機械學 報，2005,36(7):118-121
[0013] [10]倪濤，趙丁選，張紅彥.電液手控器型遙操作機器人力反饋控制策略.農業機 械學報，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.
【發明內容】
[0015] 本發明針對遙操作機器人控制領域中的操作性及安全性所存在的問題，提供一種 基于位置速度控制的遙操作機器人碰撞預警方法，具體步驟如下：
[0016] S1、基于手控器位置速度控制方法，以手控器的位置信號控制機器人運動；
[0017] S2、通過手控器的位置信號對遙操作機器人碰撞預警進行建模，構建機器人接近 目標對象的預警反力；
[0018] S3、將S1中操作者的操作力矩與S2中的接觸反饋力矩和預警反饋力矩進行融合， 對手控器和機器人的運動進行控制，實現機器智能與人類智能共同作用下控制機器人接近 目標對象，完成作業過程。
[0019] 所述步驟S1包括以下步驟：
[0020] SI. 1、搭建基于搖桿式手控器控制機器人運動的系統控制平臺；
[0021] S1.2、計算機采集人手力及手控器位置信號，傳送數據為系統控制手控器的運動 做準備；
[0022] S1.3、通過將手控器視作為積分器，根據公式
[0023] < ~ +qs qm ^ 0 l q：：l=q： f：=〇
[0024] 計算手控器期望的機器人關節角控制信號qsd。式中，kms為手控器與機器人關節角 比例因子，t sam為系統的采樣周期，f；：為n時刻手控器關節角，^/+1為t+1+l時刻手控器期望 的機器人關節角，k ms為手控器按鈕修改比例因子。
[0025] S1.4、為避免積分引起控制量的過飽和及消除積分誤差，輸入信號為零時，設定 為上一采樣時刻機器人關節角fsH。
[0026] S1.5、按照公式
[0027] xm=JTFm
[0028]計算在操縱力Fm下手控器的操作力矩
[0029] S1.6、將手控器的運動簡化為質量-阻尼-彈簧系統，手控器動力學模型表示為公 式
[0030] Mmqm + Bmqm + Kmqm = kxTm ,
[0031] 式中，Fm為手控器操縱力，J為手控器的雅可比矩陣，Mm、BdPK m為手控器的慣性系 數矩陣，阻尼系數矩陣和彈簧系數矩陣，h為操作者手動控制的權值系數。
[0032] 所述步驟S2包括以下步驟：
[0033] S2.1、根據公式
[0034] q[t + ^
[0035] 預測t秒后機器人關節角位置<。式中，咖、辦為手控器及機器人當前時刻關節角。
[0036] S2.2、通過立體視覺技術獲取作業場景圖像數據并實時重構作業對象環境。
[0037] S2.3、結合環境相機測得的背景環境(或障礙物）的高度信息，及預測得到的機器 人末端執行器位置，求解機器人末端沿速度方向與背景環境的距離d。
[0038] S2.4、依據公式
[0039] FtJk'^-d)v d<(!''
[0 d>d0
[0040]計算機器人所受的碰撞預警力Ft，式中，kt為比例因子，do為機器人末端與障礙物 間的安全距離，^為機器人末端運動速度的單位向量。
[0041] S2.5、依據公式
[0043]計算機器人與環境間的接觸反饋力矩^和預警反饋力矩式中，Fe為機器人末端 與環境間的作用力，Mt、Me為力矩向量，R為機器人末端執行器坐標系相對手控器坐標系的旋 轉矩陣。
[0044] 所述步驟S3包括以下步驟：
[0045] S3.1、按照公式
[0046] +BJmd +Kmqm =kf.m -kjz.+z^
[0047] 設計手控器的位置預測控制器。式中，1?為預警反饋力矩的權值系數。h為手控器 在操縱桿的操作力矩、Tr為機器人與環境的接觸力矩，^為預警反饋力矩，q md為手控器的運 動趨勢。
[0048] S3.2、按照公式
[0049] um - kmd(qmd -qm) + kmp(q,lld -qm)
[0050] 設計手控器的控制量um，使手控器跟隨反饋力矩輸出的運動趨勢。式中，k_、k md為 手控器的比例增益和微分增益。
[0051] S3.3、依據公式
[0052] <： qd - q) + {ijd - a J + Ktl {q(i -q,) = F,
[0053]設計機器人阻抗模型，式中，Md、Bd和Kd分別為期望阻抗模型的慣性矩陣、阻尼矩陣 和剛度矩陣，qd和qs分別為機器人參考關節角和實際關節角。
[0054] S3.4、機器人與環境間的作用力矩滿足公式
[0055] MAq + B!：IM} + = Fr,
[0056] S3.5、按公式
[0058] 計算作用力Fe經過阻抗模型后，機器人關節控制的軌跡修正量犯（At從.，
[0059] S3.6、依據公式
[0060] Q〇 = Qd-AQ
[0061 ]計算經過修正的機器人關節控制指令，式中，& = 。
[0062] S3.7、根據公式
[0063] w.' =々,.+ -么）十夂,,(義一<：/、）
[0064] 設計經阻抗修正的機器人控制量Us，實現機器人對手控器的跟隨。
[0065] S3.8、機器人與環境(或障礙物)無接觸情況時，依據公式
[0066] us =qm +kjqin -qj + ksp(cL, -qj
[0067] 設計機器人的控制量us，實現機器人對手控器的跟隨。式中，ksp、ksd為機器人控制 器的比例增益和微分增益。
[0068]本發明相對于現有技術具有如下的優點及技術效果：
[0069] 1.提出基于位置積分的位置速度控制策略，采用位置增量式的控制，改變傳統手 控器對機器人位置的點對點操作方式；
[0070] 2.降低作業效率對操作者技術熟練度的依賴；
[0071 ] 3.能夠對機器人未來時刻的姿態進行預測，在機器人與周邊環境發生碰撞前，生 成防碰撞預警力，避免由延時引起的誤操作，改善操作系統的安全性；
[0072] 4.為手控器的開發提供一種新的思路，為通用型手控器的標準化設計奠定基礎。
【附圖說明】
[0073] 圖1為位置速度控制技術路線；
[0074]圖2為碰撞預警力建模技術路線。
【具體實施方式】
[0075]下面結合實施例及附圖對本發明作進一步詳細的描述，圖1為位置速度控制技術 路線，圖2為碰撞預警力建模技術路線。
[0076]本發明根據基于位置速度控制的遙操作機器人碰撞預警方法，包括如下步驟S1、 S2、S3:
[0077] SI、基于手控器位置積分的方法，以手控器的位置信號控制機器人運動，所述步驟 S1 包括步驟 S1.1、S1.2、S1.3、S1.4、S1.5、S1.6:
[0078] SI. 1、搭建基于搖桿式手控器控制機器人運動的系統控制平臺；
[0079] S1.2、計算機采集人手力及手控器位置信號，傳送數據為系統控制手控器的運動 做準備；
[0080] S1.3、通過將手控器視作為積分器，根據公式 r t+l
[0081 ] < = q;; :矣〇 Cl) 、 €jx = € =°
[0082] 計算手控器期望的機器人關節角控制信號qsd。式中，kms為手控器與機器人關節角 比例因子，t sam為系統的采樣周期，<為1!時刻手控器關節角，if1為t+1+l時刻手控器期望 的機器人關節角，k ms為手控器按鈕修改比例因子。
[0083] S1.4、為避免積分引起控制量的過飽和及消除積分誤差，輸入信號^為零時，設定 為上一采樣時刻機器人關節角?：。
[0084] S1.5、按照公式
[0085] xm=JTFm (2)
[0086]計算在操縱力Fm下手控器的操作力矩
[0087] S1.6、將手控器的運動簡化為質量-阻尼-彈簧系統，手控器動力學模型表示為公 式
[0088] Mmqm +Bmqm +Kmqm = klTm , (3)
[0089] 式中，Fm為手控器操縱力，J為手控器的雅可比矩陣，Mm、BdPK m為手控器的慣性系 數矩陣，阻尼系數矩陣和彈簧系數矩陣，h為操作者手動控制的權值系數。
[0090] S2、根據手控器的位置信號對遙操作機器人碰撞預警進行建模，構建機器人接近 目標對象的預警反力。所述步驟S2包括步驟S2.1、S2.2、S2.3、S2.4、S2.5:
[0091] S2.1、根據公式
[0092] (f * ^ 0) (4)
[0093] 預測t秒后機器人關節角位置<。式中，咖、辦為手控器及機器人當前時刻關節角。
[0094] S2.2、通過立體視覺技術獲取作業場景圖像數據并實時重構作業對象環境。
[0095] S2.3、結合環境相機測得的背景環境(或障礙物）的高度信息，及預測得到的機器 人末端執行器位置，求解機器人末端沿速度方向與背景環境的距離d。
[0096] S2.4、依據公式 123 Ft = \k,{dn-d)v d<d0 (5)
[0 d > clL) 2
[0098]計算機器人所受的碰撞預警力Ft，式中，kt為比例因子，do為機器人末端與障礙物 間的安全距離，^為機器人末端運動速度的單位向量。 3 S2.5、依據公式 _ n
[0100] =-JT' C6) U」 RFe L艦」
[0101] 計算機器人與環境間的接觸反饋力矩Tr和預警反饋力矩Tt。式中，Fe為機器人末端 與環境間的作用力，M t、Me為力矩向量，R為機器人末端執行器坐標系相對手控器坐標系的旋 轉矩陣。
[0102] S3、將S1中操作者的操作力矩與S2中的接觸反饋力矩和預警反饋力矩進行融合， 對手控器和機器人的運動進行控制，實現機器智能與人類智能共同作用下控制機器人接近 目標對象，完成作業過程。所述步驟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] 設計手控器的位置預測控制器。式中，1?為預警反饋力矩的權值系數。h為手控器 在操縱桿的操作力矩、Tr為機器人與環境的接觸力矩，^為預警反饋力矩，q md為手控器的運 動趨勢。
[0106] S3.2、按照公式
[0107] Un = - q,n) + kinr{qiiit! -qj ( 8 )
[0108] 設計手控器的控制量Um，使手控器跟隨反饋力矩輸出的運動趨勢。式中，k_、k md為 手控器的比例增益和微分增益。
[0109] S3.3、依據公式
[0110] Mlt (qu，~ cj) 4 Bd (qri -q,) + Kd (a-qj = F (9 )
[0111] 設計機器人阻抗模型，式中，Md、Bd和Kd分別為期望阻抗模型的慣性矩陣、阻尼矩陣 和剛度矩陣，qd和qs分別為機器人參考關節角和實際關節角。
[0112] S3.4、機器人與環境間的作用力依照下列公式計算。
[0113] + Br.Ai] + KtiA(] = F (10)
[0114] S3.5、由阻抗控制理論，按公式
(11)
[0116] 計算作用力Fe經過阻抗模型后，機器人關節控制的軌跡修正量A&( ~c)。
[0117] S3.6、依據公式
[0118] Q〇 = Qd-AQ (12)
[0119] 計算經過修正的機器人關節控制指令見），式中，込=(也Uw)。
[0120] S3.7、根據公式
[0121] % > ( 13)
[0122] 設計經阻抗修正的機器人控制量Us，實現機器人對手控器的跟隨。
[0123] S3.8、機器人與環境(或障礙物)無接觸情況時，依據公式
[0124] Us = q!H -I- k^.{qm + /v\,,(<7v/ -fs) (.14》
[0125] 設計機器人的控制量us，實現機器人對手控器的跟隨。式中，ksp、k sd為機器人控制 器的比例增益和微分增益。
【主權項】
1. 一種基于位置速度控制的遙操作機器人碰撞預警方法，其特征在于，包括W下步驟： 51、 基于手控器位置速度控制方法，W手控器的位置信號控制機器人運動； 52、 通過手控器的位置信號對遙操作機器人碰撞預警進行建模，構建機器人接近目標 對象的預警反力； 53、 將Sl中操作者的操作力矩與S2中的接觸反饋力矩和預警反饋力矩進行融合，對手 控器和機器人的運動進行控制，實現機器智能與人類智能共同作用下控制機器人接近目標 對象，完成作業過程。2. 根據權利要求1所述的基于位置速度控制的遙操作機器人碰撞預警方法，其特征在 于，所述步驟Sl包括W下步驟： Sl. 1、搭建基于搖桿式手控器控制機器人運動的系統控制平臺； SI. 2、計算機采集人手力及手控器位置信號，傳送數據為系統控制手控器的運動做準 備； Sl. 3、通過將手控器視作為積分器，根據公式計算手控器期望的機器人關節角控制信號qsd，式中，kms為手控器與機器人關節角比例 因子，tsam為系統的采樣周期，g:為n時刻手控器關節角，矣為t+1 + l時刻手控器期望的機 器人關節角，kms為手控器按鈕修改比例因子； SI.4、為避免積分引起控制量的過飽和及消除積分誤差，輸入信號姑為零時，設定妃f 為上一采樣時刻機器人關節角躬； SI. 5、按照公式 Tm =/Fm 計算在操縱力Fm下手控器的操作力矩Tm. SI.6、將手控器的運動簡化為席畳-陽辰-強當系統.豐梓器動力學模型表示為公式式中，Fm為手控器操縱力，J為手控器的雅可比矩陣，Mm、Bm和Km為手控器的慣性系數矩 陣，阻尼系數矩陣和彈黃系數矩陣，kl為操作者手動控制的權值系數。3. 根據權利要求1所述的基于位置速度控制的遙操作機器人碰撞預警方法，其特征在 于，所述步驟S2包括W下步驟： 52.1、 根據公式預測t秒后機器人關節角位置妃，式中，qm、qs為手控器及機器人當前時刻關節角； 52.2、 通過立體視覺技術獲取作業場景圖像數據并實時重構作業對象環境； 52.3、 結合環境相機測得的背景環境或障礙物的高度信息，及預測得到的機器人末端 執行器位置，求解機器人末端沿速度方向與背景環境的距離d; 52.4、 依據公式計算機器人所受的碰撞預譽刀11，AT，kt刃CL W囚于，日O為機器人末端與障礙物間的 安全距離，：為機器人末端運動速度的單位向量； S2.5、依據公式計算機器人與環境間的接觸反饋力矩Tr和預警反饋力矩Tt,式中，Fe為機器人末端與環 境間的作用力，Mt、Me為力矩向量，R為機器人末端執行器坐標系相對手控器坐標系的旋轉矩 陣。4.根據權利要求1所述的基于位置速度控制的遙操作機器人碰撞預警方法，其特征在 于，所述步驟S3包括W下步驟： 53.1、 按照公式設計手控器的位置預測控制器，式中，k2為預警反饋力矩的權值系數，Tm為手控器在操 縱桿的操作力矩、Tr為機器人與環境的接觸力矩，Tt為預警反饋力矩，qmd為手控器的運動趨 勢； 53.2、 按照公式設計手巧器的巧制量Um,使手巧器跟隨反饋力矩輸出的;is動趨勢，式中，kmp、kmd為手巧 器的比例增益和微分增益； 53.3、 依據公式設計機器人阻抗模型，式中，Md、ai和Kd分別為期望阻抗模型的慣性矩陣、阻尼矩陣和剛 度矩陣，qd和qs分別為機器人參考關節角和實際關節角； 53.4、 機器人與環境間的作用力矩滿足公式 53.5、 按公式計算作用力Fe經過阻抗模型后，機器人關節控制的軌跡修正量A0( A務，A如，Aa); 53.6、 依據公式 Qc = Qd-A Q 計算經過修正的機器人關節控制指令53.7、 根據公式設計經阻抗修正的機器人控制量Us，實現機器人對手控器的跟隨； S3.8、機器人與環境(或障礙物)無接觸情況時，依據公式設計機器人的控制量Us,實現機器人對手控器的跟隨；式中，ksp、ksd為機器人控制器的 比例增益和微分增益。
【文檔編號】G05D1/02GK105911995SQ201610429644
【公開日】2016年8月31日
【申請日】2016年6月16日
【發明人】黃玲濤, 倪濤, 黃海東, 張紅彥
【申請人】吉林大學