# Navigation Path Extraction and Experimental Research of Pusher Robot Based on Binocular Vision
## 2 Materials and Methods
### Motion Modeling and Control
#### 2.2.1 Motion Modeling  
Without considering the slippage and sideslip of the crawler car [16], the motion model of the robot is shown in Figure 2. Firstly, we established the world coordinate system \(X_{\rm w}OY_{\rm w}\): \(X_{\rm w}\) represented the horizontal direction, O represented the origin of the world coordinate system, and \(Y_{\rm w}\) represented the direction perpendicular to the \(X_{\rm w}\) axis. Then, the kinematics model of the pusher robot was established in the coordinate system. The rectangle represented the chassis of the robot, the two small black squares represented the two driving wheels of the robot, and the circle inside the rectangle represented a universal wheel. Point C was located in the center of the two driving wheels, which was also the center of mass of the entire robot chassis. Point C coordinates were set to C (\(x\), \(y\)). The distance between the two wheels was represented by \(L\), and the radius of the wheel was represented by \(r\). The robot's own local coordinate system (\(X_{\rm r}CY_{\rm r}\)) was established with point C as the reference point of the robot, where \(X_{\rm r}\) pointed to the full right side of the moving robot's forward direction, and \(Y_{\rm r}\) indicated the forward direction of the mobile robot. \(\theta\) was the angle between the forward direction of the robot [17] and the world coordinate system \(X_{\rm w}\), which represented the pose angle of the robot. \(v_{l}\), \(v_{r}\), \(w_{l}\), and \(w_{r}\)  
Figure 1: The vehicle navigation hardware system.  
represented the linear and angular velocities of the left and right wheels. \(v\), \(w\), and \(O_{\rm C}\) represented the instantaneous velocity, angular velocity and instantaneous center of the robot motion, respectively.  
The robot's pose is expressed as:  
\[p\ =\ [x,y,\theta]^{T} \tag{1}\]  
The motion of the robot in the world coordinate system was mapped to the robot coordinate system by the orthogonal rotation matrix \(R_{(\theta)}\)[18]:  
\[R_{(\theta)}\ =\ \begin{bmatrix}\cos\theta&-\sin\theta&0\\ \sin\theta&\cos\theta&0\\ 0&0&1\end{bmatrix}\]  
\[\dot{p}_{j}\ =\ R_{(\theta)}{}^{-1}\cdot\dot{p} \tag{2}\]  
That is:  
\[\begin{bmatrix}\dot{x}_{j}\\ \dot{y}_{l}\\ \dot{\theta}_{j}\end{bmatrix}\ =\ \begin{bmatrix}\cos\theta&\sin\theta&0\\ -\sin\theta&\cos\theta&0\\ 0&0&1\end{bmatrix}\cdot\begin{bmatrix}\dot{x}\\ \dot{y}\\ \dot{\theta}\end{bmatrix} \tag{3}\]  
The motion equation of the robot's center of mass in the world coordinate system was as follows:  
\[\begin{cases}\dot{x}\ =\ \upsilon\cos\theta\\ \dot{y}\ =\ \upsilon\sin\theta\\ \dot{\theta}\ =\ \omega\end{cases} \tag{4}\]  
From this, the kinematics model of the robot was as follows:  
\[\begin{bmatrix}\dot{x}\\ \dot{y}\\ \dot{\theta}\end{bmatrix}\ =\ \begin{bmatrix}\cos\theta&0\\ \sin\theta&0\\ 0&1\end{bmatrix}\cdot\begin{bmatrix}\upsilon\\ \omega\end{bmatrix} \tag{5}\]  
Figure 2: Kinematics model of the pusher robot.  
It can be seen from Formula (5) that the pose of the robot was determined by the linear velocity and the angular velocity, and, according to the kinematics principle, the velocity at point C was as follows:  
\[V\ =\ \frac{V_{l}+V_{r}}{2} \tag{6}\]  
The pose vector of the mobile robot:  
\[\begin{bmatrix}\dot{x}\\ \dot{y}\\ \dot{\theta}\end{bmatrix}\ =\ \begin{bmatrix}\frac{r}{2}\cos\theta\frac{r}{2}\cos \theta\\ \frac{r}{2}\sin\theta\frac{r}{2}\sin\theta\\ -\frac{r}{L}\frac{r}{L}\end{bmatrix}\cdot\begin{bmatrix}\omega_{l}\\ \omega_{r}\end{bmatrix} \tag{7}\]  
It can be seen from Formula (7) that the pose state of the robot at each moment was obtained by using the angular velocity of the driving wheels on both sides of the robot at the corresponding moment.