【机械臂】【视觉】一、加入摄像机并实现世界坐标与像素坐标的互相转换

系列概述

临近本科毕业，考虑到未来读研的方向以及自己的兴趣方向，我选择的课题大致为"基于VLA结构的指令驱动式机械臂仿真系统的实现"。

为什么说是VLA(Vision-Language-Action)"结构"，因为就目前而言，我认为在目前剩下的几个月时间内从0实现一个正儿八经的VLA模型，所需要的时间、资金、模型资源的获取都是比较麻烦的。因此，我选择使用ROS1+LLM+视觉算法来实现一个"伪VLA"结构，就目前阶段(开题一个月)而言，我能给出的场景示意如下：

场景：指令输入"机械臂将蓝色方块夹起放到红色圆柱体上面"，系统接收指令后，先通过视觉模块确定当前系统中各个物体的坐标，再通过开源大模型，结合已知坐标信息，通过预设的prompt生成动作序列，作为参数送入ROS1架构下的启动文件中，实现动作行为在gazebo下的仿真。

当前阶段我能给出粗糙的逻辑示意图如下：

接下来，我将给出目前阶段我所计划的步骤实现，之后该系列的博客都会依照下面的框架进行更新。由于我也是初次入门ROS以及深度学习相关的内容，所以本系列博客更多的充当学习笔记的作用，在书写过程中难免会出现错误以及天真的理论理解，还请各位指正。

我将该项目的实现分成下面几个步骤(每个步骤下的博客会一步一步地更新)：

1. 实现机械臂在ROS1+Gazebo环境下的控制、仿真。目标效果是给出任意坐标的方块，机械臂要能稳定的抓取，并放置到指定的坐标。

该步骤博客目录如下：

https://blog.csdn.net/m0_75114363/article/details/156164226?spm=1001.2014.3001.5501
https://blog.csdn.net/m0_75114363/article/details/156166592?spm=1001.2014.3001.5502
https://blog.csdn.net/m0_75114363/article/details/156426200?spm=1001.2014.3001.5501
https://blog.csdn.net/m0_75114363/article/details/156544524?spm=1001.2014.3001.5502

2. 加入视觉模块与算法。目标效果是在仿真环境下，对于随意放置的方块，视觉系统需要计算出其真实坐标给予机械臂控制模块，使得机械臂能够实现对其的抓取与放置。

该步骤博客目录如下：

https://blog.csdn.net/m0_75114363/article/details/156641634?spm=1001.2014.3001.5502

3. 加入LLM(本地部署或使用API)。目标效果是对于输入的任意文本指令，LLM能根据预设的prompt,结合视觉系统给予的信息，给予执行模块对应的动作序列，使得机械臂正确地实现输入的文本指令想达到的效果，实现V-L-A的完整交互。

该步骤博客目录如下：

4. 实现整体系统的优化与完善，包括基于QT搭建软件前端、优化模型外观、加入更复杂的机械臂、实现更复杂的指令解析与运行。

该步骤博客目录如下：

我所使用的环境如下：

1. 系统:Ubuntu20.04

2. ROS1:Noetic

项目地址：

https://github.com/Dukiyaaa/Cmd2Action_ROS1

此外，额外说明一下为什么这个项目会选择ROS1做框架而不是更现代的ROS2。其实我在初期也是用的ROS2的框架，但发现机械臂夹爪始终无法夹起物体，网上相关的机械臂开源资料基本都是基于ROS1的，加上ROS1提供了grasp_fix插件可防止物体掉落，所以最终我选择了ROS1做项目框架，同时也希望自己在后期能够在ROS2上成功迁移项目。

---

章节概述

在之前的文章中，我实现了本机械臂系统基本驱动部分的开发，即给定抓取点和放置点的坐标，机械臂能够通过你运动解算成功抓起物体，并将其放在放置点上。

这一阶段，我将引入视觉，即不再直接显示给出坐标，而是通过yolov8检测出物体的坐标，进而送给执行器，实现抓取与放置的纯视觉闭环。

本文中，我将介绍视觉阶段的第一步：引入摄像机及实现世界坐标与像素坐标的互转。对于这个需求，更通俗的描述是，在相机固定在gazebo世界后，如果得知了待抓取物体在世界坐标下的坐标，那么便可以推算出其被拍到摄像机后，在图像上的像素坐标；反过来，如果我们拍到了物体，那么根据其在图片上的像素坐标，即可推算出其在世界坐标系下的坐标。

---

1. 相机的引入

我使用的是gazebo中的RGB相机和深度相机插件，但通过改变其相关相机参数，将其模拟成了常用的RGB-D相机d435i。

相关urdf(camera_d435i.urdf.xacro)如下：

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
    <!-- D435i 深度相机的 ROS 模拟配置 -->
    <xacro:macro name="d435i_camera">
        <!-- 相机 link -->
        <link name="camera_link">
            <collision>
                <origin xyz="0 0 0" rpy="0 0 0"/>
                <geometry>
                    <box size="0.09 0.025 0.025"/>
                </geometry>
            </collision>
            <visual>
                <origin xyz="0 0 0" rpy="0 0 0"/>
                <geometry>
                    <box size="0.09 0.025 0.025"/>
                </geometry>
                <material name="dark_gray">
                    <color rgba="0.2 0.2 0.2 1.0"/>
                </material>
            </visual>
            <inertial>
                <origin xyz="0 0 0" rpy="0 0 0"/>
                <mass value="0.1"/>
                <inertia ixx="1e-5" ixy="0.0" ixz="0.0" iyy="1e-5" iyz="0.0" izz="1e-5"/>
            </inertial>
        </link>
        
        <!-- 光学相机中心 link -->
        <link name="camera_color_optical_frame"/>
        <link name="camera_depth_optical_frame"/>
        
        <!-- 相机相对于世界的固定关节（俯视） -->
        <joint name="camera_fixed_joint" type="fixed">
            <parent link="world"/>
            <child link="camera_link"/>
            <!-- 放在世界上方，斜135度俯视工作区 -->
            <origin xyz="3.0 0 2.0" rpy="0 2.356 0"/>
        </joint>
        
        <!-- 光学 frame 相对于相机的关节 -->
        <joint name="camera_color_optical_joint" type="fixed">
            <parent link="camera_link"/>
            <child link="camera_color_optical_frame"/>
            <!-- Rotate so optical frame aligns with REP-103 when camera_link x:forward, y:right, z:down -->
            <origin xyz="0 0 0" rpy="1.5708 0 1.5708"/>
        </joint>
        
        <joint name="camera_depth_optical_joint" type="fixed">
            <parent link="camera_link"/>
            <child link="camera_depth_optical_frame"/>
            <origin xyz="0 0 0" rpy="1.5708 0 1.5708"/>
        </joint>
        
        <!-- Gazebo 插件配置：模拟 RealSense D435i -->
        <gazebo reference="camera_link">
            <sensor name="color_camera" type="camera">
                <camera>
                    <horizontal_fov>1.047</horizontal_fov>  <!-- 60 度 -->
                    <image>
                        <width>640</width>
                        <height>480</height>
                        <format>R8G8B8</format>
                    </image>
                    <clip>
                        <near>0.05</near>
                        <far>8.0</far>
                    </clip>
                </camera>
                <always_on>true</always_on>
                <update_rate>30</update_rate>
                <visualize>true</visualize>
                <plugin name="camera_controller" filename="libgazebo_ros_camera.so">
                    <alwaysOn>true</alwaysOn>
                    <updateRate>30.0</updateRate>
                    <cameraName>camera/color</cameraName>
                    <imageTopicName>image_raw</imageTopicName>
                    <cameraInfoTopicName>camera_info</cameraInfoTopicName>
                    <frameName>camera_color_optical_frame</frameName>
                    <hackBaseline>0.07</hackBaseline>
                </plugin>
            </sensor>
        </gazebo>
        
        <gazebo reference="camera_link">
            <sensor name="depth_camera" type="depth">
                <camera>
                    <horizontal_fov>1.047</horizontal_fov>
                    <image>
                        <width>640</width>
                        <height>480</height>
                    </image>
                    <clip>
                        <near>0.05</near>
                        <far>8.0</far>
                    </clip>
                </camera>
                <always_on>true</always_on>
                <update_rate>30</update_rate>
                <visualize>false</visualize>
                <plugin name="depth_camera_controller" filename="libgazebo_ros_depth_camera.so">
                    <alwaysOn>true</alwaysOn>
                    <updateRate>30.0</updateRate>
                    <cameraName>camera/depth</cameraName>
                    <imageTopicName>image_rect_raw</imageTopicName>
                    <cameraInfoTopicName>camera_info</cameraInfoTopicName>
                    <pointCloudTopicName>points</pointCloudTopicName>
                    <pointCloudCutoff>0.05</pointCloudCutoff>
                    <pointCloudCutoffMax>8.0</pointCloudCutoffMax>
                    <frameName>camera_depth_optical_frame</frameName>
                </plugin>
            </sensor>
        </gazebo>
    </xacro:macro>
</robot>

在这个URDF中，主要做了以下几件事情：

1. 建立camera_link：该link接入了RGB相机和深度相机插件，并通过camera_fixed_joint调节其在gazebo世界中的位置。换句话说，这个link就是摄像机。

2. 建立坐标修正关节camera_color_optical_joint和camera_depth_optical_joint：这两个关节用于保证相机拍摄到的图片以这两个关节的坐标系为基准。而这两个关节的坐标系，经过特定的rpy配置后，被设置为了朝向机械臂为Z轴，向右为X，向下为Y，这正是相机坐标转换理论里所要求的。

3. 相机参数的配置及相关话题发布：按照d435i的属性配置了相机的分辨率、FOV等，并将相机内参信息发布在camera_info话题上，图片数据发布在image_raw和image_rect_raw上，图片的坐标系参考为camera_color_optical_frame和camera_depth_optical_frame。

---

2. 内参标定

2.1. 概念解释

可以先看看这篇文章：https://silencejql.github.io/Opencv%E5%86%85%E5%8F%82%E6%A0%87%E5%AE%9A.html

在这篇文章中，阐述了什么是内参标定，以及需要标定的参数有哪些：

在实际的相机中，要想进行内参标定，一般需要使用标定板，考虑畸变，计算出相应的内参参数。

但是在gazebo中，相机模型是理想的，所以不用考虑畸变。并且，gazebo可以直接从URDF的FOV和分辨率计算出内参矩阵K，进而得到所要求的四个参数：fx,fy,cx,cy。

也就是说，根本不用我们自己计算，gazebo已经根据我们的urdf，把该干的事干了，我们只需要通过话题取数据就行。

2.2. 内参标定实现

相关的实现是非常简单的，在文件my_vision.py中，有这样的代码：

rospy.Subscriber('/camera/color/camera_info', CameraInfo, self._camera_info_callback)

def _camera_info_callback(self,msg):
        if self.fx is None:
            self.fx = msg.K[0]      # 焦距 x
            self.fy = msg.K[4]      # 焦距 y
            self.cx = msg.K[2]      # 光心 x
            self.cy = msg.K[5]      # 光心 y
            self.camera_matrix = np.array(msg.K).reshape(3, 3)
            
            rospy.loginfo(f'Camera calibrated: fx={self.fx}, fy={self.fy}, cx={self.cx}, cy={self.cy}')

之前提到过，相机内参信息是发布在camera_info话题上的，所以，我订阅了对应的话题，并在回调函数中进行了fx,fy,cx,cy的赋值，至此便非常方便地实现了内参标定。

---

3. 世界坐标与像素坐标的转换

3.1. 概念解释

可以先看看这篇文章：

https://www.cnblogs.com/yanghailin/p/17366654.html

在文章中，介绍了四种坐标系，分别是世界坐标、相机坐标、图像坐标、像素坐标，因此它们之间涉及到了三次转换：

1. 世界坐标转相机坐标：一个物体在世界坐标下的坐标为(x,y,z)，那么其在相机坐标下的坐标是多少呢？这个在数学上可通过旋转矩阵得到，在ros中可通过tf树查询得到。

2. 相机坐标转图像坐标：我们现在得到了物体在相机坐标系下的坐标，那么在拍摄好的图片上，这个物体在图片上的坐标是多少呢？这一步可通过相似三角形得到。

3. 图像坐标转像素坐标：这一步的转换其实非常简单，主要是把坐标轴由图像中心转到了图像左上角，单位由mm转为了像素，这一步通过仿射变换得到。

3.2. 代码实现

3.2.1. 世界坐标转相机坐标

# 世界坐标转相机坐标
    def world_to_cam_coordinate(self, x_world, y_world, z_world):
        ps = PointStamped()
        ps.header.stamp = rospy.Time(0)
        ps.header.frame_id = 'world' 
        ps.point.x, ps.point.y, ps.point.z = x_world, y_world, z_world

        try:
            # 从 world 变换到 camera_link
            transform = self.tf_buffer.lookup_transform(
                'camera_link', 'world', rospy.Time(0), rospy.Duration(1.0)
            )
            cam_ps = tf2_geometry_msgs.do_transform_point(ps, transform)
            return (cam_ps.point.x, cam_ps.point.y, cam_ps.point.z)
        except Exception as e:
            rospy.logwarn(f"TF transform failed: {e}")
            return None

这一步通过tf查询即可实现。

3.2.2. 世界坐标-相机坐标-图像坐标-像素坐标

# 直接从世界坐标到像素坐标，中间包含世界坐标转图像坐标的部分过程
    def world_to_pixel_coordinate(self, x_world, y_world, z_world):
        ps = PointStamped()
        ps.header.stamp = rospy.Time(0)
        ps.header.frame_id = 'world' 
        ps.point.x, ps.point.y, ps.point.z = x_world, y_world, z_world

        try:
            # 从 world 变换到 camera_color_optical_frame
            transform = self.tf_buffer.lookup_transform(
                'camera_color_optical_frame', 'world', rospy.Time(0), rospy.Duration(1.0)
            )
            cam_ps = tf2_geometry_msgs.do_transform_point(ps, transform)
            # 图像转像素坐标
            x_pixel = (cam_ps.point.x * self.fx) / cam_ps.point.z + self.cx
            y_pixel = (cam_ps.point.y * self.fy) / cam_ps.point.z + self.cy
            z_pixel = cam_ps.point.z
            return (x_pixel, y_pixel, z_pixel)
        except Exception as e:
            rospy.logwarn(f"TF transform failed: {e}")
            return None

这一小节我并没有一步一步来，先实现世界坐标转图像坐标，再转像素坐标，而是直接给出世界坐标转像素坐标的实现，原因在于世界坐标转图像坐标需要的焦距单位为mm，但gazebo给出的焦距单位为像素，所以无法单独的计算出世界坐标转图像坐标。

而对于世界坐标转像素坐标，我们先通过tf查询得到世界坐标在图像坐标系(实际就是camera_color_optical_frame)下的坐标，接着通过相似三角形+仿射变换，得到其在像素坐标下的坐标，单位为像素。核心代码如下：

x_pixel = (cam_ps.point.x * self.fx) / cam_ps.point.z + self.cx
y_pixel = (cam_ps.point.y * self.fy) / cam_ps.point.z + self.cy
z_pixel = cam_ps.point.z

这一步的推理见如下草稿：

3.2.3. 像素坐标回转世界坐标

# 像素坐标转回世界坐标
    def pixel_to_world_coordinate(self, u, v, depth):
        # 像素坐标转相机坐标
        x_cam = (u - self.cx) * depth / self.fx
        y_cam = (v - self.cy) * depth / self.fy
        z_cam = depth

        ps = PointStamped()
        ps.header.stamp = rospy.Time(0)
        ps.header.frame_id = 'camera_color_optical_frame' 
        ps.point.x, ps.point.y, ps.point.z = x_cam, y_cam, z_cam

        try:
            # 从 camera_link 变换到 world
            transform = self.tf_buffer.lookup_transform(
                'world', 'camera_color_optical_frame', rospy.Time(0), rospy.Duration(1.0)
            )
            world_ps = tf2_geometry_msgs.do_transform_point(ps, transform)
            return (world_ps.point.x, world_ps.point.y, world_ps.point.z)
        except Exception as e:
            rospy.logwarn(f"TF transform failed: {e}")
            return None

这一步就没什么好讲的了，做3.2.2的反操作即可。

---

4. 结果验证

在验证时，我写了这样的逻辑：

image_set_back = self.world_to_pixel_coordinate(0.8, 0.65, 0.05)
     if image_set_back is not None:
          rospy.loginfo(f"World to Pixel coords: {image_set_back}")

world_set_back = self.pixel_to_world_coordinate(image_set_back[0], image_set_back[1], image_set_back[2])
     if world_set_back is not None:
          rospy.loginfo(f"Pixel to World coords: {world_set_back}")

输入一个世界坐标系下的坐标(0.8,0.65,0.05)，将其送入world_to_pixel_coordinate，看看转出来的像素坐标是多少，再把该像素坐标送入pixel_to_world_coordinate，看看回转回来的世界坐标是多少，看看跟(0.8,0.65,0.05)差距大不大，结果如下：

[INFO] [1767672251.492427, 31.918000]: World to Pixel coords: (443.29689399336263, 206.99730754115987, 2.934461744067393)
[INFO] [1767672251.493873, 31.920000]: Pixel to World coords: (0.7999999999999989, 0.6500000000000005, 0.049999999999998934)

可以发现，经过世界-像素-世界的转换后，得到的坐标是(0.7999999999999989, 0.6500000000000005, 0.049999999999998934)，与原本的坐标(0.8,0.65,0.05)基本吻合，代码基本合理。

接着，我生成了一个坐标为(0.8,0.65,0.05)的方块，但给机械臂赋予的目标抓取点为(0.7999999999999989, 0.6500000000000005, 0.049999999999998934)，经过仿真，发现机械臂能够成功抓起该方块，说明我的坐标解算器的误差不会影响抓取精度。至此，验证完成闭环。

---

总结

这篇文章开始进入视觉阶段，介绍了相机的引入、内参标定、坐标转换，下一步是加入yolo算法，能够通过图像推理出像素坐标，再转回世界坐标供机械臂抓取。