CN116429101A - Track tracking control system and method based on inertial navigation - Google Patents

Abstract

The present application provides a trajectory tracking control system and method based on inertial navigation. The trajectory tracking control system based on inertial navigation is based on the nine-axis sensing system in the inertial navigation system, and an additional An odometer, which fuses the data collected by the odometer with the data collected by other sensors, thereby reducing the cumulative error of the pose information and improving the accuracy of the pose information; on the other hand, the method provided by the application In the process of data transmission, the data to be transmitted is first converted into INT16 format data, and then 2.4GHz communication method is used for data transmission, which not only increases the transmission distance, but also increases the data transmission volume.

Description

Trajectory tracking control system and method based on inertial navigation

technical field

The application belongs to the field of computers and control systems, and in particular relates to a trajectory tracking control system and method based on inertial navigation.

Background technique

Navigation technology has rapidly developed from the military field to the civilian field, benefiting all walks of life. The most commonly used navigation systems include inertial navigation system (INS, referred to as "inertial navigation system") and GPS navigation system. Among them, the inertial navigation system is an autonomous navigation system that does not depend on external information or radiate energy to the outside. Commonly used sensors for positioning and motion control, specifically including strapdown inertial navigation system and platform inertial navigation system, because strapdown inertial navigation system has high reliability, strong function, light weight, low cost, high precision and flexible use And other advantages, making the strapdown inertial navigation system has become the mainstream of the development of today's inertial navigation system. The strapdown inertial navigation system mainly includes a six-axis sensing system and a nine-axis sensing system. Among them, the nine-axis sensing system is also called an inertial sensing system, and generally includes an acceleration sensor and an angular velocity sensor, and their single, double, Three-axis combined inertial measurement unit (Inertial Measurement Unit, IMU) and magnetic sensor attitude reference system (Attitude and heading reference system, AHRS).

The nine-axis sensing system can be applied to scenarios where the control terminal is separated from the operating carrier, for example, it can be applied to underwater robots, unmanned taxis, or search and rescue robots. In practical applications, the control terminal is generally controlled by the operator. Hold, while the operating carrier, such as the underwater robot body, is separated from the operator and located in the actual environment where the operator needs to go. The operator can set the target path of the operating carrier through the control terminal, and monitor the actual operating path of the operating carrier and various parameters. Furthermore, the target path can be changed at any time according to conditions such as the actual running path and the current pose.

The pose of the running carrier can be determined based on the data collected by the nine-axis sensing system. At present, there are many algorithms for determining the pose of the running carrier, such as Kalman filter, Madgwick algorithm, etc., but the main ideas are based on The data collected by the inertial navigation is integrated. Therefore, there is an accumulation error in the integration result. As the collection time increases, the accumulation error becomes larger and larger, resulting in inaccurate navigation results.

In addition, the communication between the control terminal and the operating carrier usually adopts wireless transmission. The mainstream wireless transmission methods include Bluetooth, WIFI and 2.4GHz communication. Among them, if Bluetooth transmission adopts high baud rate transmission, it is easy to lose packets during transmission. If a low or high baud rate is used for transmission, it will cause problems such as slow transmission speed; WIFI transmission will lose packets as the transmission distance increases; 2.4GHz communication transmission can increase the transmission distance, but the amount of data transmitted is low, so , the above wireless transmission methods will lead to varying degrees that the data between the user terminal and the operating carrier cannot be transmitted to each other in a timely, accurate and complete manner.

Contents of the invention

The present application provides a trajectory tracking control system and method based on inertial navigation. The trajectory tracking control system based on inertial navigation is based on the nine-axis sensing system in the inertial navigation system, and an additional An odometer, which fuses the data collected by the odometer with the data collected by other sensors, thereby reducing the cumulative error of the pose information and improving the accuracy of the pose information; on the other hand, the method provided by the application In the process of data transmission, the data to be transmitted is first converted into INT16 format data, and then 2.4GHz communication method is used for data transmission, which not only increases the transmission distance, but also increases the data transmission volume.

The purpose of this application is to provide the following aspects:

In a first aspect, the present application provides a trajectory tracking control method based on inertial navigation, the trajectory tracking control method comprising:

S100, acquiring a first target driving route of the running carrier;

S200. Acquire first pose information of the operating carrier, where the first pose information includes first pose information, first speed information, and first position information;

S300. Comparing the first actual travel route of the operating carrier with the first target travel route to obtain a first comparison result;

S400. Generate a second target driving route according to the first comparison result and the first instruction of the user, the first instruction of the user is sent by the user terminal;

S500. Control the operating carrier to travel according to the second target travel route.

In a practicable manner, the acquiring the current pose information of the operating carrier may specifically include:

S201. Acquire real-time acceleration, real-time angular velocity, real-time magnetic force value and real-time mileage value, wherein the real-time acceleration is collected in real time by an acceleration sensor, the angular velocity is collected in real time by an angular velocity sensor, and the real-time magnetic force value is collected in real time by a magnetometer Gained, the real-time mileage value is collected by the odometer in real time;

S202. Calculate first attitude information of the running carrier according to the real-time acceleration, the real-time angular velocity, and the real-time magnetic force value;

S203. Calculate second position information according to the real-time acceleration and the real-time mileage.

Further, calculating the second position information according to the real-time acceleration and the real-time mileage may specifically include:

S231. Calculate first speed information according to the real-time acceleration;

S232. Calculate the second speed information and the first position information according to the real-time mileage value,

S233. Calculate third speed information according to the first speed information and the second speed information;

S234. Calculate second position information according to the third speed information and the first position information.

Furthermore, the calculating the third velocity information according to the first velocity information and the second velocity information may specifically be calculating the third velocity information by using a Kalman filter method based on the first velocity information and the second velocity information. Three speed information.

Furthermore, the calculating the second location information based on the third velocity information and the first location information may specifically be calculating the second location information by using a Kalman filter method based on the third velocity information and the first location information. 2. Location information.

In a practicable manner, the sending of the user instruction by the client may specifically include:

S401, converting the command to be sent by the client into data in INT16 format, and compressing the converted data into a data packet;

S402. Transmit the data packet obtained in step S401 to the running carrier in Zigbee mode.

In a second aspect, the present application also provides a track tracking control system, the track tracking control system includes a user end device and a carrier end device, wherein the carrier end device is installed on the running carrier, and the carrier end device includes an electronic control subsystem and a data acquisition subsystem, the data acquisition subsystem is used to collect the running state of the running carrier, the electronic control subsystem includes a core processor, and the core processor is configured to execute:

S100, acquiring a first target driving route of the running carrier;

S200. Acquire first pose information of the operating carrier, where the first pose information includes first pose information, first speed information, and first position information;

S300. Comparing the first actual travel route of the operating carrier with the first target travel route to obtain a first comparison result;

S400. Generate a second target driving route according to the first comparison result and/or the first user instruction, the first user instruction being sent by the user terminal;

S500. Control the operating carrier to travel according to the second target travel route.

In a third aspect, the present application also provides a trajectory tracking control program, the program used is used to implement the steps of the trajectory tracking control method described in the first aspect when executed.

In a fourth aspect, a computer-readable storage medium stores computer instructions thereon, and when the instructions are executed by a processor, the steps of the trajectory tracking control method described in the first aspect above are implemented.

Compared with the prior art, the trajectory tracking control system provided by the present application also includes an odometer, and the mileage value collected by the odometer is fused with the acceleration value collected by the acceleration sensor and the angular velocity value collected by the angular velocity sensor to calculate the pose information , so as to reduce the cumulative error and obtain more accurate pose information. Further, the target driving path can be adjusted according to the pose information. In addition, this application uses the data in INT16 format for wireless transmission using Zigbee, so that the data transmission It can not only ensure long-distance transmission, but also ensure transmission speed.

This application first realizes the calculation of inertial navigation data, and adopts the strapdown inertial navigation algorithm to solve the data, wherein the second-order Picard algorithm is used to calculate the attitude information; and, this application uses the strapdown inertial navigation to assist driving, and adopts PID control , to realize the advantages of high trajectory tracking accuracy, fast update speed of driving path if yaw occurs, and higher stability of system operation.

At the same time, the client has a visualization function, so as to realize the remote monitoring of the smart car, which can fully obtain the position and attitude information of the vehicle, and control the operating carrier more accurately, faster and more stably.

In the data reading process, the raw data of the gyroscope and the accelerometer are filtered by a fourth-order Chebyshev low-pass digital filter, which is convenient to reduce the influence of high-frequency noise and eliminate most of the external interference;

In terms of hardware selection, use inertial sensors and devices that are less disturbed, such as icm-42688, etc., to reduce the error caused by the manufacturing process of the device itself, reduce the impact of this part on the system, and then pass the shortcut The linked inertial navigation algorithm realizes the determination of the position, speed and angle of the actual smart car, and can control its trajectory;

PID control is used to control the movement of smart cars. This application uses the deviation between the current angle and the target point angle, the deviation between the current speed and the set speed, and the deviation between the current speed and the set position as the controller input. The current angle , current speed, and current position are used as the feedback quantity of the controller, so that the tracking speed is fast and the output error is small.

The trajectory tracking control system provided in this application can be applied to various practical scenarios, for example, underwater robots or search and rescue robots.

Description of drawings

Fig. 1 shows a kind of structural representation of the trajectory tracking control system based on inertial navigation;

Fig. 2 shows the flowchart of the trajectory tracking control method provided by the present application;

Fig. 3 shows a flow chart of data processing that can be realized in the present application;

Fig. 4 shows the Kalman filtering structure of INS and odometer in this example.

Explanation of reference signs

001-user terminal, 002-operating carrier terminal, 021-electronic control subsystem, 022-data acquisition subsystem.

Detailed ways

Reference will now be made in detail to the exemplary embodiments, examples of which are illustrated in the accompanying drawings. When the following description refers to the accompanying drawings, the same numerals in different drawings refer to the same or similar elements unless otherwise indicated. The implementations described in the following exemplary examples do not represent all implementations consistent with the present invention. Rather, they are merely examples of approaches consistent with aspects of the invention as recited in the appended claims.

The inertial navigation-based trajectory tracking control method and system provided by the present application will be described in detail below through specific embodiments.

First, a brief introduction to the usage scenarios of this solution.

For underwater robots, search and rescue robots, and other equipment that require remote navigation and running paths, the hardware usually includes two parts: the user end and the operating carrier end. Among them, the operating carrier end can be equipped with an inertial navigation system.

The pose information calculated by the inertial navigation system is consistent with the actual information of the running carrier, so that the motion accuracy of the smart car can be improved as much as possible. The running carrier has two characteristics of accuracy and stability during driving.

At present, algorithms for obtaining pose information by processing data read by an inertial sensor include a Kalman filtering method, a Madgwick algorithm, and a Mahony algorithm. Among them, the Kalman filter is an algorithm that uses the linear system state equation, specifically, through the input and output observation data, to optimally estimate the system state. When the measurement variance is known, it can obtain a series of data with measurement noise Among them, estimating the state of the dynamic system is the most widely used filtering method at present, and has been well applied in many fields such as communication, navigation, guidance and control; the Madgwick algorithm can synthesize a variety of sensor parameters to obtain the attitude of the sensor, which is a A nine-axis fusion method is widely used in rotorcraft. It mainly uses the quaternion differential equation to solve the current attitude, uses the accelerometer and the magnetometer to compensate respectively, and derives two attitude fusion algorithms.

This application proposes a method based on the Mahony algorithm, which is calculated by adding mileage values, thereby reducing the error generated by calculating the velocity from the attitude information, and further reducing the error generated by calculating the position information from the velocity information.

Fig. 1 shows a schematic structural diagram of a trajectory tracking control system based on inertial navigation. As shown in Fig. 1, the trajectory tracking control system includes a user terminal (001) and a running carrier terminal (002), wherein the user terminal 001 is held by the operator and is used for human-computer interaction between the operator and the operating carrier, for example, real-time viewing of the operating status, actual driving path, and pose status of the operating carrier, adjusting parameter settings, and modifying the target driving path.

In this example, the user terminal 001 includes a first display and a user terminal data storage, and the first display is used to realize man-machine graphical interaction, for example, to modify the control parameters of the controller, specifically, the angle controller Proportional parameters, differential parameters, integral parameters, etc.; for another example, display the inertial navigation data obtained by the user through the user terminal, including the original data collected by the sensor and the pose information processed by the core processor; the user terminal data memory can be used To store the information sent by the running carrier end (002).

As shown in Figure 1 again, the operating carrier end 002 is arranged on the operating carrier, and the operating carrier may specifically be an underwater robot body, a search and rescue robot body, etc., and the operating carrier end (002) includes an electronic control subsystem ( 021) and data acquisition subsystem (022).

In this example, the electronic control subsystem (021) includes a core processor, a data transceiving device and an electronic control component.

In this example, the data acquisition subsystem includes an acceleration sensor, an angular velocity sensor, a magnetometer and an odometer, wherein the acceleration sensor is mainly used to collect the acceleration of the running carrier in real time, specifically an accelerometer, and the angular velocity sensor It is mainly used to collect the angular velocity of the running carrier in real time. Specifically, it can be a gyroscope. The magnetometer is mainly used to calibrate the attitude of the running carrier. Specifically, it can be a magnetometer. The accurate mileage value traveled by the running carrier can be calculated in real time based on the angle passed.

In this example, each sensor in the data collection subsystem is rigidly connected to the running carrier, so as to ensure that the data collected by the data collecting subsystem is real data of the running state of the running carrier.

In this example, the installation method of the data acquisition subsystem needs to be able to ensure convenient direction detection and ensure that the coordinate system of the running carrier is consistent with the coordinate system of the navigation, for example, the x-axis of each sensor in the data acquisition subsystem The positive direction is the same as the right side of the running carrier, the positive direction of the y-axis is the same as the front of the running carrier, and the positive direction of the z-axis is the same as being directly above the running carrier.

In this example, inertial sensors and devices with less interference are used in hardware selection, such as icm-42688, etc., so as to reduce the error caused by the manufacturing process of the device itself and reduce the impact on the system.

In this example, the electronic control subsystem includes a core processor and an electrical control device, wherein the electrical control device is mainly used to execute instructions sent by the core processor, so that the operating carrier implements the The instructions of the core processor run.

Further, the core processor also includes a carrier data storage for storing raw data collected by the data collection subsystem and processing results based on the raw data.

In this example, the running carrier terminal 002 further includes a second display 11, the second display is used to display the data stored in the carrier data storage, and the second display 11 can be installed on the running carrier to It is convenient to call and consult data during debugging.

In this example, the carrier running end further includes a power supply subsystem, and the power supply subsystem may specifically include a power supply component and a power detection component, wherein the power supply component is used to provide the electronic control subsystem and the data acquisition subsystem with power supply.

In this example, the power state of the power supply can be displayed on the first display and/or the second display.

In this example, before starting the trajectory tracking control system, the electronic control subsystem and the data acquisition subsystem in the trajectory tracking control system are first initialized.

In this example, the core processor is used to execute the trajectory tracking control method provided in this application.

Fig. 2 shows the flowchart of the trajectory tracking control method provided by the present application. As shown in Fig. 2, the trajectory tracking control method mainly includes the following steps S100 to S500:

S100. Obtain a first target driving route of the running carrier.

In this example, the first target driving route may be set artificially, or may be calculated based on other parameters.

Further, in this example, the first target driving route may be sent to the operating carrier end by means of wireless transmission through the user end.

FIG. 3 shows a flow chart of data processing that can be realized in this application. As shown in FIG. 3 , the calculation and operation of carrier pose information in this application can be specifically shown in S200.

S200. Acquire first pose information of the running carrier, where the first pose information includes first pose information, first speed information, and first position information.

In this example, the first pose information is calculated based on the raw data collected by the data collection subsystem.

It can be understood that each sensor in the data collection subsystem may collect according to a first preset frequency, and further, the collection frequency of each sensor may be the same or different.

Fig. 4 shows the Kalman filtering structure of INS and odometer in this example. As shown in Figure 3, this example uses the Kalman filter to process the pose information and real-time mileage values calculated by the inertial navigation system, so as to obtain the current pose information of the running carrier, including attitude angle, velocity, and position.

In this example, the acquisition of the current pose information of the operating carrier may specifically include the following steps S201 to S203:

S201. Acquire real-time acceleration, real-time angular velocity, real-time magnetic force value and real-time mileage value, wherein the real-time acceleration is collected in real time by an acceleration sensor, the angular velocity is collected in real time by an angular velocity sensor, and the real-time magnetic force value is collected in real time by a magnetometer The real-time mileage value is collected by the odometer in real time.

In this example, the core processor receives the real-time acceleration, real-time angular velocity, real-time magnetic value and real-time mileage value according to a second preset frequency, wherein the second preset frequency is the same as the first preset frequency Can be the same or different.

S202. Calculate first attitude information of the running carrier according to the real-time acceleration, the real-time angular velocity, and the real-time magnetic force value.

In this example, the strapdown inertial navigation algorithm is used to solve the data, and the attitude update uses the second-order Picard algorithm to obtain more accurate first pose information.

S203. Calculate second position information according to the real-time acceleration and the real-time mileage.

The applicant found that the data collected by the acceleration sensor may be inconsistent with the actual situation due to wheel slipping, etc., resulting in inaccurate data generated based on the data collected by the acceleration sensor. Therefore, the applicant in the data collection section In the system, the odometer is used to collect the mileage value traveled by the running carrier, so as to correct the data collected by the acceleration sensor.

Furthermore, in the data reading process of this example, firstly, the raw data collected by the angular velocity sensor and the acceleration sensor are filtered using a fourth-order Chebyshev low-pass digital filter to eliminate most of the external interference, thereby reducing the high-frequency The effect of noise on the accuracy of calculation results.

Specifically, this step may specifically include the following steps S231 to S234:

S231. Calculate first speed information according to the real-time acceleration.

In this example, the first speed information can be specifically calculated according to the strapdown inertial navigation model shown in the following equations (1) and (2):

表示在导航坐标系下的加速度；Among them, v ⁿ =[v _e v _n v _u ] ^T represents the linear velocity,

Indicates the acceleration in the navigation coordinate system;

表示加速度传感器采集所得的在运行载体坐标系下的比力；

Indicates the specific force in the running carrier coordinate system collected by the acceleration sensor;

表示由运行载体坐标系变换到导航坐标系的旋转矩阵，其中，/>

表示由导航坐标系变换到运行载体坐标系的旋转矩阵，/>

表示由导航坐标系变换到运行载体坐标系旋转矩阵的关于时间的导数，具体可根据步骤S202计算得到的运行载体的三个姿态角进行计算；

Indicates the rotation matrix transformed from the running vehicle coordinate system to the navigation coordinate system, where, />

Represents the rotation matrix transformed from the navigation coordinate system to the running vehicle coordinate system, />

Indicates the derivative with respect to time transformed from the navigation coordinate system to the rotation matrix of the running carrier coordinate system, specifically, it can be calculated according to the three attitude angles of the running carrier calculated in step S202;

表示地球在导航坐标系下的旋转速率，

Indicates the rotation rate of the earth in the navigation coordinate system,

表示导航坐标系相对于地球的旋转速率，

Indicates the rotation rate of the navigation coordinate system relative to the earth,

表示去掉/>

和/>

后角速度传感器直接采集到的绕惯导轴的角速度矢量，

means remove />

and />

The angular velocity vector around the inertial axis directly collected by the rear angular velocity sensor,

g ⁿ denotes the local gravity vector.

Wherein the calculation of the first speed information is the process of solving the formula (1), and the present invention uses the average value filtering algorithm for the collected raw data and then substitutes it into the formula (1) to solve.

S232. Calculate second speed information and first location information according to the real-time mileage value.

In this example, the second speed information can be calculated using the models shown in the following equations (3), (4) and (5):

Among them, ω _R represents the angular velocity of the right wheel;

ω _L represents the angular velocity of the left wheel,

v _R represents the rotational speed of the right wheel,

v _l represents the rotational speed of the left wheel.

表示x-y平面上的航向角速率，

Indicates the heading rate on the xy plane,

r _w represents the radius of the wheel,

e represents the length between the axles,

R represents the turning radius of the car body.

In this example, further,

v _R = r _w ω _R

v _L = r _w ω _L .

In this example, the first position information can be calculated using the model shown in the following formula (6):

Among them, x _k represents the position of the center of the X axis,

y _k represents the position of the center of the Y axis,

Δt represents the sampling time.

S233. Calculate third speed information according to the first speed information and the second speed information.

In this example, the calculation of the third speed information based on the first speed information and the second speed information may specifically be calculated by using a Kalman filter method based on the first speed information and the second speed information The third speed information, thereby reducing the error existing in the first speed.

This example constructs a Kalman filter for synthesizing the third velocity information, wherein the Kalman filter uses the inertial navigation error equation as the error dynamic model of the system, specifically as shown in the following formula (7):

Among them, X=[δ _rn δ _vn ε δb _acc δb _gyro ] ^T represents the state vector;

δr ⁿ represents the position error,

δv ⁿ represents the linear velocity deviation,

ε represents the bias of the accelerometer and gyroscope,

δb _acc ＝[δb _accx δb _accy δb _accz ] ^T represents the bias of the accelerometer,

δb _gyro = [δb _gyrox δb _gyroy δb _gyroz ] ^T represents the bias of the gyroscope,

0 ₃ and I ₃ represent the identity matrix of size 3, respectively.

The drift of the above bias can be modeled as a first-order Gauss-Markov process as shown in equations (8) and (9):

where i=x,y,z,

T represents the relative time of the accelerometer,

表示陀螺仪的相关时间，

represents the relative time of the gyroscope,

η _acc = [η _accx η _accy η _accz ] ^T represents the noise of the accelerometer,

η _gyro = [η _gyrox η _gyroy η _gyroz ] ^T represents the noise of the gyroscope,

η _bacc =[η _baccx η _baccy η _baccz ] ^T and η _bgyro =[η _bgyrox η _bgyroy η _bgyroz ] ^T represent Gauss-Markov process driving noise.

Since the position error value in the odometer is very small and expressed in radians, it will cause numerical instability inside the filter. In order to avoid this problem, this example expresses the position error in the northeast sky coordinate system. The new dynamic model It can be shown in the following formula (10):

in,

Therefore, the discrete form of the system model can be written as the following formula (11):

For the model of the odometer in the Kalman filter, the position solution of the odometer is integrated with the INS at a rate of 1 Hz. In this case, the measurement model can be shown in the following formula (12):

Among them, υ ^Odo represents Gaussian white noise.

The form of the observation matrix can be expressed as the following formula (13):

Among them, n represents the size of the state vector.

The covariance matrix of the odometer measurement can be expressed as the following formula (14):

表示P_Odo(P＝x,y,z)的协方差。in,

Indicates the covariance of P _Odo (P=x,y,z).

S234. Calculate second position information according to the third speed information and the first position information.

In this example, this step may calculate the second location information in a manner similar to step S233.

S300. Compare the first actual travel route of the running carrier with the first target travel route to obtain a first comparison result.

In this example, the comparison result is a deviation between the current movement track and the preset movement track, and the deviation includes angle deviation, speed deviation and position deviation.

This example does not specifically limit the specific implementation of this step, and any method in the prior art that can be used to calculate the above-mentioned specific deviations can be used. For example, for angular deviations, the navigation data (that is, the pose information ) and the deviation of the first target travel path first carry out average value filtering, and the result is used as an angle deviation; for speed deviation, the five latest data of the first speed information can be averaged, thereby filtering the data, and then The deviation between the target speed and the target speed is used as the speed deviation; for the position deviation, the current position information and the position information of the first three times can be fitted, and the three position predictions can be made according to the fitted trajectory, and the following three position predictions can be used. Calculate the deviation from the set target trajectory respectively, and then obtain the position deviation between the current motion path and the target path.

S400. Generate a second target driving route according to the first comparison result and/or the first user instruction, the first user instruction being sent by the user terminal.

In this example, the sending of the user instruction by the client may specifically include:

S401, converting the command to be sent by the client into data in INT16 format, and compressing the converted data into a data packet;

S402. Transmit the data packet obtained in step S401 to the running carrier in Zigbee mode.

In this example, firstly, the instruction to be sent is converted into floating-point data into INT16 format data, which can reduce the space occupied by the data by half. In this case, using the Zigbee transmission method can not only ensure long-distance transmission, but also ensure Transmission rate, thereby enhancing the real-time performance of data transmission.

Optionally, for instructions to be sent with decimals, the data can be enlarged by several orders of magnitude year-on-year. Since the instructions transmitted in this scenario are still within the Zigbee transmission range after being enlarged by several orders of magnitude, the solution provided in this example has realistic feasibility.

In this example, if the first comparison result is smaller than the deviation threshold, the second target travel route may be the same as the first target travel route, that is, the operating vehicle may continue to travel according to the first target travel route.

On the contrary, if the first comparison result is greater than the deviation threshold, the second target driving route can be generated according to the current pose information and the first target driving route. It can be understood that the second target driving route is different from the first The end points of the target travel routes are the same, but at least part of the travel routes are different.

In this example, if the first comparison result is greater than the deviation threshold, the core processor sends an alarm message to the user terminal, and optionally, continuously sends the motion state of the running carrier to the user terminal for the staff Real-time monitoring through the client.

In another example, the user can adjust the first target driving route at any time through the user terminal as needed, that is, the user can send the second target driving route to the operating carrier through the user terminal at any time as needed, and the second target driving route is at least Some routes are different from the first target travel route.

It can be understood that the user terminal data storage set in the user terminal and the second storage set in the running carrier end store data independently, so as to back up each other, which is convenient for checking the correctness of data transmission.

The method and system provided by this application have promotional value. For example, for an unmanned coal mine robot, when it uses this system to track and control the motion trajectory, this system will send the robot motion state to the operator. , the system corrects the trajectory, and at the same time sends the alarm information to the command control terminal to remind the operator, the operator can choose to let the system correct itself, or choose to send a new command to control the robot to adopt a new trajectory to improve the safety of the robot .

In addition, the method and system provided in the present application can be carried on search and rescue robots, drilling shaft detection equipment, and the like.

S500. Control the operating carrier to travel according to the second target travel route.

In this example, after the core processor acquires the second target driving route, it sends a control instruction to the electronic control subsystem, and the control instruction is used to control the operating carrier to follow the second target driving route. to drive.

Optionally, the electronic control subsystem adopts PID control for the power system, for example, the deviation between the current angle and the target point angle, the deviation between the current speed and the set speed, and the deviation between the current speed and the set position are used as The input of the controller, the current angle, current speed, and current position are used as the feedback of the controller, so as to ensure that the tracking speed is fast and the output error is small.

In an example, based on the second target travel path, the speed of the left and right wheels of the running carrier can be calculated to control the movement of the running carrier, and then the pose of the running carrier at the next sampling time can be obtained, compared with the current sampling time, If the running carrier is closer to the first target travel path, the control at the previous moment is correct. Based on this, continue to calculate the speed of the left and right wheels of the running carrier according to the second target travel path so that it continues to the first target. Movement of the driving path; compared with the current sampling time, if the running carrier is farther away from the first target driving path, the current pose needs to be reacquired and the speed of the left and right wheels of the running carrier should be calculated again, so as to continuously control the subsequent movement of the running carrier.

In another example, the core processor deletes the first target route, and only controls the motion carrier to travel according to the second target travel route.

In addition, the present application also provides a trajectory tracking control program, which is used to realize the steps of the above trajectory tracking control method during execution.

Further, the present application also provides a computer-readable storage medium, on which computer instructions are stored, and when the instructions are executed by a processor, the steps of the above trajectory tracking control method are realized.

The present application has been described in detail above in conjunction with specific implementations and illustrative examples, but these descriptions should not be construed as limiting the present application. Those skilled in the art understand that without departing from the spirit and scope of the present application, various equivalent replacements, modifications or improvements can be made to the technical solutions and implementations of the present application, all of which fall within the scope of the present application. The scope of protection of the present application is subject to the appended claims.

Claims (9)

1. A trajectory tracking control method based on inertial navigation, characterized in that, the trajectory tracking control method comprises: S100, acquiring a first target driving route of the running carrier; S200. Acquire first pose information of the operating carrier, where the first pose information includes first pose information, first speed information, and first position information; S300. Comparing the first actual travel route of the operating carrier with the first target travel route to obtain a first comparison result; S400. Generate a second target driving route according to the first comparison result and/or the first user instruction, the first user instruction being sent by the user terminal; S500. Control the operating carrier to travel according to the second target travel route. 2. The trajectory tracking control method according to claim 1, wherein said obtaining the current pose information of said operating carrier comprises: the trajectory tracking control method according to claim 1, wherein S201. Acquire real-time acceleration, real-time angular velocity, real-time magnetic force value and real-time mileage value, wherein the real-time acceleration is collected in real time by an acceleration sensor, the angular velocity is collected in real time by an angular velocity sensor, and the real-time magnetic force value is collected in real time by a magnetometer Gained, the real-time mileage value is collected by the odometer in real time; S202. Calculate first attitude information of the running carrier according to the real-time acceleration, the real-time angular velocity, and the real-time magnetic force value; S203. Calculate second position information according to the real-time acceleration and the real-time mileage. 3. The trajectory tracking control method according to claim 2, wherein calculating the second position information according to the real-time acceleration and the real-time mileage value comprises: S231. Calculate first speed information according to the real-time acceleration; S232. Calculate the second speed information and the first position information according to the real-time mileage value, S233. Calculate third speed information according to the first speed information and the second speed information; S234. Calculate second position information according to the third speed information and the first position information. 4. The trajectory tracking control method according to claim 1, characterized in that said calculating the third speed information according to said first speed information and said second speed information is based on said first speed information and said The second velocity information uses a Kalman filter method to calculate the third velocity information. 5. The trajectory tracking control method according to claim 1, wherein said calculating second position information according to said third speed information and said first position information is based on said third speed information and said The first location information uses a Kalman filter method to calculate the second location information. 6. The trajectory tracking control method according to claim 1, wherein the sending of the user instruction by the user terminal comprises: S401, converting the command to be sent by the client into data in INT16 format, and compressing the converted data into a data packet; S402. Transmit the data packet obtained in step S401 to the running carrier in Zigbee mode. 7. A trajectory tracking control system, characterized in that the trajectory tracking control system includes a user end device and a carrier end device, wherein the carrier end device is installed on a running carrier, and the carrier end device includes an electronic control subsystem and data The collection subsystem, the data collection subsystem is used to collect the running status of the running carrier, the electronic control subsystem includes a core processor, and the core processor is configured to execute: S100, acquiring a first target driving route of the running carrier; S200. Acquire first pose information of the operating carrier, where the first pose information includes first pose information, first speed information, and first position information; S300. Comparing the first actual travel route of the operating carrier with the first target travel route to obtain a first comparison result; S400. Generate a second target driving route according to the first comparison result and/or the first user instruction, the first user instruction being sent by the user terminal; S500. Control the operating carrier to travel according to the second target travel route. 8. A trajectory tracking control program, the program used is used to realize the steps of the trajectory tracking control method according to any one of claims 1 to 6 when executed. 9. A computer-readable storage medium, on which computer instructions are stored, and when the instructions are executed by a processor, the steps of the trajectory tracking control method described in any one of 1 to 6 are implemented.

Images