From e1ea4aee398c63c0f2be8abc52d307052ff4f551 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?=E6=9D=8E=E5=82=B2=E7=90=A6?= <skylia7mr@gmail.com>
Date: Fri, 24 Apr 2026 03:50:51 +0000
Subject: [PATCH] =?UTF-8?q?=E6=9B=B4=E6=96=B0=20=E4=BA=91=E5=8F=B0?=
 =?UTF-8?q?=E8=A7=86=E8=A7=89=E8=AE=BE=E8=AE=A1=E6=96=B9=E6=A1=88?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 ...%89%E8%AE%BE%E8%AE%A1%E6%96%B9%E6%A1%88.md | 53 ++++++++++++++++++-
 1 file changed, 51 insertions(+), 2 deletions(-)

diff --git a/%E4%BA%91%E5%8F%B0%E8%A7%86%E8%A7%89%E8%AE%BE%E8%AE%A1%E6%96%B9%E6%A1%88.md b/%E4%BA%91%E5%8F%B0%E8%A7%86%E8%A7%89%E8%AE%BE%E8%AE%A1%E6%96%B9%E6%A1%88.md
index bd17eb9..093adbd 100644
--- a/%E4%BA%91%E5%8F%B0%E8%A7%86%E8%A7%89%E8%AE%BE%E8%AE%A1%E6%96%B9%E6%A1%88.md
+++ b/%E4%BA%91%E5%8F%B0%E8%A7%86%E8%A7%89%E8%AE%BE%E8%AE%A1%E6%96%B9%E6%A1%88.md
@@ -10,8 +10,57 @@ yaw轴云台采用**DM-G6220中空电机**，pitch轴电机采用**DM-J4310-2EC
 
 ## 云台部分
 
-两个电机均为驱控一体，均具有**速度闭环**，主控板可按照通信协议控制速度环的目标值。同时，主控板读取BMI088传感器中的角度信息，在STM32芯片上计算PID控制量输出，输出量直接作为速度环的目标值，从而形成**角度闭环**。STM32通过**UART**接收从视觉处理器中传回的像素误差信息，使用PID控制器计算控制量输出，对输出量积分后作为角度环的目标值，从而形成**像素闭环**。另外，在系统中合适位置加入适当的Kalman估计器、低通滤波器和饱和器等，从而避免执行器超出物理限位、缓解比例冲击和微分冲击等。总之，采用三环串联PID结构，最终实现稳定的目标跟踪效果。
+两个电机均为驱控一体，均具有**速度闭环**，主控板可按照通信协议控制速度环的目标值。同时，主控板读取BMI088传感器中的角度信息，在STM32芯片上计算PID控制量输出，输出量直接作为速度环的目标值，从而形成**角度闭环**。STM32通过**UART**接收从视觉处理器中传回的激光点误差信息，使用PID控制器计算控制量输出，对输出量积分后作为角度环的目标值，从而形成**激光点位置闭环**。另外，在系统中合适位置加入适当的Kalman估计器、低通滤波器和饱和器等，从而避免执行器超出物理限位、缓解比例冲击和微分冲击等。总之，采用三环串联PID结构，最终实现稳定的目标跟踪效果。
 
 ## 视觉部分
 
-Jetson Orin Nano Super通过视觉处理获取像素误差，像素误差信息通过UART传输至STM32。算法流程大致为：图像采集与预处理、目标矩形检测、透视变换与校正、激光点位置解算、计算像素误差、发送串口数据，其中激光点位置解算部分不依赖于画面中激光点色块，而是基于电激光器和摄像头的绑定关系，通过数学模型进行解算。
\ No newline at end of file
+Jetson Orin Nano Super通过视觉处理获取像素误差，像素误差信息通过UART传输至STM32。算法流程大致为：图像采集与预处理、目标矩形检测、PnP解算、激光点位置解算、计算激光点误差、发送串口数据，其中激光点位置解算部分不依赖于画面中激光点色块，而是基于预先标定完成的电激光器和摄像头的绑定关系，通过数学模型进行解算。
+
+### 激光点位置解算详细说明
+
+本方案旨在通过视觉PnP算法解算相机位姿，结合预先标定的“相机-激光器”刚体关系，计算激光点在矩形平面（世界坐标系）上的精确位置。
+
+#### 1. 前置准备：相机内参标定
+首先对相机进行内参标定，获取相机内参矩阵 $K$ 和畸变系数。这是后续所有像素坐标转空间坐标的基础。
+
+#### 2. 第一阶段：激光器与相机的联合标定（离线）
+由于激光器与相机刚性绑定，激光束在**相机坐标系**下表现为一条固定的空间直线。我们需要通过以下步骤求出这条直线的方程（起点与方向）。
+
+1.  **数据采集**：
+    -   开启激光器，将激光点投射在平面标定板（如棋盘格）上。
+    -   保持相机与激光器相对静止，改变整体姿态（距离、角度），拍摄多组（建议10组以上）包含标定板和激光点的图像。
+
+2.  **解算相机坐标系下的激光点**：
+    -   **外参获取**：对每张图像使用PnP算法，根据标定板角点计算相机相对于标定板的外参矩阵 $T_{wc}$（即世界到相机的变换）。
+    -   **反投影求交**：
+        -   提取激光点的像素坐标 $(u, v)$。
+        -   利用 $K^{-1}$ 将其转换为归一化射线方向。
+        -   **关键步骤**：利用标定板在相机坐标系下的平面方程与该射线求交，计算出激光点在**相机坐标系**下的三维坐标 $P_{c\_i}$。
+
+3.  **拟合激光线方程**：
+    -   收集所有图像对应的相机坐标系下的激光点 $\{P_{c\_1}, P_{c\_2}, ...\}$。
+    -   使用最小二乘法或PCA（主成分分析）拟合这些点，得到相机坐标系下的激光直线方程。
+    -   **记录参数**：
+        -   **方向向量 $\vec{d}_{cam}$**：拟合直线的单位方向向量。
+        -   **基准起点 $O_{cam}$**：选取该直线与相机坐标系 $Z=0$ 平面的交点作为参考原点。
+
+#### 3. 第二阶段：在线测量与位置解算
+标定完成后，即可在实际应用中实时计算激光点位置。
+
+1.  **获取当前相机位姿**：
+    -   拍摄包含目标矩形平面的图像。
+    -   根据成像原理：$\text{像素坐标} \approx K \cdot [\text{归一化平面}] (T_{wc} \cdot \text{世界坐标})$。
+    -   已知像素坐标、内参 $K$ 以及矩形平面的特征点世界坐标，利用PnP算法解算出当前的外参矩阵 $T_{wc}$（包含旋转矩阵 $R$ 和平移向量 $t$）。
+
+2.  **坐标变换至世界坐标系**：
+    将标定阶段得到的激光线参数从相机坐标系变换到世界坐标系：
+    -   **变换方向向量**（仅受旋转影响）：
+        $$ \vec{d}_{world} = R^T \cdot \vec{d}_{cam} $$
+    -   **变换基准起点**（受旋转和平移共同影响，即乘以 $T_{wc}$ 的逆矩阵）：
+        $$ O_{world} = R^T \cdot (O_{cam} - t) $$
+
+3.  **求解世界坐标**：
+    -   构建世界坐标系下的激光射线方程：$L(s) = O_{world} + s \cdot \vec{d}_{world}$。
+    -   **平面约束**：已知目标平面在世界坐标系中满足 $Z_{world} = 0$。
+    -   **联立求解**：令射线的Z分量等于0，解出参数 $s$，代回方程求得 $X, Y$。
+    -   **最终输出**：激光点在矩形平面上的世界坐标 $(x, y, 0)$。
\ No newline at end of file