Multifunctional snakelike robot control method and device based on Beidou

Abstract

The invention provides a Beidou-based multifunctional snakelike robot control method and device, and relates to the technical field of biomimetic robots, and the method comprises the steps: constructing a snakelike robot motion model; predicting a path and a road sign position observed in a map according to the snakelike robot motion model and the control information and the observation information collected at each moment; according to the path and the road sign position observed in the map, the snakelike robot moves. According to the Beidou-based multifunctional snakelike robot control method and device, a transverse fluctuation coupling nonlinear oscillator system controls movement of a snakelike robot, and sensor modules related to pipeline detection, life search and rescue and water quality detection are added to the snakelike robot to achieve three different functions.

Description

Technical Field

[0001] The present invention relates to the technical field of bionic robots, and in particular to a Beidou-based multifunctional snake-like robot control method and device. Background Art

[0002] Among bionic robots, snake-like robots have received much attention due to their unique forms of movement and wide range of application scenarios. Snake-like robots can complete tasks in some special environments by imitating the snake's flexible and varied movement methods such as winding, rolling, traveling waves, and climbing. The future development of snake-like robots will have the following two trends: Multifunctional trend. The diversification of future usage scenarios will inevitably lead to the development of snake-like robots in the direction of multifunctionality. At present, scientists have developed various forms of bionic robots such as crawling robots, snake-like robots, and wall-climbing robots. The unique forms of movement help them complete specific tasks in different environments; Intelligent trend. With the development of artificial intelligence technology, bionic robots have transitioned from traditional purely mechanical to intelligent. The intelligence of bionic robots is mainly reflected in the diversification of tasks, the humanization of actions, and the precision of control levels. This is conducive to more perfect imitation of the physiological functions of organisms and more safe completion of tasks;

[0003] In recent years, some domestic universities and technology companies have developed various snake-like robots that can move independently, but the links in putting them into practical application are relatively weak. The development of domestic snake-like robots has a certain foundation, but it is still in the initial stage of application, and there is a lot of room for future development. Summary of the invention

[0004] The present invention provides a Beidou-based multifunctional snake-like robot control method and device. A lateral wave coupled nonlinear oscillator system controls the movement of the snake-like robot. Sensor modules related to pipeline detection, life search and rescue, and water quality detection are added to the snake-like robot to achieve three different functions.

[0005] In order to solve the above technical problems, the technical solution of the present invention is as follows:

[0006] In a first aspect, a Beidou-based multifunctional snake-like robot control method comprises:

[0007] Construct a snake robot motion model;

[0008] Predicting the path and the locations of the landmarks observed in the map based on the snake-like robot motion model and the control information and observation information collected at each moment;

[0009] The snake robot moves according to the path and the locations of landmarks observed in the map.

[0010] Furthermore, a snake robot motion model is constructed, including:

[0011] A lateral wave coupling nonlinear oscillator system is constructed to control the motion state of the snake-like robot. The nonlinear oscillator model is expressed as: Where x and v are state variables, E, τ, and α are constants that control the behavior of the oscillator;

[0012] Project a signal proportional to the state variables x and v from one oscillator to the other, where the calculation formula is:

[0013] a ij and b ij and constants define the coupling between different oscillators, τ i and E i are the parameters of the oscillator.

[0014] Furthermore, based on the snake-like robot motion model and the control information and observation information collected at each moment, the path and the positions of the landmarks observed in the map are predicted, including:

[0015] The robot estimates the path and the location of the landmarks through control information and multiple observations of the landmarks in the map. t ,θ|z t ,u t ,n t ) solves the posterior probability of the robot's moving path and map, where s t ={s1, s2, ..., s t} represents the path of the robot's movement, which is composed of the position points s at each moment t Composition; θ={θ1, θ2,…,θ i} represents the map observed by the robot, which is composed of landmarks θ i Composition; t ={z1, z2, ..., z t} represents the observed quantity, u t = {u1, u2, ..., u t} represents the control amount, n t ={n1, n2, ..., n t} represents the correlation variable, n t is the identifier of the road sign observed at time t.

[0016] Furthermore, in the two-dimensional case, the posture s t Indicates position p t and the rotation angle φ t ,in, Angle φ t The corresponding rotation matrix is ​​Rt ; The relative position of the robot at consecutive moments Δp t , and the relative position of the robot and the observed landmark Δl jt It is expressed as:

[0017]

[0018] in, and Respectively represent the robot motion noise and observation noise; the relative rotation of the robot at consecutive moments ΔR t It is expressed as:

[0019]

[0020] in, Indicated by noise The rotation matrix formed; Satisfies Langevin distribution; let ε P t The variance is C Pt , The concentration parameter is The variance of Using the chord distance to represent the angle deviation, the solution to the maximum value of the nonlinear oscillator model is converted to The minimum value of is solved, where the subscript is the covariance matrix in the form of ||A|| 2 C=A T C -1 The norm of A and F is R={R1,R2,…,R t} represents the rotation matrix corresponding to the rotation angle at each moment, P = {p1, p2, ..., p t} indicates the position at each moment.

[0021] Furthermore, based on the snake-like robot motion model and the control information and observation information collected at each moment, the path and the position of the landmarks observed in the map are predicted, which also includes:

[0022] Based on the path and the location of the landmarks observed on the map, The prediction of the map is decomposed into separate estimates of different landmarks. Among them, FastSLAM consists of a particle filter and multiple separate EKFs. The particle filter estimates the path of the mobile robot, and the EKF is based on p(θ i |S t ,Z t ,n t ) completes the prediction of a single landmark.

[0023] Furthermore, the particle in FastSLAM contains the particle's historical path and the estimation of each landmark, expressed as:

[0024]

[0025] in, represents particle i at time t, represents the mean and covariance of landmark j observed by particle i at time t, Represents the total number of landmarks observed by particle i from the initial time to time t.

[0026] Furthermore, FastSLAM includes prediction, updating and importance sampling. Prediction and updating form a temporary particle set based on observation information and control information. When sampling importance, weights are assigned to particles, where the weight calculation formula is: in is the weight of particle i at time t.

[0027] In the second aspect, a Beidou-based multifunctional snake-like robot control device comprises:

[0028] A driving module, used to build a snake robot motion model;

[0029] The control module is used to predict the path and the positions of the landmarks observed in the map according to the movement model of the snake-like robot and the control information and observation information collected at each moment; the snake-like robot moves according to the path and the positions of the landmarks observed in the map.

[0030] According to a third aspect, a computing device includes:

[0031] one or more processors;

[0032] The storage device is used to store one or more programs. When the one or more programs are executed by the one or more processors, the one or more processors implement the above method.

[0033] In a fourth aspect, a computer-readable storage medium stores a program, and the program implements the above method when executed by a processor.

[0034] The above solution of the present invention includes at least the following beneficial effects:

[0035] The above scheme of the present invention uses a transverse wave coupling nonlinear oscillator system to control the movement of the snake-like robot, so that the robot uses a transverse wave method to control the movement speed and direction of the robot when moving on water or land; sensor modules related to pipeline detection, life search and rescue, and water quality detection are added to the snake-like robot to realize three different functions; pipeline detection, using the high positioning accuracy of the Beidou navigation system, using a high-degree-of-freedom snake-like robot as a carrier, carrying an ultrasonic object flaw detection sensor, so that it can pass through various complex pipelines for detection, and can respond to the staff with the precise location of cracks, deformations, and damaged parts inside the pipeline; life search and rescue, using the Beidou navigation system to plan the route, send the robot to the place where the dangerous accident occurs, and use the snake-like robot's strong ability to move in a complex environment to send air particles, harmful gases, cameras, temperature, ultrasonic waves, human infrared and other sensors into the danger zone to find out the location of the injured in the danger zone, collect relevant environmental data, and automatically generate a danger level coefficient to provide a reference for rescue personnel. BRIEF DESCRIPTION OF THE DRAWINGS

[0036] Figure 1 It is a flow chart of a Beidou-based multifunctional snake-like robot control method provided in an embodiment of the present invention.

[0037] Figure 2 is a schematic diagram of a Beidou-based multifunctional snake-like robot control device provided by an embodiment of the present invention;

[0038] Figure 3 It is a simulation experiment schematic diagram of the Beidou-based multifunctional snake-like robot control method and device method provided in an embodiment of the present invention.

[0039] Figure 4 It is a flow chart of a pipeline detection robot of a Beidou-based multifunctional snake-like robot control method provided by an embodiment of the present invention;

[0040] Figure 5 It is a flowchart of a search and rescue snake-like robot according to a Beidou-based multifunctional snake-like robot control method provided by an embodiment of the present invention;

[0041] Figure 6 It is a flowchart of a search and rescue snake-like robot according to a Beidou-based multifunctional snake-like robot control method provided in an embodiment of the present invention. DETAILED DESCRIPTION

[0042] The exemplary embodiments of the present disclosure will be described in more detail below with reference to the accompanying drawings. Although the exemplary embodiments of the present disclosure are shown in the accompanying drawings, it should be understood that the present disclosure can be implemented in various forms and should not be limited by the embodiments set forth herein. On the contrary, these embodiments are provided to enable a more thorough understanding of the present disclosure and to fully convey the scope of the present disclosure to those skilled in the art.

[0043] like Figure 1 As shown, an embodiment of the present invention proposes a Beidou-based multifunctional snake-like robot control method and device method, the method comprising:

[0044] Step 11, constructing a snake-like robot motion model;

[0045] Step 12 predicts the path and the locations of the landmarks observed in the map according to the snake-like robot motion model and the control information and observation information collected at each moment;

[0046] Step 13, the snake robot moves according to the path and the locations of the landmarks observed in the map.

[0047] In an embodiment of the present invention, by establishing a motion model of the snake-like robot, the snake-like robot can be controlled and the path planning can be performed, and the model includes information such as the robot's motion mode and motion constraints; through the snake-like robot motion model and control and observation information, the robot's path and the positions of landmarks in the map can be predicted, which can be used for subsequent mobile control; according to the predicted path and the positions of landmarks observed in the map, the snake-like robot can move, and by controlling the robot's motion mode, the robot's navigation and path tracking can be achieved; by predicting the path and the positions of landmarks in the map, the robot can accurately navigate and plan the path, thereby achieving precise movement, and can be applied to various fields, such as pipeline detection, life search and rescue, sewage detection, etc., with high practicality and effectiveness.

[0048] like Figure 1 As shown, step 11, constructing a snake-like robot motion model, specifically includes:

[0049] Step 111, construct a lateral wave coupling nonlinear oscillator system for controlling the motion state of the snake-like robot. The nonlinear oscillator model is expressed as: Where x and v are state variables, E, τ, and α are constants that control the behavior of the oscillator;

[0050] Step 112, projecting a signal proportional to the state variables x and v from one oscillator to another oscillator, wherein the calculation formula is:

[0051] a ij and bij and constants define the coupling between different oscillators, τ i and E i are the parameters of the oscillator.

[0052] In the embodiment of the present invention, the coupled nonlinear oscillator system is used to control the motion state of the snake-like robot, which can reduce the number of state variables and parameters in the model, making the body of the snake-like robot more modular, which is conducive to assembly and control; the nonlinear oscillator model is expressed as:

[0053]

[0054] In this equation, x and v are state variables, E, τ, and α are constants that control the behavior of the oscillators; the variable x will determine the angle required for the corresponding robot element to move, E controls the amplitude of its oscillation, and τ controls the period of the oscillation. Each segment of the robot is equivalent to an oscillator, and a specific gait pattern requires coupling several oscillators together; coupling is the process of projecting a signal proportional to the x and v states from one oscillator to another oscillator, and the calculation formula is:

[0055]

[0056] a ij and b ij The constant defines the coupling between different oscillators, that is, the influence of the jth oscillator on the i-th oscillator; by changing the oscillator parameter τ i and E i , the period and amplitude of the oscillation can be easily adjusted separately; making τ i Doubling can double the amplitude. i Halving has the effect of reducing the amplitude; despite the sudden change, the oscillations in the chain quickly and smoothly adapt to the new period and the new amplitude later; therefore, these parameters provide the possibility to adjust the speed of movement according to the changing conditions;

[0057] Perform simulation experiments on Adams software. Convert the model made in SolidWorks into .x_t format and import it into the simulation software Adams. Set constraints, kinematic pairs, contact forces, and drive functions. By changing the parameter τ i and E i To find the parameter value with the fastest speed;

[0058] By identifying valid action waves, the type of gait that produces the fastest motion is determined. For a given oscillation frequency, the wave parameters are tested within an acceptable range. An open-loop sinusoidal controller is used for identification. The set point (desired angle) for the i-th joint is generated as follows:

[0059]

[0060] By changing the amplitude A, the lag phase and frequency ν, different waves were simulated and tested; the oscillation amplitude A was controlled to be between 10 and 45 with a step size of 5; the phase lag It varies from 0.25 / N to 1.5 / N with a step size of 0.25 / N, where N is the number of driven joints in the robot; a phase lag of 1 / N corresponds to a wavelength of one body length, i.e. the body emits a complete S-shaped wave;

[0061] The speed is optimal when the vibration amplitude is 45, which is the maximum amplitude that the robot can physically produce; the dependence on the phase difference is also easy to see, and its optimal value is about That is, the wavelength is equivalent to twice the length of the robot; the frequency also affects the movement speed, but the effect is not significant compared with the amplitude and phase difference, and the optimal frequency is 0.5Hz; when ν=0.75Hz, the maximum speed of the motor does not allow the joint to reach a full oscillation amplitude of 45, and the maximum amplitude that can be achieved is about 20, so the result of an amplitude greater than 20 is not significant; the same is true for the larger amplitude at ν=0.5Hz, where the maximum amplitude is 30; at this point, the above data can be used to adjust the microcontroller to control the servo motor, allowing it to rotate to the corresponding angle and amplitude to achieve a suitable speed.

[0062] like Figure 1 As shown, step 12, based on the snake-like robot motion model and the control information and observation information collected at each moment, predicting the path and the position of the landmarks observed in the map, includes:

[0063] The robot estimates the path and the location of the landmarks through control information and multiple observations of the landmarks in the map. t ,θ|z t ,u t ,n t ) solves the posterior probability of the robot's moving path and map, where s t ={s1, s2, ..., s t} represents the path of the robot's movement, which is composed of the position points s at each moment t Composition; θ={θ1, θ2,…,θ i} represents the map observed by the robot, which is composed of landmarks θ i Composition; t ={z1, z2, ..., z t} represents the observed quantity, u t = {u1, u2, ..., u t} represents the control amount, n t ={n1, n2, ..., nt} represents the correlation variable, n t is the identifier of the road sign observed at time t.

[0064] In the two-dimensional case, the pose s t Indicates position p t and the rotation angle φ t ,in, Angle φ t The corresponding rotation matrix is ​​R t ; The relative position of the robot at consecutive moments Δp t , and the relative position of the robot and the observed landmark Δl jt It is expressed as:

[0065]

[0066] in, and Respectively represent the robot motion noise and observation noise; the relative rotation of the robot at consecutive moments ΔR t It is expressed as:

[0067]

[0068] in, Indicated by noise The rotation matrix formed; Satisfies Langevin distribution; let ε P t The variance is C Pt , The concentration parameter is The variance of Using the chord distance to represent the angle deviation, the solution to the maximum value of the nonlinear oscillator model is converted to The minimum value of is solved, where the subscript is the covariance moment

[0069] The array is in the form of ||A|| 2 C=A T C -1 The norm of A and F is R={R1,R2,…,R t} represents the rotation matrix corresponding to the rotation angle at each moment, P = {p1, p2, ..., p t} indicates the position at each moment.

[0070] like Figure 1 As shown, step 13, predicting the path and the position of the landmarks observed in the map according to the snake-like robot motion model and the control information and observation information collected at each moment, also includes:

[0071] Based on the path and the location of the landmarks observed on the map, The prediction of the map is decomposed into separate estimates of different landmarks. Among them, FastSLAM consists of a particle filter and multiple separate EKFs. The particle filter estimates the path of the mobile robot, and the EKF is based on p(θ i |S t ,Z t ,n t ) completes the prediction of a single landmark.

[0072] A particle in FastSLAM contains the particle's historical path and an estimate of each landmark, expressed as:

[0073]

[0074] in, represents particle i at time t, represents the mean and covariance of landmark j observed by particle i at time t, Represents the total number of landmarks observed by particle i from the initial time to time t.

[0075] FastSLAM includes prediction, update and importance sampling. Prediction and update form a temporary particle set based on observation information and control information. When sampling importance, weights are assigned to particles, where the weight calculation formula is: in is the weight of particle i at time t.

[0076] In the embodiment of the present invention, the prediction and update steps form a temporary particle set according to the observation information and the control information, and the estimation of the path by the temporary particle set is called a proposed distribution; since the proposed distribution is similar to the target distribution p(θ i |S t ,Z t ,n t ), so the particles are given weights in the resampling step, and the weight size is:

[0077]

[0078] in is the weight of particle i at time t. Particles with weights form a new particle set, and the information in the particle with the largest weight is generally selected as the path and map estimation at the current moment; the FastSLAM algorithm has the problem of particle degradation, that is, the number of particles with large weights is small, and the particle filter has deviations in the path estimation; therefore, the FastSLAM algorithm needs to resample the particles; the judgment method for executing the resampling step is to set a threshold for the number of valid particles, and resample when it is less than the threshold Neff; the calculation method for the number of valid particles is:

[0079]

[0080] Where n p represents the number of particles, and the weight in the formula is the normalized weight.

[0081] In the embodiment of the present invention, the environment of the simulation experiment is a rectangle of 7m×8m. The robot starts from the origin in the lower left corner and contains 42 landmarks. The robot can observe the landmarks within 1 meter of its own position within a 180° range, and the data association of all landmarks is known, as shown in the following figure. Figure 3 As shown;

[0082] Lines represent the robot's moving path and asterisks represent the locations of landmarks. Lines represent the robot's moving path and circles represent the locations of landmarks. The data set collected from the simulation environment in this experiment includes 82 sets of odometer data between robots and 1,348 sets of observation data of 42 landmarks by robots. It can be seen that the FastSLAM algorithm's estimates of the path and landmarks are very close to the true values.

[0083] like Figure 2 As shown, an embodiment of the present invention also provides a Beidou-based multifunctional snake-like robot control method and device 20, comprising:

[0084] A driving module, used to build a snake robot motion model;

[0085] The control module is used to predict the path and the positions of the landmarks observed in the map according to the movement model of the snake-like robot and the control information and observation information collected at each moment; the snake-like robot moves according to the path and the positions of the landmarks observed in the map.

[0086] Optional, drive module The drive module is composed of each section of the snake's body. The snake-like robot has a total of nine sections. Except for the head, each section of the robot is equipped with a servo motor. The drive module is realized by the coordinated rotation of each servo motor in the robot's body. The coordinated rotation of the servo motor is controlled by the control module. Regarding the coordinated rotation of the servo motor, it is not actually the case that the faster the servo motor rotates, the faster the snake crawls. The crawling speed of the snake is in a coordinated relationship with the frequency and amplitude of the servo motor's rotation. Through mathematical modeling and simulation experiments, the frequency and amplitude of the servo motor's rotation can be calculated. The relationship between the movement speed of the snake and the snake is to better control the snake robot to reach a suitable speed; the snake robot has a high degree of freedom and flexibility, and has the ability to avoid joint overruns and strange positions, and can complete obstacle avoidance, trajectory tracking and other movement effects in a small environment; the material of the snake robot shell is selected to be electroplated NI-W (NI-WP) alloy. The Ni-W alloy coating is a functional coating with high hardness, low stress, excellent heat resistance, corrosion resistance and wear resistance. The coating has the properties of acid resistance, heat resistance, wear resistance and high hardness, which prevents it from being damaged in some harsh environments. , especially in some acidic environments, it can prevent corrosion very well, and it also provides better high temperature protection for itself during fire rescue. Its wear-resistant and high hardness performance can also make it less likely to deform during earthquake rescue. The snake body structure model is designed using SolidWorks software. The hollow interior of the snake head can encapsulate the main control chip and communication module, which can provide good protection. The cylindrical part of the snake head is designed to enable it to better connect with the cylindrical protrusion of the next section. The front of the snake body is a cylindrical protrusion, and the inside is a cylindrical depression, which is beneficial to the two sections of the body. The interconnected parts between them, the empty part in the middle is mainly to reduce the overall weight of the snake-like robot, and can be filled with counterweights to achieve the effect of coinciding its center of gravity with the geometric center, so that it can maintain balance in the water; the robot's limbs are to enhance the robot's athletic ability on land; the modeling of the robot's tail is inspired by the tail of a sea snake, which can enable the robot to swim faster in the water; a servo motor is installed inside the robot to provide power to the robot; the outer edges and corners of the snake-like robot are rounded to achieve a high degree of bionic similarity and beautiful effect;

[0087] The control module consists of an STM32L0 chip encapsulated in the snake head and an STM32L0 chip encapsulated in each section of the snake body; the function of the STM32L0 chip in each section of the robot body is to send PWM signals with different duty cycles to the servo motor to control the rotation of the servo motor; the function of the STM32L0 chip in the robot head is to communicate with the STM32L0 in each section of the body, and instruct the STM32L0 in the body to output the required PWM signal so that the servo motor can rotate in a coordinated manner; the STM32L0 in the head and body achieve synchronous communication through the I2C bus.

[0088] Optionally, the snake-like robot further includes a communication module, which is composed of a Beidou navigation module and a 4G module and is encapsulated in the head of the snake. The Beidou navigation module is responsible for providing location information, and the 4G module is responsible for uploading the information collected by the snake-like robot to a database, and the mobile phone and computer will read the information from the database;

[0089] The snake-like robot also includes special function modules, which are mainly composed of various sensors; in order for the snake-like robot to achieve three different functions, the special function modules are divided into the following three combinations, as shown in the following table:

[0090]

[0091]

[0092] The sensors corresponding to the special function modules will be installed in various parts of the robot's body according to the content to be detected and its functional characteristics. Among them, the water quality monitoring robot, due to its special working environment, must make its body center of gravity and geometric center coincide with each other so that the robot can maintain balance in the water. Therefore, when installing the sensors, it is necessary to weigh their optimal installation positions; all sensors are connected to the STM32L0 chip on the snake's head, and all detected information is integrated into the snake's head, and sent to the computer or mobile phone APP of relevant detection or search and rescue personnel through the communication module on the snake's head.

[0093] In the embodiment of the present invention, three types of snake-like robots are designed, including a pipeline detection robot, a search and rescue snake-like robot, and a water quality detection robot;

[0094] Pipeline inspection robot, reference Figure 4, the ultrasonic flaw detection sensor will detect cracks, holes and deformations on the inner wall of the pipeline, the camera will take photos of the lesions, and then obtain the location of the lesions through the Beidou navigation system, and finally send the location information, damage and photos of the lesions to the database through the 4G chip, and the inspectors will use mobile phones or computers to receive the information on the database; the robot will initialize and enter the pipeline first, then start the positioning module to prepare for movement, start the ultrasonic flaw detection sensor to detect the pipeline and start moving; when the pipeline is detected to be damaged, the camera will be started to take pictures, and then the information of the damaged part will be uploaded to the database, and it will be determined whether it is finished. If it is finished, the robot will return to the original route or wait for recovery. If it is not finished, the infrared obstacle avoidance module will be started to bypass the damaged part and go to the next inspection; during the inspection, the internal situation of the pipeline can be reflected in real time. If there is a crack in the pipeline, an alarm will be issued immediately to notify the staff, which can avoid the further expansion of the pipeline wound. The use of snake-like robots as carriers makes the robot more flexible and the detection cost will be reduced accordingly;

[0095] Sewage detection robot, reference Figure 5 The robot first starts automatic navigation and prepares to move. Then the robot moves by calling the navigation path in the database. After the robot reaches the preset detection point, it turns on the sensor for detection and sends the detection information to the database in real time. After detecting all the detection points, the robot will determine whether to add new detection points. If there are new detection points, the robot will continue to detect. If not, it will return to the initial position. The snake robot can detect at various positions and depths of the water body and send the data to the detection personnel in real time. The Beidou navigation system is used to plan the robot's route in rivers and lakes, and the real-time water quality conditions are returned to the inspectors through sensors such as PH, conductivity, turbidity, and water temperature.

[0096] Life-saving robot, reference Figure 6 First, start the navigation and positioning module on the robot, the robot is ready to move after initialization, and then set the navigation mode to automatic navigation. If it is, the robot will call the database data to obtain the navigation path. If not, the robot will be controlled by the rescue personnel on the software according to its location information and camera. When the robot reaches the danger zone, it will turn on all sensors to collect environmental information, and finally upload the collected information and determine whether to go to the next location for search and rescue; when an accident occurs, such as fire, earthquake, chemical raw material leakage, the snake robot will replace people to reach the danger zone, find the location of the wounded, and collect environmental information of the danger zone, such as smoke concentration, whether there is toxic gas in the environment, etc. Through this information, rescue personnel can better plan rescue plans and make corresponding safety protection, so that the life safety of rescue personnel can be better guaranteed.

[0097] It should be noted that the device is a device corresponding to the above method, and all implementation methods in the above method embodiment are applicable to this embodiment and can achieve the same technical effect.

[0098] The embodiment of the present invention further provides a computing device, comprising: a processor, a memory storing a computer program, wherein when the computer program is executed by the processor, the method described above is executed. All implementations in the above method embodiment are applicable to this embodiment and can achieve the same technical effect.

[0099] The embodiment of the present invention also provides a computer-readable storage medium storing instructions, which, when executed on a computer, enable the computer to execute the method described above. All implementations in the above method embodiment are applicable to this embodiment and can achieve the same technical effect.

[0100] Those of ordinary skill in the art will appreciate that the units and algorithm steps of each example described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are performed in hardware or software depends on the specific application and design constraints of the technical solution. Professional and technical personnel can use different methods to implement the described functions for each specific application, but such implementation should not be considered to be beyond the scope of the present invention.

[0101] Those skilled in the art can clearly understand that, for the convenience and brevity of description, the specific working processes of the systems, devices and units described above can refer to the corresponding processes in the aforementioned method embodiments and will not be repeated here.

[0102] In the embodiments provided by the present invention, it should be understood that the disclosed devices and methods can be implemented in other ways. For example, the device embodiments described above are only schematic. For example, the division of the units is only a logical function division. There may be other division methods in actual implementation, such as multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed. Another point is that the mutual coupling or direct coupling or communication connection shown or discussed can be through some interfaces, indirect coupling or communication connection of devices or units, which can be electrical, mechanical or other forms.

[0103] The units described as separate components may or may not be physically separated, and the components shown as units may or may not be physical units, that is, they may be located in one place or distributed on multiple network units. Some or all of the units may be selected according to actual needs to achieve the purpose of the solution of this embodiment.

[0104] In addition, each functional unit in each embodiment of the present invention may be integrated into one processing unit, or each unit may exist physically separately, or two or more units may be integrated into one unit.

[0105] If the functions are implemented in the form of software functional units and sold or used as independent products, they can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention, or the part that contributes to the prior art, or the part of the technical solution, can be embodied in the form of a software product. The computer software product is stored in a storage medium, including several instructions for a computer device (which can be a personal computer, server, or network device, etc.) to perform all or part of the steps of the method described in each embodiment of the present invention. The aforementioned storage medium includes: various media that can store program codes, such as USB flash drives, mobile hard disks, ROM, RAM, magnetic disks, or optical disks.

[0106] In addition, it should be noted that in the apparatus and method of the present invention, it is obvious that each component or each step can be decomposed and / or recombined. These decompositions and / or recombinations should be regarded as equivalent schemes of the present invention. Moreover, the steps of performing the above-mentioned series of processing can naturally be performed in chronological order according to the order of description, but it is not necessary to perform them in chronological order, and some steps can be performed in parallel or independently of each other. For those of ordinary skill in the art, it is understood that all or any steps or components of the method and apparatus of the present invention can be implemented in any computing device (including processors, storage media, etc.) or a network of computing devices in hardware, firmware, software or a combination thereof, which can be achieved by those of ordinary skill in the art using their basic programming skills after reading the description of the present invention.

[0107] Therefore, the purpose of the present invention can also be achieved by running a program or a group of programs on any computing device. The computing device can be a well-known general device. Therefore, the purpose of the present invention can also be achieved by simply providing a program product containing a program code that implements the method or device. That is to say, such a program product also constitutes the present invention, and the storage medium storing such a program product also constitutes the present invention. Obviously, the storage medium can be any well-known storage medium or any storage medium developed in the future. It should also be pointed out that in the device and method of the present invention, it is obvious that each component or each step can be decomposed and / or recombined. These decompositions and / or recombinations should be regarded as equivalent schemes of the present invention. In addition, the steps of performing the above-mentioned series of processing can naturally be performed in chronological order according to the order of description, but it is not necessary to perform them in chronological order. Some steps can be performed in parallel or independently of each other.

[0108] The above is a preferred embodiment of the present invention. It should be pointed out that for ordinary technicians in this technical field, several improvements and modifications can be made without departing from the principles of the present invention. These improvements and modifications should also be regarded as the scope of protection of the present invention.

Claims

1. A Beidou-based multifunctional snake-like robot control method, characterized in that: The method comprises: Construct a snake robot motion model; Predicting the path and the locations of the landmarks observed in the map based on the snake-like robot motion model and the control information and observation information collected at each moment; The snake robot moves according to the path and the locations of landmarks observed in the map.

2. The Beidou-based multifunctional snake-like robot control method and device method according to claim 1 is characterized in that: Construct a snake robot motion model, including: A lateral wave coupling nonlinear oscillator system is constructed to control the motion state of the snake-like robot. The nonlinear oscillator model is expressed as: Where x and v are state variables, E, τ, and α are constants that control the behavior of the oscillator; Project a signal proportional to the state variables x and v from one oscillator to the other, where the calculation formula is: a ij and b ij and constants define the coupling between different oscillators, τ i and E i are the parameters of the oscillator.

3. The Beidou-based multifunctional snake-like robot control method and device method according to claim 2 is characterized in that: According to the snake-like robot motion model, and the control information and observation information collected at each moment, the path and the positions of the landmarks observed in the map are predicted, including: The robot estimates the path and the location of the landmarks through control information and multiple observations of the landmarks in the map. t ,θ|z t ,u t ,n t ) solves the posterior probability of the robot's moving path and map, where s t ={s1, s2, ..., s t } represents the path of the robot's movement, which is composed of the position points s at each moment t Composition; θ={θ1, θ2,…,θ i } represents the map observed by the robot, which is composed of landmarks θ i Composition; t ={z1, z2, ..., z t } represents the observed quantity, u t = {u1, u2, ..., u t } represents the control amount, n t ={n1, n2, ..., n t } represents the correlation variable, n t is the sign of the road sign observed at time t.

4. The Beidou-based multifunctional snake-like robot control method and device method according to claim 3 is characterized in that: In the two-dimensional case, the pose s t Indicates position p t and the rotation angle φ t ,in, Angle φ t The corresponding rotation matrix is ​​R t ; The relative position of the robot at consecutive moments Δp t , and the relative position of the robot and the observed landmark Δl jt It is expressed as: in, and Respectively represent the robot motion noise and observation noise; the relative rotation of the robot at continuous moments ΔR t It is expressed as: in, Indicated by noise The rotation matrix formed; Satisfies Langevin distribution; let εP t The variance is C Pt , The concentration parameter is The variance of Using the chord distance to represent the angle deviation, the solution to the maximum value of the nonlinear oscillator model is converted to The minimum value of is solved, where the subscript is the covariance matrix in the form of ||A|| 2 C=A T C -1 The norm of A and F is R={R1,R2,…,R t } represents the rotation matrix corresponding to the rotation angle at each moment, P = {p1, p2, ..., p t } indicates the position at each moment.

5. The Beidou-based multifunctional snake-like robot control method and device method according to claim 4 is characterized in that: According to the snake-like robot motion model, and the control information and observation information collected at each moment, the path and the position of the landmarks observed in the map are predicted, and further comprising: Based on the path and the location of the landmarks observed on the map, The prediction of the map is decomposed into separate estimates of different landmarks. Among them, FastSLAM consists of a particle filter and multiple separate EKFs. The particle filter estimates the path of the mobile robot, and the EKF is based on p(θ i |S t ,Z t ,n t ) completes the prediction of a single landmark.

6. The Beidou-based multifunctional snake-like robot control method and device method according to claim 5 is characterized in that: A particle in FastSLAM contains the particle's historical path and an estimate of each landmark, expressed as: Among them, Y t [i] represents particle i at time t, represents the mean and covariance of landmark j observed by particle i at time t, Represents the total number of landmarks observed by particle i from the initial time to time t.

7. The Beidou-based multifunctional snake-like robot control method and device method according to claim 6 is characterized in that: FastSLAM includes prediction, update and importance sampling. Prediction and update form a temporary particle set based on observation information and control information. When sampling importance, weights are assigned to particles, where the weight calculation formula is: in is the weight of particle i at time t.

8. A Beidou-based multifunctional snake-like robot control method and device, characterized in that: include: A driving module, used to build a snake robot motion model; The control module is used to predict the path and the positions of the landmarks observed in the map according to the movement model of the snake-like robot and the control information and observation information collected at each moment; the snake-like robot moves according to the path and the positions of the landmarks observed in the map.

9. A computing device, characterized in that include: one or more processors; A storage device, used for storing one or more programs, when the one or more programs are executed by the one or more processors, the one or more processors implement the method according to any one of claims 1 to 7.

10. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores a program, which, when executed by a processor, implements the method according to any one of claims 1 to 7.

Images