使用水星Mecury人形机器人搭建VR遥操作控制平台！

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)