CN113124880A - Mapping and positioning method and device based on data fusion of two sensors - Google Patents

Abstract

The application relates to a mapping and positioning method and device based on data fusion of two sensors. The method comprises the following steps: acquiring external parameter information of the distance measuring device arranged on the movable platform; in the moving process of the movable platform, acquiring distance information between the distance measuring device and at least one distance measuring base station in real time; meanwhile, the relative pose relationship of the movable platform at two different moments is obtained through an instant positioning and map building sensor; and performing data fusion based on the external reference information and the distance information and relative pose relationship acquired in a time period to obtain a target map. The method and the device have higher mapping or positioning accuracy.

Description

Mapping and positioning method and device based on data fusion of two sensors

Technical Field

The application relates to the technical field of artificial intelligence, in particular to a method and a device for establishing a map and positioning based on data fusion of two sensors.

Background

With the development of positioning technology, synchronous positioning and mapping (SLAM) technology becomes an important positioning and navigation technology. The SLAM technology is a technology that, in the absence of environment prior information, data in an environment is acquired in real time through an instant positioning and mapping sensor, and mapping or positioning is performed by using acquired feature point information in the environment.

With the increasing requirement for the accuracy of the positioning technology, the SLAM technology has the disadvantage that the accuracy of SLAM is seriously affected when the feature point information of the environment is insufficient or the environment features are single.

Disclosure of Invention

Therefore, it is necessary to provide a mapping and positioning method and device based on two kinds of sensor data fusion with high accuracy.

A map building method based on data fusion of two sensors is applied to a movable platform for installing a distance measuring device and an instant positioning and map building sensor, and a distance measuring base station is preset around the movable platform, and the method comprises the following steps:

acquiring external parameter information of the distance measuring device arranged on the movable platform;

in the moving process of the movable platform, acquiring distance information between the distance measuring device and at least one distance measuring base station in real time; meanwhile, the relative pose relationship of the movable platform at two different moments is obtained through an instant positioning and map building sensor;

and performing data fusion based on the external reference information and the distance information and relative pose relationship acquired in a time period to obtain a target map.

In one embodiment, the mapping method based on two kinds of sensor data fusion further includes: when the environment is a long and narrow environment, at least two ranging base stations are preset in the long and narrow environment; the real-time acquisition of the distance information between the ranging device and at least one ranging base station in the moving process of the movable platform comprises the following steps: and in the moving process of the movable platform, the distance information between the distance measuring device and at least two distance measuring base stations is acquired in real time.

In one embodiment, the data fusion based on the external reference information and the distance information and the relative pose relationship acquired in the time period includes: constructing a first constraint function according to the external reference information and the distance information which is close to the actual distance between the ranging device and the ranging base station; constructing a second constraint function according to the fact that the relative pose relation is close to the actual pose change of the movable platform between two different moments; and performing joint optimization processing based on the first constraint function and the second constraint function to obtain the coordinates of the ranging base station in the coordinate system of the target map.

In one embodiment, the performing optimization based on the first constraint function and the second constraint function to obtain the coordinates of the ranging base station in the coordinate system of the target map includes: and building a first optimization function and solving the first optimization function based on the accumulated calculation of the first constraint function and the second constraint function in the time period to obtain the coordinates of the ranging base station in the coordinate system of the target map.

In one embodiment, the constructing the first constraint function includes the following steps: obtaining a coordinate system of the distance measuring device and an external parameter conversion matrix of the coordinate system of the movable platform according to the external parameter information of the movable platform on which the distance measuring device is arranged; constructing a first conversion matrix function of a coordinate system of the movable platform and a coordinate system of the target map at a first moment; obtaining a second conversion matrix function of the coordinate system of the distance measuring device and the coordinate system of the target map at the first moment based on the external reference conversion matrix and the first conversion matrix function, and obtaining a second translation matrix function of the coordinate system of the distance measuring device to the target map at the first moment; constructing a third transformation matrix function of the coordinate system of at least one ranging base station and the coordinate system of the target map, and obtaining a third translation matrix function of the coordinate system of the ranging base station and the coordinate system of the target map; according to the fact that the difference of the translation amounts of the second translation matrix function and the third translation matrix function is close to the distance information, a first constraint function is constructed:

wherein epsilon₁(κ)_iA first constraint function representing the position of the kth distance measuring device at the time of ith, map representing the coordinate system of the target map, b representing the coordinate system of the movable platform, u_bA coordinate system representing said distance measuring device,

the extrinsic transformation matrix is represented as a function of,

a first transformation matrix function representing the ith time instant, ()_tA translation matrix is represented that represents the translation of the image,

representing said second translation matrix function at the i-th instant,

representing said third translation matrix function,

represents the distance between the ranging device at the ith time and the ranging base station at the kth time.

In one embodiment, the constructing the second constraint function includes the following steps: constructing a fourth transformation matrix function of the coordinate system of the movable platform on the target map at the first moment; constructing a fifth transformation matrix function of the coordinate system of the movable platform on the target map at the second moment; obtaining a sixth conversion matrix function based on the fourth conversion function matrix and the fifth conversion function matrix; according to the fact that the relative pose relation of the movable platform between the first time and the second time is close to the sixth conversion matrix function, a second constraint function is constructed:

wherein epsilon₂Representing a second constraint function between the ith and jth instants with respect to said movable platform, map representing the coordinate system of said target map, b_iCoordinate system representing the movable platform at time i, b_jA coordinate system representing the movable platform at time j,

a transformation matrix representing the coordinate system of the movable platform and the coordinate system of said target map at time i,

a transformation matrix representing the coordinate system of the movable platform and the coordinate system of said target map at time j,

a transition matrix representing the movable platform at time j and time i.

In one embodiment, the performing joint optimization processing based on the first constraint function and the second constraint function to obtain the coordinates of the ranging base station in the coordinate system of the target map includes the following steps: building a first optimization function based on the cumulative calculation of the first constraint function and the second constraint function in the time period:

wherein, Ε₁Representing a first optimization function, i representing an ith time instant, j representing a jth time instant; solving the first optimization function to obtain a transformation matrix of a coordinate system of the ranging base station and a coordinate system of the target map; and calculating the coordinate of the ranging base station in the coordinate system of the target map according to the coordinate system of the ranging base station and the transformation matrix of the coordinate system of the target map.

A positioning method based on data fusion of two sensors is applied to a movable platform for installing a distance measuring device and an instant positioning and map building sensor, and a distance measuring base station is preset in the environment around the movable platform, and the method comprises the following steps:

acquiring a map, wherein the position of the ranging base station is presented on the map;

acquiring external parameter information of the distance measuring device arranged on the movable platform; obtaining distance information between the ranging device and at least one ranging base station;

meanwhile, environmental point cloud data are obtained through a real-time positioning and map building sensor;

and performing data fusion on the basis of the distance information, the external reference information, the environmental point cloud data and the map, and determining the current pose of the movable platform in the map.

In one embodiment, the positioning method based on two kinds of sensor data fusion further includes: at least two ranging base stations are preset around the movable platform in the long and narrow environment; the obtaining of the distance information between the ranging device and at least one ranging base station includes: and in the moving process of the movable platform, the distance information between the distance measuring device and at least two distance measuring base stations is acquired in real time.

In one embodiment, the step of determining the current pose of the movable platform based on the data fusion of the distance information, the external reference information, the first link point cloud data and the map includes: constructing a third constraint function according to the external reference information and the distance information which is close to the actual distance between the ranging device and the ranging base station; constructing a fourth constraint function according to the coordinate point set of the environment point cloud data close to the coordinate system of the map; and performing joint optimization processing based on the third constraint function and the fourth constraint function to obtain the current pose of the movable platform in the map.

In one embodiment, the component third constraint function includes the following steps: obtaining a coordinate system of the distance measuring device and an external parameter conversion matrix of the coordinate system of the movable platform according to the external parameter information of the distance measuring device arranged on the movable platform; constructing a seventh transformation matrix function of the coordinate system of the movable platform and the coordinate system of the map; obtaining a coordinate system of the distance measuring device and an eighth conversion matrix function of the coordinate system of the map according to the external reference conversion matrix and the seventh conversion matrix function, and obtaining an eighth translation matrix function from the distance measuring device to the coordinate system of the map; acquiring a transformation matrix of a coordinate system of at least one ranging base station and a coordinate system of the map to obtain a translation matrix of the coordinate system of the ranging base station and the coordinate system of the map; according to the distance information close to the difference between the eighth translation matrix function and the translation amount of the translation matrix of the coordinate system of the ranging base station and the translation matrix of the coordinate system of the map, constructing a third constraint function:

wherein epsilon₃(κ) a third constraint function based on the pose of the kth ranging base station with respect to the movable platform; map represents the coordinate system of the target map, b represents the coordinate system of the movable platform, u_bA coordinate system representing said distance measuring device,

the extrinsic transformation matrix is represented as a function of,^mapT^brepresents a seventh transfer matrix function, () t represents a translation matrix,

representing said eighth translation matrix function,

a coordinate system translation matrix representing a coordinate system of the ranging base station and the map,

represents the distance between the ranging device and the kth ranging base station.

In one embodiment, the constructing the fourth constraint function includes the following steps: according to the coordinate point set of the environment point cloud data close to the coordinate system of the map, constructing a fourth constraint function:

ε₄(x)＝^mapT^b·p_x-p_m,x

wherein map represents the coordinate system of the map, b represents the coordinate system of the movable platform,^mapT^btransformation matrix, p, representing the coordinate system of the movable platform and the coordinate system of said map_xRepresenting the coordinates, p, of the xth current laser spot measured by the instant positioning and mapping sensor in the coordinate system of the movable platform_m,xRepresents a group of_xA point in a set of coordinate points that is closest to a set of coordinate points in the map.

In one embodiment, the performing joint optimization processing based on the third constraint function and the fourth constraint function to obtain the current pose of the movable platform in the map includes the following steps: and constructing a second optimization function according to the third constraint function and the fourth constraint function:

and solving the second optimization function to obtain a coordinate system of the movable platform and a transformation matrix of the coordinate system of the map, so as to obtain the current pose of the movable platform in the coordinate system of the map.

The utility model provides a build picture device based on two kinds of sensor data fusion, is applied to the movable platform of installation range unit and instant location and map construction sensor, predetermines the range finding basic station around the movable platform, the device includes:

the first acquisition module is used for acquiring distance information between the distance measuring device and at least one distance measuring base station in real time in the moving process of the movable platform; meanwhile, the relative pose relationship of the movable platform at two different moments is obtained through an instant positioning and map building sensor;

the external reference information acquisition module is used for acquiring external reference information of the distance measuring device arranged on the movable platform;

and the data fusion module is used for carrying out data fusion on the basis of the external reference information and the distance information and relative pose relation acquired in a time period to obtain a target map.

The utility model provides a positioner based on two kinds of sensor data fusion, is applied to the movable platform of installation range unit and instant location and map construction sensor, predetermines the range finding basic station in the environment around the movable platform, the device includes:

the map acquisition module is used for acquiring a map, and the map is provided with coordinates for presenting the ranging base station;

the external reference information acquisition module is used for acquiring external reference information of the distance measuring device arranged on the movable platform; obtaining distance information between the ranging device and at least one ranging base station;

the point cloud data acquisition module is used for acquiring environmental point cloud data through the instant positioning and map building sensor;

and the data fusion module is used for carrying out data fusion on the basis of the distance information, the external reference information, the environmental point cloud data and the map and determining the current pose of the movable platform in the map.

A computer device comprising a memory and a processor, the memory storing a computer program, the processor implementing the following steps when executing the computer program:

in the moving process of the movable platform, acquiring distance information between the distance measuring device and at least one distance measuring base station in real time; meanwhile, the relative pose relationship of the movable platform at two different moments is obtained through an instant positioning and map building sensor;

acquiring external parameter information of the distance measuring device arranged on the movable platform;

and performing data fusion based on the external reference information and the distance information and relative pose relationship acquired in a time period to obtain a target map.

A computer-readable storage medium, on which a computer program is stored which, when executed by a processor, carries out the steps of:

in the moving process of the movable platform, acquiring distance information between the distance measuring device and at least one distance measuring base station in real time; meanwhile, the relative pose relationship of the movable platform at two different moments is obtained through an instant positioning and map building sensor;

acquiring external parameter information of the distance measuring device arranged on the movable platform;

and performing data fusion based on the external reference information and the distance information and relative pose relationship acquired in a time period to obtain a target map.

The scheme integrates the distance information, the relative pose relationship between the instant positioning and the map construction sensor and the external reference information, and compared with the positioning technology in the prior art, the distance information plays a role in positioning redundancy and improves the accuracy of map construction or positioning; in addition, the method and the device realize tight coupling in a data layer, do not depend on one data as prior information, and have higher mapping or positioning accuracy.

Drawings

FIG. 1 is a diagram of an application environment of a mapping method based on two sensor data fusion in one embodiment;

FIG. 2 is a schematic flow chart diagram of a mapping method based on two sensor data fusion in one embodiment;

FIG. 3 is a schematic flow chart of a positioning method based on data fusion of two sensors in another embodiment;

FIG. 4 is a block diagram of an embodiment of a mapping device based on two sensor data fusion;

FIG. 5 is a block diagram of a positioning device based on fusion of two sensor data in one embodiment;

FIG. 6 is a diagram illustrating an internal structure of a computer device according to an embodiment.

Detailed Description

In order to make the objects, technical solutions and advantages of the present application more apparent, the present application is described in further detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative of the present application and are not intended to limit the present application.

The synchronous positioning and mapping (SLAM) technology is a key technology for realizing full-autonomous mobile positioning navigation. Because the accuracy of the SLAM is influenced by the acquired environment characteristic point information, the identification capability of the SLAM technology is poor in an open environment with an oversized range, or in long and narrow special environments such as tunnels and galleries, or in other environments which cannot effectively restrict a certain direction, and the positioning accuracy is not high enough.

Thus, a mapping and positioning method and a device thereof based on two sensor data fusion are provided.

As shown in fig. 2, an embodiment of the present application provides a mapping method based on data fusion of two sensors, which is applied to a movable platform for installing a distance measuring device and an instant positioning and mapping sensor, where a distance measuring base station is preset around the movable platform, and the method includes the following steps:

and S110, acquiring external reference information of the distance measuring device installed on the movable platform.

The external parameter information of the distance measuring device installed on the movable platform can be obtained in an external parameter calibration mode in the prior art or obtained in a manual input mode. The extrinsic information may be a transformation matrix of coordinates of the ranging device to coordinates of the movable platform coordinate system.

S120, in the moving process of the movable platform, acquiring distance information between the distance measuring device and at least one distance measuring base station in real time; meanwhile, the relative pose relationship of the movable platform at two different moments is obtained through the instant positioning and map building sensor.

Wherein, movable platform refers to the equipment that can independently remove, including but not limited to automatic guide car, robot, autopilot car, unmanned aerial vehicle, intelligent house.

The distance measuring device and the distance measuring base station have wireless distance measuring functions, including but not limited to devices which adopt Bluetooth, WiFi and UWB technologies for communication. The distance measuring device is arranged on the movable platform and moves along with the movable platform; the ranging base station is arranged in the environment where the movable platform is located. Preferably, distance information is introduced as positioning redundancy based on UWB technology, and adaptability to environment is strong. Because the distance information can not be influenced by the intensity of illumination and the intensity of satellite signals, the technical scheme of the application can be suitable for the environment with poor illumination conditions and weak satellite signals. And in the communication process of the ranging device and the ranging base station, the distance between the ranging device and the ranging base station can be calculated according to the time stamp.

The instant position and map sensor is used to scan the surrounding environment information, for example, instant position and map sensor (slam sensor) including but not limited to lidar, camera, ultrasound. The real-time positioning and mapping sensor scans surrounding environment information in real time, so that the relative pose relationship of the movable platform between two different positions is obtained.

And S130, performing data fusion based on the external reference information and the distance information and relative pose relationship acquired in a time period to obtain a target map.

More specifically, the distance information at a plurality of moments acquired within a preset time period acquires the relative pose relationship of the movable platforms at two different moments. The data fusion is the fusion of data layers, and the track is not estimated independently according to distance information or relative pose relation. And constructing an optimization function and solving to obtain the coordinates of the ranging base station in the coordinate system of the target map according to the fact that the difference between the translation amounts of the translation matrix from the ranging device to the map coordinate system and the translation matrix from the ranging base station to the map coordinate system is close to the distance information, and the matrix conversion relation between the pose of the movable platform in the map coordinate system at the first moment and the pose of the movable platform in the map coordinate system at the second moment is close to the relative pose relation between the two different poses of the movable platform. And then, according to the position of the ranging base station and the environmental information scanned by the instant positioning and map building sensor, building a map to obtain a target map.

According to the map building method based on data fusion of the two sensors, the distance between the ranging device on the movable platform and the ranging base station is obtained, then the instant positioning and map building sensor is combined to collect surrounding environment information, the relative pose relation of the movable platform between two different positions is calculated, the external reference information is combined to calculate the positioning of the ranging base station, and finally the instant positioning and map building sensor is combined to build the map, so that the error of manually calculating the positioning of the ranging base station can be reduced, and meanwhile, the map building efficiency is improved.

In a specific operation method, the input distance measuring device is installed on external reference information of the movable platform. The movable platform can be controlled to move along a path in a manual remote control mode. In the moving process of the movable platform, the distance information at different moments is continuously acquired within a period of time, and meanwhile, the relative pose relation of the movable platform at two different moments is continuously acquired. And obtaining a target map based on the external reference information and a data fusion result of the distance information and the relative pose relation acquired in the time period in the data layer.

In one embodiment, when the environment is a long and narrow environment, the mapping method presets at least two ranging base stations in the long and narrow environment. The real-time acquisition of the distance information between the ranging device and at least one ranging base station in the moving process of the movable platform comprises the following steps: and in the moving process of the movable platform, the distance information between the distance measuring device and at least two distance measuring base stations is acquired in real time.

The long and narrow environment specifically refers to an environment with a relatively long distance between two ends and a relatively short distance between two sides, such as a tunnel, a corridor, a gallery and a roadway.

When the environment is a narrow and long environment, the environmental point cloud data collected by the instant positioning and mapping sensor is easy to be constrained and lost in a certain direction, and the coordinate system of the map is assumed to be in three directions of an X axis, a Y axis and a Z axis of a cartesian coordinate system (X, Y and Z). In a long and narrow environment, the environmental characteristics are insufficient, and effective constraint cannot be performed on a certain direction, so that constraint loss in the X direction is caused.

At least two ranging base stations are preset in the long and narrow environment, and are explained as follows:

take the process of drawing as an example. If the initial position, the first position and the second position are taken on the moving path of the movable platform, the coordinates of the movable platform at the three positions are calculated based on the instant positioning and map construction sensor, the default initial position is (0,0,0), and the first position is (X)₁,Y₁,Z₁) The second position is (X)₂,Y₂,Z₂). When a ranging base station is preset in the long and narrow environment, the coordinate of the first ranging base station is assumed to be (X)_U1，Y_U1,Z_U1) And solving according to the distance calculation formulas of the first ranging base station and the first position, the second position and the third position:

X_U1²+Y_U1²+Z_U1²＝d₁₀²(equation one)

(X_U1-X₁)²+(Y_U1-Y₁)²+(Z_U1-Z₁)²＝d₁₁²(equation two)

(X_U1-X₂)²+(Y_U1-Y₂)²+(Z_U1-Z₂)²＝d₁₂²(equation three)

Because the instant positioning and map building sensor is arranged in the X-axis directionLack of constraint on, X₁、X₂Is unknown. Based on the distance information of the location and map building sensor, the ranging base station and the ranging device, Y can be known₁、Z₁、Y₂、Z₂Distance d between first ranging base station and initial position₁₀Distance d between first ranging base station and first position₁₁Distance d between first ranging base station and second position₁₂。

Then, by solving both equations one and two, there are four unknowns X₁、X_U1、Y_U1、Z_U1Two equations cannot solve for four unknowns.

Through the combined solution of equation one, equation two and equation three, five unknowns X are obtained₁、X₂、X_U1、Y_U1、Z_U1The three equations cannot solve for five unknowns.

When two ranging base stations are preset in the long and narrow environment, the coordinate of the second ranging base station is assumed to be (X)_U2，Y_U2，Z_U2) And solving according to the distance calculation formulas of the second ranging base station and the first position, the second position and the third position:

X_U2²+Y_U2²+Z_U2²＝d₂₀²(equation four)

(X_U2-X₁)²+(Y_U2-Y₁)²+(Z_U2-Z₁)²＝d₂₁²(equation five)

(X_U2-X₂)²+(Y_U2-Y₂)²+(Z_U2-Z₂)²＝d₂₂²(equation six)

Then, through the combined solution of equation one, equation two, equation four and equation five, there are seven unknowns X₁、X_U1、Y_U1、Z_U1、X_U2、Y_U2、Z_U2The four equations cannot solve for seven unknowns.

By equation one, equationTwo, three, four, five and six joint solutions with eight unknowns X₁、X₂、X_U1、Y_U1、Z_U1、X_U2、Y_U2、Z_U2Six equations cannot solve for eight unknowns.

By analogy, if five positions are taken on the path of the movable platform, there are ten equations, with X₁、X₂、X₃、X₄、X_U1、Y_U1、Z_U1、X_U2、Y_U2、Z_U2Ten unknowns, ten equations may solve for ten unknowns.

Similarly, the number of the base stations participating in the ranging is increased, and the number of the equations can be increased to solve the coordinates of the ranging base stations respectively.

Such as the application environment shown in fig. 1. In thetunnel 100, adistance measuring device 201 and a positioning andmapping sensor 202 are installed on therobot 200, and external reference information of therobot 200 installed with thedistance measuring device 201 is acquired. A plurality of distance measuringbase stations 101 are arranged on two sides of a road of thetunnel 100, the distancemeasuring base stations 101 and thedistance measuring devices 201 communicate by adopting UWB technology, and therobot 200 is provided with a mobile terminal computer to process data. In order to obtain the global coordinate of the rangingbase station 101 in thetunnel 100 and ensure the consistency between the rangingbase station 101 and the laser map, firstly, a laser point cloud map with a UWB mark needs to be established to control the robot to move a section of path, and in the moving process of therobot 200, the distance information between the rangingdevice 201 and at least one rangingbase station 101 is obtained in real time; meanwhile, laser point clouds of therobot 200 at two different moments (a position A and a position B) are obtained through the instant positioning andmap building sensor 202, the laser point clouds at the current moment are spliced, and the position of each ranging base station is marked at the same time, so that a target map is obtained. The mobile end computer can be, but is not limited to, various industrial personal computers, notebook computers, smart phones and tablet computers. According to the existing point cloud map and the position information of each ranging base station in the map, the current robot position can be obtained through current laser observation and ranging base station observation.

In one embodiment, the data fusion based on the external reference information and the distance information and the relative pose relationship acquired in the time period includes: constructing a first constraint function according to the external reference information and the distance information which is close to the actual distance between the ranging device and the ranging base station; constructing a second constraint function according to the fact that the relative pose relation is close to the actual pose change of the movable platform between two different moments; and performing joint optimization processing based on the first constraint function and the second constraint function to obtain the coordinates of the ranging base station in the coordinate system of the target map.

In this embodiment, the joint optimization process is to solve by using an optimization method. In the embodiment, the optimization method is used for solving by constructing two constraint functions, so that the accuracy of coordinate calculation of the ranging base station in the target map coordinate system can be improved.

In one embodiment, the performing optimization based on the first constraint function and the second constraint function to obtain the coordinates of the ranging base station in the coordinate system of the target map includes: and building a first optimization function and solving the first optimization function based on the accumulated calculation of the first constraint function and the second constraint function in the time period to obtain the coordinates of the ranging base station in the coordinate system of the target map.

In one embodiment, the constructing the first constraint function includes the following steps: obtaining a coordinate system of the distance measuring device and an external parameter conversion matrix of the coordinate system of the movable platform according to the external parameter information of the movable platform on which the distance measuring device is arranged; constructing a first conversion matrix function of a coordinate system of the movable platform and a coordinate system of the target map at a first moment; obtaining a second conversion matrix function of the coordinate system of the distance measuring device and the coordinate system of the target map at the first moment based on the external reference conversion matrix and the first conversion matrix function, and obtaining a second translation matrix function of the coordinate system of the distance measuring device to the target map at the first moment; constructing a third transformation matrix function of the coordinate system of at least one ranging base station and the coordinate system of the target map, and obtaining a third translation matrix function of the coordinate system of the ranging base station and the coordinate system of the target map; according to the fact that the difference of the translation amounts of the second translation matrix function and the third translation matrix function is close to the distance information, a first constraint function is constructed:

wherein epsilon₁(κ)_iA first constraint function representing the position of the kth distance measuring device at the time of ith, map representing the coordinate system of the target map, b representing the coordinate system of the movable platform, u_bA coordinate system representing said distance measuring device,

the extrinsic transformation matrix is represented as a function of,

a first transformation matrix function representing the ith time instant, ()_tA translation matrix is represented that represents the translation of the image,

representing said second translation matrix function at the i-th instant,

representing said third translation matrix function,

represents the distance between the ranging device at the ith time and the ranging base station at the kth time. Wherein u is_kRepresenting the coordinate system of the kth ranging base station.

In one embodiment, the constructing the second constraint function includes the following steps: constructing a fourth transformation matrix function of the coordinate system of the movable platform on the target map at the first moment; constructing a fifth transformation matrix function of the coordinate system of the movable platform on the target map at the second moment; obtaining a sixth conversion matrix function based on the fourth conversion function matrix and the fifth conversion function matrix; according to the fact that the relative pose relation of the movable platform between the first time and the second time is close to the sixth conversion matrix function, a second constraint function is constructed:

wherein epsilon₂Representing a second constraint function between the ith and jth instants with respect to said movable platform, map representing the coordinate system of said target map, b_iCoordinate system representing the movable platform at time i, b_jA coordinate system representing the movable platform at time j,

a transformation matrix representing the coordinate system of the movable platform and the coordinate system of said target map at time i,

a transformation matrix representing the coordinate system of the movable platform and the coordinate system of said target map at time j,

a transition matrix representing the movable platform at time j and time i.

In one implementation, the performing a joint optimization process based on the first constraint function and the second constraint function to obtain the coordinates of the ranging base station in the coordinate system of the target map includes the following steps: building a first optimization function based on the cumulative calculation of the first constraint function and the second constraint function in the time period:

wherein, Ε₁Representing a first optimization function, i representing an ith time instant, j representing a jth time instant; and solving the first optimization function to obtain a transformation matrix of the coordinate system of the ranging base station and the coordinate system of the target map, so as to obtain the coordinate of the ranging base station in the coordinate system of the target map.

In one embodiment, as shown in fig. 3, there is provided a positioning method based on data fusion of two sensors, applied to a movable platform equipped with a distance measuring device and an instant positioning and mapping sensor, where a distance measuring base station is preset in an environment around the movable platform, the method including:

s210, obtaining a map, wherein the position of the ranging base station is shown on the map.

Wherein the map is an environment map of the walking of the movable platform. The location of the ranging base station on the map is known.

S220, acquiring external reference information of the distance measuring device installed on the movable platform.

The distance measuring device is arranged on the external reference information of the movable platform and can be obtained in a calibration mode. The extrinsic information may be a transformation matrix of coordinates of the ranging device to coordinates of the movable platform coordinate system.

Wherein, movable platform refers to the equipment that can independently remove, including but not limited to automatic guide car, robot, autopilot car, unmanned aerial vehicle, intelligent house.

S230, obtaining distance information between the ranging device and at least one ranging base station; and meanwhile, environmental point cloud data is obtained through the instant positioning and map building sensor.

The distance measuring device and the distance measuring base station have wireless distance measuring functions, including but not limited to devices which adopt Bluetooth, WiFi and UWB technology for communication, and the like. The distance measuring device is arranged on the movable platform and moves along with the movable platform; the ranging base station is arranged on the map.

Preferably, the device communicates using UWB technology. Ultra Wide Band (UWB) technology is a wireless carrier communication technology. Which transmits data by sending and receiving extremely narrow pulses with nanosecond or subnanosecond order. UWB is susceptible to interference of other obstacles to cause disappearance of observed data or noise increase, and therefore UWB and laser combined positioning method is proposed. When the UWB communication equipment is adopted, the distance between the ranging device and the ranging base station can be calculated according to the time stamp in the communication process of the ranging device and the ranging base station.

The instant positioning and mapping sensor is used to scan surrounding environment information (specifically, environment point cloud data), for example, the instant positioning and mapping sensor may be a (slam sensor) sensor, which may include but is not limited to a laser radar, a camera, and an ultrasonic wave. The instant positioning and mapping sensor scans surrounding environment information in real time.

And S240, performing data fusion on the basis of the distance information, the external reference information, the environmental point cloud data and the map, and determining the current pose of the movable platform in the map.

And constructing an optimization function and solving the optimization function according to the fact that the difference between the translation amounts of the translation matrix from the ranging device to the map coordinate system and the translation matrix from the ranging base station to the map coordinate system is close to the distance information and the matching result based on the matching of the environment point cloud data and the target map is close to the coordinate of the environment point cloud data in the map coordinate system, and determining the current pose of the movable platform in the map.

According to the positioning method based on the data fusion of the two sensors, the environment point cloud data, the external reference information related to the distance measuring device on the movable platform and the distance information between the distance measuring device and the distance measuring base station are obtained by the instant positioning and map building sensor through the position of the distance measuring base station of the environment map, the position and the attitude of the movable platform are calculated, the movable platform is guaranteed to walk on the road, and the accurate position of the movable platform can be calculated according to the combination of the distance measuring base station and the positioning and map building sensor even if other lanes block the movable platform.

When the environment is a narrow and long environment, the environmental point cloud data collected by the instant positioning and mapping sensor is easy to be constrained and lost in a certain direction, and the coordinate system of the map is assumed to be in three directions of an X axis, a Y axis and a Z axis of a cartesian coordinate system (X, Y and Z). In a long and narrow environment, effective constraint cannot be performed on a certain direction, and constraint loss in the X direction is caused. The principle is similar to that of drawing construction and is not repeated.

In one embodiment, the step of determining the current pose of the movable platform based on the data fusion of the distance information, the external reference information, the first link point cloud data and the map includes: constructing a third constraint function according to the external reference information and the distance information which is close to the actual distance between the ranging device and the ranging base station; constructing a fourth constraint function according to the coordinate point set of the environment point cloud data close to the coordinate system of the map; and performing joint optimization processing based on the third constraint function and the fourth constraint function to obtain the current pose of the movable platform in the map.

In this embodiment, the joint optimization process is to solve by using an optimization method. According to the method, the two constraint functions are constructed to solve the optimization method, so that the accuracy of the pose of the movable platform in the map coordinate system can be improved.

In one embodiment, the component third constraint function includes the following steps: obtaining a coordinate system of the distance measuring device and an external parameter conversion matrix of the coordinate system of the movable platform according to the external parameter information of the distance measuring device arranged on the movable platform; constructing a seventh transformation matrix function of the coordinate system of the movable platform and the coordinate system of the map; obtaining a coordinate system of the distance measuring device and an eighth conversion matrix function of the coordinate system of the map according to the external reference conversion matrix and the seventh conversion matrix function, and obtaining an eighth translation matrix function from the distance measuring device to the coordinate system of the map; acquiring a transformation matrix of a coordinate system of at least one ranging base station and a coordinate system of the map to obtain a translation matrix of the coordinate system of the ranging base station and the coordinate system of the map; according to the distance information close to the difference between the eighth translation matrix function and the translation amount of the translation matrix of the coordinate system of the ranging base station and the translation matrix of the coordinate system of the map, constructing a third constraint function:

wherein epsilon₃(κ) a third constraint function based on the pose of the kth ranging base station with respect to the movable platform; map represents the coordinate system of the target map, b represents the coordinate system of the movable platform, u_bA coordinate system representing said distance measuring device,

the extrinsic transformation matrix is represented as a function of,^mapT^brepresents a seventh transfer matrix function, () t represents a translation matrix,

representing said eighth translation matrix function,

a coordinate system translation matrix representing a coordinate system of the ranging base station and the map,

represents the distance between the ranging device and the kth ranging base station.

In one embodiment, the constructing the fourth constraint function includes the following steps: according to the coordinate point set of the environment point cloud data close to the coordinate system of the map, constructing a fourth constraint function:

ε₄(x)＝^mapT^b·p_x-p_m,x

wherein map represents the coordinate system of the map, and b represents the coordinate system of the movable platform，^mapT^bTransformation matrix, p, representing the coordinate system of the movable platform and the coordinate system of said map_xRepresenting the coordinates of the xth current laser spot measured by the instant positioning and mapping sensor in the coordinate system of the movable platform, pm, x represents p_xA point in a set of coordinate points that is closest to a set of coordinate points in the map.

In one implementation, the performing joint optimization processing based on the third constraint function and the fourth constraint function to obtain the current pose of the movable platform in the map includes the following steps: and constructing a second optimization function according to the third constraint function and the fourth constraint function:

and solving the second optimization function to obtain a coordinate system of the movable platform and a transformation matrix of the coordinate system of the map, so as to obtain the current pose of the movable platform in the coordinate system of the map.

And obtaining the coordinate of the movable platform in the coordinate system of the map after obtaining the coordinate system of the movable platform and the transformation matrix of the coordinate system of the map.

It should be understood that although the various steps in the flow charts of fig. 2-3 are shown in order as indicated by the arrows, the steps are not necessarily performed in order as indicated by the arrows. The steps are not performed in the exact order shown and described, and may be performed in other orders, unless explicitly stated otherwise. Moreover, at least some of the steps in fig. 2-3 may include multiple steps or multiple stages, which are not necessarily performed at the same time, but may be performed at different times, which are not necessarily performed in sequence, but may be performed in turn or alternately with other steps or at least some of the other steps.

In one embodiment, as shown in fig. 4, there is provided a mapping apparatus based on data fusion of two sensors, applied to a movable platform equipped with a distance measuring apparatus and an instant positioning and mapping sensor, where a distance measuring base station is preset around the movable platform, the apparatus including: a first obtainingmodule 310, an external referenceinformation obtaining module 320 and adata fusion module 330, wherein:

a first obtainingmodule 310, configured to obtain, in real time, distance information between the ranging apparatus and at least one ranging base station during a moving process of the movable platform; meanwhile, the relative pose relationship of the movable platform at two different moments is obtained through the instant positioning and map building sensor.

An external referenceinformation obtaining module 320, configured to obtain external reference information of the distance measuring device installed on the movable platform.

And thedata fusion module 330 is configured to perform data fusion based on the external reference information and the distance information and the relative pose relationship acquired in a time period to obtain a target map.

In one embodiment, at least two ranging base stations are preset around the movable platform. The first obtainingmodule 310 is further configured to obtain, in real time, distance information between the ranging device and at least two of the ranging base stations during the moving process of the movable platform.

In one embodiment, thedata fusion module 320 includes: a first constraint function constructing unit, configured to construct a first constraint function according to the external parameter information and the distance information that is close to the actual distance between the ranging apparatus and the ranging base station; the second constraint function construction unit is used for constructing a second constraint function according to the fact that the relative pose relation is close to the actual pose change of the movable platform between two different moments; and the optimization processing unit is used for performing combined optimization processing on the basis of the first constraint function and the second constraint function to obtain the coordinates of the ranging base station in the coordinate system of the target map.

In one embodiment, the optimization processing unit is further configured to cumulatively calculate a first constraint function and a second constraint function based on the time period, construct a first optimization function, and solve the first optimization function to obtain coordinates of the ranging base station in the coordinate system of the target map.

In one embodiment, the constructing the first constraint function includes the following steps: obtaining a coordinate system of the distance measuring device and an external parameter conversion matrix of the coordinate system of the movable platform according to the external parameter information of the distance measuring device arranged on the movable platform; constructing a first conversion matrix function of a coordinate system of the movable platform and a coordinate system of the target map at a first moment; obtaining a second conversion matrix function of the coordinate system of the distance measuring device and the coordinate system of the target map at the first moment based on the external reference conversion matrix and the first conversion matrix function, and obtaining a second translation matrix function of the coordinate system of the distance measuring device to the target map at the first moment; constructing a third transformation matrix function of the coordinate system of at least one ranging base station and the coordinate system of the target map, and obtaining a third translation matrix function of the coordinate system of the ranging base station and the coordinate system of the target map; according to the fact that the difference of the translation amounts of the second translation matrix function and the third translation matrix function is close to the distance information, a first constraint function is constructed:

wherein epsilon₁(κ)_iA first constraint function representing the position of the kth distance measuring device at the time of ith, map representing the coordinate system of the target map, b representing the coordinate system of the movable platform, u_bA coordinate system representing said distance measuring device,

the extrinsic transformation matrix is represented as a function of,

a first transformation matrix function representing the ith time instant, ()_tA translation matrix is represented that represents the translation of the image,

representing said second translation matrix function at the i-th instant,

representing said third translation matrix function,

represents the distance between the ranging device at the ith time and the ranging base station at the kth time.

In one embodiment, the constructing the second constraint function includes the following steps: constructing a fourth transformation matrix function of the coordinate system of the movable platform on the target map at the first moment; constructing a fifth transformation matrix function of the coordinate system of the movable platform on the target map at the second moment; obtaining a sixth conversion matrix function based on the fourth conversion function matrix and the fifth conversion function matrix; according to the fact that the relative pose relation of the movable platform between the first time and the second time is close to the sixth conversion matrix function, a second constraint function is constructed:

wherein epsilon₂Representing a second constraint function between the ith and jth instants with respect to said movable platform, map representing the coordinate system of said target map, b_iCoordinate system representing the movable platform at time i, b_jA coordinate system representing the movable platform at time j,

a transformation matrix representing the coordinate system of the movable platform and the coordinate system of said target map at time i,

a transformation matrix representing the coordinate system of the movable platform and the coordinate system of said target map at time j,

a transition matrix representing the movable platform at time j and time i.

In one embodiment, the optimization processing unit includes: a first optimization function construction subunit, configured to construct a first optimization function based on the cumulative calculation of the first constraint function and the second constraint function in the time period:

wherein, Ε₁Representing a first optimization function, i representing an ith time instant, j representing a jth time instant; and the solving subunit is used for solving the first optimization function to obtain a transformation matrix of the coordinate system of the ranging base station and the coordinate system of the target map, so as to obtain the coordinates of the ranging base station in the coordinate system of the target map.

In one embodiment, as shown in fig. 5, there is provided a positioning apparatus based on data fusion of two sensors, applied to a movable platform equipped with a distance measuring apparatus and an instant positioning and mapping sensor, where a distance measuring base station is preset in an environment around the movable platform, the apparatus including: themap obtaining module 410, the external referenceinformation obtaining module 320, the second obtainingmodule 340 and the data fusion module 440, wherein:

amap obtaining module 410, configured to obtain a map, where the map shows the location of the ranging base station.

An external referenceinformation obtaining module 320, configured to obtain external reference information of the distance measuring device installed on the movable platform;

a first obtainingmodule 310, configured to obtain distance information between the ranging apparatus and at least one ranging base station.

And a second obtainingmodule 340, configured to obtain the environmental point cloud data through the instant positioning and mapping sensor.

And the data fusion module 440 is configured to perform data fusion based on the distance information, the external reference information, the environmental point cloud data and the map, and determine a current pose of the movable platform in the map.

In one embodiment, thedata fusion module 330 includes: a third constraint function constructing unit, configured to construct a third constraint function according to the external parameter information and the distance information that is close to the actual distance between the ranging apparatus and the ranging base station; the fourth constraint function building unit is used for building a fourth constraint function according to the coordinate point set of the environment point cloud data close to the coordinate system of the map; and the optimization processing unit is used for carrying out combined optimization processing on the basis of the third constraint function and the fourth constraint function to obtain the current pose of the movable platform in the map.

In one embodiment, the component third constraint function includes the following steps: obtaining a coordinate system of the distance measuring device and an external parameter conversion matrix of the coordinate system of the movable platform according to the external parameter information of the distance measuring device arranged on the movable platform; constructing a seventh transformation matrix function of the coordinate system of the movable platform and the coordinate system of the map; obtaining a coordinate system of the distance measuring device and an eighth conversion matrix function of the coordinate system of the map according to the external reference conversion matrix and the seventh conversion matrix function, and obtaining an eighth translation matrix function from the distance measuring device to the coordinate system of the map; acquiring a transformation matrix of a coordinate system of at least one ranging base station and a coordinate system of the map to obtain a translation matrix of the coordinate system of the ranging base station and the coordinate system of the map; according to the distance information close to the difference between the eighth translation matrix function and the translation amount of the translation matrix of the coordinate system of the ranging base station and the translation matrix of the coordinate system of the map, constructing a third constraint function:

wherein epsilon₃(κ) a third constraint function based on the pose of the kth ranging base station with respect to the movable platform; map represents the coordinate system of the target map, b represents the coordinate system of the movable platform, u_bA coordinate system representing said distance measuring device,

the extrinsic transformation matrix is represented as a function of,^mapT^brepresents a seventh transfer matrix function, () t represents a translation matrix,

representing said eighth translation matrix function,

a coordinate system translation matrix representing a coordinate system of the ranging base station and the map,

represents the distance between the ranging device and the kth ranging base station.

In one embodiment, the constructing the fourth constraint function includes the following steps: according to the coordinate point set of the environment point cloud data close to the coordinate system of the map, constructing a fourth constraint function:

ε₄(x)＝^mapT^b·p_x-p_m,x

wherein map represents the coordinate system of the map, b represents the coordinate system of the movable platform,^mapT^btransformation matrix, p, representing the coordinate system of the movable platform and the coordinate system of said map_xRepresenting the coordinates, p, of the xth current laser spot measured by the instant positioning and mapping sensor in the coordinate system of the movable platform_m,xRepresents a group of_xClosest of the mapsA point in the set of coordinate points of the set of coordinate points.

In one embodiment, the optimization processing unit includes: a second optimization function construction subunit, configured to construct a second optimization function according to the third constraint function and the fourth constraint function:

the transformation matrix solving subunit is used for solving the second optimization function to obtain a transformation matrix of the coordinate system of the movable platform and the coordinate system of the map; and the pose calculation subunit is used for calculating the current pose of the movable platform in the coordinate system of the map according to the coordinate system of the movable platform and the transformation matrix of the coordinate system of the map.

For the specific definition of the mapping device based on the fusion of the two sensor data, reference may be made to the above definition of the mapping method based on the fusion of the two sensor data, and for the specific definition of the positioning device based on the fusion of the two sensor data, reference may be made to the above definition of the positioning method based on the fusion of the two sensor data, which is not described herein again. The modules in the map building device based on the fusion of the two sensor data or the positioning device based on the fusion of the two sensor data can be wholly or partially realized by software, hardware and a combination thereof. The modules can be embedded in a hardware form or independent from a processor in the computer device, and can also be stored in a memory in the computer device in a software form, so that the processor can call and execute operations corresponding to the modules.

In one embodiment, a computer device is provided, which may be a terminal, and its internal structure diagram may be as shown in fig. 6. The computer device includes a processor, a memory, a communication interface, a display screen, and an input device connected by a system bus. Wherein the processor of the computer device is configured to provide computing and control capabilities. The memory of the computer device comprises a nonvolatile storage medium and an internal memory. The non-volatile storage medium stores an operating system and a computer program. The internal memory provides an environment for the operation of an operating system and computer programs in the non-volatile storage medium. The communication interface of the computer device is used for carrying out wired or wireless communication with an external terminal, and the wireless communication can be realized through WIFI, an operator network, NFC (near field communication) or other technologies. The computer program is executed by a processor to implement a mapping method based on the fusion of two sensor data or a positioning method based on the fusion of two sensor data. The display screen of the computer equipment can be a liquid crystal display screen or an electronic ink display screen, and the input device of the computer equipment can be a touch layer covered on the display screen, a key, a track ball or a touch pad arranged on the shell of the computer equipment, an external keyboard, a touch pad or a mouse and the like.

Those skilled in the art will appreciate that the architecture shown in fig. 6 is merely a block diagram of some of the structures associated with the disclosed aspects and is not intended to limit the computing devices to which the disclosed aspects apply, as particular computing devices may include more or less components than those shown, or may combine certain components, or have a different arrangement of components.

In one embodiment, a computer device is further provided, which includes a memory and a processor, the memory stores a computer program, and the processor implements the steps of the above method embodiments when executing the computer program.

In an embodiment, a computer-readable storage medium is provided, on which a computer program is stored which, when being executed by a processor, carries out the steps of the above-mentioned method embodiments.

It will be understood by those skilled in the art that all or part of the processes of the methods of the embodiments described above can be implemented by hardware instructions of a computer program, which can be stored in a non-volatile computer-readable storage medium, and when executed, can include the processes of the embodiments of the methods described above. Any reference to memory, storage, database or other medium used in the embodiments provided herein can include at least one of non-volatile and volatile memory. Non-volatile Memory may include Read-Only Memory (ROM), magnetic tape, floppy disk, flash Memory, optical storage, or the like. Volatile Memory can include Random Access Memory (RAM) or external cache Memory. By way of illustration and not limitation, RAM can take many forms, such as Static Random Access Memory (SRAM) or Dynamic Random Access Memory (DRAM), among others.

The technical features of the above embodiments can be arbitrarily combined, and for the sake of brevity, all possible combinations of the technical features in the above embodiments are not described, but should be considered as the scope of the present specification as long as there is no contradiction between the combinations of the technical features.

The above-mentioned embodiments only express several embodiments of the present application, and the description thereof is more specific and detailed, but not construed as limiting the scope of the invention. It should be noted that, for a person skilled in the art, several variations and modifications can be made without departing from the concept of the present application, which falls within the scope of protection of the present application. Therefore, the protection scope of the present patent shall be subject to the appended claims.

Claims (26)

1. A map building method based on data fusion of two sensors is applied to a movable platform for installing a distance measuring device and an instant positioning and map building sensor, and is characterized in that a distance measuring base station is preset in the environment around the movable platform, and the method comprises the following steps:

acquiring external parameter information of the distance measuring device arranged on the movable platform;

in the moving process of the movable platform, acquiring distance information between the distance measuring device and at least one distance measuring base station in real time; meanwhile, the relative pose relationship of the movable platform at two different moments is obtained through an instant positioning and map building sensor;

and performing data fusion based on the external reference information and the distance information and relative pose relationship acquired in a time period to obtain a target map.

2. The method of claim 1, further comprising:

when the environment is a long and narrow environment, at least two ranging base stations are preset in the long and narrow environment;

the real-time acquisition of the distance information between the ranging device and at least one ranging base station in the moving process of the movable platform comprises the following steps:

and in the moving process of the movable platform, the distance information between the distance measuring device and at least two distance measuring base stations is acquired in real time.

3. The method according to claim 1 or 2, wherein the data fusion based on the external reference information and the distance information and the relative pose relationship acquired in a time period comprises:

constructing a first constraint function according to the external reference information and the distance information which is close to the actual distance between the ranging device and the ranging base station;

constructing a second constraint function according to the fact that the relative pose relation is close to the actual pose change of the movable platform between two different moments;

and performing joint optimization processing based on the first constraint function and the second constraint function to obtain the coordinates of the ranging base station in the coordinate system of the target map.

4. The method of claim 3, wherein the performing optimization based on the first constraint function and the second constraint function to obtain the coordinates of the ranging base station in the coordinate system of the target map comprises:

and building a first optimization function and solving the first optimization function based on the accumulated calculation of the first constraint function and the second constraint function in the time period to obtain the coordinates of the ranging base station in the coordinate system of the target map.

5. The method of claim 3, wherein said constructing a first constraint function comprises the steps of:

obtaining a coordinate system of the distance measuring device and an external parameter conversion matrix of the coordinate system of the movable platform according to the external parameter information of the movable platform on which the distance measuring device is arranged;

constructing a first conversion matrix function of a coordinate system of the movable platform and a coordinate system of the target map at a first moment;

obtaining a second conversion matrix function of the coordinate system of the distance measuring device and the coordinate system of the target map at the first moment based on the external reference conversion matrix and the first conversion matrix function, and obtaining a second translation matrix function of the coordinate system of the distance measuring device to the target map at the first moment;

constructing a third transformation matrix function of the coordinate system of at least one ranging base station and the coordinate system of the target map, and obtaining a third translation matrix function of the coordinate system of the ranging base station and the coordinate system of the target map;

according to the fact that the difference of the translation amounts of the second translation matrix function and the third translation matrix function is close to the distance information, a first constraint function is constructed:

wherein epsilon₁(κ)_iA first constraint function representing the position of the kth distance measuring device at the time of ith, map representing the coordinate system of the target map, b representing the coordinate system of the movable platform, u_bA coordinate system representing said distance measuring device,

the extrinsic transformation matrix is represented as a function of,

a first transformation matrix function representing the ith time instant, ()_tA translation matrix is represented that represents the translation of the image,

representing said second translation matrix function at the i-th instant,

representing said third translation matrix function,

represents the distance between the ranging device at the ith time and the ranging base station at the kth time.

6. The method of claim 5, wherein said constructing a second constraint function comprises the steps of:

constructing a fourth transformation matrix function of the coordinate system of the movable platform on the target map at the first moment;

constructing a fifth transformation matrix function of the coordinate system of the movable platform on the target map at the second moment;

obtaining a sixth conversion matrix function based on the fourth conversion function matrix and the fifth conversion function matrix;

according to the fact that the relative pose relation of the movable platform between the first time and the second time is close to the sixth conversion matrix function, a second constraint function is constructed:

wherein epsilon₂Representing a second constraint function between the ith and jth instants with respect to said movable platform, map representing the coordinate system of said target map, b_iCoordinate system representing the movable platform at time i, b_jA coordinate system representing the movable platform at time j,

a transformation matrix representing the coordinate system of the movable platform and the coordinate system of said target map at time i,

a transformation matrix representing the coordinate system of the movable platform and the coordinate system of said target map at time j,

a transition matrix representing the movable platform at time j and time i.

7. The method according to claim 6, wherein the joint optimization processing is performed based on the first constraint function and the second constraint function to obtain the coordinates of the ranging base station in the coordinate system of the target map, and the method comprises the following calculation steps:

building a first optimization function based on the cumulative calculation of the first constraint function and the second constraint function in the time period:

wherein, Ε₁Representing a first optimization function, i representing an ith time instant, j representing a jth time instant;

solving the first optimization function to obtain a transformation matrix of a coordinate system of the ranging base station and a coordinate system of the target map; thereby obtaining the coordinates of the ranging base station in the coordinate system of the target map.

8. A positioning method based on data fusion of two sensors is applied to a movable platform for installing a distance measuring device and an instant positioning and map building sensor, and is characterized in that a distance measuring base station is preset in the environment around the movable platform, and the method comprises the following steps:

obtaining a map, wherein the map is provided with coordinates for presenting the ranging base station;

acquiring external parameter information of the distance measuring device arranged on the movable platform;

obtaining distance information between the ranging device and at least one ranging base station; meanwhile, environmental point cloud data are obtained through a real-time positioning and map building sensor;

and performing data fusion on the basis of the distance information, the external reference information, the environmental point cloud data and the map, and determining the current pose of the movable platform in the map.

9. The method of claim 8, wherein the step of determining the current pose of the movable platform based on the data fusion of the distance information, the external reference information, the first link point cloud data, and the map comprises:

constructing a third constraint function according to the external reference information and the distance information which is close to the actual distance between the ranging device and the ranging base station;

constructing a fourth constraint function according to the coordinate point set of the environment point cloud data close to the coordinate system of the map;

and performing joint optimization processing based on the third constraint function and the fourth constraint function to obtain the current pose of the movable platform in the map.

10. The method of claim 9, wherein said building a third constraint function comprises the steps of:

obtaining a coordinate system of the distance measuring device and an external parameter conversion matrix of the coordinate system of the movable platform according to the external parameter information of the distance measuring device arranged on the movable platform;

constructing a seventh transformation matrix function of the coordinate system of the movable platform and the coordinate system of the map;

obtaining a coordinate system of the distance measuring device and an eighth conversion matrix function of the coordinate system of the map according to the external reference conversion matrix and the seventh conversion matrix function, and obtaining an eighth translation matrix function from the distance measuring device to the coordinate system of the map;

acquiring a transformation matrix of a coordinate system of at least one ranging base station and a coordinate system of the map to obtain a translation matrix of the coordinate system of the ranging base station and the coordinate system of the map;

according to the distance information close to the difference between the eighth translation matrix function and the translation amount of the translation matrix of the coordinate system of the ranging base station and the translation matrix of the coordinate system of the map, constructing a third constraint function:

wherein epsilon₃(κ) a third constraint function based on the pose of the kth ranging base station with respect to the movable platform; map represents the coordinate system of the target map, b represents the coordinate system of the movable platform, u_bA coordinate system representing said distance measuring device,

the extrinsic transformation matrix is represented as a function of,^mapT^brepresents a seventh transfer matrix function, () t represents a translation matrix,

representing said eighth translation matrix function,

representing the distance measuring baseThe coordinate system of the station and the coordinate system translation matrix of said map,

represents the distance between the ranging device and the kth ranging base station.

11. The method of claim 10, wherein said constructing a fourth constraint function comprises the steps of:

according to the coordinate point set of the environment point cloud data close to the coordinate system of the map, constructing a fourth constraint function:

ε₄(x)＝^mapT^b·p_x-p_m,x

wherein map represents the coordinate system of the map, b represents the coordinate system of the movable platform,^mapT^btransformation matrix, p, representing the coordinate system of the movable platform and the coordinate system of said map_xRepresenting the coordinates, p, of the xth current laser spot measured by the instant positioning and mapping sensor in the coordinate system of the movable platform_m,xRepresents a group of_xA point in a set of coordinate points that is closest to a set of coordinate points in the map.

12. The method according to claim 11, wherein performing a joint optimization process based on the third constraint function and the fourth constraint function to obtain a current pose of the movable platform in the map comprises:

and constructing a second optimization function according to the third constraint function and the fourth constraint function:

solving a second optimization function to obtain a coordinate system of the movable platform and a transformation matrix of the coordinate system of the map;

thereby obtaining the current pose of the movable platform in the coordinate system of the map.

13. The utility model provides a build picture device based on two kinds of sensor data fusion, is applied to the movable platform of installation range unit and instant location and map construction sensor, its characterized in that, predetermines the range finding basic station around the movable platform, the device includes:

the first acquisition module is used for acquiring distance information between the distance measuring device and at least one distance measuring base station in real time in the moving process of the movable platform;

the second acquisition module is used for acquiring the relative pose relationship of the movable platform between two different moments through the instant positioning and map building sensor;

the external reference information acquisition module is used for acquiring external reference information of the distance measuring device arranged on the movable platform;

and the data fusion module is used for carrying out data fusion on the basis of the external reference information and the distance information and relative pose relation acquired in a time period to obtain a target map.

14. The apparatus of claim 1, wherein when the environment is an elongated environment, at least two ranging stations are preset in the elongated environment;

the first obtaining module is further configured to obtain distance information between the ranging device and at least two of the ranging base stations in real time during the moving process of the movable platform.

15. The apparatus of claim 13 or 14, wherein the data fusion module comprises:

a first constraint function constructing unit, configured to construct a first constraint function according to the external parameter information and the distance information that is close to the actual distance between the ranging apparatus and the ranging base station;

the second constraint function construction unit is used for constructing a second constraint function according to the fact that the relative pose relation is close to the actual pose change of the movable platform between two different moments;

and the optimization processing unit is used for performing combined optimization processing on the basis of the first constraint function and the second constraint function to obtain the coordinates of the ranging base station in the coordinate system of the target map.

16. The apparatus according to claim 15, wherein the optimization processing unit is further configured to construct a first optimization function based on the cumulative calculation of the first constraint function and the second constraint function in the time period, and solve the first optimization function to obtain the coordinates of the ranging base station in the coordinate system of the target map.

17. The apparatus of claim 15, wherein said constructing the first constraint function comprises the steps of:

obtaining a coordinate system of the distance measuring device and an external parameter conversion matrix of the coordinate system of the movable platform according to the external parameter information of the distance measuring device arranged on the movable platform;

constructing a first conversion matrix function of a coordinate system of the movable platform and a coordinate system of the target map at a first moment;

obtaining a second conversion matrix function of the coordinate system of the distance measuring device and the coordinate system of the target map at the first moment based on the external reference conversion matrix and the first conversion matrix function, and obtaining a second translation matrix function of the coordinate system of the distance measuring device to the target map at the first moment;

constructing a third transformation matrix function of the coordinate system of at least one ranging base station and the coordinate system of the target map, and obtaining a third translation matrix function of the coordinate system of the ranging base station and the coordinate system of the target map;

according to the fact that the difference of the translation amounts of the second translation matrix function and the third translation matrix function is close to the distance information, a first constraint function is constructed:

wherein epsilon₁(κ)_iA first constraint function representing the position of the kth distance measuring device at the time of ith, map representing the coordinate system of the target map, b representing the coordinate system of the movable platform, u_bA coordinate system representing said distance measuring device,

the extrinsic transformation matrix is represented as a function of,

a first transformation matrix function representing the ith time instant, ()_tA translation matrix is represented that represents the translation of the image,

representing said second translation matrix function at the i-th instant,

representing said third translation matrix function,

represents the distance between the ranging device at the ith time and the ranging base station at the kth time.

18. The apparatus of claim 17, wherein said constructing the second constraint function comprises the steps of:

constructing a fourth transformation matrix function of the coordinate system of the movable platform on the target map at the first moment;

constructing a fifth transformation matrix function of the coordinate system of the movable platform on the target map at the second moment;

obtaining a sixth conversion matrix function based on the fourth conversion function matrix and the fifth conversion function matrix;

according to the fact that the relative pose relation of the movable platform between the first time and the second time is close to the sixth conversion matrix function, a second constraint function is constructed:

wherein epsilon₂Representing a second constraint function between the ith and jth instants with respect to said movable platform, map representing the coordinate system of said target map, b_iCoordinate system representing the movable platform at time i, b_jA coordinate system representing the movable platform at time j,

a transformation matrix representing the coordinate system of the movable platform and the coordinate system of said target map at time i,

transformation matrix representing the coordinate system of the movable platform and the coordinate system of the target map at the j time

A transition matrix representing the movable platform at time j and time i.

19. The apparatus of claim 18, wherein the optimization processing unit comprises:

a first optimization function construction subunit, configured to construct a first optimization function based on the cumulative calculation of the first constraint function and the second constraint function in the time period:

wherein, Ε₁Representing a first optimization function, i representing an ith time instant, j representing a jth time instant;

and the solving subunit is used for solving the first optimization function to obtain a transformation matrix of the coordinate system of the ranging base station and the coordinate system of the target map, so as to obtain the coordinates of the ranging base station in the coordinate system of the target map.

20. The utility model provides a positioner based on two kinds of sensor data fusion, is applied to the movable platform of installation range unit and instant location and map construction sensor, its characterized in that presets the range finding basic station in the environment around the movable platform, the device includes:

the map acquisition module is used for acquiring a map, and the map is provided with coordinates for presenting the ranging base station;

the external reference information acquisition module is used for acquiring external reference information of the distance measuring device arranged on the movable platform;

a first obtaining module, configured to obtain distance information between the ranging apparatus and at least one ranging base station;

the second acquisition module is used for acquiring environmental point cloud data through the instant positioning and map building sensor;

and the data fusion module is used for carrying out data fusion on the basis of the distance information, the external reference information, the environmental point cloud data and the map and determining the current pose of the movable platform in the map.

21. The apparatus of claim 20, wherein the data fusion module comprises:

a third constraint function constructing unit, configured to construct a third constraint function according to the external parameter information and the distance information that is close to the actual distance between the ranging apparatus and the ranging base station;

the fourth constraint function building unit is used for building a fourth constraint function according to the coordinate point set of the environment point cloud data close to the coordinate system of the map;

and the optimization processing unit is used for carrying out combined optimization processing on the basis of the third constraint function and the fourth constraint function to obtain the current pose of the movable platform in the map.

22. The apparatus of claim 21, wherein said component third constraint function comprises the steps of:

obtaining a coordinate system of the distance measuring device and an external parameter conversion matrix of the coordinate system of the movable platform according to the external parameter information of the distance measuring device arranged on the movable platform;

constructing a seventh transformation matrix function of the coordinate system of the movable platform and the coordinate system of the map;

obtaining a coordinate system of the distance measuring device and an eighth conversion matrix function of the coordinate system of the map according to the external reference conversion matrix and the seventh conversion matrix function, and obtaining an eighth translation matrix function from the distance measuring device to the coordinate system of the map;

acquiring a transformation matrix of a coordinate system of at least one ranging base station and a coordinate system of the map to obtain a translation matrix of the coordinate system of the ranging base station and the coordinate system of the map;

according to the distance information close to the difference between the eighth translation matrix function and the translation amount of the translation matrix of the coordinate system of the ranging base station and the translation matrix of the coordinate system of the map, constructing a third constraint function:

wherein epsilon₃(κ) a third constraint function based on the pose of the kth ranging base station with respect to the movable platform; map represents the coordinate system of the target map, b represents the coordinate system of the movable platform, u_bA coordinate system representing said distance measuring device,

the extrinsic transformation matrix is represented as a function of,^mapT^brepresents a seventh transfer matrix function, () t represents a translation matrix,

representing said eighth translation matrix function,

a coordinate system translation matrix representing a coordinate system of the ranging base station and the map,

represents the distance between the ranging device and the kth ranging base station.

23. The apparatus of claim 22, wherein said constructing a fourth constraint function comprises:

according to the coordinate point set of the environment point cloud data close to the coordinate system of the map, constructing a fourth constraint function:

ε₄(x)＝^mapT^b·p_x-p_m,x

wherein map represents the coordinate system of the map, b represents the coordinate system of the movable platform,^mapT^btransformation matrix, p, representing the coordinate system of the movable platform and the coordinate system of said map_xRepresenting the coordinates, p, of the xth current laser spot measured by the instant positioning and mapping sensor in the coordinate system of the movable platform_m,xRepresents a group of_xA point in a set of coordinate points that is closest to a set of coordinate points in the map.

24. The apparatus of claim 23, wherein the optimization processing unit comprises:

a second optimization function construction subunit, configured to construct a second optimization function according to the third constraint function and the fourth constraint function:

the transformation matrix solving subunit is used for solving the second optimization function to obtain a transformation matrix of the coordinate system of the movable platform and the coordinate system of the map;

and the pose calculation subunit is used for calculating the current pose of the movable platform in the coordinate system of the map according to the coordinate system of the movable platform and the transformation matrix of the coordinate system of the map.

25. A computer device comprising a memory and a processor, the memory storing a computer program, characterized in that the processor when executing the computer program implements the steps of the two-sensor-data-fusion-based mapping method according to any one of claims 1 to 7 and the two-sensor-data-fusion-based localization method according to any one of claims 8 to 12.

26. A computer-readable storage medium, on which a computer program is stored, which, when being executed by a processor, carries out the steps of the mapping method based on two sensor data fusions of any one of claims 1 to 7 and the localization method based on two sensor data fusions of any one of claims 8 to 12.

Images