WO2022110653A1 - 一种位姿确定方法及装置、电子设备和计算机可读存储介质 - Google Patents

一种位姿确定方法及装置、电子设备和计算机可读存储介质 Download PDF

Info

Publication number
WO2022110653A1
WO2022110653A1 PCT/CN2021/092487 CN2021092487W WO2022110653A1 WO 2022110653 A1 WO2022110653 A1 WO 2022110653A1 CN 2021092487 W CN2021092487 W CN 2021092487W WO 2022110653 A1 WO2022110653 A1 WO 2022110653A1
Authority
WO
WIPO (PCT)
Prior art keywords
line segment
pose
radar
traveling device
map
Prior art date
Application number
PCT/CN2021/092487
Other languages
English (en)
French (fr)
Inventor
唐庆
王兆圣
陆潇
刘余钱
Original Assignee
浙江商汤科技开发有限公司
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by 浙江商汤科技开发有限公司 filed Critical 浙江商汤科技开发有限公司
Publication of WO2022110653A1 publication Critical patent/WO2022110653A1/zh

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
    • G01S13/88—Radar or analogous systems specially adapted for specific applications
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
    • G01C21/30—Map- or contour-matching
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
    • G01S13/86—Combinations of radar systems with non-radar systems, e.g. sonar, direction finder
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
    • G01S13/88—Radar or analogous systems specially adapted for specific applications
    • G01S13/881—Radar or analogous systems specially adapted for specific applications for robotics

Definitions

  • the present disclosure relates to the field of computer technology, and in particular, to a method and apparatus for determining a pose, an electronic device, and a computer-readable storage medium.
  • radar-based pose estimation technology has been widely used, such as in the automatic driving or assisted driving of smart cars.
  • the position and orientation of smart cars can be estimated by radar, which can help smart cars rationally plan the next driving route. .
  • the embodiment of the present disclosure proposes a technical solution for pose determination.
  • a pose determination method including:
  • the estimated pose of the traveling device in the line segment map determine the estimated pose of the radar of the traveling device in the line segment map, and the things in the line segment map are represented by line segments;
  • the target pose of the traveling device in the line segment map is determined.
  • determining the estimated pose of the radar of the traveling device in the line segment map according to the estimated pose of the traveling device in the line segment map including:
  • the estimated poses of the traveling device at a second moment are obtained through interpolation processing, and the second moment includes the point cloud collected by the radar. time;
  • the estimated pose of the traveling device at the second moment is obtained.
  • the method before the matching of the point cloud collected by the radar with the line segments in the line segment map, the method further includes:
  • Matching the point cloud collected by the radar with the line segments in the line segment map including:
  • the point cloud collected by the radar is matched with the target line segment.
  • the first preset condition includes:
  • the absolute value of the included angle between the line segment and the facing direction is less than the first threshold
  • the distance between the line segment and the first line segment is less than a distance threshold, and the first line segment is a line segment whose absolute value of the included angle with the orientation is less than a second threshold value.
  • the point cloud collected by the radar is matched with the line segment in the line segment map, and it is determined according to the matching result that the radar is in the location where the matching is performed.
  • the poses in the line segment map including:
  • the estimated pose of the radar determine the coordinates of the point cloud scanned by the radar in the line segment map
  • a target transformation matrix is determined, wherein the point cloud at the coordinates is translated and rotated by the target transformation matrix and the line segment map The line segments in match;
  • the estimated pose of the radar is translated and rotated to obtain the pose of the radar in the line segment map.
  • the determining the target pose of the traveling device in the line segment map according to the matched pose of the radar includes:
  • the matched pose of the radar determine the matched pose of the traveling device
  • the matched pose of the traveling device and the estimated pose of the traveling device are fused to obtain the target pose of the traveling device in the line segment map.
  • the method for determining the pose in response to the pose when the method for determining the pose in response to the pose is periodically executed, before determining the estimated pose of the radar of the traveling device in the line segment map, further include:
  • the pose of the traveling device in the line segment map is estimated to obtain the current cycle of the traveling device.
  • the estimated pose of the device, and the sensor data is collected based on the motion information of the traveling device.
  • the line segment map is obtained by performing straight line fitting on an initial map, where the initial map includes at least one of an occupation grid map and a site design drawing.
  • the traveling device includes a traveling device based on an embedded platform, the traveling device runs in a sand table, and the line segment map is a map of the sand table.
  • an apparatus for determining a pose including:
  • the estimated pose determination part is configured to determine the estimated pose of the radar of the traveling device in the line segment map according to the estimated pose of the traveling device in the line segment map, and the things in the line segment map pass through line segment representation;
  • the radar pose determination part is configured to match the point cloud collected by the radar with the line segments in the line segment map according to the estimated pose of the radar, and determine the location of the radar after matching according to the matching result.
  • the target pose determination part is configured to determine the target pose of the traveling device in the line segment map according to the matched pose of the radar.
  • the estimated pose determination part is further configured to use the estimated poses of the traveling device at a plurality of first moments to obtain the traveling device at the second time through interpolation processing.
  • the estimated pose at the time, the second time includes the time when the radar collects the point cloud; according to the estimated pose of the driving device at the second time, the radar is obtained at the second time The estimated pose at the moment.
  • the apparatus further includes:
  • a target line segment determination part configured to determine a target line segment in the line segment map that satisfies a first preset condition according to the orientation in the estimated pose of the traveling device
  • the radar pose determination part is configured to match the point cloud collected by the radar with the target line segment.
  • the first preset condition includes:
  • the absolute value of the included angle between the line segment and the facing direction is less than the first threshold
  • the distance between the line segment and the first line segment is less than a distance threshold, and the first line segment is a line segment whose absolute value of the included angle with the orientation is less than a second threshold value.
  • the radar pose determination part is further configured to determine the coordinates of the point cloud scanned by the radar in the line segment map according to the estimated pose of the radar;
  • the coordinates of the point cloud in the line segment map and the line segments in the line segment map determine a target conversion matrix, wherein the point cloud at the coordinates is translated and rotated by the target conversion matrix and is different from the line segment in the line segment map.
  • Line segment matching; through the target transformation matrix, the estimated pose of the radar is translated and rotated to obtain the pose of the radar in the line segment map.
  • the target pose determination part is further configured to determine the pose of the traveling device after matching according to the pose of the radar after matching; The pose is fused with the estimated pose of the traveling device to obtain the target pose of the traveling device in the line segment map.
  • the apparatus further includes:
  • the current estimated pose determination part is configured to, according to the target pose of the traveling device determined in the previous cycle and the sensing data collected by the traveling device in the current cycle, determine the position of the traveling device in the line segment map.
  • the estimated pose of the traveling device in the current cycle is obtained, and the sensing data is collected based on the motion information of the traveling device.
  • the line segment map is obtained by performing straight line fitting on an initial map, where the initial map includes at least one of an occupation grid map and a site design drawing.
  • the traveling device includes a traveling device based on an embedded platform, the traveling device runs in a sand table, and the line segment map is a map of the sand table.
  • an electronic device comprising: a processor; a memory configured to store instructions executable by the processor; wherein the processor is configured to invoke the instructions stored in the memory, to perform the above method.
  • a computer-readable storage medium having computer program instructions stored thereon, the computer program instructions implementing the above method when executed by a processor.
  • a computer program including computer-readable codes, and when the computer-readable codes are executed in an electronic device, the processor in the electronic device implements the above when executed. method.
  • the estimated pose of the radar of the traveling device in the line segment map can be determined according to the estimated pose of the traveling device in the line segment map, and the radar is collected according to the pre-estimated estimated pose.
  • Matching the point cloud in the line segment map with the line segment in the line segment map reduces the range of the matching area in the line segment map and reduces the calculation amount of the pose determination process.
  • the point cloud is used to match the line segments in the line segment map, which reduces the calculation amount of the pose determination process and improves the efficiency of the pose determination of the driving equipment.
  • FIG. 1 shows a schematic diagram of the architecture of a pose determination system according to an embodiment of the present disclosure
  • FIG. 2 shows a flowchart of a pose determination method according to an embodiment of the present disclosure
  • FIG. 3A shows a schematic diagram of a target line segment in a line segment map according to an embodiment of the present disclosure
  • 3B shows a schematic diagram of a target line segment in another line segment map according to an embodiment of the present disclosure
  • FIG. 4 shows a block diagram of an apparatus for determining a pose according to an embodiment of the present disclosure
  • FIG. 5 shows a block diagram of an electronic device according to an embodiment of the present disclosure
  • FIG. 6 shows a block diagram of an electronic device according to an embodiment of the present disclosure.
  • the pose determination method provided by the embodiments of the present disclosure may be executed by a traveling device, or may be executed by the traveling device sending the collected data to other devices, for example, an electronic device such as a terminal device or a server.
  • the terminal device can be a user equipment (User Equipment, UE), a mobile device, a user terminal, a terminal, a cellular phone, a cordless phone, a Personal Digital Assistant (PDA), a handheld device, a computing device, a vehicle-mounted device, a wearable devices, etc.
  • UE User Equipment
  • PDA Personal Digital Assistant
  • FIG. 1 is a schematic structural diagram of an exemplary pose determination system 10 provided by an embodiment of the present disclosure; as shown in FIG. 1 , the pose determination system 10 includes a terminal device/server 100 and a driving device 200 (Fig. A traveling device 200-1) is shown by way of example in 1).
  • the traveling device 200-1 can send the collected motion information or sensor data, as well as the collected point cloud and other data to the terminal device/server 100 during the driving process;
  • the relative relationship between the position and attitude of the device and the radar, as well as data such as the estimated position and attitude of the driving device in the line segment map, the terminal device/server 100 determines The estimated pose of the radar of the driving device 200-1 in the line segment map, according to the estimated pose of the radar of the traveling device 200-1, the point cloud collected by the radar is matched with the line segment in the line segment map, and according to the matching result Determine the pose of the radar in the line segment map after matching, and finally determine the target pose of the traveling device 200-1 in the line segment map according to the pose of the matched radar, and use the traveling device 200-1 in the line segment map to determine the pose of the target.
  • the target pose is sent to the traveling device 200-1.
  • the traveling device by sending the collected data to other devices such as a terminal device or a server by the traveling device, and determining the target pose of the traveling device 200-1 by other devices, the calculation amount of the traveling device can be reduced.
  • FIG. 2 shows a flowchart of a pose determination method according to an embodiment of the present disclosure. As shown in FIG. 2 , the pose determination method includes:
  • step S11 the estimated pose of the radar of the traveling device in the line segment map is determined according to the estimated pose of the traveling device in the line segment map.
  • connection segments for example, roads and buildings are represented by line segments, etc.
  • the line segment map can be implemented in many ways, and the line segment map will be described in detail later in conjunction with the possible implementation methods of the present disclosure.
  • the traveling device here may be a device whose geographic location can be changed, and the change of the geographic location may be an autonomous change of the traveling device, or a change caused by an external force.
  • the traveling device may be a traveling device in the field of unmanned driving or artificial intelligence education.
  • the traveling device may be a vehicle (for example, an embedded self-driving car), a ship, an aircraft and other devices, or it may also be a robot, such as Sweeping robots, handling robots and educational robots, etc.
  • the pose can include a position and an attitude
  • the position can be the coordinates of the device on the map
  • the pose can be the direction of the device on the map.
  • the estimated pose of the traveling device may be a pre-estimated pose, which may be estimated according to the pose determined at the target time of the traveling device.
  • the estimated pose of the traveling device will be estimated later in combination with the possible implementations of the embodiments of the present disclosure. posture is described in detail.
  • the estimated pose of the radar of the traveling device in the line segment map may be determined by the estimated pose of the traveling device according to the relative relationship between the pose of the traveling device and the radar.
  • the relative relationship In the case where the radar base is fixed on the traveling equipment, the relative relationship is often fixed. Therefore, the relative relationship can be manually determined in advance, that is, the relative relationship is a preset value.
  • step S12 according to the estimated pose of the radar, the point cloud collected by the radar is matched with the line segment in the line segment map, and it is determined according to the matching result that the radar is in the line segment map after matching. 's pose.
  • the point cloud collected by the radar is located in the polar coordinate system of the radar, such as the polar coordinate system with the radar as the center point, the point cloud obtained by the radar scanning the surrounding objects is located around the center point in the polar coordinate system .
  • the estimated position and attitude of the radar in the line segment map After the estimated position and attitude of the radar in the line segment map is obtained, the estimated position and attitude of the radar in the world coordinate system are determined. Therefore, the polar coordinates can be calculated according to the estimated position and attitude of the radar in the world coordinate system.
  • the point cloud in the system is mapped to the world coordinate system, and the position and attitude of the point cloud scanned by the radar in the world coordinate system are obtained. That is, according to the estimated pose of the radar in the line segment map and the pose of the radar scanned point cloud in the radar polar coordinate system, the pose of the radar scanned point cloud in the line segment map is obtained.
  • the pose is a pre-estimated pose, that is, the pre-estimated general position and direction of the radar-scanned point cloud in the line-segment map, it can be determined according to The general position and direction of the point cloud scanned by the radar in the line segment map.
  • the "matching” mentioned here can be understood as finding the line segment corresponding to the thing represented by the point cloud in the line segment map.
  • the point cloud can be translated and rotated to find the line segment that matches the point cloud as much as possible Coinciding line segments, once a line segment with the expected degree of coincidence with the point cloud is found, the matching is considered complete.
  • the point cloud In the process of matching the point cloud with the line segments in the line segment map, the point cloud is moved and rotated, and a line segment that overlaps the point cloud as much as possible is found in the line segment map. Since the point cloud is the relative position information of the points scanned by the radar on the surrounding things, and the line segment in the line segment map also represents the location information of the thing, therefore, when the point cloud and the line segment are matched, you can The position and posture of the radar is reversed, and the reversed posture can be considered as the radar's posture when scanning the point cloud.
  • step S13 the target pose of the traveling device in the line segment map is determined according to the matched pose of the radar.
  • the pose of the traveling device in the line segment map can be determined according to the relative relationship between the pose of the traveling device and the radar.
  • the pose is referred to as the target pose here.
  • the target pose may also be determined according to other factors, which will be described in detail later in conjunction with possible implementations of the embodiments of the present disclosure.
  • the estimated pose of the radar of the traveling device in the line segment map can be determined according to the estimated pose of the traveling device in the line segment map, and the radar is collected according to the pre-estimated estimated pose.
  • Matching the point cloud in the line segment map with the line segment in the line segment map reduces the range of the matching area in the line segment map and reduces the calculation amount of the pose determination process.
  • the point cloud is used to match the line segments in the line segment map, which reduces the calculation amount of the pose determination process and improves the efficiency of the pose determination of the driving equipment.
  • the pose determination method provided by the embodiment of the present disclosure may also be implemented in a variety of ways.
  • the pose of the traveling device is determined by performing point-line matching on a line segment map.
  • the line segment map is obtained by performing straight line fitting on an initial map, and the initial map includes at least one of an occupancy grid map and a site design map.
  • a real-time positioning and mapping algorithm can be used to map the driving site of the driving equipment to obtain the occupancy grid map of the driving site.
  • the line segment map can be obtained by directly fitting a straight line to the initial image, which improves the efficiency of drawing the line segment map.
  • the line segment map is determined directly based on the site design drawing. Since the site design drawing is often a relatively simple line drawing, it is more suitable for the straight line fitting algorithm, and the obtained line segment map is more accurate, and there is no need to pass other algorithms.
  • the initial map is constructed, which is more convenient.
  • the obtained line segment map is a very lightweight map compared to other maps, which can improve the speed of pose estimation.
  • the estimated pose of the traveling device may be estimated based on the pose determined at the target time of the traveling device.
  • the pose determination method in the embodiment of the present disclosure is periodically executed.
  • the pose at the target moment may be the target pose determined in the previous cycle, and in addition, the pose at the target moment may also be the pose determined by other means, such as the pose determined by a position sensor such as a global positioning system .
  • the method further includes: according to The target pose of the traveling device determined in the previous cycle and the sensing data collected by the traveling device in the current cycle are used to estimate the pose of the traveling device in the line segment map to obtain the traveling device in the current cycle.
  • the estimated pose, the sensing data is collected based on the motion information of the traveling equipment.
  • the motion information of the traveling equipment may include at least one of attitude (yaw) angle, angular velocity, acceleration, wheel speed, etc., and the sensor data may be collected by sensors such as an inertial measurement unit (IMU) or a wheel speedometer. data.
  • attitude yaw
  • angular velocity angular velocity
  • acceleration acceleration
  • wheel speed etc.
  • sensor data may be collected by sensors such as an inertial measurement unit (IMU) or a wheel speedometer. data.
  • IMU inertial measurement unit
  • the data collected by the front-end IMU in the current cycle can be integrated and fused, and the wheel speedometer can be collected. Integrate and fuse the wheel speed of the current cycle to obtain the estimated pose of the driving equipment in the current cycle; or, based on the target pose of the driving device determined in the previous cycle, Kalman filter fusion can be performed on the sensor data collected by the sensor to obtain the current cycle.
  • the estimated pose of the driving device based on the target pose of the driving equipment determined in the previous cycle.
  • the angular velocity collected by the IMU will be integrated in time, so that the angle change of the driving equipment can be obtained; By integrating the speed, the change in displacement of the traveling equipment can be obtained. Then, based on the target pose of the traveling device determined in the previous cycle, plus the obtained change in angle and displacement, the estimated pose of the traveling device in the current cycle can be obtained.
  • the Kalman prediction equation will be used, and based on the target pose of the traveling equipment determined in the previous cycle, the angles and displacements obtained from different sensing data will be calculated according to The ratio is fused to obtain the estimated pose of the current cycle driving equipment.
  • the fusion ratio is determined by Kalman Gain (Kg), and Kg is updated by the Kalman update equation according to the prediction result of the Kalman prediction equation, so that the update The prediction results of the latter Kalman prediction equation are more accurate.
  • the target pose used in the process of determining the estimated pose for the initial period may be manually set or calculated according to other parameters manually set.
  • the estimated pose can be determined at a preset frequency in the current cycle, and the estimated poses at multiple first moments in the current cycle can be obtained. For example, 100 can be output in 1 second. frequency to output the estimated pose.
  • the pose of the traveling device in the line segment map is estimated by using the target pose determined in the previous cycle and the sensor data collected by the traveling device in the current cycle, so that the position and orientation of the traveling device can be accurately estimated. pose, which facilitates subsequent point-line matching and improves the accuracy of the determined pose.
  • determining the estimated pose of the radar of the traveling device in the line segment map according to the estimated pose of the traveling device in the line segment map includes: using the traveling device For the estimated poses at a plurality of first moments, the estimated poses of the traveling equipment at a second moment are obtained through interpolation processing, and the second moment includes the moment when the radar collects the point cloud; according to the The estimated pose of the traveling device at the second moment is obtained, and the estimated pose of the radar at the second moment is obtained.
  • the radar scans the surrounding objects at a certain frequency to obtain the point cloud.
  • the moment when the radar collects the point cloud is called the second moment, and there can be multiple second moments. Since the first moment at which the front end determines the estimated pose of the traveling device may be different from the second moment, the estimated pose of the traveling device at multiple first moments can be used to obtain the estimated pose of the traveling device at the second moment through interpolation processing. Estimating pose.
  • the estimated poses within the data range at the first moment will be interpolated by means of interpolation to obtain the estimated poses of the driving equipment at the second moment;
  • interpolation processing is performed on a plurality of estimated poses outside the data range at the first moment to obtain the estimated pose of the traveling device at the second moment.
  • the estimated pose of the second moment obtained by interpolation is the estimated pose of the driving equipment. Therefore, according to the relative relationship between the radar and the pose of the driving equipment, the estimated pose of the second moment obtained by interpolation can be obtained. The estimated pose at the second moment.
  • the estimated pose at the moment when the radar collects the point cloud is accurately obtained, which facilitates subsequent point-line matching and improves the determined pose accuracy.
  • the method before the matching of the point cloud collected by the radar with the line segments in the line segment map, the method further includes: determining, according to the orientation in the estimated pose of the traveling device, determining The target line segment in the line segment map that satisfies the first preset condition; matching the point cloud collected by the radar with the line segment in the line segment map includes: matching the point cloud collected by the radar with the target line segment. match.
  • the traveling device In the process of traveling on the track, the traveling device often travels along the track, so the orientation of the traveling device and the two sides of the track where the traveling device is located are often parallel or nearly parallel.
  • the things in the line segment map are also represented by line segments. Therefore, the matching line segments can be filtered according to the orientation of the driving device, and the line segments can be filtered by setting conditions, and the line segment that matches the orientation of the driving device can be selected.
  • the first preset condition includes:
  • the absolute value of the included angle between the line segment and the facing direction is less than the first threshold
  • the distance between the line segment and the first line segment is smaller than a distance threshold, and the first line segment is a line segment whose absolute value of the included angle with the orientation is smaller than a second threshold value.
  • the first threshold may be, for example, 30°, so that a line segment within ⁇ 30° of the traveling device orientation can be selected, as shown in FIG. 3A , for the line segment map of the circular track, the traveling device orientation is shown in FIG.
  • the set of line segments a within which the absolute value of the angle is less than ⁇ 30° is shown in FIG. 3A .
  • the second threshold is smaller than the first threshold, and may be, for example, 3° or 5°, so that the first line segment is a line segment that is parallel or approximately parallel to the orientation of the traveling device. After the first line segment is determined, a line segment whose distance from the first line segment is smaller than the distance threshold is selected as a target line segment for subsequent matching.
  • the distance threshold may be an empirical value set according to experience, and the value is not limited in this embodiment of the present disclosure.
  • FIG. 3B for the line segment map of the circular track, the orientation of the traveling equipment is as shown in FIG. 3B , and FIG. 3B shows the first line segment that is nearly parallel to the orientation of the traveling equipment, and the distance to the first line segment is less than Set b of line segments with distance thresholds.
  • the distance of the line segment may be the distance between the midpoints of the two line segments, or may be the distance between all points on the two line segments. The smallest value in the distance set.
  • the manner of determining the distance between the line segments is not limited in this embodiment of the present disclosure.
  • the point cloud collected by the radar is matched with the line segment in the line segment map, and it is determined according to the matching result that the radar is located in the location after the matching.
  • the pose in the line segment map includes: determining the coordinates of the point cloud scanned by the radar in the line segment map according to the estimated pose of the radar; according to the coordinates of the point cloud in the line segment map and the line segment in the line segment map, determine a target conversion matrix, wherein, the point cloud at the coordinate is matched with the line segment in the line segment map after translation and rotation by the target conversion matrix; through the target conversion matrix, to The estimated pose of the radar is translated and rotated to obtain the pose of the radar in the line segment map.
  • the line segment map can be searched for a line segment that can overlap or completely overlap with the point cloud as much as possible. After that, the matching of point cloud and line segment is realized.
  • the radar-scanned point cloud in the line-segment map can be obtained according to the estimated pose of the radar in the line-segment map (world coordinate system) and the pose of the radar-scanned point cloud in the radar polar coordinate system. coordinate of.
  • the point cloud in the line segment map, can be translated and rotated through a transformation matrix, and a target transformation matrix that makes the point cloud match the line segments in the line segment map during the translation and rotation process is determined.
  • a target transformation matrix that makes the point cloud match the line segments in the line segment map during the translation and rotation process is determined.
  • the distance between the point in the point cloud and the nearest line segment can be calculated. When the sum of the distance between the point in the point cloud and the nearest line segment is the smallest, it is considered that the point cloud matches the line segment in the map. .
  • the point cloud scanned by the radar represents the relative positional relationship of the things around the radar, after the point cloud is matched with the line segment in the map, it indicates the actual pose when the radar scans the point cloud, and the estimated pose is rotated by the target rotation matrix. back pose. Therefore, the more accurate pose of the radar should be the pose after the rotation and translation of the estimated pose through the target transformation matrix. Then, the predicted pose of the radar can be translated and rotated through the target transformation matrix to obtain the radar. Pose in the segment map.
  • the process of matching the point cloud and the line segment in the embodiment of the present disclosure may be implemented by an algorithm, for example, may be implemented based on the Gauss-Newton method, the Levenberg-Marquardt method, or the trust region dogleg method.
  • the point cloud collected by the radar is matched with the line segments in the line segment map according to the pre-estimated estimated pose.
  • the cloud is matched with the line segment in the line segment map, which reduces the calculation amount of the pose determination process and improves the efficiency of the pose determination of the driving equipment.
  • determining the target pose of the traveling device in the line segment map according to the matched pose of the radar includes: determining the matched pose of the radar according to the matched pose.
  • the pose of the traveling device; the matching pose of the traveling device is fused with the estimated pose of the traveling device to obtain the target pose of the traveling device in the line segment map.
  • the traveling device can determine the pose of the matched traveling device according to the relative relationship between the radar and the pose of the traveling device. Then, the matched pose of the traveling device is fused with the estimated pose of the traveling device, and the fused pose is used as the target pose of the traveling device in the line segment map. For example, the pose of the matched traveling device and the estimated pose of the traveling device can be weighted and averaged to obtain the fused target pose; or the Kalman filter algorithm can be used to fuse the two to obtain the fused target. Pose, the Kalman filter algorithm will fuse the matching pose of the traveling device with the estimated pose of the traveling device in proportion through the Kalman prediction equation to obtain the fused target pose, and the proportion of fusion is obtained by Kalman gain. To determine, the Kalman gain is updated by the Kalman update equation according to the prediction result of the Kalman prediction equation, so that the prediction result of the updated Kalman prediction equation is more accurate.
  • the estimated pose of the traveling device after interpolation to the second moment can be used to fuse with the matched pose of the traveling device, or the estimated pose and matching of the traveling device at the first moment can be used directly.
  • the poses of the rear driving equipment are fused.
  • the shortcomings of different pose determination methods can be compensated for each other, and errors caused by various uncertain factors can be reduced. Improve the accuracy of the obtained target pose.
  • the traveling device includes a traveling device based on an embedded platform, for example, a car running in a sandbox and configured for artificial intelligence teaching, an indoor sweeping robot, and the like.
  • the computing performance of a driving device based on an embedded platform is usually low, and the driving device has a high requirement for real-time determination of the pose. Increase the speed of pose determination.
  • the traveling device operates in a sand table
  • the line segment map is a map of the sand table. Due to the relatively monotonous environment of the sand table, in many road sections, the two frames before and after the radar scan are not sufficiently different. Matching between the point clouds of adjacent frames, or matching between adjacent frames, cannot be accurately matched. The pose of the traveling device. However, in the embodiment of the present disclosure, by matching the point cloud of the radar with the line segment of the map, the positioning accuracy can be significantly improved.
  • the car taught by artificial intelligence drives automatically in the sand table, and the line segment map of the sand table can be established in advance through the site design drawing of the sand table. Roads, buildings, obstacles, etc. in the sand table are represented by line segments in the line segment map.
  • the first estimated pose S 1 in the line segment map of the car can be set manually, and according to the sensor data collected at each first moment of the initial cycle, combined with the estimated pose S 1 , determine the The estimated poses S 2 , S 3 ??S n of the car at each subsequent first moment (n is the number at the first moment), by interpolating S 1 ?? S n , and according to the relative relationship between the radar and the car The pose is transformed to obtain the estimated pose P 1 ?? P n of the radar at each second moment.
  • the target line segments that meet the first preset conditions are screened from the line segment map, and the radar scanning points are determined according to the estimated poses P 1 ?? P n of the radar
  • the coordinates of the cloud in the line segment map translate and rotate the coordinates of the point cloud based on the transformation matrix, until a line segment that basically coincides with the point cloud is found in the target line segment, determine the transformation matrix at this time as the target transformation matrix, and use the target
  • the transformation matrix translates and rotates the estimated pose P 1 ?? P n of the radar to obtain the matched radar pose P' 1 ......
  • the above method may be performed by a pose determination module on the traveling device, and may also be implemented by a processor invoking computer-readable instructions stored in a memory.
  • the present disclosure also provides a pose determination device, an electronic device, a computer-readable storage medium, and a program, all of which can be used to implement any one of the pose determination methods provided by the present disclosure. Record accordingly.
  • FIG. 4 shows a block diagram of a position determination apparatus according to an embodiment of the present disclosure.
  • the apparatus 40 includes:
  • the estimated pose determination part 401 is configured to determine the estimated pose of the radar of the traveling device in the line segment map according to the estimated pose of the traveling device in the line segment map, the things in the line segment map represented by line segments;
  • the radar pose determination part 402 is configured to match the point cloud collected by the radar with the line segments in the line segment map according to the estimated pose of the radar, and determine that the radar is in the line segment after matching according to the matching result. the pose in the line segment map;
  • the target pose determination part 403 is configured to determine the target pose of the traveling device in the line segment map according to the matched pose of the radar.
  • the estimated pose determination part 401 is further configured to use the estimated poses of the traveling device at a plurality of first moments to obtain the traveling device at the first time through interpolation processing.
  • the estimated pose at the second time, the second time includes the time when the radar collects the point cloud; according to the estimated pose of the driving device at the second time, the radar is obtained at the second time.
  • the estimated pose at the second moment is further configured to use the estimated poses of the traveling device at a plurality of first moments to obtain the traveling device at the first time through interpolation processing.
  • the estimated pose at the second time, the second time includes the time when the radar collects the point cloud; according to the estimated pose of the driving device at the second time, the radar is obtained at the second time.
  • the estimated pose at the second moment is further configured to use the estimated poses of the traveling device at a plurality of first moments to obtain the traveling device at the first time through interpolation processing.
  • the apparatus further includes:
  • a target line segment determination part configured to determine a target line segment in the line segment map that satisfies a first preset condition according to the orientation in the estimated pose of the traveling device
  • the radar pose determination part 402 is further configured to match the point cloud collected by the radar with the target line segment.
  • the first preset condition includes:
  • the absolute value of the included angle between the line segment and the facing direction is less than the first threshold
  • the distance between the line segment and the first line segment is less than a distance threshold, and the first line segment is a line segment whose absolute value of the included angle with the orientation is less than a second threshold value.
  • the radar pose determination part 402 is further configured to determine the coordinates of the point cloud scanned by the radar in the line segment map according to the estimated pose of the radar;
  • the coordinates of the point cloud in the line segment map and the line segments in the line segment map determine a target transformation matrix, so that the point cloud at the coordinates is translated and rotated by the target transformation matrix and is the same as the line segment in the line segment map.
  • Line segment matching; through the target transformation matrix, the estimated pose of the radar is translated and rotated to obtain the pose of the radar in the line segment map.
  • the target pose determination part 403 is further configured to determine the pose of the traveling device after matching according to the pose of the radar after matching; The pose of the vehicle is fused with the estimated pose of the traveling device to obtain the target pose of the traveling device in the line segment map.
  • the apparatus further includes:
  • the current estimated pose determination part is configured to, according to the target pose of the traveling device determined in the previous cycle and the sensing data collected by the traveling device in the current cycle, determine the position of the traveling device in the line segment map.
  • the estimated pose of the traveling device in the current cycle is obtained, and the sensing data is collected based on the motion information of the traveling device.
  • the line segment map is obtained by performing straight line fitting on an initial map, where the initial map includes at least one of an occupation grid map and a site design drawing.
  • the traveling device includes a traveling device based on an embedded platform, the traveling device runs in a sand table, and the line segment map is a map of the sand table.
  • the functions or modules included in the apparatus provided in the embodiments of the present disclosure may be configured to execute the methods described in the above method embodiments, and for implementation, reference may be made to the above method embodiments.
  • a "part" may be a part of a circuit, a part of a processor, a part of a program or software, etc., of course, a unit, a module or a non-modularity.
  • Embodiments of the present disclosure further provide a computer-readable storage medium, on which computer program instructions are stored, and when the computer program instructions are executed by a processor, the foregoing method is implemented.
  • Computer-readable storage media can be volatile or non-volatile computer-readable storage media.
  • An embodiment of the present disclosure further provides an electronic device, comprising: a processor and a memory configured to store instructions executable by the processor; wherein the processor is configured to invoke the instructions stored in the memory to execute the above method.
  • Embodiments of the present disclosure also provide a computer program product, including computer-readable codes.
  • a processor in the device executes the pose for implementing the pose provided by any of the above embodiments.
  • a directive to determine the method is also provided.
  • Embodiments of the present disclosure further provide another computer program product configured to store computer-readable instructions, which, when executed, cause the computer to perform the operations of the pose determination method provided by any one of the foregoing embodiments.
  • An embodiment of the present disclosure further provides a computer program, which, when executed by a processor, implements the pose determination method provided by the embodiment of the present disclosure.
  • the electronic device may be provided as a terminal, server or other form of device.
  • FIG. 5 shows a block diagram of an electronic device 800 according to an embodiment of the present disclosure.
  • electronic device 800 may be a mobile phone, computer, digital broadcast terminal, messaging device, game console, tablet device, medical device, fitness device, personal digital assistant, etc. terminal.
  • electronic device 800 may include one or more of the following components: processing component 802, memory 804, power supply component 806, multimedia component 808, audio component 810, input/output (I/O) interface 812, sensor component 814 , and the communication component 816 .
  • the processing component 802 generally controls the overall operation of the electronic device 800, such as operations associated with display, phone calls, data communications, camera operations, and recording operations.
  • the processing component 802 can include one or more processors 820 to execute instructions to perform all or some of the steps of the methods described above.
  • processing component 802 may include one or more modules that facilitate interaction between processing component 802 and other components.
  • processing component 802 may include a multimedia module to facilitate interaction between multimedia component 808 and processing component 802.
  • Memory 804 is configured to store various types of data to support operation at electronic device 800 . Examples of such data include instructions for any application or method operating on electronic device 800, contact data, phonebook data, messages, pictures, videos, and the like. Memory 804 may be implemented by any type of volatile or nonvolatile storage device or combination thereof, such as static random access memory (SRAM), electrically erasable programmable read only memory (EEPROM), erasable Programmable Read Only Memory (EPROM), Programmable Read Only Memory (PROM), Read Only Memory (ROM), Magnetic Memory, Flash Memory, Magnetic or Optical Disk.
  • SRAM static random access memory
  • EEPROM electrically erasable programmable read only memory
  • EPROM erasable Programmable Read Only Memory
  • PROM Programmable Read Only Memory
  • ROM Read Only Memory
  • Magnetic Memory Flash Memory
  • Magnetic or Optical Disk Magnetic Disk
  • Power supply assembly 806 provides power to various components of electronic device 800 .
  • Power supply components 806 may include a power management system, one or more power supplies, and other components associated with generating, managing, and distributing power to electronic device 800 .
  • Multimedia component 808 includes a screen that provides an output interface between the electronic device 800 and the user.
  • the screen may include a liquid crystal display (LCD) and a touch panel (TP). If the screen includes a touch panel, the screen may be implemented as a touch screen to receive input signals from a user.
  • the touch panel includes one or more touch sensors to sense touch, swipe, and gestures on the touch panel. The touch sensor may not only sense the boundaries of a touch or swipe action, but also detect the duration and pressure associated with the touch or swipe action.
  • the multimedia component 808 includes a front-facing camera and/or a rear-facing camera. When the electronic device 800 is in an operation mode, such as a shooting mode or a video mode, the front camera and/or the rear camera may receive external multimedia data. Each of the front and rear cameras can be a fixed optical lens system or have focal length and optical zoom capability.
  • Audio component 810 is configured to output and/or input audio signals.
  • audio component 810 includes a microphone (MIC) that is configured to receive external audio signals when electronic device 800 is in operating modes, such as calling mode, recording mode, and voice recognition mode.
  • the received audio signal may be further stored in memory 804 or transmitted via communication component 816 .
  • audio component 810 also includes a speaker for outputting audio signals.
  • the I/O interface 812 provides an interface between the processing component 802 and a peripheral interface module, which may be a keyboard, a click wheel, a button, or the like. These buttons may include, but are not limited to: home button, volume buttons, start button, and lock button.
  • Sensor assembly 814 includes one or more sensors for providing status assessment of various aspects of electronic device 800 .
  • the sensor assembly 814 can detect the on/off state of the electronic device 800, the relative positioning of the components, such as the display and the keypad of the electronic device 800, the sensor assembly 814 can also detect the electronic device 800 or one of the electronic device 800 Changes in the position of components, presence or absence of user contact with the electronic device 800 , orientation or acceleration/deceleration of the electronic device 800 and changes in the temperature of the electronic device 800 .
  • Sensor assembly 814 may include a proximity sensor configured to detect the presence of nearby objects in the absence of any physical contact.
  • Sensor assembly 814 may also include a light sensor, such as a CMOS or CCD image sensor, for use in imaging applications.
  • the sensor assembly 814 may also include an acceleration sensor, a gyroscope sensor, a magnetic sensor, a pressure sensor, or a temperature sensor.
  • Communication component 816 is configured to facilitate wired or wireless communication between electronic device 800 and other devices.
  • Electronic device 800 may access wireless networks based on communication standards, such as WiFi, 2G or 3G, or a combination thereof.
  • the communication component 816 receives broadcast signals or broadcast related information from an external broadcast management system via a broadcast channel.
  • the communication component 816 also includes a near field communication (NFC) module to facilitate short-range communication.
  • NFC near field communication
  • the NFC module may be implemented based on radio frequency identification (RFID) technology, infrared data association (IrDA) technology, ultra-wideband (UWB) technology, Bluetooth (BT) technology and other technologies.
  • RFID radio frequency identification
  • IrDA infrared data association
  • UWB ultra-wideband
  • Bluetooth Bluetooth
  • electronic device 800 may be implemented by one or more application specific integrated circuits (ASICs), digital signal processors (DSPs), digital signal processing devices (DSPDs), programmable logic devices (PLDs), field programmable A programmed gate array (FPGA), controller, microcontroller, microprocessor or other electronic component implementation is used to perform the above method.
  • ASICs application specific integrated circuits
  • DSPs digital signal processors
  • DSPDs digital signal processing devices
  • PLDs programmable logic devices
  • FPGA field programmable A programmed gate array
  • controller microcontroller, microprocessor or other electronic component implementation is used to perform the above method.
  • a non-volatile computer-readable storage medium such as a memory 804 comprising computer program instructions executable by the processor 820 of the electronic device 800 to perform the above method is also provided.
  • FIG. 6 shows a block diagram of an electronic device 1900 according to an embodiment of the present disclosure.
  • the electronic device 1900 may be provided as a server.
  • electronic device 1900 includes processing component 1922, which further includes one or more processors, and a memory resource represented by memory 1932 configured to store instructions executable by processing component 1922, such as an application program.
  • An application program stored in memory 1932 may include one or more modules, each corresponding to a set of instructions.
  • the processing component 1922 is configured to execute instructions to perform the above-described methods.
  • the electronic device 1900 may also include a power supply assembly 1926 configured to perform power management of the electronic device 1900, a wired or wireless network interface 1950 configured to connect the electronic device 1900 to a network, and an input output (I/O) interface 1958 .
  • Electronic device 1900 may operate based on an operating system stored in memory 1932, such as Windows Server TM , Mac OS X TM , Unix TM , Linux TM , FreeBSD TM or the like.
  • a non-volatile computer-readable storage medium such as memory 1932 comprising computer program instructions executable by processing component 1922 of electronic device 1900 to perform the above-described method.
  • the present disclosure may be a system, method and/or computer program product.
  • the computer program product may include a computer-readable storage medium having computer-readable program instructions (computer program) loaded thereon for causing a processor to implement various aspects of the embodiments of the present disclosure.
  • a computer-readable storage medium may be a tangible device that can hold and store instructions for use by the instruction execution device, and may be a volatile storage medium or a non-volatile storage medium.
  • the computer-readable storage medium may be, for example, but is not limited to, an electrical storage device, a magnetic storage device, an optical storage device, an electromagnetic storage device, a semiconductor storage device, or any suitable combination of the foregoing.
  • Non-exhaustive list of computer readable storage media include: portable computer disks, hard disks, random access memory (RAM), read only memory (ROM), erasable programmable read only memory (EPROM) or flash memory), static random access memory (SRAM), portable compact disk read only memory (CD-ROM), digital versatile disk (DVD), memory sticks, floppy disks, mechanically coded devices, such as printers with instructions stored thereon Hole cards or raised structures in grooves, and any suitable combination of the above.
  • RAM random access memory
  • ROM read only memory
  • EPROM erasable programmable read only memory
  • flash memory static random access memory
  • SRAM static random access memory
  • CD-ROM compact disk read only memory
  • DVD digital versatile disk
  • memory sticks floppy disks
  • mechanically coded devices such as printers with instructions stored thereon Hole cards or raised structures in grooves, and any suitable combination of the above.
  • Computer-readable storage media are not to be construed as transient signals per se, such as radio waves or other freely propagating electromagnetic waves, electromagnetic waves propagating through waveguides or other transmission media (eg, light pulses through fiber optic cables), or through electrical wires transmitted electrical signals.
  • the computer readable program instructions described herein may be downloaded to various computing/processing devices from a computer readable storage medium, or to an external computer or external storage device over a network such as the Internet, a local area network, a wide area network, and/or a wireless network.
  • the network may include copper transmission cables, fiber optic transmission, wireless transmission, routers, firewalls, switches, gateway computers, and/or edge servers.
  • a network adapter card or network interface in each computing/processing device receives computer-readable program instructions from a network and forwards the computer-readable program instructions for storage in a computer-readable storage medium in each computing/processing device .
  • Computer program instructions for carrying out operations of the present disclosure may be assembly instructions, instruction set architecture (ISA) instructions, machine instructions, machine-dependent instructions, microcode, firmware instructions, state setting data, or instructions in one or more programming languages.
  • Source or object code written in any combination, including object-oriented programming languages, such as Smalltalk, C++, etc., and conventional procedural programming languages, such as the "C" language or similar programming languages.
  • the computer readable program instructions may execute entirely on the user's computer, partly on the user's computer, as a stand-alone software package, partly on the user's computer and partly on a remote computer, or entirely on the remote computer or server implement.
  • the remote computer may be connected to the user's computer through any kind of network, including a local area network (LAN) or a wide area network (WAN), or may be connected to an external computer (eg, using an Internet service provider through the Internet connect).
  • LAN local area network
  • WAN wide area network
  • custom electronic circuits such as programmable logic circuits, field programmable gate arrays (FPGAs), or programmable logic arrays (PLAs) can be personalized by utilizing state information of computer readable program instructions.
  • Computer readable program instructions are executed to implement various aspects of the present disclosure.
  • These computer readable program instructions may be provided to a processor of a general purpose computer, special purpose computer or other programmable data processing apparatus to produce a machine that causes the instructions when executed by the processor of the computer or other programmable data processing apparatus , resulting in means for implementing the functions/acts specified in one or more blocks of the flowchart and/or block diagrams.
  • These computer readable program instructions can also be stored in a computer readable storage medium, these instructions cause a computer, programmable data processing apparatus and/or other equipment to operate in a specific manner, so that the computer readable medium on which the instructions are stored includes An article of manufacture comprising instructions for implementing various aspects of the functions/acts specified in one or more blocks of the flowchart and/or block diagrams.
  • Computer readable program instructions can also be loaded onto a computer, other programmable data processing apparatus, or other equipment to cause a series of operational steps to be performed on the computer, other programmable data processing apparatus, or other equipment to produce a computer-implemented process , thereby causing instructions executing on a computer, other programmable data processing apparatus, or other device to implement the functions/acts specified in one or more blocks of the flowcharts and/or block diagrams.
  • each block in the flowchart or block diagrams may represent a module, segment, or portion of instructions, which comprises one or more functions for implementing the specified logical function(s) executable instructions.
  • the functions noted in the blocks may occur out of the order noted in the figures. For example, two blocks in succession may, in fact, be executed substantially concurrently, or the blocks may sometimes be executed in the reverse order, depending upon the functionality involved.
  • each block of the block diagrams and/or flowchart illustrations, and combinations of blocks in the block diagrams and/or flowchart illustrations can be implemented in dedicated hardware-based systems that perform the specified functions or actions , or can be implemented in a combination of dedicated hardware and computer instructions.
  • the computer program product can be specifically implemented by hardware, software or a combination thereof.
  • the computer program product is embodied as a computer storage medium, and in another optional embodiment, the computer program product is embodied as a software product, such as a software development kit (Software Development Kit, SDK), etc. Wait.
  • a software development kit Software Development Kit, SDK
  • Embodiments of the present disclosure relate to a method and device for determining a pose, an electronic device, and a computer-readable storage medium.
  • the method includes: determining, according to an estimated pose of the traveling device in a line segment map, where the radar of the traveling device is located.
  • the estimated pose in the line segment map, the things in the line segment map are represented by line segments; according to the estimated pose of the radar, the point cloud collected by the radar is matched with the line segments in the line segment map, and according to the estimated pose of the radar
  • the matching result determines the pose of the radar in the line segment map after matching; and determines the target pose of the traveling device in the line segment map according to the pose of the radar after the matching.
  • the embodiments of the present disclosure can reduce the calculation amount of the pose determination process, and improve the efficiency of the pose determination of the traveling equipment.

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Electromagnetism (AREA)
  • Robotics (AREA)
  • Navigation (AREA)
  • Traffic Control Systems (AREA)

Abstract

一种位姿确定方法及装置、电子设备和计算机可读存储介质，该方法包括：根据行驶设备在线段地图中的预估位姿，确定行驶设备的雷达在线段地图中的预估位姿，线段地图中的事物通过线段表示（S11）；根据雷达的预估位姿，对雷达采集的点云与线段地图中的线段进行匹配，并根据匹配结果确定匹配后雷达在线段地图中的位姿（S12）；根据匹配后雷达的位姿，确定行驶设备在线段地图中的目标位姿（S13）。

Description

一种位姿确定方法及装置、电子设备和计算机可读存储介质
相关申请的交叉引用
本申请基于申请号为202011362399.X、申请日为2020年11月27日的中国专利申请提出，并要求该中国专利申请的优先权，该中国专利申请的全部内容在此引入本申请作为参考。
技术领域
本公开涉及计算机技术领域，尤其涉及一种位姿确定方法及装置、电子设备和计算机可读存储介质。
背景技术
目前，基于雷达的位姿估计技术已经取得了广泛应用，例如应用于智能车的自动驾驶或辅助驾驶中，通过雷达来估计智能车的位置和朝向，能够帮助智能车合理规划下一步的驾驶路线。
发明内容
本公开实施例提出了一种位姿确定的技术方案。
根据本公开实施例的一方面，提供了一种位姿确定方法，包括：
根据行驶设备在线段地图中的预估位姿，确定所述行驶设备的雷达在所述线段地图中的预估位姿，所述线段地图中的事物通过线段表示；
根据所述雷达的预估位姿，对所述雷达采集的点云与所述线段地图中的线段进行匹配，并根据匹配结果确定匹配后所述雷达在所述线段地图中的位姿；
根据匹配后所述雷达的位姿，确定所述行驶设备在所述线段地图中的目标位姿。
在一种可能的实现方式中，所述根据行驶设备在线段地图中的预估位姿，确定所述行驶设备的雷达在所述线段地图中的预估位姿，包括：
利用所述行驶设备在多个第一时刻的预估位姿，通过插值处理得到所述行驶设备在第二时刻的预估位姿，所述第二时刻包括所述雷达采集所述点云的时刻；
根据所述行驶设备在所述第二时刻的预估位姿，得到所述雷达在所述第二时刻的预估位姿。
在一种可能的实现方式中，在所述对所述雷达采集的点云与所述线段地图中的线段进行匹配前，还包括：
根据所述行驶设备的预估位姿中的朝向，确定所述线段地图中满足第一预设条件的目标线段；
对所述雷达采集的点云与所述线段地图中的线段进行匹配，包括：
对所述雷达采集的点云与所述目标线段进行匹配。
在一种可能的实现方式中，所述第一预设条件，包括：
线段与所述朝向的夹角的绝对值小于第一阈值；
线段与第一线段的距离小于距离阈值，所述第一线段为与所述朝向的夹角的绝对值小于第二阈值的线段。
在一种可能的实现方式中，所述根据雷达的预估位姿，对所述雷达采集的点云与所述线段地图中的线段进行匹配，并根据匹配结果确定匹配后所述雷达在所述线段地图中的位姿，包括：
根据所述雷达的预估位姿，确定所述雷达扫描的点云在所述线段地图中的坐标；
根据所述点云在所述线段地图中的坐标和所述线段地图中的线段，确定目标转换矩阵，其中，所述坐标处的点云经所述目标转换矩阵平移旋转后与所述线段地图中的线段匹配；
通过所述目标转换矩阵，对所述雷达的预估位姿进行平移旋转，得到所述雷达在所述线段地图中的位姿。
在一种可能的实现方式中，所述根据匹配后所述雷达的位姿，确定所述行驶设备在所述线段地图中的目标位姿，包括：
根据匹配后所述雷达的位姿，确定匹配后所述行驶设备的位姿；
将匹配后所述行驶设备的位姿与所述行驶设备的预估位姿进行融合，得到所述行驶设备在所述线段地图中的目标位姿。
在一种可能的实现方式中，所述响应于所述位姿确定方法周期性执行的情况下，在所述确定所述行驶设备的雷达在所述线段地图中的预估位姿前，还包括：
根据上一周期确定的所述行驶设备的目标位姿，以及当前周期所述行驶设备采集的传感数据，对所述行驶设备在线段地图中的位姿进行预估，得到当前周期所述行驶设备的预估位姿，所述传感数据为基于所述行驶设备的运动信息采集的。
在一种可能的实现方式中，所述线段地图是通过对初始地图进行直线拟合得到的，所述初始地图包括：占据栅格地图和场地设计图中的至少一个。
在一种可能的实现方式中，所述行驶设备包括基于嵌入式平台的行驶设备，所述行驶设备运行于沙盘中，所述线段地图为所述沙盘的地图。
根据本公开实施例的一方面，提供了一种位姿确定装置，包括：
预估位姿确定部分，被配置为根据行驶设备在线段地图中的预估位姿，确定所述行驶设备的雷达在所述线段地图中的预估位姿，所述线段地图中的事物通过线段表示；
雷达位姿确定部分，被配置为根据所述雷达的预估位姿，对所述雷达采集的点云与所述线段地图中的线段进行匹配，并根据匹配结果确定匹配后所述雷达在所述线段地图中的位姿；
目标位姿确定部分，被配置为根据匹配后所述雷达的位姿，确定所述行 驶设备在所述线段地图中的目标位姿。
在一种可能的实现方式中，所述预估位姿确定部分，还被配置为利用所述行驶设备在多个第一时刻的预估位姿，通过插值处理得到所述行驶设备在第二时刻的预估位姿，所述第二时刻包括所述雷达采集所述点云的时刻；根据所述行驶设备在所述第二时刻的预估位姿，得到所述雷达在所述第二时刻的预估位姿。
在一种可能的实现方式中，所述装置还包括：
目标线段确定部分，被配置为根据所述行驶设备的预估位姿中的朝向，确定所述线段地图中满足第一预设条件的目标线段；
所述雷达位姿确定部分，被配置为对所述雷达采集的点云与所述目标线段进行匹配。
在一种可能的实现方式中，所述第一预设条件，包括：
线段与所述朝向的夹角的绝对值小于第一阈值；
线段与第一线段的距离小于距离阈值，所述第一线段为与所述朝向的夹角的绝对值小于第二阈值的线段。
在一种可能的实现方式中，所述雷达位姿确定部分，还被配置为根据所述雷达的预估位姿，确定所述雷达扫描的点云在所述线段地图中的坐标；根据所述点云在所述线段地图中的坐标和所述线段地图中的线段，确定目标转换矩阵，其中，所述坐标处的点云经所述目标转换矩阵平移旋转后与所述线段地图中的线段匹配；通过所述目标转换矩阵，对所述雷达的预估位姿进行平移旋转，得到所述雷达在所述线段地图中的位姿。
在一种可能的实现方式中，所述目标位姿确定部分，还被配置为根据匹配后所述雷达的位姿，确定匹配后所述行驶设备的位姿；将匹配后所述行驶设备的位姿与所述行驶设备的预估位姿进行融合，得到所述行驶设备在所述线段地图中的目标位姿。
在一种可能的实现方式中，所述装置还包括：
当前预估位姿确定部分，被配置为根据上一周期确定的所述行驶设备的目标位姿，以及当前周期所述行驶设备采集的传感数据，对所述行驶设备在线段地图中的位姿进行预估，得到当前周期所述行驶设备的预估位姿，所述传感数据为基于所述行驶设备的运动信息采集的。
在一种可能的实现方式中，所述线段地图是通过对初始地图进行直线拟合得到的，所述初始地图包括：占据栅格地图和场地设计图中的至少一个。
在一种可能的实现方式中，所述行驶设备包括基于嵌入式平台的行驶设备，所述行驶设备运行于沙盘中，所述线段地图为所述沙盘的地图。
根据本公开实施例的一方面，提供了一种电子设备，包括：处理器；被配置为存储处理器可执行指令的存储器；其中，所述处理器被配置为调用所述存储器存储的指令，以执行上述方法。
根据本公开实施例的一方面，提供了一种计算机可读存储介质，其上存储有计算机程序指令，所述计算机程序指令被处理器执行时实现上述方法。
根据本公开实施例的一方面，提供了一种计算机程序，包括计算机可读代码，在所述计算机可读代码在电子设备中运行的情况下，所述电子设备中的处理器执行时实现上述方法。
在本公开实施例中，能够根据行驶设备在线段地图中的预估位姿来确定行驶设备的雷达在线段地图中的预估位姿，通过根据预先估计的预估位姿，来对雷达采集的点云与线段地图中的线段进行匹配，缩小了线段地图中进行匹配的区域的范围，减少了位姿确定过程的计算量。另外，通过用线段简略地表示地图中的事物，在匹配时，是利用点云与线段地图中的线段进行匹配，减少了位姿确定过程的计算量，提高了行驶设备位姿确定的效率。
应当理解的是，以上的一般描述和后文的细节描述仅是示例性和解释性的，而非限制本公开。根据下面参考附图对示例性实施例的详细说明，本公开的其它特征及方面将变得清楚。
附图说明
此处的附图被并入说明书中并构成本说明书的一部分，这些附图示出了符合本公开的实施例，并与说明书一起用于说明本公开的技术方案。
图1示出根据本公开实施例的位姿确定系统的架构示意图；
图2示出根据本公开实施例的位姿确定方法的流程图；
图3A示出根据本公开实施例的一种线段地图中目标线段的示意图；
图3B示出根据本公开实施例的另一种线段地图中目标线段的示意图；
图4示出根据本公开实施例的一种位姿确定装置的框图；
图5示出根据本公开实施例的一种电子设备的框图；
图6示出根据本公开实施例的一种电子设备的框图。
具体实施方式
以下将参考附图详细说明本公开的各种示例性实施例、特征和方面。附图中相同的附图标记表示功能相同或相似的元件。尽管在附图中示出了实施例的各种方面，但是除非特别指出，不必按比例绘制附图。
在这里专用的词“示例性”意为“用作例子、实施例或说明性”。这里作为“示例性”所说明的任何实施例不必解释为优于或好于其它实施例。
本文中术语“和/或”，仅仅是一种描述关联对象的关联关系，表示可以存在三种关系，例如，A和/或B，可以表示：单独存在A，同时存在A和B，单独存在B这三种情况。另外，本文中术语“至少一种”表示多种中的任意一种或多种中的至少两种的任意组合，例如，包括A、B、C中的至少一种，可以表示包括从A、B和C构成的集合中选择的任意一个或多个元素。
另外，为了更好地说明本公开，在下文的具体实施方式中给出了众多的具体细节。本领域技术人员应当理解，没有某些具体细节，本公开同样可以实施。在一些实例中，对于本领域技术人员熟知的方法、手段、元件和电路 未作详细描述，以便于凸显本公开的主旨。
在相关技术中，智能车往往需要及时地得到当前位姿，然而，由于智能车等行驶设备的处理性能往往有限，因此，急需减少位姿估计过程的计算量，以提高位姿估计的速度。
本公开实施例提供的位姿确定方法可以由行驶设备来执行，或者，也可以是由行驶设备将采集的数据发送给其它设备，由其它设备来执行，例如可以由终端设备或服务器等电子设备执行，终端设备可以是用户设备(User Equipment，UE)、移动设备、用户终端、终端、蜂窝电话、无绳电话、个人数字处理(Personal Digital Assistant，PDA)、手持设备、计算设备、车载设备、可穿戴设备等。
示例性的，图1是本公开实施例提供的示例性的位姿确定系统10的架构示意图；如图1所示，位姿确定系统10中包括终端设备/服务器100，以及行驶设备200(图1中示例性地示出了行驶设备200-1)。行驶设备200-1在行驶过程中可以将采集的运动信息或传感数据，以及采集的点云等数据，均发送至终端设备/服务器100；终端设备/服务器100中预先存储有线段地图，行驶设备与雷达的位姿的相对关系，以及行驶设备在线段地图中的预估位姿等数据，终端设备/服务器100根据预先存储的行驶设备200-1在线段地图中的预估位姿，确定行驶设备200-1的雷达在线段地图中的预估位姿，根据行驶设备200-1的雷达的预估位姿，对雷达采集的点云与线段地图中的线段进行匹配，并根据匹配结果确定匹配后雷达在线段地图中的位姿，并根据匹配后雷达的位姿，最终确定出行驶设备200-1在线段地图中的目标位姿，并将行驶设备200-1在线段地图中的目标位姿发送至行驶设备200-1。
这里，通过由行驶设备将采集的数据发送给终端设备或服务器等其它设备，由其它设备来确定行驶设备200-1的目标位姿，可以减少行驶设备的计算量。
对于位姿确定方法应用于行驶设备中的情况，以下将进行详细说明。
图2示出根据本公开实施例的位姿确定方法的流程图，如图2所示，所述位姿确定方法包括：
在步骤S11中，根据行驶设备在线段地图中的预估位姿，确定所述行驶设备的雷达在所述线段地图中的预估位姿。
线段地图中的事物通过线段进行表示，例如通过线段来表示道路和建筑物等等，线段地图的实现方式可以有很多种，后文会结合本公开可能的实现方式，对线段地图做详细描述。
这里的行驶设备可以是地理位置可变更的设备，地理位置的变更可以是行驶设备自主变更，也可以是由外力导致的变更。行驶设备可以是无人驾驶领域、人工智能教育领域的行驶设备，示例性地，行驶设备例如可以是车辆(例如，嵌入式自动驾驶小车)、船舶、飞行器等设备，或者也可以是机器人，例如扫地机器人、搬运机器人和教育机器人等等。
位姿可以包括位置和姿态，位置可以是设备在地图中的坐标，姿态可以 是设备在地图中的方向。
行驶设备的预估位姿可以是预先估计的位姿，可以是根据行驶设备目标时刻确定的位姿进行估计得到的，后文会结合本公开实施例可能的实现方式对行驶设备的预估位姿做详细描述。
行驶设备的雷达在所述线段地图中的预估位姿，可以是根据行驶设备与雷达的位姿的相对关系，由行驶设备的预估位姿来确定的。在雷达底座在行驶设备上固定的情况下，该相对关系往往是固定不变的，因此，该相对关系可以预先由人工确定，即该相对关系为预设值。
在步骤S12中，根据所述雷达的预估位姿，对所述雷达采集的点云与所述线段地图中的线段进行匹配，并根据匹配结果确定匹配后所述雷达在所述线段地图中的位姿。
由于雷达采集的点云是位于雷达的极坐标系中的，该坐标系例如是以雷达为中心点的极坐标系，雷达对周围事物进行扫描得到的点云位于极坐标系中的中心点周围。
在得到了雷达在线段地图中的预估位姿后，即确定了雷达在世界坐标系中的估计位置和姿态，因此，可以根据雷达在世界坐标系中的预估位置和姿态，将极坐标系中的点云映射到世界坐标系中，得到雷达扫描的点云在世界坐标系中的位置和姿态。即根据雷达在线段地图中的预估位姿，和雷达扫描的点云在雷达极坐标系中的位姿，得到雷达扫描的点云在线段地图中的位姿。
在得到雷达扫描的点云在线段地图中的位姿后，由于该位姿为预先估计的位姿，即预先估计的雷达扫描的点云在线段地图中的大体位置和方向，因此，可以根据雷达扫描的点云在线段地图中的大体位置和方向，对雷达采集的点云与线段地图中的线段进行匹配，而不用对整个线段地图中的所有线段进行匹配，可以减少匹配时线段的数量，提高效率。
这里所说的“匹配”，可理解为在线段地图中，寻找与点云所表示的事物相对应的线段，在一个示例中，可以通过对点云进行平移旋转，去寻找尽可能与点云重合的线段，一旦找到与点云的重合程度达到预期的线段，则视为匹配完成。
在点云与所述线段地图中的线段进行匹配的过程中，会对点云进行移动和旋转，在线段地图中寻找与点云尽可能地重合的线段。由于点云是雷达对周围事物扫描得到的点的相对位置信息，而线段地图中的线段表征的也是事物的位置信息，因此，在点云与线段匹配时，可以根据点云在线段地图中的位置反推雷达的位姿，反推出来的位姿即可认为是扫描点云时雷达的位姿。
后文会结合本公开实施例可能的实现方式对匹配的过程进行详细描述。
在步骤S13中，根据匹配后所述雷达的位姿，确定所述行驶设备在所述线段地图中的目标位姿。
在雷达的位姿确定后，可以根据行驶设备与雷达的位姿的相对关系，来确定行驶设备在线段地图中的位姿，为便于后续描述，这里将该位姿称为目标位姿。另外，还可以根据其它因素来确定目标位姿，后文会结合本公开实 施例可能的实现方式进行详细描述。
在本公开实施例中，能够根据行驶设备在线段地图中的预估位姿来确定行驶设备的雷达在线段地图中的预估位姿，通过根据预先估计的预估位姿，来对雷达采集的点云与线段地图中的线段进行匹配，缩小了线段地图中进行匹配的区域的范围，减少了位姿确定过程的计算量。另外，通过用线段简略地表示地图中的事物，在匹配时，是利用点云与线段地图中的线段进行匹配，减少了位姿确定过程的计算量，提高了行驶设备位姿确定的效率。
本公开实施例提供的位姿确定方法还可以有多种实现方式，如前文所述，本公开实施例中，通过线段地图进行点线匹配来确定行驶设备的位姿，在一种可能的实现方式中，所述线段地图是通过对初始地图进行直线拟合得到的，所述初始地图包括：占据栅格地图和场地设计图中的至少一个。
占据栅格地图的构建方式可以有多种，例如，可以通过即时定位与建图算法(GMapping)对行驶设备的行驶场地进行建图，得到行驶场地的占据栅格地图。
对初始地图进行直线拟合的方式可以有多种，例如，基于最小二乘法的直线拟合、基于梯度下降法的直线拟合。
本公开实施例中，通过直接对初始图像进行直线拟合，即可得到线段地图，提高了绘制线段地图的效率。另外，直接基于场地设计图来确定线段地图，由于场地设计图往往是较为简单的线条图，因此，更加适用于直线拟合算法，得到的线段地图的准确性更高，且无需通过其它算法对初始地图进行构建，便利性较高。
另外，得到的线段地图相对于其它地图，是非常轻量级的地图，能够提高位姿估计的速度。
如前文所述，行驶设备的预估位姿可以是根据行驶设备目标时刻确定的位姿进行估计得到的，在一种可能的实现方式中，在本公开实施例的位姿确定方法周期性执行的情况下，目标时刻的位姿可以是上一周期确定的目标位姿，另外，目标时刻的位姿也可以是通过其它方式确定的位姿，例如通过全球定位系统等位置传感器确定的位姿。
在一种可能的实现方式中，在所述位姿确定方法周期性执行的情况下，在所述确定所述行驶设备的雷达在所述线段地图中的预估位姿前，还包括：根据上一周期确定的所述行驶设备的目标位姿，以及当前周期所述行驶设备采集的传感数据，对所述行驶设备在线段地图中的位姿进行预估，得到当前周期所述行驶设备的预估位姿，所述传感数据为基于所述行驶设备的运动信息采集的。
行驶设备的运动信息可包括姿态(yaw)角、角速度、加速度、轮速等信息中的至少一种，传感数据可以是惯性测量部分(Inertial measurement unit，IMU)或轮速计等传感器采集的数据。
在对行驶设备在线段地图中的位姿进行预估的过程中，可以基于上一周期确定的行驶设备的目标位姿，对当前周期前端IMU采集的数据进行积分融 合，并对轮速计采集的轮速进行积分融合，得到当前周期行驶设备的预估位姿；或者，可以基于上一周期确定的行驶设备的目标位姿，对传感器采集的传感数据进行卡尔曼滤波融合，得到当前周期行驶设备的预估位姿。
在通过积分融合的方式得到行驶设备的预估位姿的过程中，会对IMU采集到的角速度在时间上进行积分，既能够得到行驶设备的角度的变化量；通过对轮速计采集的轮速进行积分，既能够得到行驶设备位移的变化量。那么，基于上一周期确定的行驶设备的目标位姿，再加上得到的角度的变化量和位移的变化量，既能够得到当前周期行驶设备的预估位姿。
在通过卡尔曼滤波融合得到行驶设备的预估位姿的过程中，会通过卡尔曼预测方程，基于上一周期确定的行驶设备的目标位姿，将不同传感数据得到的角度和位移分别按比例进行融合，得到当前周期行驶设备的预估位姿，融合的比例由卡尔曼增益(Kalman Gain，Kg)来确定，Kg由卡尔曼更新方程根据卡尔曼预测方程的预测结果来更新，使得更新后的卡尔曼预测方程的预测结果更准确。
对于起始周期确定预估位姿过程中所使用的目标位姿，可以是由人工设定的，或者是根据人工设定的其它参数进行计算得到的。
由于行驶设备可能在不停地运动，因此，可以在当前周期内按预设频率确定预估位姿，得到当前周期内多个第一时刻的预估位姿，例如，可以按1秒输出100次的频率来输出预估位姿。
本公开实施例中，通过上一周期确定的目标位姿，以及当前周期行驶设备采集的传感数据，对行驶设备在线段地图中的位姿进行预估，能够准确地预估行驶设备的位姿，方便后续进行点线匹配，提高了确定的位姿的准确性。
在确定行驶设备的预估位姿后，即可对雷达的预估位姿进行确定。在一种可能的实现方式中，所述根据行驶设备在线段地图中的预估位姿，确定所述行驶设备的雷达在所述线段地图中的预估位姿，包括：利用所述行驶设备在多个第一时刻的预估位姿，通过插值处理得到所述行驶设备在第二时刻的预估位姿，所述第二时刻包括所述雷达采集所述点云的时刻；根据所述行驶设备在所述第二时刻的预估位姿，得到所述雷达在所述第二时刻的预估位姿。
雷达是按一定频率扫描周围事物得到点云，为方便描述，这里将雷达采集点云的时刻称为第二时刻，第二时刻可以是有多个的。由于前端确定行驶设备预估位姿的第一时刻可能与第二时刻不同，因此，可以利用行驶设备在多个第一时刻的预估位姿，通过插值处理得到行驶设备在第二时刻的预估位姿。
在插值的过程中，会通过内插的方式，对多个第一时刻的数据范围内的预估位姿进行插值处理，得到行驶设备在第二时刻的预估位姿；此外，还可以通过外插的方式，对多个第一时刻的数据范围以外的预估位姿进行插值处理，得到行驶设备在第二时刻的预估位姿。
插值得到的第二时刻的预估位姿是行驶设备的预估位姿，因此，可以根据雷达和行驶设备位姿的相对关系，由插值得到的第二时刻的预估位姿，得 到雷达在第二时刻的预估位姿。
在本公开实施例中，通过对行驶设备的预估位姿进行插值的方式，准确地得到了雷达采集点云的时刻的预估位姿，方便后续进行点线匹配，提高了确定的位姿的准确性。
在一种可能的实现方式中，在所述对所述雷达采集的点云与所述线段地图中的线段进行匹配前，还包括：根据所述行驶设备的预估位姿中的朝向，确定所述线段地图中满足第一预设条件的目标线段；对所述雷达采集的点云与所述线段地图中的线段进行匹配，包括：对所述雷达采集的点云与所述目标线段进行匹配。
行驶设备在轨道中行驶的过程中，往往是沿着轨道行驶，因此行驶设备的朝向和所处的轨道两侧往往是平行或接近于平行的状态。而线段地图中的事物也是通过线段来表示的，因此，可以根据行驶设备的朝向来筛选进行匹配的线段，可以通过设置条件来筛选线段，选择与行驶设备的朝向相符合的线段。
在一种可能的实现方式中，所述第一预设条件，包括：
线段与所述朝向的夹角的绝对值小于第一阈值；
线段与第一线段的距离小于距离阈值，所述第一线段为与所述朝向的夹角的绝对值小于第二阈值的线段。
第一阈值例如可以是30°，这样可以选取行驶设备朝向±30°以内的线段，如图3A所示，对于环形轨道的线段地图，行驶设备朝向如图3A所示，与行驶设备朝向的夹角的绝对值小于±30°以内的线段集合a如图3A所示。
在确定与所示朝向的夹角的绝对值小于第一阈值的线段的情况下，对于环形的小车轨道，按逆时针或顺时针的顺序，依次确定构成线段地图中的每一条线段在坐标系中的角度；然后再确定坐标系中小车朝向的角度，根据小车朝向的角度，确定与小车朝向的夹角的绝对值小于第一阈值的线段。
第二阈值小于第一阈值，例如可以是3°或5°等值，这样第一线段即为与行驶设备的朝向平行或近似平行的线段。在确定出第一线段后，再选取出与第一线段的距离小于距离阈值的线段，作为后续匹配的目标线段。距离阈值可以是根据经验设置的经验值，本公开实施例对取值不作限定。
如图3B所示，对于环形轨道的线段地图，行驶设备朝向如图3B所示，图3B中示出了与行驶设备的朝向近乎平行的第一线段，以及与第一线段的距离小于距离阈值的线段集合b。
本公开实施例的实现方式中，确定线段之间的距离的方式可以有很多种，例如，线段的距离可以是两个线段的中点之间的距离，或者可以是两个线段上所有点的距离集合中的最小值。对确定线段之间的距离的方式，本公开实施例对此不作限定。
本公开实施例中，通过根据所述行驶设备的预估位姿中的朝向，确定所述线段地图中满足第一预设条件的目标线段，可以筛选掉不必要的局外匹配，可以减少后续进行匹配的线段的数量，减少了匹配过程中的计算量，提高了 位姿确定的效率。
在一种可能的实现方式中，根据所述雷达的预估位姿，对所述雷达采集的点云与所述线段地图中的线段进行匹配，并根据匹配结果确定匹配后所述雷达在所述线段地图中的位姿，包括：根据所述雷达的预估位姿，确定所述雷达扫描的点云在所述线段地图中的坐标；根据所述点云在所述线段地图中的坐标和所述线段地图中的线段，确定目标转换矩阵，其中，所述坐标处的点云经所述目标转换矩阵平移旋转后与所述线段地图中的线段匹配；通过所述目标转换矩阵，对所述雷达的预估位姿进行平移旋转，得到所述雷达在所述线段地图中的位姿。
点云与所述线段地图中的线段进行匹配的过程中，可在线段地图中寻找可以与点云尽可能地重合或者完全重合的线段，当找到与点云尽可能地重合或完全重合的线段后，即实现了点云与线段的匹配。
在匹配的过程中，可根据雷达在线段地图(世界坐标系)中的预估位姿，和雷达扫描的点云在雷达极坐标系中的位姿，得到雷达扫描的点云在线段地图中的坐标。
然后，可在所述线段地图中，通过转换矩阵对所述点云进行平移旋转，确定平移旋转过程中，使得所述点云与所述线段地图中的线段匹配的目标转换矩阵。在旋转的过程中，可计算点云中的点与最近的线段的距离，在点云中的点与最近的线段的距离之和最小的情况下，即视为点云与地图中的线段匹配。
由于雷达扫描的点云表征雷达周围事物的相对位置关系，因此，在点云与地图中的线段匹配后，表明雷达扫描点云时的实际位姿，是通过目标旋转矩阵对预估位姿旋转后的位姿。因此，雷达更准确的位姿应当是通过目标转换矩阵对预估位姿进行旋转平移后的位姿，那么，可以通过目标转换矩阵，对所述雷达的预估位姿进行平移旋转，得到雷达在线段地图中的位姿。
本公开实施例中的点云与线段匹配的过程可通过算法来实现，例如，可以基于高斯牛顿法、列文伯格-马尔夸特法或信赖域狗腿(dogleg)法等方法来实现。
本公开实施例中，通过根据预先估计的预估位姿，来对雷达采集的点云与线段地图中的线段进行匹配，由于通过线段简略地表示地图中的事物，因此在匹配时，利用点云与线段地图中的线段进行匹配，减少了位姿确定过程的计算量，提高了行驶设备位姿确定的效率。
在一种可能的实现方式中，根据匹配后所述雷达的位姿，确定所述行驶设备在所述线段地图中的目标位姿，包括：根据匹配后所述雷达的位姿，确定匹配后所述行驶设备的位姿；将匹配后所述行驶设备的位姿与所述行驶设备的预估位姿进行融合，得到所述行驶设备在所述线段地图中的目标位姿。
行驶设备可以根据雷达和行驶设备的位姿的相对关系，来确定匹配后行驶设备的位姿。然后将匹配后行驶设备的位姿与行驶设备的预估位姿进行融合，将融合后的位姿作为行驶设备在线段地图中的目标位姿。例如，可以 对匹配后行驶设备的位姿与行驶设备的预估位姿进行加权平均，得到融合后的目标位姿；或者也可以通过卡尔曼滤波算法对二者进行融合，得到融合后的目标位姿，卡尔曼滤波算法会通过卡尔曼预测方程，将匹配后行驶设备的位姿与行驶设备的预估位姿按比例进行融合，得到融合后的目标位姿，融合的比例由卡尔曼增益来确定，卡尔曼增益由卡尔曼更新方程根据卡尔曼预测方程的预测结果来更新，使得更新后的卡尔曼预测方程的预测结果更准确。
在融合的过程中，可以利用插值到第二时刻后的行驶设备的预估位姿与匹配后行驶设备的位姿进行融合，也可以直接利用第一时刻的行驶设备的预估位姿与匹配后行驶设备的位姿进行融合。
本公开实施例中，通过将匹配后行驶设备的位姿与行驶设备的预估位姿进行融合，使得不同位姿确定方式的缺点互相弥补，降低各种不确定性因素带来的误差，可以提高得到的目标位姿的准确性。
在一种可能的实现方式中，所述行驶设备包括基于嵌入式平台的行驶设备，例如，运行于沙盘中被配置为人工智能教学的小车、室内扫地机器人等等。基于嵌入式平台的行驶设备其计算性能通常较低，而行驶设备对位姿确定的实时性要求较高，因此，通过本公开实施例提供的位姿确定方法，能够减少行驶设备的计算量，提高位姿确定的速度。
在一种可能的实现方式中，所述行驶设备运行于沙盘中，所述线段地图为所述沙盘的地图。由于沙盘的环境较为单调，在很多路段，雷达扫描的前后两帧没有足够的差异性，通过相邻帧的点云之间进行匹配，或者相邻帧之间进行点线匹配，无法准确匹配出行驶设备的位姿。而本公开实施例中，通过雷达的点云与地图的线段进行匹配，可以显著提高定位精度。
在一个应用示例中，人工智能教学的小车在沙盘中自动行驶，可预先通过沙盘的场地设计图建立沙盘的线段地图，沙盘中的道路、建筑、障碍物等以线段表示在线段地图中。
在初始周期中，可人为设定小车在线段地图中的第一个预估位姿S 1，并根据初始周期每一个第一时刻采集的传感数据，结合预估位姿S 1，确定在后续的每个第一时刻小车的预估位姿S 2，S 3……S n(n为第一时刻的数量)，通过对S 1……S n进行插值，并根据雷达与小车的相对位姿进行变换，得到雷达在每一个第二时刻的预估位姿P 1……P n。
根据小车的预估位姿S 2，S 3……S n从线段地图中筛选出符合第一预设条件的目标线段，根据雷达的预估位姿P 1……P n确定雷达扫描的点云在所述线段地图中的坐标，基于转换矩阵对点云的坐标进行平移旋转，直到在目标线段中找到与点云基本重合的线段，确定此时的转换矩阵为目标转换矩阵，利用该目标转换矩阵对雷达的预估位姿P 1……P n进行平移旋转，得到匹配后雷达的位姿P’ 1……P’ n，进而根据雷达与小车之间的相对位姿关系，得到匹配后小车的位姿S 1”,S” 2，S” 3……S” n，将该位姿与小车的预估位姿S 1,S 2，S 3……S n进行融合，即得到小车在初始周期的目标位姿S 1’,S’ 2，S’ 3……S’ n。
以S’ n取代上文中的S 1，即可开始下一周期的小车目标位姿的确定。
上述方法可以由行驶设备上的位姿确定模块来执行，还可以通过处理器调用存储器中存储的计算机可读指令的方式来实现。
可以理解，本公开提及的上述各个方法实施例，在不违背原理逻辑的情况下，均可以彼此相互结合形成结合后的实施例，限于篇幅，本公开实施例不再赘述。本领域技术人员可以理解，在具体实施方式的上述方法中，各步骤的具体执行顺序应当以其功能和可能的内在逻辑确定。
此外，本公开还提供了位姿确定装置、电子设备、计算机可读存储介质、程序，上述均可用来实现本公开提供的任一种位姿确定方法，相应技术方案和描述和参见方法部分的相应记载。
图4示出根据本公开实施例的位置确定装置的框图，如图4所示，所述装置40包括：
预估位姿确定部分401，被配置为根据行驶设备在线段地图中的预估位姿，确定所述行驶设备的雷达在所述线段地图中的预估位姿，所述线段地图中的事物通过线段表示；
雷达位姿确定部分402，被配置为根据所述雷达的预估位姿，对所述雷达采集的点云与所述线段地图中的线段进行匹配，并根据匹配结果确定匹配后所述雷达在所述线段地图中的位姿；
目标位姿确定部分403，被配置为根据匹配后所述雷达的位姿，确定所述行驶设备在所述线段地图中的目标位姿。
在一种可能的实现方式中，所述预估位姿确定部分401，还被配置为利用所述行驶设备在多个第一时刻的预估位姿，通过插值处理得到所述行驶设备在第二时刻的预估位姿，所述第二时刻包括所述雷达采集所述点云的时刻；根据所述行驶设备在所述第二时刻的预估位姿，得到所述雷达在所述第二时刻的预估位姿。
在一种可能的实现方式中，所述装置还包括：
目标线段确定部分，被配置为根据所述行驶设备的预估位姿中的朝向，确定所述线段地图中满足第一预设条件的目标线段；
所述雷达位姿确定部分402，还被配置为对所述雷达采集的点云与所述目标线段进行匹配。
在一种可能的实现方式中，所述第一预设条件，包括：
线段与所述朝向的夹角的绝对值小于第一阈值；
线段与第一线段的距离小于距离阈值，所述第一线段为与所述朝向的夹角的绝对值小于第二阈值的线段。
在一种可能的实现方式中，所述雷达位姿确定部分402，还被配置为根据所述雷达的预估位姿，确定所述雷达扫描的点云在所述线段地图中的坐标；根据所述点云在所述线段地图中的坐标和所述线段地图中的线段，确定目标转换矩阵，使得所述坐标处的点云经所述目标转换矩阵平移旋转后与所述线段地图中的线段匹配；通过所述目标转换矩阵，对所述雷达的预估位姿进行平移旋转，得到所述雷达在所述线段地图中的位姿。
在一种可能的实现方式中，所述目标位姿确定部分403，还被配置为根据匹配后所述雷达的位姿，确定匹配后所述行驶设备的位姿；将匹配后所述行驶设备的位姿与所述行驶设备的预估位姿进行融合，得到所述行驶设备在所述线段地图中的目标位姿。
在一种可能的实现方式中，所述装置还包括：
当前预估位姿确定部分，被配置为根据上一周期确定的所述行驶设备的目标位姿，以及当前周期所述行驶设备采集的传感数据，对所述行驶设备在线段地图中的位姿进行预估，得到当前周期所述行驶设备的预估位姿，所述传感数据为基于所述行驶设备的运动信息采集的。
在一种可能的实现方式中，所述线段地图是通过对初始地图进行直线拟合得到的，所述初始地图包括：占据栅格地图和场地设计图中的至少一个。
在一种可能的实现方式中，所述行驶设备包括基于嵌入式平台的行驶设备，所述行驶设备运行于沙盘中，所述线段地图为所述沙盘的地图。
在一些实施例中，本公开实施例提供的装置具有的功能或包含的模块可以被配置为执行上文方法实施例描述的方法，其实现可以参照上文方法实施例的描述。
在本公开实施例以及其他的实施例中，“部分”可以是部分电路、部分处理器、部分程序或软件等等，当然也可以是单元，还可以是模块也可以是非模块化的。
本公开实施例还提出一种计算机可读存储介质，其上存储有计算机程序指令，所述计算机程序指令被处理器执行时实现上述方法。计算机可读存储介质可以是易失性或非易失性计算机可读存储介质。
本公开实施例还提出一种电子设备，包括：处理器和被配置为存储处理器可执行指令的存储器；其中，所述处理器被配置为调用所述存储器存储的指令，以执行上述方法。
本公开实施例还提供了一种计算机程序产品，包括计算机可读代码，在计算机可读代码在设备上运行的情况下，设备中的处理器执行用于实现如上任一实施例提供的位姿确定方法的指令。
本公开实施例还提供了另一种计算机程序产品，被配置为存储计算机可读指令，指令被执行时使得计算机执行上述任意一实施例提供的位姿确定方法的操作。
本公开实施例还提供一种计算机程序，该计算机程序被处理器执行时，实现本公开实施例提供的位姿确定方法。
电子设备可以被提供为终端、服务器或其它形态的设备。
图5示出根据本公开实施例的一种电子设备800的框图。例如，电子设备800可以是移动电话，计算机，数字广播终端，消息收发设备，游戏控制台，平板设备，医疗设备，健身设备，个人数字助理等终端。
参照图5，电子设备800可以包括以下一个或多个组件：处理组件802，存储器804，电源组件806，多媒体组件808，音频组件810，输入/输出(I/O)的 接口812，传感器组件814，以及通信组件816。
处理组件802通常控制电子设备800的整体操作，诸如与显示，电话呼叫，数据通信，相机操作和记录操作相关联的操作。处理组件802可以包括一个或多个处理器820来执行指令，以完成上述的方法的全部或部分步骤。此外，处理组件802可以包括一个或多个模块，便于处理组件802和其他组件之间的交互。例如，处理组件802可以包括多媒体模块，以方便多媒体组件808和处理组件802之间的交互。
存储器804被配置为存储各种类型的数据以支持在电子设备800的操作。这些数据的示例包括用于在电子设备800上操作的任何应用程序或方法的指令，联系人数据，电话簿数据，消息，图片，视频等。存储器804可以由任何类型的易失性或非易失性存储设备或者它们的组合实现，如静态随机存取存储器(SRAM)，电可擦除可编程只读存储器(EEPROM)，可擦除可编程只读存储器(EPROM)，可编程只读存储器(PROM)，只读存储器(ROM)，磁存储器，快闪存储器，磁盘或光盘。
电源组件806为电子设备800的各种组件提供电力。电源组件806可以包括电源管理系统，一个或多个电源，及其他与为电子设备800生成、管理和分配电力相关联的组件。
多媒体组件808包括在所述电子设备800和用户之间的提供一个输出接口的屏幕。在一些实施例中，屏幕可以包括液晶显示器(LCD)和触摸面板(TP)。如果屏幕包括触摸面板，屏幕可以被实现为触摸屏，以接收来自用户的输入信号。触摸面板包括一个或多个触摸传感器以感测触摸、滑动和触摸面板上的手势。所述触摸传感器可以不仅感测触摸或滑动动作的边界，而且还检测与所述触摸或滑动操作相关的持续时间和压力。在一些实施例中，多媒体组件808包括一个前置摄像头和/或后置摄像头。当电子设备800处于操作模式，如拍摄模式或视频模式时，前置摄像头和/或后置摄像头可以接收外部的多媒体数据。每个前置摄像头和后置摄像头可以是一个固定的光学透镜系统或具有焦距和光学变焦能力。
音频组件810被配置为输出和/或输入音频信号。例如，音频组件810包括一个麦克风(MIC)，当电子设备800处于操作模式，如呼叫模式、记录模式和语音识别模式时，麦克风被配置为接收外部音频信号。所接收的音频信号可以被进一步存储在存储器804或经由通信组件816发送。在一些实施例中，音频组件810还包括一个扬声器，用于输出音频信号。
I/O接口812为处理组件802和外围接口模块之间提供接口，上述外围接口模块可以是键盘，点击轮，按钮等。这些按钮可包括但不限于：主页按钮、音量按钮、启动按钮和锁定按钮。
传感器组件814包括一个或多个传感器，用于为电子设备800提供各个方面的状态评估。例如，传感器组件814可以检测到电子设备800的打开/关闭状态，组件的相对定位，例如所述组件为电子设备800的显示器和小键盘，传感器组件814还可以检测电子设备800或电子设备800一个组件的位置改变，用户 与电子设备800接触的存在或不存在，电子设备800方位或加速/减速和电子设备800的温度变化。传感器组件814可以包括接近传感器，被配置用来在没有任何的物理接触时检测附近物体的存在。传感器组件814还可以包括光传感器，如CMOS或CCD图像传感器，用于在成像应用中使用。在一些实施例中，该传感器组件814还可以包括加速度传感器，陀螺仪传感器，磁传感器，压力传感器或温度传感器。
通信组件816被配置为便于电子设备800和其他设备之间有线或无线方式的通信。电子设备800可以接入基于通信标准的无线网络，如WiFi，2G或3G，或它们的组合。在一个示例性实施例中，通信组件816经由广播信道接收来自外部广播管理系统的广播信号或广播相关信息。在一个示例性实施例中，所述通信组件816还包括近场通信(NFC)模块，以促进短程通信。例如，在NFC模块可基于射频识别(RFID)技术，红外数据协会(IrDA)技术，超宽带(UWB)技术，蓝牙(BT)技术和其他技术来实现。
在示例性实施例中，电子设备800可以被一个或多个应用专用集成电路(ASIC)、数字信号处理器(DSP)、数字信号处理设备(DSPD)、可编程逻辑器件(PLD)、现场可编程门阵列(FPGA)、控制器、微控制器、微处理器或其他电子元件实现，用于执行上述方法。
在示例性实施例中，还提供了一种非易失性计算机可读存储介质，例如包括计算机程序指令的存储器804，上述计算机程序指令可由电子设备800的处理器820执行以完成上述方法。
图6示出根据本公开实施例的一种电子设备1900的框图。例如，电子设备1900可以被提供为一服务器。参照图6，电子设备1900包括处理组件1922，其进一步包括一个或多个处理器，以及由存储器1932所代表的存储器资源，被配置为存储可由处理组件1922的执行的指令，例如应用程序。存储器1932中存储的应用程序可以包括一个或一个以上的每一个对应于一组指令的模块。此外，处理组件1922被配置为执行指令，以执行上述方法。
电子设备1900还可以包括一个电源组件1926被配置为执行电子设备1900的电源管理，一个有线或无线网络接口1950被配置为将电子设备1900连接到网络，和一个输入输出(I/O)接口1958。电子设备1900可以操作基于存储在存储器1932的操作系统，例如Windows Server TM，Mac OS X TM，Unix TM,Linux TM，FreeBSD TM或类似。
在示例性实施例中，还提供了一种非易失性计算机可读存储介质，例如包括计算机程序指令的存储器1932，上述计算机程序指令可由电子设备1900的处理组件1922执行以完成上述方法。
本公开可以是系统、方法和/或计算机程序产品。计算机程序产品可以包括计算机可读存储介质，其上载有用于使处理器实现本公开实施例的各个方面的计算机可读程序指令(计算机程序)。
计算机可读存储介质可以是可以保持和存储由指令执行设备使用的指令的有形设备，可为易失性存储介质或非易失性存储介质。计算机可读存储介 质例如可以是――但不限于――电存储设备、磁存储设备、光存储设备、电磁存储设备、半导体存储设备或者上述的任意合适的组合。计算机可读存储介质的更具体的例子(非穷举的列表)包括：便携式计算机盘、硬盘、随机存取存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(EPROM或闪存)、静态随机存取存储器(SRAM)、便携式压缩盘只读存储器(CD-ROM)、数字多功能盘(DVD)、记忆棒、软盘、机械编码设备、例如其上存储有指令的打孔卡或凹槽内凸起结构、以及上述的任意合适的组合。这里所使用的计算机可读存储介质不被解释为瞬时信号本身，诸如无线电波或者其他自由传播的电磁波、通过波导或其他传输媒介传播的电磁波(例如，通过光纤电缆的光脉冲)、或者通过电线传输的电信号。
这里所描述的计算机可读程序指令可以从计算机可读存储介质下载到各个计算/处理设备，或者通过网络、例如因特网、局域网、广域网和/或无线网下载到外部计算机或外部存储设备。网络可以包括铜传输电缆、光纤传输、无线传输、路由器、防火墙、交换机、网关计算机和/或边缘服务器。每个计算/处理设备中的网络适配卡或者网络接口从网络接收计算机可读程序指令，并转发该计算机可读程序指令，以供存储在各个计算/处理设备中的计算机可读存储介质中。
用于执行本公开操作的计算机程序指令可以是汇编指令、指令集架构(ISA)指令、机器指令、机器相关指令、微代码、固件指令、状态设置数据、或者以一种或多种编程语言的任意组合编写的源代码或目标代码，所述编程语言包括面向对象的编程语言—诸如Smalltalk、C++等，以及常规的过程式编程语言—诸如“C”语言或类似的编程语言。计算机可读程序指令可以完全地在用户计算机上执行、部分地在用户计算机上执行、作为一个独立的软件包执行、部分在用户计算机上部分在远程计算机上执行、或者完全在远程计算机或服务器上执行。在涉及远程计算机的情形中，远程计算机可以通过任意种类的网络—包括局域网(LAN)或广域网(WAN)—连接到用户计算机，或者，可以连接到外部计算机(例如利用因特网服务提供商来通过因特网连接)。在一些实施例中，通过利用计算机可读程序指令的状态信息来个性化定制电子电路，例如可编程逻辑电路、现场可编程门阵列(FPGA)或可编程逻辑阵列(PLA)，该电子电路可以执行计算机可读程序指令，从而实现本公开的各个方面。
这里参照根据本公开实施例的方法、装置(系统)和计算机程序产品的流程图和/或框图描述了本公开的各个方面。应当理解，流程图和/或框图的每个方框以及流程图和/或框图中各方框的组合，都可以由计算机可读程序指令实现。
这些计算机可读程序指令可以提供给通用计算机、专用计算机或其它可编程数据处理装置的处理器，从而生产出一种机器，使得这些指令在通过计算机或其它可编程数据处理装置的处理器执行时，产生了实现流程图和/或框图中的一个或多个方框中规定的功能/动作的装置。也可以把这些计算机可读 程序指令存储在计算机可读存储介质中，这些指令使得计算机、可编程数据处理装置和/或其他设备以特定方式工作，从而，存储有指令的计算机可读介质则包括一个制造品，其包括实现流程图和/或框图中的一个或多个方框中规定的功能/动作的各个方面的指令。
也可以把计算机可读程序指令加载到计算机、其它可编程数据处理装置、或其它设备上，使得在计算机、其它可编程数据处理装置或其它设备上执行一系列操作步骤，以产生计算机实现的过程，从而使得在计算机、其它可编程数据处理装置、或其它设备上执行的指令实现流程图和/或框图中的一个或多个方框中规定的功能/动作。
附图中的流程图和框图显示了根据本公开的多个实施例的系统、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段或指令的一部分，所述模块、程序段或指令的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。在有些作为替换的实现中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个连续的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，框图和/或流程图中的每个方框、以及框图和/或流程图中的方框的组合，可以用执行规定的功能或动作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。
该计算机程序产品可以具体通过硬件、软件或其结合的方式实现。在一个可选实施例中，所述计算机程序产品具体体现为计算机存储介质，在另一个可选实施例中，计算机程序产品具体体现为软件产品，例如软件开发包(Software Development Kit，SDK)等等。
以上已经描述了本公开的各实施例，上述说明是示例性的，并非穷尽性的，并且也不限于所披露的各实施例。在不偏离所说明的各实施例的范围和精神的情况下，对于本技术领域的普通技术人员来说许多修改和变更都是显而易见的。本文中所用术语的选择，旨在最好地解释各实施例的原理、实际应用或对市场中的技术的改进，或者使本技术领域的其它普通技术人员能理解本文披露的各实施例。
工业实用性
本公开实施例涉及一种位姿确定方法及装置、电子设备和计算机可读存储介质，所述方法包括：根据行驶设备在线段地图中的预估位姿，确定所述行驶设备的雷达在所述线段地图中的预估位姿，所述线段地图中的事物通过线段表示；根据雷达的预估位姿，对所述雷达采集的点云与所述线段地图中的线段进行匹配，并根据匹配结果确定匹配后所述雷达在所述线段地图中的位姿；根据匹配后所述雷达的位姿，确定所述行驶设备在所述线段地图中的目标位姿。本公开实施例可减少位姿确定过程的计算量，提高了行驶设备位姿确定的效率。

Claims (21)

  1. 一种位姿确定方法，包括：
    根据行驶设备在线段地图中的预估位姿，确定所述行驶设备的雷达在所述线段地图中的预估位姿，所述线段地图中的事物通过线段表示；
    根据所述雷达的预估位姿，对所述雷达采集的点云与所述线段地图中的线段进行匹配，并根据匹配结果确定匹配后所述雷达在所述线段地图中的位姿；
    根据匹配后所述雷达的位姿，确定所述行驶设备在所述线段地图中的目标位姿。
  2. 根据权利要求1所述方法，其中，所述根据行驶设备在线段地图中的预估位姿，确定所述行驶设备的雷达在所述线段地图中的预估位姿，包括：
    利用所述行驶设备在多个第一时刻的预估位姿，通过插值处理得到所述行驶设备在第二时刻的预估位姿，所述第二时刻包括所述雷达采集所述点云的时刻；
    根据所述行驶设备在所述第二时刻的预估位姿，得到所述雷达在所述第二时刻的预估位姿。
  3. 根据权利要求1或2所述方法，其中，在所述对所述雷达采集的点云与所述线段地图中的线段进行匹配前，还包括：
    根据所述行驶设备的预估位姿中的朝向，确定所述线段地图中满足第一预设条件的目标线段；
    所述对所述雷达采集的点云与所述线段地图中的线段进行匹配，包括：
    对所述雷达采集的点云与所述目标线段进行匹配。
  4. 根据权利要求3所述方法，其中，所述第一预设条件，包括：
    线段与所述朝向的夹角的绝对值小于第一阈值；
    线段与第一线段的距离小于距离阈值，所述第一线段为与所述朝向的夹角的绝对值小于第二阈值的线段。
  5. 根据权利要求1-4任一所述方法，其中，所述根据所述雷达的预估位姿，对所述雷达采集的点云与所述线段地图中的线段进行匹配，并根据匹配结果确定匹配后所述雷达在所述线段地图中的位姿，包括：
    根据所述雷达的预估位姿，确定所述雷达扫描的点云在所述线段地图中的坐标；
    根据所述点云在所述线段地图中的坐标和所述线段地图中的线段，确定目标转换矩阵，其中，所述坐标处的点云经所述目标转换矩阵平移旋转后与所述线段地图中的线段匹配；
    通过所述目标转换矩阵，对所述雷达的预估位姿进行平移旋转，得到所述雷达在所述线段地图中的位姿。
  6. 根据权利要求1-5任一所述方法，其中，所述根据匹配后所述雷达的 位姿，确定所述行驶设备在所述线段地图中的目标位姿，包括：
    根据匹配后所述雷达的位姿，确定匹配后所述行驶设备的位姿；
    将匹配后所述行驶设备的位姿与所述行驶设备的预估位姿进行融合，得到所述行驶设备在所述线段地图中的目标位姿。
  7. 根据权利要求1-6任一所述方法，其中，在所述位姿确定方法周期性执行的情况下，在所述确定所述行驶设备的雷达在所述线段地图中的预估位姿前，还包括：
    根据上一周期确定的所述行驶设备的目标位姿，以及当前周期所述行驶设备采集的传感数据，对所述行驶设备在线段地图中的位姿进行预估，得到当前周期所述行驶设备的预估位姿，所述传感数据为基于所述行驶设备的运动信息采集的。
  8. 根据权利要求1-7任一所述方法，其中，所述线段地图是通过对初始地图进行直线拟合得到的，所述初始地图包括：占据栅格地图和场地设计图中的至少一个。
  9. 根据权利要求1-8任一所述方法，其中，所述行驶设备包括基于嵌入式平台的行驶设备，所述行驶设备运行于沙盘中，所述线段地图为所述沙盘的地图。
  10. 一种位姿确定装置，包括：
    预估位姿确定部分，被配置为根据行驶设备在线段地图中的预估位姿，确定所述行驶设备的雷达在所述线段地图中的预估位姿，所述线段地图中的事物通过线段表示；
    雷达位姿确定部分，被配置为根据所述雷达的预估位姿，对所述雷达采集的点云与所述线段地图中的线段进行匹配，并根据匹配结果确定匹配后所述雷达在所述线段地图中的位姿；
    目标位姿确定部分，被配置为根据匹配后所述雷达的位姿，确定所述行驶设备在所述线段地图中的目标位姿。
  11. 根据权利要求10所述装置，其中，所述预估位姿确定部分，还被配置为利用所述行驶设备在多个第一时刻的预估位姿，通过插值处理得到所述行驶设备在第二时刻的预估位姿，所述第二时刻包括所述雷达采集所述点云的时刻；根据所述行驶设备在所述第二时刻的预估位姿，得到所述雷达在所述第二时刻的预估位姿。
  12. 根据权利要求10或11所述装置，其中，所述装置还包括：
    目标线段确定部分，被配置为根据所述行驶设备的预估位姿中的朝向，确定所述线段地图中满足第一预设条件的目标线段；
    所述雷达位姿确定部分，被配置为对所述雷达采集的点云与所述目标线段进行匹配。
  13. 根据权利要求12所述装置，其中，所述第一预设条件，包括：
    线段与所述朝向的夹角的绝对值小于第一阈值；
    线段与第一线段的距离小于距离阈值，所述第一线段为与所述朝向的夹 角的绝对值小于第二阈值的线段。
  14. 根据权利要求10-13任一项所述装置，其中，所述雷达位姿确定部分，还被配置为根据所述雷达的预估位姿，确定所述雷达扫描的点云在所述线段地图中的坐标；根据所述点云在所述线段地图中的坐标和所述线段地图中的线段，确定目标转换矩阵，其中，所述坐标处的点云经所述目标转换矩阵平移旋转后与所述线段地图中的线段匹配；通过所述目标转换矩阵，对所述雷达的预估位姿进行平移旋转，得到所述雷达在所述线段地图中的位姿。
  15. 根据权利要求10-14任一项所述装置，其中，所述目标位姿确定部分，还被配置为根据匹配后所述雷达的位姿，确定匹配后所述行驶设备的位姿；将匹配后所述行驶设备的位姿与所述行驶设备的预估位姿进行融合，得到所述行驶设备在所述线段地图中的目标位姿。
  16. 根据权利要求10-15任一项所述装置，其中，所述装置还包括：
    当前预估位姿确定部分，被配置为根据上一周期确定的所述行驶设备的目标位姿，以及当前周期所述行驶设备采集的传感数据，对所述行驶设备在线段地图中的位姿进行预估，得到当前周期所述行驶设备的预估位姿，所述传感数据为基于所述行驶设备的运动信息采集的。
  17. 根据权利要求10-16任一项所述装置，其中，所述线段地图是通过对初始地图进行直线拟合得到的，所述初始地图包括：占据栅格地图和场地设计图中的至少一个。
  18. 根据权利要求10-17任一项所述装置，其中，所述行驶设备包括基于嵌入式平台的行驶设备，所述行驶设备运行于沙盘中，所述线段地图为所述沙盘的地图。
  19. 一种电子设备，包括：
    处理器；
    被配置为存储处理器可执行指令的存储器；
    其中，所述处理器被配置为调用所述存储器存储的指令，执行权利要求1至9中任意一项所述的方法。
  20. 一种计算机可读存储介质，其上存储有计算机程序指令，所述计算机程序指令被处理器执行时实现权利要求1至9中任意一项所述的方法。
  21. 一种计算机程序，包括计算机可读代码，在所述计算机可读代码在电子设备中运行的情况下，所述电子设备中的处理器执行时实现权利要求1至9中任意一项所述的方法。
PCT/CN2021/092487 2020-11-27 2021-05-08 一种位姿确定方法及装置、电子设备和计算机可读存储介质 WO2022110653A1 (zh)

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
CN202011362399.XA CN112433211B (zh) 2020-11-27 2020-11-27 一种位姿确定方法及装置、电子设备和存储介质
CN202011362399.X 2020-11-27

Publications (1)

Publication Number Publication Date
WO2022110653A1 true WO2022110653A1 (zh) 2022-06-02

Family

ID=74698665

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/CN2021/092487 WO2022110653A1 (zh) 2020-11-27 2021-05-08 一种位姿确定方法及装置、电子设备和计算机可读存储介质

Country Status (2)

Country Link
CN (1) CN112433211B (zh)
WO (1) WO2022110653A1 (zh)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112433211B (zh) * 2020-11-27 2022-11-29 浙江商汤科技开发有限公司 一种位姿确定方法及装置、电子设备和存储介质
CN113406659A (zh) * 2021-05-28 2021-09-17 浙江大学 一种基于激光雷达信息的移动机器人位置重识别方法
CN113776533A (zh) * 2021-07-29 2021-12-10 北京旷视科技有限公司 可移动设备的重定位方法及装置

Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101000507A (zh) * 2006-09-29 2007-07-18 浙江大学 移动机器人在未知环境中同时定位与地图构建的方法
US8369606B2 (en) * 2010-07-21 2013-02-05 Palo Alto Research Center Incorporated System and method for aligning maps using polyline matching
CN103941264A (zh) * 2014-03-26 2014-07-23 南京航空航天大学 一种室内未知环境下使用激光雷达定位方法
US20170075355A1 (en) * 2015-09-16 2017-03-16 Ford Global Technologies, Llc Vehicle radar perception and localization
CN109341688A (zh) * 2018-09-05 2019-02-15 南京理工大学 一种基于构造顺序的地图调用定位算法
CN109655805A (zh) * 2019-01-25 2019-04-19 南京理工大学 一种基于扫描线段重合长度估计的激光雷达定位方法
CN110631554A (zh) * 2018-06-22 2019-12-31 北京京东尚科信息技术有限公司 机器人位姿的确定方法、装置、机器人和可读存储介质
CN111325796A (zh) * 2020-02-28 2020-06-23 北京百度网讯科技有限公司 用于确定视觉设备的位姿的方法和装置
CN111522022A (zh) * 2020-04-20 2020-08-11 西安电子科技大学 基于激光雷达的机器人进行动态目标检测方法
CN112433211A (zh) * 2020-11-27 2021-03-02 浙江商汤科技开发有限公司 一种位姿确定方法及装置、电子设备和存储介质

Family Cites Families (15)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
RU2249232C2 (ru) * 2003-04-15 2005-03-27 Открытое акционерное общество "Корпорация "Фазотрон-Научно-исследовательский институт радиостроения" (ОАО "Корпорация "Фазотрон-НИИР") Способ сопровождения радиоконтрастного объекта по направлению и устройство сопровождения радиоконтрастного объекта по направлению
WO2013051047A1 (ja) * 2011-10-03 2013-04-11 古野電気株式会社 表示装置、表示プログラム、及び表示方法
CN104575075B (zh) * 2015-01-14 2016-09-28 合肥革绿信息科技有限公司 一种基于北斗的城市路网车辆坐标校正方法及装置
US9679216B2 (en) * 2015-05-07 2017-06-13 The United States Of America As Represented By The Secretary Of The Air Force Morphological automatic triangle orientation detection
CN106123890A (zh) * 2016-06-14 2016-11-16 中国科学院合肥物质科学研究院 一种多传感器数据融合的机器人定位方法
US10192098B2 (en) * 2016-09-09 2019-01-29 MorphoTrak, LLC Palm print image matching techniques
CN107704821B (zh) * 2017-09-29 2020-06-09 河北工业大学 一种弯道的车辆位姿计算方法
JP6880080B2 (ja) * 2018-07-02 2021-06-02 ベイジン ディディ インフィニティ テクノロジー アンド ディベロップメント カンパニー リミティッド ポイントクラウドに基づく姿勢推定を用いた車両ナビゲーションシステム
CN111316284A (zh) * 2019-02-13 2020-06-19 深圳市大疆创新科技有限公司 车道线检测方法、装置、系统与车辆、存储介质
CN109839112B (zh) * 2019-03-11 2023-04-07 中南大学 地下作业设备定位方法、装置、系统及存储介质
CN110926485B (zh) * 2019-11-11 2021-10-08 华中科技大学 一种基于直线特征的移动机器人定位方法及系统
CN111223145A (zh) * 2020-01-03 2020-06-02 上海有个机器人有限公司 数据处理方法、系统、服务装置及其存储介质
CN111590595B (zh) * 2020-06-30 2021-09-28 深圳市银星智能科技股份有限公司 一种定位方法、装置、移动机器人及存储介质
CN111929699B (zh) * 2020-07-21 2023-05-09 北京建筑大学 一种顾及动态障碍物的激光雷达惯导里程计与建图方法及系统
CN111983635B (zh) * 2020-08-17 2022-03-29 浙江商汤科技开发有限公司 位姿确定方法及装置、电子设备和存储介质

Patent Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101000507A (zh) * 2006-09-29 2007-07-18 浙江大学 移动机器人在未知环境中同时定位与地图构建的方法
US8369606B2 (en) * 2010-07-21 2013-02-05 Palo Alto Research Center Incorporated System and method for aligning maps using polyline matching
CN103941264A (zh) * 2014-03-26 2014-07-23 南京航空航天大学 一种室内未知环境下使用激光雷达定位方法
US20170075355A1 (en) * 2015-09-16 2017-03-16 Ford Global Technologies, Llc Vehicle radar perception and localization
CN110631554A (zh) * 2018-06-22 2019-12-31 北京京东尚科信息技术有限公司 机器人位姿的确定方法、装置、机器人和可读存储介质
CN109341688A (zh) * 2018-09-05 2019-02-15 南京理工大学 一种基于构造顺序的地图调用定位算法
CN109655805A (zh) * 2019-01-25 2019-04-19 南京理工大学 一种基于扫描线段重合长度估计的激光雷达定位方法
CN111325796A (zh) * 2020-02-28 2020-06-23 北京百度网讯科技有限公司 用于确定视觉设备的位姿的方法和装置
CN111522022A (zh) * 2020-04-20 2020-08-11 西安电子科技大学 基于激光雷达的机器人进行动态目标检测方法
CN112433211A (zh) * 2020-11-27 2021-03-02 浙江商汤科技开发有限公司 一种位姿确定方法及装置、电子设备和存储介质

Also Published As

Publication number Publication date
CN112433211A (zh) 2021-03-02
CN112433211B (zh) 2022-11-29

Similar Documents

Publication Publication Date Title
WO2022110653A1 (zh) 一种位姿确定方法及装置、电子设备和计算机可读存储介质
CN108596116B (zh) 测距方法、智能控制方法及装置、电子设备和存储介质
CN109870157B (zh) 确定车体位姿的方法及装置、制图方法
JP7236565B2 (ja) 位置姿勢決定方法、装置、電子機器、記憶媒体及びコンピュータプログラム
WO2021212964A1 (zh) 定位方法及装置、电子设备和存储介质
US9641814B2 (en) Crowd sourced vision and sensor-surveyed mapping
US20180213358A1 (en) Scene sharing-based navigation assistance method and terminal
US20210158560A1 (en) Method and device for obtaining localization information and storage medium
EP3579215A1 (en) Electronic device for generating map data and operating method therefor
TWI767217B (zh) 坐標系對齊的方法及裝置、電子設備和計算機可讀存儲介質
WO2019047725A1 (zh) 无人飞行器的航区规划方法、装置和遥控器
KR102277503B1 (ko) 객체 인식 기반 실내 측위를 위한 단말장치, 서비스 서버 및 그 방법
CN113205549A (zh) 深度估计方法及装置、电子设备和存储介质
WO2022110776A1 (zh) 定位方法及装置、电子设备、存储介质、计算机程序产品、计算机程序
CN110865405A (zh) 融合定位方法及装置、移动设备控制方法及电子设备
CN113094966A (zh) 用于使用粒子滤波器进行定位的基于射频的虚拟运动模型
WO2020168744A1 (zh) 车辆标定系统及方法
WO2022110785A1 (zh) 定位方法及装置、电子设备、存储介质、计算机程序产品、计算机程序
US20160343156A1 (en) Information display device and information display program
CN109086843B (zh) 一种基于二维码的移动机器人导航方法
WO2022237071A1 (zh) 定位方法及装置、电子设备、存储介质和计算机程序
WO2022110777A1 (zh) 定位方法及装置、电子设备、存储介质、计算机程序产品、计算机程序
WO2022110801A1 (zh) 数据处理方法及装置、电子设备和存储介质
US20240053746A1 (en) Display system, communications system, display control method, and program
CN112162292A (zh) 点云数据处理方法及其装置

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 21896168

Country of ref document: EP

Kind code of ref document: A1

NENP Non-entry into the national phase

Ref country code: DE

122 Ep: pct application non-entry in european phase

Ref document number: 21896168

Country of ref document: EP

Kind code of ref document: A1

122 Ep: pct application non-entry in european phase

Ref document number: 21896168

Country of ref document: EP

Kind code of ref document: A1

32PN Ep: public notification in the ep bulletin as address of the adressee cannot be established

Free format text: NOTING OF LOSS OF RIGHTS PURSUANT TO RULE 112(1) EPC (EPO FORM 1205A DATED 27/11/2024)