\documentclass[11pt,a4paper]{article} \usepackage[utf8]{inputenc} \usepackage[T1]{fontenc} \usepackage[ngerman]{babel} \usepackage[margin=2.4cm]{geometry} \usepackage{amsmath,amssymb,bm,mathtools} \usepackage{booktabs} \usepackage{xcolor} \usepackage{listings} \usepackage{graphicx} \usepackage{enumitem} \usepackage[hidelinks]{hyperref} \definecolor{codebg}{rgb}{0.96,0.96,0.97} \definecolor{kw}{rgb}{0.2,0.3,0.7} \lstset{ basicstyle=\ttfamily\small, backgroundcolor=\color{codebg}, frame=single, rulecolor=\color{gray!40}, breaklines=true, columns=fullflexible, keepspaces=true, showstringspaces=false, numberstyle=\tiny, xleftmargin=4pt, xrightmargin=4pt, aboveskip=6pt, belowskip=6pt, } \newcommand{\R}{\mathbb{R}} \newcommand{\vx}{\mathbf{x}} \newcommand{\vX}{\mathbf{X}} \newcommand{\vu}{\mathbf{u}} \newcommand{\vn}{\mathbf{n}} \newcommand{\vp}{\mathbf{p}} \newcommand{\vt}{\mathbf{t}} \newcommand{\vq}{\mathbf{q}} \newcommand{\va}{\hat{\mathbf{a}}} \newcommand{\vY}{\mathbf{Y}} \newcommand{\vv}{\mathbf{v}} \newcommand{\vr}{\mathbf{r}} \newcommand{\vd}{\mathbf{d}} \newcommand{\norm}[1]{\left\lVert #1 \right\rVert} \title{\textbf{Roboter-Pose-Erkennung aus Mehrkamera-ArUco-Aufnahmen}\\[2pt] \large Pipeline, mathematische Grundlagen und Konfiguration} \author{appRobotRendering --- technische Dokumentation} \date{\today} \begin{document} \maketitle \begin{abstract} \noindent Diese Dokumentation beschreibt eine Bildverarbeitungs-Pipeline, die aus synchronen Fotos eines Roboterarms von mehreren kalibrierten Kameras die Gelenkstellung (sieben Achsen $x,y,z,a,b,c,e$) rekonstruiert. Auf dem Roboter und einer Referenzplatte (\emph{Board}) sind ArUco-Marker angebracht. Aus den \emph{vier Eckpunkten} jedes Markers wird per Mehrsicht-Triangulation eine volle 6-DOF-Markerpose (Position \emph{und} Flächennormale) bestimmt; aus diesen Beobachtungen werden über das Vorwärtskinematik-Modell die Gelenkwinkel geschätzt. Es werden vier austauschbare Schätzverfahren beschrieben und an simulierten Szenen mit bekannter Grundwahrheit validiert (mittlerer Winkelfehler $0{,}25^\circ$, Positionsfehler $0{,}10$\,mm). \end{abstract} \tableofcontents \bigskip % ===================================================================== \section{Einführung und Problemstellung} % ===================================================================== Gegeben sind $C$ kalibrierte Kameras, die denselben Roboter gleichzeitig fotografieren. Der Roboter trägt eine kinematische Kette starrer Glieder (\emph{Links}); auf den Gliedern und auf der fixen Bodenplatte sitzen ArUco-Marker bekannter Größe. Das \emph{Board} ist die Wurzel der Kette und definiert das Weltkoordinatensystem $W$. Gesucht ist der Gelenkvektor \[ \vq = (x, y, z, a, b, c, e) \in \R^7, \] mit linearen Achsen $x,e$ (in mm) und rotatorischen Achsen $y,z,a,b,c$ (in Grad). Die zentrale Idee ist, dass ein ArUco-Marker \emph{kein Punkt} ist, sondern ein ebenes Quadrat bekannter Kantenlänge mit vier detektierten Eckpunkten. Werden diese vier Ecken über mehrere Kameras trianguliert, liefert jeder einzelne Marker neben seiner Position auch eine \emph{gemessene} Orientierung. Dadurch genügt bereits ein einziger sichtbarer Marker, um den Winkel des Gliedes zu bestimmen, auf dem er sitzt. % ===================================================================== \section{Notation und Koordinatensysteme} % ===================================================================== Punkte im Weltsystem $W$ werden mit $\vX\in\R^3$ bezeichnet (Einheit mm). Eine Kamera $c$ besitzt die Intrinsikmatrix $K_c\in\R^{3\times3}$, die Verzeichnungskoeffizienten $D_c$ sowie die Extrinsik $(R_c, \vt_c)$ in \emph{world-to-camera}-Konvention, \[ \vX^{\mathrm{cam}} = R_c\,\vX + \vt_c , \qquad R_c\in SO(3). \] Das Kamerazentrum im Weltsystem ist $\mathbf{C}_c = -R_c^{\top}\vt_c$. Die normierte (intrinsikfreie) Projektion eines Weltpunkts lautet \begin{equation} \tilde{\pi}(\vX;R_c,\vt_c)= \frac{1}{[\vX^{\mathrm{cam}}]_3}\begin{pmatrix}[\vX^{\mathrm{cam}}]_1\\[2pt][\vX^{\mathrm{cam}}]_2\end{pmatrix}\in\R^2 . \label{eq:proj} \end{equation} Eine beobachtete Pixelkoordinate $\vu$ wird mittels $\tilde{\vu}=\mathrm{undistort}(\vu;K_c,D_c)$ in dieselbe normierte Ebene überführt. Rotationen werden bei der Optimierung über den Rodrigues-Vektor $\bm{\omega}\in\R^3$ mit $R=\exp([\bm{\omega}]_\times)$ parametrisiert. % ===================================================================== \section{Pipeline-Architektur} % ===================================================================== Die Verarbeitung erfolgt in vier Stufen (Skript \texttt{run/run\_pipeline.bat}): \begin{center} \begin{tabular}{@{}lll@{}} \toprule Stufe & Skript & Ausgabe \\ \midrule 1 & \texttt{1\_detect\_aruco\_observations.py} & \texttt{*\_aruco\_detection.json} (Ecken) \\ 2 & \texttt{2\_estimate\_camera\_from\_observations.py} & \texttt{*\_camera\_pose.json} \\ 3 & \texttt{3\_multiview\_bundle\_adjustment\_v4.py} & \texttt{aruco\_positions\_initial.json} \\ 3b & \texttt{3b\_corner\_marker\_poses.py} & \texttt{aruco\_marker\_poses.json} \\ 4 & \texttt{pose\_estimation.py} & \texttt{robot\_state.json} \\ \bottomrule \end{tabular} \end{center} Stufe 3 (reine Center-Triangulation) wird für Visualisierung und Vergleich beibehalten; die eigentliche Posenschätzung nutzt die in Stufe 3b erzeugten Eckposen. % ===================================================================== \section{Stufe 1: ArUco-Detektion} % ===================================================================== Pro Bild liefert der OpenCV-ArUco-Detektor für jeden erkannten Marker $m$ die vier Eckpunkte $\vu_{m,0},\dots,\vu_{m,3}$ im Uhrzeigersinn sowie deren Mittelpunkt. Zusätzlich werden Qualitätsmaße (Fläche, Schärfe als Laplace-Varianz, Kontrast, Randabstand, Kantenverhältnis) berechnet und eine heuristische Konfidenz abgeleitet. Die Eckpunkte $\vu_{m,k}$ sind die \emph{primären} Messungen; der Mittelpunkt ist nur ihr Mittelwert. % ===================================================================== \section{Stufe 2: Kamera-Pose-Schätzung} % ===================================================================== Die Board-Marker haben bekannte Weltpositionen $\vX_i$ (aus \texttt{robot.json}). Aus ihren beobachteten Mittelpunkten wird die Pose jeder Kamera geschätzt. Ein robuster Anfangswert ergibt sich aus \texttt{solvePnP} (IPPE-Square) je Marker, gefolgt von einem starren Ausgleich der per-Marker-Kamerazentren. Verfeinert wird mit Levenberg--Marquardt auf den normierten Reprojektionsresiduen: \begin{equation} \min_{\bm{\theta}}\ \sum_{i} \norm{\,\tilde{\vu}_i - \tilde{\pi}\!\big(\vX_i;\,R(\bm{\omega}),\vt\big)}^2, \qquad \bm{\theta}=(\bm{\omega},\vt)\in\R^6 . \label{eq:campose} \end{equation} Aus der Jacobi-Matrix am Optimum wird eine $6\times6$-Kovarianz und daraus die Unsicherheit von Kamerazentrum und Orientierung propagiert. Bemerkung: Da die Board-Marker nahezu koplanar sind ($z\approx0$), ist die Tiefe schlecht konditioniert --- eine korrekte Intrinsik ist deshalb essenziell (siehe Abschnitt~\ref{sec:limits}). % ===================================================================== \section{Stufe 3b: Eck-Triangulation und Normalenschätzung} % ===================================================================== \subsection{Mehrsicht-Triangulation (DLT)} Sei $V_{m,k}\subseteq\{1,\dots,C\}$ die Menge der Kameras, die Ecke $k$ von Marker $m$ sehen. Mit der normierten Projektionsmatrix $P_c=[\,R_c \mid \vt_c\,]$ (Zeilen $\vp_{c,1}^\top,\vp_{c,2}^\top,\vp_{c,3}^\top$) und der entzerrten Beobachtung $\tilde{\vu}=(x,y)$ liefert jede Kamera zwei lineare Gleichungen in den homogenen Punktkoordinaten $\vX_h$: \begin{equation} x\,\vp_{c,3}^\top \vX_h - \vp_{c,1}^\top \vX_h = 0,\qquad y\,\vp_{c,3}^\top \vX_h - \vp_{c,2}^\top \vX_h = 0 . \end{equation} Das Stapeln über alle $c\in V_{m,k}$ ergibt $A\,\vX_h=\mathbf 0$; die Lösung ist der rechte Singulärvektor zum kleinsten Singulärwert von $A$, gefolgt von der Dehomogenisierung $\vX = \vX_h^{1:3}/\vX_h^{4}$. Dies wird für alle vier Ecken ausgeführt und liefert $\vY_0,\dots,\vY_3$. \subsection{Position und Normale} Die Markerposition ist der Schwerpunkt $\bar{\vY}=\tfrac14\sum_k\vY_k$. Die Normale ist der Normalenvektor der Ausgleichsebene durch die vier Ecken: mit $M=[\vY_0-\bar{\vY};\dots;\vY_3-\bar{\vY}]\in\R^{4\times3}$ und der Singulärwertzerlegung $M=U\Sigma V^\top$ ist \begin{equation} \vn = \pm\,\vv_3 \quad(\text{kleinster Singulärvektor}), \end{equation} wobei das Vorzeichen über die ArUco-Eckreihenfolge so gewählt wird, dass $\vn$ zur Kamera weist (Konvention wie in der Grundwahrheit): $\vn\leftarrow-\vn$, falls $\vn\cdot\big((\vY_1-\vY_0)\times(\vY_2-\vY_0)\big)>0$. Die Validierung (\texttt{benchmark/stage0\_corner\_normals.py}) ergibt einen Median-Normalenfehler von $1{,}2^\circ$ ($p_{90}=3{,}0^\circ$); die Aufschlüsselung je Glied (Tabelle~\ref{tab:normals}) zeigt, dass selbst die kleinen, weit entfernten Fingermarker eine Orientierung von rund $1^\circ$ liefern --- die Grundlage dafür, dass ein einzelner Marker seinen Gelenkwinkel bestimmen kann. \begin{table}[h] \centering \begin{tabular}{@{}lccc@{}} \toprule Glied & \# Marker & Normalenfehler Mittel [$^\circ$] & Max [$^\circ$] \\ \midrule Arm1 & 3 & 0{,}64 & 0{,}94 \\ Arm2 & 4 & 0{,}78 & 1{,}04 \\ Ellbow & 4 & 0{,}96 & 1{,}99 \\ FingerA & 2 & 0{,}96 & 1{,}14 \\ FingerB & 2 & 1{,}43 & 1{,}44 \\ Board & 41 & 1{,}79 & 7{,}07 \\ \bottomrule \end{tabular} \caption{Genauigkeit der aus Ecken abgeleiteten Normale je Glied (Scene8).} \label{tab:normals} \end{table} % ===================================================================== \section{Vorwärtskinematik} % ===================================================================== Die Links werden topologisch (Wurzel zuerst) sortiert. Für Link $l$ mit Eltern-% link $p$ gilt \begin{equation} T_l \;=\; T_p \cdot \underbrace{\mathrm{Tr}(\mathbf{m}_l)\,R_{xyz}(\bm{\rho}_l)}_{\text{Mount}} \cdot \underbrace{\mathrm{Tr}(\mathbf{o}_l)\,R_{xyz}(\bm{\sigma}_l)}_{\text{Gelenkursprung}} \cdot Q_l(q_l), \end{equation} mit der Gelenkbewegung \begin{equation} Q_l(q_l)= \begin{cases} R(\hat{\mathbf{s}}_l,\,q_l) & \text{revolut (Achse }\hat{\mathbf{s}}_l,\text{ Winkel }q_l),\\ \mathrm{Tr}(\hat{\mathbf{s}}_l\, q_l) & \text{linear (Verschiebung }q_l),\\ I & \text{fest.} \end{cases} \end{equation} Ein Marker mit lokaler Position $\vp_m^{\text{lok}}$ und lokaler Normale $\vn_m^{\text{lok}}$ erscheint im Weltsystem als \begin{equation} \vp_m(\vq)=T_{l(m)}(\vq)\begin{psmallmatrix}\vp_m^{\text{lok}}\\1\end{psmallmatrix}, \qquad \vn_m(\vq)=R\big(T_{l(m)}(\vq)\big)\,\vn_m^{\text{lok}} . \label{eq:fkmarker} \end{equation} Die Kette dieses Roboters ist in Tabelle~\ref{tab:chain} zusammengefasst. Bemerkenswert: \emph{Base}, \emph{Hand} und \emph{Palm} tragen keine eigenen Marker, und beide Finger teilen sich die Variable $e$ (mit entgegengesetzten Achsen --- ein Greifer). \begin{table}[h] \centering \begin{tabular}{@{}llllc@{}} \toprule Link & Eltern & Gelenktyp & Variable & Achse \\ \midrule Board & --- & (Wurzel) & --- & --- \\ Base & Board & linear & $x$ & $[1,0,0]$ \\ Arm1 & Base & revolut & $y$ & $[-1,0,0]$ \\ Ellbow & Arm1 & revolut & $z$ & $[-1,0,0]$ \\ Arm2 & Ellbow & revolut & $a$ & $[0,-1,0]$ \\ Hand & Arm2 & revolut & $b$ & $[1,0,0]$ \\ Palm & Hand & revolut & $c$ & $[0,-1,0]$ \\ FingerA & Palm & linear & $e$ & $[1,0,0]$ \\ FingerB & Palm & linear & $e$ & $[-1,0,0]$ \\ \bottomrule \end{tabular} \caption{Kinematische Kette: sieben Freiheitsgrade, davon zwei linear ($x,e$).} \label{tab:chain} \end{table} % ===================================================================== \section{Pose-Estimation: Vier Schätzverfahren} % ===================================================================== Die Beobachtungen sind die Eckposen $\{(\vY_m, \vn^{o}_m)\}_{m\in\mathcal M}$ aus Stufe 3b. Alle Verfahren parametrisieren über die \emph{Gelenkvariablen} $\vq$ (nicht über Links), wodurch geteilte Variablen ($e$) und markerlose Glieder ($x,b,c$) generisch behandelt werden. Das gemeinsame Residuum eines Markers kombiniert Position und Normale, \begin{equation} \vr_m(\vq)=\Big[\;\vp_m(\vq)-\vY_m\,;\;\; w_n\big(\vn_m(\vq)-\vn^{o}_m\big)\Big]\in\R^6, \label{eq:res} \end{equation} mit der Normalengewichtung $w_n$ (Parameter \texttt{normal\_weight}). \subsection{Blockstruktur der Kette} Für die sequenziellen Verfahren wird die Kette in Blöcke zerlegt: Von der Wurzel fortschreitend werden die Variablen markerloser Gelenke gesammelt und mit der Variable des nächsten markertragenden Gelenks zu einem Block zusammengefasst, der aus den Markern dieses Gliedes (zuzüglich Geschwistern mit geteilter Variable) geschätzt wird. Für diesen Roboter ergeben sich \[ B_1=\{x,y\}\ \text{(Arm1)},\quad B_2=\{z\}\ \text{(Ellbow)},\quad B_3=\{a\}\ \text{(Arm2)},\quad B_4=\{b,c,e\}\ \text{(Finger)} . \] \subsection{Verfahren A --- \texttt{sequential\_vector}} Analytische Winkelbestimmung pro rotatorischem Gelenk mit $\ge2$ sichtbaren Markern auf demselben Glied. Mit der Weltachse $\va$ des Gelenks (aus der FK bei $q=0$) bildet man für Markerpaare $(i,j)$ den Modellvektor $\vd^{\text{mod}}_{ij}=\vp_j(q{=}0)-\vp_i(q{=}0)$ (im Weltsystem, da der lokale Linkframe durch die Elterngelenke bereits rotiert ist) und den Beobachtungs-% vektor $\vd^{\text{obs}}_{ij}=\vY_j-\vY_i$. Nach Projektion senkrecht zur Achse, $\vd^{\perp}=\vd-(\vd\cdot\va)\va$, ist der vorzeichenbehaftete Drehwinkel \begin{equation} \varphi_{ij}=\operatorname{atan2}\!\Big(\va\cdot(\vd^{\text{mod},\perp}_{ij}\times \vd^{\text{obs},\perp}_{ij}),\; \vd^{\text{mod},\perp}_{ij}\cdot \vd^{\text{obs},\perp}_{ij}\Big). \end{equation} Der Gelenkwinkel ist das mit $w_{ij}=\norm{\vd^{\text{mod},\perp}_{ij}}\,\norm{\vd^{\text{obs},\perp}_{ij}}$ gewichtete \emph{zirkuläre} Mittel $q^*=\operatorname{atan2}(\sum w\sin\varphi,\ \sum w\cos\varphi)$. Lineare, markerlose oder Einzelmarker-Gelenke werden über den Block-FK-Löser (Verfahren B) ergänzt. Sehr schnell, keine lokalen Minima, benötigt aber $\ge2$ Marker pro Gelenk. \subsection{Verfahren B --- \texttt{sequential\_fk}} Blockweiser nichtlinearer Ausgleich entlang der Kette. Für Block $B$ mit Variablen $\vq_B$ und Markern $\mathcal M_B$ werden die vorhergehenden Blöcke $\vq_{5^\circ$). % ===================================================================== \section{Validierung und Ergebnisse} % ===================================================================== \subsection{Methodik} Die Validierung nutzt synthetische Szenen aus einem Blender-Generator (\texttt{run/run\_sceneGenerator.bat}). Dieser rendert pro Pose $C=7$ Ansichten und schreibt neben Bildern (\texttt{render\_*.png}) und Intrinsiken (\texttt{render\_*.npz}) zwei Grundwahrheits-Dateien: \texttt{render\_*.json} mit den wahren Marker-Weltposen und \texttt{pose.json} mit den wahren Gelenkwinkeln. Drei Werkzeuge vergleichen die Rekonstruktion gegen diese Wahrheit: \texttt{stage0\_corner\_normals.py} (Normalenfehler der Triangulation), \texttt{9\_evaluateMarker.py} (Positions-/Normalenfehler je Marker und Glied) sowie \texttt{eval\_pose.py} (Gelenkwinkelfehler). Der Treiber \texttt{run\_benchmark.py} aggregiert über Szenen und Verfahren. \subsection{Ergebnisse} Über zehn simulierte Posen mit bekannter Grundwahrheit (eine fehlerhaft gerenderte Pose ausgeschlossen) ergeben sich die folgenden mittleren Gelenk-Winkelfehler. Die Standardabweichung über die Szenen ist das Maß für die \emph{Stabilität}. \begin{center} \begin{tabular}{@{}lccccc@{}} \toprule Verfahren & Mittel [$^\circ$] & Std [$^\circ$] & Schlechtest. [$^\circ$] & Mittel [mm] & Schlechtest. [mm]\\ \midrule \texttt{sequential\_vector} & 0{,}315 & 0{,}128 & 1{,}717 & 0{,}144 & 0{,}712\\ \texttt{sequential\_fk} & 0{,}434 & 0{,}171 & 1{,}838 & 0{,}158 & 0{,}851\\ \texttt{global\_ba} & 0{,}253 & 0{,}134 & 1{,}568 & 0{,}103 & 0{,}390\\ \textbf{\texttt{hybrid}} & \textbf{0{,}253} & \textbf{0{,}134} & \textbf{1{,}568} & \textbf{0{,}103} & \textbf{0{,}390}\\ \bottomrule \end{tabular} \end{center} Alle Verfahren liegen unter $0{,}5^\circ$ Mittelwert; \texttt{hybrid}/\texttt{global\_ba} sind am genauesten und am stabilsten und rekonstruieren auch die markerlosen Gelenke $b,c$ rein aus den Fingermarkern. % ===================================================================== \section{Grenzen und Ausblick} \label{sec:limits} % ===================================================================== \begin{itemize}[leftmargin=1.4em,itemsep=2pt] \item \textbf{Intrinsik.} Da die Board-Marker koplanar sind, ist die Kamerantiefe schlecht konditioniert; ein fehlerhaftes $f_y$ (z.\,B. ein früherer Brennweitenfehler von $\sim$15\,\% bei 16:9) verfälscht alle Höhen. Korrekte npz-Intrinsiken sind Voraussetzung. \item \textbf{Sicht-arme Posen.} Sind zu wenige Marker eines Gliedes sichtbar, ist der zugehörige Winkel schlecht bestimmt (\texttt{confidence=low}). Abhilfe: Multi-Pose durch Drehen der Achse $a$. \item \textbf{Adaptive Normalengewichtung.} Ein festes $w_n$ ist ein Kompromiss; eine pro-Marker nach Sichtqualität skalierte Gewichtung könnte Best- und Worst-Case gleichzeitig optimieren. \item \textbf{Reale Kameras.} Stufe 1 nutzt derzeit dieselbe Intrinsik-npz für alle Bilder (Simulation: identische Kameras); reale Aufbauten benötigen je Kamera eine eigene Kalibrierung. \end{itemize} \end{document}