An integrated system of inertial navigation, motor driver and odometer for mobile intelligent body

Abstract

The present invention discloses an integrated system of inertial navigation, motor driver and odometer for a mobile intelligent body, belonging to the field of integration technology. The technical problem to be solved by the present invention is how to fully interact and utilize the data among the IMU, odometer and motor driver, reduce the mileage error of the odometer when the wheels slip, and improve the odometer output accuracy and motor control quality. The technical solution is as follows: it includes a motor driver unit, an IMU inertial navigation unit and an inertial-drive fusion odometer unit. The motor driver unit is used to realize closed-loop control of the servo motor; the IMU inertial navigation unit is used to realize real-time measurement of acceleration and angular velocity, and provide acceleration and angular velocity signals to the motor driver unit, the inertial-drive fusion odometer unit and upper-layer applications; the inertial-drive fusion odometer unit is used to obtain high-precision odometer information based on encoder pulses collected by the driver and combined with drive current and IMU signals, and output corrected odometer information.

Description

Technical Field

[0001] The present invention relates to the field of integration technology, in particular to an inertial navigation, motor driver and odometer integrated system for a mobile intelligent body. Background Art

[0002] In the process of environmental scanning and reconstruction in fields like the metaverse and digital twins, IMUs, odometry, and motor drivers are three key sensing and control components frequently designed. IMUs and odometry provide upper-level algorithms with motion information such as the intelligent agent's acceleration and displacement. Motor drivers directly control the speed and direction of wheel movement. They are crucial information and execution devices for algorithms like high-precision positioning and feature matching that integrate multi-sensor information. Their accuracy significantly impacts the ultimate positioning, navigation, and control accuracy of the intelligent agent.

[0003] Specifically, the IMU can provide inertial navigation information, including acceleration and angular velocity, for algorithms such as SLAM. Its data output frequency is high, and its accuracy is high in the short term. However, during long-term operation, there is inevitable error accumulation, resulting in reduced accuracy during long-term use. The odometer uses wheel speed to calculate the displacement of the intelligent body, which can compensate for the long-term drift problem of the IMU to a certain extent. However, its displacement calculation results are affected by wheel slippage, and cumulative errors will also occur over long periods of time. In conventional applications, the motor driver usually only receives upper-level control command information. Based on its own current sampling, encoder resolution, and other information, it uses control algorithms such as FOC to output motor drive control signals to control the motor speed.

[0004] In conventional applications, IMUs, odometers, and motor drivers are usually developed and used as three independent devices. This results in a lack of interconnection between their data, making it impossible to fully mine and apply the data, limiting the actual accuracy of each component. At the same time, the separate design of the three also brings inconvenience to installation, layout, and routing.

[0005] Therefore, how to fully interact and utilize the data between the IMU, odometer and motor driver, reduce the mileage error of the odometer when the wheels slip, and improve the odometer output accuracy and motor control quality is a technical problem that needs to be solved urgently. Summary of the Invention

[0006] The technical task of the present invention is to provide an integrated system of inertial navigation, motor driver and odometer for a mobile intelligent body to solve the problem of how to fully interact and utilize the data among the IMU, odometer and motor driver, reduce the mileage error of the odometer when the wheels slip, and improve the odometer output accuracy and motor control quality.

[0007] The technical task of the present invention is achieved in the following way: an inertial navigation, motor driver and odometer integrated system for a mobile intelligent body, including a motor driver unit, an IMU inertial navigation unit and an inertial-drive fusion odometer unit. The motor driver unit is used to realize closed-loop control of the servo motor; the IMU inertial navigation unit is used to realize real-time measurement of acceleration and angular velocity, and provide acceleration and angular velocity signals for the motor driver unit, the inertial-drive fusion odometer unit and upper-layer applications; the inertial-drive fusion odometer unit is used to obtain high-precision odometer information based on the encoder pulses collected by the driver and combined with the drive current and IMU signal, and output the corrected odometer information.

[0008] Preferably, the motor driver unit includes a power drive circuit, an encoder and a current sampling circuit, and the current sampling circuit and the encoder respectively control the power drive circuit to obtain the motor control current through the PWM control signal of the motor vector control algorithm provided by the main processing chip;

[0009] The IMU inertial navigation unit includes a three-axis accelerometer and a three-axis gyroscope. The IMU inertial navigation element, the three-axis accelerometer is used to achieve real-time measurement of acceleration; the three-axis gyroscope is used to achieve real-time measurement of angular velocity.

[0010] More preferably, the motor driver unit shares the driving current obtained by its own current sampling circuit, the encoder pulse signal collected by the encoder, and the control current data output by the power drive circuit with the inertial-drive fusion odometer unit in the form of software external variables, and the IMU inertial navigation unit shares the processed acceleration and angular velocity information with the motor driver unit and the inertial-drive fusion odometer unit through UART or I2C.

[0011] More preferably, the motor driver unit adds a PI adaptive module on the basis of the FOC vector control algorithm, uses the z-acceleration component provided by the IMU inertial navigation unit to calculate the slope of the current driving road, and adaptively adjusts the coefficients Kp and Ki in the PI adaptive module according to the size of the slope:

[0012] When the slope is positive, that is, when going uphill, the coefficients Kp and Ki in the PI adaptive module are increased so that the motor drive unit can achieve faster, more stable and more precise control when the slope changes;

[0013] When the slope is negative, that is, downhill, the coefficients Kp and Ki in the PI adaptive module are adjusted to be smaller, so that the motor drive unit can achieve faster, more stable and more precise control when the slope changes.

[0014] Preferably, the PI adaptive module is implemented as follows:

[0015] The acceleration and angular velocity data of the vehicle during movement are obtained through the IMU inertial navigation unit;

[0016] Decompose the acceleration data measured by the IMU into the acceleration of linear motion and the acceleration of gravity;

[0017] Since the gravitational acceleration to which a vehicle is subjected when driving is consistent with the gravity of the earth and is perpendicular to the road surface, the gravitational acceleration is used to determine the standard reference frame in which the vehicle is located;

[0018] The angular velocity data measured by the IMU and the angle of the vehicle's reference frame can be used to calculate the lateral and longitudinal angle changes of the vehicle. The change in the vehicle's front and rear tilt angle is the slope angle of the road surface. At the same time, the road slope angle data measured by the IMU is filtered, such as low-pass filtering and median filtering, to improve the accuracy and reliability of the data.

[0019] After obtaining the slope information, the PID parameters are adjusted adaptively according to the slope. The driving power PI at the current slope is calculated using the slope information. The formula is as follows:

[0020] P1=P0+mg·cosθ·v;

[0021] Where m is the vehicle mass; g is the acceleration due to gravity; θ is the road slope calculated based on the IMU inertial navigation unit information; v is the current speed; and PO is the driving power on a flat road (slope is 0). The formula is as follows:

[0022] P0=F f ·v

[0023] Among them, F f It is the resistance encountered during driving (calculated according to actual conditions, which may include friction resistance, air resistance, acceleration resistance, etc.);

[0024] According to the relationship between P1 and P0, the coefficients Kp and Ki in the PI adaptive module are adaptively adjusted to achieve slope adaptive control.

[0025] More preferably, the corrected odometer information output by the inertial-drive fusion odometer unit is obtained by applying a Kalman filter algorithm to a displacement value obtained by twice integrating the forward direction acceleration obtained by the IMU inertial navigation unit and a pulse value obtained by encoder pulse conversion.

[0026] More preferably, the Kalman filter algorithm is as follows:

[0027] The real-time driving power is calculated by using the driving current and current voltage information collected by the current adopting circuit of the motor driver unit;

[0028] Combined with the slope information measured by the IMU inertial navigation unit, when there is no obvious change in the slope, determine whether the wheel is slipping:

[0029] If the driving power remains unchanged and the speed increases significantly, or the speed remains stable and the driving power decreases significantly, it means that the wheel is slipping;

[0030] When the speed or power changes more, the possibility of wheel slip is greater;

[0031] Adjust the Kalman gain in the Kalman filter algorithm based on whether the wheel slips:

[0032] When it is determined that the possibility of slip is high, the Kalman gain is reduced and the weight corresponding to the integration result of the IMU inertial navigation unit is increased;

[0033] When the slip possibility is determined to be small, the Kalman gain is increased and the weight corresponding to the encoder is increased.

[0034] More preferably, the inertial-drive fusion odometer unit corrects the odometer information as follows:

[0035] The current sampling circuit of the motor driver unit detects the driving current Iq and bus voltage Vbus of the driving motor, and the power P of the driving motor at this time is q =I q ·V bus ;

[0036] Combine the driving power with the power calculated based on the slope information: P1=P0+mg·cosθ·v, and compare P in real time. q With P1:

[0037] When P q When it is close to P1 or the difference is stable, it means there is no slip;

[0038] When P1 is constant, P q When it decreases significantly, it is determined that slippage occurs at this moment;

[0039] At the same time, using P q The rate of change of ΔP q Measure the size of the slip possibility:

[0040] When ΔP q When it is 0, it means slip occurs;

[0041] ΔP q When it is 1, the wheel is in a completely slipping state;

[0042] in,

[0043] At the same time, the final result output by the inertial-drive fusion odometry unit is the mileage information obtained by fusing the information of the IMU inertial navigation unit and the encoder pulse information, calculated using the Kalman filter. The relevant parameters of the Kalman filter are feedforward corrected according to the slip judgment result, thereby realizing the correction of the Kalman filter fusion result according to the degree of slip.

[0044] Among them, the simplified expression of the Kalman filter algorithm is as follows:

[0045] L=(1-K)L imu +K·L en ;

[0046] Among them, L imu The mileage is obtained by integrating the acceleration in the driving direction twice; L en is the mileage calculated using the number of pulses obtained by the driver encoder; K is the Kalman gain matrix, which is understood as the weighted coefficient between the two. By adjusting the value of K, L is controlled imu and L en The weight in the final output;

[0047] According to ΔP q Determine the current road slippage status:

[0048] When ΔP q The smaller (closer to 0), the less wheel slip there is. Adjust the value of Kalman filter gain K so that L en The proportion increases;

[0049] On the contrary, when ΔP q The larger the value (closer to 1), the closer the vehicle is to a complete slip state. en A large error will occur, and the value of the Kalman filter gain K is adjusted so that L en The proportion decreased.

[0050] The inertial navigation, motor driver and odometer integrated system of the mobile intelligent body of the present invention has the following advantages:

[0051] (1) This invention addresses the need for high-precision wheel odometer information in key algorithms and processes such as digital twin environment scanning, mapping, and positioning. By tightly coupling multiple signals, it improves control performance and wheel odometer accuracy, providing more accurate information for upper-layer applications such as digital twin environment scanning, mapping, and positioning.

[0052] (2) This invention integrates three separate devices commonly used in mobile intelligent bodies: servo motor drivers, IMUs, and wheel odometers, to form a multifunctional inertial-drive-odometer integrated device. This reduces the number of components and wiring while enabling deep information fusion between various components, improving performance in motor control and odometer accuracy.

[0053] (3) The present invention calculates high-precision odometer information based on the encoder pulses collected by the driver and combines the drive current and IMU signals, outputs the corrected odometer information, and provides more accurate and reliable odometer information for external applications such as upper-level SLAM or three-dimensional reconstruction; the data reception, processing and external communication of the motor driver unit, IMU inertial navigation unit and inertial-drive fusion odometer unit are all through the main processing chip of the shared motor driver (which can be a high-performance DSP or STM32 MCU, etc.), thereby reducing the number of processors, reducing costs and development complexity, while achieving a deeper and more comprehensive integration and utilization of multiple data resources;

[0054] (4) The present invention implements feedforward adaptation of the Kalman filter algorithm under slip conditions, which can improve the real-time performance of the algorithm and the accuracy of the final result (corrected mileage information);

[0055] (5) The present invention fully interacts and utilizes data between the motor driver unit, the IMU inertial navigation unit, and the inertial-drive fusion odometer unit to solve the odometer's mileage error problem when the wheels slip. It also enables the motor driver to have a slope-sensing adaptive function, improves the odometer output accuracy and motor control quality, and provides better support for upper-level mapping, positioning, and other tasks.

[0056] (6) The present invention solves the problems of poor control quality caused by the servo motor controller's inability to perceive the slope in advance and compensate for it, and low positioning accuracy caused by the conventional wheel odometer's inability to identify errors caused by wheel slippage, through the tight coupling of IMU information and motor control algorithms. This can provide better support for the implementation of upper-level applications such as mapping and positioning. BRIEF DESCRIPTION OF THE DRAWINGS

[0057] The present invention will be further described below with reference to the accompanying drawings.

[0058] Attachment Figure 1 This is a schematic diagram of the structure of the integrated system of inertial navigation, motor drive and odometer for mobile intelligent bodies;

[0059] Attachment Figure 2 This is a schematic diagram of the working principle of the motor driver unit;

[0060] Attachment Figure 3 Schematic diagram of the inertial-drive fusion odometry unit. DETAILED DESCRIPTION

[0061] The following is a detailed description of an integrated system of inertial navigation, motor drive and odometer for a mobile intelligent body of the present invention with reference to the accompanying drawings and specific embodiments.

[0062] Example:

[0063] As attached Figure 1 As shown, this embodiment provides an integrated inertial navigation, motor driver, and odometer system for a mobile intelligent body, including a motor driver unit, an IMU inertial navigation unit, and an inertial-drive fusion odometer unit. The motor driver unit is used to implement closed-loop control of the servo motor; the IMU inertial navigation unit is used to implement real-time measurement of acceleration and angular velocity, providing acceleration and angular velocity signals to the motor driver unit, the inertial-drive fusion odometer unit, and upper-layer applications; the inertial-drive fusion odometer unit is used to obtain high-precision odometer information based on encoder pulses collected by the driver and combined with drive current and IMU signals, and output corrected odometer information to provide more accurate and reliable odometer information for upper-layer external applications such as SLAM or three-dimensional reconstruction. Data reception, processing, and external communication of the three parts are all achieved through the shared motor driver's main processing chip (which can be a high-performance DSP or STM32 MCU, etc.), thereby reducing the number of processors, reducing costs and development complexity, while achieving a deeper and more comprehensive integration and utilization of multiple data resources.

[0064] The motor driver unit in this embodiment includes a power drive circuit, an encoder and a current sampling circuit. The current sampling circuit and the encoder respectively control the power drive circuit to obtain the motor control current through the PWM control signal of the motor vector control algorithm provided by the main processing chip.

[0065] The IMU inertial navigation unit in this embodiment includes a three-axis accelerometer and a three-axis gyroscope. The IMU inertial navigation element, the three-axis accelerometer is used to achieve real-time measurement of acceleration; the three-axis gyroscope is used to achieve real-time measurement of angular velocity.

[0066] Compared to conventional discrete components, the integrated solution enables data sharing among the three components. The motor driver unit shares data such as the drive current captured by its own current sampling circuit, the encoder pulse signals collected by the encoder, and the control current output by the driver with the odometry module algorithm via software external variables. The acceleration and angular velocity information processed by the IMU is shared with the inertial-drive fusion odometry unit via UART or I2C for use in its algorithm.

[0067] The motor driver unit in this embodiment shares the driving current obtained by its own current sampling circuit, the encoder pulse signal collected by the encoder, and the control current data output by the power drive circuit with the inertial-drive fusion odometer unit in the form of software external variables. The IMU inertial navigation unit shares the processed acceleration and angular velocity information with the motor driver unit and the inertial-drive fusion odometer unit through UART or I2C.

[0068] The motor drive unit is based on the conventional servo motor drive hardware and software system, mainly including the main processor, current sampling circuit, encoder, power drive circuit and other hardware and software modules. The control algorithm adopts the conventional servo motor FOC vector control algorithm. Figure 2 As shown, the motor driver unit in this embodiment adds a PI adaptive module on the basis of the FOC vector control algorithm, uses the z-acceleration component provided by the IMU inertial navigation unit to calculate the slope of the current driving road, and adaptively adjusts the coefficients Kp and Ki in the PI adaptive module according to the slope:

[0069] When the slope is positive, that is, when going uphill, the coefficients Kp and Ki in the PI adaptive module are increased so that the motor drive unit can achieve faster, more stable and more precise control when the slope changes;

[0070] When the slope is negative, that is, downhill, the coefficients Kp and Ki in the PI adaptive module are adjusted to be smaller, so that the motor drive unit can achieve faster, more stable and more precise control when the slope changes.

[0071] The PI adaptive module in this embodiment is specifically implemented as follows:

[0072] The acceleration and angular velocity data of the vehicle during movement are obtained through the IMU inertial navigation unit;

[0073] Decompose the acceleration data measured by the IMU into the acceleration of linear motion and the acceleration of gravity;

[0074] Since the gravitational acceleration to which a vehicle is subjected when driving is consistent with the gravity of the earth and is perpendicular to the road surface, the gravitational acceleration is used to determine the standard reference frame in which the vehicle is located;

[0075] The angular velocity data measured by the IMU and the angle of the vehicle's reference frame can be used to calculate the lateral and longitudinal angle changes of the vehicle. The change in the vehicle's front and rear tilt angle is the slope angle of the road surface. At the same time, the road slope angle data measured by the IMU is filtered, such as low-pass filtering and median filtering, to improve the accuracy and reliability of the data.

[0076] After obtaining the slope information, the PID parameters are adjusted adaptively according to the slope. The driving power PI at the current slope is calculated using the slope information. The formula is as follows:

[0077] P1=P0+mg·cosθ·v;

[0078] Where m is the vehicle mass; g is the acceleration due to gravity; θ is the road slope calculated based on the IMU inertial navigation unit information; v is the current speed; and PO is the driving power on a flat road (slope is 0). The formula is as follows:

[0079] P0=F f ·v

[0080] Among them, F f It is the resistance encountered during driving (calculated according to actual conditions, which may include friction resistance, air resistance, acceleration resistance, etc.);

[0081] According to the relationship between P1 and P0, the coefficients Kp and Ki in the PI adaptive module are adaptively adjusted to achieve slope adaptive control.

[0082] For example: Defining coefficients This means that when k1≠1, meaning there's a slope effect, Kp = k1*Kp and Ki = ki*Kp. This means the proportional and integral coefficients for a straight road are multiplied by K1 to create the new proportional and integral coefficients, achieving adaptive coefficient adjustment. Alternatively, the proportional and integral coefficients can be adaptively adjusted using other methods, such as table lookup.

[0083] Conventional wheel odometers typically obtain wheel speed information by installing an encoder on the wheel or directly reading signals from the motor controller. This information is then used to calculate vehicle mileage based on the wheel radius. However, this method can produce errors when there is wheel slip: while wheel speed remains constant during wheel slip, the mileage calculated is less than the result directly calculated from wheel speed, and may even be zero (in the case of complete slip). In this case, the mileage calculated using conventional methods will be greater than the actual mileage, resulting in significant errors.

[0084] Since the motor driver unit, IMU inertial navigation unit and inertial-drive fusion odometer unit are integrated, the information of the three can be comprehensively utilized to correct the mileage results of wheel slip conditions. Figure 3 As shown, the corrected odometer information output by the inertial-drive fusion odometer unit in this embodiment is obtained by the Kalman filtering algorithm through the displacement value obtained by twice integrating the forward direction acceleration obtained by the IMU inertial navigation unit and the pulse value obtained by encoder pulse conversion.

[0085] In a conventional Kalman filter algorithm, the Kalman gain corresponds to the weight distribution of the encoder and IMU integration results. The larger the Kalman gain, the larger the weight corresponding to the encoder, and vice versa. The Kalman filter algorithm can perform rolling optimization based on historical data to seek the optimal solution for the Kalman gain. However, due to the lack of feedforward information, indiscriminate optimization will result in reduced computing efficiency, poor real-time performance, and poor final result accuracy. In this embodiment, the motor driver unit, IMU inertial navigation unit, and inertial-drive fusion odometer unit are tightly integrated. The drive current information and IMU information are used to first perform slip identification, and the conventional Kalman filter algorithm is feedforwarded based on the identification results, which can significantly improve the real-time performance and final accuracy of the filter algorithm.

[0086] The Kalman filter algorithm in this embodiment is specifically as follows:

[0087] (1) The current of the motor driver unit is used to calculate the real-time driving power by using the driving current and current voltage information collected by the circuit;

[0088] (2) Combined with the slope information measured by the IMU inertial navigation unit, when there is no obvious change in the slope, determine whether the wheel is slipping:

[0089] If the driving power remains unchanged and the speed increases significantly, or the speed remains stable and the driving power decreases significantly, it means that the wheel is slipping;

[0090] When the speed or power changes more, the possibility of wheel slip is greater;

[0091] (3) According to the result of whether the wheel slips, adjust the Kalman gain in the Kalman filter algorithm:

[0092] ① When it is determined that the possibility of slip is high, reduce the Kalman gain and increase the weight corresponding to the IMU inertial navigation unit integration result;

[0093] ② When the possibility of slip is determined to be small, increase the Kalman gain and the weight corresponding to the encoder.

[0094] Through the above process, the feedforward adaptation of the filtering algorithm under the slip condition is achieved, which can improve the real-time performance of the algorithm and the accuracy of the final result (corrected mileage information).

[0095] The inertial-drive fusion odometer unit corrects the odometer information in this embodiment as follows:

[0096] The current sampling circuit of the motor driver unit detects the driving current Iq and bus voltage Vbus of the driving motor, and the power P of the driving motor at this time is q =Iq ·V bus ;

[0097] Combine the driving power with the power calculated based on the slope information: P1=P0+mg·cosθ·v, and compare P in real time. q With P1:

[0098] When P q When it is close to P1 or the difference is stable, it means there is no slip;

[0099] When P1 is constant, P q When it decreases significantly, it is determined that slippage occurs at this moment;

[0100] At the same time, using P q The rate of change of ΔP q A measure of the likelihood of slippage:

[0101] When ΔP q When it is 0, it means slip occurs;

[0102] ΔP q When it is 1, the wheel is in a completely slipping state;

[0103] in,

[0104] At the same time, the final result output by the inertial-drive fusion odometry unit is the mileage information obtained by fusing the information of the IMU inertial navigation unit and the encoder pulse information, calculated using the Kalman filter. The relevant parameters of the Kalman filter are feedforward corrected according to the slip judgment result, thereby realizing the correction of the Kalman filter fusion result according to the degree of slip.

[0105] Among them, the simplified expression of the Kalman filter algorithm is as follows:

[0106] L=(1-K)L imu +K·L en ;

[0107] Among them, L imu The mileage is obtained by integrating the acceleration in the driving direction twice; L en is the mileage calculated using the number of pulses obtained by the driver encoder; K is the Kalman gain matrix, which is understood as the weighted coefficient between the two. By adjusting the value of K, L is controlled imu and L en The weight in the final output;

[0108] According to ΔP q Determine the current road slippage status:

[0109] When ΔP qThe smaller (closer to 0), the less wheel slip there is. Adjust the value of Kalman filter gain K so that L en The proportion increases;

[0110] On the contrary, when ΔP q The larger the value (closer to 1), the closer the vehicle is to a complete slip state. en A large error will occur, and the value of the Kalman filter gain K is adjusted so that L en The proportion decreased.

[0111] Through the above process, it is possible to determine the slip state based on the driver's information, correct the odometer output result, and reduce the mileage error caused by slipping in ordinary odometers.

[0112] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the above embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the above embodiments, or replace some or all of the technical features therein with equivalents. However, these modifications or replacements do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. An integrated system of inertial navigation, motor drive and odometer for a mobile intelligent body, characterized in that: It includes a motor driver unit, an IMU inertial navigation unit, and an inertial-drive fusion odometry unit. The motor driver unit is used to implement closed-loop control of the servo motor. The IMU inertial navigation unit is used to achieve real-time measurement of acceleration and angular velocity, providing acceleration and angular velocity signals to the motor driver unit, the inertial-drive fusion odometry unit, and upper-layer applications. The inertial-drive fusion odometry unit is used to obtain high-precision odometry information based on the encoder pulses collected by the driver and combined with the drive current and IMU signals, and output the corrected odometry information. The motor driver unit adds a PI adaptive module based on the FOC vector control algorithm. It uses the z-acceleration component provided by the IMU inertial navigation unit to calculate the slope of the current road surface, and adaptively adjusts the coefficients Kp and Ki in the PI adaptive module according to the slope: When the slope is positive, that is, when going uphill, the coefficients Kp and Ki in the PI adaptive module are increased so that the motor drive unit can achieve faster, more stable and more precise control when the slope changes; When the slope is negative, that is, downhill, the coefficients Kp and Ki in the PI adaptive module are reduced, so that the motor drive unit can achieve faster, more stable and precise control when the slope changes.

2. The mobile intelligent body inertial navigation, motor driver and odometer integrated system according to claim 1, characterized in that: The motor driver unit includes a power drive circuit, an encoder and a current sampling circuit. The current sampling circuit and the encoder respectively control the power drive circuit to obtain the motor control current through the PWM control signal of the motor vector control algorithm provided by the main processing chip; The IMU inertial navigation unit includes a three-axis accelerometer and a three-axis gyroscope. The IMU inertial navigation element, the three-axis accelerometer is used to achieve real-time measurement of acceleration; the three-axis gyroscope is used to achieve real-time measurement of angular velocity.

3. The integrated system of inertial navigation, motor driver and odometer for mobile intelligent body according to claim 1 or 2, characterized in that: The motor driver unit shares the driving current obtained by its own current sampling circuit, the encoder pulse signal collected by the encoder, and the control current data output by the power drive circuit with the inertial-drive fusion odometer unit in the form of software external variables. The IMU inertial navigation unit shares the processed acceleration and angular velocity information with the motor driver unit and the inertial-drive fusion odometer unit through UART or I2C.

4. The integrated system of inertial navigation, motor driver and odometer for mobile intelligent body according to claim 3, characterized in that: The specific implementation of the PI adaptive module is as follows: The acceleration and angular velocity data of the vehicle during movement are obtained through the IMU inertial navigation unit; Decompose the acceleration data measured by the IMU into the acceleration of linear motion and the acceleration of gravity; Using gravitational acceleration to determine the standard reference frame of the vehicle; The angular velocity data measured by the IMU and the angle of the vehicle reference system can be used to calculate the lateral and longitudinal angle changes of the vehicle. The change in the vehicle's front and rear tilt angle is the slope angle of the road surface. At the same time, the road slope angle data measured by the IMU is filtered. After obtaining the slope information, the PID parameters are adjusted adaptively according to the slope. The driving power PI at the current slope is calculated using the slope information. The formula is as follows: ; Where m is the mass of the vehicle; g is the acceleration due to gravity; is the road slope calculated based on the IMU inertial navigation unit information, is the current speed, P0 is the driving power on a flat road, and the formula is as follows: ; in, It is the resistance encountered during driving; According to the relationship between P1 and P0, the coefficients Kp and Ki in the PI adaptive module are adaptively adjusted to achieve slope adaptive control.

5. The integrated system of inertial navigation, motor driver and odometer for mobile intelligent body according to claim 1, characterized in that: The corrected odometer information output by the inertial-drive fusion odometer unit is obtained by using a Kalman filter algorithm to obtain a displacement value obtained by twice integrating the forward direction acceleration obtained by the IMU inertial navigation unit and a pulse value obtained by encoder pulse conversion.

6. The integrated system of inertial navigation, motor driver and odometer for mobile intelligent body according to claim 5, characterized in that: The Kalman filter algorithm is as follows: The real-time driving power is calculated by using the driving current and current voltage information collected by the current adopting circuit of the motor driver unit; Combined with the slope information measured by the IMU inertial navigation unit, when there is no obvious change in the slope, determine whether the wheel is slipping: If the driving power remains unchanged and the speed increases significantly, or the speed remains stable and the driving power decreases significantly, it means that the wheel is slipping; When the speed or power changes more, the possibility of wheel slip is greater; Adjust the Kalman gain in the Kalman filter algorithm based on whether the wheel slips: When it is determined that the possibility of slip is high, the Kalman gain is reduced and the weight corresponding to the integration result of the IMU inertial navigation unit is increased; When the slip possibility is determined to be small, the Kalman gain is increased and the weight corresponding to the encoder is increased.

7. The integrated system of inertial navigation, motor driver and odometer for mobile intelligent body according to claim 6, characterized in that: The inertial-drive fusion odometer unit corrects the odometer information as follows: The current sampling circuit of the motor driver unit detects the driving current Iq and bus voltage Vbus of the driving motor, and the power of the driving motor at this time is P q = I q · V bus ; Combining this driving power with the power obtained and calculated based on the slope information: , real-time comparison P q and : when P q and When the difference is close or stable, it means there is no slip; when constant, P q When it decreases significantly, it is determined that slippage occurs at this moment; At the same time, using P q The rate of change Δ P q Measure the size of the slip possibility: when When it is 0, it means slip occurs; When it is 1, the wheel is in a completely slipping state; in, ; At the same time, the final result output by the inertial-drive fusion odometry unit is the mileage information obtained by fusing the information of the IMU inertial navigation unit and the encoder pulse information, calculated using the Kalman filter. The relevant parameters of the Kalman filter are feedforward corrected according to the slip judgment result, thereby realizing the correction of the Kalman filter fusion result according to the degree of slip. Among them, the simplified expression of the Kalman filter algorithm is as follows: L =(1-K) L imu +K· L en ; in, L imu The mileage is obtained by integrating the acceleration in the driving direction twice; L en is the mileage calculated using the number of pulses obtained by the driver encoder; K is the Kalman gain matrix, which is understood as the weighted coefficient between the two. By adjusting the value of K, the control L imu and L en The weight in the final output; according to Determine the current road slippage status: when The smaller the value, the less wheel slippage there is. Adjust the value of Kalman filter gain K so that L en The proportion increases; On the contrary, when The larger the value, the closer the current state is to complete slip. L en A large error will be generated. Adjust the value of Kalman filter gain K so that L en The proportion decreased.

Images