CN117516545A - Heading machine navigation positioning method, system and device based on ultrasonic technology - Google Patents

Heading machine navigation positioning method, system and device based on ultrasonic technology Download PDF

Info

Publication number
CN117516545A
CN117516545A CN202311686624.9A CN202311686624A CN117516545A CN 117516545 A CN117516545 A CN 117516545A CN 202311686624 A CN202311686624 A CN 202311686624A CN 117516545 A CN117516545 A CN 117516545A
Authority
CN
China
Prior art keywords
coordinate system
ultrasonic
heading machine
coordinates
transmitting module
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202311686624.9A
Other languages
Chinese (zh)
Inventor
贾曲
田原
陈宁
张婷
郭皇煌
李小燕
索艳春
董孟阳
李涛
冯高辉
布朋生
郭进彬
赵宇阳
王炳效
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Taiyuan Institute of China Coal Technology and Engineering Group
Shanxi Tiandi Coal Mining Machinery Co Ltd
Original Assignee
Taiyuan Institute of China Coal Technology and Engineering Group
Shanxi Tiandi Coal Mining Machinery Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Taiyuan Institute of China Coal Technology and Engineering Group, Shanxi Tiandi Coal Mining Machinery Co Ltd filed Critical Taiyuan Institute of China Coal Technology and Engineering Group
Priority to CN202311686624.9A priority Critical patent/CN117516545A/en
Publication of CN117516545A publication Critical patent/CN117516545A/en
Pending legal-status Critical Current

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Measurement Of Velocity Or Position Using Acoustic Or Ultrasonic Waves (AREA)

Abstract

The invention discloses a method, a system and a device for navigation and positioning of a heading machine based on an ultrasonic technology, which relate to the field of automation of the heading machine and comprise the following steps: establishing a geodetic coordinate system and a carrier coordinate system of a heading machine; acquiring coordinates of the four ultrasonic receiving modules under a carrier coordinate system and pose parameters of the heading machine; comparing the sum of the distances between each ultrasonic transmitting module and the four ultrasonic receiving modules, and determining the ultrasonic transmitting module with the shortest sum of the distances as a target ultrasonic transmitting module; and determining the coordinates of the target ultrasonic wave transmitting module under the carrier coordinate system based on the distances between the target ultrasonic wave transmitting module and the four ultrasonic wave receiving modules and the coordinates of the four ultrasonic wave receiving modules under the carrier coordinate system, and determining the coordinates of the excavator under the geodetic coordinate system by combining the pose parameters of the excavator. The invention solves the problems of poor environmental adaptability and easy shielding of propagation of sensors such as machine vision, laser radar and the like, and improves the adaptability and accuracy of navigation and positioning of the heading machine.

Description

Heading machine navigation positioning method, system and device based on ultrasonic technology
Technical Field
The invention relates to the field of development machine automation, in particular to a development machine navigation positioning method, system and device based on an ultrasonic technology.
Background
The tunneling working face is narrow in space, severe in environment, high in danger, dense in personnel, high in labor intensity of workers and high in occupational health threat, so that the tunneling working face is less humanized, and the tunneling working face is always the target pursued by the industry. The key working procedure of tunneling operation is section cutting, wherein the key index is the control precision of the section outline boundary, a precisely-adjusted laser director fixed at the back of a roadway projects a light spot to the section, and a worker judges the section boundary by taking the light spot as a reference and controls a tunneling machine to cut. The method mainly hangs the laser indicator on the top of the back of the roadway, is easily shielded by obstacles in the laser propagation process, and needs to move the hanging position along with the increase of footage. The existing navigation method has the defects that as the heading machine advances, the distance between the heading machine and the navigation system increases, so that the detection resolution and the detection precision of the navigation system are reduced, and in addition, the situation that no shielding exists between the heading machine and the navigation system is required, and in a roadway provided with large equipment such as the heading machine, a dust removal system, a ventilation system, a conveying belt and the like, the condition is often difficult to meet.
In recent years, the machine vision technology becomes a hot spot in development of the navigation technology of the heading machine, the conventional vision navigation system of the heading machine generally comprises a vision module hung on a tunnel roof and a laser transmitter module, a laser transmitter irradiates laser beams onto a photosensitive plate at the tail of the heading machine, the machine vision acquires an arrangement image of the laser beams on the photosensitive plate, and then the spatial position information of the heading machine is obtained through image processing and pose calculation algorithms. However, this method does not solve the problem of easy shielding of the laser propagation path, and the visual method is easily affected by factors such as light and dust in the environment.
Disclosure of Invention
The invention aims to provide a navigation and positioning method, a navigation and positioning system and a navigation and positioning device for a heading machine based on an ultrasonic technology, which can determine the coordinates of the heading machine under a geodetic coordinate system by combining the position and posture parameters of the heading machine through ultrasonic ranging, and improve the adaptability and the accuracy of navigation and positioning of the heading machine.
In order to achieve the above object, the present invention provides the following solutions:
an ultrasonic technology-based heading machine navigation positioning method, which comprises the following steps:
establishing a geodetic coordinate system and a carrier coordinate system of the heading machine;
acquiring coordinates of the four ultrasonic receiving modules under a carrier coordinate system and pose parameters of the heading machine;
comparing the sum of the distances between each ultrasonic transmitting module and four ultrasonic receiving modules in a plurality of ultrasonic transmitting modules which are sequentially and equidistantly arranged on a roadway top plate, and determining the ultrasonic transmitting module with the shortest sum of the distances as a target ultrasonic transmitting module;
determining coordinates of the target ultrasonic wave transmitting module under a carrier coordinate system based on distances between the target ultrasonic wave transmitting module and the four ultrasonic wave receiving modules and coordinates of the four ultrasonic wave receiving modules under the carrier coordinate system;
and determining the coordinates of the heading machine under the geodetic coordinate system based on the pose parameters of the heading machine and the coordinates of the target ultrasonic wave transmitting module under the carrier coordinate system.
Optionally, establishing a carrier coordinate system specifically includes:
taking the center of the upper surface of the heading machine body as a coordinate origin o m Taking the longitudinal axis of the tunneling machine body as y m Axis, y m The axis rotates clockwise by 90 degrees to x m In the vertical direction x m o m y m The plane upwards is z m The axes construct a carrier coordinate system.
Optionally, establishing a geodetic coordinate system specifically includes:
with the sphere center of the earth as the origin o of coordinates n Taking the earth normal east as x n Axis y is the true north of earth n Axis, z n The axis is directed vertically to the ground to construct a geodetic coordinate system.
Optionally, determining the coordinates of the target ultrasonic emission module in the carrier coordinate system based on the distances between the target ultrasonic emission module and the four ultrasonic receiving modules and the coordinates of the four ultrasonic receiving modules in the carrier coordinate system specifically includes:
setting the position coordinate of the target ultrasonic wave transmitting module under a carrier coordinate system as W (x, y, z);
based on the distance s between the target ultrasonic wave transmitting module and four ultrasonic wave receiving modules 1 、s 2 、s 3 、s 4 The four ultrasonic receiving modules are arranged on the carrier coordinatesDetermining the coordinate K of the target ultrasonic emission module in the carrier coordinate system by the coordinate in the system 1 (x 1 ,y 1 ,0)、K 2 (x 2 ,y 2 ,0)、K 3 (x 3 ,y 3 ,0)、K 4 (x 4 ,y 4 0), a distance equation set is established as follows:
the system of distance equations is converted into a matrix by using a least squares estimation method, and a least squares estimation E is obtained as follows:
wherein,
substituting the coordinates of the target ultrasonic wave transmitting module in the x-axis direction and the y-axis direction in the position coordinates of the target ultrasonic wave transmitting module in the carrier coordinate system into any equation of the distance equation set, calculating to obtain the coordinates of the z-axis direction, and further determining the coordinates of the target ultrasonic wave transmitting module in the carrier coordinate system.
Optionally, determining the coordinates of the heading machine in the geodetic coordinate system based on the pose parameters of the heading machine and the coordinates of the target ultrasonic emission module in the carrier coordinate system specifically includes:
based on pose parameters of the heading machine under a carrier coordinate system, constructing a rotation matrix of the pose parameters under a geodetic coordinate system by taking anticlockwise rotation as positive, wherein the rotation matrix of the pose parameters specifically comprises: a rotation matrix of roll angle, a rotation matrix of pitch angle and a rotation matrix of course angle;
the rotation matrix of the course angle is as follows:
the rotation matrix of the roll angle is as follows:
the rotation matrix of the pitch angle is as follows:
based on the rotation matrix of the roll angle, the rotation matrix of the pitch angle and the rotation matrix of the course angle, the position coordinate of the heading machine at the time t of the geodetic coordinate system is calculated as follows:
wherein J is t-1 (x t-1 ,y t-1 ,z t-1 ) Actually measuring position coordinates, J, of the heading machine at the time t-1 of a carrier coordinate system t (x t ,y t ,z t ) The position coordinate of the heading machine at the time T of the geodetic coordinate system is given, and T is the displacement variable quantity of the heading machine from the time T-1 to the time T under the airborne coordinate system;
T＝W t (x t ,y t ,z t )-W t-1 (x t-1 ,y t-1 ,z t-1 )；
W t-1 (x t-1 ,y t-1 ,z t-1 ) For the position of the ultrasonic wave transmitting module under the airborne coordinate system at the time t-1, W t (x t ,y t ,z t ) The position of the ultrasonic wave transmitting module under the airborne coordinate system at the time t.
An ultrasonic technology-based heading machine navigation positioning system, the system comprising:
the coordinate system establishing module is used for establishing a geodetic coordinate system and a carrier coordinate system of the heading machine;
the acquisition module is used for acquiring coordinates of the four ultrasonic receiving modules under a carrier coordinate system and pose parameters of the heading machine;
the comparison module is used for comparing the sum of the distances between each ultrasonic transmitting module and four ultrasonic receiving modules in a plurality of ultrasonic transmitting modules which are sequentially arranged on the roadway top plate at equal intervals, and determining the ultrasonic transmitting module with the shortest sum of the distances as a target ultrasonic transmitting module;
the carrier coordinate calculation module is used for determining the coordinates of the target ultrasonic wave transmitting module under the carrier coordinate system based on the distances between the target ultrasonic wave transmitting module and the four ultrasonic wave receiving modules and the coordinates of the four ultrasonic wave receiving modules under the carrier coordinate system;
and the geodetic coordinate calculation module is used for determining the coordinates of the tunneling machine under the geodetic coordinate system based on the pose parameters of the tunneling machine and the coordinates of the target ultrasonic emission module under the carrier coordinate system.
An ultrasonic technology-based navigation and positioning device for a heading machine, the device comprising: the device comprises: the system comprises a plurality of ultrasonic transmitting modules, four ultrasonic receiving modules, a signal receiving and transmitting module, an inertial navigation and controller;
the ultrasonic wave transmitting modules and the signal receiving and transmitting modules are arranged on the roadway top plate, and the ultrasonic wave transmitting modules are sequentially and equidistantly arranged; the four ultrasonic receiving modules are respectively arranged at four corners of the top of the heading machine; the plurality of ultrasonic wave transmitting modules and the four ultrasonic wave receiving modules are both in wireless connection with the signal receiving and transmitting module, and the signal receiving and transmitting module is connected with the controller;
the inertial navigation device is arranged at the center of the top of the heading machine and is in wireless connection with the signal receiving and transmitting module; the controller is applied to the method of any one of claims 1 to 5 to calculate the coordinates of the heading machine in the geodetic coordinate system.
Optionally, the controller is a computer.
An electronic device comprising a memory, a processor and a computer program stored on the memory and executable on the processor, the processor implementing a method as described above when executing the computer program.
A computer readable storage medium having stored thereon a computer program which when executed implements a method as described above.
According to the specific embodiment provided by the invention, the invention discloses the following technical effects:
according to the invention, the target ultrasonic wave transmitting module is determined through ultrasonic ranging, the coordinates of the ultrasonic wave transmitting module under the carrier coordinate system are determined based on the distance between the ultrasonic wave transmitting module and the heading machine and the coordinates of the four ultrasonic wave receiving modules under the carrier coordinate system, and then the coordinates of the heading machine under the ground coordinate system are determined by combining the pose parameters of the heading machine. The invention solves the problems of poor environmental adaptability and easy shielding of propagation of sensors such as machine vision, laser radar and the like, and improves the adaptability and accuracy of navigation and positioning of the heading machine.
Drawings
In order to more clearly illustrate the embodiments of the present invention or the technical solutions of the prior art, the drawings that are needed in the embodiments will be briefly described below, it being obvious that the drawings in the following description are only some embodiments of the present invention, and that other drawings may be obtained according to these drawings without inventive effort for a person skilled in the art.
FIG. 1 is a schematic flow chart of a heading machine navigation positioning method based on an ultrasonic technology provided by an embodiment of the invention;
fig. 2 is a schematic structural diagram of a heading machine navigation positioning system based on an ultrasonic technology according to an embodiment of the present invention;
fig. 3 is a structural connection diagram of a heading machine navigation positioning device based on an ultrasonic technology according to an embodiment of the present invention.
Symbol description:
the device comprises an ultrasonic transmitting module-1, an ultrasonic receiving module-2, an inertial navigation device-3, a signal receiving and transmitting module-4 and a controller-5.
Detailed Description
The following description of the embodiments of the present invention will be made clearly and completely with reference to the accompanying drawings, in which it is apparent that the embodiments described are only some embodiments of the present invention, but not all embodiments. All other embodiments, which can be made by those skilled in the art based on the embodiments of the invention without making any inventive effort, are intended to be within the scope of the invention.
The invention aims to provide a navigation and positioning method, a navigation and positioning system and a navigation and positioning device for a heading machine based on an ultrasonic technology.
In order that the above-recited objects, features and advantages of the present invention will become more readily apparent, a more particular description of the invention will be rendered by reference to the appended drawings and appended detailed description.
As shown in fig. 1, the invention provides a heading machine navigation positioning method based on an ultrasonic technology, which comprises the following steps:
s1, establishing a geodetic coordinate system and a carrier coordinate system of a heading machine.
S2, acquiring coordinates of the four ultrasonic receiving modules under a carrier coordinate system and pose parameters of the heading machine.
S3, comparing the sum of the distances between each ultrasonic transmitting module and four ultrasonic receiving modules in a plurality of ultrasonic transmitting modules which are sequentially arranged on the roadway top plate at equal intervals, and determining the ultrasonic transmitting module with the shortest sum of the distances as a target ultrasonic transmitting module.
S4, determining the coordinates of the target ultrasonic wave transmitting module under the carrier coordinate system based on the distances between the target ultrasonic wave transmitting module and the four ultrasonic wave receiving modules and the coordinates of the four ultrasonic wave receiving modules under the carrier coordinate system.
S5, determining the coordinates of the heading machine under the geodetic coordinate system based on the pose parameters of the heading machine and the coordinates of the target ultrasonic wave transmitting module under the carrier coordinate system.
As an optional implementation manner, in step S1, establishing a carrier coordinate system specifically includes:
S11. taking the center of the upper surface of the heading machine body as a coordinate origin o m Taking the longitudinal axis of the tunneling machine body as y m Axis, y m The axis rotates clockwise by 90 degrees to x m In the vertical direction x m o m y m The plane upwards is z m The axes construct a carrier coordinate system.
As an optional implementation manner, in step S1, establishing a geodetic coordinate system specifically includes:
s12, taking the sphere center of the earth as the origin o of coordinates n Taking the earth normal east as x n Axis y is the true north of earth n Axis, z n The axis is directed vertically to the ground to construct a geodetic coordinate system. Wherein z is n Constructing a geodetic coordinate system with an axis pointing perpendicular to the ground and to the direction of the sky can also be understood as being in the vertical x n o n y n The plane upwards is z n The axes construct a geodetic coordinate system.
As an optional embodiment, step S4 specifically includes:
s41, setting the position coordinates of the target ultrasonic wave transmitting module under a carrier coordinate system as W (x, y, z).
S42, based on distance s between the target ultrasonic wave transmitting module and the four ultrasonic wave receiving modules 1 、s 2 、s 3 、s 4 And determining the coordinate K of the target ultrasonic transmitting module under the carrier coordinate system by the coordinates of the four ultrasonic receiving modules under the carrier coordinate system 1 (x 1 ,y 1 ,0)、K 2 (x 2 ,y 2 ,0)、K 3 (x 3 ,y 3 ,0)、K 4 (x 4 ,y 4 0), a distance equation set is established as follows:
s43, converting the distance equation set into a matrix by using a least square estimation method to obtain a least square estimation E as follows:
wherein,
s44, substituting the coordinates of the target ultrasonic wave transmitting module in the x-axis direction and the y-axis direction in the position coordinates of the target ultrasonic wave transmitting module in the carrier coordinate system into any equation of the distance equation set, calculating to obtain the coordinates of the z-axis direction, and further determining the coordinates of the target ultrasonic wave transmitting module in the carrier coordinate system.
As an optional embodiment, step S5 specifically includes:
s51, constructing a rotation matrix of pose parameters under a geodetic coordinate system based on the fact that the pose parameters of the heading machine under a carrier coordinate system are positive in anticlockwise rotation, wherein the rotation matrix of the pose parameters specifically comprises: a rotation matrix of roll angle, a rotation matrix of pitch angle, and a rotation matrix of heading angle.
The rotation matrix of the course angle is:
the rotation matrix of the roll angle is:
the rotation matrix of pitch angle is:
s52, calculating a position coordinate of the heading machine at a time t of a geodetic coordinate system based on a rotation matrix of a roll angle, a rotation matrix of a pitch angle and a rotation matrix of a course angle, wherein the position coordinate is as follows:
wherein J is t-1 (x t-1 ,y t-1 ,z t-1 ) In order to actually measure the position coordinates of the heading machine at the time t-1 of the carrier coordinate system,J t (x t ,y t ,z t ) The position coordinate of the heading machine at the time T of the geodetic coordinate system is T, and T is the displacement variable quantity of the heading machine from the time T-1 to the time T under the airborne coordinate system. 0
T＝W t (x t ,y t ,z t )-W t-1 (x t-1 ,y t-1 ,z t-1 )。
W t-1 (x t-1 ,y t-1 ,z t-1 ) For the position of the ultrasonic wave transmitting module under the airborne coordinate system at the time t-1, W t (x t ,y t ,z t ) The position of the ultrasonic wave transmitting module under the airborne coordinate system at the time t.
The initial position of the heading machine under the carrier coordinate system can be measured by a total station.
Example two
As shown in fig. 2, the present invention further provides a heading machine navigation positioning system based on an ultrasonic technology, the system comprising:
the coordinate system establishment module 101 is used for establishing a geodetic coordinate system and a carrier coordinate system of the heading machine.
The acquiring module 102 is configured to acquire coordinates of the four ultrasonic receiving modules in a carrier coordinate system and pose parameters of the heading machine.
And the comparison module 103 is used for comparing the sum of the distances between each ultrasonic transmitting module and the four ultrasonic receiving modules in the plurality of ultrasonic transmitting modules which are sequentially arranged on the roadway top plate at equal intervals, and determining the ultrasonic transmitting module with the shortest sum of the distances as the target ultrasonic transmitting module.
The carrier coordinate calculation module 104 is configured to determine the coordinates of the target ultrasonic wave transmitting module in the carrier coordinate system based on the distances between the target ultrasonic wave transmitting module and the four ultrasonic wave receiving modules and the coordinates of the four ultrasonic wave receiving modules in the carrier coordinate system.
The geodetic coordinate calculation module 105 is configured to determine the coordinates of the heading machine in the geodetic coordinate system based on the pose parameters of the heading machine and the coordinates of the target ultrasonic emission module in the carrier coordinate system.
Example III
As shown in fig. 3, the present invention further provides a heading machine navigation positioning device based on an ultrasonic technology, where the device includes: the device comprises: the system comprises a plurality of ultrasonic wave transmitting module development machines 1, four ultrasonic wave receiving module development machines 2, a signal receiving and transmitting module development machine 4 and an inertial navigation and controller development machine 5.
The plurality of ultrasonic wave transmitting module heading machines 1 and the signal receiving and transmitting module heading machines 4 are arranged on a roadway roof, and the plurality of ultrasonic wave transmitting module heading machines 1 are sequentially arranged at equal intervals; the four ultrasonic receiving module heading machines 2 are respectively arranged at four corners of the top of the heading machine; the plurality of ultrasonic wave transmitting module development machines 1 and the four ultrasonic wave receiving module development machines 2 are both in wireless connection with the signal receiving and transmitting module development machine 4, and the signal receiving and transmitting module development machine 4 is connected with the controller development machine 5; the controller heading machine 5 is a computer.
The inertial navigation device heading machine 3 is arranged in the center of the top of the heading machine, and the inertial navigation device heading machine 3 is in wireless connection with the signal receiving and transmitting module heading machine 4; the controller heading machine 5 is applied to the method of any one of the above to calculate the coordinates of the heading machine in the geodetic coordinate system.
Because the ultrasonic wave beam angle is limited, in the advancing process of tunneling equipment, the ultrasonic wave receiving module tunneling machine 2 fixed at the top of the tunneling machine influences the ultrasonic wave distance measurement precision when moving out of the beam angle range of the ultrasonic wave transmitting module tunneling machine 1, so that the ultrasonic wave transmitting module tunneling machine 1 needs to be moved to a station in order to improve the distance measurement precision, three ultrasonic wave transmitting module tunneling machines 1 are respectively arranged on a tunnel top plate at equal intervals in sequence in order to reduce station moving times, and in order to distinguish the ultrasonic wave transmitting module tunneling machines 1, signal marking is carried out on each ultrasonic wave transmitting module tunneling machine 1 and each ultrasonic wave receiving module tunneling machine 2 in a labeling mode.
The signal receiving and transmitting module development machine 4 synchronously collects data sent by the inertial navigation device development machine 3 and the ultrasonic wave transmitting module development machine 1, wherein the data specifically comprises: pose parameters of the heading machine under the carrier coordinate system and distances s between the target ultrasonic wave transmitting module heading machine 1 and the four ultrasonic wave receiving module heading machines 2 respectively 1 、s 2 、s 3 、s 4 。
Example IV
The invention also provides an electronic device comprising a memory, a processor and a computer program stored on the memory and executable on the processor, the processor implementing a method as above when executing the computer program.
Example five
The present invention also provides a computer readable storage medium having stored thereon a computer program which when executed implements a method as above.
The invention applies the characteristics that the ultrasonic wave has strong directivity in the propagation process, is less influenced by dust, does not depend on external environment and has certain autonomy, and the invention builds a geodetic coordinate system and a carrier coordinate system of the heading machine; acquiring coordinates of the four ultrasonic receiving modules under a carrier coordinate system and pose parameters of the heading machine; comparing the sum of the distances between each ultrasonic transmitting module and the four ultrasonic receiving modules, and determining the ultrasonic transmitting module with the shortest sum of the distances as a target ultrasonic transmitting module; and determining the coordinates of the target ultrasonic wave transmitting module under the carrier coordinate system based on the distances between the target ultrasonic wave transmitting module and the four ultrasonic wave receiving modules and the coordinates of the four ultrasonic wave receiving modules under the carrier coordinate system, and determining the coordinates of the excavator under the geodetic coordinate system by combining the pose parameters of the excavator.
The invention solves the problems of poor environmental adaptability and easy shielding of propagation of sensors such as machine vision, laser radar and the like, improves the adaptability and accuracy of navigation positioning of the heading machine, realizes the airborne navigation positioning of the heading machine, provides technical support for autonomous navigation and planning cutting of the heading machine, and realizes the intellectualization of matched equipment of the heading machine.
In the present specification, each embodiment is described in a progressive manner, and each embodiment is mainly described in a different point from other embodiments, and identical and similar parts between the embodiments are all enough to refer to each other. For the system disclosed in the embodiment, since it corresponds to the method disclosed in the embodiment, the description is relatively simple, and the relevant points refer to the description of the method section.
The principles and embodiments of the present invention have been described herein with reference to specific examples, the description of which is intended only to assist in understanding the methods of the present invention and the core ideas thereof; also, it is within the scope of the present invention to be modified by those of ordinary skill in the art in light of the present teachings. In summary, the present description should not be construed as limiting the invention.

Claims (10)

1. The heading machine navigation positioning method based on the ultrasonic technology is characterized by comprising the following steps of:
establishing a geodetic coordinate system and a carrier coordinate system of the heading machine;
acquiring coordinates of the four ultrasonic receiving modules under a carrier coordinate system and pose parameters of the heading machine;
comparing the sum of the distances between each ultrasonic transmitting module and four ultrasonic receiving modules in a plurality of ultrasonic transmitting modules which are sequentially and equidistantly arranged on a roadway top plate, and determining the ultrasonic transmitting module with the shortest sum of the distances as a target ultrasonic transmitting module;
determining coordinates of the target ultrasonic wave transmitting module under a carrier coordinate system based on distances between the target ultrasonic wave transmitting module and the four ultrasonic wave receiving modules and coordinates of the four ultrasonic wave receiving modules under the carrier coordinate system;
and determining the coordinates of the heading machine under the geodetic coordinate system based on the pose parameters of the heading machine and the coordinates of the target ultrasonic wave transmitting module under the carrier coordinate system.
2. The method for navigating and positioning a heading machine based on ultrasonic technology according to claim 1, wherein the establishing of the carrier coordinate system specifically comprises:
taking the center of the upper surface of the heading machine body as a coordinate origin o m Taking the longitudinal axis of the tunneling machine body as y m Axis, y m The axis rotates clockwise by 90 degrees to x m In a vertical directionx m o m y m The plane upwards is z m The axes construct a carrier coordinate system.
3. The method for navigating and positioning a heading machine based on the ultrasonic technology according to claim 1, wherein the establishing of the geodetic coordinate system specifically comprises:
with the sphere center of the earth as the origin o of coordinates n Taking the earth normal east as x n Axis y is the true north of earth n Axis, z n The axis is directed vertically to the ground to construct a geodetic coordinate system.
4. The method for navigating and positioning a heading machine based on an ultrasonic technology according to claim 1, wherein the determining the coordinates of the target ultrasonic transmitting module in the carrier coordinate system based on the distances between the target ultrasonic transmitting module and the four ultrasonic receiving modules and the coordinates of the four ultrasonic receiving modules in the carrier coordinate system specifically comprises:
setting the position coordinate of the target ultrasonic wave transmitting module under a carrier coordinate system as W (x, y, z);
based on the distance s between the target ultrasonic wave transmitting module and four ultrasonic wave receiving modules 1 、s 2 、s 3 、s 4 And determining the coordinates K of the target ultrasonic transmitting module under the carrier coordinate system by the coordinates of the four ultrasonic receiving modules under the carrier coordinate system 1 (x 1 ,y 1 ,0)、K 2 (x 2 ,y 2 ,0)、K 3 (x 3 ,y 3 ,0)、K 4 (x 4 ,y 4 0), a distance equation set is established as follows:
the system of distance equations is converted into a matrix by using a least squares estimation method, and a least squares estimation E is obtained as follows:
wherein,
substituting the coordinates of the target ultrasonic wave transmitting module in the x-axis direction and the y-axis direction in the position coordinates of the target ultrasonic wave transmitting module in the carrier coordinate system into any equation of the distance equation set, calculating to obtain the coordinates of the z-axis direction, and further determining the coordinates of the target ultrasonic wave transmitting module in the carrier coordinate system.
5. The method for navigating and positioning a heading machine based on the ultrasonic technology according to claim 1, wherein the method for determining the coordinates of the heading machine in the geodetic coordinate system based on the pose parameters of the heading machine and the coordinates of the target ultrasonic emission module in the carrier coordinate system specifically comprises:
based on pose parameters of the heading machine under a carrier coordinate system, constructing a rotation matrix of the pose parameters under a geodetic coordinate system by taking anticlockwise rotation as positive, wherein the rotation matrix of the pose parameters specifically comprises: a rotation matrix of roll angle, a rotation matrix of pitch angle and a rotation matrix of course angle;
the rotation matrix of the course angle is as follows:
the rotation matrix of the roll angle is as follows:
the rotation matrix of the pitch angle is as follows:
based on the rotation matrix of the roll angle, the rotation matrix of the pitch angle and the rotation matrix of the course angle, the position coordinate of the heading machine at the time t of the geodetic coordinate system is calculated as follows:
wherein J is t-1 (x t-1 ,y t-1 ,z t-1 ) Actually measuring position coordinates, J, of the heading machine at the time t-1 of a carrier coordinate system t (x t ,y t ,z t ) The position coordinate of the heading machine at the time T of the geodetic coordinate system is given, and T is the displacement variable quantity of the heading machine from the time T-1 to the time T under the airborne coordinate system;
T＝W t (x t ,y t ,z t )-W t-1 (x t-1 ,y t-1 ,z t-1 )；
W t-1 (x t-1 ,y t-1 ,z t-1 ) For the position of the ultrasonic wave transmitting module under the airborne coordinate system at the time t-1, W t (x t ,y t ,z t ) The position of the ultrasonic wave transmitting module under the airborne coordinate system at the time t.
6. An ultrasonic technology-based heading machine navigation positioning system, characterized in that the system comprises:
the coordinate system establishing module is used for establishing a geodetic coordinate system and a carrier coordinate system of the heading machine;
the acquisition module is used for acquiring coordinates of the four ultrasonic receiving modules under a carrier coordinate system and pose parameters of the heading machine;
the comparison module is used for comparing the sum of the distances between each ultrasonic transmitting module and four ultrasonic receiving modules in a plurality of ultrasonic transmitting modules which are sequentially arranged on the roadway top plate at equal intervals, and determining the ultrasonic transmitting module with the shortest sum of the distances as a target ultrasonic transmitting module;
the carrier coordinate calculation module is used for determining the coordinates of the target ultrasonic wave transmitting module under the carrier coordinate system based on the distances between the target ultrasonic wave transmitting module and the four ultrasonic wave receiving modules and the coordinates of the four ultrasonic wave receiving modules under the carrier coordinate system;
and the geodetic coordinate calculation module is used for determining the coordinates of the tunneling machine under the geodetic coordinate system based on the pose parameters of the tunneling machine and the coordinates of the target ultrasonic emission module under the carrier coordinate system.
7. An ultrasonic technology-based heading machine navigation positioning device, which is characterized by comprising: the device comprises: the system comprises a plurality of ultrasonic transmitting modules, four ultrasonic receiving modules, a signal receiving and transmitting module, an inertial navigation and controller;
the ultrasonic wave transmitting modules and the signal receiving and transmitting modules are arranged on the roadway top plate, and the ultrasonic wave transmitting modules are sequentially and equidistantly arranged; the four ultrasonic receiving modules are respectively arranged at four corners of the top of the heading machine; the plurality of ultrasonic wave transmitting modules and the four ultrasonic wave receiving modules are both in wireless connection with the signal receiving and transmitting module, and the signal receiving and transmitting module is connected with the controller;
the inertial navigation device is arranged at the center of the top of the heading machine and is in wireless connection with the signal receiving and transmitting module; the controller is applied to the method of any one of claims 1 to 5 to calculate the coordinates of the heading machine in the geodetic coordinate system.
8. The ultrasonic technology-based heading machine navigation positioning device of claim 7, wherein the controller is a computer.
9. An electronic device comprising a memory, a processor and a computer program stored on the memory and executable on the processor, the processor implementing the method of any one of claims 1 to 5 when executing the computer program.
10. A computer readable storage medium, characterized in that a computer program is stored thereon, which computer program, when executed, implements the method according to any of claims 1 to 5.
CN202311686624.9A 2023-12-08 2023-12-08 Heading machine navigation positioning method, system and device based on ultrasonic technology Pending CN117516545A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202311686624.9A CN117516545A (en) 2023-12-08 2023-12-08 Heading machine navigation positioning method, system and device based on ultrasonic technology

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202311686624.9A CN117516545A (en) 2023-12-08 2023-12-08 Heading machine navigation positioning method, system and device based on ultrasonic technology

Publications (1)

Publication Number Publication Date
CN117516545A true CN117516545A (en) 2024-02-06

Family

ID=89745682

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202311686624.9A Pending CN117516545A (en) 2023-12-08 2023-12-08 Heading machine navigation positioning method, system and device based on ultrasonic technology

Country Status (1)

Country Link
CN (1) CN117516545A (en)

Similar Documents

Publication Publication Date Title
US10876307B2 (en) Construction management system and method
US10774505B2 (en) Work machine control system, work machine, and work machine control method
CN109115173B (en) Monocular vision measuring method for position and attitude of heading machine body based on linear positioning model
CN109669468B (en) Measuring device, unmanned aerial vehicle, control device and control method thereof, and storage medium
La et al. Autonomous robotic system for high-efficiency non-destructive bridge deck inspection and evaluation
CN106338245A (en) Non-contact movement measuring method for workpiece
JP6966218B2 (en) Imaging equipment calibration equipment, work machines and calibration methods
US11486988B2 (en) Method for calibrating the alignment of a moving object sensor
WO2020073929A1 (en) Operation trolley, and robotic arm optical target positioning device and positioning method thereof
JP2018036189A (en) Calibration device and calibration method
JPWO2020003497A1 (en) Imaging equipment calibration equipment, monitoring equipment, work machines and calibration methods
CN111707235A (en) Ground object measuring method based on three-dimensional laser scanning technology
CN112684797B (en) Obstacle map construction method
Chou et al. Mirror-assisted calibration of a multi-modal sensing array with a ground penetrating radar and a camera
CN109670204B (en) Carrier rocket video image interpretation error correction method
CN111521996A (en) Laser radar installation calibration method
CN117516545A (en) Heading machine navigation positioning method, system and device based on ultrasonic technology
CN115857520A (en) Unmanned aerial vehicle carrier landing state monitoring method based on combination of vision and ship state
CN112857367B (en) Heading machine pose detection method based on machine vision and inertial navigation
CN116704019A (en) Drilling and anchoring robot monocular vision positioning method based on anchor rod network
US20220018950A1 (en) Indoor device localization
CN116626686A (en) Heading machine positioning and orientation method, system, equipment and storage medium
US11415695B2 (en) Distance measuring system with layout generation functionality
CN111637889A (en) Tunneling machine positioning method and system based on inertial navigation and laser radar three-point distance measurement
JPH0611344A (en) Measuring method of position and attitude of moving body

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination