阅读说明：本技术 一种移动机器人的姿态估计方法及终端设备 (Attitude estimation method of mobile robot and terminal equipment ) 是由 石鹏 林辉 卢维 殷俊 于 2019-10-16 设计创作，主要内容包括：本发明提供一种移动机器人的姿态估计方法,所述移动机器人包括车体,其中所述车体上固定安装有IMU和里程计,该方法包括：对所述IMU所采集的IMU数据进行互补滤波,以估计所述移动机器人的运动姿态,其中所述运动姿态包括偏航角、俯仰角和横滚角；以及利用所述里程计的更新周期和所述里程计所采集的里程计数据校正所述运动姿态中的偏航角,并将校正后的偏航角更新为所述运动姿态中的偏航角。本发明还提供相应的终端设备。本发明能够准确估计每个姿态中的偏航角。(The invention provides a posture estimation method of a mobile robot, wherein the mobile robot comprises a vehicle body, an IMU and a milemeter are fixedly arranged on the vehicle body, and the method comprises the following steps: complementarily filtering IMU data acquired by the IMU to estimate a motion attitude of the mobile robot, wherein the motion attitude comprises a yaw angle, a pitch angle and a roll angle; and correcting the yaw angle in the motion attitude by using the update cycle of the odometer and the odometer data acquired by the odometer, and updating the corrected yaw angle into the yaw angle in the motion attitude. The invention also provides corresponding terminal equipment. The invention can accurately estimate the yaw angle in each attitude.)

1. An attitude estimation method of a mobile robot including a vehicle body on which an IMU and a odometer are fixedly mounted, comprising:

complementarily filtering IMU data acquired by the IMU to estimate a motion attitude of the mobile robot, wherein the motion attitude comprises a yaw angle, a pitch angle and a roll angle; and

and correcting the yaw angle in the motion attitude by utilizing the update cycle of the odometer and the odometer data acquired by the odometer, and updating the corrected yaw angle into the yaw angle in the motion attitude.

2. The method of claim 1, wherein the odometry data collected by the odometer includes an odometer angular velocity w 'at a current time t (k)'_bz(k) And a mileometer angular velocity w 'at the update time t (k-m)'_bz(k-m)；

The correcting the yaw angle in the kinematic pose using the updated period of the odometer and the odometer data collected by the odometer comprises:

obtaining a yaw angle ψ of the current time t (k) estimated by complementary filtering_kAnd yaw angle ψ of the update time t (k-m)_k-mAnd calculating a first yaw angle increment from the updating time t (k-m) to the current time t (k) according to formula 1

Wherein the update period is m Δ T;

and (c) calculating angular speed w 'according to the mileage at the current moment t (k)'_bz(k) And a odometer angular velocity w 'at the update time t (k-m)'_bz(k-m) calculating a second yaw angle increment from the update time t (k-m) to the current time t (k)

Determining the second yaw angle increment

calculating the final yaw angle increment according to formula 2

Calculating the corrected yaw angle psi 'of the current time t (k) according to formula 3'_k，

3. A method as claimed in claim 2, characterised by odometry angular speed w 'according to the current time t (k)'_bz(k) And a odometer angular velocity w 'at the update time t (k-m)'_bz(k-m) calculating a second yaw angle increment from the update time t (k-m) to the current time t (k)

respectively calculating the odometer angle of the current time t (k) according to formula 4Speed w'_bz(k) Component w 'in the navigation coordinate system Z axis'_nz(k) And a odometer angular velocity w 'at the update time t (k-m)'_bz(k-m) component w 'in the Z-axis of the navigation coordinate system'_nz(k-m)，

Wherein the content of the first and second substances,

calculating the angular velocity according to equation 5

Wherein λ is₀Is a parameter that varies according to the update frequency of the odometer, and 0 ≦ λ₀≤1；

Calculating the second yaw angle increment according to equation 6

4. The method as recited in claim 2, wherein the IMU data includes gyroscope data and accelerometer data, the gyroscope data including a gyroscope angular velocity vector w (k) ═ w for the current time t (k)_bx(k) w_by(k) w_bz(k)]^T；

The determining the second yaw angle increment

estimating the absolute value of the angular velocity of the mobile robot along the Z-axis of the carrier coordinate system according to equation 7

Wherein, w_bzThe value of the component of the angular velocity vector w (k) of the gyroscope on the Z axis of the carrier coordinate system is represented, m' is an adjustable parameter and satisfies 0<m′≤m；

Threshold value omega according to odometer angular speed range_EDetermining a weighting value α, where ω_E>0, when

5. The method as claimed in claim 4, wherein the weighting value α is calculated according to equation 8,

wherein λ is₁Is an adjustable proportional parameter, and₁>0。

6. the method of claim 1, wherein the IMU data includes gyroscope data and accelerometer data, the gyroscope data including the current time t(k) The gyroscope angular velocity vector w (k) ═ w_bx(k) w_by(k) w_bz(k)]^TThe accelerometer data includes an acceleration vector a (k) ═ a at the current time t (k)_bx(k) a_by(k) a_bz(k)]^T；

Complementarily filtering IMU data acquired by the IMU to estimate a motion pose, comprising:

determining initialization parameters of the complementary filtering, wherein the initialization parameters of the complementary filtering comprise a proportional parameter kp, an integral parameter ki and an initial integral error vector e_Int＝[0 0 0]^T；

Calculating an integral error vector e of the current time t (k) according to equation 9_Int(k) And then the amplitude limit is carried out,

e_Int(k)＝e_Int(k-1)+ki*e_aΔ T formula 9, the values of Δ T,

wherein e is_aRepresents an error vector and is represented by e_aCalculated as a ' (k) × (-g '), where a ' (k) is an acceleration vector a (k) ═ a_bx(k) a_by(k) a_bz(k)]^TG' is the projection of a unit vector in the gravity acceleration direction under a carrier coordinate system, and delta T is the updating period of the IMU;

using said error vector e_aAnd the clipped integrated error vector e_Int(k) Correcting the gyro angular velocity vector w (k) ═ w at the current time t (k)_bx(k) w_by(k) w_bz(k)]^TObtaining the corrected angular velocity vector of the current time t (k)

A quaternion q (k) ═ q at the current time t (k) is calculated according to equation 10₀(k) q₁(k) q₂(k) q₃(k)]^T，

Wherein the content of the first and second substances,

a quaternion q (k) ═ q at the current time t (k)₀(k) q₁(k) q₂(k) q₃(k)]^TAnd processing to obtain the attitude of the current time t (k).

7. A method as claimed in claim 6, characterized in that the clipped integrated error vector e_Int(k) Comprises the following steps:

wherein e is_Int(k)_iFor said integrated error vector e_Int(k) The ith component of, -e_{Int_E}Is a minimum integral error threshold, e_{Int_E}Is the maximum integrated error threshold.

8. The method as recited in claim 6 wherein said utilizing said error vector e_aAnd the clipped integrated error vector e_Int(k) Correcting the gyro angular velocity vector w (k) ═ w at the current time t (k)_bx(k) w_by(k) w_bz(k)]^TThe method comprises the following steps:

according to the acceleration vector a (k) ═ a_bx(k) a_by(k) a_bz(k)]^TThe modulus value of (1) is adaptively changed according to the proportional parameter kp to obtain a changed proportional parameter kp';

clipping the changed comparative example parameter kp ', wherein if kp ' is less than 0, kp ' is 0;

calculating the corrected angular velocity vector of the current time t (k) according to formula 11

9. The method of claim 8 wherein the acceleration vector a (k) has a modulus greater thanOr less than

When the modulus of the acceleration vector a (k) is less than

wherein g is the modulus of the gravity acceleration vector,

10. A terminal device comprising a processor and a memory, the memory storing instructions that, when executed, cause the processor to perform a pose estimation method of a mobile robot according to any of claims 1-9.

Technical Field

The disclosed embodiments of the present invention relate to the field of mobile robot navigation and positioning technologies, and more particularly, to a method for estimating a posture of a mobile robot and a terminal device.

Background

In the application of the mobile robot, navigation positioning is a key technology of the application, and accurate positioning is required for navigation. The current methods for estimating the attitude include those based on visual sensors, GPS, and the like. The method based on the visual sensor has higher requirements on the surface texture and illumination of the surrounding environment; the method based on the laser radar has higher requirements on the structural characteristics of the surrounding environment, and the GPS cannot be applied to sheltered areas and indoors and can only be used in open outdoor environment.

Disclosure of Invention

According to an embodiment of the present invention, the present invention provides a method for estimating an attitude of a mobile robot and a terminal device to solve the above problem.

According to a first aspect of the present invention, an exemplary attitude estimation method of a mobile robot including a vehicle body on which an IMU and an odometer are fixedly mounted, includes: complementarily filtering IMU data acquired by the IMU to estimate a motion attitude of the mobile robot, wherein the motion attitude comprises a yaw angle, a pitch angle and a roll angle; and correcting the yaw angle in the motion attitude by using the update cycle of the odometer and the odometer data acquired by the odometer, and updating the corrected yaw angle into the yaw angle in the motion attitude.

According to a second aspect of the present invention, an exemplary terminal device is disclosed, the terminal device comprising a processor and a memory, the memory storing instructions that, when executed, cause the processor to perform the method of pose estimation of a mobile robot of the first aspect described above.

The invention has the following beneficial effects: the motion attitude of the mobile robot is estimated by performing complementary filtering on IMU data acquired by the IMU, the pitch angle and the roll angle in each attitude can be accurately estimated, and then the yaw angle in the motion attitude is corrected by utilizing the update period of the odometer and the odometer data acquired by the odometer, so that the yaw angle in each attitude can be accurately estimated, and the estimation of the yaw angle is more accurate.

Drawings

Fig. 1 is a schematic structural diagram of a mobile robot according to an embodiment of the present invention.

Fig. 2 is a flowchart of a posture estimation method of a mobile robot according to a first embodiment of the present invention.

Fig. 3 is a flowchart of a posture estimation method of a mobile robot according to a second embodiment of the present invention.

Fig. 4 is a flowchart of a posture estimation method of a mobile robot according to a third embodiment of the present invention.

Fig. 5 is a schematic structural diagram of a terminal device according to an embodiment of the present invention.

FIG. 6 is a schematic diagram of a non-volatile storage medium according to an embodiment of the present invention.

Detailed Description

In order to make those skilled in the art better understand the technical solution of the present invention, the technical solution of the present invention is further described in detail below with reference to the accompanying drawings and the detailed description.

For a clear understanding of the present invention, the following description will first be made of a mobile robot.

As shown in fig. 1, which is a schematic structural diagram of a mobile robot according to an embodiment of the present invention, the mobile robot 100 may include a wheel robot. The mobile robot 100 includes a vehicle body 101, and an inertial navigation unit (IMU) 102 and an odometer (odometer) 103 are fixedly mounted on the vehicle body 101. The IMU102 includes a three-axis gyroscope and a three-axis accelerometer. The positions of the IMU102 and the odometer 103 on the vehicle body are subject to practical application and are not limited herein. Since the IMU102 and the odometer 103 are fixedly mounted on the vehicle body 101, it is assumed that the coordinate system of the IMU102 and the coordinate system of the odometer 103 coincide with the carrier coordinate system of the mobile robot. The coordinate system of the mobile robot 100 will be described below.

The coordinate system of the mobile robot 100 includes a carrier coordinate system and a navigation coordinate system. The carrier coordinate system is denoted as the b coordinate system, where X^bThe axis pointing forward of the vehicle body, Y^bThe axis points to the left side of the vehicle body, Z^bThe axis is perpendicular to the cross section of the vehicle body and points upwards, and the three axial directions accord with the right-hand rule, the origin O^bIs the center of the vehicle body. The navigation coordinate system is denoted as the n coordinate system, where XⁿOⁿYⁿPlane parallel to horizontal plane, XⁿX of b coordinate system with axis pointing to and at initial time^bThe projection of the axes on the horizontal plane being coincident, ZⁿThe axis pointing in the sky direction, YⁿThe axial direction is determined by the right-hand rule, origin OⁿOrigin O of b coordinate system as initial time^b。

The solution of the invention is described below in connection with the mobile robot in fig. 1.

As shown in fig. 2, a flowchart of a posture estimation method of a mobile robot according to a first embodiment of the present invention is a method that can be executed by a terminal device, such as a mobile terminal, a desktop computer, a server, etc., and includes:

step 110: and performing complementary filtering on IMU data acquired by the IMU to estimate the motion attitude of the mobile robot.

Wherein the motion attitude comprises a yaw angle psi, a pitch angle theta and a roll angle gamma.

The IMU includes a three-axis gyroscope and a three-axis accelerometer, and the IMU data includes gyroscope data collected by the three-axis gyroscope and accelerometer data collected by the three-axis accelerometer.

The IMU data collected at different moments are subjected to complementary filtering, so that the posture of the mobile robot at the corresponding moment can be estimated, and further the postures at different moments form a motion posture. The estimated poses at different times may be stored, for example, in a memory.

Wherein, in some embodiments, the mobile robot is initialized to estimate the initial pose of the mobile robot before the complementary filtering. The initial pose of the mobile robot refers to the pose of the mobile robot when it is at rest and is estimated from the accelerometer data. The accelerometer data at rest includes an acceleration vector a ═ a_bx a_by a_bz]^TThe initial attitude comprising yaw angle psi₀Angle of pitch theta₀And roll angle γ₀. The mobile robot is in a static state, the data measured by the triaxial accelerometer is the modulus of the gravity acceleration vector, and the yaw angle psi₀Angle of pitch theta₀And roll angle γ₀The calculation formula of (a) is as follows:

ψ₀＝0

wherein g is a modulus of the gravity acceleration vector.

Step 120: and correcting the yaw angle in the motion attitude by using the update period of the odometer and the odometer data collected by the odometer, and updating the corrected yaw angle into the yaw angle in the motion attitude.

The odometry data includes odometry angular velocity. The update cycle of the odometer refers to an update cycle of the odometer data.

And correcting the yaw angle in the motion attitude by using the update period of the odometer and the odometer data, and further taking the corrected yaw angle as the yaw angle in the motion attitude. For example, the yaw angle in the attitude at a certain time is corrected using the update cycle of the odometer and the odometer data at that time, and the corrected yaw angle is used as the yaw angle in the attitude at that time.

In this embodiment, the motion attitude of the mobile robot is estimated by performing complementary filtering on IMU data acquired by the IMU, the pitch angle and the roll angle in each attitude can be accurately estimated, and then the yaw angle in the motion attitude is corrected by using the update period of the odometer and the odometer data acquired by the odometer, and the yaw angle in each attitude can be accurately estimated, so that the estimation of the yaw angle is more accurate.

In some embodiments, the odometer-collected odometer data includes an odometer angular velocity w 'at the current time t (k)'_bz(k) And a mileometer angular velocity w 'at the update time t (k-m)'_bz(k-m). Wherein the update time t (k-m) refers to a time point at which the odometer data is updated.

As shown in fig. 3, which is a flowchart of a posture estimation method of a mobile robot according to a second embodiment of the present invention, on the basis of the first embodiment in fig. 1, the above step 120 includes:

step 1201: obtaining estimated by complementary filteringYaw angle ψ at present time t (k)_kAnd updating the yaw angle psi at the time t (k-m)_k-mAnd calculating a first yaw angle increment from the updating time t (k-m) to the current time t (k) according to the formula 1

Wherein the update period of the odometer is m delta T.

As described above, the motion attitude estimated by the complementary filtering is stored in the memory, and the yaw angle ψ at the current time t (k) can be acquired from the memory_kAnd updating the yaw angle psi at the time t (k-m)_k-m。

Step 1202: calculating angular speed w 'according to mileage at current moment t (k)'_bz(k) And a mileometer angular velocity w 'at the update time t (k-m)'_bz(k-m) calculating a second yaw angle increment from the update time t (k-m) to the current time t (k)

I.e. second yaw angle increment

Is calculated from odometry data.

Step 1203: determining a second yaw angle increment

The weight value of alpha.

Wherein alpha is more than or equal to 0 and less than or equal to 1.

Step 1204: calculating the final yaw angle increment according to formula 2

I.e. for the first yaw angle increment

And a second yaw angle increment

Carrying out weighted average to obtain the final yaw angle increment

Step 1205: calculating the corrected yaw angle ψ 'of the current time t (k) according to equation 3'_k。

Increment the final yaw angle

Yaw angle psi with update time t (k-m)_k-mAdding to obtain the corrected yaw angle psi at the current time t (k)_k. Corrected yaw angle psi'_kYaw angle ψ of current time t (k) estimated by substitution of complementary filtering_kYaw angle ψ as the current time t (k)_kI.e. psi_k＝ψ’_k。

In this embodiment, a first yaw angle increment obtained by estimating the IMU data through complementary filtering and a second yaw angle increment obtained by calculating according to the odometer angular velocity are subjected to weighted fusion to obtain a final yaw angle increment, and a corrected yaw angle is calculated by using the final yaw angle increment, so that the yaw angle estimated through complementary filtering is corrected, and the estimation result of the yaw angle is more accurate.

In some embodiments, in step 1202, the angular velocity w 'is calculated according to the mileage at the current time t (k)'_bz(k) And a mileometer angular velocity w 'at the update time t (k-m)'_bz(k-m) calculating a second yaw angle from the update time t (k-m) to the current time t (k)Increment of

The method comprises the following specific steps:

firstly, according to formula 4, the odometer angular velocity w 'at the current time t (k) is respectively calculated'_bz(k) Component w 'in the navigation coordinate system Z axis'_nz(k) And a mileometer angular velocity w 'at the update time t (k-m)'_bz(k-m) component w 'in the Z-axis of the navigation coordinate system'_nz(k-m)，

Wherein the content of the first and second substances,

respectively, at time t (k)_i) The pitch angle and roll angle of the mobile robot.

Then, according to equation 5, the angular velocity is calculated

Wherein λ is₀Is a parameter that varies according to the update frequency of the odometer, and 0 ≦ λ₀Less than or equal to 1. Wherein, in some embodiments, the smaller the update frequency, λ is₀The larger.

It can be seen that the angular velocity

Is the odometer angular velocity w 'at the current time t (k)'_bz(k) Component w 'in the navigation coordinate system Z axis'_nz(k) And a mileometer angular velocity w 'at the update time t (k-m)'_bz(k-m) component w 'in the Z-axis of the navigation coordinate system'_nz(k-m) weighted average.

Then, a second yaw angle increment is calculated according to equation 6

It can be seen that the second yaw angle increment

Is the diagonal velocity

And performing integral operation.

In some embodiments, the IMU data includes gyroscope data including a gyroscope angular velocity vector w (k) ═ w at the current time t (k) and accelerometer data_bx(k) w_by(k) w_bz(k)]^T。

In some embodiments, in step 1203 above, a second yaw angle increment is determined

The weight value of alpha. The method comprises the following specific steps:

firstly, according to formula 7, estimating the absolute value of the angular velocity of the mobile robot along the Z axis of the carrier coordinate system

Wherein, w_bzThe value of the component of the angular velocity vector w (k) of the gyroscope on the Z axis of the carrier coordinate system is represented, m' is an adjustable parameter and satisfies 0<m′≤m。

Subsequently, a threshold value ω according to the odometer angular velocity range_EDetermining a weighting value α, where ω_E>0, when

Time, weightThe value alpha is greater than the predetermined value when

When the weighting value α is smaller than the preset value, the preset value is between 0 and 1. It can be seen that when

When the weight value alpha is larger, when

The weighting value alpha is smaller.

It can be seen that the absolute value of the angular velocity of the mobile robot along the Z-axis of the carrier coordinate systemThreshold value omega of angular speed range of odometer_EComparing to determine a second yaw angle increment

The weight value of alpha.

In this embodiment, the second yaw angle increment estimated by the complementary filtering is adaptively determined by comparing the gyroscope data with the odometer data

The weighted value alpha ensures that when the measurement error of gyroscope data is small, the yaw angle increment obtained by complementary filtering estimation of IMU data is used more truthfully, and when the measurement error of odometer angular velocity is small, the yaw angle increment obtained by calculation of odometer angular velocity data is used more truthfully, so that the estimation of the yaw angle has better effect under different motion states of the vehicle body.

In some embodiments, the weighting value a is calculated according to equation 8,

wherein λ is₁In adjustable ratioExample parameter, and λ₁>0。

The weighting value α may also be calculated according to other ways, and the present invention is not limited thereto.

In some embodiments, the IMU data includes gyroscope data including a gyroscope angular velocity vector w (k) ═ w at the current time t (k) and accelerometer data_bx(k) w_by(k) w_bz(k)]^TThe accelerometer data includes an acceleration vector a (k) ═ a at the current time t (k)_bx(k) a_by(k) a_bz(k)]^T。

As shown in fig. 4, which is a flowchart of a posture estimation method of a mobile robot according to a third embodiment of the present invention, on the basis of the first embodiment in fig. 1, the above step 110 includes:

step 1101: initialization parameters for the complementary filtering are determined.

Wherein, the initialization parameters of the complementary filtering comprise a proportional parameter kp, an integral parameter ki and an initial integral error vector e_Int＝[0 0 0]^T。

The initialization parameters of the complementary filtering are determined after the mobile robot is initialized, that is, after the initialization of the IMU and the odometer on the vehicle body is completed. The initialization operation of the mobile robot is to estimate the initial pose of the mobile robot, and is described in detail in the above embodiments.

Step 1102: calculating an integral error vector e of the current time t (k) according to equation 9_Int(k) And then the amplitude limit is carried out,

e_Int(k)＝e_Int(k-1)+ki*e_aΔ T equation 9

Wherein e is_aRepresents an error vector and is represented by e_aCalculated as a ' (k) × (-g '), where a ' (k) is an acceleration vector a (k) ═ a_bx(k) a_by(k) a_bz(k)]^TAnd g' is the projection of a unit vector in the gravity acceleration direction in a carrier coordinate system, and delta T is the updating period of the IMU.

Wherein, the calculation formula of g' is as follows:

wherein q is₀(k-1)、q₁(k-1)、q₂(k-1) and q₃(k-1) is a quaternion calculated from the attitude at time t (k-1), i.e., Q (k-1) ═ Q₀(k-1) q₁(k-1) q₂(k-1) q₃(k-1)q₄(k-1)]^TWherein the attitude at time t (k-1) comprises the yaw angle psi_k-1Angle of pitch theta_k-1And roll angle γ_k-1。

In some embodiments, the clipped integrated error vector e_Int(k) Comprises the following steps:

wherein e is_Int(k)_iAs an integral error vector e_Int(k) The ith component of, -e_{Int_E}Is a minimum integral error threshold, e_{Int_E}Is the maximum integrated error threshold.

It can be seen that for the integrated error vector e_Int(k) Limiting the amplitude of the component e_Int(k)_iIs limited to the interval [ -e ]_{Int_E},e_{Int_E}]Then the pair is subjected to a clipping operation, so that the component e_Int(k)_iIs at a set minimum integrated error threshold value-e_{Int_E}And a maximum integral error threshold e_{Int_E}In the meantime.

Step 1103: using error vectors e_aAnd the clipped integrated error vector e_Int(k) Correcting the gyro angular velocity vector w (k) ═ w at the current time t (k)_bx(k) w_by(k) w_bz(k)]^TObtaining the corrected angular velocity vector at the current time t (k)

Step 1104: the quaternion q (k) ═ q at the current time t (k) is calculated according to equation 10₀(k) q₁(k) q₂(k) q₃(k)]^T。

Wherein the content of the first and second substances,

as can be seen,

is the corrected angular velocity vector according to the current time t (k)

Calculated and used to calculate the amount of transformation of the quaternion q (k) over the time interval Δ T.

Step 1105: and processing the quaternion Q (k) at the current time t (k) to obtain the attitude of the current time t (k).

The processing includes normalization and conversion. The quaternion q (k) at the current time t (k) is normalized, and then the normalized quaternion q (k) is converted into an attitude angle (i.e., yaw angle ψ)_kAngle of pitch theta_kAnd roll angle γ_k) Thereby obtaining the attitude at the current time t (k).

In this embodiment, by integrating the error amount e_Int(k) Performing clipping to prevent the integration error from affecting the accuracy of attitude estimation too much when estimating attitude for a long time, and using the error vector e_aAnd the clipped integrated error vector e_Int(k) Correcting the gyro angular velocity vector w (k) ═ w at the current time t (k)_bx(k) w_by(k) w_bz(k)]^TAnd the estimation precision of the complementary filtering is improved.

In some embodiments, in the above step 1103, the error vector e is utilized_aAnd the clipped integrated error vector e_Int(k) Correcting the gyro angular velocity vector w (k) ═ w at the current time t (k)_bx(k) w_by(k) w_bz(k)]^TThe method comprises the following steps:

firstly, according to the acceleration vector a (k) ═ a_bx(k) a_by(k) a_bz(k)]^TThe comparative example parameter kp is adaptively varied to obtain a varied comparative example parameter kp'. Specifically, in some embodiments, when the acceleration vector a (k) has a modulus value greater than

Or less than

When kp' is less than kp; when the modulus of the acceleration vector a (k) is less than

And is greater than

When kp' ═ kp. g is the modulus of the gravity acceleration vector,

and

is a threshold scale parameter.

Subsequently, the changed comparative example parameter kp ' is clipped, wherein if kp ' <0, kp ' ═ 0.

Then, according to formula 11, the corrected angular velocity vector at the current time t (k) is calculated

In the present embodiment, the correction gyro angular velocity vector w (k) ═ is implemented by adaptively changing the proportional parameter kp according to the motion acceleration condition of the vehicle body to obtain the changed proportional parameter kp ″w_bx(k) w_by(k) w_bz(k)]^TObtaining corrected angular velocity vector

Thereby improving the estimation accuracy of the complementary filtering.

Fig. 5 is a schematic structural diagram of a terminal device according to an embodiment of the present invention. The terminal device includes a memory 510, a processor 520, an output device 530, and a bus 540.

Memory 510 may include both read-only memory and random-access memory, and provides instructions and data to processor 520. A portion of the memory 510 may also include non-volatile storage media, such as non-volatile random access memory (NVRAM).

The memory 510 stores elements, executable modules or data structures, or subsets thereof, or expanded sets thereof: the operation instructions comprise various operation instructions for realizing various operations; an operating system, including various system programs, is used to implement various basic services and to handle hardware-based tasks. The memory 510 also stores motion trajectories characterized by associated data of the locations, times, and their ID values of the moving objects that appear in the video frames of the video.

The output devices 530 include a display device such as a Cathode Ray Tube (CRT) or a Liquid Crystal Display (LCD), etc., and a speaker or similar audio output device. Some embodiments include devices such as touch screens that function as both input and output devices.

In a particular application, the various components of the terminal are coupled together by a bus 540, where the bus 540 may include a power bus, a control bus, a status signal bus, and the like, in addition to a data bus. But for clarity of illustration the various busses are labeled as bus 540 in the figures.

In some embodiments, processor 520, by invoking instructions stored by memory 510, may perform the following operations:

complementary filtering is carried out on IMU data acquired by the IMU so as to estimate the motion attitude of the mobile robot, wherein the motion attitude comprises a yaw angle, a pitch angle and a roll angle; and

and correcting the yaw angle in the motion attitude by using the update period of the odometer and the odometer data collected by the odometer, and updating the corrected yaw angle into the yaw angle in the motion attitude.

For a detailed description of the functions of the components of the terminal device in the embodiments of the present invention, please refer to the related description of the methods in the corresponding embodiments above.

The method disclosed in the above embodiments of the present invention may be applied to the processor 520, or implemented by the processor 520. Processor 520 may be an integrated circuit chip having signal processing capabilities. In implementation, the steps of the above method may be performed by integrated logic circuits of hardware or instructions in the form of software in the processor 520. The processor 520 may be a general purpose processor, a Digital Signal Processor (DSP), an Application Specific Integrated Circuit (ASIC), an off-the-shelf programmable gate array (FPGA) or other programmable logic device, discrete gate or transistor logic, discrete hardware components. The various methods, steps and logic blocks disclosed in the embodiments of the present invention may be implemented or performed. A general purpose processor may be a microprocessor or the processor may be any conventional processor or the like. The steps of the method disclosed in connection with the embodiments of the present invention may be directly implemented by a hardware decoding processor, or implemented by a combination of hardware and software modules in the decoding processor. The software module may be located in ram, flash memory, rom, prom, or eprom, registers, etc. storage media as is well known in the art. The storage medium is located in the memory 510, and the processor 520 reads the information in the memory 510 and performs the steps of the above method in combination with the hardware thereof.

The present invention also provides an embodiment of a non-volatile storage medium, as shown in fig. 6, the non-volatile storage medium 600 stores instructions 601 executable by a processor, and the instructions 601 are used for executing the method in the above embodiment. Specifically, the storage medium 800 may be specifically the memory 510 shown in fig. 5 or be a part of the memory 510.

It will be apparent to those skilled in the art that many modifications and variations can be made in the apparatus and method while maintaining the teachings of the present disclosure. Accordingly, the above disclosure should be considered limited only by the scope of the following claims.