A path planning method combined with three-dimensional modeling

Abstract

The invention discloses a path planning method combined with three-dimensional modeling, comprising: obtaining a GPS covered road network of a target scene, dividing a non-GPS covered road network area into a VPS custom area, performing real scene three-dimensional reconstruction on the VPS custom area, and obtaining a real scene model of the VPS custom area; defining a transition area between the GPS covered road network area and the VPS custom area, and selecting an optimal path connection point in the transition area; connecting a path in the GPS covered road network area and a path in the VPS custom area through the optimal path connection point, planning the path of the VPS custom area through the real scene model of the VPS custom area, and forming a fused navigation path; navigating according to the fused navigation path, performing navigation point deviation detection, and if deviation occurs, performing new starting point planning to generate a new fused navigation path.

Description

Technical Field

[0001] The present invention relates to the technical field of path planning, and in particular to a path planning method combined with three-dimensional modeling. Background Art

[0002] GPS (Global Positioning System) navigation technology has been widely used in various navigation scenarios, especially in large-scale, long-distance route navigation, which is stable and reliable. GPS navigation relies on satellite signal positioning and can provide a high coverage range and basic navigation services. However, the positioning accuracy of civilian GPS is usually between a few meters and more than ten meters. This accuracy may be affected by factors such as building obstruction and signal reflection in cities, resulting in further increase in positioning errors. In addition, GPS navigation usually relies on preset map data, and the number and accuracy of target points are limited, which cannot meet users' needs for precise navigation. For example, in a complex urban environment, it is difficult for GPS navigation to accurately guide users to specific building entrances, parking spaces, or small shops.

[0003] VPS is a positioning technology based on camera visual information. It uses the camera of a device (such as a mobile phone, drone, or AR device) to collect image data of the surrounding scene in real time, and uses these images to match the pre-built environment map (or a database of known feature points) to obtain the precise position and direction (pose) of the device. VPS is a positioning method based on visual perception, and is particularly good at providing high-precision location information in scenarios where GPS signals are weak or unavailable (such as indoors, underground parking lots, and densely built areas in cities).

[0004] VPS relies on pre-built visual maps or feature databases for navigation. It is unable to update dynamic obstacles (such as temporary construction and obstacles), and may also lead to deviations in environmental understanding due to map aging. At the transition point where GPS switches to VPS, if VPS cannot understand and perceive the surrounding environment well, the navigation path may seem feasible but actually impractical (such as GPS believes that a small passage between buildings is passable, but VPS cannot bypass the obstacle). In the process of switching from GPS to VPS, if there are multiple path connection points (such as multiple entrances to the same target area), it is necessary to select an "optimal switching point". However, due to the different understanding capabilities of GPS and VPS for the surrounding environment, existing technologies may not be able to select the optimal switching point, and are prone to selecting a suboptimal point, resulting in an incoherent path after switching.

[0005] In view of the above problems in the prior art, a path planning method combined with three-dimensional modeling is the purpose of the present invention. Summary of the invention

[0006] In view of this, an object of the present invention is to propose a path planning method combined with three-dimensional modeling, which can solve the above-mentioned problems.

[0007] The present invention provides a path planning method combined with three-dimensional modeling, comprising:

[0008] Obtain the GPS coverage road network of the target scene, divide the non-GPS coverage road network area into a VPS custom area, perform real-scene 3D reconstruction on the VPS custom area, and obtain a real-scene model of the VPS custom area;

[0009] Define the transition area between the GPS coverage area and the VPS custom area, and select the optimal path connection point in the transition area;

[0010] Connect the paths in the GPS covered road network area and the paths in the VPS custom area through the optimal path connection point, plan the paths in the VPS custom area through the real scene model of the VPS custom area, and form a fusion navigation path;

[0011] Navigate according to the fused navigation path, perform navigation point deviation detection, and if deviation occurs, plan a new starting point to generate a new fused navigation path.

[0012] Further, the real-scene three-dimensional reconstruction of the VPS custom area to obtain the real-scene model of the VPS custom area includes:

[0013] Get the panoramic video of the VPS custom area, extract the panoramic video to get image frames, build the map through the image frames, calculate the camera pose of each image frame, and take the image frame with the first camera pose angle as the key frame;

[0014] Extract feature points from key frames, match feature points between adjacent key frames to obtain feature matching relationships, triangulate and estimate the 3D points of the target scene based on the feature points and feature matching relationships, correct the key frames of the target scene through reprojection errors, and obtain an optimized set of key frames;

[0015] Perform 3D reconstruction based on the 3D points of the key frame set optimized for the target scene to obtain a 3D point cloud model, perform dense reconstruction and key frame image texture mapping on the 3D point cloud model to generate a real scene model;

[0016] Restore point cloud models and reality meshes to true scale.

[0017] Further, the key frame of the target scene is corrected by reprojection error, including:

[0018] The reprojection error is calculated by the camera pose and 3D point position of each key frame. The calculation formula of the reprojection error is:

[0019] ,

[0020] in, is the reprojection error, K is the camera parameter, R is the rotation variable in the camera pose, t is the translation variable in the camera pose, For feature points, is a three-dimensional point;

[0021] The key frames whose reprojection errors are greater than the translation threshold are eliminated to obtain an optimized key frame set.

[0022] Further, restoring the point cloud model and the real scene model to the real scale includes:

[0023] Collect an image set containing GPS coordinate information, input the image set into the point cloud model for repositioning, and obtain the camera pose of the image set in the coordinate system of the point cloud model;

[0024] The error function is constructed by the camera pose of the image set in the coordinate system of the point cloud model and the GPS coordinate information of the original image set. The scale factor s is obtained by minimizing the error function. The point cloud model and the real scene model are restored to the real scale through the scale factor s. The calculation formula of the scale factor is:

[0025] ,

[0026] Among them, Error is the error function, s is the scale factor, R is the rotation variable in the camera pose, t is the translation variable in the camera pose, is the GPS coordinates, is the camera pose coordinate, and N is the number of feature points in the point cloud model.

[0027] Further, the step of defining a transition area between the GPS coverage road network area and the VPS custom area and selecting an optimal path connection point in the transition area includes:

[0028] A distance range Dtransition is defined between the end point of the GPS path and the start point of the VPS path. With Dsample as the sampling interval, a candidate point set Pcandidates is generated. The candidate point set meets the following conditions:

[0029] ,

[0030] in, and They represent the distances from the candidate point p to the end point of the nearest GPS path and the start point of the nearest VPS path respectively;

[0031] According to each candidate point Path length to the end point of the GPS path And the path length from each candidate point to the starting point of the VPS path , calculate the total path length, the calculation formula is as follows:

[0032] ,

[0033] Among them, α and β are the weight factors of GPS and VPS paths respectively;

[0034] Select the total path length The smallest candidate point is used as the optimal path connection point , the calculation formula is as follows:

[0035] ,

[0036] If there are multiple optimal path connection points, the candidate point with the smallest change in path direction is selected as the optimal path connection point.

[0037] Further, the selecting a candidate point with the smallest change in path direction as the optimal path connection point includes:

[0038] Calculate the change in path direction of multiple optimal path connection points respectively , the calculation formula is as follows:

[0039] ,

[0040] in, and GPS to candidate points and from the candidate point Direction angle to VPS;

[0041] The total score function is constructed by the change in path direction and the total path length, and the candidate point with the lowest total frequency score function is selected as the optimal path connection point. The total score function formula is as follows:

[0042] ,

[0043] in, is the smoothness weight, which is used to control the influence of path smoothness.

[0044] Further, the method of connecting the path in the GPS covered road network area and the path in the VPS custom area through the optimal path connection point, planning the path in the VPS custom area through the real scene model of the VPS custom area, and forming a fused navigation path includes:

[0045] Use GPS to navigate to the optimal path connection point and perform AR visual positioning in the VPS custom area;

[0046] Generate navigation points based on the real-size real-scene model, connect the navigation points to form navigation edges, and build a navigation graph structure through the navigation points and navigation edges;

[0047] The navigation starting point is obtained through AR visual positioning, and the nearest navigation point is found in the navigation graph structure to connect the navigation starting point. The shortest path in the navigation graph structure exists in the graph to form a navigation path.

[0048] Further, generating navigation points according to the real-size real-scene model, connecting the navigation points to form navigation edges, and constructing a navigation graph structure through the navigation points and the navigation edges includes:

[0049] Acquire all point data and line data of the real-size real scene model, set up new point data at the intersection of the intersecting line data, and break two intersecting line data into four connected but non-intersecting line data with the intersection point as the new starting point to form new line data;

[0050] Traverse all new line data, generate a number of new point data at fixed intervals along each line, and construct the new point data and the original point data into a navigation point set;

[0051] Traverse the navigation points on each line data, connect the adjacent and connected navigation points to form a navigation edge set, and build a navigation graph structure through the navigation point set and the navigation edge set.

[0052] Further, the navigation starting point is obtained through AR visual positioning, and the nearest navigation point is found in the navigation graph structure to connect the navigation starting point. The shortest path in the graph exists in the navigation graph structure, and forming the navigation path includes:

[0053] Acquire the surrounding scene video through the camera, intercept the image frame to be located through the surrounding scene video, extract the features of the image frame to be located, search the features of the image frame to be located in the optimized key frame set, and select the N candidate key frames that are most similar to the image frame to be located;

[0054] The image frame to be positioned and the candidate key frame are respectively subjected to 2D-2D feature matching, 2D-3D feature point correspondence, and minimization of reprojection error calculation to obtain the camera pose, and the position information of the camera pose is obtained as the navigation starting point.

[0055] Further, the navigating according to the fused navigation path, detecting the deviation of the navigation point, planning a new starting point if a deviation occurs, and generating a new fused navigation path includes:

[0056] Get the user's real-time location coordinates , calculate the user's real-time location coordinates The closest navigation point to the navigation path Coordinate distance , if the coordinate distance is greater than the deviation threshold , then deviate from the navigation path;

[0057] The user's real-time location coordinates As the new navigation starting point, obtain the real-time location coordinates of the user in the original navigation path Nearest navigation point As a connection point;

[0058] The user's real-time location coordinates Add to the navigation path graph structure, update the edge weights in the graph structure based on the distance between the user's current location and the surrounding path navigation points. With the nearest navigation point The distance between , then the weight of the newly added edge is:

[0059] ;

[0060] By gradually updating the user's real-time location coordinates The shortest path to other navigation points in the graph structure until the user's real-time location coordinates are found Go to the target navigation point The shortest path, the update formula is:

[0061] ,

[0062] Among them, u is the current user's real-time location coordinates, v is u's adjacent navigation point, and w(u,v) is the edge weight from u to v;

[0063] From the target node Start by backtracking through the node sequence and generating a new navigation path , the generated as a new navigation path.

[0064] Beneficial effects of the present invention:

[0065] First, the panoramic camera can fully capture the image information of the target area to reduce information loss. The three-dimensional point cloud model can be restored to a real-scale real-life model through GPS coordinate information combined with error function optimization. By collecting key frames at intervals of camera pose angles, the computational overhead of redundant information processing can be reduced while retaining good coverage of the scene. Finally, the point cloud model and texture map real-life model are generated for subsequent accurate navigation of users.

[0066] Secondly, by defining the transition area and calculating the scoring function, the path length and direction change (smoothness) are comprehensively considered, so that the most reasonable path connection point can be found in the navigation between the end point of the PS path and the starting point of the VPS path.

[0067] Third, AR visual positioning combined with real-life models can accurately calculate the navigation starting point. Building a graph structure based on navigation points and navigation edges can effectively find the optimal path. By detecting users’ deviation from the navigation path in real time and updating the navigation starting point, the navigation path can be ensured to be always accurate. Inserting the user’s current location into the graph structure can intelligently incorporate the user’s location into the global path for re-optimization. By correcting the starting point and path structure in real time, it can cope with dynamic environmental changes, especially in mixed areas (combined with GPS and VPS coverage areas), to avoid navigation disconnection due to user switching. By locally updating instead of regenerating the complete navigation path, the computational complexity of path planning is significantly reduced. BRIEF DESCRIPTION OF THE DRAWINGS

[0068] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings in the following description are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0069] Figure 1 It is a flow chart of the method of this embodiment. DETAILED DESCRIPTION

[0070] To facilitate understanding by those skilled in the art, the structure of the present invention is now further described in detail with reference to the embodiments and the accompanying drawings. It should be understood that the steps mentioned in the present embodiment, unless otherwise specified in their order, can be adjusted in order according to actual needs, and can even be executed simultaneously or partially simultaneously.

[0071] like Figure 1 As shown, an embodiment of the present invention provides a path planning method combined with three-dimensional modeling, including:

[0072] S1 obtains the GPS coverage road network of the target scene, divides the non-GPS coverage road network area into a VPS custom area, performs real-scene three-dimensional reconstruction on the VPS custom area, and obtains a real-scene model of the VPS custom area;

[0073] In this step, the target scene is divided into GPS road network areas and VPS (Visual Positioning System, a real-time positioning and navigation technology based on visual images. It achieves high-precision positioning of devices (such as mobile phones, drones, robots, etc.) by matching the images captured by the camera with the pre-built three-dimensional scene model (or feature point database).) custom areas; the GPS road network area includes navigation routes covered by existing third-party maps (such as Amap, Tencent Map, etc.), generally including park main roads, driveways and other areas; the VPS custom area includes other areas to be navigated, such as parks that are not covered by the GPS road network.

[0074] S101 obtains a panoramic video of a VPS custom area, extracts frames from the panoramic video to obtain image frames, constructs a map through the image frames, calculates the camera pose of each image frame, and takes image frames at intervals of a first camera pose angle as key frames;

[0075] In this step, a panoramic camera is used to capture 360° video of the target area to obtain a panoramic video, and then a series of image frames are obtained by frame extraction. , input the SLAM algorithm (Simultaneous Localization and Mapping algorithm) that supports panoramic cameras frame by frame, and perform map construction and camera pose (i.e. camera position + angle) calculation to obtain the camera pose of each image frame Since the image frames are dense, according to the camera pose relationship, the image frames separated by the first camera pose angle (such as 30°) are taken as key frames. , where KF is the key frame, j is the jth key frame, r is the angle between the two key frames, and N is the first camera pose angle. In this application, N can be 30°. The key frame extraction method is used to avoid excessive processing of redundant image data, reduce computing resource requirements, and improve system efficiency.

[0076] S102 extracts feature points from the key frames, matches feature points between adjacent key frames to obtain feature matching relationships, triangulates and estimates the three-dimensional points of the target scene based on the feature points and the feature matching relationships, corrects the key frames of the target scene by reprojection errors, and obtains an optimized set of key frames;

[0077] S1021 calculates the reprojection error through the camera pose and 3D point position of each key frame. The calculation formula of the reprojection error is:

[0078] ,

[0079] in, is the reprojection error, K is the camera parameter, R is the rotation variable in the camera pose, t is the translation variable in the camera pose, For feature points, is a three-dimensional point;

[0080] S1022 removes key frames whose reprojection errors are greater than the translation threshold, and obtains an optimized key frame set.

[0081] In this step, the key frame set is used to obtain the feature points p2d of each key frame through the feature extraction algorithm. The feature matching relationship between the two key frames is obtained by the feature matching algorithm for the two sets of feature points of adjacent key frames. According to the feature points and the feature matching relationship, the triangulation algorithm is used to estimate the 3D point P3D of the scene. The reprojection error is calculated through the camera pose and 3D point position of each frame. The key frames with errors greater than the translation threshold t are eliminated (that is, the camera pose estimation accuracy of these key frames is low), and the optimized key frame set is obtained. ,in is the reprojection error, which is calculated as: , where K is the camera parameter, which is generally a known parameter, R is the rotation variable in the camera pose, and t is the translation variable in the camera pose. The key frame set is corrected by reprojection error, the camera pose estimation result is optimized, and the key frames with unstable camera estimation are eliminated to improve the modeling accuracy.

[0082] S103 performs 3D reconstruction based on the 3D points of the key frame set after optimization of the target scene to obtain a 3D point cloud model, and performs dense reconstruction and key frame image texture mapping on the 3D point cloud model to generate a real scene model;

[0083] In this step, the optimized keyframe set is input into the SFM-type 3D reconstruction algorithm (Structure from Motion, i.e. "Recovering Structure from Motion" is a reconstruction algorithm in the field of computer vision, which is used to restore the 3D geometric structure of the scene (i.e. reconstructing the 3D point cloud model) and the camera pose (position and direction) of each viewpoint through multiple camera images) for conventional reconstruction to obtain a point cloud model. , further dense reconstruction and texture mapping are performed to obtain the real scene model . Reconstruct sparse point clouds into dense point clouds and complete high-precision texture mapping to generate highly realistic real-scene models and expand the application scope of the models.

[0084] S104 restores the point cloud model and the real scene model to their real scale.

[0085] S1041 collects an image set containing GPS coordinate information, inputs the image set into the point cloud model for repositioning, and obtains the camera pose of the image set in the coordinate system of the point cloud model;

[0086] In this step, a camera device (such as a drone) with absolute information such as GPS / RTK is used to collect images of the target area with rich textures to obtain an image set containing GPS information. , GPS coordinate information is recorded as . The above image set , input the above point cloud model for repositioning, and obtain the camera pose of the image set in the coordinate system of the point cloud model, recorded as , R is the rotation part, t is the translation part, in order to unify with GPS coordinate notation, Recorded as .

[0087] S1042 constructs an error function through the camera pose of the image set in the coordinate system of the point cloud model and the GPS coordinate information of the original image set, minimizes the error function to obtain the scale factor s, and restores the point cloud model and the real scene model to the real scale through the scale factor s. The calculation formula of the scale factor is:

[0088] ,

[0089] Among them, Error is the error function, s is the scale factor, R is the rotation variable in the camera pose, t is the translation variable in the camera pose, is the GPS coordinates, is the camera pose coordinate, N is the number of feature points in the point cloud model;

[0090] In this step, based on the image set The scale factor s can be solved by minimizing the following error function in the coordinates of the two coordinate systems (GPS and point cloud model): , scale the above point cloud model and real scene model by s times to obtain a real scale model restored 1:1 with the real scene and .

[0091] S2 defines the transition area between the GPS coverage road network area and the VPS custom area, and selects the optimal path connection point in the transition area;

[0092] S201 defines a distance range Dtransition between the end point of the GPS path and the start point of the VPS path, takes Dsample as the sampling interval, and generates a candidate point set Pcandidates, which satisfies the following conditions:

[0093] ,

[0094] in, and They represent the distances from the candidate point p to the end point of the nearest GPS path and the start point of the nearest VPS path respectively;

[0095] In this step, a transition area A is set between the end point of the GPS path and the start point of the VPS path, that is, a reasonable transition distance range Dtransition from GPS to VPS is defined. In area A, a series of candidate point sets Pcandidates are generated with Dsample as the sampling interval. These candidate points will be used as potential path connection points and meet the following conditions:

[0096] ,

[0097] in, and They represent the distances from the candidate point p to the nearest GPS path and the nearest VPS path respectively.

[0098] S202 Based on each candidate point Path length to the end point of the GPS path And the path length from each candidate point to the starting point of the VPS path , calculate the total path length, the calculation formula is as follows:

[0099] ,

[0100] Among them, α and β are the weight factors of GPS and VPS paths respectively;

[0101] In this step, for each candidate point , calculate from the GPS end point to The path length , and from Path length to the VPS origin Taking into account the smoothness of the transition and the requirements of the shortest path, the total path length is defined is the weighted sum of the following formulas:

[0102]

[0103] Among them, α and β are the weight factors of GPS and VPS path segments respectively, which can be adjusted according to specific needs.

[0104] S203 Select the total path length The smallest candidate point is used as the optimal path connection point , the calculation formula is as follows:

[0105] .

[0106] In this step, Each point in , calculate its corresponding , and select The smallest point As the optimal path connection point:

[0107]

[0108] If the GPS route or VPS route changes, re-execute the above steps to dynamically adjust the route connection points position to ensure the optimality and consistency of the navigation path.

[0109] S204: If there are multiple optimal path connection points, then select the candidate point with the smallest change in path direction as the optimal path connection point.

[0110] S2041 calculates the change in the path direction of multiple optimal path connection points respectively , the calculation formula is as follows:

[0111] ,

[0112] in, and GPS to candidate points and from the candidate point Direction angle to VPS;

[0113] S2042 constructs a total scoring function based on the change in path direction and the total path length, and selects the candidate point with the lowest total frequency score function as the optimal path connection point. The total scoring function formula is as follows:

[0114] ,

[0115] in, is the smoothness weight, which is used to control the influence of path smoothness.

[0116] In this step, if there are multiple path connection points such that If they are equal, further selection can be made by path smoothness. To ensure the smoothness of path connection, the change in path direction can be calculated , and added to the path selection.

[0117] Since GPS navigation and AR navigation rely on environmental perception to different degrees, there may be deviations in the understanding of the surrounding environment when the two are connected. Existing technologies make it difficult to select the optimal connection point, which may cause the user to experience path incoherence or unreasonable guidance after switching to AR navigation. Selecting connection points based solely on the shortest path may cause drastic changes in direction, making path planning uneven and unable to be actually executed. Considering smoothness may increase the path length and reduce navigation efficiency, so the rationality and operability of the path can be achieved through the total scoring function (comprehensive consideration of path length and direction changes).

[0118] S3 connects the paths in the GPS covered road network area and the paths in the VPS customized area through the optimal path connection point, plans the paths in the VPS customized area through the real scene model of the VPS customized area, and forms a fusion navigation path;

[0119] S301 uses GPS to navigate to the optimal path connection point and performs AR visual positioning in the VPS custom area;

[0120] In this step, after the user selects the navigation destination, the path connection points around the destination are selected, and the GPS route + VPS route under each connection point are spliced ​​in sequence. The shortest path is selected as the navigation plan, and the corresponding path connection point is determined. The traditional GPS navigation capability is turned on, and the current location is navigated to the above path connection point. After arriving at the path connection point, the VPS modeling area is entered, and AR visual positioning is started to navigate the VPS custom area.

[0121] S302 generates navigation points according to the real-size real-scene model, connects the navigation points to form navigation edges, and constructs a navigation graph structure through the navigation points and the navigation edges;

[0122] In this step, the real-life model aligned with the real scale is imported into a visual editor that supports point selection of coordinates. In the editor, the optimal path connection point is used as the starting point, and any desired target point is used as the end point. With reference to the actual road of the real-life model, a traversable route is freely drawn, and the route is analyzed according to the following steps:

[0123] S3021 acquires all point data and line data in the real-size real scene model, sets up new point data at the intersection of the intersecting line data, and breaks two intersecting line data into four connected but non-intersecting line data with the intersection point as a new starting point, to form new line data;

[0124] S3022 traverses all new line data, generates a number of new point data at fixed intervals along each line, and constructs the new point data and the original point data into a navigation point set;

[0125] S3023 traverses the navigation points on each line data, connects adjacent and connected navigation points to form a navigation edge set, and constructs a navigation graph structure through the navigation point set and the navigation edge set.

[0126] In this step, all point data and line data in the editor are extracted. For intersecting line data, new point data is set up at the intersection point, and the two intersecting lines are broken into four connected but non-intersecting routes with the intersection point as the new starting point. Each route is divided into independent path segments at the intersection. Traverse all independent path line data after interruption in the above steps, and generate a series of navigation points at fixed intervals along each route. These points will be used for subsequent path planning, navigation guidance and visualization. Create an "edge" set to store adjacent and connected navigation points, traverse the points on each navigation route, create edges between adjacent connected points, and add them to the edge set. After the relationship between points and edges is established, it is used to construct the "graph" structure later. The existing technology rarely involves customization of VPS paths. This solution only needs to click on the real-life model to define the destination point you want to reach, and the corresponding planned path can be automatically generated.

[0127] S303 obtains the navigation starting point through AR visual positioning, finds the nearest navigation point in the navigation graph structure to connect the navigation starting point, and the shortest path in the graph exists in the navigation graph structure to form a navigation path.

[0128] In this step, the user turns on the camera and scans the surrounding scene. Through AR visual positioning, the user's current precise location is obtained as the navigation starting point, and the target location manually selected by the user is used as the navigation end point. Through the above-mentioned graph structure shortest path algorithm, a navigation route within the pure VPS road range is planned. The specific steps are as follows:

[0129] S3031 obtains the surrounding scene video through the camera, intercepts the image frame to be located through the surrounding scene video, extracts the features of the image frame to be located, retrieves the features of the image frame to be located in the optimized key frame set, and selects N candidate key frames that are most similar to the image frame to be located;

[0130] S3032 performs 2D-2D feature matching, 2D-3D feature point correspondence, and minimization of reprojection error calculation on the image frame to be positioned and the candidate key frame, obtains the camera pose, and obtains the position information of the camera pose as the navigation starting point.

[0131] In this step, the user turns on the camera, scans the surrounding scene, intercepts the image frame Iq in the camera video stream, and performs AR visual positioning. First, the image frame Iq to be positioned is feature extracted, and the key frame number set rKF is searched to select the N (for example, 5) candidate key frames kf that are most similar to the image frame Iq to be positioned. The image frame Iq to be positioned and the N candidate key frames kf are respectively subjected to 2D-2D feature matching, 2D-3D feature point correspondence, and a minimum reprojection error algorithm to obtain the camera pose (position + posture). The position information is the result of AR visual positioning, that is, the starting point of navigation, and the error of the visual positioning method is close to the centimeter level. After confirming the above-mentioned navigation starting point and navigation end point, the navigation path can be planned based on the graph structure of the VPS custom route in step 302. In the graph structure, the navigation point closest to the starting point is found and connected, and the dijkstra algorithm is used to find the shortest path in the graph, and each point is connected in turn to form a pure VPS navigation route.

[0132] S4 navigates according to the fused navigation path and performs navigation point deviation detection. If deviation occurs, a new starting point is planned to generate a new fused navigation path.

[0133] S401 Obtaining the user's real-time location coordinates , calculate the user's real-time location coordinates The closest navigation point to the navigation path Coordinate distance , if the coordinate distance is greater than the deviation threshold , then deviate from the navigation path;

[0134] S402 The user's real-time location coordinates As the new navigation starting point, obtain the real-time location coordinates of the user in the original navigation path Nearest navigation point As a connection point;

[0135] S403 The user's real-time location coordinates Add to the navigation path graph structure, update the edge weights in the graph structure based on the distance between the user's current location and the surrounding path navigation points. With the nearest navigation point The distance between , then the weight of the newly added edge is:

[0136] ;

[0137] S404 gradually updates the user's real-time location coordinates The shortest path to other navigation points in the graph structure until the user's real-time location coordinates are found Go to the target navigation point The shortest path, the update formula is:

[0138] ,

[0139] Where u is the current user's real-time location coordinate, v is u's adjacent navigation point, and w(u, v) is the edge weight from u to v;

[0140] In this step, the Dijkstra algorithm can be used to extract the user's real-time location coordinates in the new graph structure. Go to the target navigation point Recalculate the shortest path.

[0141] S405 From the target node Start by backtracking through the node sequence and generating a new navigation path , the generated as a new navigation path.

[0142] In this step, VPS relies on pre-built visual maps or feature databases for navigation. It cannot update dynamic obstacles (such as temporary construction and obstacles), and may also cause environmental understanding deviations due to map aging. Users may deviate from the path for these reasons. Therefore, by detecting the user's deviation from the navigation path in real time and updating the navigation starting point, the navigation path is always accurate. Inserting the user's current location into the graph structure can intelligently incorporate the user's location into the global path re-optimization. By correcting the starting point and path structure in real time, it can cope with dynamic environmental changes, especially mixed areas (combination of GPS and VPS coverage areas), to avoid navigation disconnection due to user switching positions. By locally updating instead of regenerating the complete navigation path, the computational complexity of path planning is significantly reduced.

[0143] It will be appreciated by those skilled in the art that embodiments of the present invention may be provided as methods, systems, or computer program products. Therefore, the present invention may take the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware. Furthermore, the present invention may take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0144] The present invention is described with reference to flowcharts and / or block diagrams of methods, devices (systems), and computer program products according to embodiments of the present invention. It should be understood that each process and / or block in the flowchart and / or block diagram, as well as the combination of processes and / or blocks in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowchart and / or block diagram. Figure 1 A process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0145] These computer program instructions may also be stored in a computer-readable memory capable of directing a computer or other programmable data processing device to operate in a specific manner, so that the instructions stored in the computer-readable memory produce an article of manufacture comprising an instruction device, which implements the process Figure 1 A process or multiple processes and / or boxes Figure 1 A function specified in one or more boxes.

[0146] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operating steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing instructions for implementing the process. Figure 1 A process or multiple processes and / or boxes Figure 1 A step that specifies a function in one or more boxes.

[0147] It should be noted that in the claims, any reference signs placed between brackets shall not be construed as limiting the claims. The word "comprising" does not exclude the presence of components or steps not listed in the claim. The word "a" or "an" preceding a component does not exclude the presence of a plurality of such components. The invention may be implemented by means of hardware comprising several different components and by means of a suitably programmed computer. In a unit claim enumerating several means, several of these means may be embodied by the same item of hardware. The use of the words first, second, and third etc. does not indicate any order. These words may be interpreted as names.

[0148] Although the preferred embodiments of the present invention have been described, those skilled in the art may make additional changes and modifications to these embodiments once they have learned the basic creative concept. Therefore, the appended claims are intended to be interpreted as including the preferred embodiments and all changes and modifications that fall within the scope of the present invention.

[0149] Obviously, those skilled in the art may make various changes and modifications to the present invention without departing from the spirit and scope of the present invention. Thus, if these modifications and variations of the present invention fall within the scope of the claims of the present invention and their equivalents, the present invention is also intended to include these modifications and variations.

[0150] In the present invention, unless otherwise clearly specified and limited, the terms "installed", "connected", "connected", "fixed" and the like should be understood in a broad sense, for example, it can be a fixed connection, a detachable connection, or an integral connection; it can be a mechanical connection or an electrical connection; it can be a direct connection or an indirect connection through an intermediate medium, it can be the internal connection of two elements or the interaction relationship between two elements. For ordinary technicians in this field, the specific meanings of the above terms in the present invention can be understood according to specific circumstances.

[0151] In the description of this specification, the description with reference to the terms "one embodiment", "some embodiments", "example", "specific example", or "some examples" etc. means that the specific features, structures, materials or characteristics described in conjunction with the embodiment or example are included in at least one embodiment or example of the present invention. In this specification, the schematic representation of the above terms should not be understood as necessarily referring to the same embodiment or example. Moreover, the specific features, structures, materials or characteristics described may be combined in any one or more embodiments or examples in a suitable manner. In addition, those skilled in the art may combine and combine the different embodiments or examples described in this specification and the features of the different embodiments or examples, unless they are contradictory.

Claims

1. A path planning method combined with three-dimensional modeling, characterized in that: include: Get the GPS coverage road network of the target scene, divide the non-GPS coverage road network area into VPS custom areas, perform real-scene 3D reconstruction on the VPS custom areas, and obtain the real-scene model of the VPS custom areas. Specifically: Get the panoramic video of the VPS custom area, extract the panoramic video to get image frames, build the map through the image frames, calculate the camera pose of each image frame, and take the image frame with the first camera pose angle as the key frame; Extract feature points from key frames, match feature points between adjacent key frames to obtain feature matching relationships, triangulate and estimate the 3D points of the target scene based on the feature points and feature matching relationships, correct the key frames of the target scene through reprojection errors, and obtain an optimized set of key frames; Perform 3D reconstruction based on the 3D points of the key frame set optimized for the target scene to obtain a 3D point cloud model, perform dense reconstruction and key frame image texture mapping on the 3D point cloud model to generate a real scene model; Restore point cloud models and reality models to real scale; Define the transition area between the GPS coverage area and the VPS custom area, and select the optimal path connection point in the transition area. Specifically: A distance range Dtransition is defined between the end point of the GPS path and the start point of the VPS path. With Dsample as the sampling interval, a candidate point set Pcandidates is generated. The candidate point set meets the following conditions: ， in, and They represent the distances from the candidate point p to the end point of the nearest GPS path and the start point of the nearest VPS path respectively; According to each candidate point Path length to the end point of the GPS path And the path length from each candidate point to the starting point of the VPS path , calculate the total path length, the calculation formula is as follows: ， Among them, α and β are the weight factors of GPS and VPS paths respectively; Select the total path length The smallest candidate point is used as the optimal path connection point , the calculation formula is as follows: ， If there are multiple optimal path connection points, the candidate point with the smallest change in path direction is selected as the optimal path connection point. Specifically: Calculate the change in path direction of multiple optimal path connection points respectively , the calculation formula is as follows: ， in, and GPS to candidate points and from the candidate point Direction angle to VPS; The total score function is constructed by the change in path direction and the total path length, and the candidate point with the lowest total score function is selected as the optimal path connection point. The total score function formula is as follows: ， in, is the smoothness weight, which is used to control the impact of path smoothness; Connect the paths in the GPS covered road network area and the paths in the VPS custom area through the optimal path connection point, plan the paths in the VPS custom area through the real scene model of the VPS custom area, and form a fusion navigation path. Specifically: Use GPS to navigate to the optimal path connection point and perform AR visual positioning in the VPS custom area; Generate navigation points based on the real-size real-scene model, connect the navigation points to form navigation edges, and build a navigation graph structure through the navigation points and navigation edges; The navigation starting point is obtained through AR visual positioning, and the nearest navigation point is found in the navigation graph structure to connect the navigation starting point. The shortest path in the navigation graph structure exists in the graph to form a navigation path; Navigate according to the fused navigation path, perform navigation point deviation detection, and if deviation occurs, plan a new starting point to generate a new fused navigation path. Specifically: Get the user's real-time location coordinates , calculate the user's real-time location coordinates The closest navigation point to the navigation path Coordinate distance , if the coordinate distance is greater than the deviation threshold , then deviate from the navigation path; The user's real-time location coordinates As the new navigation starting point, obtain the real-time location coordinates of the user in the original navigation path Nearest navigation point As a connection point; The user's real-time location coordinates Add to the navigation path graph structure, update the edge weights in the graph structure based on the distance between the user's current location and the surrounding path navigation points. With the nearest navigation point The distance between , then the weight of the newly added edge is: ； By gradually updating the user's real-time location coordinates The shortest path to other navigation points in the graph structure until the user's real-time location coordinates are found Go to the target navigation point The shortest path, the update formula is: ， Among them, u is the current user's real-time location coordinates, v is u's adjacent navigation point, and w(u,v) is the edge weight from u to v; From the target node Start by backtracking through the node sequence and generating a new navigation path , the generated as a new navigation path.

2. A path planning method combined with three-dimensional modeling according to claim 1, characterized in that: The key frame of the target scene corrected by reprojection error includes: The reprojection error is calculated by the camera pose and 3D point position of each key frame. The calculation formula of the reprojection error is: ， in, is the reprojection error, K is the camera parameter, R is the rotation variable in the camera pose, t is the translation variable in the camera pose, For feature points, is a three-dimensional point; The key frames whose reprojection errors are greater than the translation threshold are eliminated to obtain an optimized key frame set.

3. The path planning method combined with three-dimensional modeling according to claim 1, characterized in that: Restoring the point cloud model and the real scene model to the real scale includes: Collect an image set containing GPS coordinate information, input the image set into the point cloud model for repositioning, and obtain the camera pose of the image set in the coordinate system of the point cloud model; The error function is constructed by the camera pose of the image set in the coordinate system of the point cloud model and the GPS coordinate information of the original image set. The scale factor s is obtained by minimizing the error function. The point cloud model and the real scene model are restored to the real scale through the scale factor s. The calculation formula of the scale factor is: ， Among them, Error is the error function, s is the scale factor, R is the rotation variable in the camera pose, t is the translation variable in the camera pose, is the GPS coordinates, is the camera pose coordinate, and N is the number of feature points in the point cloud model.

4. The path planning method combined with three-dimensional modeling according to claim 1, characterized in that: Generating navigation points according to the real-size real-scene model, connecting the navigation points to form navigation edges, and constructing a navigation graph structure through the navigation points and navigation edges includes: Acquire all point data and line data of the real-size real scene model, set up new point data at the intersection of the intersecting line data, and break two intersecting line data into four connected but non-intersecting line data with the intersection point as the new starting point to form new line data; Traverse all new line data, generate a number of new point data at fixed intervals along each line, and construct the new point data and the original point data into a navigation point set; Traverse the navigation points on each line data, connect the adjacent and connected navigation points to form a navigation edge set, and build a navigation graph structure through the navigation point set and the navigation edge set.

5. The path planning method combined with three-dimensional modeling according to claim 1, characterized in that: The method of obtaining the navigation starting point through AR visual positioning and finding the nearest navigation point in the navigation graph structure to connect the navigation starting point, wherein the shortest path in the graph exists in the navigation graph structure, and forming the navigation path includes: Acquire the surrounding scene video through the camera, intercept the image frame to be located through the surrounding scene video, extract the features of the image frame to be located, search the features of the image frame to be located in the optimized key frame set, and select the N candidate key frames that are most similar to the image frame to be located; The image frame to be positioned and the candidate key frame are respectively subjected to 2D-2D feature matching, 2D-3D feature point correspondence, and minimization of reprojection error calculation to obtain the camera pose, and the position information of the camera pose is obtained as the navigation starting point.

Images