## 2 Beidou-Binocular Cooperative Navigation Model
### Intelligent Collaborative Fusion and Path Planning Module
#### 2.3.1 Multi-Sensor Data Fusion  
The Beidou navigation positioning module provides global position information, but is affected by signal interference and blockage. The binocular vision recognition module provides local environmental perception data with high accuracy, but cannot provide global position references. The intelligent collaborative fusion module uses the Unscented Kalman Filter (UKF) to fuse Beidou navigation data with binocular vision data, thereby generating a unified robot state estimate. The choice of the Unscented Kalman Filter (UKF) is critical for the BBCN model, and this choice was made after careful consideration of alternatives such as the Extended Kalman Filter (EKF) and the Particle Filter (PF). The robot's kinematic model and the sensor measurement functions are inherently non-linear. The EKF addresses non-linearity by performing a first-order Taylor series linearization, which can lead to significant estimation errors and even divergence when non-linearities are pronounced. The PF, while highly effective for non-Gaussian and non-linear systems, is computationally intensive as it requires a large number of particles to accurately approximate the posterior distribution, posing a challenge for real-time implementation on embedded hardware. The UKF, in contrast, uses the unscented transform to deterministically select a set of \"sigma points\" that capture the true mean and covariance of the state distribution up to the second order. This approach avoids the need for Jacobian matrix calculations and provides higher accuracy than the EKF for non-linear systems, while remaining significantly more computationally efficient than the PF. Therefore, the UKF represents the optimal trade-off between accuracy, robustness, and computational feasibility for our real-time multi-sensor fusion task. The system's state vector \(x_{k}\) includes the robot's position, velocity, attitude, and errors from both the Beidou and vision sensors:  
\[x_{k}=[x,y,\theta,v_{x},v_{y},\omega,b_{\text{GNSS}},b_{\text{vision}}]^{T} \tag{12}\]  
where \(x,y,\theta\) are the robot's position information; \(v_{x},v_{y}\) are the velocity components; \(\omega\) is the angular velocity; \(b_{\text{GNSS}}\) is the Beidou positioning error; and \(b_{\text{vision}}\) is the vision error. Assuming the robot's state \(x_{k}\) at time \(t_{k}\) is updated through a motion model, the state transition equation is as follows:  
\[x_{k+1}=f(x_{k},u_{k})+w_{k} \tag{13}\]  
where \(f(x_{k},u_{k})\) is the state transition function, \(u_{k}\) is the control input (such as speed and direction), and \(w_{k}\) is the process noise. The observation model associates the measurements from the Beidou navigation module and the binocular vision module with the state vector:  
\[z_{k}=h(x_{k})+v_{k} \tag{14}\]  
where \(z_{k}\) includes the positions measured by Beidou \([x_{\text{GNSS}},y_{\text{GNSS}}]\) and by vision \([x_{\text{vision}},y_{\text{vision}}]\); \(h(x_{k})\) is the observation function; and \(v_{k}\) is the observation noise. The UKF models the state and observation uncertainties using unscented transforms without requiring linearization of the state equations, making it suitable for non-linear systems. The application of the UKF in the BBCN model is shown in Figure 5.  
By decomposing the state covariance matrix \(P_{k}\), \(2n+1\) sigma points are generated:  
\[ \begin{cases}\chi_{0}=x_{k}\\ \chi_{i}=x_{k}\pm\sqrt{(n+\lambda)P_{k}},\quad i=1,\dots,n\end{cases}  \tag{15}\]  
Each sigma point is substituted into the state transition equation, and the predicted observations are obtained through the observation model. By combining the actual observations \(z_{k}\), the state is updated using the Kalman gain:  
\[ \begin{cases}x_{k+1}=\hat{x}_{k+1|k}+K_{k}(z_{k}-\hat{z}_{k+1|k})\\ K_{k}=P_{xz}P_{zz}^{-1}\\ z_{k+1}=h(\chi_{k+1})\\ \chi_{k+1}=f(\chi_{k},u_{k})\end{cases}