201 lines
7.2 KiB
TeX
201 lines
7.2 KiB
TeX
\documentclass[a4paper,11pt]{article}
|
|
|
|
% Zeichencodierung für deutsche Umlaute
|
|
\usepackage[utf8]{inputenc}
|
|
\usepackage[T1]{fontenc}
|
|
|
|
% Grafiken einbinden
|
|
\usepackage{graphicx}
|
|
|
|
% Deutsche Sprache (Datum, Trennungen, etc.)
|
|
%\usepackage[ngerman]{babel}
|
|
|
|
% Optional: bessere Schrift
|
|
\usepackage{lmodern}
|
|
|
|
\usepackage{caption}
|
|
|
|
% Then in your document preamble
|
|
\captionsetup[figure]{font=smaller}
|
|
|
|
|
|
\title{How to get a robot position from video images}
|
|
\author{Christoph Kendel}
|
|
\date{\today}
|
|
|
|
|
|
|
|
\begin{document}
|
|
|
|
\maketitle
|
|
|
|
\begin{center}
|
|
\begin{minipage}{0.89\linewidth}
|
|
\section{Info}
|
|
|
|
My Robot-Arm does not have homing switches. I use WebCams to get the position of each joint. This way
|
|
I can get the initial position without movement as well as contiuously check the position while working under load.
|
|
|
|
\vspace{2mm}
|
|
\hspace{5mm}
|
|
I work with Aruco markers. With budget low resolution WebCams I can get reliable positions (when using two cams). But the angular recognizion of the markers turned out
|
|
not to be reliable. Thus
|
|
I have to calculate the angle of each element from the relative positions of the markers. Here I explain how I these calculations are done.
|
|
|
|
\end{minipage}
|
|
\end{center}
|
|
|
|
\pagebreak
|
|
\section{Angles}
|
|
|
|
The linear $x$ Position is easy to get: the markers 201, 204, 200, 198, 229, 243 have all a fixed relative $x$ position. Thus the average
|
|
can be taken, wich is saved as $x_{242}$ the x-position of the hand-rotation-joint.
|
|
|
|
All the other values are the angles {\tt y, z, a, b, c} and {\tt e}. % (e for the opening-of the hand).
|
|
|
|
\subsection{Bizeps {\bf \tt y}}
|
|
|
|
To find the angle of the bizeps (upper arm) there are different options, depending on the angle-of-view and which
|
|
ArUcos are visable.
|
|
|
|
|
|
|
|
\begin{figure}[h!]
|
|
\includegraphics[width=\linewidth]{pic/robot_image_b.png}
|
|
\caption{Video Capture with recognized Aruco markers}
|
|
\end{figure}
|
|
|
|
|
|
\noindent
|
|
The calculation is based on two approaches: Position of the Markers 243 etc on the one hand, and relative position 198 $\leftrightarrow$ 229 on the other hand.
|
|
|
|
|
|
\vspace{2mm}
|
|
|
|
\noindent
|
|
\begin{minipage}{0.39\linewidth}
|
|
Several ways to calculate {\tt y}:
|
|
\begin{itemize}
|
|
\item 229 $\leftrightarrow$ 198 $\tan(y) = \frac{\Delta z}{\Delta y} $
|
|
\item From $y_{\rm Axis}$ and the positions of 243, 229, 198 (each position on its own) we can calculate $\tan(y+\delta)$
|
|
with a known $\delta$ from the geometry.
|
|
|
|
Thus we get {\tt y}
|
|
\end{itemize}
|
|
\end{minipage}\hfill
|
|
\begin{minipage}{0.6\linewidth}
|
|
\includegraphics[width=\linewidth]{pic/robot_sideView_measurements.pdf}
|
|
\end{minipage}
|
|
|
|
\vspace{2mm}
|
|
|
|
\noindent
|
|
Each availiable (if marker is visible) approach to calculate {\tt y} is done, and the mean is calculated. We can check if they deviate too much, and give
|
|
warnings.
|
|
|
|
\subsection{Ellbow -- Rotation}
|
|
|
|
At the ellbow there is a motor that turns the forearm arround. Depending on which Arucus are visible, the position {\tt a} of the motor can be calculated:
|
|
|
|
\begin{itemize}
|
|
\item {\tt x} of 223 or ... (only the $x$ Position, in the graphics in green). It is indipendent of all other angles.
|
|
\end{itemize}
|
|
%
|
|
%
|
|
the
|
|
{\tt y} and {\tt z} position of 223 etc can't be used, as it depends on the angle of the forearm and the angle of the bizeps. Thus is it less reliable. (Although it would be a nice
|
|
thing to use as a double--check). With the $x_{223}$ compared to the $x_{226}$ i can calculate the ellbow turning {\tt a} value:
|
|
|
|
|
|
$$
|
|
\sin({\tt a} + \alpha_{\rm }) = \frac{\Delta x}{35\,\rm mm} = \frac{x_{223} - x_{226}}{35\,\rm mm}
|
|
$$
|
|
|
|
|
|
\noindent
|
|
as I know the angle $\alpha$ (yellow triangle) from the printed geometry of the arm. The $x_{226}$ is calculated from all visible markers of the
|
|
sled and bizeps, as their markers are fixed in $x$ position. This $\alpha$ is calculated for all markers of the forearm.
|
|
|
|
|
|
\begin{figure}[h!]
|
|
\hspace*{-4.5mm}
|
|
\includegraphics[height=0.3\linewidth]{pic/robot_frontView_forearm.pdf}\hspace{6mm}
|
|
\includegraphics[height=0.3\linewidth]{pic/robot_sideView_forearm.pdf}
|
|
\caption{Front view and sideview of the forearm with calculation variables.}
|
|
\end{figure}
|
|
|
|
\noindent
|
|
As the rotation {\tt a} may change for different markers, the median is taken.
|
|
|
|
|
|
\subsection{Forearm}
|
|
|
|
The position of the forearm motor (controlling motor in the bizeps) can be calculated from the difference between the bizeps
|
|
position $y$ and the angle of the forearm. Thus we need the forearm--angle. That can be calculated in two ways
|
|
%
|
|
%
|
|
\begin{itemize}
|
|
\item relative position of two markers on the forearm, that are on one line.
|
|
\item relative $y, z$ position of one marker on the forearm and one marker on the ellbow, e.g. 242 or 222.
|
|
\end{itemize}
|
|
%
|
|
Again:
|
|
As I might get different results, I have to find a median value.
|
|
|
|
|
|
\vspace{2mm}
|
|
|
|
\hfill $\Rightarrow$ Hand-Joint $P$ is known.
|
|
|
|
\subsection{Hand}
|
|
|
|
To get the hand position, the easiest way I see is to calculate the middle position between both finger-markers%
|
|
\footnote{The
|
|
other way to calculate the hand position without the markers orientation would be: calculate the middle-hand-plane from
|
|
the forearm rotation, with the distance of the finger-point to that plane, you'd also get the finger-opening and the projectet position of the finger-marker
|
|
to that plane, you'd get the angle.}
|
|
($F_1$, $F_2$ here shown in green).
|
|
The direction $P \to M$ in combination with the forearm-direction results in the angle {\tt b}.
|
|
|
|
|
|
\begin{figure}[h!]
|
|
\hfill
|
|
\includegraphics[height=0.3\linewidth]{pic/robot_hand_sideView.pdf}
|
|
\hfill
|
|
\includegraphics[height=0.26\linewidth]{pic/robot_hand_topView.pdf}
|
|
\hfill {}
|
|
\caption{ Hand with points to calculate the values for {\tt b, c, e}.}
|
|
\end{figure}
|
|
|
|
|
|
\noindent
|
|
The Distance between the hand markers is a result of the opening of the fingers {\tt e}.
|
|
So I can calculate {\tt e} from that distance.
|
|
|
|
Furthermore the direction of the distance between the grippers gives me the rotation of the hand {\tt c}.
|
|
|
|
\paragraph{Limitations with the Hand:} The results depend heavily on the accuracy of $P$ and $M$. Both are never measured directly. So have to
|
|
double check if they make sense. Only if the distance $\overline {PM}$ checks up to be the mechanical correct, that can be used to evaluate the correctnes of $P$ and $M$.
|
|
And only if the point $M$ lies in the plane of the rotation {\tt a} arround the forearm.
|
|
|
|
|
|
\section{Program}
|
|
|
|
The NodeJS Program calculates the angles and checks, if they are the ones at the moment represented by the system.
|
|
|
|
First try is to just hard-code all the relevant ArucoPoints. That seems to be ok, but it gets impossible to maintain.
|
|
After a week I dont recognize my own code. It's hard to make improvements, when you first have to re-think all those
|
|
small steps, when you have constantly to look up all positions of the markers.
|
|
|
|
It seems to be a solution to save the geometry and all markers in one file. That can be edited, that can be improved.
|
|
And here I can save the relation between x,y, and the position of the Aruco Markers. Of course, at the moment
|
|
the geometry is still proitty much hard-coded. But i have some flexibility. And mainly: I can loop over {\sl all
|
|
Arucos on the Board} to find the coordinate system. Or I can loop over all Arucos that
|
|
attache to the ``arm1''.
|
|
|
|
\begin{figure}[h!]
|
|
\includegraphics[width=\linewidth]{pic/code_robot_definition.pdf}
|
|
\caption{Robot Geometry in a {\tt .json} File}
|
|
\end{figure}
|
|
|
|
\end{document} |