基于融合深度信息的自动喷雾车全局非线性轨迹优化方法

Path Optimization Method Using Fusion Depth Information and Nonlinear Pose Estimation

针对传统的基于深度信息的喷雾车轨迹优化方法存在定位精度差、浮点漂移、深度信息帧易丢失等问题,提出了一种融合深度信息的全局非线性轨迹优化方法。在喷雾车前进过程中使用Real Sense传感器实时获取连续彩色信息帧,提取并优化重叠区域的FAST特征点,计算BRIEF描述子,通过快速最近邻算法进行特征匹配,并使用Nanoflann算法加速特征匹配过程。在获取连续关键帧的匹配点对后,对特征点对进行校验,剔除误匹配点对,利用对极几何融合深度信息计算两相邻关键帧部分匹配点对的本质矩阵,并针对剩余匹配点对进行重投影获取重投影误差。统筹全局连续关键帧,综合所有关键帧中匹配点的重投影误差,构建图优化,并通过Dogleg算法多次迭代获取当前喷雾车的精确位姿。该方法避免了单一依赖深度信息估计喷雾车轨迹时,出现位姿估计误差较大和深度信息帧丢失导致无法定位的问题。采用本文算法估计的喷雾车行驶轨迹更加接近于真实轨迹,其偏离真实轨迹误差均值下降了1. 07 cm,方差下降了2. 14 cm,超调量降低了2. 13 cm,提高了车行驶轨迹的鲁棒性。

In the agricultural field spray application process, the traditional human spray, because of large amount of labor, toxic to human body, was gradually replaced by other spray methods. One of the most popular methods is the smart spray of mobile cars. For autonomous driving vehicles applied with intelligent variable spray, the detection and accurate positioning of feature points play an important role in autonomous driving of robots. Feature detection is equivalent to the eyes of the car to obtain plant information, road condition. Accurate positioning is equivalent to the brain of the car. After the car acquires color information and depth information, it finds its exact position and guides the car to drive independently. In the process of continuous development of the visual synchronous localization algorithm of self-propelled vehicle, the traditional path optimization based on the traditional filtering form has the phenomenon of poor positioning accuracy and floating point drift. For the stable running of the car, precise spray has a great impact. To solve this problem, a method of global nonlinear optimization with depth information was proposed. The RealSense camera was used to obtain continuous color and depth information frames in real time. Firstly, through the continuous color information frames obtained, the FAST feature points of the overlapped part were extracted, the scale invariance and rotation invariance were optimized, and the BRIEF description was calculated to obtain the feature description of two consecutive key frame repetition regions. Then, feature matching was performed by the nearest neighbor algorithm, and Nanoflann algorithm was used to accelerate the matching process. After obtaining the matching point pair of continuous key frames, the minimum distance method was used to screen the mismatched points, and the random sampling consistency method (RANSAC) based on the basic matrix was used to test the matching point pair. After eliminating the false match and obtaining the correct match point, PnP was used to calculate the pose change of continuous key frames, calculate the residual error, and build the incremental equation. Dogleg algorithm was used to estimate the pose of continuous key frames for multiple iterations and optimization to obtain the precise pose of spray car. At the same time, in the process of calculating the residual error iterative optimization, the bit-pose calculated by the RealSense acquisition depth information and the bit-pose calculated by the polar constraint solution were integrated into the iterative optimization. Compared with the single depth information correction mode, the algorithm effectively improved the positioning accuracy of the car. When the depth information collection was lost, the polar constraint compensated the process of vehicle posture estimation, and improved the robustness of accurate real-time acquisition of vehicle track.