CN109571464B - Initial robot alignment method based on inertia and two-dimensional code navigation - Google Patents

Initial robot alignment method based on inertia and two-dimensional code navigation Download PDF

Info

Publication number
CN109571464B
CN109571464B CN201811367651.9A CN201811367651A CN109571464B CN 109571464 B CN109571464 B CN 109571464B CN 201811367651 A CN201811367651 A CN 201811367651A CN 109571464 B CN109571464 B CN 109571464B
Authority
CN
China
Prior art keywords
angle
alignment
robot
initial
dimensional code
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201811367651.9A
Other languages
Chinese (zh)
Other versions
CN109571464A (en
Inventor
王强
陈宇
龙小军
黄科科
崔冠冠
周正
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Truking Intelligent Robot Changsha Co ltd
Original Assignee
Truking Intelligent Robot Changsha Co ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Truking Intelligent Robot Changsha Co ltd filed Critical Truking Intelligent Robot Changsha Co ltd
Priority to CN201811367651.9A priority Critical patent/CN109571464B/en
Publication of CN109571464A publication Critical patent/CN109571464A/en
Application granted granted Critical
Publication of CN109571464B publication Critical patent/CN109571464B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1679—Programme controls characterised by the tasks executed
    • B25J9/1692—Calibration of manipulator

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

The invention relates to an initial alignment method of a robot based on inertia and two-dimensional code navigation, which solves the problems of long alignment time and complex control algorithm existing in the initial value alignment of an indoor plane of a robot inertial navigation system through methods of rough alignment, fine alignment and translation position alignment, and enables the alignment time to be reduced and the calculation complexity to be low by combining an inertial sensor and the angle of a vehicle body and a binary code to carry out Kalman fusion.

Description

Initial robot alignment method based on inertia and two-dimensional code navigation
Technical Field
The invention relates to the field of navigation alignment, in particular to an initial alignment method of a robot based on inertia and two-dimensional code navigation.
Background
In the manufacturing production line, the robot can efficiently, accurately and flexibly complete the material carrying task, and the production flexibility and the enterprise competitiveness are greatly improved. The inertial navigation carrying robot has the advantages of high accuracy, strong flexibility, convenience in combination and compatibility, independence of external information, no influence of weather conditions and man-made interference on work, and wide application range, but the inertial sensor has serious drift along with long-time operation. Therefore, the integrated navigation mode of inertia-two-dimensional code navigation is adopted. In an inertial-two-dimensional code integrated navigation system, initial alignment refers to providing initial values required by navigation calculation of strapdown inertial navigation, and the initial values comprise an initial position, an initial speed and an initial attitude. Because the initial value error of the inertial navigation system has an accumulation effect in the subsequent navigation solution, the initial alignment error must be ensured within a certain range, and particularly the initial attitude error must be strictly limited.
The prior inertial navigation system comprises the following initial alignment methods: compass loop method, kalman filter method, particle filter method, and the like. When the base is obviously shaken or in a motion environment, the signal-to-noise ratio of the gyroscope output is low, the rotation angular velocity information of the earth is difficult to extract from the gyroscope output, and the analytic alignment error is large or even unavailable. In order to inhibit base shaking interference and inertial device errors, compass alignment based on a classical control theory appears earlier, along with the deep research on initial alignment, an alignment scheme is also transferred from an original frequency domain method based on the classical control theory to an optimal estimation method based on a modern control theory, a Kalman filtering algorithm is a core algorithm applied to the initial alignment by the optimal estimation method, and the alignment process is divided into two stages of coarse alignment and fine alignment because the current algorithms such as Kalman filtering generally require a rough initial attitude angle to be given before operation. And in the coarse alignment stage, a coarse initial attitude angle is obtained, and in the fine alignment stage, a high-precision attitude is obtained by using algorithms such as Kalman filtering and the like. When the initial attitude error angle is a small angle, the SINS error equation forming the Kalman algorithm state equation is approximate to a linear equation, and the current initial alignment technology based on the linear equation is relatively mature. However, these methods are affected by indoor environments, computational complexity, and the like in practical applications.
Disclosure of Invention
Technical problem to be solved
Based on the defects of the prior art, in order to solve the problems of long alignment time and complex control algorithm existing in the initial value alignment of an indoor plane of a robot inertial navigation system, the invention provides a robot initial alignment method based on inertia and two-dimensional code navigation, which is suitable for quick initial alignment work of an AGV (automatic guided vehicle) with a new entry path or fault recovery.
(II) technical scheme
In order to achieve the above object, the present invention provides an initial alignment method of a robot based on inertia and two-dimensional code navigation, the initial alignment method comprising the steps of:
s1: coarse angular alignment: initializing a strapdown inertial navigation system and a two-dimensional code scanning gun, and reading a transporter according to the code scanning gunThe angle theta between the robot and the two-dimensional code is used for quickly and coarsely aligning the carrying robot to enable the carrying robot to reach the target angle theta0The coarse error range of (1);
s2: fine angle alignment: reading the angle theta between the carrying robot and the two-dimensional code and the angular velocity Gyro of the gyroscope, wherein the initial angular velocity of the gyroscope is the angular velocity of the trolley at the rough alignment finishing moment, and performing fine alignment by using Kalman filtering to reach a target precision range;
s3: after the angle alignment, carrying out position alignment through translation to enable the position to reach the precision range, and finishing the initial alignment;
wherein, step S2 further includes the following contents: resolving and iterating by using a Kalman filtering algorithm, wherein a system model of the robot is recorded as follows:
Figure BDA0001869006970000021
wherein, x (k) is a state vector of the system at time k, z (k) is a measurement vector at time k, u (k) is an input vector at time k, A, B, H is a system state transition matrix, an input control matrix and a measurement matrix respectively, w (k) and v (k) are process noise and measurement noise respectively, and w (k) and v (k) are two uncorrelated white gaussian noise sequences; and satisfies the following conditions:
Figure BDA0001869006970000031
wherein q (k) and r (k) are Gaussian white noise mean parameters, Q (k) and R (k) are variance matrix parameters, E [ W (k) ] is an expectation function of W (k), and the expectation functions are functions of k time;
the solving steps in a single information fusion cycle are as follows:
(1) state prediction
X(k|k-1)＝AX(k-1)+BU(k-1) (4)
(2) Innovation sequence update
Zk＝Zk-HX(k|k-1) (5)
Innovation here ZkI.e. measured valueDifference from predicted value, i.e. from measured value Z at most recent momentkCorrecting the state prediction value X (k | k-1) as carrying new information about the state;
(3) state prediction mean square error matrix update
P(k|k-1)＝AP(k-1)AT+Q(k) (6)
(4) Filter gain update
Figure BDA0001869006970000032
Kalman filter gain KkIs solved under the criterion of minimum state estimation mean square error, and the value is determined by initial mean square error P (0), state prediction mean square error P (k | k-1) and measurement noise R (k);
(5) linear weighted average state estimation
X(k|k)＝X(k|k-1)+KkZk (8)
(6) State estimation mean square error update
P(k|k)＝(I-KkH)P(k|k-1) (9)
The mean square error P (k | k) is an important component of the filter, representing the accuracy and reliability of the estimation;
considering that the robot basically moves in a horizontal plane, only a heading angle needs to be regulated, and then, a Kalman filtering fine alignment is performed by taking an angle theta of the robot and a two-dimensional code measured by a code scanning gun as a measured value, wherein a system state equation and a measurement equation are as follows:
Figure BDA0001869006970000041
where dt is the actual operating cycle, Gyro is the value of angular velocity measured by the gyroscope, Angle is the Angle, QbiasFor gyroscope drift, wangleBeing angular noise, wgyroIs gyroscope noise;
Figure BDA0001869006970000042
wherein z (k) is the value of angle measured by the code scanning gun, VangleMeasuring noise for the angle;
according to multiple engineering practices, setting initial values as follows: qbias(0)＝0，wangle、wgyroAnd VangleTaken as a constant.
Further, the step S2 further includes the following specific Kalman filtering iteration step:
1) one-step prediction of current body angle
Angle(k|k-1)＝Angle(k-1|k-1)+(Gyro(k-1)-Q_bias(k-1))*dt (12)
2) Covariance prediction
P(k|k-1)＝AP(k-1|k-1)A′+Q (13)
Wherein the initial covariance matrix P (0) [ [1,0 ]]，[0，1]]In particular, set up
Figure BDA0001869006970000051
3) Filter gain update
Figure BDA0001869006970000052
Where k0 is the angle gain, k1 is the angle drift gain, and H ═ 10 is specifically set]，VangleIs a constant value;
k0 and k1 can be calculated from the formulas (13) and (14);
4) innovation sequence update
Angel_err＝z(k)-Angle(k|k-1) (15)
Wherein Angel _ err is the difference between the measured Angle z (k) and the estimated Angle Angle (k | k-1);
Angle(k|k)＝Angle(k|k-1)-k0*Angel_err (16)
Q_bias(k)＝Q_bias(k-1)-k0*Angel_err (17)
obtaining a Kalman filtering angle and an angle drift value according to the formula (16) and the formula (17);
5) new covariance matrix update
P(k|k)＝(I-KkH)P(k|k-1) (18)
And iterating until the angle error reaches the expected precision range, and stopping iterating.
Further, the step S1 further includes the following implementation procedure of angular coarse alignment: obtaining the initial position (x, y) of the robot and the angle theta of the two-dimensional code according to the code scanning gun, and firstly, giving a target angle theta0Can be found the angle error Δ θ:
Δθ＝θ0-θ (1)
and when the angle theta is not equal to 0, controlling the robot to rotate in situ, then obtaining the speed v of the wheel A and the wheel B of the steering engine by the angle theta through a position type PD control algorithm, and controlling the speed of the steering engine until the angle theta of the robot and the two-dimensional code is within a rough error range, so that rough alignment is finished.
Further, the step S3 includes the following implementation of the position alignment process: scanning the two-dimensional code according to a code scanning gun to obtain a target position (x) of the trolley0，y0) And current position (x, y), when y0-y > 0.01, if y0-y > 0, given steering wheel speed v 0.02 m/s; if y is0-y < 0, given steering wheel speed v-0.02 m/s; up to | y0-y | < 0.01, making the front and rear wheels of the robot angle 0 ° and the fixed steering wheel speed v ═ 0; when | x0-x > 0.01, if x0-x > 0, given a steering wheel speed v of 0.02m/s, if x is0-X < 0, given steering wheel speed v-0.02 m/s, up to | X0-x | < 0.01, the positional alignment ends given a steering wheel speed v ═ 0.
(III) advantageous effects
According to the technical scheme, the initial alignment method of the robot based on inertia and two-dimensional code navigation has the advantages that the method for performing initial alignment by using the two-dimensional code and the inertia sensor is provided, Kalman fusion is performed by innovatively combining the inertia sensor and the angle between the vehicle body and the binary code, so that the alignment time can be reduced, the algorithm complexity is low, and meanwhile, the manual intervention degree can be reduced.
Drawings
The features and advantages of the present invention will be more clearly understood by reference to the accompanying drawings, which are illustrative and not to be construed as limiting the invention in any way, and in which:
FIG. 1 is a flow chart of the initial alignment method of the present invention;
FIG. 2 is a flow chart of a coarse alignment implementation of the present invention;
FIG. 3 is a wheel structure view of the robot of the present invention;
FIG. 4 is a Kalman filtering flow chart of the present invention.
Detailed Description
In order to make the objects, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention, and it is obvious that the described embodiments are some, but not all, embodiments of the present invention. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.
The embodiment of the invention provides an initial alignment method of a robot based on inertia and two-dimensional code navigation, the robot using the initial alignment method can be a composite AGV (commonly used in logistics storage), which can be realized by the flow chart of the initial alignment method shown in figure 1,
the initial alignment method comprises the following steps:
s1: coarse angular alignment: initializing a strapdown inertial navigation system and a two-dimensional code scanning gun, reading an angle theta between the carrying robot and the two-dimensional code according to the code scanning gun, and performing quick coarse alignment on the carrying robot to enable the carrying robot to reach a target angle theta0The coarse error range of (1);
the block diagram of the coarse alignment implementation is shown in fig. 2, and the implementation is as follows:
obtaining the initial position (x, y) of the robot and the angle theta of the two-dimensional code through a code scanning gun, and firstly, giving a target angle theta0The angle error Δ θ can be obtained as 0 °:
Δθ＝θ0-θ (1)
when delta theta is larger than zero A wheel angle of 90 degrees, B wheel angle of minus 90 degrees, and when delta theta is smaller than zero A wheel angle of minus 90 degrees, B wheel angle of 90 degrees enables the robot to rotate in situ, namely the double steering wheels are divided into A wheels and B wheels, and the wheels 1, 2, 3 and 4 are supporting wheels, as shown in fig. 3.
And then, obtaining the speed v of the wheel A and the wheel B of the steering engine through a position type PD control algorithm (the control algorithm is shown as the following formula v (n)), controlling the speed of the steering engine until the error of the angle theta of the robot and the two-dimensional code is within the range of +/-5 degrees, and finishing the coarse alignment.
v(n)＝Kp＊Δθ(n)+Kd＊(Δθ(n)-Δθ(n-1))
V (n) -speed at the nth sampling instant is given; Δ θ (n) -angle error at nth sampling instant; kp-proportional amplification factor; kd-scale factor;
s2: fine angle alignment: reading the angle theta between the carrying robot and the two-dimensional code and the angular velocity Gyro of the gyroscope, wherein the initial angular velocity of the gyroscope is the angular velocity of the trolley at the rough alignment finishing moment, and performing fine alignment by using Kalman filtering to reach a target precision range;
as shown in fig. 4, step S2 further includes a method of using a Kalman filter algorithm to perform solution and iteration, where the Kalman filter algorithm is an iterative algorithm and the system model of the robot is:
Figure BDA0001869006970000081
wherein x (k) is the state vector of the system at time k (each vector is preferably specified), z (k) is the measurement vector at time k, u (k) is the input vector at time k, A, B, H are the system state transition matrix, the input control matrix and the measurement matrix, respectively, w (k) and v (k) are the process noise and the measurement noise, respectively, and w (k) and v (k) are two mutually uncorrelated white gaussian noise sequences; and satisfies the following conditions:
Figure BDA0001869006970000082
wherein q (k) and r (k) are Gaussian white noise mean parameters, Q (k) and R (k) are variance matrix parameters, E [ W (k) ] is an expectation function of W (k), and the expectation functions are functions of k time;
the solving steps in a single information fusion cycle are as follows:
(1) state prediction
X(k|k-1)＝AX(k-1)+BU(k-1) (4)
(2) Innovation sequence update
Zk＝Zk-HX(k|k-1) (5)
Innovation here ZkI.e. the difference between the measured value and the predicted value, i.e. from the measured value Z at the most recent momentkCorrecting the state prediction value X (k | k-1) as carrying new information about the state;
(3) state prediction mean square error matrix update
P(k|k-1)＝AP(k-1)AT+Q(k) (6)
(4) Filter gain update
Figure BDA0001869006970000091
Kalman filter gain KkIs solved under the criterion of minimum state estimation mean square error, and the value is determined by initial mean square error P (0), state prediction mean square error P (k | k-1) and measurement noise R (k);
(5) linear weighted average state estimation
X(k|k)＝X(k|k-1)+KkZk (8)
(6) State estimation mean square error update
P(k|k)＝(I-KkH)P(k|k-1) (9)
The mean square error P (k | k) is an important component of the filter, representing the accuracy and reliability of the estimation;
considering that the robot basically moves in a horizontal plane, only a heading angle needs to be regulated, and then, a Kalman filtering fine alignment is performed by taking an angle theta of the robot and a two-dimensional code measured by a code scanning gun as a measured value, wherein a system state equation and a measurement equation are as follows:
Figure BDA0001869006970000101
where dt is the actual operating cycle, Gyro is the value of angular velocity measured by the gyroscope, Angle is the Angle, QbiasFor gyroscope drift, wangleBeing angular noise, wgyroIs gyroscope noise;
Figure BDA0001869006970000102
wherein z (k) is the value of angle measured by the code scanning gun, VangleMeasuring noise for the angle;
according to multiple engineering practices, setting initial values as follows: qbias(0)＝0，wangle、wgyroAnd VangleTaken as a constant.
After the solution is performed, the angular alignment Kalman filtering iterative process of the invention is as follows:
1) one-step prediction of current body angle
Angle(k|k-1)＝Angle(k-1|k-1)+(Gyro(k-1)-Q_bias(k-1))*dt (12)
2) Covariance prediction
P(k|k-1)＝AP(k-1|k-1)A′+Q (13)
Wherein the initial covariance matrix P (0) [ [1,0 ]]，[0，1]]In particular, set up
Figure BDA0001869006970000111
3) Filter gain update
Figure BDA0001869006970000112
Where k0 is the angle gain, k1 is the angle drift gain, and H ═ 10 is specifically set]，VangleIs a constant value;
k0 and k1 can be calculated from the formulas (13) and (14);
4) innovation sequence update
Angel_err＝z(k)-Angle(k|k-1) (15)
Wherein Angel _ err is the difference between the measured Angle z (k) and the estimated Angle Angle (k | k-1);
Angle(k|k)＝Angle(k|k-1)-k0*Angel_err (16)
Q_bias(k)＝Q_bias(k-1)-k0*Angel_err (17)
obtaining a Kalman filtering angle and an angle drift value according to the formula (16) and the formula (17);
5) new covariance matrix update
P(k|k)＝(I-KkH)P(k|k-1) (18)
And iterating until the angle error reaches the expected precision range, and stopping iterating.
S3: after the angle alignment, carrying out position alignment through translation to enable the position to reach the precision range, and finishing the initial alignment;
specifically, step S3 further includes the following implementation of the alignment process: scanning the two-dimensional code according to a code scanning gun to obtain a target position (x) of the trolley0，y0) And current position (x, y), when y0-y > 0.01, if y0-y > 0, given steering wheel speed v 0.02 m/s; if y is0-y < 0, given steering wheel speed v-0.02 m/s; up to | y0-y | < 0.01, making the front and rear wheels of the robot angle 0 ° and the fixed steering wheel speed v ═ 0; when | X0-x > 0.01, if x0-x > 0, given a steering wheel speed v of 0.02m/s, if x is0-x < 0, given steering wheel speed v-0.02 m/s, up to | x0-x | < 0.01, the positional alignment ends given a steering wheel speed v ═ 0.
According to the method for performing initial alignment by using the two-dimensional code and the inertial sensor, the inertial sensor and the angle between the car body and the two-dimensional code are innovatively combined to perform Kalman fusion, so that the data are solved and iterated, the alignment precision is improved, the alignment time can be reduced, the algorithm complexity is low, the method can rapidly complete initial alignment for an AGV which enters a new path or recovers from a fault, and manual operation is reduced.
The technical features of the embodiments described above may be arbitrarily combined, and for the sake of brevity, all possible combinations of the technical features in the embodiments described above are not described, but should be considered as being within the scope of the present specification as long as there is no contradiction between the combinations of the technical features.

Claims (4)

1. A robot initial alignment method based on inertia and two-dimensional code navigation is characterized by comprising the following steps:
s1 coarse angular alignment: initializing a strapdown inertial navigation system and a two-dimensional code scanning gun, reading an angle theta between the carrying robot and the two-dimensional code according to the code scanning gun, and performing quick coarse alignment on the carrying robot to enable the carrying robot to reach a target angle theta0The coarse error range of (1);
and S2, fine angular alignment: reading the angle theta between the carrying robot and the two-dimensional code and the angular velocity Gyro of the gyroscope, wherein the initial angular velocity of the gyroscope is the angular velocity of the trolley at the rough alignment finishing moment, and performing fine alignment by using Kalman filtering to reach a target precision range;
step S2 also includes the following: resolving and iterating by using a Kalman filtering algorithm, wherein a system model of the robot is recorded as follows:
Figure FDA0003247078730000011
wherein, x (k) is a state vector of the system at time k, z (k) is a measurement vector at time k, u (k) is an input vector at time k, A, B, H is a system state transition matrix, an input control matrix and a measurement matrix respectively, w (k) and v (k) are process noise and measurement noise respectively, and w (k) and v (k) are two uncorrelated white gaussian noise sequences; and satisfies the following conditions:
Figure FDA0003247078730000012
wherein q (k) and r (k) are Gaussian white noise mean parameters, Q (k) and R (k) are variance matrix parameters, E [ W (k) ] is an expectation function of W (k), and the expectation functions are functions of k time;
the solving steps in a single information fusion cycle are as follows:
(1) state prediction
X(k|k-1)＝AX(k-1)+BU(k-1) (4)
(2) Innovation sequence update
Zk＝Zk-HX(k|k-1) (5)
Innovation here ZkI.e. the difference between the measured value and the predicted value, i.e. from the measured value Z at the most recent momentkCorrecting the state prediction value X (k | k-1) as carrying new information about the state;
(3) state prediction mean square error matrix update
P(k|k-1)＝AP(k-1)AT+Q(k) (6)
(4) Filter gain update
Figure FDA0003247078730000021
Kalman filter gain KkIs solved under the criterion of minimum state estimation mean square error, and the value is determined by initial mean square error P (0), state prediction mean square error P (k | k-1) and measurement noise R (k);
(5) linear weighted average state estimation
X(k|k)＝X(k|k-1)+KkZk (8)
(6) State estimation mean square error update
P(k|k)＝(I-KkH)P(k|k-1) (9)
The mean square error P (k | k) is an important component of the filter, representing the accuracy and reliability of the estimation;
considering that the robot basically moves in a horizontal plane, only a heading angle needs to be regulated, and then, a Kalman filtering fine alignment is performed by taking an angle theta of the robot and a two-dimensional code measured by a code scanning gun as a measured value, wherein a system state equation and a measurement equation are as follows:
Figure FDA0003247078730000022
where dt is the actual operating cycle, Gyro is the value of angular velocity measured by the gyroscope, Angle is the Angle, QbiasFor gyroscope drift, wangleBeing angular noise, wgyroIs gyroscope noise;
Figure FDA0003247078730000031
wherein z (k) is the value of the angle measured by the code scanning gun, vangleMeasuring noise for the angle;
according to multiple engineering practices, setting initial values as follows: qbias(0)＝0，wangle、wgyroAnd vangleTaking as a constant;
and S3, after the angle alignment, carrying out position alignment through translation to enable the position to reach the precision range, and finishing the initial alignment.
2. The initial alignment method according to claim 1, wherein: the step S2 further includes the following specific Kalman filtering iteration steps:
1) one-step prediction of current body angle
Angle(k|k-1)＝Angle(k-1|k-1)+(Gyro(k-1)-Q_bias(k-1))*dt (12)
2) Covariance prediction
P(k|k-1)＝AP(k-1)AT+Q(k) (13)
Wherein the initial covariance matrix P (0) [ [1,0 ]],[0,1]]In particular, set up
Figure FDA0003247078730000032
3) Filter gain update
Figure FDA0003247078730000033
Where k0 is the angle gain, k1 is the angle drift gain, and H ═ 10 is specifically set]，VangleIs a constant value;
k0 and k1 can be calculated from the formulas (13) and (14);
4) innovation sequence update
Angel_err＝z(k)-Angle(k|k-1) (15)
Wherein Angel _ err is the difference between the measured Angle z (k) and the estimated Angle Angle (k | k-1);
Angle(k|k)＝Angle(k|k-1)-k0*Angel_err (16)
Q_bias(k)＝Q_bias(k-1)-k0*Angel_err (17)
obtaining a Kalman filtering angle and an angle drift value according to the formula (16) and the formula (17);
5) new covariance matrix update
P(k|k)＝(I-KkH)P(k|k-1) (18)
And iterating until the angle error reaches the expected precision range, and stopping iterating.
3. The initial alignment method according to claim 2, wherein: the step S1 further includes the following implementation procedure of angular coarse alignment: obtaining the initial position (x, y) of the robot and the angle theta of the two-dimensional code according to the code scanning gun, and firstly, giving a target angle theta0Can be found the angle error Δ θ:
Δθ＝θ0-θ (1)
and when the angle theta is not equal to 0, controlling the robot to rotate in situ, then obtaining the speed v of the wheel A and the wheel B of the steering engine by the angle theta through a position type PD control algorithm, and controlling the speed of the steering engine until the angle theta of the robot and the two-dimensional code is within a rough error range, so that rough alignment is finished.
4. The initial alignment method according to claim 1, wherein: the step S3 further includes the following implementation of the alignment process: scanning the two-dimensional code according to a code scanning gun to obtain a target position (x) of the trolley0,y0) And current bitSet to (x, y) when | y0-y > 0.01, if y0-y > 0, given steering wheel speed v 0.02 m/s; if y is0-y < 0, given steering wheel speed v-0.02 m/s; up to | y0-y | < 0.01, making the front and rear wheels of the robot angle 0 ° and the fixed steering wheel speed v ═ 0; when | x0-x > 0.01, if x0-x > 0, given a steering wheel speed v of 0.02m/s, if x is0-x < 0, given steering wheel speed v-0.02 m/s, up to | x0-x | < 0.01, the positional alignment ends given a steering wheel speed v ═ 0.
CN201811367651.9A 2018-11-16 2018-11-16 Initial robot alignment method based on inertia and two-dimensional code navigation Active CN109571464B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201811367651.9A CN109571464B (en) 2018-11-16 2018-11-16 Initial robot alignment method based on inertia and two-dimensional code navigation

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201811367651.9A CN109571464B (en) 2018-11-16 2018-11-16 Initial robot alignment method based on inertia and two-dimensional code navigation

Publications (2)

Publication Number Publication Date
CN109571464A CN109571464A (en) 2019-04-05
CN109571464B true CN109571464B (en) 2021-12-28

Family

ID=65922706

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201811367651.9A Active CN109571464B (en) 2018-11-16 2018-11-16 Initial robot alignment method based on inertia and two-dimensional code navigation

Country Status (1)

Country Link
CN (1) CN109571464B (en)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111391849B (en) * 2020-03-11 2021-10-29 三一机器人科技有限公司 Vehicle control method, device, vehicle and readable storage medium

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101162147A (en) * 2007-11-27 2008-04-16 哈尔滨工程大学 Marine fiber optic gyroscope attitude heading reference system mooring extractive alignment method under the large heading errors
CN101216321A (en) * 2008-01-04 2008-07-09 南京航空航天大学 Rapid fine alignment method for SINS
CN101246022A (en) * 2008-03-21 2008-08-20 哈尔滨工程大学 Optic fiber gyroscope strapdown inertial navigation system two-position initial alignment method based on filtering
CN103743414A (en) * 2014-01-02 2014-04-23 东南大学 Initial alignment method of speedometer-assisted strapdown inertial navigation system during running
CN104848858A (en) * 2015-06-01 2015-08-19 北京极智嘉科技有限公司 Two-dimensional code and vision-inert combined navigation system and method for robot

Family Cites Families (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US8855929B2 (en) * 2010-01-18 2014-10-07 Qualcomm Incorporated Using object to align and calibrate inertial navigation system
US9463574B2 (en) * 2012-03-01 2016-10-11 Irobot Corporation Mobile inspection robot
AT514162A1 (en) * 2013-04-09 2014-10-15 Knapp Ag Storage and picking system for fully automated recognition and order picking of articles

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101162147A (en) * 2007-11-27 2008-04-16 哈尔滨工程大学 Marine fiber optic gyroscope attitude heading reference system mooring extractive alignment method under the large heading errors
CN101216321A (en) * 2008-01-04 2008-07-09 南京航空航天大学 Rapid fine alignment method for SINS
CN101246022A (en) * 2008-03-21 2008-08-20 哈尔滨工程大学 Optic fiber gyroscope strapdown inertial navigation system two-position initial alignment method based on filtering
CN103743414A (en) * 2014-01-02 2014-04-23 东南大学 Initial alignment method of speedometer-assisted strapdown inertial navigation system during running
CN104848858A (en) * 2015-06-01 2015-08-19 北京极智嘉科技有限公司 Two-dimensional code and vision-inert combined navigation system and method for robot

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
《基于ROS的移动机器人自主定位与导航方法研究》;王强;《中国优秀硕士学位论文全文数据库 信息科技辑》;20180430(第04期);全文 *

Also Published As

Publication number Publication date
CN109571464A (en) 2019-04-05

Similar Documents

Publication Publication Date Title
CN112013836B (en) Attitude heading reference system algorithm based on improved adaptive Kalman filtering
CN111024064B (en) SINS/DVL combined navigation method for improving Sage-Husa adaptive filtering
CN111721288B (en) Zero offset correction method and device for MEMS device and storage medium
CN109506660B (en) Attitude optimization resolving method for bionic navigation
CN114076610B (en) Error calibration and navigation method and device of GNSS/MEMS vehicle-mounted integrated navigation system
CN108413986B (en) Gyroscope filtering method based on Sage-Husa Kalman filtering
Werries et al. Adaptive Kalman filtering methods for low-cost GPS/INS localization for autonomous vehicles
CN114136315B (en) Monocular vision-based auxiliary inertial integrated navigation method and system
CN113203429B (en) Online estimation and compensation method for temperature drift error of gyroscope
CA2699137A1 (en) Hybrid inertial system with non-linear behaviour and associated method of hybridization by multi-hypothesis filtering
CN111750865A (en) Self-adaptive filtering navigation method for dual-function deep sea unmanned submersible vehicle navigation system
CN110595434B (en) Quaternion fusion attitude estimation method based on MEMS sensor
CN109571464B (en) Initial robot alignment method based on inertia and two-dimensional code navigation
ZhiWen et al. Robust innovation-based adaptive Kalman filter for INS/GPS land navigation
CN113280808A (en) Method and system for improving positioning accuracy of mobile robot
CN116429084A (en) Dynamic environment 3D synchronous positioning and mapping method
Nguyen et al. Improving the accuracy of the autonomous mobile robot localization systems based on the multiple sensor fusion methods
Qu et al. An adaptive delay-compensated filtering system and the application to path following control for unmanned surface vehicles
CN112729332B (en) Alignment method based on rotation modulation
CN115930971B (en) Data fusion processing method for robot positioning and map building
CN115790603A (en) Unmanned aerial vehicle dynamic target estimation method used in information rejection environment
Zarei et al. Performance improvement for mobile robot position determination using cubature Kalman filter
CN115014321A (en) Bionic polarization multi-source fusion orientation method based on adaptive robust filtering
CN114689081A (en) GNSS assisted MINS auto-calibration system and method
CN108957508B (en) Vehicle-mounted POS (point of sale) offline combined estimation method and device

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant
EE01 Entry into force of recordation of patent licensing contract

Application publication date: 20190405

Assignee: TRUKING TECHNOLOGY Ltd.

Assignor: TRUKING INTELLIGENT ROBOT (CHANGSHA) Co.,Ltd.

Contract record no.: X2023980047829

Denomination of invention: A Robot Initial Alignment Method Based on Inertia and QR Code Navigation

Granted publication date: 20211228

License type: Common License

Record date: 20231122

EE01 Entry into force of recordation of patent licensing contract