VR遙操作機械臂是一種將虛擬現實技術與機械臂控制相結合的系統，使用戶可以通過虛擬現實設備操控和交互實際的機械臂。這種技術可以應用于多個領域，包括遠程操作、培訓、危險環境中的工作等。

雙臂人形機器人是一種模擬人體上半身結構，包括頭部、軀干和雙臂的機器人。這種機器人設計的目標是模仿人類的上半身動作和功能，以執行各種任務，從而在虛擬現實（VR）領域中有廣泛的應用。

合虛擬現實和人型機器人可以為許多領域帶來創新和改進，提高用戶體驗，擴展應用領域，促進技術的發展。這種結合使得虛擬和現實之間的交界更加模糊，為未來創造了令人興奮的可能性。

硬件介紹
Mecury X1
這是一款名為“Mercury X1”的人形機器臂，由Elephant Robotics 精心研發。17個自由度（DOF），使其具有極高的靈活性和適應性。工作電壓為24V，配備了一個9英寸的量子點觸控屏，既現代又高效。Mercury X1最大工作半徑為450毫米，最大負載能力為1公斤，凈重8公斤，非常適合輕量級的操作任務。

它的重復定位精度高達±0.05毫米，意味著它在進行精密作業時非常可靠。機器人的壽命預計為5000小時，表明了其出色的耐用性。主控制單元采用了Jetson Xavier，輔助控制則由ESP32*1負責，這樣的配置保證了其強大的數據處理能力和穩定的控制性能。還包含了碳纖維外殼材料，使得機器臂不僅堅固耐用，同時也輕便。它裝備了Orbbec Deeyea 3D攝像頭和一個具有4個麥克風的線性陣列聲音模塊，能夠進行高效的視覺和聲音處理。通信方面，支持WIFI、Ethernet、藍牙、USB端口和RS485，兼容ROS1/ROS2，這意味著它能夠輕松集成到各種智能制造和研究環境中。

VR Oculus Quest 2
是由Facebook的子公司Oculus VR開發的一款虛擬現實（VR）頭戴設備。Quest 2有開發者模式，它允許開發者直接在設備上安裝和測試自己開發的應用。主要是由游戲引擎unity和unreal engine平臺支持。

VR控制機器人項目
軟件架構和交互設計
VR遙操作首先要解決的問題就是操作者和機器人的通信問題，在這方面我選擇的是基于HTTP協議的通信，服務器選擇Flask。雖然選擇Python可能會對性能產生不利影響，但是因為Mercury機器人提供的Python SDK——pymycobot是Python的庫,因此Python作為開發語言是最方便與機器人集成的選擇，非常適合驗證項目的可行性。

VR端我們選擇了Unity3D,豐富的社區資源使得這個項目成為可能。

服務模型為經典的C/S結構，VR端通過訪問特定的URL向機器人發送不同的命令，再通過服務器轉發到pymycobot，執行實際的動作。

另一方面是交互方面的設計，我們采用相對移動的方式，用戶按下手柄的A鍵標志著移動開始，松開標志著移動結束。

下面是VR遙控操作的通信流程：

實時視頻流
在克服VR遙操作技術難題的過程中，確保獲取低延遲的視頻流一直是關鍵挑戰之一。我們采用了一項創新性的解決方案，通過利用NVIDIA Jetson Xavier平臺所提供的Accelerated GStreamer插件，成功實現了GPU加速的視頻編解碼，旨在在保障實時性的同時，最大程度地優化帶寬利用率。

Jetson Xavier平臺的Accelerated GStreamer插件是一種強大的工具，通過充分利用GPU的計算能力，對視頻數據進行加速處理。這一創新性的技術手段不僅提高了視頻編解碼的效率，同時在大幅度減少延遲的同時，優化了網絡帶寬的利用效率。

Accelerated GStreamer是NVIDIA為其Jetson平臺提供的一組GStreamer插件，旨在通過使用GPU（圖形處理單元）加速多媒體處理任務，提高性能并降低延遲。這一套插件專為Jetson平臺設計，充分利用了其強大的GPU資源，適用于視頻編解碼、圖像處理、深度學習推理等多媒體應用。

Accelerated GStreamer — Jetson Linux Developer Guide documentation

實現過程
首先，下載編譯并編譯GStreamer官方提供的rtsp server源代碼https://github.com/GStreamer/gst-rtsp-server/blob/1.14.5/examples/test-launch.c

編譯好后會有一個test-launch的文件

通過GStreamer命令構造推流管線，這里測試采用的是

```bash
nvarguscamerasrc sensor-id=0 ! video/x-raw(memory:NVMM), format=NV12, width=3264, height=2464, framerate=21/1 ! nvv4l2h264enc insert-sps-pps=true ! h264parse ! rtph264pay name=pay0 pt=96
```

`提示：! 用于分隔不同的 GStreamer 元素`

nvarguscamerasrc sensor-id=0：

nvarguscamerasrc是 NVIDIA 提供的用于處理 Argus 相機的 GStreamer 插件。

sensor-id=0 指定使用 ID 為 0 的相機傳感器。在多攝像頭系統中，可以選擇不同的相機傳感器。

video/x-raw(memory:NVMM), format=NV12, width=3264, height=2464, framerate=21/1：

video/x-raw(memory:NVMM) 表示輸出的是 NVIDIA 內存管理的原始視頻數據。

format=NV12 指定了視頻幀的格式為 NV12，這是一種 YUV 格式。

width=3264, height=2464 指定了視頻幀的寬度和高度。

framerate=21/1 指定了視頻的幀率為 21 幀每秒。

nvv4l2h264enc insert-sps-pps=true：

nvv4l2h264enc 是 NVIDIA 提供的 H.264 編碼插件。

insert-sps-pps=true 表示在輸出流中插入 SPS（序列參數集）和 PPS（圖像參數集），這對于 H.264 視頻流的解碼是必需的。

h264parse：

h264parse 插件用于解析 H.264 數據流。

rtph264pay name=pay0 pt=96：

rtph264pay 插件用于封裝 H.264 數據流為 RTP 包。

name=pay0 為該 RTP Payloader 指定了名稱。

pt=96 指定了 RTP 負載類型（Payload Type），這里設置為 96。

將GST命令與RTSP Server聯合使用，輸入命令

```bash
./test-launch "nvarguscamerasrc sensor-id=0 ! video/x-raw(memory:NVMM), format=NV12, width=3264, height=2464, framerate=21/1 ! nvv4l2h264enc insert-sps-pps=true ! h264parse ! rtph264pay name=pay0 pt=96"
```

可以在同局域網下的另一臺主機上通過RTSP協議訪問當前主機來測試結果（VLC播放器可以接入RTSP協議流直接進行播放）。

最后，將test-launch.c內綁定的URL重新命名為left和right。并將開啟和關閉寫入腳本方便執行。

```bash
# launch-rtsp.sh
width=1920
height=1080
framerate=28
./left-rtsp-server "nvarguscamerasrc sensor-id=1 ! video/x-raw(memory:NVMM), format=NV12, width=$width, height=$height, framerate=$framerate/1 ! nvv4l2h264enc insert-sps-pps=true maxperf-enable=1 bitrate=8000000 ! h264parse ! rtph264pay name=pay0 pt=96" &
./right-rtsp-server "nvarguscamerasrc sensor-id=0 ! video/x-raw(memory:NVMM), format=NV12, width=$width, height=$height, framerate=$framerate/1 ! nvv4l2h264enc insert-sps-pps=true maxperf-enable=1 bitrate=8000000 ! h264parse ! rtph264pay name=pay0 pt=96" &
```

```bash
# stop-rtsp.sh

ps -al | grep "left|right" | awk '{print $4}' | xargs kill
```

遙操作系統
VR遙操作系統面臨的主要挑戰在于要協調處理網絡的不穩定性與對機械臂等硬件需要穩定輸入的矛盾。這兩者之間的平衡是確保遠程遙操作系統有效運行的關鍵因素之一。

首先，網絡的不穩定性可能導致延遲、數據包丟失或者不確定性的帶寬情況。這種情況會直接影響到遠程用戶與機械臂之間的實時交互。在VR遙操作系統中，用戶需要感受到虛擬環境中的實時變化，并對機械臂的運動進行即時響應。網絡延遲可能導致用戶的指令與機械臂的實際動作之間存在明顯的滯后，降低了操作的準確性和流暢性。

另一方面，機械臂等硬件設備對輸入的穩定性要求較高。由于遙操作系統需要實時傳輸用戶輸入到機械臂，輸入信號的不穩定性可能導致機械臂的運動異常或失控。這對于需要高精度和可靠性的任務，如手術操作、工業維護等，可能帶來嚴重的問題。

VR遙操作的困局(dilemma)
為了幫助你理解這個問題，首先要了解機械臂的運動方式。在傳統機械臂運動中，發給機械臂一個點位，機械臂會自動規劃到達目的地的加速與減速，以達到流暢順滑的移動；但是由于機械臂會自動進行加速減速的規劃，如果用這個方法去做遙操作，就會遇到機械臂在每兩個采樣點上頻繁加速減速的過程，導致機械臂無法連續運動。而在遙操作情況下，加速減速應該由操作者的手運動來控制，因此理論上如果想要實現順暢的遙操作，則需要機械臂有一個可以放棄自動規劃，完全使用采樣點來進行插值運動的接口。我們將這個接口命名為“速度融合接口”。

但是機械臂的底層插值運動接口通常對信號的穩定性有著極高的要求，必須要有毫秒級的穩定性才能保證機械臂穩定運行；這種完全采用采樣點的插值底層運動接口，在Mercury機器人上的VR控制方式為在給定時間內，向指定目標點移動，舉個例子，你輸入了一個坐標，還有一個時間50ms，那么機械臂就會在這50ms內向著你發的坐標移動，如果到了就會停止（這也帶來一個問題，后面會細說），如果機械臂沒到目標位置，但是時間到了的話也會停止。

理想狀態下，如果你持續且穩定地用速度融合直接給機械臂輸入移動點位的命令，且你發送的間隔，剛好和你給定的運動時間片是完全匹配的，那么理論上機械臂此時在速度不超過限速的情況下，能夠完全跟隨人手。

但是此時如果下一條指令沒有及時到來（可能因為網絡帶來的延遲波動），機械臂此時就會立刻急停，但是又因為沒有減速的過程，此時機械臂就會因為急停而產生抖動。

一個合理的改進方案是加入緩存，緩存會平滑網絡延遲帶來的誤差，給機械臂一個相對穩定的指令流，但是隨之而來的問題就是實時性的降低。因為如果緩存發揮作用，那么實際下發的速度融合參數內給定的時間片就必須要小于實際的發送間隔，因為只有消費的指令比生產的指令更慢，才能保證緩存區內始終有一定量的指令可以下發，避免產生沒有指令導致急停的尷尬。但是這樣就會導致在網絡穩定地情況下，緩沖區一直是滿的，給整個控制系統加上了一個固定的時間片x緩沖區大小的延遲。

機械臂需要穩定地控制信號，因為它的運動本質上由電機驅動。在常規的點位移動中，機械臂會通過內部自動規劃加速和減速，從而實現相對較為穩定的運動。然而，在遙操作領域，實時性是至關重要的，因此通常需要設計速度融合接口，將由VR端采樣得到的點位直接下發給機械臂。這樣的設計是為了確保實時性，但同時也帶來了延遲和穩定性難以兼得的問題。

如果我們追求實時性，從網絡傳來一個點位就立刻發送，由于我們無法完全預測下一個控制信號何時到來，則提供的時間參數可能就會和實際產生偏移，如果給的太少就會造成機械臂速度連不起來，無法運動；給的太多就會造成機械臂還沒運動完，下一個指令就發來了，造成指令堆積，降低實時性。如果追求穩定性，那就需要增加緩沖隊列，用緩沖隊列來平滑通信方式帶來的延遲波動。但是同樣的，這樣就會增加控制延遲。這也就是遙操作系統的困局：延遲和穩定性不可兼得

VR端實現
VR端的實現采用了Unity3D + XR Interactive Toolkit, 選擇Unity3D主要是因為其簡單性，可以快速上手，且能支持多種VR平臺。如果采用虛幻引擎可能需要花費大量時間在此工程之外的問題上。XR Interactive Toolkit是Unity3D的官方VR框架，使用這套系統而不是Oculus插件能夠使項目方便的移植到其他VR設備上。

開發環境方面，除了使用Unity3D自帶的Editor以外，C#編輯器使用Visual Studio 2022社區版。導入了UMP插件作為RTSP Player，以及Oculus官方的基本VR素材。

最開始的時候，我是打算采用絕對坐標一比一的轉發手柄的動作，但是我發現這樣操作很不方便。首先，Mercury機器人的臂展大概只相當于十幾歲的小孩，如果使用完全絕對坐標1：1控制很容易導致機械臂撞到關節限位。而且手柄的初始位置也是個問題。因此最后我決定采用相對的控制方式，只有在按住特定按鍵的時候才允許機械臂移動，這樣做的好處就是：在沒有按下按鈕的時候，你可以隨意調整姿勢；而在你開始運動的時候，機械臂永遠是以你當前點為基準點進行相對運動，這樣控制起來就容易得多了。

圖傳方面最開始的做法是，用服務器轉發MJPG圖片到VR端，然后以texture的方式渲染到屏幕上，這種方式的好處就是實現簡單。缺點就太多了，首先就是延遲和CPU負載的問題；如果直接使用服務器轉發圖片，不但要至少多一次拷貝，還很難調用Nvidia自帶的編解碼器。而且在VR端也需要時間進行解碼拷貝，整體延遲和CPU負載都很高。后來我想到了使用GStreamer+NV加速插件的方案，也就是上面說到的，利用了NV硬件加速以后，延遲和負載都得到了大幅度的改善。

除此之外，在開發過程中，我也對Unity3D+Quest 3作為遙操作平臺有了更深的了解。首先在Unity3D中，幾乎所有的運算都是和幀對齊的，雖然你可以開線程，但是游戲引擎給你提供的資源幾乎都是按幀進行刷新的。比如我能獲取到的手柄坐標，我能獲取到的最大刷新率就是等于游戲幀率。因此在這個平臺上，不考慮插值等操作，遙操作控制頻率采樣的上限其實就是幀率,這個數字通常是70-90hz每秒。因此我沒有采用多線程來發送信息，而是使用了Unity3D中最普遍的做法：協程。使用協程能夠保證你的操作和幀是對齊的，能夠避免很多因為不同步導致的奇怪問題。

```c#
// One example of coroutine function
IEnumerator sendUpdate()
{
   if (!flag_origin_initialized) yield break;
   Vector3 pos = gameObject.transform.position;
   pos -= origin_pos;
   pos *= 1000;
   
   Vector3 rotation = gameObject.transform.rotation.eulerAngles;
   if (isZero(pos) || isZero(rotation))
       yield break;
   rotation = angleConvert(rotation);
   string content = $"[{pos.x:F2},{pos.y:F2},{pos.z:F2},{rotation.x:F2},{rotation.y:F2},{rotation.z:F2},{System.DateTime.UtcNow.Ticks / TimeSpan.TicksPerMillisecond}]";
   Debug.Log(content);
   using (UnityWebRequest www = UnityWebRequest.Post(updateURL, content, "application/json"))
   {
       www.timeout = 1;
       yield return www.SendWebRequest();
       Debug.Log(www.result);
       if (www.result != UnityWebRequest.Result.Success)
       {
           Debug.Log(www.error);
       }
   }
}

// Use this in main thread (eg. inside Update() to sync with main loop)
StartCoroutine(sendUpdate());

```

服務器端實現
服務器方面我使用的是簡單可靠的Flask作為框架，因為我需要的僅僅只是作為RPC用途的控制框架。精簡的Flask就能可以很好的完成這個任務。

在控制系統方面，Unity3D和機械臂平臺的對齊也是一大難點，即如何將VR世界的坐標，翻譯為機械臂能聽懂的坐標。因為Mercury機器人的手臂，本質上是由兩個Mercury單臂組成的，在單臂自己的視角下，它的坐標實際上是以它自己的底座，也就是胳膊大臂關節的位置為原點的，而不是以我們期望的——腰部為原點。因此需要設計一套坐標轉換工具來實現從VR世界，到Mercury的基坐標系，再到單臂的坐標的轉換。這個過程使用到了大量的線性代數工具和機器人學理論。

比如下面這段代碼實現了VR坐標到基坐標系的轉換

```python
def vr_to_base(posture):
   position = np.array(posture[:3])
   rotation = np.array(posture[3:])
   matrix = cvt_euler_angle_to_rotation_matrix(rotation)
   T = np.vstack((np.hstack((matrix, position.reshape(3, 1))), np.array([0, 0, 0, 1])))

   TRB = np.array([[0, 0, 1, 0], [-1, 0, 0, 0], [0, 1, 0, 310], [0, 0, 0, 1]])

   Tp = np.dot(TRB, T)

   rotation_matrix = Tp[:3, :3]
   position = Tp[:3, 3]
   rotation = cvt_rotation_matrix_to_euler_angle(rotation_matrix)

   return np.hstack((position, rotation))
```

一個有趣的問題就是左右臂互為鏡像的問題，我和負責機器人算法的工程師不得不分別給兩條臂使用不同的固件以兼容使用同一套轉換算法。

前文提到，機械臂期望穩定且固定間隔的控制信號，而實現固定間隔就需要緩存系統和高精度的計時的下發系統。

首先是緩存問題，緩存主要考慮的一個問題就是線程安全。因為服務器本身是多線程運行的，兩個網絡包可能會同時到達，或者網絡包到達時剛好碰上下發，因此設計一個線程安全的雙端隊列作為緩存區是首要任務。其次就是緩沖區的大小，下發時間的間隔，以及時間片參數的問題；這些參數也是下發系統延遲表現和穩定表現的均衡器，這些參數需要大量的實驗來調試，以達到最佳表現。其中尤為難以平衡的是下發時間的間隔和時間片，因為網絡延遲是不確定的，機器處理運算也需要時間，因此實際需要的時間是要比單純下發的間隔要長的，具體長多少也是不固定的。因為這一層原因，實際上參數的設置需要更加保守才能保證系統運行穩定。

Python標準庫中自帶的time精度并不理想。因此單純靠sleep來實現計時肯定是不可行的。我目前的做法是單獨開一個線程，以大量占用CPU為代價高速輪詢來實現低延遲下發。實測證明這種方式的延遲是要比直接給一個完整的sleep要低的。

class MyThread(Thread):
   ...

   def run(self) -> None:
       while running:
           if len(self.bucket)!= 0 and time.time_ns() - self.last_time > self.time_slice:
               # do something
               self.last_time = time.time_ns()
           time.sleep(0.00001)

優化
在基本框架完成以后，還可以對下發點位進行簡單的濾波，以進一步去除抖動。人手在使用VR的時候不可能確保完美的停在一個點，在做直線運動的時候也不可能保證是完美的直線。我們可以單獨對每個分量進行控制，過濾掉較低的分量，以實現更穩定地運動軌跡。

a = final_base_coords[:3]
b = self.last_arm_base_coords[:3]
if self.last_arm_base_coords is not None:
   diff = a - b
final_base_coords[:3] = np.where(abs(diff) > 5, a, b)

另一個可能的優化是對軌跡進行平均化操作，這樣可以使得軌跡更加平滑，但是可能的trade off是要進一步增加延遲。以下是一個基于雙端隊列的滑動窗口的實現。

class SlidingWindow:
   def __init__(self, maxlen=5) -> None:
       self.lock = Lock()
       self.store = deque(maxlen=maxlen)
       for i in range(maxlen):
           self.store.append(1)
       self.maxsize = maxlen

   def append(self, obj):
       with self.lock:
           self.store.append(obj)

   def mean(self):
       with self.lock:
           return np.mean(np.array(self.store), axis=0)

   def clear(self):
       with self.lock:
           self.store.clear()

   def size(self):
       with self.lock:
           return len(self.store)

機械臂運動控制
1. 動作捕捉：佩戴VR設備（如頭盔和手套）進行實際動作。這些設備通過內置傳感器捕捉用戶的動作數據（如頭部方向、手部位置和手勢），并將這些數據實時傳輸到控制系統。

2. 虛擬現實界面：用戶在VR環境中可以看到虛擬的機械臂模型，并通過VR設備進行操作。用戶的動作被映射到虛擬模型上，實時轉化為機械臂的運動指令。

3. 數據處理和傳輸：捕捉到的動作數據需要通過軟件處理，轉化為機械臂可以理解的指令。這些指令通常涉及關節角度、速度或位置信息的計算。然后，這些指令通過網絡發送給機械臂控制器。

4. 機械臂執行：機械臂接收到指令后，通過其內部控制系統執行相應的動作。這可能包括移動到特定的位置、按照特定的路徑移動或執行復雜的手部動作。

5. 反饋機制：為了提高操作的精確性和用戶體驗，系統可能包括反饋機制，如觸覺反饋或視覺反饋，將機械臂的狀態實時反映到VR環境中，以便用戶調整其操作。

案例展示
https://youtu.be/mvpHFXcadNk

Summary
VR技術的進步正在打破物理空間的限制，使人們能夠在虛擬環境中實現復雜的操作和互動，這對于遠程控制、教育、醫療等領域具有重大意義。未來，隨著VR技術的持續發展，我們可以預見更多創新的應用出現。例如，VR在模擬復雜手術、遠程教育、災難響應訓練等領域的應用將更加廣泛。

如果是你，你會怎樣來使用Mercury X1呢？

大象機器人今天正式發布Mercury系列機器人，目前水星系列機器人共有三款產品，Mercury-A1七軸協作機械臂,Mercury-B1 半人形雙臂機器人,Mercury-X1通用輪式人形機器人。三款產品的工業設計皆由瑞典團隊精心設計而成，集成七大機器人核心算法，多種使用與開發方式。

審核編輯 黃宇