CN113843788B - Security robot based on millimeter wave and inertial fusion navigation - Google Patents
Security robot based on millimeter wave and inertial fusion navigation Download PDFInfo
- Publication number
- CN113843788B CN113843788B CN202110617824.3A CN202110617824A CN113843788B CN 113843788 B CN113843788 B CN 113843788B CN 202110617824 A CN202110617824 A CN 202110617824A CN 113843788 B CN113843788 B CN 113843788B
- Authority
- CN
- China
- Prior art keywords
- navigation
- robot
- millimeter wave
- fusion
- inertial
- 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.)
- Active
Links
- 230000004927 fusion Effects 0.000 title claims abstract description 47
- 238000005259 measurement Methods 0.000 claims abstract description 16
- 238000000034 method Methods 0.000 claims abstract description 11
- 238000012545 processing Methods 0.000 claims description 17
- 238000001914 filtration Methods 0.000 claims description 9
- 230000033001 locomotion Effects 0.000 claims description 9
- 238000004364 calculation method Methods 0.000 claims description 4
- 238000007499 fusion processing Methods 0.000 claims description 4
- 238000005070 sampling Methods 0.000 claims description 4
- 238000006243 chemical reaction Methods 0.000 claims description 3
- 238000002474 experimental method Methods 0.000 claims description 3
- 238000013178 mathematical model Methods 0.000 claims description 2
- 238000003672 processing method Methods 0.000 claims 1
- 239000000779 smoke Substances 0.000 abstract description 8
- 238000011160 research Methods 0.000 abstract description 4
- 238000010586 diagram Methods 0.000 description 13
- 239000011159 matrix material Substances 0.000 description 5
- 238000012986 modification Methods 0.000 description 3
- 230000004048 modification Effects 0.000 description 3
- 238000004891 communication Methods 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 230000014509 gene expression Effects 0.000 description 2
- 230000011218 segmentation Effects 0.000 description 2
- 230000000007 visual effect Effects 0.000 description 2
- 230000001419 dependent effect Effects 0.000 description 1
- 239000000284 extract Substances 0.000 description 1
- 230000000149 penetrating effect Effects 0.000 description 1
- 238000012827 research and development Methods 0.000 description 1
- 230000001629 suppression Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
- G01S13/86—Combinations of radar systems with non-radar systems, e.g. sonar, direction finder
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J11/00—Manipulators not otherwise provided for
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1628—Programme controls characterised by the control loop
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
- B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
- B25J9/1666—Avoiding collision or forbidden zones
-
- 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
- G01C21/1652—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 with ranging devices, e.g. LIDAR or RADAR
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
- G01S13/02—Systems using reflection of radio waves, e.g. primary radar systems; Analogous systems
- G01S13/06—Systems determining position data of a target
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
- G01S13/88—Radar or analogous systems specially adapted for specific applications
- G01S13/93—Radar or analogous systems specially adapted for specific applications for anti-collision purposes
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
- G01S7/02—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S13/00
- G01S7/28—Details of pulse systems
- G01S7/285—Receivers
- G01S7/292—Extracting wanted echo-signals
- G01S7/2923—Extracting wanted echo-signals based on data belonging to a number of consecutive radar periods
- G01S7/2927—Extracting wanted echo-signals based on data belonging to a number of consecutive radar periods by deriving and controlling a threshold value
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
- G01S7/02—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S13/00
- G01S7/35—Details of non-pulse systems
- G01S7/352—Receivers
- G01S7/354—Extracting wanted echo-signals
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Computer Networks & Wireless Communication (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Electromagnetism (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Manipulator (AREA)
Abstract
The application discloses a security robot based on millimeter wave and inertial fusion navigation, which is based on research of a frequency modulation millimeter wave navigation system, and aims to further improve the navigation precision of the security robot. Taking the navigation position output by the millimeter wave navigation system as an observed quantity, and establishing a measurement equation; the navigation information output by the inertial navigation system is used as a state quantity, and a state equation is established; outputting two navigation system devices as the input of a fusion filter, and researching and developing a security robot navigation system device based on millimeter wave and inertia fusion; and the navigation test of the robot in the smoke and dark environment is completed. The application improves the navigation precision of the security robot, can flexibly process the obstacle in the navigation path, and verifies the effectiveness and feasibility of the scheme.
Description
Technical Field
The invention relates to the technical field of robot navigation, in particular to a security robot based on millimeter wave and inertial fusion navigation.
Background
The security robot is an intelligent body for replacing people to execute dangerous work. In order for the security robot to operate autonomously in these environments, it is necessary to be able to acquire the position of the robot in real time and control its movement, so the navigation technology becomes one of the key technologies of the security robot;
The working environment of the security robot is complex, for example, disturbance of air density in a smoke environment can lead to shaking of a laser positioning focusing point, and the environment with insufficient light influences the visual sensor to receive environment information. Therefore, the visual and laser navigation methods are no longer applicable in dim, and heavy smoke environments. Aiming at the problem of robot navigation in smoke and dim environments, a sensor capable of overcoming the environment is needed to finish the robot navigation operation. In the research of a plurality of anti-interference signals, millimeter waves in the frequency range from 30GHz to 300GHz have the characteristics of infrared and microwave, and the device has the characteristics of low power consumption, strong anti-interference capability, good low elevation tracking performance, capability of penetrating plasma, stable performance, high Doppler frequency, simple structure and the like. Therefore, millimeter waves can be utilized to provide basic assurance for the security robot to complete navigation tasks in smoke, dim and other environments.
Under the background, the academic world has developed theoretical and technical researches on security robots based on millimeter waves, initially established a research and development platform of the security robots, developed security robot navigation systems based on frequency modulation millimeter waves, and navigation errors of about 0.11 meter.
In order to improve the navigation precision of the security robot to the centimeter level, so that the robot can be applied to a high-precision environment, a mode of fusing with other sensors is needed to obtain more accurate navigation information of the robot. Among the sensors, the inertial navigation sensor has the characteristics of strong anti-interference capability, good navigation precision in a short time, high autonomy and the like, and is widely focused by scientific researchers at home and abroad. Therefore, the inertial sensor can be utilized to fuse the robot position information output by the millimeter wave radar with the robot position information output by the inertial navigation system, and the optimal estimated position of the current robot is obtained through a correlation algorithm.
Disclosure of Invention
In view of the above, the invention provides a security robot navigation system based on millimeter wave and inertia fusion.
Based on the purposes, the invention provides a security robot navigation system based on millimeter wave and inertia fusion, and the software aspect comprises the following steps:
Processing clutter signals existing in millimeter wave radar receiving signals in real time;
Processing and setting a threshold value threshold of the millimeter wave radar in a dynamic interference environment in real time;
acquiring a distance information set between the millimeter wave radar and the security robot in real time by utilizing the threshold value;
Converting the distance information set into the distance between the millimeter wave radar and the geometric center of the security robot through a segmentation clustering method;
and converting the acquired distance information into position information of the space where the security robot is located by using a triangular positioning algorithm.
The optimal navigation path planning algorithm is used for dividing a navigation space, constructing a characteristic network and searching an optimal path on the characteristic network based on the Manhattan distance principle on the basis of robot kinematics; when an obstacle exists in the optimal path, the obstacle can be pushed open or the obstacle can be bypassed through a robot arm provided with a force feedback system; if the obstacle is inoperable, the robot requests the millimeter wave navigation system to reschedule a navigation path bypassing the obstacle; the robot arm of the force feedback system is provided with a force feedback sensor and a pre-application algorithm.
The speed increment of the robot in the motion state output by the real-time processing strapdown inertial navigation system is converted into the average speed of each sampling period;
real-time processing and acquiring the spatial attitude information of the robot;
and the average speed and the robot gesture information are utilized to calculate the spatial position information of the robot.
Establishing a state equation of the space position information of the robot solved by the strapdown inertial navigation system, and the state equation at the moment k:
Wherein the method comprises the steps of And/>The delta T is obtained by the calculation of the output speed increment and represents the sampling period set by the fusion core system; establishing a measurement equation for the robot space position information solved by the millimeter wave radar, and measuring the equation at the moment k:
Zk＝HZk+R
Wherein the measurement matrix R represents measurement noise based on a millimeter wave radar navigation system;
the iteration is updated by a filtering algorithm, and the specific processing is as follows:
(1) State estimation equation:
(2) Estimating a noise matrix of the equation:
Pk/k-1＝ΦPk-1ΦT+ΓQΓT
Wherein P k-1 is the optimal estimated error covariance at the previous moment, and Q is the error covariance matrix of the robot navigation system based on the inertial sensor;
(3) And (3) calculating the weight value:
Kk＝Pk/k-1HT(HPk/k-1HT+R)-1
wherein R is a robot navigation error covariance matrix based on millimeter waves;
(4) The optimal estimate updates the equation:
Sk＝Sk/k-1+Kk(Zk-HkSk/k-1)
(5) Noise matrix for optimal position estimation:
And further, in order to enable the robot navigation system to run on an embedded system and a Linux system, related programs are written in a C language.
Security robot navigation system based on millimeter wave and inertia fusion, the hardware aspect includes:
The differential signal data line is used for transmitting distance information between the millimeter wave and the robot;
The power supply module is used for providing voltages required by all modules of the whole device and comprises an electric storage module and a corresponding energy conversion device;
The storage module is used for a volume database corresponding to the obstacle outline, an optimal path navigation algorithm, a fusion filtering algorithm, a user-defined security task database and a robot treatment method aiming at the obstacle;
The wireless module is used for transmitting the resolved robot position information in real time;
The DSP module is used for realizing the calculation of signals and data calculation;
The processor module is used for controlling the robot to execute security tasks;
And the force feedback system is used for acquiring the physical quantity of the force required by the robot to capture the obstacle.
And the millimeter wave radar module is used for measuring the distance between the acquisition radar and the robot in the smoke dim environment and measuring the outline size of the obstacle.
And the strapdown inertial navigation system module is used for acquiring the position of the space where the robot is located.
The core processing module is used for processing the robot position information of the millimeter wave navigation system, the robot position information of strapdown inertial navigation measurement, the fusion algorithm output robot position information, the obstacle profile database and the robot navigation path track database; preferably, the core processing module comprises a CPU of a server and a motion control chip of the robot, and preferably, the robot core is ARM Cortex-M7.
From the above, it can be seen that the security robot navigation system based on millimeter wave and inertia fusion is provided by the invention. Based on researching the frequency modulation millimeter wave navigation system, in order to further improve the navigation precision of the security robot, according to the strapdown inertial navigation principle, researching the inertial navigation mathematical model of the security robot, developing a robot navigation experiment based on an inertial sensor, and acquiring relevant navigation error data. Constructing a state equation according to a robot motion principle; and constructing a measurement equation according to the principle of the millimeter wave navigation system. And designing a robot navigation system experimental platform based on millimeter wave and inertia fusion by using a filtering algorithm. And under the optimal navigation path, aiming at different obstacles, adopting a corresponding obstacle avoidance navigation mode, and experimental results show that: based on a fusion algorithm, under a set linear and curved navigation path, the running state of the robot is stable, the navigation error is about 0.08 m, the navigation precision is superior to that of a system before fusion, and the robot can clear roadblock under the optimal navigation path; and when the obstacle cannot be processed, re-path planning is performed, and the security robot can automatically run in the smoke, dim and other environments.
Drawings
In order to more clearly illustrate one or more embodiments of the present specification or the prior art, the drawings that are used in the description of the embodiments or the prior art will be briefly described below.
Fig. 1 is a schematic flow chart of a millimeter wave-based security robot navigation system of the present invention.
Fig. 2 is a schematic flow chart of a security robot navigation system based on an inertial sensor of the present invention.
Fig. 3 is a schematic diagram of a hardware fusion process of the security robot navigation system based on millimeter wave and inertia fusion.
Fig. 4 is a schematic diagram of software fusion processing of the security robot navigation system based on millimeter wave and inertia fusion.
Fig. 5 is a schematic diagram of a security robot navigation system based on millimeter wave and inertia fusion.
Fig. 6 is a communication schematic diagram of the security robot navigation system based on millimeter wave and inertia fusion.
Fig. 7 is a schematic diagram of a power module of the security robot navigation system based on millimeter wave and inertia fusion.
Fig. 8 is an experimental result of the security robot navigation system based on millimeter wave and inertia fusion.
Detailed Description
The present invention will be further described in detail below with reference to specific embodiments and with reference to the accompanying drawings, in order to make the objects, technical solutions and advantages of the present invention more apparent.
It should be noted that, in the embodiments of the present invention, all the expressions "first" and "second" are used to distinguish two entities with the same name but different entities or different parameters, and it is noted that the "first" and "second" are only used for convenience of expression, and should not be construed as limiting the embodiments of the present invention, and the following embodiments are not described one by one.
The embodiment of the invention provides a security robot based on millimeter wave and inertia fusion, which uses the navigation position output by a millimeter wave navigation system as an observed quantity to establish a measurement equation; the navigation information output by the inertial navigation system is used as a state quantity, and a state equation is established; designing a robot navigation system experimental platform based on millimeter wave and inertia fusion by using a Kalman filtering algorithm; the navigation experiment of the robot in the smoke and dark environment is completed, and the experimental result shows that: based on the method, the navigation precision of the robot is improved, and the effectiveness and feasibility of the scheme are verified.
Security robot based on millimeter wave and inertia fusion includes the following steps:
In a first aspect, an embodiment of the present invention provides a hardware system of a security robot based on millimeter wave and inertia fusion, fig. 1 is a schematic diagram of a security robot navigation system based on millimeter wave provided in the embodiment of the present invention, fig. 2 is a schematic diagram of a security robot navigation system based on an inertial sensor provided in the embodiment of the present invention, fig. 3 is a schematic diagram of a hardware fusion process of a security robot navigation system based on millimeter wave and inertia fusion provided in the embodiment of the present invention, and fig. 5 is a physical schematic diagram of a security robot navigation system based on millimeter wave and inertia fusion provided in the embodiment of the present invention. Fig. 6 is a communication schematic diagram of the security robot navigation system based on millimeter wave and inertia fusion. Fig. 7 is a schematic diagram of a power module of the security robot navigation system based on millimeter wave and inertia fusion.
The device comprises:
The power supply module is used for providing voltages required by all modules of the whole device and comprises an electric storage module and a corresponding energy conversion device;
The storage module is used for storing a fusion algorithm program and process key data;
The WIFI module is used for transmitting the position information data of the security robot in real time;
The server module is used for processing millimeter wave radar measurement distance information, inertial information measured by the inertial element, filtering information of the fusion algorithm and position information of the security robot;
The radar obstacle avoidance module is used for detecting the outline size of an obstacle;
The robot motion control module is used for solving an inverse kinematics equation of the robot in real time according to the set target position, acquiring motion parameters of the robot, performing closed-loop control, simultaneously performing optimal path planning, and processing obstacle profile data; preferably, the robot kernel is ARM Cortex-M7.
In a second aspect, an embodiment of the present invention provides a software system of a security robot based on millimeter wave and inertia fusion, whose basic architecture is as shown in fig. 4, where the method includes:
The millimeter wave navigation system firstly acquires echo signals of the target by utilizing a millimeter wave radar and extracts relevant distance information. And then setting passband frequency of the clutter suppression filter and a dynamic threshold, outputting related target distance information, finally accurately acquiring the distance between the millimeter wave radar and the geometric center of the target by adopting a segmentation clustering method, and arranging three or more radars indoors to realize navigation research based on multilateral positioning.
The fusion navigation system takes the navigation position output by the millimeter wave navigation system as an observed quantity, and establishes a measurement equation; the navigation information output by the inertial navigation system is used as a state quantity, and a state equation is established; running a filtering fusion algorithm program in an independent memory area of the server; preferably, the server programs are written in the C language.
The robot bottom layer control system is responsible for solving inverse kinematics equation of the robot, closed-loop control and processing obstacle avoidance data; preferably, the robot bottom layer control system adopts a Free-RTOS kernel; the final experimental result is shown in fig. 8, and it can be seen from fig. 8 that the security robot operates stably and the navigation precision is better than that of the system before fusion based on the fusion algorithm.
Finally, it should be noted that those of ordinary skill in the art should understand: the discussion of any of the embodiments above is merely exemplary and is not intended to suggest that the scope of the disclosure, including the claims, is limited to these examples; the technical features of the above embodiments or in the different embodiments may also be combined within the idea of the invention, the steps may be implemented in any order and there are many other variations of the different aspects of the invention as described above, which are not provided in detail for the sake of brevity.
Additionally, well-known power/ground connections to Integrated Circuit (IC) chips and other components may or may not be shown within the provided figures, in order to simplify the illustration and discussion, and so as not to obscure the invention. Furthermore, the devices may be shown in block diagram form in order to avoid obscuring the invention, and also in view of the fact that specifics with respect to implementation of such block diagram devices are highly dependent upon the platform within which the present invention is to be implemented (i.e., such specifics should be well within purview of one skilled in the art). Where specific details (e.g., circuits) are set forth in order to describe example embodiments of the invention, it should be apparent to one skilled in the art that the invention can be practiced without, or with variation of, these specific details. Accordingly, the description is to be regarded as illustrative in nature and not as restrictive.
While the invention has been described in conjunction with specific embodiments thereof, many alternatives, modifications, and variations of those embodiments will be apparent to those skilled in the art in light of the foregoing description.
The embodiments of the invention are intended to embrace all such alternatives, modifications and variances which fall within the broad scope of the appended claims. Therefore, any omission, modification, equivalent replacement, improvement, etc. of the present invention should be included in the scope of the present invention.
Claims (3)
1. Security robot based on millimeter wave and inertial fusion navigation, characterized by comprising: the power module is used for providing the voltage required by each module and comprises an electric storage module and an energy conversion device; the storage module is used for storing a volume database corresponding to the outline threshold value of the obstacle, an optimal path navigation algorithm, a fusion filtering algorithm, a user-defined security task database and a processing method of the robot for the obstacle; the server module is used for processing millimeter wave radar measurement distance information, inertial information measured by the inertial element, filtering information of the fusion algorithm and position information of the security robot; the core processing module is used for processing the robot position information of the millimeter wave navigation system, the robot position information of strapdown inertial navigation measurement, the fusion algorithm output robot position information, the obstacle profile database and the robot navigation path track database; the navigation system is fused, the navigation position output by the millimeter wave navigation system is used as an observed quantity, and a measurement equation is established; the navigation information output by the inertial navigation system is used as a state quantity, and a state equation is established;
The robot is used for realizing path navigation, and specifically comprises: solving an inverse kinematics equation of the robot in real time, acquiring motion parameters of the robot, performing closed-loop control, and planning an optimal navigation path; fusion processing of millimeter wave navigation information and inertial navigation information;
The optimal navigation path planning is used for dividing navigation space, constructing a characteristic network and searching an optimal path on the characteristic network based on the Manhattan distance principle on the basis of robot kinematics; when an obstacle exists in the optimal path, the obstacle can be pushed open or the obstacle can be bypassed through a robot arm provided with a force feedback system; if the obstacle is inoperable, the robot requests a re-planning of a navigation path around the obstacle from the millimeter wave navigation system.
2. The security robot based on millimeter wave and inertial fusion navigation according to claim 1, wherein the fusion navigation system specifically comprises:
the state equation at the moment k of the fusion processing of the millimeter wave navigation information and the inertial navigation information is as follows:
Wherein the method comprises the steps of And/>The delta T is obtained by the calculation of the output speed increment and represents the sampling period set by the fusion core system;
Taking the navigation position output by the millimeter wave navigation system as observed quantity, and establishing a measurement equation:
Measurement equation at time k:
Zk＝HZk+R
Wherein the method comprises the steps of R represents measurement noise based on a millimeter wave radar navigation system; according to the strapdown inertial navigation principle, an inertial navigation mathematical model of the security robot is established, a robot navigation experiment based on an inertial sensor is carried out, and navigation error data are obtained; and constructing a state equation according to the motion principle of the robot.
3. The security robot based on millimeter wave and inertial fusion navigation according to claim 1, wherein the workflow of the fusion navigation system comprises:
the speed increment of the robot in the motion state output by the real-time processing strapdown inertial navigation system is converted into the average speed of each sampling period;
real-time processing and acquiring the spatial attitude information of the robot;
The average speed and the robot gesture information are utilized to calculate the spatial position information of the robot;
the iteration is updated by a filtering algorithm, and the specific processing is as follows:
State estimation equation:
Calculating an optimal estimation update equation according to the measurement equation and the state estimation equation, and obtaining the position information of the security robot navigation system based on millimeter wave and inertia fusion:
Sk＝Sk/k-1+Kk(Zk-HkSk/k-1)。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110617824.3A CN113843788B (en) | 2021-06-03 | 2021-06-03 | Security robot based on millimeter wave and inertial fusion navigation |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110617824.3A CN113843788B (en) | 2021-06-03 | 2021-06-03 | Security robot based on millimeter wave and inertial fusion navigation |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113843788A CN113843788A (en) | 2021-12-28 |
| CN113843788B true CN113843788B (en) | 2024-04-19 |
Family
ID=78973075
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110617824.3A Active CN113843788B (en) | 2021-06-03 | 2021-06-03 | Security robot based on millimeter wave and inertial fusion navigation |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113843788B (en) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114994672B (en) * | 2022-08-03 | 2022-10-25 | 中国人民解放军国防科技大学 | Fire scene smoke scene positioning and mapping method and device based on millimeter wave radar inertia combination |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102608596A (en) * | 2012-02-29 | 2012-07-25 | 北京航空航天大学 | Information fusion method for airborne inertia/Doppler radar integrated navigation system |
| CN110658816A (en) * | 2019-09-27 | 2020-01-07 | 东南大学 | Mobile robot navigation and control method based on intelligent assembly |
| CN110750097A (en) * | 2019-10-17 | 2020-02-04 | 上海飒智智能科技有限公司 | Indoor robot navigation system and map building, positioning and moving method |
| CN111624641A (en) * | 2020-04-21 | 2020-09-04 | 上海东古智能科技有限公司 | Explosion-proof type intelligent inspection robot for oil depot area |
| CN112578394A (en) * | 2020-11-25 | 2021-03-30 | 中国矿业大学 | LiDAR/INS fusion positioning and drawing method with geometric constraint |
Family Cites Families (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110799804A (en) * | 2017-06-30 | 2020-02-14 | 深圳市大疆创新科技有限公司 | Map generation system and method |
| CN111307166B (en) * | 2018-12-11 | 2023-10-03 | 北京图森智途科技有限公司 | Method and device for constructing occupied grid map and processing equipment |
-
2021
- 2021-06-03 CN CN202110617824.3A patent/CN113843788B/en active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102608596A (en) * | 2012-02-29 | 2012-07-25 | 北京航空航天大学 | Information fusion method for airborne inertia/Doppler radar integrated navigation system |
| CN110658816A (en) * | 2019-09-27 | 2020-01-07 | 东南大学 | Mobile robot navigation and control method based on intelligent assembly |
| CN110750097A (en) * | 2019-10-17 | 2020-02-04 | 上海飒智智能科技有限公司 | Indoor robot navigation system and map building, positioning and moving method |
| CN111624641A (en) * | 2020-04-21 | 2020-09-04 | 上海东古智能科技有限公司 | Explosion-proof type intelligent inspection robot for oil depot area |
| CN112578394A (en) * | 2020-11-25 | 2021-03-30 | 中国矿业大学 | LiDAR/INS fusion positioning and drawing method with geometric constraint |
Non-Patent Citations (1)
| Title |
|---|
| 基于调频毫米波的安防移动机器人导航系统;郑睿 等;《仪器仪表学报》;第42卷(第3期);第105-113页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113843788A (en) | 2021-12-28 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Feng et al. | Kalman-filter-based integration of IMU and UWB for high-accuracy indoor positioning and navigation | |
| Xuexi et al. | SLAM algorithm analysis of mobile robot based on lidar | |
| Zhao et al. | Learning-based bias correction for time difference of arrival ultra-wideband localization of resource-constrained mobile robots | |
| Fang et al. | Graph optimization approach to range-based localization | |
| Xiong et al. | A scheme on indoor tracking of ship dynamic positioning based on distributed multi-sensor data fusion | |
| Ma et al. | An ultrasonic positioning algorithm based on maximum correntropy criterion extended Kalman filter weighted centroid | |
| Xu et al. | An improved indoor localization method for mobile robot based on WiFi fingerprint and AMCL | |
| Jiang et al. | Smartphone PDR/GNSS integration via factor graph optimization for pedestrian navigation | |
| Derkach et al. | Obstacle avoidance algorithm for small autonomous mobile robot equipped with ultrasonic sensors | |
| Lu et al. | Robot indoor location modeling and simulation based on Kalman filtering | |
| CN113843788B (en) | Security robot based on millimeter wave and inertial fusion navigation | |
| Sudheesh et al. | Non linear tracking using unscented Kalman filter | |
| Kong et al. | Hybrid indoor positioning method of BLE and PDR based on adaptive feedback EKF with low BLE deployment density | |
| Zhang et al. | A dynamic window-based UWB-Odometer fusion approach for indoor positioning | |
| Miller et al. | Trajectory optimization for continuous ergodic exploration on the motion group SE (2) | |
| Wu et al. | Research on laser navigation mapping and path planning of tracked mobile robot based on hector SLAM | |
| Lu et al. | An information potential approach for tracking and surveilling multiple moving targets using mobile sensor agents | |
| Zhang et al. | A new PHD-SLAM method based on memory attenuation filter | |
| Wang et al. | WiFi-PDR indoor fusion positioning based on EKF | |
| Kim et al. | Unscented information filtering method for reducing multiple sensor registration error | |
| Yuan et al. | Research on indoor positioning and navigation method of AGV based on multi-sensor fusion | |
| Wei et al. | Neural network based extended kalman filter for localization of mobile robots | |
| Aravind et al. | Enhancing GPS Position Estimation Using Multi-Sensor Fusion and Error-State Extended Kalman Filter | |
| Romaniuk et al. | Adaptation of Ultra Wide Band positioning system for Adaptive Monte Carlo Localization | |
| Akkaya et al. | An ANN based NARX GPS/DR system for mobile robot positioning and obstacle avoidance |
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 | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |