Outdoor mobile robot navigation method based on fusion of binocular vision, IMU and AGPS

Abstract

The application provides an outdoor mobile robot navigation method based on binocular vision, IMU and AGPS fusion, a binocular camera, an IMU and a 5G network module are carried on the outdoor mobile robot, and an AGPS function is opened; the outdoor mobile robot receives a navigation target point position through the 5G network; in the case that the environment in front is unknown, a D*‑TEB fusion algorithm is used to segmentally plan a path; the outdoor mobile robot moves according to the planned path, and fusion positioning information of the outdoor mobile robot is acquired through the AGPS, binocular vision and IMU during the movement; finally, the outdoor mobile robot reaches the target point according to the planned path, so that the positioning and navigation of the outdoor mobile robot in an unknown environment are realized. The binocular vision, IMU and AGPS are fused for positioning, the D* algorithm and the TEB algorithm are fused for path planning, the outdoor mobile robot can be positioned and navigated in the unknown environment without relying on pre-mapping, meanwhile, the cost can be effectively reduced, the outdoor mobile robot is convenient to use, and the outdoor mobile robot is practical and convenient.

Description

Technical Field

[0001] This invention belongs to the field of outdoor positioning and navigation technology for mobile outdoor robots, and in particular relates to a positioning and navigation method based on the fusion of binocular vision, IMU and AGPS multi-sensor in unknown environments. Background Technology

[0002] Outdoor mobile robots have broad application prospects, such as in smart agriculture and wilderness exploration. However, outdoor applications, especially in agriculture, present challenges due to unknown and complex environmental conditions. In the application of outdoor mobile robots, positioning and navigation technology is a key technology for their successful implementation.

[0003] Localization and navigation involve two main technologies: first, the outdoor mobile robot acquires or calculates its current position, knowing "where it is"; second, the outdoor mobile robot uses path planning algorithms to calculate a feasible path to the target point, knowing "how to get there". Among existing commonly used localization algorithms, GPS (Global Positioning System) has meter-level accuracy and cannot be directly used for outdoor mobile robot localization and navigation; LiDAR localization accuracy generally meets industrial needs, but high-precision LiDAR is expensive; visual sensor localization has the advantage of rich environmental information, but is greatly affected by lighting conditions; IMU (Inertial Measurement Unit) localization has the advantage of low cost, but is prone to drift, leading to large cumulative errors. Most existing path planning algorithms are based on known global environmental information before path planning, which makes them heavily reliant on prior knowledge such as maps, and difficult to apply to frequently changing environments such as farmland. Summary of the Invention

[0004] The purpose of this invention is to provide a navigation method for outdoor mobile robots based on the fusion of binocular vision, IMU, and AGPS (Assisted Global Positioning System) to solve the problems existing in the background art. To this end, this invention adopts the following technical solution:

[0005] A navigation method for outdoor mobile robots based on the fusion of binocular vision, IMU, and AGPS is characterized by:

[0006] The method involves equipping an outdoor mobile robot with a binocular camera, IMU, and 5G network module, and enabling AGPS functionality; the method includes the following steps:

[0007] Step 1: The outdoor mobile robot receives the navigation target point location. The navigation target point of the outdoor mobile robot is defined as the coordinates of the target point in a coordinate system established with the starting point of the outdoor mobile robot as the origin. The coordinates can be specified by remotely inputting the coordinates or by directly selecting them on the map and then sending them to the outdoor mobile robot via the 5G network.

[0008] Step 2: The outdoor mobile robot plans its path in segments when the environment ahead is unknown. The outdoor mobile robot does not need to pre-map the surrounding environment. It first selects a stage target point on the line connecting the starting point and the target point, and then plans the path based on the obstacle information observed by the binocular camera. The path planning method is to first use the D* algorithm to plan the initial path, and then use the TEB algorithm to optimize the path and output an executable path. After the binocular camera acquires environmental information, the obstacles are "expanded," that is, the boundaries of the obstacles are enlarged, and the expansion distance is set as a variable. This parameter can be adjusted during the actual operation of the outdoor mobile robot to ensure that the robot can safely avoid obstacles even when its own size changes.

[0009] Step 3: The outdoor mobile robot receives AGPS signals and performs differential processing to obtain meter-level positioning information;

[0010] Step 4: The outdoor mobile robot receives signals from the binocular camera and IMU, and the two are fused to output visual inertial odometry pose information.

[0011] Step 5: Perform coordinate transformation on the AGPS signal, converting the coordinates from the WGS-84 coordinate system to the ENU coordinate system, which is the same as the visual inertial odometry pose information;

[0012] Step 6: The AGPS positioning information in the ENU coordinate system and the visual inertial odometry pose information are fused using a graph optimization fusion method to solve the cumulative error problem of the visual inertial odometry.

[0013] Step 7: The outdoor mobile robot arrives at the target point according to the planned path.

[0014] While adopting the above technical solutions, the present invention may also adopt or combine the following further technical solutions:

[0015] As a preferred technical solution of the present invention: the specific process of the graph optimization fusion method in step 6 is as follows:

[0016] First, the pose information obtained from the binocular camera and the IMU is fused to output an estimated pose. The pose estimated by the binocular camera and the IMU is used as the local pose. Then, AGPS is used as the global sensor and converted into a factor in the factor graph. Together with the local pose estimate, it is input into the global pose graph to construct an optimization problem. Finally, a locally accurate and globally drift-free 6-DOF pose is generated.

[0017] The nodes of the factor graph are the poses in the world coordinate system. The edges between two consecutive nodes are local poses, which are local constraints obtained by the fusion of binocular vision and IMU. The other edges are global constraints generated by AGPS as a global sensor.

[0018] Factor graph optimization is a maximum likelihood estimation problem, as shown below:

[0019]

[0020] Where S is the set of pose estimation results from different methods, namely binocular vision, IMU, and AGPS. Let X represent the observations from each sensor, and let X be the pose node of the outdoor mobile robot over a period of time. The pose node can be represented as:

[0021]

[0022] in, 和 These are the position and rotation quaternions of the outdoor mobile robot in the global coordinate system, respectively. Assume that the measurements are independent and their errors follow a Gaussian distribution. The nonlinear least squares problem is obtained:

[0023]

[0024] in, It is the covariance matrix. The smaller the covariance, the more accurate the corresponding measurement. These are the observations from each sensor. This provides an approximate pose value for the outdoor mobile robot; finally, the pose estimates of each sensor are expressed as factor expressions in the factor graph, as follows:

[0025] The visual-inertial odometry factor after binocular vision and IMU fusion is:

[0026]

[0027] in, These are observations after the binocular camera and IMU are fused together. x is an approximate pose of an outdoor mobile robot. t-1For the pose of the outdoor mobile robot at time t-1, x t The pose of the outdoor mobile robot at time t;

[0028] The AGPS factor is:

[0029]

[0030] in, These are AGPS observations. This is an approximate pose value for an outdoor mobile robot. For AGPS location, The pose of the binocular camera after being fused with the IMU;

[0031] The graph optimization problem formed by the above equations can be solved using the G2O solver or the Ceres Solver solver.

[0032] This invention provides a navigation method for outdoor mobile robots based on the fusion of binocular vision, IMU, and AGPS. It adopts a technical solution that integrates binocular vision, IMU, and AGPS for positioning and combines the D* algorithm with the TEB algorithm. This method can achieve positioning and navigation of outdoor mobile robots in unknown environments without relying on pre-built maps. At the same time, it can effectively reduce costs, is easy to use, and is practical. Attached Figure Description

[0033] Figure 1 This is a schematic diagram of the navigation process of the present invention;

[0034] Figure 2 This is a schematic diagram of the path planning process of the present invention;

[0035] Figure 3 This is a schematic diagram of the multi-sensor fusion positioning method of the present invention;

[0036] Figure 4 This is a diagram of the sensor fusion factor of the present invention;

[0037] Figure 5 This is an application effect diagram of the present invention. Detailed Implementation

[0038] The invention will now be described in further detail with reference to the accompanying drawings and specific examples. A schematic diagram of the navigation process of the invention is shown below. Figure 1 As shown.

[0039] Step 1: The navigation target point for the outdoor mobile robot is defined as the position, distance, and direction of the target point relative to the starting point of the outdoor mobile robot, with the starting point of the outdoor mobile robot as the origin. The target point can be specified by remotely inputting coordinates or by selecting it directly on the map.

[0040] Step 2: The outdoor mobile robot plans its path in segments when the environment ahead is unknown. The specific implementation method is as follows: Figure 2 As shown.

[0041] First, the outdoor mobile robot receives the coordinates of the target navigation point. Based on the distance to the target point, it determines whether segmented planning is necessary. If the target is far away, segmented planning is used. Since the environment ahead is unknown, segmented planning reduces invalid paths and thus reduces computation. The outdoor mobile robot uses the D* algorithm to plan an initial path based on known environmental information. Then, it uses some key points on this path as control nodes in the TEB algorithm to optimize the path, finally outputting an executable path sequence. After the controller converts the path into motion control commands, the outdoor mobile robot executes the path. During movement, it uses obstacle information returned by sensors to determine whether a collision will occur. If the expected collision time is less than 3 seconds, the outdoor mobile robot stops and continues path planning; if it is greater than 3 seconds, it continues path planning without stopping. This design ensures the safety of the outdoor mobile robot's movement. Furthermore, in segmented planning, to explore the path ahead more promptly, the next round of path planning is set at 3 / 4 of the way to the stage target point. The first 1 / 4 of the path remains unchanged during planning, and planning proceeds backward from the next stage target point, fully utilizing the dynamic adjustment advantage of the D* algorithm. In the final stage of planning, since the stage target point coincides with the destination, there is no need for repeated planning at 3 / 4 of the way to the stage target point; the path is directly executed to the destination.

[0042] When determining whether the path collides with an obstacle, that is, whether the path intersects with an obstacle in the grid map, since the actual outdoor mobile robot will occupy a certain space, using lines to determine collisions will produce errors. Therefore, the method of expanding the obstacle outward is adopted to reduce this error.

[0043] Step 3: The outdoor mobile robot receives AGPS signals and obtains meter-level positioning information.

[0044] Step 4: The outdoor mobile robot receives signals from the binocular camera and IMU, and the two are fused to output visual inertial odometry pose information.

[0045] Step 5: Perform coordinate transformation on the AGPS signal, converting the coordinates from the WGS-84 coordinate system to the ENU coordinate system, which is the same as the VIO result. First, transform the WGS-84 coordinate system to the ECEF (Earth-Centered Earth-Fixed Coordinate System) coordinate system, and then transform it from the ECEF coordinate system to the ENU system.

[0046] Step 6: Fuse the AGPS positioning information with the visual inertial odometry information through graph optimization. The specific process is as follows: Figure 3 As shown.

[0047] First, the pose information obtained from the binocular camera and the IMU is fused to output an estimated pose. The pose estimated by the binocular camera and the IMU is used as the local pose. Then, AGPS is used as the global sensor and converted into a factor in the factor graph. Together with the local pose estimate, it is input into the global pose graph to construct an optimization problem. Finally, a locally accurate and globally drift-free 6-DOF pose is generated.

[0048] The nodes of the factor graph are the poses in the world coordinate system. The edges between two consecutive nodes are local poses, which are local constraints obtained by the fusion of binocular vision and IMU. The other edges are global constraints generated by AGPS as a global sensor.

[0049] Factor graph optimization is a maximum likelihood estimation problem, as shown below:

[0050]

[0051] Where S is the set of pose estimation results from different methods, namely binocular vision, IMU, and AGPS. Let X represent the observations from each sensor, and let X be the pose node of the outdoor mobile robot over a period of time. The pose node can be represented as:

[0052]

[0053] in, 和 These are the position and rotation quaternions of the outdoor mobile robot in the global coordinate system, respectively. Assume that the measurements are independent and their errors follow a Gaussian distribution. The nonlinear least squares problem is obtained:

[0054]

[0055] in, It is the covariance matrix. The smaller the covariance, the more accurate the corresponding measurement. These are the observations from each sensor. This provides an approximate pose value for the outdoor mobile robot; finally, the pose estimates of each sensor are expressed as factor expressions in the factor graph, as follows:

[0056] The visual-inertial odometry factor after binocular vision and IMU fusion is:

[0057]

[0058] in, These are observations after the binocular camera and IMU are fused together. x is an approximate pose of an outdoor mobile robot. t-1 For the pose of the outdoor mobile robot at time t-1, x t The pose of the outdoor mobile robot at time t;

[0059] The AGPS factor is:

[0060]

[0061] in, These are AGPS observations. This is an approximate pose value for an outdoor mobile robot. For AGPS location, The pose of the binocular camera after being fused with the IMU;

[0062] The graph optimization problem formed by the above equations can be solved using the G2O solver or the Ceres Solver solver.

[0063] Step 7: The outdoor mobile robot reaches the target point according to the planned path. The overall process is as follows: Figure 5 As shown, the robot can plan its path in real time and eventually reach the target point even when the environment ahead is unknown. In the initial state, as... Figure 5 As shown in (a), when the environment ahead is unknown, the path is planned and passes through obstacles because no obstacles are observed. After the movement begins, the planned path is adjusted in real time in segments based on the obstacle information observed by binocular vision to ensure that the robot can move safely and smoothly, while continuously moving closer to the target point. The planning process is as follows: Figure 5 As shown in (b) above, previously Figure 5 In (a), the path through the obstacle has been adjusted to a path around the obstacle; after multiple real-time path planning operations, the robot reaches the preset target point, and the entire motion path is as follows: Figure 5 (c) in the middle.

[0064] The above specific embodiments are used to explain and illustrate the present invention, and are only preferred embodiments of the present invention, not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc., made to the present invention within the spirit and scope of the claims shall fall within the protection scope of the present invention.

Claims

1. A navigation method for outdoor mobile robots based on the fusion of binocular vision, IMU, and AGPS, characterized in that: The method involves equipping an outdoor mobile robot with a binocular camera, IMU, and 5G network module, and enabling AGPS functionality; the method includes the following steps: Step 1: The outdoor mobile robot receives the location of the navigation target point. The navigation target point of the outdoor mobile robot is: a coordinate system is established with the starting point of the outdoor mobile robot as the origin, and the coordinates of the target point in the coordinate system are specified. The target point is specified by remotely inputting the coordinates or directly selecting on the map and sending it to the outdoor mobile robot through the 5G network. Step 2: The outdoor mobile robot plans its path in segments when the environment ahead is unknown. The outdoor mobile robot does not need to pre-map the surrounding environment. It first selects a stage target point on the line connecting the starting point and the target point, and then plans the path based on the obstacle information observed by the binocular camera. The path planning method is to first use the D* algorithm to plan the initial path, and then use the TEB algorithm to optimize the path and output an executable path. After the binocular camera acquires environmental information, the obstacles are "expanded," that is, the boundaries of the obstacles are enlarged, and the expanded distance is set as a variable. This distance is adjusted during the actual operation of the outdoor mobile robot to ensure that the outdoor mobile robot can safely avoid obstacles even when its own volume changes. Step 3: The outdoor mobile robot receives AGPS signals and performs differential processing to obtain meter-level positioning information; Step 4: The outdoor mobile robot receives signals from the binocular camera and IMU, and the two are fused to output visual inertial odometry pose information. Step 5: Perform coordinate transformation on the AGPS signal, converting the coordinates from the WGS-84 coordinate system to the ENU coordinate system, which is the same as the visual inertial odometry pose information; Step 6: The AGPS positioning information in the ENU coordinate system and the visual inertial odometry pose information are fused using a graph optimization fusion method to solve the cumulative error problem of the visual inertial odometry. Step 7: The outdoor mobile robot arrives at the target point according to the planned path.

2. The outdoor mobile robot navigation method based on binocular vision, IMU, and AGPS fusion as described in claim 1, characterized in that, In step 6, the specific process of the graph optimization fusion method is as follows: First, the pose information obtained from the binocular camera and the IMU is fused to output an estimated pose. The pose estimated by the binocular camera and the IMU is used as the local pose. Then, AGPS is used as the global sensor and converted into a factor in the factor graph. Together with the local pose estimate, it is input into the global pose graph to construct an optimization problem. Finally, a locally accurate and globally drift-free 6-DOF pose is generated. The nodes of the factor graph are the poses in the world coordinate system. The edges between two consecutive nodes are local poses, which are local constraints obtained by the fusion of binocular vision and IMU. The other edges are global constraints generated by AGPS as a global sensor. Factor graph optimization is a maximum likelihood estimation problem, as shown in equation (1): （1） in, It is a collection of pose estimation results from different methods, namely binocular vision, IMU, and AGPS. These are the observations from each sensor. For an outdoor mobile robot, the pose nodes are defined as follows: in, These are the position and rotation quaternions of the outdoor mobile robot in the global coordinate system, respectively. Assume that the measurements are independent and their errors follow a Gaussian distribution. This yields the nonlinear least squares problem as shown in equation (2): （2） in, It is the covariance matrix. The smaller the covariance, the more accurate the corresponding measurement. This provides an approximate pose value for the outdoor mobile robot; finally, the pose estimates of each sensor are expressed as factor expressions in the factor graph, as follows: The visual-inertial odometry factor after binocular vision and IMU fusion is given by the following formula (3): in, These are observations after the binocular camera and IMU are fused together. This represents the approximate pose of an outdoor mobile robot after the fusion of a binocular camera and an IMU. The pose of the outdoor mobile robot at time t-1. Let the outdoor mobile robot be in pose at time t. The AGPS factor is given by the following formula (4): （4） in, These are AGPS observations. This represents the approximate pose of an outdoor mobile robot using AGPS. For AGPS location, The pose of the binocular camera after being fused with the IMU; The graph optimization problem formed by the above equations (1), (2), (3), and (4) can be solved using the G2O solver or the Ceres Solver solver.

Images