本發(fā)明涉及電力機器人控制，尤其是涉及一種電力機器人遙操作共享控制方法、設備及介質。

背景技術：

1、全自主機器人由于當前控制技術的限制和環(huán)境的不確定性依然存在著一定的局限性，而遙機器人結合人類的決策能力和自主控制器的優(yōu)勢，因此廣泛應用于各個領域，包括太空探索、醫(yī)療手術、水下任務等。然而遙機器人在執(zhí)行任務過程中，在確保安全性和穩(wěn)定性的同時提升人機交互的效率仍然是遙機器人系統(tǒng)所面臨的持續(xù)挑戰(zhàn)。

2、共享控制是一種結合了操作員的認知能力和機器人的精確性和魯棒性的控制方法。它融合了操作員與機器人的控制命令，能夠有效減輕人類的工作負擔和提高任務執(zhí)行的效率，因此廣泛應用于遙機器人系統(tǒng)。觸覺共享控制是一種有效的人機交互方式，提供感官上的觸覺反饋有助于引導操作員更好的控制機器人。xu等人在文獻《visual-haptic?aidteleoperation?based?on?3-d?environment?modeling?and?updating》設計了具有視覺和觸覺輔助遙操作系統(tǒng)，虛擬環(huán)境中的輔助力由基于人工勢場的引導力和虛擬接觸力兩部分產(chǎn)生，實現(xiàn)了操作員在本地觀察虛擬環(huán)境并遙操作機械臂避障。luo等人在文獻《ateleoperation?framework?for?mobile?robots?based?on?shared?control》提出了一種混合共享控制方法，結合人工勢場法和操作員的肌電圖信號，產(chǎn)生的力反饋增強了人機交互的體驗，遙操作移動機器人運動同時實現(xiàn)更主動的避障。parsa等人在文獻《haptic-guided?shared?control?grasping:collision-free?manipulation》提出了一種共享控制系統(tǒng)，在到達至抓取階段能夠為操作員提供力的提示，并引導操作員選擇抓取方式，實現(xiàn)了任務中避免碰撞的運動。但是這些方法的觸覺反饋信息在實際機器人控制中并不能保證嚴格安全的控制輸入。

3、運動規(guī)劃是遙機器人安全控制的重要組成部分，常見運動控制方法主要分為離線軌跡規(guī)劃和在線軌跡規(guī)劃。離線軌跡規(guī)劃方法主要包括基于采樣的方法，它可以分為快速擴展隨機樹和概率路線圖法。然而，對于期望軌跡和系統(tǒng)環(huán)境變化的情況，該方法難以適用。在線軌跡規(guī)劃方法包括基于人工勢場的方法、基于數(shù)值優(yōu)化的方法等。人工勢場法通過在目標位置周圍構建引力勢場，而障礙物周圍構建斥力勢場，機器人能夠在避開障礙物的同時運動到目標點，但該方法容易陷入局部極小值的問題。典型的基于數(shù)值優(yōu)化方法是模型預測控制(mpc)，因其有效處理約束優(yōu)化問題而廣泛應用于機械臂安全控制中。sathya等人在文獻《real-time?robot?arm?motion?planning?and?control?with?nonlinear?modelpredictive?control?using?augmented?lagrangian?on?a?first-order?solver》設計了一種具有收斂保證的一階非線性求解器，能夠處理避障約束問題，通過非線性模型預測控制(nmpc)實現(xiàn)了機械臂的避障。zhu等人在文獻《real-time?dynamic?obstacle?avoidancefor?robot?manipulators?based?on?cascaded?nonlinear?mpc?with?artificialpotential?field》設計了一種串級的nmpc框架，上層mpc的目標函數(shù)中引入了基于apf的懲罰項，底層mpc用于軌跡跟蹤和安全保護，實現(xiàn)了安全平穩(wěn)的動態(tài)避障。然而上述方法主要采用的是歐幾里得范數(shù)的約束形式，當接近障礙物時約束才會起作用，實際運用中可能會導致約束違反的問題，因此，為了更主動提前避障需要增大預測時域，但同時會增加優(yōu)化問題的復雜度。

4、近年來，控制障礙函數(shù)(cbf)作為一種安全控制策略能夠有效處理約束控制問題，在機器人領域受到了廣泛關注。常見方法是通過將二次規(guī)劃(qp)問題將cbf約束和控制器相互融合。singletary等人在文獻《safety-critical?kinematic?control?of?roboticsystems》提出了一種基于速度的cbf運動學實現(xiàn)方法，并在動力學層面上設計了一種新的cbf約束形式來保證系統(tǒng)的安全，murtaza等人在文獻《safety?compliant?control?forrobotic?manipulator?with?task?and?input?constraints》引入了cbf處理避障問題，同時對操作空間的進行限制，實現(xiàn)了機械臂的安全避障。但這些方法并沒有考慮系統(tǒng)的預測狀態(tài)。為了解決這一問題，zeng等人在文獻《enhancing?feasibility?and?safety?ofnonlinear?model?predictive?control?with?discrete-time?control?barrierfunctions》在nmpc框架下提出了統(tǒng)一控制李雅普諾夫函數(shù)和cbf的兩種形式，通過在cbf約束上引入了松弛變量，權衡了優(yōu)化問題的安全性和可行性。periotto等人在文獻《mpc-cbfwith?adaptive?safety?margins?for?safety-critical?teleoperation?over?imperfectnetwork?connections》設計了一種基于mpc和魯棒cbf實時運動控制器，具有動態(tài)安全裕度能夠適應不同類型的網(wǎng)絡時延情況，實現(xiàn)了遙操作安全避開障礙物，然而，該方法主要圍繞機器人自主控制器的設計上，并沒有考慮人機交互。

5、綜上，上述在人機交互領域還存在較大的改進空間。

技術實現(xiàn)思路

1、本發(fā)明的目的就是為了提供一種提高人機交互的安全性和臨場感的電力機器人遙操作共享控制方法、設備及介質。

2、本發(fā)明的目的可以通過以下技術方案來實現(xiàn)：

3、一種電力機器人遙操作共享控制方法，包括以下步驟：

4、選取電力機器人機械臂上的關鍵點，建立遙操作中機械臂末端執(zhí)行器和關鍵點的運動學狀態(tài)表達式；

5、基于所述運動學狀態(tài)表達式，考慮障礙物的影響，結合控制障礙函數(shù)構建電力機器人遙操作的安全約束；

6、獲取操作員設置的電力機器人機械臂的參考軌跡，并將所述安全約束引入非線性模型預測控制中，構建nmpc-cbf控制器以跟蹤所述參考軌跡；

7、采用人工勢場法計算電力機器人機械臂與障礙物受斥力反饋的合力，并根據(jù)所述合力修正所述參考軌跡，完成共享控制過程。

8、進一步地，所述關鍵點選取過程中，根據(jù)障礙物的空間范圍進行選取，具體包括以下步驟：

9、定義第j個障礙物的中心位置并根據(jù)所述中心位置定義第j個障礙物的球體半徑重復此步驟得到所有障礙物的球體半徑；

10、基于所述所有障礙物的球體半徑得到所有障礙物的球體，作為所述障礙物的空間范圍；

11、根據(jù)所述電力機器人機械臂與障礙物不發(fā)生碰撞原則，基于所述障礙物的空間范圍，在電力機器人機械臂上選取關鍵點。

12、進一步地，所述建立機械臂末端執(zhí)行器和關鍵點的運動學狀態(tài)表達式的步驟包括：

13、根據(jù)選取的關鍵點，利用機械臂正運動學構建機械臂末端執(zhí)行器位姿和關鍵點位置的表達式，其中表達式分別為：

14、xee＝fkee(θ)

15、

16、其中：

17、

18、式中，xee為機械臂末端執(zhí)行器的位姿，為第i個關鍵點的位置，fkee(γ)和分別表示從基座到機械臂末端執(zhí)行器和第i個關鍵點的正運動學變換，θ為機械臂關節(jié)位置，pe為機械臂末端執(zhí)行器的位置，qe為機械臂末端執(zhí)行器的姿態(tài)；

19、基于所述機械臂末端執(zhí)行器的位姿和關鍵點的位置，構建笛卡爾空間中機械臂末端執(zhí)行器速度和關鍵點速度的表達式，其中表達式分別為：

20、

21、式中，為機械臂末端執(zhí)行器的速度，為關鍵點的速度，jee(θ)和分別表示機械臂末端執(zhí)行器和第i個關鍵點的雅可比矩陣，u為關節(jié)速度。

22、進一步地，所述構建電力機器人遙操作的安全約束的步驟包括：

23、基于所述運動學方程，考慮電力機器人自身物理特性，并結合控制障礙函數(shù)，建立自身物理安全約束；

24、基于所述運動學方程，考慮電力機器人機械臂和關鍵點與障礙物之間的安全距離，并結合控制障礙函數(shù)，構建安全距離約束；

25、所述自身物理安全約束和安全距離約束組成電力機器人遙操作的安全約束。

26、進一步地，所述自身物理安全約束包括：

27、電力機器人機械臂的關節(jié)速度約束：

28、ui,min≤ui≤ui,max

29、電力機器人機械臂的關節(jié)位置約束：

30、θi,min≤θi≤θi,max

31、電力機器人機械臂與桌面之間的高度約束：

32、hi≥hi,min

33、式中，ui為第i個關節(jié)的關節(jié)速度，ui,min和ui,max分別表示第i個關節(jié)的關節(jié)速度的下界和上界，θi為第i個關節(jié)的關節(jié)位置，θi,min和θi,max分別表示第i個關節(jié)的最小關節(jié)位置和最大關節(jié)位置，hi和hi,min分別表示第i個關鍵點與桌面的高度和最小安全高度。

34、進一步地，所述構建安全距離約束的步驟包括：

35、基于所述運動學方程，構建機械臂末端執(zhí)行器與第j個障礙物之間的安全距離，其中所述安全距離表示為：

36、

37、式中，hee(θ)為機械臂末端執(zhí)行器與第j個障礙物之間的安全距離，θ為機械臂關節(jié)位置，xee為機械臂末端執(zhí)行器的位姿，oj為第j個障礙物的中心位置，為第j個障礙物的球體半徑，為以機械臂末端執(zhí)行器為中心的球體半徑；

38、對安全距離hee(θ)求導得出與控制輸入u之間的關系：

39、

40、式中，為安全距離函數(shù)的導數(shù)，jee(θ)為機械臂末端執(zhí)行器的雅可比矩陣；

41、基于所述運動學方程，構建第i個關鍵點與第j個障礙物之間的安全距離，其中所述安全距離表示為：

42、

43、式中，為第i個關鍵點與第j個障礙物之間的安全距離，為第i個關鍵點的位置，為以第i個關鍵點為中心的球體半徑；

44、對安全距離求導得出與控制輸入u之間的關系：

45、

46、式中，為安全距離函數(shù)的導數(shù)，為第i個關鍵點的雅可比矩陣；

47、根據(jù)式(1)和式(2)，結合控制障礙函數(shù)，構建電力機器人機械臂的安全距離約束，表達式為：

48、

49、式中，α(hee(θ))＝γ1hee(θ)，0＜γ1＜∞，α為擴展k類函數(shù)，γ1為衰減率；

50、根據(jù)式(3)和式(4)，結合控制障礙函數(shù)，構建關鍵點的安全距離約束，表達式為：

51、

52、式中，γ2為衰減率；

53、將所述電力機器人機械臂的安全距離約束和關鍵點的安全距離約束作為最終的安全距離約束。

54、進一步地，所述nmpc-cbf控制器的表達式為：

55、

56、s.t.

57、xee(t)＝xee，0,

58、

59、ui,min≤ui(τ)≤ui,max,

60、θi,min≤θi(τ)≤θi,max

61、hi(τ)≥hi,min

62、t≤τ≤t+tp,

63、0＜γ1＜∞,0＜γ2＜∞,

64、i＝1,2,...,np,

65、式中，為有限時域最優(yōu)控制問題的最優(yōu)控制序列，表示關節(jié)的最優(yōu)控制，僅執(zhí)行最優(yōu)控制序列的第一個動作，n為等距節(jié)點數(shù)量，tp為預測時域，xee為機械臂末端執(zhí)行器的位姿，為參考軌跡，q和r分別表示跟蹤誤差權重矩陣和控制輸入權重矩陣，u為關節(jié)速度，作為控制輸入，j(ut)為代價函數(shù)，xee，0和xp，0表示系統(tǒng)的初始狀態(tài)，jee(θ)和分別表示機械臂末端執(zhí)行器和第i個關鍵點的雅可比矩陣，為機械臂末端執(zhí)行器的速度，為第i個關鍵點的速度，hee為機械臂末端執(zhí)行器與第j個障礙物之間的安全距離，為，為，γ1、γ2為衰減率，ui為第i個關節(jié)的關節(jié)速度，ui,min和ui,max分別表示第i個關節(jié)的關節(jié)速度的下界和上界，θi為第i個關節(jié)的關節(jié)位置，θi,min和θi,max分別表示第i個關節(jié)的最小關節(jié)位置和最大關節(jié)位置，hi和hi,min分別表示第i個關鍵點與桌面的高度和最小安全高度，np為關鍵點的數(shù)量。

66、進一步地，所述電力機器人機械臂與障礙物受斥力反饋的合力的計算步驟包括：

67、基于所述人工勢場法，考慮電力機器人機械臂與障礙物之間的斥力關系，構建人工勢場函數(shù)，所述人工勢場函數(shù)表示為：

68、

69、其中，為避免斥力在作用范圍內(nèi)產(chǎn)生力突變，將斥力場函數(shù)qrep表示為：

70、

71、式中，q為人工勢場函數(shù)，m為對電力機器人機械臂起作用的障礙物的數(shù)量，qrep為斥力場函數(shù)，ε為斥力增益系數(shù)，ρjo表示機械臂末端執(zhí)行器的位置和障礙物的距離，ρ0表示每個障礙物產(chǎn)生作用的最大距離；

72、求解所述斥力場函數(shù)qrep的負梯度，得到斥力：

73、

74、式中，frep為斥力，pe為機械臂末端執(zhí)行器的位置；

75、基于所述斥力計算所述電力機器人機械臂與障礙物受斥力反饋的合力，所述合力的表達式為：

76、

77、式中，f為合力，μ是速度增益，表示機械臂末端執(zhí)行器的速度。

78、本發(fā)明還提供一種電子設備，包括：一個或多個處理器；存儲器；和被存儲在存儲器中的一個或多個程序，所述一個或多個程序包括用于執(zhí)行如上述所述電力機器人遙操作共享控制方法的指令。

79、本發(fā)明還提供一種計算機可讀存儲介質，包括供電子設備的一個或多個處理器執(zhí)行的一個或多個程序，所述一個或多個程序包括用于執(zhí)行如上述所述電力機器人遙操作共享控制方法的指令。

80、與現(xiàn)有技術相比，本發(fā)明具有以下有益效果：

81、(1)本發(fā)明根據(jù)障礙物與電力機器人之間的關系，利用控制障礙函數(shù)設計嚴格的安全約束，將其嵌入非線性模型預測控制中，實現(xiàn)對操作員設置的參考軌跡的安全跟蹤，并結合人工勢場法來構建障礙物與電力機器人之間斥力場來進行反饋控制以引導修正參考軌跡，從而提高了人機交互的安全性和臨場感。

82、(2)本發(fā)明采用基于控制障礙函數(shù)的安全約束條件，可以通過調(diào)整衰減率來調(diào)整與障礙物之間的安全裕度，通過將安全約束條件引入非線性模型預測控制中，能夠以較小的預測時域提前做出避障反應，并實現(xiàn)高效避障。

83、(3)本發(fā)明通過引入人工勢場法進行反饋控制來修正參考軌跡，還進一步降低軌跡跟蹤誤差。