一、遥操原理 

遥操使用的是 Pika sense 去控制机械臂，Pika sense 与外部定位基站一起使用，就能获取到精度达0.3mm的 6d pose 数据，然后通过 Pika sense 与机械臂末端坐标系对齐之后，最后使用增量式控制将 6d Pose 映射给机械臂末端就能实现遥操作。

遥操的原理，总的来说就是四步：

1、获取 6d Pose 数据

2、坐标系对齐

3、增量式控制

4、将 6d Pose 数据映射到机械臂

下面，我们按照上面四步一一拆解来进行讲解。

二、获取 6d Pose 数据

2.1.Pika sense 与 station 定位原理

2.1.1.基站的工作机制

• • 每个基站配备红外 LED 阵列和两个旋转 的激光发射器（分别负责水平和垂直方向扫描）：
• • 红外 LED 以 60Hz 频率全局闪光，为整个空间提供同步信号。
  • 激光发射器由电机驱动旋转，依次发射水平和垂直激光束，以 10ms 周期交替扫描空间（共20ms完整周期）。
• • 2个激光扫描覆盖范围可达 5x5 米（单基站），4个基站协同可拓展至 10x10 米

2.1.2.Pika sense 的定位实现

• • Sense 上部传感器名为 Tracker，它表面密布多个光敏传感器，每个传感器可接收红外同步信号与激光扫描。
• • 定位计算过程：
• • 传感器记录激光到达时间，集合基站扫描周期，计算出传感器相对于基站的水平角与俯仰角。
  • 通过多个传感器（≥5个）的空间分布与时间差数据，解算 Tracker 的精确位置和朝向。
• • 本地处理器直接完成计算，无需图像处理，延迟仅 20ms ,精度达 0.3mm。

将 6d Pose 以 ros geometry_msgs/PoseStamped 的消息类型发布出 Pika/pose 话题，该话题能适配上市面上大部分机械臂的 end pose 控制。

除了 ros 的消息类型之外，如果想要去 ros 的 6d pose数据，请查看我们的 pika_sdk 。

SDK：https://github.com/agilexrobotics/pika_sdk

 三、坐标系对齐 

在第一步[获取 6d Pose 数据]中，无论是订阅 ros 话题数据还是从 pika sdk 的方式获取到的 6d Pose 数据，pika sense 的坐标系是位于夹爪中心，其 x 轴朝前，y 轴朝左， z 轴朝上，如下图所示：

不同的机械臂其末端的坐标系也会有所不同，但大部分都是 z 轴朝前，至于 x 轴跟 y 轴朝向，就取决于机械臂末端的初始旋转值。至于如何查看机械臂末端的坐标系，每款机械臂都会有所不同，一般获取的方式是通过厂家提供的上位机查看，或者在 ros rviz 中加载机械臂模型查看。

知道了 pika sense 以及机械臂末端的坐标系后，通过将获取到 Pika sense 的 6d Pose 转换成齐次变换矩阵。然后与一个调整矩阵相乘将 Pika sense 坐标系调整到机械臂末端坐标系。此部分的工作就算完成了。

 四、增量式控制 

在第二步[坐标系对齐]中，我们将 Pika sense 的坐标系与机械臂末端坐标系对齐（z 轴朝前）。

❓：那么问题来了，当我手持 pika sense 时，向前移动，那它 z 轴的值就一定是正向增长的吗

📌：不一定， pose 的值跟它的 base_link 有关，如果 base_link 的 z 轴刚好跟 pika sense 的 z 轴方向一致，那么它的 z 轴确实可以正向增长，但 pika sense 的 base_link 是由 pika sense 与 基站校准时生成的一个 x 轴朝前，y 轴朝左，z 轴朝上的坐标系，也就是说 base_link 是随机生成的。

❓：那么我们如何将 Pika sense 的坐标映射给机械臂末端呢？如何可以让 Pika sense 向前，机械臂末端也向前，向左机械臂末端也向左呢？

在遥操作中，Pika sense 给出的位姿是绝对位姿，但是我们不希望机械臂直接跳到这个绝对位姿。相反，我们希望机械臂能够跟随操作者从当前位置开始的相对运动。简单来说，就是将操作设备的绝对位姿变化，转换为机械臂需要执行的相对位姿指令。

其核心代码就是下面这段：

# 增量式控制
def calc_pose_incre(self,base_pose, pose_data):
    begin_matrix = tools.xyzrpy2Mat(base_pose[0], base_pose[1], base_pose[2],
                                                base_pose[3], base_pose[4], base_pose[5])
    zero_matrix = tools.xyzrpy2Mat(self.initial_pose_rpy[0],self.initial_pose_rpy[1],self.initial_pose_rpy[2],
self.initial_pose_rpy[3],self.initial_pose_rpy[4],self.initial_pose_rpy[5])
    end_matrix = tools.xyzrpy2Mat(pose_data[0], pose_data[1], pose_data[2],
                                            pose_data[3], pose_data[4], pose_data[5])
    result_matrix = np.dot(zero_matrix, np.dot(np.linalg.inv(begin_matrix), end_matrix))
    xyzrpy = tools.mat2xyzrpy(result_matrix)
return xyzrpy

这个函数正是利用了变换矩阵的运算法则来实现增量控制。我们来逐步分解代码：

1. 1. 输入参数:

• • base_pose: 这是遥操作开始时的基准位姿。当遥操作触发时，系统会记录下当时操作设备的位姿，并将其存为 self.base_pose。这个位姿是计算后续所有增量的“起点”或“参考零点”。
• • pose_data: 这是当前时刻从操作设备（Pika sense）实时接收到的位姿数据。

• 2. 矩阵转换: 函数首先将三个关键的位姿（用 [x, y, z, roll, pitch, yaw] 格式表示）转换成4x4的齐次变换矩阵。这通常由 tools.xyzrpy2Mat 函数完成。
• • begin_matrix: 由 base_pose 转换而来，代表遥操作开始时操作设备的位姿矩阵。我们称之为 T_begin。
• • zero_matrix: 由 self.initial_pose_rpy 转换而来，代表遥操作开始时机械臂末端的位姿矩阵。这是机械臂运动的“起始点”。我们称之为 T_zero。
• • end_matrix: 由 pose_data 转换而来，代表当前时刻操作设备的位姿矩阵。我们称之为 T_end。
• 3. 核心计算: 这是最关键的一行代码：

result_matrix = np.dot(zero_matrix, np.dot(np.linalg.inv(begin_matrix), end_matrix))

我们用矩阵乘法来分析它：

所以，result_matrix 最终得到的是机械臂在世界坐标系下，当前应该达到的目标位姿矩阵。

公式表示: Result = T_zero * (T_begin)⁻¹ * T_end

- np.linalg.inv(begin_matrix): 计算 begin_matrix 的逆矩阵，即 (T_begin)⁻¹。在机器人学中，一个变换矩阵的逆矩阵表示其反向变换。
- np.dot(np.linalg.inv(begin_matrix), end_matrix): 这一步计算的是 (T_begin)⁻¹ * T_end。这个运算的物理意义是：从 begin 坐标系变换到 end 坐标系所需要进行的变换。换句话说，它精确地描述了从遥操作开始到现在，操作设备所产生的相对位姿变化（增量）。我们称这个增量为 ΔT。
- np.dot(zero_matrix, ...): 这一步计算的是 T_zero * ΔT。它的物理意义是：将刚刚计算出的相对位姿变化 (ΔT) 应用到机械臂的初始位姿 (T_zero) 上。

1. 4. 结果转换与返回:
• • xyzrpy = tools.mat2xyzrpy(result_matrix): 将计算出的4x4目标位姿矩阵 result_matrix 转换回机器人控制器能理解的 [x, y, z, roll, pitch, yaw] 格式。
• • return xyzrpy: 返回这个计算出的目标位姿。

 五、将 6d Pose 数据映射到机械臂 

在通过增量式控制时我们获取到了机械臂需要执行的相对位姿指令。但问题是每一款机械臂的控制指令都不太一样，这就需要为每一款机械臂编写不同的控制接口，比如 Piper、Xarm 的机械臂，它们都能直接下发 xyzrpy 或是 xyz + 四元数去对机械臂进行控制，只不过 Piper 使用的是 rostopic 方式去发布，而 Xarm 使用的是 rosservice 方式去请求。ur 的机械臂，它使用的是 xyz 以及旋转矢量的方式去下发。

总的来说，要想将增量式算到的 6d Pose 数据下发到机械臂对其进行控制的话，最后还要做的就是适配好机械臂的控制接口。




 六、总结 

这篇文章详细阐述了基于 Pika sense 实现机械臂遥操作的核心技术原理，整个流程可以概括为四个关键步骤：
1、获取6D位姿数据：首先，利用 Pika sense 和外部定位基站组成的系统来精确捕捉操作者的手部运动。基站通过红外同步信号和旋转激光扫描空间，Pika sense 上的光敏传感器接收这些信号，并实时解算出自身高精度的六自由度（6D）位姿（位置和姿态），然后通过 ROS 话题或 SDK 发布这些数据。

2、坐标系对齐：由于 Pika sense 的坐标系与不同机械臂末端的坐标系定义不一致，必须进行对齐。通过获取 Pika sense 和目标机械臂各自的坐标系定义，计算出一个转换矩阵，将 Pika sense 的位姿数据转换到与机械臂末端相匹配的坐标系下，确保后续控制的直观性。

3、增量式控制：为了让机械臂能够平滑地跟随操作者的相对运动，而不是突兀地跳到某个绝对位置，文章采用了增量式控制策略。该方法以遥操作开始时的手部姿态和机械臂姿态为基准，通过矩阵运算实时计算出手部从“起始点”到“当前点”的相对位姿变化量（增量），再将这个增量应用到机械臂的初始位姿上，从而得到机械臂当前的目标位姿。

4、映射到机械臂：最后一步是将计算出的目标位姿指令发送给机械臂执行。由于不同品牌和型号的机械臂（如 PiPER、XArm、 UR）有各自不同的控制接口和通信协议（如 ROS topic、ROS service、 特定格式指令），因此需要编写相应的适配代码，将标准的6D位姿数据格式化为特定机械臂能够识别和执行的命令，最终实现精准的遥操作控制。