CN117406607B - Wheeled robot traveling state estimation method based on improved Kalman filtering - Google Patents

Wheeled robot traveling state estimation method based on improved Kalman filtering Download PDF

Info

Publication number
CN117406607B
CN117406607B CN202311694201.1A CN202311694201A CN117406607B CN 117406607 B CN117406607 B CN 117406607B CN 202311694201 A CN202311694201 A CN 202311694201A CN 117406607 B CN117406607 B CN 117406607B
Authority
CN
China
Prior art keywords
wheel
state
square root
representing
robot
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
CN202311694201.1A
Other languages
Chinese (zh)
Other versions
CN117406607A (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.)
Jiangxi Handalong Technology Co ltd
Original Assignee
Jiangxi Handalong Technology 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 Jiangxi Handalong Technology Co ltd filed Critical Jiangxi Handalong Technology Co ltd
Priority to CN202311694201.1A priority Critical patent/CN117406607B/en
Publication of CN117406607A publication Critical patent/CN117406607A/en
Application granted granted Critical
Publication of CN117406607B publication Critical patent/CN117406607B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B13/00—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion
    • G05B13/02—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric
    • G05B13/04—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators
    • G05B13/042—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators in which a parameter or coefficient is automatically adjusted to optimise the performance

Landscapes

  • Engineering & Computer Science (AREA)
  • Health & Medical Sciences (AREA)
  • Artificial Intelligence (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Evolutionary Computation (AREA)
  • Medical Informatics (AREA)
  • Software Systems (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

The invention discloses a wheeled robot advancing state estimation method based on improved Kalman filtering, which comprises the steps of firstly, constructing a longitudinal, lateral and yaw three-degree-of-freedom dynamic model and a wheel model according to the dynamic characteristics of a wheeled robot; then defining a wheel longitudinal force observation sliding mode surface, constructing a self-adaptive sliding mode observer, and realizing the wheel longitudinal force stable observation estimation of the robot; then, constructing a square root volume Kalman filter, and correcting and updating parameters to be estimated for the square root volume Kalman filter by adopting a square root criterion; and finally, optimizing the non-Gaussian noise of the square root volume Kalman filter by adopting a dung beetle optimizing algorithm, so as to realize the accurate estimation of the running state parameters of the wheeled robot. The invention can be used in the field of wheeled robot control, increases the longitudinal force of the robot wheel in the filter estimation, carries out self-adaptive optimization on the filter parameters, and has good state estimation accuracy and robustness.

Description

Wheeled robot traveling state estimation method based on improved Kalman filtering
Technical Field
The invention relates to the technical field of wheeled robot monitoring, in particular to a wheeled robot traveling state estimation method based on improved Kalman filtering.
Background
The robot has the advantages of accurate control, labor cost saving, safety and high efficiency, and is an important means for realizing intelligent manufacturing transformation. Compared with the traditional robot, the driving mode of the wheeled intelligent mobile robot (hereinafter referred to as wheeled robot) is independently controllable, the power transmission efficiency is high, the speed is high, the active safety control of the wheeled intelligent mobile robot is easier to realize, and the method is a main development direction of the wheeled intelligent mobile robot in the future.
The active safety control of the wheeled robot is based on the premise that state parameter information in the advancing process is accurately acquired. Because of technical limitations or the high price of some sensors, the information such as the tire force and yaw motion state is difficult to directly measure. Therefore, it is a technical problem that needs to be solved by those skilled in the art to accurately estimate these difficult-to-measure state parameters using some existing low-cost sensors.
At present, the travelling state estimation method of the wheeled robot mainly comprises Kalman filtering, extended Kalman filtering, unscented Kalman filtering, particle filtering and the like, wherein the Kalman filtering is widely applied to a linear model, the extended Kalman filtering solves the problem that the Kalman filtering cannot process a nonlinear model, the unscented Kalman filtering converts a nonlinear system into a linear system for processing by using unscented transformation, but the structural parameters of a filter system become more complex, and optimal filtering parameters are difficult to determine under the conditions of noise influence, sensor failure and the like.
Disclosure of Invention
According to the embodiment of the application, the wheeled robot state estimation method based on improved Kalman filtering is provided, and real-time accurate estimation of the wheeled robot state is achieved by combining the self-adaptive sliding mode observer and the Kalman filter.
The embodiment of the application provides a wheeled robot travelling state estimation method based on improved Kalman filtering, which comprises the following steps:
step one, constructing a longitudinal, transverse and yaw nonlinear three-degree-of-freedom dynamic model and a wheel model according to the dynamic characteristics of a wheeled robot;
step two, defining a wheel longitudinal force observation sliding mode surface according to the wheel dynamics characteristics, constructing a self-adaptive sliding mode observer to estimate the wheel longitudinal force, and realizing the wheel longitudinal force stable observation estimation of the robot;
thirdly, constructing a square root volume Kalman filter based on the nonlinear three-degree-of-freedom dynamics model in the first step and the longitudinal wheel force observed and estimated in the second step, butting the square root volume Kalman filter with a robot dynamics system, determining the to-be-estimated quantity, the input quantity and the observed quantity of the square root volume Kalman filter, and correcting and updating parameters to be estimated of the square root volume Kalman filter by adopting a square root criterion;
and step four, based on the square root volume Kalman filter constructed in the step three, adopting a dung beetle optimization algorithm to perform optimization processing on non-Gaussian noise of the square root volume Kalman filter, enhancing the robustness of the filter, and finally realizing accurate estimation of the running state parameters of the wheeled robot.
As some embodiments of the present application, in step one, the established nonlinear three-degree-of-freedom dynamics model is shown in the following formula:
in the method, in the process of the invention,is yaw rate; />In the form of a derivative of yaw rate; />Is the front wheel corner; />Is the total mass of the robot; />For robot centroid around->The moment of inertia of the shaft; />And->Longitudinal force and transverse force, respectively +.>Representing the left front wheel>Representing the right front wheel>Representing the left rear wheel>Represents the right rear wheel, for example: />Representing the longitudinal force of the left front wheel, +.>Representing the longitudinal force of the right front wheel, +.>Representing the longitudinal force of the left rear wheel, +.>Representing the longitudinal force of the right rear wheel; />Representing the front track,>representing the rear track; the distances between the mass center and the front and rear axes are +.>And->；/>Is the longitudinal acceleration; />Is the lateral acceleration.
As some embodiments of the present application, in step one, the wheel side force is related to the wheel side angle and the vertical load, so the wheel side angle and the vertical load of each wheel are:
in the method, in the process of the invention,and->The longitudinal speed and the transverse speed of the wheeled robot are respectively; />Is a vertical load; />Is the wheel slip angle; />Is the centroid slip angle; />Is the height of the centroid to the ground.
The transverse force of the wheel is calculated according to a semi-empirical magic tire formula, and the transverse force model is as follows:
in the method, in the process of the invention,indicating wheels->Representing the lateral force of the wheel; />Is a rigidity factor; />Is a curve shape factor; />Is the peak factor; />Is a curve curvature factor.
In the second step, as some embodiments of the present application, first, a wheel longitudinal force observation slip plane is defined as；Is a sliding mode surface function; />Is the rotational angular velocity of the wheel; />The expression of the estimated value is:
in the method, in the process of the invention,the moment of inertia of the wheels; />The effective rotation radius of the wheel; />Driving moment for the wheels; />Estimating a longitudinal force of the wheel; />Is a sliding mode gain coefficient; />Is an observer gain factor; />Is a sign function; />Is a derivative of the wheel angular velocity estimate.
To be used forAs Lyapunov function candidates, the stability conditions of the adaptive sliding mode observer were analyzed:
in the method, in the process of the invention,is a Lyapunov judgment function; />In the differential form of a Lyapunov decision function; />Is a differential form of the sliding mode surface function.
Due toIs bounded, so there is a positive value +.>Such that:
in the method, in the process of the invention,is a positive constant term.
Further written as:
according to the stability conditions of the adaptive sliding-mode observer, i.e.Such that:
the method comprises the following steps:
to reduce the estimation result jitter, an adaptive saturation function is selected as follows:
in the method, in the process of the invention,a factor indicating a rate of change of the determining function; />Is an adaptive saturation function which is continuously and smoothly at [ -1,1]And the value is converted, so that the phenomenon of abrupt value change is avoided.
Further correcting the estimated error value of the longitudinal force of the wheel, and finally obtaining the sliding mode surface as follows:
when the state error tends to stabilize, there isThe method comprises the steps of carrying out a first treatment on the surface of the Thus, an accurate estimate of the wheel longitudinal force is obtained as follows:
as some embodiments of the present application, in step three, the discrete state equation and the observation equation of the square root volume kalman filter of the constructed nonlinear system are shown as follows:
wherein the state variablesThe method comprises the steps of carrying out a first treatment on the surface of the Measurement variable->The method comprises the steps of carrying out a first treatment on the surface of the Input variable->The method comprises the steps of carrying out a first treatment on the surface of the Wherein the input wheel longitudinal force is obtained through estimation in the second step; />Is state process noise; />For measuring process noise; />Is->A time state vector; />Is a state transfer function; />For measuring transfer functions; upper corner mark->Representing a transpose operation on the indicated quantities.
As some embodiments of the present application, in step three, based on the wheel robot dynamics model, a first-order euler discrete method is adopted, and a state equation of the system is expressed as:
in the method, in the process of the invention,is the system sampling time.
The measurement equation of the system is designed as:
as some embodiments of the present application, in step three, the square root volume kalman filter design process is as follows:
1) Setting initial values of state and covariance matrix square root form:
wherein,is the initial value of the state; />Square root factors that are state covariance matrices; />Is a state covariance matrix.
2) Calculating the current state volume point:
wherein,for corresponding->A state volume point at time; />For corresponding->Square root of state covariance matrix of time; />Is->A time state estimator; />Is a set of volumetric points.
3) Propagation of volume points:
wherein,is the volume point after propagation; />Is a corresponding state transfer function; />Is a natural number greater than 0.
4) Estimating the square root of the state prediction mean and covariance matrix:
in the method, in the process of the invention,representing a trigonometric decomposition of the matrix; />Representing a process noise covariance matrix->Square root of (2);is a propagation process matrix; />The weight coefficient of the corresponding moment; />Is according to->Time estimated +.>Time state quantity; />Is according to->Time of day prediction +.>The square root of the time state covariance matrix.
5) Calculating an update state volume point:
wherein,to update the state volume point of the phase.
6) The volume point propagates through the measurement equation:
wherein,for updating the phase-propagated volume vector, +.>Measuring an equation transfer function for the update stage; />Is->Time input quantity.
7) Estimating the square root of the measurement prediction mean and covariance matrix:
in the method, in the process of the invention,representing a process noise covariance matrix->Square root of (2); matrix->Is a process matrix; />Estimating a value for the measured mean value; />To update the square root of the phase covariance matrix.
8) Calculating a cross covariance matrix:
wherein,is a cross covariance matrix; />Is a process matrix.
9) Calculating Kalman gain:
wherein,is the kalman gain.
10 Updating the state estimator:
wherein,is the final state estimate.
11 Square root of the estimated update error variance):
wherein,is the square root of the final error variance.
After the step, the wheeled mobile robot is obtainedIs used for the estimation of the estimated value of (a).
In step four, as some embodiments of the present application, a dung beetle optimization algorithm is used to optimize a noise covariance matrix in a filter algorithm. The matrix and the elements on the matrix are set as the algorithm optimizing dimension as shown in the following formula:
in the method, in the process of the invention,state noise values respectively representing longitudinal vehicle speed, lateral vehicle speed and yaw rate; />Process noise values respectively representing longitudinal acceleration and lateral acceleration; />Representing a diagonal matrix.
Taking the root mean square error between the estimated value of the filter algorithm constructed in the third step and the measured value of the sensor as an optimization algorithm objective function, wherein the fitness function is as follows:
in the method, in the process of the invention,sampling time; />Is->Real measured variables of time; />Is->The time of day is an estimate output by the filter.
One or more technical solutions provided in the embodiments of the present application at least have the following technical effects or advantages:
1. according to the invention, a nonlinear three-degree-of-freedom dynamic model is established according to the wheel dynamic characteristics of the wheeled robot, a self-adaptive sliding mode observation algorithm is designed to rapidly and accurately estimate the longitudinal force of the wheel, the wheel motion state error of the robot is introduced into the sliding mode surface for analysis, and stable and reliable longitudinal force information of the wheel is provided for the subsequent filter design.
2. The invention takes the motion characteristics of the wheeled robot into consideration, and takes the travelling parameters which can be fed back in real time by the wheeled robot as the model input quantity to estimate the state of the established dynamic system model of the wheeled robot. The invention introduces the idea of square root filtering to restrict the filter based on the volume Kalman filtering, adopts the dung beetle optimization algorithm to adaptively optimize the non-Gaussian noise of the square root volume Kalman filter, improves the robustness of the filter, and improves the estimation accuracy to some extent.
3. The traveling state parameter estimation algorithm based on the dynamic model of the wheeled mobile robot is combined with the self-adaptive sliding mode observation algorithm to estimate the longitudinal force of the wheel, so that the requirement on the vehicle-mounted sensor is reduced. Meanwhile, the estimation process is basically not influenced by structural parameter changes such as robot quality, and the method has wider applicability and good anti-interference performance.
Drawings
Fig. 1 is a flowchart of a method for estimating traveling state information of a wheeled robot according to an embodiment of the present invention;
fig. 2 is a dynamic model diagram of a wheeled robot according to an embodiment of the present invention;
FIG. 3 is a technical roadmap of the improved optimized filter estimation method for the wheeled robot proposed by the present invention;
fig. 4 is a graph showing a comparison between a lateral vehicle speed estimated by the method for estimating the state of a wheeled robot according to the present invention and a conventional volumetric kalman filter algorithm and a real value.
Detailed Description
In order to make the objects, technical solutions and advantages of the embodiments of the present invention more clear, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention. It will be apparent that the described embodiments are some, but not all, embodiments of the invention. All other embodiments, which can be made by those skilled in the art based on the embodiments of the invention without making any inventive effort, are intended to be within the scope of the invention.
Example 1: as shown in fig. 1 and 2, in this embodiment, a description will be given of a wheeled robot traveling state estimation method based on an improved kalman filter, taking a lateral vehicle speed at the time of traveling of a wheeled robot as an example, including the steps of:
step one, constructing a longitudinal, transverse and yaw nonlinear three-degree-of-freedom dynamic model and a wheel model according to the dynamic characteristics of the wheeled robot.
The established nonlinear three-degree-of-freedom dynamics model is shown as follows:
in the method, in the process of the invention,is yaw rate; />In the form of a derivative of yaw rate; />Is the front wheel corner; />Is the total mass of the robot; />For robot centroid around->The moment of inertia of the shaft; />And->Longitudinal force and transverse force, respectively +.>Representing the left front wheel>Representing the right front wheel>Representing the left rear wheel>Representing the right rear wheel; />Representing the front track,>representing the rear track; the distance between the centroid and the front and rear axes is +.>And->；/>Is the longitudinal acceleration; />Is the lateral acceleration.
The wheel side force is related to the wheel side angle and the vertical load, so the wheel side angle and the vertical load of each wheel are respectively:
in the method, in the process of the invention,and->The longitudinal speed and the transverse speed of the wheeled robot are respectively; />Is a vertical load; />Is the wheel slip angle; />Is the centroid slip angle; />Is the height of the centroid to the ground.
The transverse force of the wheel is calculated according to a semi-empirical magic tire formula, and the transverse force model is as follows:
in the method, in the process of the invention,representing a wheel; />Is a rigidity factor; />Is a curve shape factor; />Is the peak factor; />Is a curve curvature factor.
And secondly, defining a wheel longitudinal force observation sliding mode surface according to the dynamic characteristics of the wheel, constructing a self-adaptive sliding mode observer to estimate the wheel longitudinal force, and realizing the stable observation and estimation of the wheel longitudinal force of the robot.
First, define the longitudinal force observation slip plane of the wheel as；/>Is a sliding mode surface function; />Is the rotational angular velocity of the wheel; />The expression of the estimated value is:
in the method, in the process of the invention,the moment of inertia of the wheels; />The effective rotation radius of the wheel; />Driving moment for the wheels; />Estimating a longitudinal force of the wheel; />Is a sliding mode gain coefficient; />Is an observer gain factor; />Is a sign function; />Is a derivative of the wheel angular velocity estimate.
To be used forAs Lyapunov function candidates, the stability conditions of the adaptive sliding mode observer were analyzed:
in the method, in the process of the invention,is a Lyapunov judgment function; />In the differential form of a Lyapunov decision function; />Is a differential form of the sliding mode surface function.
Due toIs bounded, so there is a positive value +.>Such that:
in the method, in the process of the invention,is a positive constant term.
Further written as:
according to the stability conditions of the adaptive sliding-mode observer, i.e.Such that:
the method comprises the following steps:
to reduce the estimation result jitter, an adaptive saturation function is selected as follows:
in the middle of，A factor indicating a rate of change of the determining function; />Is an adaptive saturation function which is continuously and smoothly at [ -1,1]And the value is converted, so that the phenomenon of abrupt value change is avoided.
Further correcting the estimated error value of the longitudinal force of the wheel, and finally obtaining the sliding mode surface as follows:
when the state error tends to stabilize, there isThe method comprises the steps of carrying out a first treatment on the surface of the Thus, an accurate estimate of the wheel longitudinal force is obtained as follows:
thirdly, constructing a square root volume Kalman filter based on the nonlinear three-degree-of-freedom dynamics model in the first step and the longitudinal force of the wheel observed and estimated in the second step, butting the square root volume Kalman filter with a robot dynamics system, determining the to-be-estimated quantity, the input quantity and the observed quantity of the square root volume Kalman filter, and correcting and updating parameters to be estimated of the square root volume Kalman filter by adopting square root criteria.
The discrete state equation and the observation equation of the square root volume Kalman filter of the constructed nonlinear system are shown as follows:
wherein the state variablesThe method comprises the steps of carrying out a first treatment on the surface of the Measurement variable->The method comprises the steps of carrying out a first treatment on the surface of the Input variable->The method comprises the steps of carrying out a first treatment on the surface of the Wherein the input wheel longitudinal force is obtained through estimation in the second step; />Is state process noise; />For measuring process noise; />Is->A time state vector; />Is a state transfer function; />For measuring transfer functions; upper corner mark->Representing a transpose operation on the indicated quantities.
Based on a wheel type robot dynamics model, a first-order Euler discrete method is adopted, and a state equation of the system is expressed as follows:
in the method, in the process of the invention,is the system sampling time.
The measurement equation of the system is designed as:
the square root volume kalman filter design process is as follows:
1) Setting initial values of state and covariance matrix square root form:
wherein,is the initial value of the state; />Square root factors that are state covariance matrices; />Is a state covariance matrix.
2) Calculating the current state volume point:
wherein,for corresponding->A state volume point at time; />For corresponding->Square root of state covariance matrix of time; />Is->Time of day state estimator；/>Is a set of volumetric points.
3) Propagation of volume points:
wherein,is the volume point after propagation; />For the corresponding state transfer function +.>Is a natural number greater than 0.
4) Estimating the square root of the state prediction mean and covariance matrix:
in the method, in the process of the invention,representing a trigonometric decomposition of the matrix; />Representing a process noise covariance matrix->Square root of (2);is a propagation process matrix; />The weight coefficient of the corresponding moment; />Is according to->Time estimated +.>Time state quantity; />Is according to->Time of day prediction +.>The square root of the time state covariance matrix. />
5) Calculating an update state volume point:
wherein,to update the state volume point of the phase.
6) The volume point propagates through the measurement equation:
wherein,to update the phase propagated volume vector; />Measuring an equation transfer function for the update stage; />Is->Time input quantity.
7) Estimating the square root of the measurement prediction mean and covariance matrix:
in the method, in the process of the invention,representing a process noise covariance matrix->Square root of (2); matrix->Is a process matrix; />Estimating a value for the measured mean value; />To update the square root of the phase covariance matrix.
8) Calculating a cross covariance matrix:
wherein,is a cross covariance matrix; />Is a process matrix.
9) Calculating Kalman gain:
wherein,is the kalman gain.
10 Updating the state estimator:
wherein,is the final state estimate.
11 Square root of the estimated update error variance):
wherein,is the square root of the final error variance.
After the step, the wheeled mobile robot is obtainedIs used for the estimation of the estimated value of (a).
And step four, based on the square root volume Kalman filter constructed in the step three, adopting a dung beetle optimization algorithm to perform optimization processing on non-Gaussian noise of the square root volume Kalman filter, enhancing the robustness of the filter, and finally realizing accurate estimation of the running state parameters of the wheeled robot, wherein a specific technical roadmap is shown in figure 3.
The dung beetle optimization algorithm is used for optimizing a noise covariance matrix in the filter algorithm. The matrix and the elements on the matrix are set as the algorithm optimizing dimension as shown in the following formula:
/>
in the method, in the process of the invention,state noise values respectively representing longitudinal vehicle speed, lateral vehicle speed and yaw rate; />Process noise values respectively representing longitudinal acceleration and lateral acceleration; />Representing a diagonal matrix.
Taking the root mean square error between the estimated value of the filter algorithm constructed in the third step and the measured value of the sensor as an optimization algorithm objective function, wherein the fitness function is as follows:
in the method, in the process of the invention,sampling time; />Is->Real measured variables of time; />Is->The time of day is an estimate output by the filter.
After the designed wheeled robot motion state estimation method based on the improved Kalman filtering is applied, a comparison graph of the lateral vehicle speed estimation of the proposed state estimation method and the traditional volume Kalman filtering algorithm is given as shown in fig. 4, and as can be seen from the graph, the estimation method designed by the method can more accurately and effectively estimate the robot motion state, and the estimation accuracy of the method is improved by about 10% compared with that of the traditional volume Kalman filtering algorithm, so that the effectiveness of the wheeled robot motion state estimation method based on the improved Kalman filtering is verified.
While preferred embodiments of the present invention have been described, additional variations and modifications in those embodiments may occur to those skilled in the art once they learn of the basic inventive concepts. It is therefore intended that the following claims be interpreted as including the preferred embodiments and all such alterations and modifications as fall within the scope of the invention.
It will be apparent to those skilled in the art that various modifications and variations can be made to the present invention without departing from the spirit or scope of the invention. Thus, it is intended that the present invention also include such modifications and alterations insofar as they come within the scope of the appended claims or the equivalents thereof.

Claims (1)

1. The wheeled robot traveling state estimation method based on the improved Kalman filtering is characterized by comprising the following steps of:
step one, constructing a longitudinal, transverse and yaw nonlinear three-degree-of-freedom dynamic model and a wheel model according to the dynamic characteristics of a wheeled robot;
step two, defining a wheel longitudinal force observation sliding mode surface according to the wheel dynamics characteristics, constructing a self-adaptive sliding mode observer to estimate the wheel longitudinal force, and realizing the wheel longitudinal force stable observation estimation of the robot;
thirdly, constructing a square root volume Kalman filter based on the nonlinear three-degree-of-freedom dynamics model in the first step and the longitudinal wheel force observed and estimated in the second step, butting the square root volume Kalman filter with a robot dynamics system, determining the to-be-estimated quantity, the input quantity and the observed quantity of the square root volume Kalman filter, and correcting and updating parameters to be estimated of the square root volume Kalman filter by adopting a square root criterion;
step four, based on the square root volume Kalman filter constructed in the step three, adopting a dung beetle optimization algorithm to perform optimization processing on non-Gaussian noise of the square root volume Kalman filter, and realizing accurate estimation of the travelling state parameters of the wheeled robot;
the nonlinear three-degree-of-freedom dynamics model established in the first step is shown in the following formula:
，
，
，
in the method, in the process of the invention,is yaw rate; />In the form of a derivative of yaw rate; />Is the front wheel corner; />Is the total mass of the robot; />For robot centroid around->The moment of inertia of the shaft; />And->Longitudinal force and transverse force, respectively +.>Representing the left front wheel>Representing the right front wheel>Representing the left rear wheel>Representing the right rear wheel; />Representing the front track,>representing the rear track; the distances between the mass center and the front and rear axes are +.>And->；/>Is the longitudinal acceleration; />Is the lateral acceleration;
the wheel model established in the first step is as follows:
since wheel side force is related to wheel slip angle and vertical load, the wheel slip angle and vertical load for each wheel are expressed as:
，
，
，
，
，
，
，
，
，
in the method, in the process of the invention,and->The longitudinal speed and the transverse speed of the wheeled robot are respectively; />Is a vertical load; />Is the wheel slip angle;is the centroid slip angle; />Is the height of the centroid to the ground;
the transverse force of the wheel is calculated according to a semi-empirical magic tire formula, and the wheel transverse force model is as follows:
，
in the method, in the process of the invention,representing a wheel; />Is a rigidity factor; />Is a curve shape factor; />Is the peak factor; />Is a curve curvature factor;
in the second step, according to the dynamic characteristics of the wheel, the defined longitudinal force observation sliding die surface of the wheel is；/>Is a sliding mode surface function; />Is the rotational angular velocity of the wheel; />The expression of the estimated value is:
，
in the method, in the process of the invention,the moment of inertia of the wheels; />The effective rotation radius of the wheel; />Driving moment for the wheels; />Estimating a longitudinal force of the wheel; />Is a sliding mode gain coefficient; />Is an observer gain factor; />Is a sign function; />Is a differential form of the wheel angular velocity estimate;
to be used forAs Lyapunov function candidates, the stability conditions of the adaptive sliding mode observer were analyzed:
，
in the method, in the process of the invention,is a Lyapunov judgment function; />In the differential form of a Lyapunov decision function; />A differential form of a sliding mode surface function;
due toIs bounded, so there is a positive value +.>Such that:
，
in the method, in the process of the invention,is a positive constant term;
further written as:
，
according to the stability conditions of the adaptive sliding-mode observer, i.e.Such that:
，
the method comprises the following steps:
，
to reduce the estimation result jitter, an adaptive saturation function is selected as follows:
，
in the method, in the process of the invention,a factor indicating a rate of change of the determining function; />Is an adaptive saturation function which is continuously and smoothly at [ -1,1]The value is converted, so that the phenomenon of abrupt value change is avoided;
further correcting the estimated error value of the longitudinal force of the wheel, and finally obtaining the sliding mode surface as follows:
，
when the state error tends to stabilize, there isThus, an accurate estimate of the wheel longitudinal force is obtained as follows:
；
in the third step, the discrete state equation and the observation equation of the square root volume Kalman filter of the constructed nonlinear system are shown as follows:
，
wherein the state variablesThe method comprises the steps of carrying out a first treatment on the surface of the Measurement variable->The method comprises the steps of carrying out a first treatment on the surface of the Input variable->The method comprises the steps of carrying out a first treatment on the surface of the Wherein the input wheel longitudinal force is obtained through estimation in the second step; />Is state process noise; />For measuring process noise; />Is->A time state vector; />Is a state transfer function; />For measuring transfer functions; upper corner mark->Representing a transpose operation on the indicated quantity;
based on a wheel type robot dynamics model, a first-order Euler discrete method is adopted, and a state equation of the system is expressed as follows:
，
in the method, in the process of the invention,sampling time for the system;
the measurement equation of the system is designed as:
；
in the third step, the square root volume Kalman filter design process is as follows:
1) Setting initial values of state and covariance matrix square root form:
，
wherein,is the initial value of the state; />Square root factors that are state covariance matrices; />Is a state covariance matrix;
2) Calculating the current state volume point:
，
wherein,for corresponding->A state volume point at time; />For corresponding->Square root of state covariance matrix of time; />Is->Time state estimator,/-, for>Is a volume point set;
3) Propagation of volume points:
，
wherein,is the volume point after propagation; />Is a corresponding state transfer function; />A natural number greater than 0;
4) Estimating the square root of the state prediction mean and covariance matrix:
，
in the method, in the process of the invention,representing a trigonometric decomposition of the matrix; />Representing a process noise covariance matrix->Square root of (2); />Is a propagation process matrix; />The weight coefficient of the corresponding moment; />Is according to->Time estimated +.>Time state quantity; />Is according to->Time of day prediction +.>Square root of moment state covariance matrix;
5) Calculating an update state volume point:
，
wherein,status volume points for the update phase;
6) The volume point propagates through the measurement equation:
，
wherein,to update the phase propagated volume vector; />Measuring an equation transfer function for the update stage;is->Time input quantity;
7) Estimating the square root of the measurement prediction mean and covariance matrix:
，
in the method, in the process of the invention,representing a process noise covariance matrix->Square root of (2); matrix->For the process matrix->Estimating a value for the measured mean value; />For updating the square root of the phase covariance matrix;
8) Calculating a cross covariance matrix:
，
wherein,is a cross covariance matrix>Is a process matrix;
9) Calculating Kalman gain:
，
wherein,is Kalman gain;
10 Updating the state estimator:
，
wherein,the final state estimation value;
11 Square root of the estimated update error variance):
，
wherein,is the square root of the final error variance;
then get the wheeled mobile robotIs a function of the estimated value of (2);
in the fourth step, the dung beetle optimization algorithm is used for optimizing a noise covariance matrix in the filter algorithm, and the matrix and elements on the matrix are set as algorithm optimizing dimensions, as shown in the following formula:
，
，
，
in the method, in the process of the invention,state noise values respectively representing longitudinal vehicle speed, lateral vehicle speed and yaw rate; />Process noise values respectively representing longitudinal acceleration and lateral acceleration; />Representing a diagonal matrix;
taking the root mean square error between the estimated value of the filter algorithm constructed in the third step and the measured value of the sensor as an optimization algorithm objective function, wherein the fitness function is as follows:
，
in the method, in the process of the invention,sampling time; />Is->Real measured variables of time; />Is->The time of day is an estimate output by the filter.
CN202311694201.1A 2023-12-12 2023-12-12 Wheeled robot traveling state estimation method based on improved Kalman filtering Active CN117406607B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202311694201.1A CN117406607B (en) 2023-12-12 2023-12-12 Wheeled robot traveling state estimation method based on improved Kalman filtering

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202311694201.1A CN117406607B (en) 2023-12-12 2023-12-12 Wheeled robot traveling state estimation method based on improved Kalman filtering

Publications (2)

Publication Number Publication Date
CN117406607A CN117406607A (en) 2024-01-16
CN117406607B true CN117406607B (en) 2024-03-19

Family

ID=89494709

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202311694201.1A Active CN117406607B (en) 2023-12-12 2023-12-12 Wheeled robot traveling state estimation method based on improved Kalman filtering

Country Status (1)

Country Link
CN (1) CN117406607B (en)

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR20160110773A (en) * 2015-03-12 2016-09-22 숙명여자대학교산학협력단 Method and apparatus for tracking moving object by using kalman filter
CN108981691A (en) * 2018-06-08 2018-12-11 北京航空航天大学 A kind of sky polarised light integrated navigation filters online and smoothing method
CN110095635A (en) * 2019-05-08 2019-08-06 吉林大学 A kind of longitudinal vehicle speed estimation method of all-wheel drive vehicles
CN116552548A (en) * 2023-07-06 2023-08-08 华东交通大学 Four-wheel distributed electric drive automobile state estimation method
CN116680873A (en) * 2023-05-12 2023-09-01 吉林大学 Vehicle state estimation method based on improved self-adaptive extended Kalman filtering
CN116923428A (en) * 2023-09-07 2023-10-24 华东交通大学 Combined estimation method for electric automobile centroid side deflection angle and tire side force
CN116991076A (en) * 2023-09-26 2023-11-03 江西省汉达隆科技有限公司 Wheeled robot steering control method based on state estimation information input

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR20160110773A (en) * 2015-03-12 2016-09-22 숙명여자대학교산학협력단 Method and apparatus for tracking moving object by using kalman filter
CN108981691A (en) * 2018-06-08 2018-12-11 北京航空航天大学 A kind of sky polarised light integrated navigation filters online and smoothing method
CN110095635A (en) * 2019-05-08 2019-08-06 吉林大学 A kind of longitudinal vehicle speed estimation method of all-wheel drive vehicles
CN116680873A (en) * 2023-05-12 2023-09-01 吉林大学 Vehicle state estimation method based on improved self-adaptive extended Kalman filtering
CN116552548A (en) * 2023-07-06 2023-08-08 华东交通大学 Four-wheel distributed electric drive automobile state estimation method
CN116923428A (en) * 2023-09-07 2023-10-24 华东交通大学 Combined estimation method for electric automobile centroid side deflection angle and tire side force
CN116991076A (en) * 2023-09-26 2023-11-03 江西省汉达隆科技有限公司 Wheeled robot steering control method based on state estimation information input

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
电力巡检机器人状态估计方法研究;姚贺明;《万方学位论文》;20230829;第1-60页 *

Also Published As

Publication number Publication date
CN117406607A (en) 2024-01-16

Similar Documents

Publication Publication Date Title
US20210240192A1 (en) Estimation method and estimator for sideslip angle of straight-line navigation of agricultural machinery
CN116992697B (en) Intelligent electric vehicle running state information estimation method
CN107016157B (en) Pavement self-adaptive longitudinal speed estimation system and method for distributed driving electric automobile
CN116923428B (en) Combined estimation method for electric automobile centroid side deflection angle and tire side force
CN116991076B (en) Wheeled robot steering control method based on state estimation information input
CN116552548B (en) Four-wheel distributed electric drive automobile state estimation method
CN110588657B (en) Joint estimation method for vehicle motion state and road gradient
CN110884499B (en) Method and system for determining vehicle mass center slip angle
CN112298354B (en) State estimation method for steering wheel and front wheel corner of steering system of unmanned automobile
CN113830088B (en) Intelligent semi-trailer tractor trajectory tracking prediction control method and vehicle
CN117272525A (en) Intelligent electric automobile road adhesion coefficient estimation method
CN107985318A (en) The wheel loader logitudinal centre of gravity and height of C.G. method for dynamic estimation
CN112643670A (en) Flexible joint control method based on sliding-mode observer
CN111796522A (en) Vehicle state estimation method
CN117406607B (en) Wheeled robot traveling state estimation method based on improved Kalman filtering
CN106671985A (en) Electric vehicle dynamics system modeling method
CN112287289A (en) Vehicle nonlinear state fusion estimation method for cloud control intelligent chassis
CN115946707B (en) Method and system for estimating tire force of all-line-control electric automobile driven by four-wheel hub motor
CN108413923B (en) Vehicle roll angle and pitch angle estimation method based on robust hybrid filtering
CN116279676A (en) Train speed control method based on fractional order sliding mode and Kalman filtering
CN116409327A (en) Road surface adhesion coefficient estimation method considering transient characteristics of tire under lateral working condition
US20040153216A1 (en) Method for estimating a vehicle's velocity
CN114879700A (en) Relative motion parameter estimation method for intelligent vehicle formation driving evaluation
CN114435371A (en) Road slope estimation method and device
CN114590264A (en) Pavement adhesion coefficient estimation method based on deep integration network adaptive Kalman filtering

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