WO2021139590A1

WO2021139590A1 - Indoor localization and navigation apparatus based on bluetooth and slam, and method therefor

Info

Publication number: WO2021139590A1
Application number: PCT/CN2020/141624
Authority: WO
Inventors: 雷鸣; 杜珣弤
Original assignee: 三个机器人公司; 雷鸣
Priority date: 2020-01-06
Filing date: 2020-12-30
Publication date: 2021-07-15
Also published as: CN113074727A

Abstract

An indoor localization and navigation apparatus (100), comprising: a Bluetooth module (111), configured to acquire Mesh network information of the indoor localization and navigation apparatus (100), and acquire addresses, attributes, RSSI, IQ data, angles, and arrival times of a plurality of Bluetooth nodes in a Mesh network; a vision module (112), configured to acquire an image around a robot; an odometer module (113), configured to estimate the position of the robot in real time; a data storage and processing module (114), configured to receive data information acquired and calculated by the Bluetooth module (111), the vision module (112), and the odometer module (113); a position integration and estimation module (115), configured to integrate data in the data storage and processing module (114); a map construction module (116), configured to create a three-dimensional environment map according to spatial position information of the Bluetooth nodes and feature points; and a path planning and motion control module (117), configured to drive the robot and perform path planning and navigation according to the created three-dimensional environment map. The localization and navigation apparatus (100) can effectively improve the localization and navigation accuracy.

Description

Indoor positioning and navigation device and method based on Bluetooth and SLAM

Technical field

This application relates to an indoor positioning and navigation device and method based on Bluetooth and SLAM.

Background technique

With the development of mobile Internet technology, smart mobile terminal technology and Internet of Things technology, people's demand for indoor positioning services is becoming more and more diversified, making indoor positioning and navigation technology a research hotspot in the field of intelligent positioning services. SLAM (simultaneous localization and mapping) technology is a classic problem in the robotics field. Generally, the SLAM problem can be described as: the robot starts to move from an unknown location in an unknown environment, and is based on the location estimation and map during the movement. Self-positioning, and at the same time, an incremental map is built on the basis of self-positioning to realize autonomous positioning and navigation of the robot. At present, indoor positioning and navigation technology uses a variety of physical sensors to achieve positioning and navigation, such as cameras, IMUs, etc. In the complex indoor environment, it is susceptible to environmental factors or the sensor itself is low in accuracy, resulting in low positioning accuracy. For example, the factors that are affected by each sensor are different. The camera is illuminated, and the camera cannot obtain feature points in the dark. The IMU is an external factor, such as man-made drag and ground slippage. There will be cumulative errors in long-term work.

Summary of the invention

In some embodiments, the present application discloses an indoor positioning and navigation device, wherein the indoor positioning and navigation device is built in a movable intelligent platform or robot, and includes: a Bluetooth module for obtaining information about the indoor positioning and navigation device Mesh network information, obtain the addresses, attributes, RSSI (Received Signal Strength Indication, received signal strength indication), IQ (In-phase Quadrature, same direction orthogonal) data, angle, and arrival time of multiple Bluetooth nodes in the Mesh network. The vision module is used to collect images around the robot, and perform grayscale processing, image correction, and feature point extraction and feature line segment extraction on the image. The odometer module is used to calculate the change of the position of the robot at each time relative to the position of the previous time through relative positioning, and to estimate the position of the robot in real time. The data storage and processing module is used to receive the data information collected and calculated by the Bluetooth module, the vision module and the odometer module. The position fusion and estimation module is used to fuse the data in the data storage and processing module to obtain the real-time position of the indoor positioning and navigation device. The map building module is used to store the real-time location of the indoor positioning and navigation device estimated by the location fusion and estimation module, and the three-dimensional environment map created based on the spatial location information of the Bluetooth nodes and feature points in the data storage and processing module, as well as path planning and The motion control module is used to drive the robot and perform path planning and navigation according to the three-dimensional environment map created by the map building module.

In some embodiments, this application discloses an indoor positioning and navigation method, including: calibrating a camera in a vision module; acquiring data collected by a Bluetooth module, a vision module, and an odometer module; performing Kalman filtering on the collected data; The real-time position of the indoor positioning and navigation device, the spatial position of the Bluetooth node, and the feature point are calculated to create a three-dimensional environment map, and positioning and navigation are performed according to the created three-dimensional environment map.

In some embodiments, a computer device is provided, including a memory and one or more processors, and computer-readable instructions are stored in the memory. When the computer-readable instructions are executed by the processor, the The one or more processors execute or implement the indoor positioning and navigation methods disclosed in some embodiments of the present application.

In some embodiments, one or more non-volatile storage media storing computer-readable instructions are provided. When the computer-readable instructions are executed by one or more processors, the one or more processors execute or implement The indoor positioning and navigation method disclosed in some embodiments of the present application.

In some embodiments, a computer program is provided, which, when executed by a processor, implements the indoor positioning and navigation method disclosed in some embodiments of the present application.

The details of one or more embodiments of the present application are set forth in the following drawings and description. Other features and advantages of this application will become apparent from the description, drawings and claims.

Description of the drawings

In order to more clearly describe the technical solutions in the embodiments of the present application, the following will briefly introduce the drawings needed in the embodiments. Obviously, the drawings in the following description are only some embodiments of the present application. A person of ordinary skill in the art can obtain other drawings based on these drawings without creative work.

Fig. 1 is a block diagram of an indoor positioning and navigation device based on Bluetooth and SLAM provided by one or more embodiments of the present application.

Fig. 2 is a working flow chart of the vision module in the indoor positioning and navigation device based on Bluetooth and SLAM provided by one or more embodiments of the present application.

Fig. 3 is a flowchart of Kalman filtering of the position fusion and estimation module in the Bluetooth and SLAM indoor positioning and navigation device provided by one or more embodiments of the present application.

FIG. 4 is a positioning model diagram of a Bluetooth module in an indoor positioning and navigation device based on Bluetooth and SLAM provided by one or more embodiments of the present application.

Fig. 5 is a motion model diagram of an odometer module in an indoor positioning and navigation device based on Bluetooth and SLAM provided by one or more embodiments of the present application.

[Corrected according to Rule 91 04.03.2021]
Fig. 6 is a working flow chart of an indoor positioning and navigation device based on Bluetooth and SLAM provided by one or more embodiments of the present application.
FIG. 7 is a diagram of the internal structure of a computer device provided by one or more embodiments of the present application.

Detailed ways

In order to make the technical solutions and beneficial effects of the present application clearer, the following further describes the present application in detail with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described here are only used to explain the present application, and are not used to limit the present application.

FIG. 1 is a block diagram of an indoor positioning and navigation device 100 based on Bluetooth and SLAM provided by an embodiment of the present application. As shown in FIG. 1, the indoor positioning and navigation device 100 based on Bluetooth and SLAM includes: a Bluetooth module 111, a vision module 112, an odometer module 113, a data storage and processing module 114, a position fusion and estimation module 115, a map construction module 116, and a path Planning and Motion Control 117. The indoor positioning and navigation device 100 can be built into a sweeping robot or an indoor robot, and is used to implement map construction and precise positioning and navigation of the robot.

As shown in FIG. 1, the Bluetooth module 111 is a set of basic circuits of a chip that integrates Bluetooth functions, which can be used for wireless network communication, and can realize data transmission and audio transmission functions. The Bluetooth module 111 is located in the indoor positioning and navigation device 100 and has intelligent networking and positioning functions, specifically including the realization of networking and positioning of Bluetooth devices in the Bluetooth network. The Bluetooth module 111 obtains the Mesh network where the indoor positioning and navigation device 100 is located, the address, attributes, RSSI (Received Signal Strength Indication, received signal strength indication), IQ (In-phase Quadrature, same direction orthogonality) of the Bluetooth node in the Bluetooth network ) Data, angle (including direction angle and elevation angle), time of arrival and other information. The unit of RSSI is dbm, which is generally a negative value, which can reflect the distance between two Bluetooth nodes in the Bluetooth network. The address information of the Bluetooth node is an identifier of the Bluetooth node, expressed in bytes; the attributes of the Bluetooth node include: friendly nodes, low-power nodes, relay nodes, standard nodes, and proxy nodes.

Further, the Bluetooth module 111 obtains TOA (Time Of Arrive), RSSI, IQ data, and AOA (Angle Of Arrive) from surrounding Bluetooth nodes to the robot (including the indoor positioning and navigation device 100) through the positioning function. The Bluetooth module 111 and the RSSI ranging model and the phase difference ranging model are constructed based on the TOA, RSSI, phase and AOA positioning model equations, as follows:

Among them, (x, y, z) represents the spatial position of the indoor positioning and navigation device 100, the spatial position of the Bluetooth node B _i is (X _bi , Y _bi , Z _bi ), and θ _i is the Bluetooth node B _i and indoor positioning and navigation The azimuth angle between the bluetooth antenna array planes of the device 100,

Is the elevation angle between the Bluetooth node B _i and the Bluetooth antenna array plane of the indoor positioning and navigation device 100, D _1i is the distance between the indoor positioning and navigation device 100 and the Bluetooth device B _i obtained by the RSSI ranging model, and D _2i is determined by The distance between the indoor positioning and navigation device 100 and the Bluetooth device B _i obtained by the TOA ranging model, where D _3i is the distance between the indoor positioning and navigation device 100 and the Bluetooth device B _i obtained by the phase difference ranging model. By obtaining the best solution of the above positioning model by the least square method, _{the spatial position of the Bluetooth node B i} relative to the indoor positioning and navigation device 100 can be calculated, that is, the value of (x, y, z). Specifically about the parameter θ _i ,

Refer to Figure 4 for D _1i , D _2i and D _3i. FIG. 4 is a positioning model diagram of a Bluetooth module in an indoor positioning and navigation device based on Bluetooth and SLAM provided by an embodiment of the present application. _{_{(X bi, Y bi, Z}} bi) coordinates of a representative node B _i Bluetooth network, B _'i B _i is a Bluetooth Node projected on a two-dimensional plane.

The vision module 112 is used to collect images around the robot, and perform grayscale processing, image correction, and feature point extraction and feature line segment extraction on the collected images, that is, preprocessing the image to remove lens distortion. The influence of feature description, and the ORB (Oriented Fast and Rotated Brief) feature point detection method is used to extract feature points and feature line segments in the image. Specifically, the wide-angle camera in the vision module 112 is used to collect image information of the environment where the robot is located. In this application, a 12*9 10mm*10mm black and white checkerboard calibration board is used to calibrate the wide-angle camera in the vision module 112 of the indoor positioning and navigation device 100. The calibration method can avoid the shortcomings of high equipment requirements and cumbersome operation of the traditional method, and has higher accuracy compared with the current calibration method. The vision module 112 obtains the camera internal parameters and distortion parameter information. For example, the camera internal parameters are the horizontal focal length fx, the vertical focal length fy, the principal point abscissa u0, and the principal point ordinate v0, and the distortion parameters include radial distortion parameters k1, k2, and k2. k3 and tangential distortion parameters p1 and p2 include but are not limited to these.

The odometer module 113 is used to calculate the change of the position of the robot at each time relative to the position of the previous time through relative positioning, so as to realize the real-time estimation of the position. That is, calculate the X-direction offset Δx, the Y-direction offset Δy, and the Z-direction angular offset Δφ relative to the previous time at each time of the robot. As shown in Figure 5, Figure 5 is a motion model diagram of the odometer module in the Bluetooth and SLAM-based indoor positioning and navigation device provided by an embodiment of the present application. X(k-1) is the state quantity at time k-1, X(k) is the state quantity at the next time, that is, the state quantity at time k, then u(k) is the offset of time k with respect to time k-1, The values of u(k) are the X-direction offset Δx, the Y-direction offset Δy, and the Z-direction angular offset Δφ. The data storage and processing module 115 is coupled with the Bluetooth module 111, the vision module 112, and the odometer module 113, and is configured to receive data information collected and calculated by the Bluetooth module 111, the vision module 112, and the odometer module 113. Specifically, in addition to the attributes, address, RSSI, IQ data, angle (elevation and azimuth), arrival time and other information of the Bluetooth node collected by the above-mentioned Bluetooth module, these data information also include the wide-angle camera internal parameters and distortion parameters collected by the vision module. Information, feature point image coordinates and description, feature line segment and description, etc., as well as the position change of the intelligent platform or robot collected by the odometer module. The data calculated by the above data information includes the spatial position of the Bluetooth node, the spatial position of the feature point, and the real-time updated intelligent platform or robot position information obtained by the position fusion and estimation module.

The location fusion and estimation module 115 is coupled with the data storage and processing module 114 to fuse the data in the data storage and processing module 114. The fusion algorithms include least squares method, LM algorithm, BA algorithm, and intelligent optimization algorithm (such as Genetic algorithm, particle swarm algorithm, ant colony algorithm, etc.), Kalman filter algorithm, etc. This application uses one of these algorithms, such as Kalman filter as an example, for implementation and description to obtain the real-time position of the indoor positioning and navigation device 100. After Kalman filtering, real-time accurate positioning of the indoor positioning and navigation device 100 can be realized. Those skilled in the art should understand that this embodiment cannot be used as a limitation of the application

The map construction module 116 is coupled with the position fusion and estimation module 115, and is used to store the real-time position of the indoor positioning and navigation device 100 estimated by the position fusion and estimation module 115, and the space of Bluetooth nodes and feature points in the data storage and processing module 114 A three-dimensional map of the environment created by the location.

The path planning and motion control 117 is coupled with the map construction module 116, and is used for path planning and navigation according to the three-dimensional environment map stored in the map construction module 116. The path planning and motion control 117 is used to drive the robot accordingly, and obtain the current position information of the robot in real time. Each module is optionally implemented as a non-transitory computer-readable medium containing logic, stored instructions, firmware, and/or a combination thereof. For the logic implemented with stored instructions and/or solid state, a processor may be provided to execute such instructions so that the existing indoor positioning and navigation device executes the method described herein.

Fig. 2 is a working flow chart of the vision module in the Bluetooth and SLAM indoor positioning and navigation device provided by an embodiment of the present application. It includes the following steps:

Step 202: The wide-angle camera in the vision module 112 collects image information around the indoor positioning and navigation device 100.

Step 204: The vision module 112 preprocesses the collected images. Including the gray-scale processing of the image.

Step 206: The vision module 112 corrects the image distortion.

Step 208: Extract the feature point information in the image information, and the method used is the ORB feature extraction that comes with OpenCV. This method is characterized by fast calculation speed, and has certain characteristics of anti-noise and anti-rotation. After processing the image using the ORB feature extraction method, a series of feature point data can be obtained, and the feature information will be stored in the feature database. The ORB feature points in the image include lights, wall corners, etc.

Step 210: Extract a characteristic line segment in the image information. The characteristic line segment is a 1sd line segment, which is generally an edge of a wall or an edge line of an object with a certain length, such as an edge line of a square lamp, etc.

Fig. 3 is a flowchart of the Kalman filtering of the position fusion and estimation module in the Bluetooth and SLAM-based indoor positioning and navigation device provided by an embodiment of the present application. Kalman filtering mainly includes the following steps:

Step 302: sub-module initialization, specifically refers to the definition and initialization of variables and matrices involved in Kalman filtering.

Step 304: The position fusion and estimation module establishes a motion equation and an observation equation. Specifically:

_{The posture change U k+1} (Δx(k+1),Δy(k+1),Δφ(k+1)) of the indoor positioning and navigation device 100 at k+1 time is obtained by the mileage module 114 data. The system state X(k) at time k calculates the system state X(k+1|k) at time k+1:

X(k+1|k)=X(k)+U _k+1 +Q _k+1 …………(2)

Among them, Q _k+1 is the motion equation noise at k+1 time, this noise is determined by the positioning accuracy of the odometer module itself; U _k+1 is the attitude change at k+1 time.

Further, according to the system state X(k+1|k) of the indoor positioning and navigation device 100 at time k+1, the system observation at time k+1 is estimated

which is

among them,

Among them, B _i represents a Bluetooth node in the Bluetooth network, and its coordinates are (X _bi , Y _bi , Z _bi ), which is relative to the system state quantity X(k+1| k) azimuth angle θ _i and elevation angle

F _i represents one of the feature points. The three-dimensional coordinates of the feature point are (X _fi , Y _fi , Z _fi ), f is the focal length of the camera, W _k+1 is the noise of the observation equation at k+1, and H is The Jacobian matrix of the observation equation versus the state quantity.

Step 306: Update the covariance equation, the calculation formula is as follows:

P(k+1)=(P(k+1|k) ^-1 +H ^T *R ^-1 *H) ^-1 …………(4)

Among them, P(k+1) is the covariance matrix at k+1, and R is the observed noise covariance matrix;

Step 308: Update the gain matrix, the calculation formula is as follows:

K=P(k+1)*H ^T *R ^-1 …………(5)

Among them, K is the gain matrix;

Step 310: Update the state vector, the calculation formula is as follows:

X(k+1)=X(k+1|k)+K*ΔL…………(6)

Where X(k+1) is the state vector at time k+1,

L _k+1 is the real observation at k+1,

_{_{_{f i (u i, v i}}} ) is the feature point in time k + 1 _{_{_{F i (X fi, Y fi}}} , Z fi) corresponding to the image coordinates. Due to the problems of wheel slipping, dragging or cumulative error, the position obtained only by the equation of motion will be wrong. Therefore, the indoor positioning and navigation device 100 uses other indoor information, such as feature points, and the observation equation composed of Bluetooth nodes to correct it. Improve the accuracy of calculations.

Fig. 6 is a working flow chart of an indoor positioning and navigation device based on Bluetooth and SLAM provided by an embodiment of the present application. FIG. 6 will be described in conjunction with FIG. 1. Specifically include the following steps:

Step 602: Calibrate the camera in the vision module 112.

Step 604: Obtain data collected by the Bluetooth module 111, the vision module 112, and the mileage module 113.

Step 606: Fusion of the collected data. When Kalman filter fusion is used in an embodiment, refer to the method flowchart of FIG. 3 for specific filtering steps.

Step 608: The map construction module 116 creates a three-dimensional environment map according to the calculated real-time position of the indoor positioning and navigation device, the spatial position of the Bluetooth node, and the feature point.

Step 610: The path planning and motion control module 117 performs positioning and navigation according to the created three-dimensional environment map.

Advantageously, the Bluetooth and SLAM-based indoor positioning and navigation device and method of the present application can collect mileage information by detecting and tracking ORB feature points, Bluetooth node information, and odometer, and constructing a scene map through Kalman filtering for accurate robot accuracy. Positioning, path planning and navigation can greatly improve positioning accuracy. Compared with the existing indoor positioning and navigation technology, the method disclosed in the present application can not only effectively reduce the influence caused by environmental factors to improve the positioning accuracy, but also provide a more reliable and accurate three-dimensional environment map.

In some embodiments, the various modules in the aforementioned indoor positioning and navigation device include a Bluetooth module, a vision module, an odometer module, a data storage and processing module, a position fusion and estimation module, a map construction module, and a path planning and motion control module, It can be implemented in whole or in part by software, hardware and a combination thereof. The above-mentioned modules may be embedded in the form of hardware or independent of the processor in the computer equipment, or may be stored in the memory of the computer equipment in the form of software, so that the processor can call and execute the operations corresponding to the above-mentioned modules.

In some embodiments, a computer device is provided. The computer device may be a movable intelligent platform or a robot, and its internal structure diagram may be as shown in FIG. 7. The computer equipment includes a processor, a memory, a network interface, an input device, a Bluetooth device and a driving device connected through a system bus. Among them, the processor of the computer device is used to provide calculation and control capabilities. The memory of the computer device includes a non-volatile storage medium and an internal memory. The non-volatile storage medium stores an operating system and computer readable instructions. The internal memory provides an environment for the operation of the operating system and computer-readable instructions in the non-volatile storage medium. The network interface of the computer device is used to communicate with an external terminal or server through a network connection. When the computer-readable instructions are executed by the processor, an indoor positioning and navigation method is realized. The input device of the computer equipment can be a touch layer covered on the display screen, it can also be a button, a trackball or a touchpad set on the housing of the computer equipment, and it can also be an external keyboard, a touchpad or a mouse. The Bluetooth device of the computer device can communicate with other devices with Bluetooth communication capabilities, and the motion device of the computer device can be any form of device that can drive the computer device to move. In addition, the computer device may also include a display screen, and the display screen of the computer device may be a liquid crystal display screen or an electronic ink display screen,

Those skilled in the art can understand that the structure shown in FIG. 7 is only a block diagram of a part of the structure related to the solution of the present application, and does not constitute a limitation on the computer device to which the solution of the present application is applied. The specific computer device may Including more or fewer parts than shown in the figure, or combining some parts, or having a different arrangement of parts.

In some embodiments, a computer device is provided, including a memory and one or more processors, and computer-readable instructions are stored in the memory. When the computer-readable instructions are executed by the processor, the The one or more processors execute a method, including: calibrating the camera in the vision module; acquiring data collected by the Bluetooth module, vision module, and odometer module; fusing the collected data; obtaining the indoor positioning and navigation device according to the calculation Create a three-dimensional environment map based on the real-time location, Bluetooth node, and spatial location of feature points; and perform positioning and navigation based on the created three-dimensional environment map.

In some embodiments, one or more computer program products are provided, including a storage medium, and the storage medium stores computer-readable instructions. When the computer-readable instructions are executed by one or more processors, the one or more processors Implementing a method includes: calibrating the camera in the vision module; acquiring data collected by the Bluetooth module, vision module, and odometer module; fusing the collected data; obtaining the real-time position, Bluetooth node, and characteristics of the indoor positioning and navigation device according to calculations Create a three-dimensional environment map based on the spatial position of the point; and perform positioning and navigation based on the created three-dimensional environment map.

In some embodiments, one or more non-volatile storage media storing computer-readable instructions are provided. When the computer-readable instructions are executed by one or more processors, the one or more processors execute a The method includes: calibrating the camera in the vision module; acquiring the data collected by the Bluetooth module, the vision module and the odometer module; fusing the collected data; obtaining the real-time position of the indoor positioning navigation device, the Bluetooth node, and the space of the feature point according to the calculation Create a three-dimensional environment map based on the location; and perform positioning and navigation based on the created three-dimensional environment map.

In some embodiments, one or more computer programs are provided, which execute a method when executed by one or more processors, including: calibrating the camera in the vision module; acquiring the Bluetooth module, the vision module, and the odometer module. Integrate the collected data; create a three-dimensional environment map based on the calculated real-time position of the indoor positioning and navigation device, the spatial location of the Bluetooth node, and the feature point; and perform positioning and navigation based on the created three-dimensional environment map.

In some embodiments, the method executed by the one or more processors is the indoor positioning and navigation method in some embodiments of the present application.

A person of ordinary skill in the art can understand that all or part of the processes in the above-mentioned embodiment methods can be implemented by instructing relevant hardware through a computer program. The computer program can be stored in a non-volatile computer readable storage. In the medium, when the computer program is executed, it may include the processes of the above-mentioned method embodiments. Wherein, any reference to memory, storage, database, or other media used in the embodiments provided in this application may include non-volatile and/or volatile memory. Non-volatile memory may include read only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM), or flash memory. Volatile memory may include random access memory (RAM) or external cache memory. As an illustration and not a limitation, RAM is available in many forms, such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), double data rate SDRAM (DDRSDRAM), enhanced SDRAM (ESDRAM), synchronous chain Channel (Synchlink) DRAM (SLDRAM), memory bus (Rambus) direct RAM (RDRAM), direct memory bus dynamic RAM (DRDRAM), and memory bus dynamic RAM (RDRAM), etc.

It should be understood that although the various steps in the flowcharts of FIGS. 2, 3, and 6 are displayed in sequence as indicated by the arrows, these steps are not necessarily performed in sequence in the order indicated by the arrows. Unless specifically stated in this article, the execution of these steps is not strictly limited in order, and these steps can be executed in other orders. Moreover, at least some of the steps in Figures 2, 3, and 6 may include multiple sub-steps or multiple stages. These sub-steps or stages are not necessarily executed at the same time, but can be executed at different times. These sub-steps Or the execution order of the stages is not necessarily carried out sequentially, but may be executed alternately or alternately with at least a part of other steps or sub-steps or stages of other steps.

In some embodiments, the indoor positioning and navigation device and method of the present application can construct a scene map based on Bluetooth node information, image feature points, feature line segment information, and real-time location information estimated by the odometer in the Mesh network, and can be used for robots Precise positioning and path planning. In some embodiments, in order to solve the problem of positioning accuracy of indoor positioning services, multiple positioning sensors can be used for data fusion, which has strong anti-interference ability, realizes higher-precision positioning, and effectively reduces the influence caused by environmental factors to improve positioning. Accuracy, and can provide a more reliable and accurate three-dimensional environment map, so that it can be extended and applied more widely. For example, Bluetooth is not affected by light and slippage, but it will be blocked by obstacles such as walls and tables, resulting in low positioning accuracy. Therefore, in some embodiments, multiple positioning such as Bluetooth, camera, and odometer can be used. Combining the advantages of sensors, learn from each other's strengths. In some embodiments, the device or method of the present application constructs a scene map using Bluetooth node information, feature points and feature information collected by the vision module, and real-time location information estimated by the odometer module, so as to achieve precise positioning and navigation functions.

The technical features of the above embodiments can be combined arbitrarily. In order to make the description concise, all possible combinations of the technical features in the above embodiments are not described. However, as long as there is no contradiction in the combination of these technical features, they should all be combined. It is considered as the range described in this specification. The above descriptions are only the preferred embodiments of this application and are not intended to limit this application. Any modification, equivalent replacement and improvement made within the spirit and principle of this application shall be included in the protection of this application. Within range.

Claims

An indoor positioning and navigation device, wherein the indoor positioning and navigation device is built in a movable intelligent platform or robot, and includes:

The Bluetooth module is used to obtain the Mesh network information of the indoor positioning and navigation device, and obtain the addresses, attributes, RSSI, IQ data, angles, and arrival time of multiple Bluetooth nodes in the Mesh network;

The vision module is used to collect images around the robot, and perform grayscale processing, image correction, and feature point extraction and feature line segment extraction on the image;

The odometer module is used to calculate the change of the position of the robot at each time relative to the position of the previous time through relative positioning, and to estimate the position of the robot in real time;

The data storage and processing module is used to receive the data information collected and calculated by the Bluetooth module, the vision module and the odometer module;

The position fusion and estimation module is used to fuse the data in the data storage and processing module to obtain the real-time position of the indoor positioning and navigation device;

The map building module is used to store the real-time location of the indoor positioning and navigation device estimated by the location fusion and estimation module, as well as a three-dimensional environment map created based on the spatial location information of the Bluetooth nodes and feature points in the data storage and processing module, and

The path planning and motion control module is used to drive the robot and perform path planning and navigation according to the three-dimensional environment map created by the map building module.
An indoor positioning and navigation method, including:

Calibration of the camera in the vision module;

Obtain the data collected by the Bluetooth module, the vision module and the odometer module;

Fusion of collected data;

Create a three-dimensional environment map based on the calculated real-time position of the indoor positioning and navigation device, the spatial position of the Bluetooth node, and the feature point; and

Perform positioning and navigation according to the created three-dimensional environment map.
The method according to claim 2, wherein the Bluetooth module obtains Mesh network information of the indoor positioning and navigation device, and obtains addresses, attributes, RSSI, IQ data, angles, and arrival times of multiple Bluetooth nodes in the Mesh network.
The method according to any one of claims 2 or 3, wherein the vision module collects images around the robot, and performs grayscale processing, image correction, and feature point extraction and feature line segment extraction on the image.
The method according to any one of claims 2-4, wherein the odometer module calculates the change of the position of the robot at each time relative to the position of the previous time through relative positioning, and estimates the position of the robot in real time.
The method according to any one of claims 2-5, wherein the data fusion is fusion of the data through a position fusion and estimation module.
The device or method according to any one of claims 1-6, wherein the Bluetooth module and the RSSI ranging model and the phase difference ranging model construct a positioning model equation based on TOA, RSSI, phase, and AOA, and pass the equation

Calculate the real-time position of the indoor positioning and navigation device, where (x, y, z) represents the spatial position of the indoor positioning and navigation device, the spatial position of Bluetooth node B i is (X bi , Y bi , Z bi ), and θ i is Bluetooth The azimuth angle between node B i and the bluetooth antenna array plane of the indoor positioning and navigation device,
Is the elevation angle between the Bluetooth node B i and the Bluetooth antenna array plane of the indoor positioning and navigation device, D 1i is the distance between the indoor positioning and navigation device and the Bluetooth device B i obtained by the RSSI ranging model, and D 2i is the distance measured by TOA The distance between the indoor positioning and navigation device and the Bluetooth device B i obtained by the model, D 3i is the distance between the indoor positioning and navigation device and the Bluetooth device B i obtained by the phase difference ranging model.
The device, method, non-volatile storage medium or program according to any one of claims 1-7, wherein the camera in the vision module is calibrated by a 12*9 10mm*10mm black and white checkerboard calibration board, optional Preferably, the camera in the vision module includes camera parameter and distortion parameter information.
The indoor positioning and navigation device, method, non-volatile storage medium or program according to any one of claims 1-8, wherein the vision module is also used to correct image distortion.
The device or method according to any one of claims 1-9, wherein said fusing data includes:

Perform fusion based on at least one selected from the group consisting of least squares method, LM algorithm, BA algorithm, intelligent optimization algorithm, and Kalman filter algorithm,

The Kalman filter algorithm includes at least one selected from the group consisting of initializing each module, establishing a motion equation, an observation equation, updating a covariance matrix, updating a gain matrix, and updating a state vector.
The device or method according to any one of claims 1-10, wherein said fusing data comprises:

By formula

X(k+1|k)=X(k)+U k+1 +Q k+1

Estimate the system state X(k+1|k) of the indoor positioning and navigation device at k+1, where X(k) is the system state at k, Q k+1 is the motion equation noise at k+1, U k+1 is the amount of attitude change at k+1;

Optionally,

According to the system state X(k+1|k) at k+1 time, through the formula

, which is

Estimate the system observations at time k+1,

among them,

And

Among them, the coordinates (X bi , Y bi , Z bi ) are the coordinates of the Bluetooth node B i in the Bluetooth network, which are relative to the system state quantity X(k+1|k) of the indoor positioning and navigation device at time k+1 Azimuth angle θ i and elevation angle
F i represents one of the feature points. The three-dimensional coordinates of the feature point are (X fi , Y fi , Z fi ), f is the focal length of the camera, W k+1 is the noise of the observation equation at k+1, and H is The Jacobian matrix of the observation equation versus the state quantity.
The device or method according to any one of claims 1-11, wherein said fusing data comprises:

Update the calculation formula of the covariance equation as

P(k+1)=(P(k+1|k) -1 +H T *R -1 *H) -1

Where P(k+1) is the covariance matrix at k+1, and R is the observed noise covariance matrix;

And/or

Update the gain matrix calculation formula as

K=P(k+1)*H T *R -1

Among them, K is the gain matrix;

And/or

Update the calculation formula of the state vector as

X(k+1)=X(k+1|k)+K*ΔL

Among them, X(k+1) is the state vector at time k+1,
L k+1 is the real observation at k+1,
And f i (u i, v i ) is the feature point in time k + 1 F i (X fi, Y fi , Z fi) corresponding to the image coordinates.
A non-volatile storage medium storing computer-readable instructions, which when executed by one or more processors, cause one or more processors to execute any one of claims 2-12 method.
A computer program product, comprising a storage medium storing computer readable instructions, and when the computer readable instructions are executed by one or more processors, the one or more processors execute claims 2-12 Any of the methods described.
A computer program that, when executed by a processor, implements the method described in any one of claims 2-12.