Seamless positioning system and method based on port environment

Abstract

The invention provides a seamless positioning system and method based on a port environment. The method comprises the following steps: step 1, a cloud platform issues an image acquisition instruction to an intelligent driving device; 2, after the intelligent driving device receives the image acquisition instruction, a sensor assembly is controlled to acquire sensor data; 3, the intelligent driving device obtains a point cloud map according to the sensor data and a graph optimization theory; and 4, carrying out matching positioning on the point cloud map, and obtaining a vehicle pose by adopting a point cloud registration algorithm so as to realize seamless positioning. According to the technical scheme, the problems of global point cloud map building and global seamless positioning in a high dynamic environment (frequent movement of quay cranes and ships) are solved.

Description

Technical Field

[0001] The present application relates to the field of intelligent driving technology, and in particular to a seamless positioning system and method based on a port environment. Background Art

[0002] With the development of science and technology, the automatic identification of vehicles and containers, the improvement of positioning technology, and the application of unmanned container trucks in my country's ports and terminals are becoming more and more popular, which greatly reduces labor costs, speeds up work efficiency, and improves work accuracy. The commonly used vehicle positioning technology is based on the GPS (Global Positioning System) device on the vehicle. However, due to the obstruction of the quay crane, especially the large ship and bridge crane of the continuous quay crane, the signal strength is insufficient, and the GPS device cannot realize the positioning function. Therefore, the traditional GPS device cannot effectively locate the vehicle in this environment. Moreover, the lane lines on the quay crane are different from the lane lines on ordinary roads, which are very chaotic. It is impossible to achieve stable operation in the port based on the lanes on normal roads. The existing technical solution goes to the quay crane area based on GPS positioning data, and conducts continuous spatial scanning to establish the point cloud map data of the cargo ship in the quay crane area, detects the GPS positioning signal in real time, and matches the point cloud objects in the point cloud map data with the GPS positioning data. When the GPS positioning signal does not meet the preset signal strength threshold, a new path is generated for loading and unloading operations. However, the existing technical solution can only perform navigation switching in planning control, which increases the difficulty of algorithm design of the planning module. In addition, during port operations, the platform sends the location of the quay crane and the yard station. The vehicle passes the station location and navigates to the destination based on the navigation path. The location of the quay crane is generally obtained through the RTK equipment installed on the port machinery equipment, and the yard is generally obtained by confirming the bay number coordinates by marking. Through the above scheme, the quay crane station coordinates cannot be accurately sent, which greatly affects the operation efficiency. Moreover, most port operations are unmanned container truck formation operations, and the local point cloud established by a single vehicle cannot be shared with other vehicles, affecting the operation efficiency. Summary of the invention

[0003] In view of this, the present invention provides a seamless positioning system and method based on a port environment to solve technical problems such as the difficulty in algorithm design in the prior art, the inability to accurately send the coordinates of the Onbridge site, the inability to share other vehicles, and thus affecting work efficiency.

[0004] The present invention provides a seamless positioning system based on a port environment, which is applied to an intelligent driving vehicle. The system comprises: a cloud platform, which is used to issue a map acquisition instruction; a sensor component, which is arranged on the vehicle and comprises a combined navigation, a wheel speed sensor and a laser radar sensor, and is used to acquire sensor data and send it to an intelligent driving device; the intelligent driving device is arranged on the vehicle and is respectively connected to the cloud platform and the sensor component, and is used to control the sensor component to acquire sensor data after receiving the map acquisition instruction sent by the cloud platform, obtain a point cloud map according to the sensor data and graph optimization theory, match and locate the point cloud map, and adopt a point cloud registration algorithm to obtain the vehicle posture, thereby realizing seamless positioning.

[0005] Furthermore, the cloud platform is also used to receive and store the point cloud map sent by the intelligent driving device; the intelligent driving device is also used to send the point cloud map to the cloud platform.

[0006] The present invention also provides a method for a seamless positioning system based on a port environment, the method comprising: step 1, the cloud platform sends a mapping instruction to the intelligent driving device; step 2, after the intelligent driving device receives the mapping instruction, it controls the sensor component to obtain sensor data; step 3, the intelligent driving device obtains a point cloud map based on the sensor data and graph optimization theory; step 4, matches and locates the point cloud map, and uses a point cloud registration algorithm to obtain the vehicle posture, thereby achieving seamless positioning.

[0007] Furthermore, the method also includes: step 5, the intelligent driving device uploads the point cloud map to the cloud platform; step 6, the cloud platform sends the point cloud map to other vehicles; step 7, after the point cloud map is sent, other vehicles complete the point cloud map update.

[0008] Furthermore, the step 3 includes: step 31, using the lidar sensor to synchronously build local maps, and using inertial navigation to estimate the vehicle posture; step 32, jointly optimizing the local mapping and the combined navigation posture; step 33, estimating the vehicle posture by lidar, and integrating it with the inertial navigation posture to construct the posture; step 34, according to the constructed sub-map and posture constraints, using graph optimization to optimize the radar related frames to obtain a point cloud map.

[0009] Furthermore, the specific method of using graph optimization is: through the objective function , the optimization is completed in the form of summing n edges, where x k and z k is the optimization variable, Ω is the information matrix, and e function is the error.

[0010] Furthermore, the step 4 includes: step 41, matching and positioning the point cloud map to determine whether the vehicle is located in the working area, if so, go to step 42, if not, go to step 43; step 42, using the point cloud registration algorithm to obtain the vehicle posture; step 43, after the vehicle moves, go to step 41.

[0011] Furthermore, the step 4 also includes: step 40, the cloud platform sends the task operation to the vehicle, and after receiving the task operation, the vehicle drives to the operation area and goes to step 41.

[0012] Further, the step 42 includes: step 421, determining the target point cloud and the source point cloud; step 422, according to the function , determine the corresponding point cloud, and construct an initial point pair set, where pi is the source point cloud and qj is the target point cloud; step 423, according to the corresponding points and the objective function , calculate the rotation matrix R and the translation matrix T; step 424, according to the matrix R and the vector T, obtain the changed point cloud coordinates, and calculate whether the distance error between the current point cloud and the target point cloud is less than the preset threshold; step 425, if it is less than, the matching is completed.

[0013] Further, the step 42 includes: step 426, if it is not less than, go to step 422.

[0014] The present invention provides a seamless positioning system and method based on a port environment. The technical solution sends a map collection instruction through a cloud platform, so that an intelligent driving device controls a sensor component to collect sensor data, and performs real-time mapping, global map optimization, and matching positioning based on the data, thereby solving the problems of global point cloud map construction and global seamless positioning in a high dynamic environment (quay cranes, frequent movement of ships). BRIEF DESCRIPTION OF THE DRAWINGS

[0015] Figure 1 It is a schematic diagram of the overall framework of a seamless positioning system based on a port environment provided by the present invention; Figure 2 It is a flow chart of a seamless positioning method based on a port environment provided by the present invention; Figure 3 It is a flow chart of another seamless positioning method based on port environment provided by the present invention; Figure 4 This is a flow chart of a seamless positioning system operation based on a port environment provided by the present invention; Figure 5 It is a schematic diagram of a factor graph optimization algorithm provided by the present invention; Figure 6 It is a schematic diagram of the point cloud registration algorithm flow provided by the present invention; Figure 7It is a schematic diagram of the PCD diagram of the operating quay crane provided by the present invention; Figure 8 It is a schematic diagram of the continuous quay crane matching trajectory provided by the present invention. DETAILED DESCRIPTION

[0016] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0017] Embodiment 1: The present invention provides a system and method for seamless positioning based on a port environment, such as Figure 1 As shown, the system includes a cloud platform, a sensor component, and an intelligent driving device. Figure 3 As shown, the method steps are as follows.

[0018] Step 1: The cloud platform sends a map acquisition instruction to the intelligent driving device; Step 2: After receiving the map acquisition instruction, the intelligent driving device controls the sensor component to acquire sensor data; Step 3: The intelligent driving device obtains a point cloud map based on the sensor data and graph optimization theory; Step 4: Match and locate the point cloud map, and use the point cloud registration algorithm to obtain the vehicle posture, thereby achieving seamless positioning.

[0019] The present invention provides a seamless positioning system and method based on a port environment. The technical solution issues mapping instructions through a cloud platform, so that an intelligent driving device controls a sensor component to collect sensor data, performs real-time mapping, global map optimization, and matching positioning based on the data, thereby solving technical problems such as the difficulty of algorithm design in the prior art, the inability to accurately issue Onkyo site coordinates, and the inability to share other vehicles, which in turn affects work efficiency.

[0020] Embodiment 2: The present invention provides a system and method for seamless positioning based on a port environment, such as Figure 1 As shown, the system includes a cloud platform, a sensor component, and an intelligent driving device. Figure 2 As shown, the method steps are as follows.

[0021] Step 1: The cloud platform sends a map acquisition instruction to the intelligent driving device; Step 2: After receiving the map acquisition instruction, the intelligent driving device controls the sensor component to acquire sensor data; Step 3: The intelligent driving device obtains a point cloud map based on the sensor data and graph optimization theory; like Figure 4 As shown, the seamless positioning system based on the port environment provided by the present invention is mainly used to complete the three tasks: map collection task, map distribution task, and operation task. Steps 1, 2, and 3 are steps for executing the map collection task, and the specific implementation process of step 3 is as follows.

[0022] Step 31, using the laser radar sensor to synchronously build local maps, and then using inertial navigation to estimate the vehicle posture; Step 32, jointly optimizing the local mapping and the combined navigation pose; Step 33, estimating the vehicle posture by laser radar and integrating it with the inertial navigation posture to construct the posture; Step 34, according to the constructed sub-map and the constraints of the posture, the radar related frames are optimized by using graph optimization to obtain a point cloud map.

[0023] When executing the map acquisition task, the vehicle end (i.e., the intelligent driving device on the vehicle, AD end) starts the map building task after receiving the map acquisition command, such as Figure 7 As shown in Figure 1, laser SLAM (Simultaneous Localization and Mapping) local mapping and combined navigation pose are used for joint optimization mapping. Figure 5 As shown in the figure, the LiDAR pose estimation result is directly integrated with the INS (inertial navigation positioning) pose to construct the pose, and the pose graph is optimized based on the classical pose graph optimization theory, and then the final pose is output. Graph optimization is an effective method for solving large-scale positioning and mapping problems. When only the pose composition graph is optimized without considering the converged observation points, it is called a pose graph optimization algorithm. The specific method of using graph optimization is: through the objective function , the optimization is completed in the form of summing n edges, where x k and z k is the optimization variable, Ω is the information matrix, and the e function is the error. As mentioned above, the system factor graph construction includes the IMU (inertial sensor, also known as inertial measurement unit) measurement constraints, the pose constraints of the submap and radar frame, and the constraints of the global pose measurement. The G2O graph optimization library is used to optimize the radar related frames to obtain the point cloud map. Since graph optimization is an existing technology, the details will not be explained here.

[0024] Step 4: Match and locate the point cloud map, and use the point cloud registration algorithm to obtain the vehicle posture, thereby achieving seamless positioning; Step 5, the intelligent driving device uploads the point cloud map to the cloud platform; Step 6: The cloud platform sends the point cloud map to other vehicles; Step 7: After the point cloud map is sent, other vehicles complete the point cloud map update.

[0025] The present invention provides a seamless positioning system and method based on a port environment. The technical solution issues mapping instructions through a cloud platform, so that an intelligent driving device controls a sensor component to collect sensor data, performs real-time mapping, global map optimization, and matching positioning based on the data, thereby solving technical problems such as the difficulty of algorithm design in the prior art, the inability to accurately issue Onkyo site coordinates, and the inability to share other vehicles, which in turn affects work efficiency.

[0026] Embodiment three: The present invention provides a system and method for seamless positioning based on a port environment, such as Figure 1 As shown, the system includes a cloud platform, a sensor component, and an intelligent driving device. Figure 2 As shown, the method steps are as follows.

[0027] Step 1: The cloud platform sends a map acquisition instruction to the intelligent driving device; Step 2: After receiving the map acquisition instruction, the intelligent driving device controls the sensor component to acquire sensor data; Step 3: The intelligent driving device obtains a point cloud map based on the sensor data and graph optimization theory; Step 4: Match and locate the point cloud map, and use the point cloud registration algorithm to obtain the vehicle posture, thereby achieving seamless positioning; As can be seen from the foregoing, the system provided by the present invention is mainly used to complete three tasks, and step 4 is the step of executing the operation task, and its specific implementation process is as follows.

[0028] Step 40: The cloud platform sends the task operation to the vehicle, and after receiving the task operation, the vehicle drives to the operation area; Step 41, match and locate the point cloud map to determine whether the vehicle is located in the operation area, if yes, go to step 42, if not, go to step 43; Step 42, using a point cloud registration algorithm to obtain the vehicle posture; like Figure 6 As shown, point cloud matching and positioning use the ICP matching algorithm, that is, the point cloud registration algorithm to obtain the vehicle posture. The specific steps are as follows.

[0029] Step 421, determining the target point cloud and the source point cloud; Step 422, according to the function , determine the corresponding point cloud and construct the initial point pair set, where p i is the source point cloud, q j is the target point cloud; Step 423, based on the corresponding points and the target function , calculate the rotation matrix R and translation matrix T; Step 424, obtaining the changed point cloud coordinates according to the matrix R and the vector T, and calculating whether the distance error between the current point cloud and the target point cloud is less than a preset threshold; Step 425: If it is less than, the matching is completed; if it is less than, the matching is completed.

[0030] Since the ICP matching algorithm is an existing technology, it will not be described here in detail.

[0031] Step 43, after the vehicle moves, go to step 41.

[0032] After continuous matching and positioning, the continuous quay crane matching trajectory is as follows Figure 8 In addition, as can be seen from the foregoing, the system for seamless positioning based on port environment provided by the present invention is mainly used to complete the three tasks: map collection task, map distribution task, and operation task. Steps 5, 6, and 7 are steps for executing the map distribution task.

[0033] Step 5, the intelligent driving device uploads the point cloud map to the cloud platform; Step 6: The cloud platform sends the point cloud map to other vehicles; Step 7: After the point cloud map is sent, other vehicles complete the point cloud map update.

[0034] The present invention provides a seamless positioning system and method based on a port environment. The technical solution issues mapping instructions through a cloud platform, so that an intelligent driving device controls a sensor component to collect sensor data, performs real-time mapping, global map optimization, and matching positioning based on the data, thereby solving technical problems such as the difficulty of algorithm design in the prior art, the inability to accurately issue Onkyo site coordinates, and the inability to share other vehicles, which in turn affects work efficiency.

[0035] In summary, the embodiment of the present invention provides a seamless positioning system and method based on the port environment. The technical solution does not need to switch between local coordinates and global coordinates, which reduces the difficulty of designing the algorithm of the planning control module. Moreover, the solution matches and locates through the point cloud map to obtain the vehicle posture to achieve seamless positioning and the quay crane site coordinates, which greatly improves the work efficiency. At the same time, the solution can also realize the sharing of point cloud maps by multiple vehicles and can perform platoon operations. It solves the technical problems of the difficulty of algorithm design in the prior art, the inability to accurately issue the coordinates of the Enbridge site, the inability to share other vehicles, and thus affecting the work efficiency.

[0036] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention should be included in the scope of protection of the present invention.

Claims

1. A seamless positioning system based on a port environment, applied to intelligent driving vehicles, characterized in that: The system comprises: Cloud platform, used to issue map acquisition instructions; A sensor assembly, which is arranged on the vehicle and includes a combined navigation, a wheel speed sensor and a laser radar sensor, and is used to obtain sensor data and send it to the intelligent driving device; The intelligent driving device is installed on the vehicle and connected to the cloud platform and sensor components respectively. It is used to control the sensor components to obtain sensor data after receiving the map acquisition instructions sent by the cloud platform, and obtain the point cloud map based on the sensor data and graph optimization theory. The point cloud map is then matched and positioned, and the vehicle posture is obtained using the point cloud registration algorithm, thereby achieving seamless positioning.

2. A seamless positioning system based on port environment according to claim 1, characterized in that: The cloud platform is also used to receive and store the point cloud map sent by the intelligent driving device; the intelligent driving device is also used to send the point cloud map to the cloud platform.

3. A method using the seamless positioning system based on port environment as described in claims 1-2, characterized in that: The method comprises: Step 1: The cloud platform sends a map acquisition instruction to the intelligent driving device; Step 2: After receiving the map acquisition instruction, the intelligent driving device controls the sensor component to acquire sensor data; Step 3: The intelligent driving device obtains a point cloud map based on the sensor data and graph optimization theory; Step 4: Match and locate the point cloud map, and use the point cloud registration algorithm to obtain the vehicle posture, thereby achieving seamless positioning.

4. A seamless positioning method based on port environment according to claim 3, characterized in that: The method further comprises: Step 5, the intelligent driving device uploads the point cloud map to the cloud platform; Step 6: The cloud platform sends the point cloud map to other vehicles; Step 7: After the point cloud map is sent, other vehicles complete the point cloud map update.

5. A seamless positioning method based on port environment according to claim 3, characterized in that: The step 3 comprises: Step 31, using a laser radar sensor to perform synchronous local mapping, and then using inertial navigation to estimate the vehicle posture; Step 32, jointly optimizing the local mapping and the inertial navigation posture; Step 33, estimating the vehicle posture by laser radar, and integrating it with the vehicle posture estimated by inertial navigation to construct a constrained posture; Step 34, according to the constructed local map and constrained pose, the radar related frames are optimized by using graph optimization to obtain a point cloud map.

6. A seamless positioning method based on port environment according to claim 5, characterized in that: The specific method of using graph optimization is: , the optimization is completed in the form of summing n edges, where x k and z k is the optimization variable, Ω is the information matrix, and e function is the error.

7. A seamless positioning method based on port environment according to claim 3, characterized in that: The step 4 comprises: Step 41, match and locate the point cloud map to determine whether the vehicle is located in the operation area, if yes, go to step 42, if not, go to step 43; Step 42, using a point cloud registration algorithm to obtain the vehicle posture; Step 43, after the vehicle moves, go to step 41.

8. A seamless positioning method based on port environment according to claim 7, characterized in that: The step 4 also includes: In step 40 , the cloud platform sends the task operation to the vehicle. After receiving the task operation, the vehicle drives to the operation area and goes to step 41 .

9. A seamless positioning method based on port environment according to claim 7, characterized in that: The step 42 comprises: Step 421, selecting a target point cloud and a source point cloud; Step 422, according to the function , determine the corresponding point cloud and construct the initial point pair set, where p i is the source point cloud, q j is the target point cloud; Step 423, based on the corresponding points and the target function , calculate the rotation matrix R and translation matrix T; Step 424, obtaining the changed point cloud coordinates according to the matrix R and the vector T, and calculating whether the distance error between the changed point cloud and the target point cloud is less than a preset threshold; Step 425: If less than, the matching is completed.

10. A seamless positioning method based on port environment according to claim 9, characterized in that: The step 42 comprises: Step 426: if not less than, go to step 422.

Images