[發明專利]機器人軌跡跟蹤控制方法、磁性醫療機器人及存儲介質

有效

• 申請號: 202111295113.5
• 申請日: 2021-11-03
• 公開（公告）號: CN114019798B
• 公開（公告）日: 2023-08-11
• 發明（設計）人: 劉佳;徐天添;吳新宇;黃哲俊;楊志恒;黨少博;潘仲明
• 申請（專利權）人: 中國科學院深圳先進技術研究院
• 主分類號: G05B13/04
• 分類號: G05B13/04
• 代理公司: 深圳市威世博知識產權代理事務所(普通合伙) 44280
• 代理人: 何倚雯

本申請涉及機器人軌跡跟蹤控制領域，公開了一種機器人軌跡跟蹤控制方法、磁性醫療機器人及存儲介質。該方法包括：獲取機器人基于實際運動參數的實際運動軌跡坐標以及參考軌跡坐標；計算實際軌跡坐標和參考軌跡坐標的坐標誤差；以坐標誤差的最小化作為迭代目標，對機器人的實際運動參數進行優化迭代得到優化運動參數；其中，每次迭代的初始值均采用上一次迭代的優化值；以優化運動參數控制機器人運動。通過上述方式，本申請能夠提高機器人的控制精度與穩定性。

技術領域

本申請涉及機器人軌跡跟蹤控制領域，特別是涉及一種機器人軌跡跟蹤控制方法、磁性醫療機器人及存儲介質。

背景技術

隨著信息化、工業化不斷融合，以機器人科技為代表的智能產業蓬勃興起，成為當代科技創新的一個重要標志。醫療與機器人結合也成為科技創新中的一個重要板塊。醫療機器人的范圍非常廣泛，用于醫療全階段的機器人或者機器人化設備都可以叫做醫療機器人。醫療機器人目前大致可以分為手術機器人、康復機器人、醫用服務機器人和智能設備等。

其中，在靶向治療與微創手術中經常要用到一種微型機器人。隨著微納米加工技術的發展，微型機器人也在不斷優化，目前人們通常叫它們為人造微納機器人。受自然界微生物自由運動的啟發，人造微納機器人近些年得到了廣泛的關注與研究，通過電場、磁場、光場等手段可以有效地驅動這些微納機器人，在無創手術、靶向藥物運輸和生物傳感檢測等領域具有廣泛的應用。在多種操控方案中，磁控驅動可以無線式精確操縱微納機器人，通過改變外部磁場的梯度和方向，會對磁控微納機器人施加力和力矩，進而使其沿著期望的軌跡運動。

在對磁性微納機器人進行實時軌跡跟蹤控制的問題中，大多采用基于幾何方法的軌跡跟蹤中的純跟蹤方法(Pure-Pursuit)和Stanley方法。Pure-Pursuit方法通過計算從當前位置到某個目標位置的距離誤差的比例控制器。當前探測距離過大時，跟蹤性能較差，易偏離軌跡；而Stanley方法考慮了角度誤差和距離誤差，但在非連續路徑上表現不佳。對于基于模型預測優化控制的方法，其優化求解的速度有待提升。

發明內容

本申請主要解決的技術問題是提供一種機器人軌跡跟蹤方法，能夠實現提高機器人的控制精度與穩定性。

為了解決上述技術問題，本申請采用的一種技術方案是：提供了一種機器人軌跡跟蹤控制方法，包括：

獲取機器人基于實際運動參數的實際軌跡坐標以及參考軌跡坐標；計算實際軌跡坐標和參考軌跡坐標的坐標誤差；以坐標誤差的最小化作為迭代目標，對機器人的實際運動參數進行優化迭代得到優化運動參數；其中，每次迭代的初始值均采用上一次迭代的優化值；以優化運動參數控制機器人運動。

進一步地，計算實際軌跡坐標和參考軌跡坐標的坐標誤差，包括，基于實際軌跡坐標和參考軌跡坐標的坐標差值和實際運動參數得到坐標誤差。

進一步地，基于實際軌跡坐標和參考軌跡坐標的坐標差值和實際運動參數得到坐標誤差，包括，以參考運動參數構建狀態系數矩陣，計算狀態系數矩陣與坐標差值的乘積，實際運動參數與控制系數矩陣的乘積；將兩乘積之和作為坐標誤差。

其中，參考運動參數為其中，v_r表示參考運動速度，w_r表示參考運動角速度；狀態系數矩陣為控制系數矩陣為坐標誤差為其中，e₁、e₂、e₃表示實際軌跡坐標和參考軌跡坐標的坐標差值。

進一步地，獲取機器人基于實際運動參數的實際軌跡坐標以及參考軌跡坐標，包括，獲取預測范圍內每個步長下機器人基于實際運動參數的實際軌跡坐標；計算實際軌跡坐標和參考軌跡坐標的坐標誤差，包括，計算每個步長下實際軌跡坐標和參考軌跡坐標的坐標誤差；以坐標誤差的最小化作為迭代目標，包括，以所有步長下坐標誤差中的坐標差值之和，以及控制范圍內實際運動參數之和的最小化作為迭代目標。

進一步地，該方法還包括，預測范圍大于或等于控制范圍。