1. A method for three-dimensional positioning based on multi-direction ultrasonic ranging and IMU comprises the following steps:

s1, calculating the orientation of the body according to the data measured by the IMU;

s2, obtaining the distance between the body and the wall surface through a plurality of ultrasonic sensors;

s3, calculating the three-dimensional coordinates of the body according to the calculated orientation and the obtained distance;

s4, interpolating the orientation by using quaternion spherical linear interpolation to obtain the track of the orientation; and

s5, interpolating the three-dimensional coordinates by spline interpolation to obtain the track of the three-dimensional coordinates.

2. The method of claim 1, wherein S1 further comprises:

and obtaining pitching, rolling and yaw angular velocities by adopting an IMU update and Madgwick algorithm and combining an accelerometer and a gyroscope of the IMU, and then correcting the yaw angular velocities through a geomagnetic sensor.

3. The method of claim 1, wherein the plurality of ultrasonic sensors is three ultrasonic ranging sensors and the longitudinal axes of the three ultrasonic ranging sensors are perpendicular two by two.

4. The method of claim 3, wherein S2 further comprises:

and coding waveforms sent by the three ultrasonic ranging sensors differently, so that different ultrasonic ranging sensors send different ultrasonic coding sequences, and judging whether the ultrasonic ranging sensors are corresponding echoes according to the received ultrasonic coding sequences.

5. The method of claim 4, wherein the coded sequence of ultrasonic waves emitted by the three ultrasonic ranging sensors is represented by 8 bits, the 8 bits being 11111111, 11010111 and 11101011, wherein 0 indicates no ultrasonic waves are output and 1 indicates ultrasonic waves are output.

6. The method of claim 5, wherein the three ultrasonic ranging sensors emit ultrasonic waves at a frequency of 40 Hz.

7. The method of claim 1, wherein the Spline interpolation is a Catmull-Rom Spline interpolation.

8. The method of claim 1, wherein the IMU and the plurality of ultrasonic sensors are secured together by one of an adhesive, set screws, and a dedicated mold.

9. The method of claim 1, wherein the IMU and the plurality of ultrasonic sensors are integrated together by circuitry.

10. An apparatus for three-dimensional localization based on multi-directional ultrasonic ranging and an IMU, comprising a processor and a memory, wherein the memory has stored thereon computer program instructions which, when executed by the processor, implement the method of any of claims 1-9.

Background

Currently, the mainstream three-dimensional positioning technology in the field of virtual reality adopts a navigation base station-based mode, such as HTC Vive equipment. For example, 2 navigation stations are erected in the operation space, the navigation stations emit high-frequency infrared signals, and the positioning objects receive the infrared signals and calculate the related angle information according to the synchronous signals. The method needs to fix a preset navigation station on the wall surface, use a high tripod to erect, access a power supply to the navigation station, and synchronize the navigation station and equipment between the navigation stations. In addition, for example, the Kinect device uses an infrared camera to emit structural infrared light, and analyzes a return pattern to obtain a distance from an object to the camera, which requires a large amount of calculation and cannot calculate the distance if the device is blocked.

Disclosure of Invention

In view of the above technical problems, the present disclosure provides a method for three-dimensional positioning based on multi-directional ultrasonic ranging and an IMU, comprising: s1, calculating the orientation of the body according to the data measured by the IMU; s2, obtaining the distance between the body and the wall surface through a plurality of ultrasonic sensors; s3, calculating the three-dimensional coordinates of the body according to the calculated orientation and the obtained distance; s4, interpolating the orientation by using quaternion spherical linear interpolation to obtain the track of the orientation; s5, interpolating the three-dimensional coordinates by spline interpolation to obtain the track of the three-dimensional coordinates.

In a preferred embodiment, S1 further includes: and obtaining pitching, rolling and yaw angular velocities by adopting an IMU update and Madgwick algorithm and combining an accelerometer and a gyroscope of the IMU, and then correcting the yaw angular velocities through a geomagnetic sensor.

In a preferred embodiment, the plurality of ultrasonic sensors is three ultrasonic ranging sensors, and the longitudinal axes of the three ultrasonic ranging sensors are perpendicular to each other.

In a preferred embodiment, S2 further includes: and coding waveforms sent by the three ultrasonic ranging sensors differently, so that different ultrasonic ranging sensors send different ultrasonic coding sequences, and judging whether the ultrasonic ranging sensors are corresponding echoes according to the received ultrasonic coding sequences.

In a preferred embodiment, the coded sequence of ultrasonic waves emitted by the three ultrasonic ranging sensors is represented by 8 bits, the 8 bits being 11111111, 11010111 and 11101011, wherein 0 indicates no ultrasonic wave is output and 1 indicates ultrasonic wave is output.

In a preferred embodiment, the three ultrasonic ranging sensors emit ultrasonic waves at a frequency of 40 Hz.

In a preferred embodiment, the Spline interpolation is a Catmull-Rom Spline interpolation.

In a preferred embodiment, the IMU and the plurality of ultrasonic sensors are fixed together by one of an adhesive, a set screw, and a dedicated mold.

In a preferred embodiment, the IMU and the plurality of ultrasonic sensors are integrated together by a circuit.

In one aspect of the disclosure, an apparatus for three-dimensional localization based on multi-directional ultrasonic ranging and IMU is provided, comprising a processor and a memory, wherein the memory has stored thereon computer program instructions that, when executed by the processor, implement the method of any of the above.

Compared with the prior art, the beneficial effects of the disclosure are: the method can avoid a complex setting environment and does not need to set a navigation station, solves the problem of three-dimensional positioning in a low-cost and high-flexibility mode, further avoids signal interference among a plurality of ultrasonic ranging, is simple and quick in calculation, can be coded into a single chip microcomputer or a simple calculation unit for execution, and has great advantage in cost.

Drawings

The novel features of the invention are set forth with particularity in the appended claims. A better understanding of the features and advantages of the present invention will be obtained by reference to the following detailed description that sets forth illustrative embodiments, in which the principles of the invention are utilized, and the accompanying drawings. The drawings are only for purposes of illustrating embodiments and are not to be construed as limiting the invention. Also, in the drawings, wherein like reference numerals refer to like elements throughout:

FIG. 1 illustrates a flow chart of a method for three-dimensional localization based on multi-directional ultrasonic ranging and IMU in accordance with an exemplary embodiment of the present disclosure;

FIG. 2 shows a schematic diagram of an arrangement of a plurality of ultrasonic sensors according to an exemplary embodiment of the present disclosure;

FIG. 3 shows a schematic diagram of ranging by a plurality of ultrasonic sensors according to an exemplary embodiment of the present disclosure;

FIG. 4 shows a schematic diagram of different encoding modes of a plurality of ultrasonic sensors according to an exemplary embodiment of the present disclosure; and

fig. 5 and 6 show schematic diagrams of computing three-dimensional coordinates of an ontology according to exemplary embodiments of the present disclosure.

Detailed Description

Exemplary embodiments of the present disclosure will be described in more detail below with reference to the accompanying drawings. While exemplary embodiments of the present disclosure are shown in the drawings, it should be understood that the present disclosure may be embodied in various forms and should not be limited to the embodiments set forth herein. Rather, these embodiments are provided so that this disclosure will be thorough and complete, and will fully convey the scope of the disclosure to those skilled in the art. Nothing in the following detailed description is intended to indicate that any particular component, feature, or step is essential to the invention. Those skilled in the art will appreciate that various features or steps may be substituted for or combined with one another without departing from the scope of the present disclosure.

FIG. 1 illustrates a flow chart of a method for three-dimensional localization based on multi-directional ultrasonic ranging and IMU in accordance with an exemplary embodiment of the present disclosure. The imu (inertial Measurement unit) is an inertial Measurement unit, which is a sensor used primarily to detect and measure acceleration and rotational motion. Inertial measurement units typically include accelerometers and angular rate meters (gyroscopes), which are core components of inertial systems. The disclosure provides a method for three-dimensional positioning based on multi-direction ultrasonic ranging and IMU, comprising the following steps: s1, calculating the orientation of the body according to the data measured by the IMU; s2, obtaining the distance between the body and the wall surface through a plurality of ultrasonic sensors; s3, calculating the three-dimensional coordinates of the body according to the calculated orientation and the obtained distance; s4, interpolating the orientation using quaternion spherical linear interpolation (Slerp) to obtain a trajectory of the orientation, e.g. a trajectory equation of the orientation; s5, interpolating the three-dimensional coordinates using Spline interpolation (e.g., Catmull-Rom Spline interpolation) to obtain a trajectory of the three-dimensional coordinates, e.g., a trajectory equation of the three-dimensional coordinates.

In a preferred embodiment, calculating the orientation of the body from the data measured by the IMU comprises using IMU update and Madgwick algorithms in combination with the accelerometer and gyroscope of the IMU to derive pitch (yaw), roll (pitch) and yaw (roll) angular velocities. Because the accuracy of the yaw angular velocity may be reduced after the IMU update algorithm is operated for a long time, the yaw angular velocity can be corrected by the geomagnetic sensor. For details of IMU update and Madgwick algorithms, reference may be made to the paperhttps://x-io.co.uk/open-source-imu-and-ahrs-algorithms/。

The differential equation for which the orientation expressed in quaternion is to be solved is as follows:

wherein dQ is the orientation represented by a quaternion, q₀，q₁，q₂，q₃A quaternion, ω, representing the last moment in time_x，ω_y，ω_zRepresenting the angular velocity of rotation about the x, y, z axes, respectively.

FIG. 2 shows a schematic diagram of an arrangement of a plurality of ultrasonic sensors according to an exemplary embodiment of the present disclosure; FIG. 3 shows a schematic diagram of ranging by multiple ultrasonic sensors according to an exemplary embodiment of the present disclosure. Only the orientation data does not allow the body to be positioned in three-dimensional coordinates. Therefore, the scheme needs ultrasonic sensors in multiple directions to obtain the distance between the body and the wall surface, and then the three-dimensional coordinate of the body is calculated by combining orientation data. In a preferred embodiment, the plurality of ultrasonic sensors is three ultrasonic ranging sensors, and the longitudinal axes of the three ultrasonic ranging sensors are perpendicular to each other.

When a plurality of ultrasonic ranging sensors measure the distance between the body and the wall surface, the ultrasonic echoes in a plurality of directions sent by the ultrasonic ranging sensors can be received by other sensors due to multiple reflection, and the distance can be interfered by calculation, so that the echo distance can not be stably calculated. For example, a time division approach may be used, where only one ultrasonic ranging sensor is active at a time, but this may cause the positioning system to consume more latency. The technical scheme of the invention adopts an anti-interference coding technology to code the waveform sent by the ultrasonic transmitter, so that different ultrasonic transmitters send different ultrasonic coding sequences, and a receiving end can judge whether the echo corresponds to the ultrasonic transmitter according to the received ultrasonic waveform. In a preferred embodiment, the waveforms emitted by the three ultrasonic ranging sensors are encoded differently, so that different ultrasonic ranging sensors emit different ultrasonic encoding sequences, and whether the waveforms are corresponding echoes is determined according to the received ultrasonic encoding sequences. For example, the ultrasonic signal is encoded and controlled by 1 byte in a binary encoding method. In particular, the three ultrasonic ranging sensors emit an ultrasonic coding sequence, represented by 8 bits, at different frequencies, preferably at a frequency of 40 Hz. FIG. 4 shows a schematic diagram of different encoding modes of a plurality of ultrasonic sensors according to an exemplary embodiment of the present disclosure. The 8 bits of the ultrasonic wave coded sequence output by the three ultrasonic ranging sensors are 11111111, 11010111 and 11101011, wherein 0 indicates that no ultrasonic wave is output, i.e. silence, and 1 indicates that an ultrasonic wave is output.

Fig. 5 and 6 show schematic diagrams of computing three-dimensional coordinates of an ontology according to exemplary embodiments of the present disclosure. As shown in FIG. 5, the height h1 of the horizontal plane of the body from the roof is calculated by the following formula:

by applying the above formula to the other two wall surfaces, the distances h2 and h3 from the plane of the body to the other two wall surfaces can be obtained, and thus the three-dimensional coordinates of the body can be measured, as shown in fig. 6.

For the trajectory of the three-dimensional position, a trajectory between two three-dimensional positions before and after given by three-dimensional positioning is used. The spline interpolation algorithm is to use a mathematical function to generate a smooth interpolation curve by controlling the estimated variance and using some characteristic nodes and a polynomial fitting method for some limited point values. In an embodiment of the invention, the three-dimensional coordinates are interpolated using a Catmull-Rom spline interpolation to obtain a trajectory curve between two three-dimensional positions. Suppose there are four vertices P_i-2，P_i-1，P_i，P_i+1Then, for in P_i-1，P_iThe interpolation of the trajectory between can be performed by using the following formula:

where u can vary from 0 to 1, the coordinates will vary from P_i-1Change to P_iAnd τ represents the degree of curve curvature.

Considering two three-dimensional orientations q before and after in quaternion₁And q is₂Then a spherical linear interpolation is performed on the three-dimensional orientation, resulting in a quaternion described by the following equation:

where u can vary from 0 to 1, then the orientation is from q₁Change to q₂And theta represents q₁And q is₂The included angle therebetween.

In a preferred embodiment, a plurality of ultrasonic sensors of the present invention may be combined into a single body, wherein the IMU and the plurality of ultrasonic sensors are fixed together by one of an adhesive, a set screw and a dedicated mold to achieve modular assembly of the sensors, facilitating replacement of individual sensing elementsThe device can achieve higher flexibility and configurability. In a preferred embodiment, the IMU and the plurality of ultrasonic sensors are integrated together by a circuit to achieve miniaturization and integration of the sensor assembly. The data transmission modes of the plurality of sensors may include, for example: (1) the data are transmitted to a computer in a wireless or wired mode, and are calculated and interpolated by a program on the computer; (2) the micro-miniature computing device such as a single chip microcomputer is used for realizing local computation, and the computation result is transmitted to a computer so as to be used by applications such as virtual reality. Examples of computers include personal computers (e.g., pocket PCs), tablet PCs or tablet PCs (e.g.,iPad、galaxy Tab), telephone, smartphone (e.g.,iPhone, Android-enabled device,) Or a personal digital assistant. The transmission of sensor data or the transmission of calculation results (results of local calculation using a micro-computing device such as a single chip microcomputer) data may be performed, for example, by a wired transmission such as a Universal Serial Bus (USB) port, an ethernet port, and an IEEE1394 port; or wirelessly, such as via bluetooth, wifi, infrared interfaces, etc. The fusion calculation of the sensor data comprises three-dimensional position calculation of the orientation data and the distance data, interpolation of the orientation data and interpolation of three-dimensional coordinates. The calculation of the three-dimensional position and the interpolation calculation can be implemented using software programs. The software program may be written in any programming language, such as C, C + +, Java, or Visual Basic. Software programs may also run on application frameworks, for exampleA frame,A framework or any other application framework.

The technical scheme of the invention adopts the method and the equipment, can avoid complex setting environment, does not need to arrange indoor environment in advance, does not need to set a navigation station, solves the three-dimensional positioning problem in a low-cost and high-flexibility mode, further avoids signal interference among a plurality of ultrasonic ranging, has simple and quick calculation, can be coded into a single chip microcomputer or a simple calculation unit for execution, and has great advantage in cost.

In one aspect of the disclosure, there is also provided an apparatus for three-dimensional localization based on multi-directional ultrasonic ranging and IMU, comprising a processor and a memory, wherein the memory has stored thereon computer program instructions which, when executed by the processor, implement the method of any of the above.

In yet another aspect of the present disclosure, there is also provided a machine-readable storage medium having stored thereon computer program instructions, wherein the computer program instructions, when executed by a processor, implement the method for time management as described above. In some implementations, the machine-readable storage medium is a tangible component of a digital processing device. In other embodiments, the machine-readable storage medium is optionally removable from the digital processing apparatus. In some embodiments, the machine-readable storage medium may include, by way of non-limiting example, a usb disk, a removable hard disk, a Read-Only Memory (ROM), a Random Access Memory (RAM), a flash Memory, a programmable Read-Only Memory (PROM), an erasable programmable Read-Only Memory (EPROM), a solid-state Memory, a magnetic disk, an optical disk, a cloud computing system or service, and so forth.

It should be understood that the various steps recited in the method embodiments of the present disclosure may be performed in a different order, and/or performed in parallel. Moreover, method embodiments may include additional steps and/or omit performing the illustrated steps. The scope of the invention is not limited in this respect.

In the description provided herein, numerous specific details are set forth. However, it is understood that embodiments of the disclosure may be practiced without these specific details. In some embodiments, well-known methods, structures and techniques have not been shown in detail in order not to obscure an understanding of this description.

While exemplary embodiments of the present invention have been shown and described herein, it will be readily understood by those skilled in the art that such embodiments are provided by way of example only. Numerous modifications, changes, and substitutions will now occur to those skilled in the art without departing from the invention. It should be understood that various alternatives to the embodiments of the invention described herein may be employed in practicing the invention. It is intended that the following claims define the scope of the invention and that methods and structures within the scope of these claims and their equivalents be covered thereby.