Control methods, devices, electronic equipment and storage media for robot chassis

Abstract

This disclosure provides a control method, apparatus, electronic device, and storage medium for a robot chassis, relating to the field of robot control technology. The method includes: acquiring measurement information of the robot chassis at the current moment from multiple sensors; predicting multiple motion state items and at least one deviation item of a measurement item at the current moment based on state estimation information from the previous moment; updating the measurement information according to the predicted value of the deviation item, and updating the predicted values ​​of the multiple motion state items based on the updated measurement information to obtain state estimation information at the current moment; determining a control quantity based on the state estimation information, and controlling the robot chassis based on the control quantity. This disclosure reduces the impact of measurement deviation on the state estimation results by estimating and compensating for sensor measurement deviations during the state estimation process, thereby improving the accuracy and stability of robot chassis state estimation and motion control.

Description

Technical Field

[0001] This disclosure relates to the field of robot control technology, and more specifically, to a control method, apparatus, electronic device, and storage medium for a robot chassis. Background Technology

[0002] With the development of robotics technology, mobile robots are widely used in service, logistics, inspection, and collaboration scenarios. These applications typically rely on real-time estimation of the robot chassis state, such as estimating the robot chassis's position, speed, and other state information, to support functions such as navigation, motion control, and obstacle avoidance.

[0003] In existing schemes, the state of the robot chassis can typically be recursively estimated based on its motion states at different times, using state transition equations and observation equations. However, the state estimation results obtained by existing schemes have significant errors. Therefore, further improvements are needed to enhance the accuracy of robot chassis state estimation. Summary of the Invention

[0004] This disclosure provides a control method, apparatus, electronic device, computer-readable storage medium, and computer program product for a robot chassis, which can solve the problem of low accuracy in state estimation in existing technologies. The technical solution provided by this disclosure is as follows: According to one aspect of the present disclosure, a control method for a robot chassis is provided. The method is applied to the robot chassis, which is equipped with a controller and multiple sensors. The method is executed by the controller and includes: The robot chassis is acquired from the multiple sensors at the current moment, and the measurement information includes the measurement values ​​of multiple measurement items. Based on the state estimation information of the robot chassis at the previous moment, the state of the robot chassis at the current moment is predicted to obtain the prediction result at the current moment; wherein, the state estimation information includes the estimated values ​​of multiple motion state items and the estimated value of the deviation item of at least one of the multiple measurement items, and the prediction result includes the predicted values ​​of the multiple motion state items and the predicted value of the deviation item of the at least one measurement item. Based on the predicted value of the deviation term of at least one measurement item in the prediction result, the measurement information at the current moment is updated to obtain the updated measurement information; based on the updated measurement information, the predicted values ​​of multiple motion state items in the prediction result are updated; based on the updated predicted values ​​of multiple motion state items and the predicted value of the deviation term of at least one measurement item in the prediction result, the state estimation information of the robot chassis at the current moment is obtained. Based on the estimated values ​​of the multiple motion state items in the current state estimation information of the robot chassis, the control quantity used to control the robot chassis at the current moment is obtained, and the robot chassis is controlled based on the control quantity at the current moment.

[0005] According to another aspect of the present disclosure, a control device for a robot chassis is provided. The device is applied to a controller of the robot chassis, the robot chassis being provided with a controller and a plurality of sensors. The device includes: The acquisition module is used to acquire the measurement information of the robot chassis collected by the multiple sensors at the current moment, and the measurement information includes the measurement values ​​of multiple measurement items; The state estimation module is used to predict the current state of the robot chassis based on the state estimation information of the previous moment, and obtain the prediction result for the current moment; wherein, the state estimation information includes the estimated values ​​of multiple motion state items and the estimated value of the deviation item of at least one of the multiple measurement items, and the prediction result includes the predicted values ​​of the multiple motion state items and the predicted value of the deviation item of the at least one measurement item. The measurement update module is used to update the measurement information at the current moment based on the predicted value of the deviation term of at least one measurement item in the prediction result, to obtain updated measurement information; update the predicted values ​​of multiple motion state items in the prediction result based on the updated measurement information; and obtain the state estimation information of the robot chassis at the current moment based on the updated predicted values ​​of multiple motion state items and the predicted value of the deviation term of at least one measurement item in the prediction result. The control module is used to obtain the control quantity for controlling the robot chassis at the current moment based on the estimated values ​​of the multiple motion state items in the current moment state estimation information of the robot chassis, and to control the robot chassis based on the control quantity at the current moment.

[0006] Optionally, when the state estimation module predicts the current state of the robot chassis based on the state estimation information of the robot chassis at the previous moment, it is specifically used for: Obtain the control quantity used to control the robot chassis in the previous moment; Based on the state estimation information of the robot chassis at the previous moment and the control quantity used to control the robot chassis at the previous moment, the state of the robot chassis at the current moment is predicted.

[0007] Optionally, when the state estimation module predicts the state of the robot chassis at the current moment based on the state estimation information of the robot chassis at the previous moment and the control quantity used to control the robot chassis at the previous moment, it specifically performs the following: Based on the state estimation information of the robot chassis at the previous moment and the control quantity used to control the robot chassis at the previous moment, the state of the robot chassis at the current moment is predicted by a preset state transition equation. The state transition equation describes the correspondence between the state estimation information of the robot chassis at the previous moment and the control quantity used to control the robot chassis at the previous moment, and the state information of the robot chassis at the current moment.

[0008] Optionally, the control quantities used to control the robot chassis include: the expected values ​​of the linear velocities of the robot chassis in the horizontal and vertical directions, and the expected values ​​of the angular velocities of the robot chassis. When the state estimation module predicts the current state of the robot chassis based on the state estimation information of the robot chassis at the previous moment and the control variables used to control the robot chassis at the previous moment, it uses a preset state transition equation. Specifically, it is used to: Based on the expected values ​​of the linear velocities of the robot chassis in the horizontal and vertical directions in the control variables of the previous moment, and the estimated values ​​of the linear velocities of the robot chassis in the horizontal and vertical directions in the state estimation information of the previous moment, the estimated values ​​of the linear acceleration of the robot chassis in the horizontal and vertical directions of the previous moment are determined. Based on the expected value of the angular velocity of the robot chassis in the control quantity of the previous moment and the estimated value of the angular velocity of the robot chassis in the state estimation information of the previous moment, the estimated value of the angular acceleration of the robot chassis in the previous moment is determined. Based on the state estimation information of the robot chassis at the previous moment, the estimated values ​​of the linear acceleration of the robot chassis in the horizontal and vertical directions at the previous moment, and the estimated value of the angular acceleration of the robot chassis at the previous moment, the state of the robot chassis at the current moment is predicted.

[0009] As an optional embodiment, the multiple sensors include an encoder and an inertial measurement unit (IMU). The encoder measures the linear velocity of the robot chassis in the horizontal and vertical directions, and the IMU measures the angular velocity of the robot chassis. The deviation of the at least one measurement includes the deviation of the encoder's measurement and the deviation of the IMU's measurement. The plurality of motion state items include The deviation term of the at least one measurement item includes ; in, These refer to the horizontal and vertical positions of the robot chassis, respectively. The heading angle of the robot chassis. Let be the linear velocities of the robot chassis in the horizontal and vertical directions. Let be the angular velocity of the robot chassis. The deviation term of the encoder's measurement items. This refers to the deviation term of the measurement items of the IMU.

[0010] Optionally, the state transition equation is:

[0011] in, The time difference between the current moment and the previous moment. and According to The control quantity obtained at time t The linear accelerations of the robot chassis in the horizontal and vertical directions at that moment. According to The control quantity obtained at time t The angular acceleration of the robot chassis at the specified time. For the state estimation information at time k-1, and The prediction result at time k is... and The deviation terms of the encoder measurements at time k-1 are respectively. random perturbation value, The deviation term of the IMU's measured values The random perturbation value.

[0012] Optionally, when the control module obtains the control quantity for controlling the robot chassis at the current moment based on the estimated values ​​of the plurality of motion state items in the current moment's state estimation information of the robot chassis, it specifically uses: Obtain the expected state information at the current moment, wherein the expected state information includes the expected values ​​of the plurality of motion state items; Based on the estimated values ​​of the multiple motion state items in the current state estimation information of the robot chassis and the current desired state information, the control quantity used to control the robot chassis at the current moment is obtained through the Active Disturbance Rejection Control (ADRC) algorithm.

[0013] Optionally, when the measurement update module updates the predicted values ​​of multiple motion state items in the prediction result based on the updated measurement information, it is specifically used for: For each measurement value in the updated measurement information, based on the difference between the measurement value and the predicted value of the corresponding motion state item in the prediction result at the current time, the predicted value of the corresponding motion state item in the prediction result at the current time is updated to obtain the estimated value of the corresponding motion state item at the current time.

[0014] As an optional embodiment, each sensor is preset with its own noise value for the corresponding measurement item; When the measurement update module updates the predicted value of the corresponding motion state item in the prediction result at the current time based on the difference between the measured value and the predicted value of the corresponding motion state item in the prediction result at the current time, it is specifically used for: Based on the difference between the measured value and the predicted value of the corresponding motion state item in the prediction result at the current time, and the noise value corresponding to the measured value, the predicted value of the corresponding motion state item in the prediction result at the current time is updated. The state estimation module is also used for: The validity of the measured values ​​of each sensor is judged to obtain a validity judgment result, which is either valid or invalid. For each sensor, if the validity judgment result of the measurement item collected by the sensor is determined to be invalid, the noise value corresponding to the sensor is increased by a preset factor.

[0015] Optionally, the plurality of sensors further includes: ultra-wideband (UWB) and a vision sensor, wherein, The visual sensor measures the position of the robot chassis in the horizontal and vertical directions, as well as the heading angle of the robot chassis. The UWB measurement items are the horizontal and vertical positions of the robot chassis; Each sensor has its own priority, and each sensor is configured with a corresponding acquisition frequency based on its priority. Each sensor acquires the measurement value of its corresponding measurement item at its corresponding acquisition frequency.

[0016] According to another aspect of the present disclosure, a controller for a robot chassis is provided, the robot chassis being provided with a plurality of sensors and the controller, the controller being configured to execute the control method for the robot chassis described above.

[0017] According to another aspect of the present disclosure, a computer-readable storage medium is provided having a computer program stored thereon, which, when executed by a processor, implements the above-described robot chassis control method.

[0018] According to another aspect of the present disclosure, a computer program product is provided, including a computer program that, when executed by a processor, implements the above-described robot chassis control method.

[0019] The beneficial effects of the technical solutions provided in this disclosure are: This embodiment of the disclosure acquires measurement information of the robot chassis collected by multiple sensors at the current moment, thereby obtaining measurement data related to the current motion state of the robot chassis and providing a data foundation for state estimation of the robot chassis. By predicting the deviation of multiple motion state items and at least one measurement item at the current moment based on the state estimation information from the previous moment, it achieves synchronous prediction of the current motion state of the robot chassis and sensor measurement deviations. By updating the measurement information at the current moment based on the predicted value of the deviation item, it compensates for the measurement deviation in the sensor measurement information, reducing the impact of measurement deviation on state estimation. Furthermore, by updating the measurement information based on the updated measurement information, it achieves the following: The predicted values ​​of motion state terms are updated, enabling the correction of the predicted motion state using measurement information after deviation compensation, thus improving the accuracy of robot chassis state estimation. By using the updated predicted values ​​of multiple motion state terms and the predicted values ​​of deviation terms as the state estimation information at the current moment, recursive estimation of motion state and measurement deviation is achieved, providing a basis for state prediction and deviation compensation at subsequent moments. By determining the control quantity based on the estimated values ​​of multiple motion state terms at the current moment and controlling the robot chassis based on this control quantity, control of the robot chassis based on the state estimation results after deviation compensation is achieved, improving the accuracy and stability of robot chassis motion control. Attached Figure Description

[0020] To more clearly illustrate the technical solutions in the embodiments of this disclosure, the accompanying drawings used in the description of the embodiments of this disclosure will be briefly introduced below.

[0021] Figure 1 A schematic diagram of the system architecture to which the control method for the robot chassis provided in the embodiments of this disclosure applies; Figure 2 A flowchart illustrating a control method for a robot chassis provided in an embodiment of this disclosure; Figure 3 This is a schematic diagram of a process for determining a corrected measurement value, provided in an embodiment of the present disclosure. Figure 4This is a schematic flowchart illustrating the determination of a control quantity according to an embodiment of the present disclosure; Figure 5 A flowchart illustrating a control method for a robot chassis provided in an embodiment of this disclosure; Figure 6 This is a schematic diagram of the structure of a control device for a robot chassis provided in an embodiment of the present disclosure; Figure 7 This is a schematic diagram of the structure of an electronic device provided in an embodiment of this disclosure. Detailed Implementation

[0022] The embodiments of this disclosure are described below with reference to the accompanying drawings. It should be understood that the embodiments described below with reference to the accompanying drawings are exemplary descriptions for explaining the technical solutions of the embodiments of this disclosure, and do not constitute a limitation on the technical solutions of the embodiments of this disclosure.

[0023] Those skilled in the art will understand that, unless otherwise stated, the singular forms “a,” “an,” “the,” and “the” used herein may also include the plural forms. It should be further understood that the terms “comprising” and “including” as used in embodiments of this disclosure mean that the corresponding feature can be implemented as the presented feature, information, data, step, operation, element, and / or component, but do not exclude implementation as other features, information, data, steps, operations, elements, components, and / or combinations thereof supported by the art.

[0024] It should be understood that when we say that a component is "connected" or "coupled" to another component, the component can be directly connected or coupled to the other component, or it can mean that the component and the other component establish a connection relationship through an intermediate component. Furthermore, the term "connected" or "coupled" as used herein can include wireless connections or wireless coupling. The term "and / or" as used herein indicates at least one of the items defined by the term; for example, "A and / or B" or "A, B" indicates implementation as "A", or implementation as "B", or implementation as "A and B".

[0025] In this disclosure, the terms "module" or "unit" refer to a computer program or part of a computer program that has a predetermined function and works with other related parts to achieve a predetermined goal, and can be implemented wholly or partially using software, hardware (such as processing circuitry or memory), or a combination thereof. Similarly, a processor (or multiple processors or memory) can be used to implement one or more modules or units. Furthermore, each module or unit can be part of an overall module or unit that includes the functionality of that module or unit.

[0026] To make the objectives, technical solutions, and advantages of this disclosure clearer, the embodiments of this disclosure will be described in further detail below with reference to the accompanying drawings.

[0027] A chassis is a basic motion platform used to support and drive equipment. A chassis may include components such as a frame, drive mechanism, steering mechanism, and control actuators. The frame supports the equipment body and other components mounted on it; the drive mechanism provides driving force; the steering mechanism changes the direction of movement; and the control actuators respond to control commands and control the drive mechanism, steering mechanism, etc. Through the cooperation of these components, the chassis can achieve forward, backward, and turning movements, providing the motion basis for the equipment to perform corresponding tasks.

[0028] A robot chassis refers to the platform used in a robot. It serves as the robot's motion actuator, supporting the robot body, sensors, controllers, and other functional components. It drives the robot's movement based on control commands output by the controller. The motion state of the robot chassis can be characterized by multiple motion state terms, such as position, linear velocity, and angular velocity, representing the chassis's motion state at a given moment. During robot operation, sensors located on the robot chassis or other parts of the robot collect measurement information related to the chassis's motion state. Based on this measurement information, the robot chassis's motion state is estimated, and then controlled according to the estimated motion state.

[0029] An X-shaped chassis is a robot chassis that employs an X-shaped wheel arrangement. An X-shaped chassis can have multiple drive wheels arranged in a predetermined tilt direction, forming an overall X-shape. Different drive wheels can generate driving forces in their respective directions. By coordinating and controlling the movement direction and speed of multiple drive wheels, a combined driving force in different directions can be formed, thereby driving the robot chassis to perform corresponding movements. For example, an X-shaped chassis can achieve forward, backward, lateral, diagonal, and in-situ rotation movements. Compared to chassis that can only move longitudinally and change direction through a steering mechanism, the X-shaped chassis has a higher degree of freedom and flexibility. In some implementations, the X-shaped chassis can use Mecanum wheels, omnidirectional wheels, or other drive wheels capable of multi-directional movement.

[0030] A sensor is a detection device that can sense a physical quantity to be measured and convert it into measurement information that can be recognized and processed by a controller or other processing device. One or more sensors can be installed in the robot chassis to collect measurement information related to the robot chassis's motion state. Different sensors can be used to measure different physical quantities, and correspondingly, the measurement information collected by different sensors can include the measured values ​​of one or more measurement items. Measurement items can include, for example, position, linear velocity, angular velocity, linear acceleration, etc.

[0031] In this embodiment of the disclosure, the robot can be equipped with multiple sensors, which can be of the same type or different types. The sensors may include, for example, one or more of an inertial measurement unit (IMU), an encoder, a vision sensor, and an ultra-wideband (UWB) sensor.

[0032] The inertial measurement unit (IMU) is used to acquire the angular velocity of the robot chassis. Encoders, which can be installed on the drive wheels, motors, or other moving parts, are used to determine the linear velocity of the robot chassis. Vision sensors acquire images of the robot's surrounding environment and, combined with visual positioning and other processing methods, obtain the robot chassis's position and heading angle. UWB sensors can wirelessly interact with pre-set UWB devices via robot-side equipment to determine the robot chassis's position.

[0033] Since different types of sensors have different measurement principles and characteristics, multiple sensors can be used to obtain measurement information corresponding to different motion states of the robot chassis. The measurement information from multiple sensors can be fused by a state estimation method to obtain the state estimation information of the robot chassis.

[0034] A measurement item refers to information related to the motion state of the robot chassis, measured by sensors. For example, a measurement item can be position, linear velocity, angular velocity, or linear acceleration. One sensor can correspond to one measurement item or multiple measurement items.

[0035] Motion state terms refer to information used to characterize the motion state of the robot chassis. Motion state terms may include, for example, the robot chassis's position, linear velocity, and angular velocity. It should be noted that measurement terms and motion state terms can have a corresponding relationship, but they do not need to be completely identical. Measurement terms characterize the measurement results obtained directly or indirectly by sensors, while motion state terms constitute the state of the robot chassis to be estimated. For example, the position, linear velocity, and angular velocity of the robot chassis can be estimated using measurement information such as linear acceleration, angular velocity, and position collected by sensors.

[0036] A deviation term is a state term that characterizes the measurement deviation that occurs when a sensor measures a corresponding quantity. There may be a certain deviation between the measured value output by the sensor and the actual value of the corresponding physical quantity. This deviation may originate from factors such as the sensor's own characteristics, installation status, or operating environment, and may change as the robot chassis operates.

[0037] State estimation refers to the process of determining the motion state of a robot chassis at a given moment based on existing state information and measurement information collected by sensors. Since the measurement information collected by sensors may be affected by factors such as measurement noise and measurement bias, state estimation algorithms can be used to synthesize the state change patterns of the robot chassis and the sensor measurement information to obtain the estimated state information of the robot chassis.

[0038] A state transition equation is a mathematical model used to describe the state transition relationship between adjacent moments of a robot chassis. State transition equations can be established based on the kinematic model, dynamic model, or other models that can describe the state changes of the robot chassis.

[0039] An observation equation is a mathematical model that describes the correspondence between the state of a robot chassis and the measurement information from sensors. Through the observation equation, the predicted results obtained based on the state transition equation can be mapped to the sensor's measurement space, and compared with the actual measurement information collected by the sensors to correct the prediction results. Different types of sensors can correspond to different observation relationships, and measurement information from multiple sensors can also be used together to update the state of the robot chassis.

[0040] The Extended Kalman Filter (EKF) is a recursive state estimation method suitable for nonlinear systems. Based on the state transition and observation relationships of the nonlinear system, EKF predicts the state at the current moment, building upon the state estimation result from the previous moment. It then corrects the prediction result by incorporating measurement information acquired by sensors at the current moment, thus obtaining the state estimation result for the current moment.

[0041] Specifically, the EKF processing can include a prediction process and an update process. During the prediction process, the current state prediction result can be obtained based on the state estimation information from the previous time step and the corresponding state transition equation, and the uncertainty of the state estimation can be predicted. During the update process, the predicted state can be corrected based on the difference between the measurement information currently collected by the sensors and the predicted measurement information, thereby obtaining updated state estimation information. As the robot chassis moves, the above prediction and update processes can be repeated to achieve recursive estimation of the robot chassis state.

[0042] In this embodiment of the disclosure, multiple motion state terms of the robot chassis and the deviation term of at least one measurement term can be used together as the state to be estimated. Therefore, during the EKF prediction process, multiple motion state terms and corresponding deviation terms can be predicted; during the update process, the corresponding measurement information can be updated based on the predicted value of the deviation term, and the predicted value of the motion state term can be corrected using the updated measurement information, thereby obtaining the current state estimation information of the robot chassis.

[0043] Active Disturbance Rejection Control (ADRC) is a control method that estimates and compensates for disturbances during system operation to achieve system control. ADRC treats both internal uncertainties and external factors as disturbances and compensates accordingly based on the estimated disturbances, thereby reducing the impact of disturbances on the system control process.

[0044] ADRC can include components such as a tracking differentiator, an extended state observer (ESO), and error feedback control. The tracking differentiator tracks the target input and obtains the corresponding tracking signal; the extended state observer estimates the system state and the disturbances it experiences; and the error feedback control determines the appropriate control input based on the target state, the current state, and the disturbance estimation results.

[0045] The technical background of this disclosure can be described as follows: The X-type Mecanum wheel chassis needs to be precisely moved to a designated location in environments such as factories and warehouses. This task faces two core challenges: First, the chassis itself doesn't know its location. The chassis is equipped with various sensors: encoders measure wheel rotations, IMUs measure acceleration and rotation, cameras measure position, and UWB measures distance. However, each sensor has its limitations: encoders accumulate drift, IMUs become inaccurate over time, cameras update slowly and have delays, and UWB is noisy. Relying on any single sensor alone cannot provide reliable position information.

[0046] Second, the chassis cannot predict how the environment will interfere with it. Ground friction will change, wheels may slip, load may shift, and motor response will not be an ideal linear relationship. These disturbances cannot be predicted in advance and are difficult to describe precisely with mathematical models.

[0047] This disclosure uses two core technologies to solve these two problems respectively: EKF to solve the problem of "where am I", and ADRC to solve the problem of "how to accurately reach the target".

[0048] In existing robot chassis control processes, the control quantity used to control the robot chassis is generally determined by a corresponding controller based on the deviation between the target motion state and the current motion state. For example, a proportional-integral-derivative (PID) controller can be used to determine the corresponding control quantity based on the deviation between the target motion state and the current motion state. The control parameters of such controllers are usually preset. When the load, operating environment, or external disturbances of the robot chassis change, the preset control parameters have limited adaptability to different operating conditions. Especially for X-type chassis with multi-wheel cooperative motion characteristics, the motions of different drive wheels are interconnected. When the robot chassis performs movements such as turning and lateral movement, high accuracy and stability of motion control are required. Using fixed control parameters may be difficult to adapt to different operating states, thus affecting the dynamic response performance and motion stability of the robot chassis.

[0049] Furthermore, controlling the robot chassis typically requires acquiring its current motion state. Some solutions utilize sensors such as encoders and inertial measurement units (IMUs) to collect motion state information and determine the chassis's motion state based on this sensor data. However, different sensors may introduce varying degrees of measurement error. For example, encoder measurements may be affected by the motion of the drive wheels, and IMU measurements may contain biases. During continuous operation, these measurement errors can impact the accuracy of chassis state estimation.

[0050] In other approaches, sensors can be calibrated before use to reduce the deviation between sensor measurements and actual values. However, sensor measurement deviations are not constant and may change during robot chassis operation. Therefore, processing sensor measurement information solely based on pre-calibrated parameters is insufficient to continuously adapt to changes in sensor measurement deviations, potentially leading to discrepancies between the robot chassis's state estimation and its actual motion state. Since the robot chassis's control variables are typically determined based on the state estimation results, deviations in the state estimation can further impact the robot chassis's motion control performance.

[0051] To improve the robot chassis's adaptability to external disturbances, control methods such as ADRC can be employed. For example, the system state and disturbances can be estimated using ESO, and the robot chassis can be controlled based on the estimation results. However, when using the same observation process to simultaneously estimate the motion state and disturbances, the motion state estimation and disturbance estimation may interfere with each other. If there are deviations in sensor measurements or the robot chassis is subjected to significant disturbances, the state estimation results and control effectiveness may be affected.

[0052] Alternatively, some solutions employ Model Predictive Control (MPC) to control the robot chassis. MPC predicts the robot chassis's motion state over a future period based on its system model and determines the corresponding control variables by solving an optimization problem, thereby controlling the robot chassis's motion. However, MPC typically requires repeated state prediction and optimization solving during the control process. For robot chassis with multiple degrees of freedom and multiple drive wheels, the large number of state and control variables to be processed can increase the computational overhead of the control process. Furthermore, the control results of MPC are dependent on the system model used; discrepancies between the actual operating characteristics of the robot chassis and the system model can also affect the control effectiveness.

[0053] Therefore, in the motion control of the robot chassis, on the one hand, the measurement deviation of the sensors may affect the accuracy of the robot chassis state estimation; on the other hand, changes in the operating conditions of the robot chassis and the disturbances it experiences also place higher demands on the adaptability of the control method. How to improve the accuracy of robot chassis state estimation, and on this basis, improve the accuracy and stability of robot chassis motion control, still needs further improvement.

[0054] The robot chassis control method, apparatus, electronic equipment, computer-readable storage medium, and computer program product disclosed herein are intended to solve at least one of the above-mentioned technical problems of the prior art.

[0055] This disclosure achieves information complementarity between different sensors by fusing measurement information collected by multiple sensors through extended Kalman filtering, reduces the impact of measurement errors of a single sensor on the state estimation results, and improves the accuracy of robot chassis state estimation.

[0056] By incorporating sensor deviation terms into the state estimation process, the motion state of the robot chassis and sensor deviations are estimated synchronously. The estimated deviations are then used to compensate for the sensor measurement information, achieving online estimation and dynamic compensation of sensor deviations. This reduces the cumulative errors caused by factors such as sensor zero drift and measurement deviations, and improves the accuracy and stability of state estimation during long-term operation of the robot chassis.

[0057] By adjusting the corresponding measurement noise covariance according to the working state and measurement information of different sensors and the motion state of the robot chassis, and determining the influence of different sensors on the state estimation results based on the adjusted measurement noise covariance, the dynamic adjustment of the fusion weights of multiple sensors is realized, which improves the adaptability of the multi-sensor fusion process to sensor state changes and different motion conditions.

[0058] By employing extended Kalman filtering for multi-sensor state estimation and using an extended state observer in active disturbance rejection control to estimate the total disturbance during robot chassis operation, state estimation and disturbance estimation are performed separately, achieving synergy between state estimation and disturbance compensation. This reduces the impact of sensor measurement errors, model errors, and external disturbances on robot chassis control.

[0059] By using an extended state observer, the model error, external disturbances, and other factors that are difficult to model accurately during the operation of the robot chassis are uniformly estimated. Based on the estimated total disturbance, the control quantity is compensated, realizing online estimation and active compensation of the disturbances to the robot chassis. This improves the robot chassis's anti-disturbance capability and motion stability under conditions such as load changes, ground condition changes, and external disturbances.

[0060] By employing a nonlinear feedback function in the extended state observer and adjusting the corresponding feedback gain according to the error magnitude, the observation gain is adapted to different error states, improving the response capability to disturbances and state changes, and reducing the possibility of oscillations and overshoot during the control process.

[0061] By smoothing the target input of the robot chassis through a tracking differentiator, corresponding tracking signals and their differential signals are generated, achieving a smooth transition of the target input, reducing overshoot and oscillation caused by sudden changes in target input, and improving the smoothness and dynamic response performance of the robot chassis motion process.

[0062] By adopting an active disturbance rejection control method that does not require online solving of constrained optimization problems, and estimating and compensating for system dynamics and external disturbances that are difficult to model accurately as total disturbances, the computational overhead of the control process is reduced while the dependence on the accurate dynamic model of the robot chassis is decreased, thereby improving the real-time performance of the robot chassis control and its adaptability to model uncertainties.

[0063] Therefore, this disclosure achieves accurate estimation of the robot chassis motion state and timely compensation for disturbances during operation through multi-sensor fusion, online estimation and compensation of sensor bias, dynamic adjustment of fusion weights, coordination of state estimation and disturbance estimation, active compensation of total disturbance, and smoothing of target input. This improves the accuracy, stability, anti-disturbance capability, dynamic response performance, and real-time performance of robot chassis motion control.

[0064] The following description of several exemplary embodiments illustrates the technical solutions of this disclosure and the technical effects produced by these solutions. It should be noted that the following embodiments can be referenced, learned from, or combined with each other. Identical terms, similar features, and similar implementation steps in different embodiments will not be repeated.

[0065] Figure 1 This diagram illustrates the system architecture applicable to the robot chassis control method provided in this embodiment. The system includes a controller, a chassis, and electronic devices such as motors. The controller is a chassis controller. The controller executes EKF and ADRC algorithms to estimate the chassis state based on measurement information collected by sensors and generates corresponding control quantities based on the state estimation results. The motors provide driving force for the chassis movement. The chassis may include four motors, each connected to a corresponding wheel to drive the wheels to rotate, thereby achieving forward, backward, turning, lateral, or rotational movements of the chassis. The controller can output corresponding control quantities to the four motors to adjust the rotational speed or output torque of each wheel, thereby achieving chassis motion state control.

[0066] In this embodiment, the controller can acquire measurement information of the chassis collected by sensors at various times. Each measurement information includes the measurement values ​​of multiple measurement items. The controller can estimate the state of the chassis at the current time based on the EKF, the measurement information at the current time, and the state estimation information of the chassis at the previous time, to obtain the state estimation information of the chassis at the current time. In this embodiment, the state estimation information is designed to include the estimated values ​​of multiple state items and the estimated value of the error of the sensor corresponding to at least one of the multiple state items. The controller acquires the desired state information at the current time, obtains the control quantity at the current time based on the desired state information and the estimated values ​​of the state items in the current chassis state estimation information, and controls the chassis based on the control quantity at the current time.

[0067] This disclosure provides a control method for a robot chassis. The method is applied to a robot chassis on which sensors and a controller are mounted. The method is executed by the controller, such as... Figure 2 As shown, the method includes S201-S204.

[0068] S201. Obtain measurement information of the robot chassis at the current moment from multiple sensors.

[0069] This measurement information includes the measured values ​​of multiple measurement items.

[0070] In this embodiment of the disclosure, the robot chassis can be a device disposed at the bottom of the robot for supporting the robot and driving its movement. The robot chassis can perform forward, backward, turning, lateral movement, or rotation under the control of the controller.

[0071] Sensors are devices used to sense physical quantities related to the motion state of the robot chassis and convert these sensed physical quantities into measurement information that the controller can process. The robot chassis can be equipped with multiple sensors, each of which can collect measurement values ​​for one or more measurement items. A measurement item refers to a category of information related to the motion state of the robot chassis, and may include at least one of position, linear velocity, angular velocity, or heading angle.

[0072] The current time can be the time corresponding to the current state estimation period. The controller can receive measurement information collected by each sensor at the current time, or it can determine the measurement information corresponding to the current time based on the timestamp carried by the measurement information. The measurement items, acquisition frequencies, and arrival times of the measurement information of different sensors can be the same or different.

[0073] As an optional embodiment, the multiple sensors include an encoder and an inertial measurement unit (IMU), the encoder measuring the linear velocity of the robot chassis in the horizontal and vertical directions, and the IMU measuring the angular velocity of the robot chassis. The aforementioned sensors also include: ultra-wideband (UWB) and vision sensors, among which, The visual sensor measures the position of the robot chassis in the horizontal and vertical directions, as well as the heading angle of the robot chassis. The UWB measurement item is the position of the robot chassis in the horizontal and vertical directions; Each sensor has its own priority, and each sensor is configured with a corresponding acquisition frequency based on its priority. Each sensor acquires the measurement value of its corresponding measurement item at its corresponding acquisition frequency.

[0074] In this embodiment, the multiple sensors may include an encoder and an inertial measurement unit (IMU). The encoder may be mounted on the drive wheels, motor, or other moving parts of the robot chassis to detect the rotation of the corresponding moving parts. The controller can determine the linear velocity of the robot chassis in the horizontal and vertical directions based on the rotation information collected by the encoder and the kinematic relationship of the robot chassis. The IMU may include a gyroscope or other detection device to detect the angular velocity of the robot chassis rotating about a preset axis.

[0075] Optionally, the aforementioned sensors may further include ultra-wideband (UWB) sensors and vision sensors. The vision sensors can acquire images of the environment surrounding the robot chassis and determine the robot chassis's horizontal and vertical position, as well as its heading angle, through visual positioning processing. The UWB sensors can wirelessly interact with a pre-set UWB base station via UWB devices on the robot chassis side, determining the robot chassis's horizontal and vertical position based on information such as transmission time, time difference of arrival, or angle of arrival.

[0076] Different sensors can have different measurement characteristics. For example, encoders and IMUs can provide speed information of the robot chassis at a high acquisition frequency, vision sensors can provide pose information of the robot chassis, and UWB sensors can provide position reference information of the robot chassis. The controller can configure corresponding priorities for each sensor and configure corresponding acquisition frequencies according to the priorities of each sensor, so that each sensor can acquire the measurement values ​​of the corresponding measurement items at its own acquisition frequency.

[0077] S202. Based on the state estimation information of the robot chassis at the previous moment, predict the state of the robot chassis at the current moment to obtain the prediction result at the current moment.

[0078] The state estimation information includes estimated values ​​of multiple motion state items and estimated values ​​of deviation items of at least one of the multiple measurement items. The prediction result includes predicted values ​​of multiple motion state items and predicted values ​​of deviation items of at least one measurement item.

[0079] In this embodiment of the disclosure, the previous moment can be the moment corresponding to the previous state estimation cycle of the current state estimation cycle. The state estimation information of the previous moment is used to characterize the result of estimating the state of the robot chassis at the previous moment based on the measurement information obtained at and before the previous moment.

[0080] Motion state terms refer to information categories used to characterize the motion state of the robot chassis, which may include at least one of the robot chassis's position, heading angle, linear velocity, and angular velocity. The estimated value of each motion state term is used to characterize the estimated motion state of the robot chassis at the previous moment.

[0081] In this embodiment of the disclosure, the deviation term is used to characterize the measurement deviation generated when the sensor measures the corresponding measurement item. There may be a deviation between the measured value output by the sensor and the actual value of the corresponding physical quantity. This deviation may be caused by factors such as sensor zero drift, installation error, device characteristics, or changes in the operating environment.

[0082] In this embodiment of the disclosure, the measurement deviation of at least one measurement item can be used as the state to be estimated, so that the deviation item of the measurement item participates in the state estimation together with multiple motion state items of the robot chassis. For example, the deviation item may include the deviation item of the linear velocity measurement value or the deviation item of the angular velocity measurement value.

[0083] Specifically, the controller can extrapolate the estimated values ​​of each motion state term and at least one deviation term from the previous moment to the current moment based on the state change pattern of the robot chassis, thus obtaining the prediction result for the current moment. The predicted values ​​of the motion state terms in the prediction result are used to characterize the predicted result of the current motion state of the robot chassis without using the measurement information at the current moment for correction; the predicted values ​​of the deviation terms are used to characterize the measurement deviations that the corresponding sensors may have at the current moment.

[0084] In one implementation, a state transition equation for the robot chassis can be pre-established, and the state estimate information from the previous moment can be input into this equation to obtain the prediction result for the current moment. For motion state terms such as position, heading angle, linear velocity, and angular velocity, prediction can be made based on the kinematic relationships of the robot chassis. For deviation terms that change relatively slowly, prediction can be made based on a pre-defined deviation change model; for example, the deviation term can be modeled as a state that remains constant or changes slowly due to random disturbances. Thus, the prior state of the robot chassis at the current moment can be obtained before updating the state using the measurement information at the current moment.

[0085] As an optional embodiment, the deviation of the at least one measurement item mentioned above includes the deviation of the encoder's measurement item and the deviation of the IMU's measurement item; The above-mentioned multiple motion state items include The deviation term of at least one of the above measurement items includes ; in, These represent the horizontal and vertical positions of the robot's chassis, respectively. The heading angle of the robot chassis. Let be the linear velocities of the robot chassis in the horizontal and vertical directions. Let be the angular velocity of the robot chassis. This refers to the deviation term of the encoder's measurement items. This refers to the deviation term of the measurement items of the IMU.

[0086] In this embodiment of the disclosure, the robot chassis is in the first... State estimation information at time step It can be represented by the following state vector:

[0087] in, and These represent the horizontal and vertical positions of the robot chassis at time k, respectively. The heading angle of the robot chassis at time k represents the heading angle of the chassis at time k. and Let represent the linear velocities of the robot chassis at time k in the horizontal and vertical directions, respectively. This represents the angular velocity of the robot chassis at time k; and These represent the measurement deviations generated by the encoder when measuring the linear velocities in the horizontal and vertical directions at time k, respectively. This indicates the measurement deviation generated when the IMU measures the angular velocity of the robot chassis at time k, such as the zero bias of the angular velocity of the gyroscope; This indicates the matrix transpose.

[0088] Specifically, the encoder can obtain the linear velocity measurement value of the robot chassis based on the rotation of each drive wheel. Due to factors such as drive wheel diameter error, encoder installation error, or drive wheel slippage, the linear velocity measurement value obtained by the encoder may deviate from the actual linear velocity of the robot chassis. Therefore, it is possible to set... and , respectively characterizing the deviation in linear velocity measurement in the two directions.

[0089] The IMU (Integrated Measurement Unit) obtains the angular velocity measurement value of the robot chassis through the gyroscope. However, the angular velocity measurement value may be inaccurate due to factors such as gyroscope zero-point drift, temperature changes, or component aging. Therefore, settings can be configured... Characterizes the angular velocity measurement bias of the IMU.

[0090] In this embodiment of the disclosure, by incorporating the aforementioned deviation terms along with motion state terms such as position, heading angle, linear velocity, and angular velocity into the state estimation process, the measurement deviations of the encoder and IMU can be estimated online while estimating the motion state of the robot chassis, providing a basis for subsequently updating the corresponding measurement information using the deviation terms.

[0091] As an optional embodiment, the above-mentioned prediction of the current state of the robot chassis based on the state estimation information of the robot chassis at the previous moment includes: Obtain the control parameters used to control the robot chassis in the previous moment; Based on the state estimation information of the robot chassis at the previous moment and the control variables used to control the robot chassis at the previous moment, the state of the robot chassis at the current moment is predicted.

[0092] In this embodiment of the disclosure, the control quantity refers to the control command determined by the controller at the previous moment for controlling the movement of the robot chassis. The control quantity can be used to instruct the robot chassis to move according to the corresponding motion state, and may include at least one of the following: the desired linear velocity of the robot chassis, the desired angular velocity, the target rotational speed of the drive wheels, or the motor drive signal.

[0093] Specifically, the state estimation information of the robot chassis at the previous moment is used to characterize the motion state of the robot chassis at the previous moment and the measurement deviation of the corresponding sensors; the control quantity at the previous moment is used to characterize the control action applied to the robot chassis from the previous moment to the current moment. The controller can combine the above state estimation information and control quantity to determine the changes in the state of the robot chassis between adjacent moments, and extrapolate the state estimation result of the previous moment to the current moment to obtain the prediction result of the current moment.

[0094] For example, the controller can predict the current position and heading angle based on the estimated linear velocity and angular velocity from the previous moment, as well as the time difference between adjacent moments. It can also determine the linear and angular acceleration of the robot chassis based on the difference between the desired motion state indicated by the control input at the previous moment and the motion state at the previous moment, thereby predicting the current linear and angular velocity. For sensor deviations, the estimated value of the deviation from the previous moment can be extrapolated to the current moment based on a preset deviation variation pattern.

[0095] In this embodiment of the disclosure, by simultaneously considering the state estimation information and control quantity of the previous moment when predicting the current state of the robot chassis, the prediction result can reflect the influence of the control action on the motion state of the robot chassis from the previous moment to the current moment, thereby improving the accuracy of the current state prediction.

[0096] As an optional embodiment, the above-mentioned prediction of the current state of the robot chassis based on the state estimation information of the robot chassis at the previous moment and the control quantity used to control the robot chassis at the previous moment includes: Based on the state estimation information of the robot chassis at the previous moment and the control quantity used to control the robot chassis at the previous moment, the state of the robot chassis at the current moment is predicted by a preset state transition equation. The aforementioned state transition equation describes the correspondence between the state estimation information of the robot chassis at the previous moment and the control quantity used to control the robot chassis at the previous moment, and the state information of the robot chassis at the current moment.

[0097] In this embodiment, the state transition equation is a mathematical model describing the state changes of the robot chassis between adjacent time points. It can be pre-established based on the kinematic model, dynamic model, or actual motion response of the robot chassis. The input to the state transition equation may include estimated values ​​of multiple motion state terms from the previous time point, estimated values ​​of deviation terms of at least one measurement term, and control quantities from the previous time point. The output may include predicted values ​​of multiple motion state terms and predicted values ​​of corresponding deviation terms from the current time point.

[0098] Specifically, the state transition equation can determine the state change of the robot chassis from the previous moment to the current moment based on the estimated values ​​of motion state terms such as position, heading angle, linear velocity, and angular velocity at the previous moment, combined with the control quantity at the previous moment and the time difference between adjacent moments, and thus obtain the predicted values ​​of motion state terms such as position, heading angle, linear velocity, and angular velocity at the current moment.

[0099] For the deviation term, a corresponding deviation change model can be preset in the state transition equation. For example, when the sensor's measurement deviation changes slowly between adjacent time steps, the deviation term can be set to maintain the estimated value of the previous time step, or a corresponding random disturbance can be superimposed on the estimated value of the deviation term at the previous time step to obtain the predicted value of the deviation term at the current time step.

[0100] In one implementation, the state transition equation can be summarized as follows:

[0101] in, This represents the state estimate information of the robot chassis at the previous moment. This represents the control quantity used to control the robot chassis at the previous moment. This represents the preset state transition function. This represents the random perturbation during the state transition process. This represents the prediction result for the current moment obtained based on the state estimation information from the previous moment.

[0102] In this embodiment, the state estimation result of the robot chassis at the previous moment can be extrapolated to the current moment using the state transition equation, and the prediction result at the current moment can reflect the influence of the control quantity on the change of the robot chassis state, providing a basis for subsequent correction of the prediction result using the measurement information at the current moment.

[0103] As an optional embodiment, the control quantities for controlling the robot chassis include: the expected values ​​of the linear velocities of the robot chassis in the horizontal and vertical directions, and the expected values ​​of the angular velocities of the robot chassis. Based on the state estimation information of the robot chassis at the previous moment and the control variables used to control the robot chassis at the previous moment, the state of the robot chassis at the current moment is predicted through a preset state transition equation, including: Based on the expected values ​​of the linear velocities of the robot chassis in the horizontal and vertical directions in the control variables of the previous moment, and the estimated values ​​of the linear velocities of the robot chassis in the horizontal and vertical directions in the state estimation information of the previous moment, the estimated values ​​of the linear acceleration of the robot chassis in the horizontal and vertical directions of the previous moment are determined. Based on the expected value of the angular velocity of the robot chassis in the control quantity of the previous moment and the estimated value of the angular velocity of the robot chassis in the state estimation information of the previous moment, the estimated value of the angular acceleration of the robot chassis in the previous moment is determined. Based on the state estimation information of the robot chassis at the previous moment, the estimated values ​​of the linear acceleration of the robot chassis in the horizontal and vertical directions at the previous moment, and the estimated value of the angular acceleration of the robot chassis at the previous moment, the state of the robot chassis at the current moment is predicted.

[0104] In this embodiment, the control quantity can be a chassis motion control command output by the control layer. The expected linear velocities of the robot chassis in the horizontal and vertical directions are used to represent the target motion velocities of the robot chassis in the two translational motion directions, and the expected angular velocities are used to represent the target rotational velocities of the robot chassis around a preset axis. The aforementioned control quantities can be further converted into target rotational speeds of each drive wheel through the inverse kinematics solution of the robot chassis, and executed by the motor driver.

[0105] Because the robot chassis is affected by factors such as motor response characteristics, chassis mass, and motion inertia, its actual speed usually cannot immediately reach the desired speed. Therefore, the linear acceleration and angular acceleration estimates of the robot chassis can be determined based on the difference between the desired speed and the corresponding speed estimate. In one implementation, a first-order command tracing model can be used for this determination.

[0106]

[0107]

[0108] in, and Let represent the expected values ​​of the linear velocities in the horizontal and vertical directions at the previous moment, respectively. This represents the expected value of the angular velocity at the previous moment; , and These represent the estimated values ​​of the corresponding velocities at the previous moment; and These represent the estimated linear acceleration values ​​in two directions, respectively. This represents the estimated value of angular acceleration; , and These represent the equivalent time constants of the corresponding motion channels, which can be predetermined based on the step response test of the robot chassis.

[0109] When the desired speed is greater than the corresponding speed estimate, the determined acceleration estimate can be positive; when the desired speed is less than the corresponding speed estimate, the determined acceleration estimate can be negative. Therefore, the acceleration estimate can reflect the influence of the control variable on the robot chassis speed change between adjacent time points.

[0110] After obtaining the estimated linear and angular acceleration values, the linear and angular velocities of the robot chassis can be predicted by combining the time difference between adjacent moments. Furthermore, based on the linear velocity, angular velocity, and heading angle before prediction, the position and heading angle of the robot chassis can be predicted. For sensor deviations, predictions can be made synchronously according to a preset deviation change model. Therefore, without establishing a complete dynamic model from motor voltage, current, and electromagnetic torque to chassis acceleration, the state prediction results can still reflect the effect of the control quantity on the robot chassis's motion state at the previous moment.

[0111] As an optional embodiment, the above state transition equation is:

[0112] in, The time difference between the current moment and the previous moment. and According to The control quantity obtained at time t The linear accelerations of the robot chassis in the horizontal and vertical directions at that moment. According to The control quantity obtained at time t The angular acceleration of the robot chassis at all times. For the state estimation information at time k-1, and The prediction result at time k is... and The deviation terms of the encoder measurements at time k-1 are respectively. random perturbation value, The deviation term of the IMU measurement at time k-1 The random perturbation value.

[0113] In this embodiment of the disclosure, the prediction result at the current moment can be expressed as: This represents the time difference between the current moment and the previous moment. and They respectively represent according to the first The estimated linear accelerations of the robot chassis in the horizontal and vertical directions are determined by the control variables at each moment. Indicates according to the first The estimated value of angular acceleration determined by the control quantity at a given time; Indicates the first State estimation information at any given time; This indicates that the first [item] has not yet been utilized. The first time obtained after correcting the measurement information at time. Predicted results at any given time.

[0114] and They represent the first The random disturbance values ​​of the linear velocity measurement deviation term of the time encoder in the horizontal and vertical directions. Indicates the first The random disturbance value of the IMU angular velocity measurement deviation term at time step. The above random disturbance is used to characterize the slow change of sensor deviation between adjacent time steps; if the deviation change is not considered, the corresponding random disturbance can also be set to zero.

[0115] During position prediction, the linear velocities of the robot chassis in two directions within the body coordinate system can be transformed to the world coordinate system based on the heading angle from the previous moment, and multiplied by the time difference to obtain the position change. The heading angle can be predicted by integrating the angular velocity from the previous moment, optionally... It can also be expressed as , This means normalizing the predicted heading angle to a preset angle range. The velocity prediction is determined based on the velocity estimate and corresponding acceleration estimate from the previous moment. Therefore, the state transition equations can yield the predicted values ​​of position, heading angle, linear velocity, angular velocity, and sensor bias at the current moment.

[0116] S203. Based on the predicted value of the deviation term of at least one measurement item in the prediction result, update the measurement information at the current moment to obtain updated measurement information; based on the updated measurement information, update the predicted values ​​of multiple motion state items in the prediction result; based on the updated predicted values ​​of multiple motion state items and the predicted value of the deviation term of at least one measurement item in the prediction result, obtain the state estimation information of the robot chassis at the current moment.

[0117] In this embodiment of the disclosure, for a measured value with a corresponding deviation term in the measurement information at the current moment, the predicted value of the deviation term at the current moment can be used to correct the measured value, thereby reducing the impact of systematic measurement deviations of the sensor system on the state estimation result. The deviation compensation process can be expressed as:

[0118] in, This represents the raw measurement information at the current moment. This represents the predicted value of at least one deviation term at the current time. This indicates the correspondence between deviation terms and measurement terms. This indicates the updated measurement information. For measurements without a set deviation term, the original measurement value can be retained.

[0119] For example, the linear velocity measurements in the horizontal and vertical directions obtained from the encoder, and the angular velocity measurements obtained from the IMU, can be updated according to the following formulas:

[0120]

[0121]

[0122] in, and These represent the linear velocity measurements obtained by the encoder in two directions at the current moment. This represents the angular velocity measurement obtained by the IMU at the current moment; , and These represent the predicted values ​​of the corresponding deviation terms at the current time.

[0123] Subsequently, the updated measurement information can be compared with the prediction results at the current moment, and the difference between the two can be used to correct the predicted values ​​of the corresponding motion state terms. The linear velocity measurement information of the encoder can be used to correct the linear velocity prediction value, the angular velocity measurement information of the IMU can be used to correct the angular velocity prediction value, the position measurement information of the vision sensor or UWB sensor can be used to correct the position prediction value, and the heading angle measurement information of the vision sensor can be used to correct the heading angle prediction value.

[0124] After correction, the updated predicted values ​​of multiple motion state terms can be combined with the predicted value of at least one deviation term to obtain the current state estimate of the robot chassis. For example, the current state estimate can be expressed as:

[0125] in, , , , , and This represents the estimated value of the motion state term obtained after correction using the measurement information at the current moment.

[0126] Therefore, the sensor measurement information is updated with the predicted value of the deviation term, and the updated measurement information is then used to correct the motion state prediction result, thereby reducing the impact of sensor measurement deviation on the robot chassis state estimation.

[0127] As an optional embodiment, updating the predicted values ​​of multiple motion state items in the prediction result based on the updated measurement information includes: For each measurement value in the updated measurement information, based on the difference between the measurement value and the predicted value of the corresponding motion state item in the prediction result at the current time, the predicted value of the corresponding motion state item in the prediction result at the current time is updated to obtain the estimated value of the corresponding motion state item at the current time.

[0128] In this embodiment of the disclosure, for each measurement value in the updated measurement information, the motion state item corresponding to that measurement value can be determined first. For example, the position measurement value corresponds to the position state item, the linear velocity measurement value corresponds to the linear velocity state item in the same direction, the heading angle measurement value corresponds to the heading angle state item, and the angular velocity measurement value corresponds to the angular velocity state item.

[0129] Regarding the first For each measurement value, the difference between that measurement value and the corresponding predicted value of the motion state term can be calculated:

[0130] in, This indicates the first measurement in the updated measurement information. One measurement value, This represents the predicted value of the motion state term corresponding to the measured value at the current time (i.e., time k). This indicates the difference between the two. For the heading angle, this difference can be normalized to a preset angle range to avoid results that do not match the actual angle difference when the heading angle crosses the boundary.

[0131] The controller can determine the correction amount for the predicted value of the corresponding motion state term based on the above differences, and combine the correction amount with the predicted value to obtain the estimated value of the corresponding motion state term at the current time:

[0132] in, Indicates the first The update coefficients corresponding to each measurement value are used to control the degree to which the measurement value corrects the predicted value of the corresponding motion state term. The update coefficients can be determined based on the uncertainty of the state prediction and the reliability of the measurement value. For example, when using an extended Kalman filter, the update coefficients can be determined by the corresponding elements in the Kalman gain.

[0133] When the reliability of the measured values ​​is high, a larger update coefficient can be set, allowing the state estimation results to be corrected more based on the current measured values. When the noise of the measured values ​​is high or the reliability is low, a smaller update coefficient can be set, allowing the state estimation results to retain more of the predicted values ​​obtained from the state transition equation. Therefore, the above update is not simply replacing the predicted values ​​with the measured values, but rather fusing the two based on the respective uncertainties of the measurement information and the prediction results.

[0134] When multiple sensors provide measurements corresponding to the same motion state term, the aforementioned difference calculation and state update can be performed sequentially for each measurement. The state estimation result obtained from each update can serve as the current prediction basis when the next measurement is updated. Therefore, multiple measurements can be used to progressively correct the corresponding motion state term, improving the accuracy and stability of the robot chassis state estimation.

[0135] As an optional embodiment, each sensor is preset with its own noise value for the corresponding measurement item; For each measurement value in the updated measurement information, the above-mentioned updating of the predicted value of the corresponding motion state item in the current prediction result based on the difference between the measurement value and the predicted value of the corresponding motion state item in the current prediction result includes: Based on the difference between the measured value and the predicted value of the corresponding motion state item in the prediction result at the current time, as well as the noise value corresponding to the measured value, the predicted value of the corresponding motion state item in the prediction result at the current time is updated. The method also includes: The validity of the measured values ​​of each sensor is judged, and the validity judgment result is obtained, which is either valid or invalid. For each sensor, if the validity judgment result of the measurement item collected by the sensor is determined to be invalid, the noise value corresponding to the sensor is increased by a preset factor.

[0136] In this embodiment, each sensor can preset a corresponding noise value for its respective collected measurement item. The noise value characterizes the measurement uncertainty of the corresponding measurement item and can be represented by the measurement noise variance, measurement noise covariance, or elements of the measurement noise covariance matrix. When a sensor collects multiple measurement items, noise values ​​can be set separately for different measurement items, and the multiple noise values ​​constitute the measurement noise covariance matrix corresponding to that sensor.

[0137] With sensors For example, its nominal measurement noise covariance matrix can be expressed as:

[0138] in, Indicates sensor The number of measurement items provided This indicates the sensor's first... The nominal noise variance corresponding to each measurement item. Each noise value can be predetermined based on the sensor's nominal accuracy, static sampling data, calibration test results, or historical operating data.

[0139] A lower measurement noise value indicates a higher reliability of the corresponding measurement value; a higher measurement noise value indicates a higher degree of uncertainty in the corresponding measurement value. In extended Kalman filtering, the measurement noise value participates in the calculation of the Kalman gain, thus affecting the degree to which the corresponding measurement value corrects for the state prediction result.

[0140] Besides measurement noise, state prediction processes can also correspond to process noise. Process noise is used to characterize state changes that the state transition equations fail to accurately describe, such as motor response fluctuations, drive wheel slippage, load variations, and slow changes in sensor bias. Process noise can satisfy:

[0141] in, This represents the process noise covariance matrix. The process noise corresponding to the deviation term in the state vector can be used to control the response speed of the deviation estimate to new measurement information: when the process noise corresponding to the deviation term is large, the deviation estimate is allowed to change rapidly between adjacent time points; when the process noise corresponding to the deviation term is small, the deviation estimate result can be more stable.

[0142] In one implementation, the state vector of the robot chassis at time k can be represented as:

[0143] in, and This indicates the position of the robot chassis in the world coordinate system. Indicates the heading angle. and Represents the linear velocity in two directions in the body coordinate system. Indicates angular velocity; and These represent the deviations in linear velocity measurement obtained from the encoder in two directions, respectively. This indicates that the angular velocity of the IMU gyroscope is zero bias.

[0144] Specifically, state prediction and covariance prediction can be expressed as:

[0145]

[0146] in, This represents the state estimate at the previous moment. This indicates the chassis motion control command from the previous moment. This represents the predicted state value at the current moment. This represents the state estimation error covariance matrix at the previous time step. This represents the prediction error covariance matrix at the current time. The Jacobian matrix represents the state transition function.

[0147] In this embodiment of the disclosure, the Jacobian matrix of the state transition function is the first-order partial derivative matrix of the state transition function with respect to the state vector at the previous time step, and can be expressed as:

[0148] make , and in accordance with Given the state arrangement order, when using the above first-order instruction following model, the Jacobian matrix can be expressed as:

[0149] Among them, the partial derivatives of position prediction with respect to heading angle and linear velocity are used to characterize the impact of small changes in heading angle or linear velocity on the position prediction results; the velocity state corresponds to... It originates from the first-order instruction follow-up model; when the deviation term adopts the random walk model, its corresponding diagonal element is 1. If linear acceleration and angular acceleration are regarded as external inputs that have been determined by the control quantity and are no longer differentiated with respect to the velocity state in this linearization, then the diagonal element corresponding to the velocity state can also be set to 1.

[0150] In this embodiment of the disclosure, different sensors can provide measurement information asynchronously according to their respective acquisition frequencies. The controller can perform an EKF measurement update for each type of sensor data that arrives, using the corresponding measurement function, measurement matrix, and measurement noise covariance matrix.

[0151] In this embodiment of the disclosure, after obtaining the prediction result at the current moment based on the state estimation equation, the prediction result can be corrected based on the sensor measurement value.

[0152] The four wheel speeds of the encoder can be converted into two linear velocities and angular velocities in the body coordinate system using forward kinematics. (Encoder measurement vector) and measurement function It can be represented as:

[0153]

[0154] The corresponding measurement function Jacobian matrix can be expressed as:

[0155] The encoder's two linear velocity measurement models explicitly include and Therefore, the encoder measurement residual can not only be used to update the linear velocity state, but also to update the corresponding encoder deviation term via Kalman gain.

[0156] Measurement vector of the angular velocity measurement model of the IMU gyroscope Measurement function It can be represented as:

[0157] The corresponding Jacobian matrix It can be represented as:

[0158] Since the columns of the Jacobian matrix corresponding to the angular velocity state and the IMU deviation state are all non-zero, the IMU angular velocity measurement residual can simultaneously affect the angular velocity estimate and the IMU angular velocity deviation estimate.

[0159] The vision sensor can provide the position and heading angle of the robot chassis in the world coordinate system, and its measurement vector... and measurement function and Jacobi matrix It can be represented as:

[0160]

[0161]

[0162] UWB sensors can provide the position of the robot chassis in the world coordinate system, and their measurement vectors Measurement function and Jacobi matrix It can be represented as:

[0163]

[0164] While vision sensors and UWB sensors do not directly measure encoder deviation, the absolute position measurements they provide can limit position drift caused by velocity integration. Since the state covariance matrix can contain cross-covariances between position, velocity, and deviation states, the absolute position residual can indirectly affect the velocity and encoder deviation states through Kalman gain, thus providing an absolute reference for online identification of encoder deviation.

[0165] It should be noted that, in this embodiment of the disclosure, the predicted observation is determined based on the predicted values ​​of the motion state term and the corresponding predicted values ​​of the deviation term in the prediction results using the above-described observation model. The actual measured values ​​collected by the sensor are then compared with the predicted observation. This can be seen as first correcting the actual measured values ​​collected by the sensor using the predicted values ​​of the deviation term, and then comparing the corrected measured values ​​with the corresponding predicted values ​​of the motion state term. Therefore, the above process can be considered as first updating the measured values ​​based on the predicted values ​​of the deviation term, and then updating the predicted values ​​of the motion state term based on the updated measured values.

[0166] like Figure 3 As shown, Figure 3 This is a schematic diagram illustrating a process for determining corrected measurement values ​​according to an embodiment of the present disclosure. Before periodically performing state estimation, the state estimation information and covariance matrix of the robot chassis can be initialized, and a prediction step is periodically performed based on the state estimation information and control variables of the previous moment to obtain the state prediction result and prediction covariance matrix of the current moment. Further, the wheel speed estimate is calculated based on the state prediction result, and the wheel speed covariance is calculated based on the prediction covariance matrix. Subsequently, the controller determines whether sensor measurement information has been acquired; if no sensor measurement information is acquired, the prediction step continues; if sensor measurement information is acquired, an update step is performed, calculating the Kalman gain based on the difference between the sensor measurement value and the corresponding predicted measurement value, and updating the state estimation information and covariance matrix based on the Kalman gain.

[0167] Since the state estimation information includes both motion state terms and sensor bias terms, these terms are updated synchronously during the same Kalman update. The controller extracts the sensor bias estimate from the updated state estimation information and uses this estimate to correct the measurements acquired by the sensors, obtaining the corrected measurements.

[0168] It should be noted that, as Figure 3 As shown, in this embodiment of the present disclosure, after updating the state estimation information, the measured value can be further corrected. The deviation reference of the corresponding sensor can be updated according to the deviation increment obtained in this update, and the deviation increment is set to zero after the measurement value correction is completed, so that the deviation increment relative to the updated deviation reference can be re-estimated in the next processing cycle. The updated state estimation information, covariance matrix and deviation reference are used for the prediction step at the next time step.

[0169] The above-mentioned prediction values ​​of multiple motion state items in the prediction result are updated based on the updated measurement information. Based on the updated prediction values ​​of multiple motion state items and the prediction value of the deviation term of at least one measurement item in the prediction result, the state estimation information of the robot chassis at the current moment is obtained, including: Based on the difference between the measured value and the predicted value of the corresponding motion state item in the prediction result at the current time, the predicted value of the corresponding motion state item in the prediction result at the current time, as well as the predicted value of each measured item, are updated to obtain the state estimation information at the current time.

[0170] In this embodiment, the prediction results of the robot chassis at the current moment can be updated as a whole based on extended Kalman filtering. Specifically, a state vector can be constructed by combining multiple motion state terms and the deviation term of at least one measurement term, and each state term in the state vector can be updated as a whole based on the measurement information collected by the sensors. Therefore, while updating the predicted values ​​of multiple motion state terms, the predicted values ​​of the corresponding deviation terms can also be updated to obtain the state estimation information of the robot chassis at the current moment.

[0171] Specifically, for the sensor that arrives at the current moment The measurement information can be updated by following this procedure. First, calculate the innovation residual:

[0172] in, Indicates sensor The current measurement vector, This represents the predicted measurement value obtained based on the current state prediction results. The residuals involving the heading angle can be normalized to... This innovative residual is also known as the measurement residual.

[0173] The deviation compensation measurements for the encoder and IMU are expressed as follows:

[0174]

[0175]

[0176] This deviation compensation measurement can be understood as an equivalent intermediate form in the calculation of innovation residuals. For example, the innovation residual of the encoder's horizontal linear velocity satisfies:

[0177] The innovation residual is the difference between the measured value in the updated measurement information and the predicted value of the corresponding motion state term in the prediction result at the current moment. The updated measured value is obtained by correcting the actual measured value acquired by the sensor based on the predicted value of the corresponding deviation term. This innovation residual characterizes the degree of deviation between the corrected measurement result and the state prediction result, and is used to update the state prediction result at the current moment in conjunction with the Kalman gain.

[0178] To ensure that the bias term continues to change during the measurement update process, the measurement Jacobian matrix corresponding to the state vector can be used when calculating the Kalman gain, for example... and In other words, the deviation term is a quantity that changes as the state is updated.

[0179] Based on this, the innovation covariance matrix is ​​calculated. and Kalman gain :

[0180]

[0181] in, Indicates based on sensor The measurement noise covariance matrix determined by the current validity state is used to estimate the uncertainty of the innovation residual, and is used to characterize the possible range and confidence level of the difference between the actual measurement value and the measurement prediction value.

[0182] Then update the complete state vector:

[0183] And update the state estimation error covariance matrix:

[0184] For the first The updated state estimate covariance matrix at time step 1 is used to characterize the state estimation after the completion of the first step. After the measurement update of the sensor-like sensor, the uncertainty of each state estimate and the correlation between different state terms; For the first The prediction covariance matrix at time step is used to characterize the uncertainty of the state prediction result before updating using the current measurement information. Indicates based on the first Information at time 1 Predictions are made at all times, for example, the predicted value mentioned above. , can also be expressed as ; It is an identity matrix, and its dimensions are the same as those of the state vector; For the first Time of the first The Kalman gain matrix corresponding to a sensor type is used to determine the degree of correction of the sensor's measurement information to the predicted values ​​of each state. For the first Time of the first The measurement matrix corresponding to the sensor type, or the Jacobian matrix corresponding to the measurement model, is used to describe the correspondence between each state item in the state vector and each measurement item of the sensor. For the first Time of the first The measurement noise covariance matrix corresponding to a sensor class is used to characterize the uncertainty of the sensor's measurement information; superscript Indicates matrix transpose; This is a sensor type identifier used to indicate the sensor currently performing the measurement update; This indicates the estimated period for the current time or current state.

[0185] The above covariance update in Joseph form helps maintain the symmetry and positive semidefiniteness of the covariance matrix. Alternatively, under computational resource constraints, the following formula can also be used for the update:

[0186] It should be noted that the measurement update is performed on the complete state vector, not just the motion state terms such as position, heading angle, linear velocity, and angular velocity. Dividing the Kalman gain into blocks according to the motion state terms and the bias term, it can be expressed as:

[0187] in, Let Kalman gain be the motion state term and the bias term, respectively. The motion state term and the bias term are updated accordingly as follows:

[0188]

[0189] in, This represents the predicted value of the deviation term. This represents the estimated value of the deviation term after correction for the current measurement information. Therefore, the predicted value of the deviation term is not constant throughout the current period, but can change as current measurement information arrives and the Kalman update process proceeds.

[0190] For encoders and IMUs, since their measurement Jacobian matrix directly includes the non-zero elements corresponding to the bias term, the corresponding measurement residuals can directly update the bias term. For vision sensors and UWB sensors, although their measurement Jacobian matrix does not directly include the bias term, the Kalman gain's bias term row is still non-zero in the case of cross-covariance between the motion state term and the bias term in the prediction covariance matrix. Therefore, the absolute position residuals can indirectly correct the encoder bias term.

[0191] When multiple sensor measurement information is received sequentially within the same control cycle, a sequential update method can be adopted. The state estimate and covariance matrix updated by the previous sensor can be used as the prior state and prior covariance for the subsequent sensor to perform measurement updates. Therefore, the deviation term can be corrected multiple times within the same control cycle. After all valid measurement information has been updated, the final state estimate for the current moment is obtained, and this state estimate is used for robot chassis control and state prediction for the next moment.

[0192] In this embodiment of the disclosure, the controller can determine the validity of the measured values ​​collected by each sensor based on at least one of the following: communication and drive status, physical rationality, statistical consistency, and continuous abnormal status.

[0193] For example, when a data frame times out, verification fails, timestamp is abnormal, the measured value is non-numerical or infinite, or the measured value exceeds the sensor's range or the robot chassis's allowed state change range, the corresponding measured value can be determined to be invalid.

[0194] Alternatively, the normalized squared innovation can be calculated based on the innovation residuals and the innovation covariance matrix. :

[0195] in, To innovate the covariance matrix, This is for innovation residuals. When the normalized squared innovation exceeds a preset threshold corresponding to the measurement dimension, it can be determined that the measurement information is inconsistent with the current state prediction result. For measurement information that is occasionally abnormal or whose reliability is reduced but is still usable, the noise value of the corresponding sensor can be increased by a preset factor:

[0196] in, This indicates the preset amplification factor. As the measurement noise covariance matrix increases, the innovation covariance matrix also increases, and the corresponding Kalman gain decreases. Therefore, the correction effect of the sensor's measured values ​​on the state prediction results and the bias term predictions is reduced. In other words, increasing the noise value reduces the weighting of ineffective or low-reliability sensors, rather than increasing the actual measurement noise of the sensor.

[0197] In one implementation, for anomalous measurement information that still has reference value, the corresponding noise value can be increased before it is used in the update; for measurement information that is obviously erroneous, has communication interruption, or is outside the physical range, the noise value can be increased to a sufficiently large value so that its update effect is close to zero, or the current measurement update of that sensor can be skipped directly. Other effective sensors can still continue to participate in state estimation.

[0198] Optionally, the number of consecutive anomalies can also be counted. When the number of consecutive anomalies reaches a preset number, the corresponding sensor can be marked as unavailable; during recovery, multiple consecutive measurements can be required to pass a validity check to avoid frequent switching between valid and invalid sensor states. After the sensor becomes valid again, its measurement noise covariance matrix can be gradually restored to the nominal value to avoid sudden changes in Kalman gain.

[0199] In this embodiment of the disclosure, within each state estimation cycle, the controller can first obtain the predicted state value at the current moment through the state transition equation based on the state estimation information and control quantity of the previous moment, and obtain the predicted covariance matrix using the Jacobian matrix and process noise covariance matrix of the state transition equation; then, the controller performs validity judgment on the measurement information of each sensor arriving in the current cycle, and determines the corresponding measurement noise covariance matrix based on the judgment result; then, according to the timestamp order of the measurement information, the controller calculates the innovation residual, innovation covariance matrix and Kalman gain in sequence using the measurement function corresponding to each sensor, the measurement Jacobian matrix and the adjusted measurement noise covariance matrix, and updates the complete state vector and covariance matrix.

[0200] Through the above processing, the state estimation process can determine the impact of each measurement value on the state update based on the corresponding noise value, and can also reduce the fusion weight by increasing the noise value when the sensor is invalid. At the same time, motion state terms such as position and velocity, as well as the bias terms of encoder and IMU, can all change synchronously with the measurement residuals during the Kalman update process, thereby achieving unified processing of multi-sensor measurement noise, systematic bias, and state prediction uncertainty.

[0201] S204. Based on the estimated values ​​of the multiple motion state items in the current state estimation information of the robot chassis, obtain the control quantity used to control the robot chassis at the current moment, and control the robot chassis based on the control quantity at the current moment.

[0202] In this embodiment, the state estimation information can be obtained by the aforementioned extended Kalman filter process and may include estimated values ​​of motion state terms such as the robot chassis's position in the horizontal plane, heading angle, linear velocity, and angular velocity. The controller uses this state estimation information as closed-loop feedback to determine the control quantity based on the difference between the target motion state and the actual motion state of the robot chassis. Since the state estimation information provided to the controller is the result of fusing sensor measurement information and estimating the corresponding measurement deviations online, the impact of feedback errors caused by single sensor noise, zero-point drift, or wheel slippage on the control quantity can be reduced.

[0203] As an example, the current state feedback can be represented as:

[0204] in, and These are the estimated positions of the robot chassis in the horizontal and vertical directions, respectively. This is the estimated heading angle. and These are the estimated linear velocities in two directions, respectively. This is an estimate of the angular velocity. The horizontal and vertical directions here can be two mutually orthogonal directions in the world coordinate system, or they can be defined as two mutually orthogonal directions in the chassis coordinate system according to the control target.

[0205] The controller can employ an active disturbance rejection (ADRR) controller to determine the control input. An ADRR controller may include a tracking differentiator, an extended state observer, and an error feedback control module. The tracking differentiator is used to arrange a smooth transition process for the target input; the extended state observer is used to estimate the unmodeled dynamics of the robot chassis, parameter variations, and external forces as a total disturbance; the error feedback control module is used to generate the control input based on the target state, the state estimation result, and the total disturbance estimation result.

[0206] Therefore, S204 can adjust the fixed gain according to the position or velocity error, and can also actively compensate for the total disturbance that occurs in real time during the control process.

[0207] After receiving the control input, the controller converts it into motion control commands for the robot chassis and sends these commands to the motor drivers. The motor drivers then drive each drive wheel to rotate according to the motion control commands, causing the robot chassis's motion state to approximate the corresponding desired state. In this embodiment, the control input for controlling the robot chassis may include the desired linear velocity and angular velocity of the robot chassis in the horizontal and vertical directions. The controller can determine the target rotational speed of each drive wheel based on this control input and send the target rotational speed of each drive wheel to the corresponding motor driver. Based on the target rotational speed of each drive wheel, the motor driver, through speed control, current control, and pulse width modulation, drives each drive wheel to rotate, thereby controlling the robot chassis to move according to the corresponding linear and angular velocities.

[0208] For an X-type chassis, the mechanical motion commands generated by the controller can be represented as:

[0209] The target rotational speeds of each drive wheel are obtained through the inverse kinematics of the X-type chassis:

[0210] in, The inverse kinematic transformation matrix is ​​determined based on the drive wheel radius, drive wheel mounting position, roller direction, and wheel number. This represents the target speed vector for each drive wheel. Different wheel sequences or roller mounting directions can correspond to different... However, this does not affect the aforementioned control process.

[0211] As an optional embodiment, the control quantity used to control the robot chassis at the current moment is obtained based on the estimated values ​​of multiple motion state items in the current moment's state estimation information of the robot chassis, including: Obtain the expected state information at the current moment, wherein the expected state information includes the expected values ​​of the plurality of motion state items; Based on the estimated values ​​of the multiple motion state items in the current state estimation information of the robot chassis and the current desired state information, the control quantity used to control the robot chassis at the current moment is obtained through the ADRC algorithm.

[0212] like Figure 4 As shown, Figure 4 This is a schematic diagram illustrating a process for determining a control quantity according to an embodiment of this disclosure. In this embodiment, the controller acquires the desired state information at the current moment, which includes the desired values ​​of multiple motion state items; based on the estimated values ​​of multiple motion state items in the current moment's robot chassis state estimation information and the desired state information at the current moment, the controller obtains the control quantity used to control the robot chassis at the current moment.

[0213] The desired state information at the current moment can be provided by the upper-level path planning module, trajectory planning module, remote control module, or task control module. It can be represented as:

[0214] The desired state information can include the desired values ​​of all motion state items, or it can include only the desired values ​​of some motion state items depending on the current control mode. For example, in trajectory tracking mode, it can include the desired position, desired heading angle, desired linear velocity, and desired angular velocity; in speed control mode, it can include the desired linear velocity and desired angular velocity. For desired state items not directly provided by the upper-level module, they can be obtained by differentiation, difference, or interpolation based on the reference trajectory.

[0215] In this embodiment of the disclosure, the controller may be a controller including an ADRC module, which is implemented based on the ADRC algorithm and includes a tracking differentiator, an extended state observer, and an error feedback control module.

[0216] The tracking differentiator is used to smooth the desired state.

[0217] For the horizontal position Vertical position and heading angle any control channel in The tracking differentiator can be adjusted according to the desired input of the channel. Generate a smooth tracking signal and its differential signal Discrete form, for example, is

[0218]

[0219] in, To control the cycle, These are parameters used to adjust the transition speed. The tracking differential function, used to approximate the tracking signal to the desired input, can be set... The response speed and smoothness can be adjusted. This process avoids directly applying abrupt changes in desired position, desired heading angle, or desired speed to the controlled object, thereby reducing control overshoot, oscillation, and mechanical shock during cornering, starting, stopping, or speed switching.

[0220] When the upper layer has already provided a continuous desired trajectory that satisfies velocity and acceleration constraints, the desired trajectory and its differential signal can also be directly used as... and Alternatively, a first-order inertial element can be used to replace the aforementioned tracking differentiator.

[0221] The Extended State Observer (ESO) is used to estimate the total disturbance: For each control channel The controlled object of this channel can be written in a second-order extended form:

[0222] in, This is the control input for this channel. This is an estimate of the control gain. Represents known or unknown system dynamics. Indicates external disturbance. It represents the total disturbance after unifying the internal nonlinearity of the system, model parameter errors, wheel-ground friction changes, load changes, wheel slippage, slope and collision forces.

[0223] The extended state observer can employ a third-order nonlinear structure:

[0224]

[0225]

[0226]

[0227] in, The observation input for this channel is determined by the state estimation information at the current time; for aisle, Channels and aisle, They can be respectively , and . and These are the observations for the position and velocity states of the channel, respectively. This is the estimated total disturbance for the channel. , and This is the observer gain.

[0228] The nonlinear feedback function can be expressed as:

[0229] in, , , This is the sign function. When the error is small, this function can generate a higher equivalent gain to improve observation accuracy; when the error is large, it can prevent the feedback quantity from growing too quickly with the error, thus balancing the convergence capability and noise suppression capability of the observer.

[0230] In the collaborative architecture disclosed herein, the extended Kalman filter (ESO) is used to fuse sensor measurement information and output motion state estimates, while the extended state observer (ESO) is primarily used to estimate the total disturbance during trajectory tracking. In other words, the ESO can receive the EKF output as an observation input, but it does not replace the aforementioned multi-sensor state estimation process. This division of labor reduces the impact of the mixture of sensor measurement errors and disturbance estimates on control stability.

[0231] In this embodiment of the disclosure, the controller can determine the state error based on the tracking differentiator output and the EKF state estimate. For the control channel... ,have

[0232]

[0233] in, This is an estimate of the position-type motion state term of the channel. For example, the estimated value of the corresponding velocity-type motion state term; The two can be respectively and , The two can be respectively and , The two can be respectively and The heading angle error can be normalized to keep it within a preset angle range.

[0234] The nonlinear state error feedback control quantity can be expressed as:

[0235] And compensation is made based on the total disturbance estimate:

[0236] in, and For error feedback coefficients, , and For nonlinear feedback parameters, This is the disturbance compensation term. For cases using a normalized object model, the control law can also be written as... The two forms correspond to Despite the differences in definition and dimensions, during implementation, it is sufficient to keep the model of the controlled object consistent with the definition of the control law.

[0237] With the aforementioned control law, the controller can correct the control quantity based on the current estimate of the total disturbance from the ESO without waiting for the error integral term to accumulate gradually. When the load on the robot chassis, ground friction, wheel slippage, or external forces change, the total disturbance estimate changes accordingly, and the disturbance compensation term is adjusted accordingly, thereby improving the control system's adaptability to changes in operating conditions and sudden disturbances. Since this control law is in an explicit calculation form, it does not require solving a constrained optimization problem online in each control cycle, making it suitable for execution within the high-frequency control cycles of the robot chassis.

[0238] In this embodiment of the disclosure, the outputs of the three control channels can be synthesized into a control vector in the world coordinate system:

[0239] When the translation control variable is determined in the world coordinate system and the chassis drive command is executed in the machine coordinate system, coordinate transformation can be performed based on the current estimated heading angle:

[0240]

[0241] The controller can also limit the amplitude and rate of change of the control quantity based on the maximum allowable linear velocity, maximum angular velocity, maximum acceleration of the robot chassis, or the allowable speed of the motor. The limited control quantity is decomposed into the target speed of each drive wheel through the inverse kinematics of the X-shaped chassis, and then executed by the corresponding motor driver. After execution, the sensors continue to collect measurement information from the robot chassis, the EKF updates the state estimation information, and the ADRC generates the control quantity for the next control cycle accordingly, thus forming a closed-loop control.

[0242] In this embodiment, by using the multi-sensor fusion state estimate obtained from EKF as the closed-loop feedback of ADRC, the impact of sensor noise, drift, and bias on the control quantity can be reduced, improving the accuracy of trajectory tracking. By arranging the transition process for the desired state through the tracking differentiator, overshoot, oscillation, and mechanical shock caused by sudden changes in target input can be reduced, improving motion smoothness. By unifying model errors and external effects into a total disturbance and estimating it online through an extended state observer, and by setting a disturbance compensation term in the control law, the dependence of the control process on the precise dynamic model can be reduced, improving the robot chassis's anti-disturbance capability and operational stability under conditions such as load changes, ground condition changes, wheel slippage, and external collisions. By adopting an explicit error feedback and disturbance compensation control law, online computational overhead can be reduced, improving the real-time performance of embedded control. Thus, state estimation, target smoothing, and disturbance compensation work together to achieve accurate tracking of the robot chassis's motion state and timely compensation for operational disturbances.

[0243] The following is combined Figure 5 A specific embodiment of this disclosure is given, such as... Figure 5 As shown, Figure 5 This is a flowchart illustrating a control method for a robot chassis provided in an embodiment of this disclosure.

[0244] This disclosure provides a control method for a robot chassis, which can be executed by a controller of the robot chassis. The robot chassis is equipped with four Mecanum wheels, four motor drivers, an encoder, an inertial measurement unit (IMU), a vision sensor, and an ultra-wideband (UWB) sensor. The controller includes a state estimation module and an active disturbance rejection control (ADRC) module. The state estimation module, based on the extended Kalman filter algorithm, is used to fuse measurement information collected by various sensors and estimate the motion state of the robot chassis and the sensor measurement deviations. The ADRC module, also based on the ADRC algorithm, includes a tracking differentiator, an extended state observer, and an error feedback control module, used to determine the control quantities for controlling the robot chassis based on the desired state information and the state estimation information.

[0245] Specifically, the encoder, IMU, vision sensor, and UWB sensor respectively collect the measurement values ​​of corresponding measurement items of the robot chassis and send the collected measurement information to the state estimation module. The state estimation module predicts the current state of the robot chassis based on the state estimation information from the previous moment and the control input, and updates the prediction result using the measurement information from the current moment to obtain the current state estimation information of the robot chassis. The state estimation module then sends the estimated values ​​of multiple motion state items from the state estimation information to the ADRC module.

[0246] The ADRC module acquires the desired state information of the robot chassis at the current moment and determines the control quantity used to control the robot chassis at the current moment based on the desired state information and the estimated values ​​of multiple motion state terms output by the state estimation module. Specifically, the ADRC module smooths the desired state information through a tracking differentiator, estimates the total disturbance during the operation of the robot chassis through an extended state observer, and determines the control quantity through an error feedback control module based on the smoothed desired state information, the estimated values ​​of multiple motion state terms, and the estimated value of the total disturbance.

[0247] The ADRC module generates motion control commands for the robot chassis based on the control input and sends these commands to the X-type chassis. The X-type chassis, based on the motion control commands, determines the target speeds of the four motors using inverse kinematics and sends these target speeds to their respective motor drivers. Each motor driver then drives its corresponding Mecanum wheel to rotate according to the received target speed, causing the robot chassis to move at the corresponding linear and angular velocities.

[0248] After the robot chassis moves, the encoder, IMU, vision sensor, and UWB sensor continue to collect measurement information from the robot chassis and send the newly collected measurement information to the state estimation module. The state estimation module updates the state estimation information of the robot chassis based on the newly collected measurement information, and the ADRC module determines the control input for the next moment based on the updated state estimation information, thus forming a closed-loop control process for the robot chassis.

[0249] In this embodiment, steps S1 to S4 are executed sequentially.

[0250] The method includes the following steps: S1. Obtain the measurement information of the robot chassis at the current moment.

[0251] The controller acquires measurement information of the robot chassis at the current moment from the encoder, IMU, vision sensor, and UWB sensor. For each set of measurement information, the controller records the sensor identifier, measurement timestamp, measurement item, measurement value, and data status, and converts the measurement timestamps of different sensors to a unified control clock.

[0252] Encoders are installed on each Mecanum wheel or the corresponding drive motor to collect rotational information from each drive wheel. Based on the rotational information of the drive wheels, the radius of the drive wheels, the installation position of the drive wheels, and the kinematic relationship of the X-type Mecanum wheel chassis, the controller determines the linear velocity measurements of the robot chassis in the longitudinal and lateral directions of the chassis coordinate system.

[0253] The IMU is fixedly mounted on the robot chassis. The gyroscope sensing axis of the IMU corresponds to the vertical rotation axis of the robot chassis and is used to collect the angular velocity measurement value of the robot chassis rotating around the vertical axis.

[0254] The visual sensor acquires images of the environment surrounding the robot chassis and performs visual positioning based on the environmental features in the images, obtaining the position measurement values ​​of the robot chassis in the first and second directions of the world coordinate system, as well as the heading angle measurement value.

[0255] UWB sensors interact wirelessly with multiple UWB base stations set up in the operating environment via UWB tags on the side of the robot chassis. Based on the ranging results and the positions of each UWB base station, the position measurement values ​​of the robot chassis in the first and second directions of the world coordinate system are determined.

[0256] The encoder and IMU acquire measurement information at a first acquisition frequency, the vision sensor acquires measurement information at a second acquisition frequency lower than the first acquisition frequency, and the UWB sensor acquires measurement information at a third acquisition frequency lower than the second acquisition frequency. The controller writes the measurement information acquired by different sensors into a measurement queue according to the timestamp of each measurement. When the measurement information from the corresponding sensor arrives, the controller uses that measurement information to perform the corresponding state update; simultaneous arrival of measurement information from different sensors is not required.

[0257] S2. Predict the current state of the robot chassis.

[0258] The controller acquires the state estimation information of the robot chassis at the previous moment and the control quantity actually used to control the robot chassis at the previous moment.

[0259] The state estimation information includes the position estimates of the robot chassis in the first and second directions of the world coordinate system, the heading angle estimates of the robot chassis, the linear velocity estimates of the robot chassis in the longitudinal and lateral directions of the chassis coordinate system, the angular velocity estimates of the robot chassis, the longitudinal linear velocity deviation estimates of the encoder, the lateral linear velocity deviation estimates of the encoder, and the angular velocity zero bias estimates of the IMU.

[0260] The control quantities actually used to control the robot chassis at the previous moment included the linear velocity command of the robot chassis in the longitudinal direction, the linear velocity command in the lateral direction, and the angular velocity command in the chassis coordinate system. This control quantity is the actual control quantity sent to the robot chassis drive layer by the controller after amplitude limiting and rate of change limitation.

[0261] The controller determines the estimated longitudinal linear acceleration at the previous moment based on the difference between the longitudinal linear velocity command and the estimated longitudinal linear velocity, combined with the equivalent time constant of the longitudinal motion channel. Similarly, the controller determines the estimated lateral linear acceleration at the previous moment based on the difference between the lateral linear velocity command and the estimated lateral linear velocity, combined with the equivalent time constant of the lateral motion channel. Finally, the controller determines the estimated angular acceleration at the previous moment based on the difference between the angular velocity command and the estimated angular velocity, combined with the equivalent time constant of the rotation channel.

[0262] The controller determines the predicted position and heading angle at the current moment by using a preset state transition equation, based on the estimated position, heading angle, linear velocity, and angular velocity at the previous moment, as well as the time difference between the current moment and the previous moment.

[0263] The controller determines the predicted linear velocity value at the current moment based on the estimated linear velocity, estimated linear acceleration, and time difference from the previous moment; and determines the predicted angular velocity value at the current moment based on the estimated angular velocity, estimated angular acceleration, and time difference from the previous moment.

[0264] The encoder's longitudinal linear velocity deviation, encoder's lateral linear velocity deviation, and IMU's angular velocity zero bias are predicted using a random walk model. The controller uses the estimated values ​​of each deviation term from the previous time step as the base predicted values ​​of the corresponding deviation term at the current time step, and describes the changes of each deviation term between adjacent time steps using process noise.

[0265] The controller linearizes the state transition equation based on the state estimate from the previous time step, obtaining the corresponding Jacobian matrix. Based on this Jacobian matrix, the covariance matrix of the state estimate from the previous time step, and the process noise covariance matrix, the controller determines the prediction covariance matrix for the current time step.

[0266] The process noise covariance matrix is ​​used to describe the state prediction uncertainty caused by errors in the robot chassis motion model, linear acceleration estimation, angular acceleration estimation, and sensor bias. The process noise corresponding to the encoder bias term and the IMU zero bias term is set to be greater than zero, so that the corresponding bias terms can continuously change and update during the operation of the robot chassis.

[0267] At this point, the controller obtains the prediction results for the current moment. These prediction results include the position prediction, heading angle prediction, linear velocity prediction, angular velocity prediction, encoder linear velocity deviation prediction, IMU angular velocity zero bias prediction, and prediction covariance matrix.

[0268] S3. Update the prediction results based on the measurement information at the current moment.

[0269] The controller sets up measurement models, measurement matrices, and basic measurement noise covariance matrices for the encoder, IMU, vision sensor, and UWB sensor, respectively.

[0270] The encoder's measurement model is used to describe the correspondence between the longitudinal and lateral linear velocity measurements acquired by the encoder, the longitudinal and lateral linear velocity states of the robot chassis, and the encoder's linear velocity deviation.

[0271] The IMU's measurement model is used to describe the correspondence between the angular velocity measurements acquired by the IMU, the angular velocity state of the robot chassis, and the zero bias of the IMU's angular velocity.

[0272] The measurement model of the vision sensor is used to describe the correspondence between the position and heading angle measurements collected by the vision sensor and the position and heading angle states of the robot chassis.

[0273] The measurement model of the UWB sensor is used to describe the correspondence between the position measurement values ​​collected by the UWB sensor and the position state of the robot chassis.

[0274] The measurement matrix for each sensor is obtained by linearizing the state vector according to the corresponding measurement model. The measurement matrix for the encoder has valid elements set at the positions corresponding to the linear velocity state and the encoder deviation state; the measurement matrix for the IMU has valid elements set at the positions corresponding to the angular velocity state and the IMU zero bias state; the measurement matrix for the vision sensor has valid elements set at the positions corresponding to the position state and the heading angle state; and the measurement matrix for the UWB sensor has valid elements set at the positions corresponding to the position state.

[0275] For each set of measurement information that arrives, the controller determines the validity of the set of measurement information based on the measurement timestamp, the range of measurement values, the integrity of the measurement data, the rate of change of adjacent measurement values, and the consistency between the measurement value and the prediction result.

[0276] The controller determines the measurement information as valid if the timestamp of the measurement information is within the allowable delay range, the measured value is within the sensor's range, the measurement data is complete, the rate of change of adjacent measured values ​​does not exceed the corresponding threshold, and the difference between the measured value and the predicted result is within the allowable range.

[0277] If the measurement information does not meet the above conditions, the controller determines the measurement information as invalid and expands the measurement noise covariance matrix of the corresponding sensor by a preset factor. After the measurement noise covariance matrix is ​​expanded, the Kalman gain corresponding to the measurement information decreases, thereby reducing the impact of invalid measurement information on the state estimation results.

[0278] Based on the current state prediction result, the controller determines the measurement prediction value through the measurement model corresponding to the current sensor, and determines the difference between the actual measurement value collected by the sensor and the measurement prediction value to obtain the innovation residual.

[0279] For encoder measurement information, the measurement predictions include linear velocity predictions and encoder linear velocity deviation predictions. For IMU measurement information, the measurement predictions include angular velocity predictions and IMU angular velocity zero-bias predictions. Therefore, when forming innovative residuals, the encoder linear velocity deviation predictions and IMU angular velocity zero-bias predictions are used to correct the influence of the corresponding measurement information on the motion state.

[0280] The controller determines the innovation covariance matrix based on the measurement matrix, prediction covariance matrix, and adjusted measurement noise covariance matrix corresponding to the current sensor, and determines the Kalman gain based on the prediction covariance matrix, measurement matrix, and innovation covariance matrix.

[0281] The controller updates the complete state vector based on the Kalman gain and the innovation residual to obtain the current position estimate, heading angle estimate, linear velocity estimate, angular velocity estimate, encoder linear velocity deviation estimate, and IMU angular velocity zero bias estimate.

[0282] During this update process, the Kalman gain includes both the gain component corresponding to the motion state and the gain component corresponding to the deviation state. Therefore, after the measurement information arrives, the estimated values ​​of position, heading angle, linear velocity, and angular velocity change, and the estimated values ​​of encoder linear velocity deviation and IMU angular velocity zero bias also change with the Kalman update process.

[0283] While the measurement information acquired by visual and UWB sensors does not directly include encoder bias or IMU zero drift, the position, velocity, heading angle, and bias states exhibit cross-covariance in the state covariance matrix. Therefore, the absolute position measurement information provided by visual and UWB sensors can indirectly correct for linear velocity and sensor bias states through this cross-covariance, suppressing the continuous accumulation of encoder errors and IMU zero drift.

[0284] The controller updates the state estimate covariance matrix based on the Kalman gain, measurement matrix, prediction covariance matrix, and measurement noise covariance matrix to represent the uncertainty of each state estimate after the measurement update.

[0285] When multiple types of measurement information exist within the same state estimation period, the controller executes measurement updates sequentially according to the timestamp order of the measurement information, and uses the state vector and covariance matrix updated by the previous measurement information as the input for the next measurement information update.

[0286] After the measurement information within the current cycle is processed, the controller determines the final position estimate, heading angle estimate, linear velocity estimate, angular velocity estimate, encoder linear velocity deviation estimate, and IMU angular velocity zero bias estimate as the state estimate information of the robot chassis at the current moment, and sends the motion state estimate to the ADRC module.

[0287] S4. Determine the control quantity based on the state estimation information and control the robot chassis.

[0288] The controller obtains the desired state information at the current moment from the trajectory planning module. The desired state information includes the desired position of the robot chassis in the first and second directions of the world coordinate system, the desired heading angle, the desired linear velocity, and the desired angular velocity.

[0289] The controller maps the expected state information at the current moment to the estimated state information at the current moment according to the timestamp, and divides the control process of the robot chassis into the first direction translation channel, the second direction translation channel, and the heading angle rotation channel.

[0290] The first direction translation channel receives the desired position and desired linear velocity in the first direction, and uses the estimated position and linear velocity in the first direction as state feedback. The second direction translation channel receives the desired position and desired linear velocity in the second direction, and uses the estimated position and linear velocity in the second direction as state feedback. The heading angle rotation channel receives the desired heading angle and desired angular velocity, and uses the estimated heading angle and angular velocity as state feedback.

[0291] S41. Process the desired state information by tracking the differentiator.

[0292] The ADRC module sets up tracking differentiators for each of the three motion channels. Each tracking differentiator receives the desired position type input for the corresponding motion channel and generates continuously changing tracking values ​​and their derivatives based on the current tracking state, control cycle, and tracking speed parameters.

[0293] The tracking differentiator of the first direction translation channel outputs the smoothed expected position and smoothed expected linear velocity in the first direction; the tracking differentiator of the second direction translation channel outputs the smoothed expected position and smoothed expected linear velocity in the second direction; the tracking differentiator of the heading angle rotation channel outputs the smoothed expected heading angle and smoothed expected angular velocity.

[0294] The tracking differentiator limits the rate of change of the desired state between adjacent control cycles, so that the smooth desired state continuously approaches the desired state given by the trajectory planning module from the current motion state of the robot chassis, reducing overshoot, oscillation and mechanical shock caused by sudden changes in the desired state when the robot chassis starts, stops, reverses and turns.

[0295] S42. Estimate the total disturbance using an extended state observer.

[0296] The ADRC module runs third-order extended state observers for each of the three motion channels. Each extended state observer receives the position-class state estimate of the corresponding channel, the actual control component output in the previous control cycle, and the control gain of that channel.

[0297] The extended state observer of the first direction translation channel receives the position estimate in the first direction, the extended state observer of the second direction translation channel receives the position estimate in the second direction, and the extended state observer of the heading angle rotation channel receives the heading angle estimate.

[0298] Each extended state observer includes a first observation state, a second observation state, and a third observation state. The first observation state is used to track the position-type state of the corresponding channel, the second observation state is used to track the velocity-type state of the corresponding channel, and the third observation state is used to track the total disturbance experienced by the corresponding channel.

[0299] The extended state observer determines the observation error between the first observation state and the corresponding position class state estimate, and corrects the first observation state, the second observation state and the third observation state respectively through a nonlinear feedback function.

[0300] The nonlinear feedback function adjusts the equivalent observation gain according to the magnitude of the observation error. When the observation error is within a preset small error range, it increases the observation sensitivity to small state changes; when the observation error exceeds the preset small error range, it reduces the growth rate of the feedback quantity with increasing error, suppressing excessive response caused by larger errors and measurement noise.

[0301] The total disturbance characterized by the third observation state includes model errors of the robot chassis, motor response hysteresis, changes in wheel-ground friction coefficient, wheel slippage, load changes, mass distribution changes, slope effects, and external collision effects. The extended state observer updates the third observation state in each control cycle, so that the total disturbance estimate changes with the actual operating state of the robot chassis.

[0302] In this control architecture, the extended Kalman filter state estimation module is responsible for fusing sensor measurement information and estimating the motion state of the robot chassis and sensor deviations; the extended state observer receives the motion state estimate output by the extended Kalman filter and estimates the total disturbance during the robot chassis control process.

[0303] S43. Determine the state error of each motion channel.

[0304] For the translation channel in the first direction, the ADRC module determines the difference between the smoothed expected position and the position estimate in the first direction as the first position error, and the difference between the smoothed expected linear velocity and the linear velocity estimate in the first direction as the first velocity error.

[0305] For the second-direction translation channel, the ADRC module determines the difference between the smoothed expected position and the position estimate in the second direction as the second position error, and the difference between the smoothed expected linear velocity and the linear velocity estimate in the second direction as the second velocity error.

[0306] For the heading angle rotation channel, the ADRC module determines the difference between the smoothed desired heading angle and the estimated heading angle as the heading angle error, and normalizes the heading angle error to a preset angle range to determine the angle difference in the minimum rotation direction; the ADRC module determines the difference between the smoothed desired angular velocity and the estimated angular velocity as the angular velocity error.

[0307] S44. Generate the basic control components for each motion channel.

[0308] The error feedback control module performs nonlinear feedback processing on the position and velocity errors of each motion channel.

[0309] For each motion channel, the error feedback control module processes position-type errors through a first nonlinear feedback function and multiplies the processing result by the position feedback coefficient; processes velocity-type errors through a second nonlinear feedback function and multiplies the processing result by the velocity feedback coefficient; and adds the two processing results to obtain the basic control component of the motion channel without total disturbance compensation.

[0310] When the error between the robot chassis and the desired trajectory is large, the basic control component drives the robot chassis to quickly approach the desired trajectory; when the robot chassis approaches the desired trajectory, the error feedback control module finely adjusts the small error to reduce steady-state tracking error and oscillation of the control quantity near the desired state.

[0311] S45. Compensate the basic control components based on the total disturbance estimate.

[0312] The ADRC module determines the disturbance compensation component for each motion channel based on the total disturbance estimate and the corresponding control gain of each motion channel.

[0313] The ADRC module subtracts the corresponding disturbance compensation component from the basic control component of the first direction translation channel to obtain the first translation control component; subtracts the corresponding disturbance compensation component from the basic control component of the second direction translation channel to obtain the second translation control component; and subtracts the corresponding disturbance compensation component from the basic control component of the heading angle rotation channel to obtain the rotation control component.

[0314] When the load on the robot chassis, ground friction, wheel slippage, slope, or external forces change, the extended state observer updates the total disturbance estimate. The ADRC module then adjusts the disturbance compensation component synchronously based on the updated total disturbance estimate, so that the control quantity compensates for the corresponding disturbance before the tracking error accumulates significantly.

[0315] S46. Limit the control components.

[0316] The controller limits the amplitude and rate of change of the first translation control component, the second translation control component, and the rotation control component based on the maximum allowable linear velocity, maximum angular velocity, maximum linear acceleration, maximum angular acceleration of the robot chassis, and the allowable rotational speed of the drive wheels.

[0317] Amplitude limiting is used to restrict control components that exceed the robot chassis's motion capabilities to corresponding upper or lower limits. Rate of change limiting is used to limit the change in control components between adjacent control cycles to corresponding thresholds, preventing abrupt changes in control commands.

[0318] The controller determines the control component after amplitude and rate of change limits as the actual control component applied in the current control cycle, and feeds back the actual control component to the corresponding extended state observer as the control input used for disturbance estimation in the next control cycle.

[0319] S47. Perform coordinate transformation and generate chassis motion control commands.

[0320] The controller determines the rotational relationship between the world coordinate system and the robot chassis coordinate system based on the estimated heading angle at the current moment.

[0321] Based on this rotational relationship, the controller converts the first and second translational control components in the world coordinate system into longitudinal linear velocity commands and lateral linear velocity commands in the robot chassis coordinate system, and determines the rotational control component as the angular velocity command of the robot chassis.

[0322] The longitudinal linear velocity command, the lateral linear velocity command, and the angular velocity command together constitute the control quantities used to control the robot chassis at the current moment.

[0323] S48. Decompose the control quantity into the target speed of each drive wheel. Based on the inverse kinematics of the X-type Mecanum wheel chassis, and combined with the radius of the Mecanum wheel drive wheel, the distance between the front and rear wheels, the distance between the left and right wheels, the roller mounting direction, and the wheel number, the controller decomposes the longitudinal linear velocity command, the lateral linear velocity command, and the angular velocity command into the target rotational speeds of the left front wheel, the right front wheel, the left rear wheel, and the right rear wheel.

[0324] The controller sends each target speed to the corresponding motor driver. Each motor driver compares the target speed with the actual speed detected by the encoder, determines the target current through speed control, and generates a motor drive signal through current control and pulse width modulation to drive the corresponding Mecanum wheel to rotate.

[0325] The four Mecanum wheels rotate in coordination according to the corresponding target speed, enabling the robot chassis to perform longitudinal, lateral, diagonal, or rotational movements, and making the actual position, heading angle, linear velocity, and angular velocity of the robot chassis approach the desired state at the current moment.

[0326] S49. Enter the next control cycle.

[0327] After the controller completes the output of the control quantity for the current control cycle, it saves the actual output control quantity, the tracking state of the tracking differentiator, and the observation states of the extended state observer.

[0328] Each sensor continues to collect measurement information from the robot chassis at the next moment. The controller re-executes state prediction, measurement update, and deviation estimation based on the new measurement information to obtain the state estimate information for the next moment; the ADRC module redetermines the control variables based on the state estimate information and the desired state information for the next moment. Thus, S1 to S4 are executed cyclically during the operation of the robot chassis, forming a closed-loop control process for the robot chassis.

[0329] Through the above processing, the extended Kalman filter state estimation module fuses measurement information from the encoder, IMU, vision sensor, and UWB sensor, and estimates the encoder linear velocity deviation and IMU angular velocity zero bias online, reducing the impact of single sensor measurement errors and accumulated sensor deviations on the state estimation results. The ADRC module uses the state estimation results for closed-loop control, smooths the desired state by tracking differentiators, estimates the total disturbance caused by model errors and external actions by extending the state observer, and determines the control quantity through error feedback and disturbance compensation, thereby improving the accuracy of robot chassis state estimation and the stability, dynamic response capability, and disturbance rejection capability of motion control.

[0330] This disclosure provides a control device for a robot chassis. The device is applied to the controller of the robot chassis, which is equipped with a controller and multiple sensors, such as... Figure 6As shown, the control device 60 of the robot chassis may include: an acquisition module 601, a state estimation module 602, a measurement update module 603, and a control module 604, wherein, The acquisition module 601 is used to acquire the measurement information of the robot chassis collected by the multiple sensors at the current moment, and the measurement information includes the measurement values ​​of multiple measurement items; The state estimation module 602 is used to predict the state of the robot chassis at the current moment based on the state estimation information of the robot chassis at the previous moment, and obtain the prediction result at the current moment; wherein, the state estimation information includes the estimated values ​​of multiple motion state items and the estimated value of the deviation item of at least one of the multiple measurement items, and the prediction result includes the predicted values ​​of the multiple motion state items and the predicted value of the deviation item of the at least one measurement item. The measurement update module 603 is used to update the measurement information at the current moment based on the predicted value of the deviation term of at least one measurement item in the prediction result, to obtain updated measurement information; update the predicted values ​​of multiple motion state items in the prediction result based on the updated measurement information; and obtain the state estimation information of the robot chassis at the current moment based on the updated predicted values ​​of multiple motion state items and the predicted value of the deviation term of at least one measurement item in the prediction result. The control module 604 is used to obtain the control quantity for controlling the robot chassis at the current moment based on the estimated values ​​of the plurality of motion state items in the current moment state estimation information of the robot chassis, and to control the robot chassis based on the control quantity at the current moment.

[0331] Optionally, when the state estimation module predicts the current state of the robot chassis based on the state estimation information of the robot chassis at the previous moment, it is specifically used for: Obtain the control quantity used to control the robot chassis in the previous moment; Based on the state estimation information of the robot chassis at the previous moment and the control quantity used to control the robot chassis at the previous moment, the state of the robot chassis at the current moment is predicted.

[0332] Optionally, when the state estimation module predicts the state of the robot chassis at the current moment based on the state estimation information of the robot chassis at the previous moment and the control quantity used to control the robot chassis at the previous moment, it specifically performs the following: Based on the state estimation information of the robot chassis at the previous moment and the control quantity used to control the robot chassis at the previous moment, the state of the robot chassis at the current moment is predicted by a preset state transition equation. The state transition equation describes the correspondence between the state estimation information of the robot chassis at the previous moment and the control quantity used to control the robot chassis at the previous moment, and the state information of the robot chassis at the current moment.

[0333] Optionally, the control quantities used to control the robot chassis include: the expected values ​​of the linear velocities of the robot chassis in the horizontal and vertical directions, and the expected values ​​of the angular velocities of the robot chassis. When the state estimation module predicts the current state of the robot chassis based on the state estimation information of the robot chassis at the previous moment and the control variables used to control the robot chassis at the previous moment, it uses a preset state transition equation. Specifically, it is used to: Based on the expected values ​​of the linear velocities of the robot chassis in the horizontal and vertical directions in the control variables of the previous moment, and the estimated values ​​of the linear velocities of the robot chassis in the horizontal and vertical directions in the state estimation information of the previous moment, the estimated values ​​of the linear acceleration of the robot chassis in the horizontal and vertical directions of the previous moment are determined. Based on the expected value of the angular velocity of the robot chassis in the control quantity of the previous moment and the estimated value of the angular velocity of the robot chassis in the state estimation information of the previous moment, the estimated value of the angular acceleration of the robot chassis in the previous moment is determined. Based on the state estimation information of the robot chassis at the previous moment, the estimated values ​​of the linear acceleration of the robot chassis in the horizontal and vertical directions at the previous moment, and the estimated value of the angular acceleration of the robot chassis at the previous moment, the state of the robot chassis at the current moment is predicted.

[0334] As an optional embodiment, the multiple sensors include an encoder and an inertial measurement unit (IMU). The encoder measures the linear velocity of the robot chassis in the horizontal and vertical directions, and the IMU measures the angular velocity of the robot chassis. The deviation of the at least one measurement includes the deviation of the encoder's measurement and the deviation of the IMU's measurement. The plurality of motion state items include The deviation term of the at least one measurement item includes ; in, These refer to the horizontal and vertical positions of the robot chassis, respectively. The heading angle of the robot chassis. Let be the linear velocities of the robot chassis in the horizontal and vertical directions. Let be the angular velocity of the robot chassis. This is the deviation term of the encoder's measured values. This refers to the deviation term of the measurement items of the IMU.

[0335] Optionally, the state transition equation is:

[0336] in, The time difference between the current moment and the previous moment. and According to The control quantity obtained at time t The linear accelerations of the robot chassis in the horizontal and vertical directions at that moment. According to The control quantity obtained at time t The angular acceleration of the robot chassis at the specified time. For the state estimation information at time k-1, and The prediction result at time k is... and The deviation terms of the encoder measurements at time k-1 are respectively. random perturbation value, The deviation term of the IMU measurement at time k-1 The random perturbation value.

[0337] Optionally, when the control module obtains the control quantity for controlling the robot chassis at the current moment based on the estimated values ​​of the plurality of motion state items in the current moment's state estimation information of the robot chassis, it specifically uses: Obtain the expected state information at the current moment, wherein the expected state information includes the expected values ​​of the plurality of motion state items; Based on the estimated values ​​of the multiple motion state items in the current state estimation information of the robot chassis and the current desired state information, the control quantity used to control the robot chassis at the current moment is obtained through the Active Disturbance Rejection Control (ADRC) algorithm.

[0338] Optionally, when the measurement update module updates the predicted values ​​of multiple motion state items in the prediction result based on the updated measurement information, it is specifically used for: For each measurement value in the updated measurement information, based on the difference between the measurement value and the predicted value of the corresponding motion state item in the prediction result at the current time, the predicted value of the corresponding motion state item in the prediction result at the current time is updated to obtain the estimated value of the corresponding motion state item at the current time.

[0339] As an optional embodiment, each sensor is preset with its own noise value for the corresponding measurement item; When the measurement update module updates the predicted value of the corresponding motion state item in the prediction result at the current time based on the difference between the measured value and the predicted value of the corresponding motion state item in the prediction result at the current time, it is specifically used for: Based on the difference between the measured value and the predicted value of the corresponding motion state item in the prediction result at the current time, and the noise value corresponding to the measured value, the predicted value of the corresponding motion state item in the prediction result at the current time is updated. The state estimation module is also used for: The validity of the measured values ​​of each sensor is judged to obtain a validity judgment result, which is either valid or invalid. For each sensor, if the validity judgment result of the measurement item collected by the sensor is determined to be invalid, the noise value corresponding to the sensor is increased by a preset factor.

[0340] Optionally, the plurality of sensors further includes: ultra-wideband (UWB) and a vision sensor, wherein, The visual sensor measures the position of the robot chassis in the horizontal and vertical directions, as well as the heading angle of the robot chassis. The UWB measurement items are the horizontal and vertical positions of the robot chassis; Each sensor has its own priority, and each sensor is configured with a corresponding acquisition frequency based on its priority. Each sensor acquires the measurement value of its corresponding measurement item at its corresponding acquisition frequency.

[0341] The apparatus of this disclosure embodiment can execute the method provided in this disclosure embodiment, and its implementation principle is similar, and it has corresponding technical effects. The actions performed by each module in the apparatus of each embodiment of this disclosure correspond to the steps in the method of each embodiment of this disclosure. For a detailed functional description of each module of the apparatus, please refer to the description in the corresponding method shown above, and it will not be repeated here.

[0342] This disclosure provides a controller for a robot chassis, the robot chassis being equipped with multiple sensors and the controller, the controller being configured to execute the steps of the method provided in any optional embodiment of this disclosure. Compared with the prior art, this disclosure achieves the following: By acquiring measurement information of the robot chassis collected by multiple sensors at the current moment, it realizes the acquisition of measurement data related to the current motion state of the robot chassis, providing a data foundation for state estimation of the robot chassis; by predicting the deviation of multiple motion state items and at least one measurement item at the current moment based on the state estimation information of the previous moment, it realizes the synchronous prediction of the current motion state of the robot chassis and sensor measurement deviation; by updating the measurement information at the current moment based on the predicted value of the deviation item, it realizes the compensation of measurement deviation in sensor measurement information, reducing the impact of measurement deviation on state estimation; by updating the measurement information at the current moment based on the predicted value of the deviation item, it realizes the compensation of measurement deviation in sensor measurement information, reducing the impact of measurement deviation on state estimation; and by updating the measurement information based on the updated measurement information, it realizes the compensation of measurement deviation in sensor measurement information, reducing the impact of measurement deviation on state estimation. The measurement information updates the predicted values ​​of multiple motion state items, enabling the correction of the predicted motion state using the measurement information after deviation compensation, thus improving the accuracy of robot chassis state estimation. By using the updated predicted values ​​of multiple motion state items and the predicted values ​​of the deviation item as the state estimation information at the current moment, recursive estimation of motion state and measurement deviation is achieved, providing a basis for state prediction and deviation compensation at subsequent moments. By determining the control quantity based on the estimated values ​​of multiple motion state items at the current moment and controlling the robot chassis based on this control quantity, control of the robot chassis based on the state estimation result after deviation compensation is achieved, improving the accuracy and stability of robot chassis motion control.

[0343] In one alternative embodiment, an electronic device is provided, such as Figure 7 As shown, Figure 7 The illustrated electronic device 4000 includes a processor 4001 and a memory 4003. The processor 4001 and the memory 4003 are connected, for example, via a bus 4002. Optionally, the electronic device 4000 may further include a transceiver 4004, which can be used for data interaction between the electronic device and other electronic devices, such as sending and / or receiving data. It should be noted that in practical applications, the transceiver 4004 is not limited to one type, and the structure of the electronic device 4000 does not constitute a limitation on the embodiments of this disclosure.

[0344] Processor 4001 may be a CPU (Central Processing Unit), a general-purpose processor, a DSP (Digital Signal Processor), an ASIC (Application Specific Integrated Circuit), an FPGA (Field Programmable Gate Array), or other programmable logic devices, transistor logic devices, hardware components, or any combination thereof. It may implement or execute the various exemplary logic blocks, modules, and circuits described in conjunction with this disclosure. Processor 4001 may also be a combination that implements computational functions, such as including one or more microprocessor combinations, a combination of a DSP and a microprocessor, etc.

[0345] Bus 4002 may include a pathway for transmitting information between the aforementioned components. Bus 4002 may be a PCI (Peripheral Component Interconnect) bus or an EISA (Extended Industry Standard Architecture) bus, etc. Bus 4002 can be divided into address bus, data bus, control bus, etc. For ease of representation, Figure 7 The bus is represented by a single thick line, but this does not mean that there is only one bus or one type of bus.

[0346] The memory 4003 may be ROM (Read Only Memory) or other types of static storage devices capable of storing static information and instructions, RAM (Random Access Memory) or other types of dynamic storage devices capable of storing information and instructions, or EEPROM (Electrically Erasable Programmable Read Only Memory), CD-ROM (Compact Disc Read Only Memory) or other optical disc storage, optical disc storage (including compressed optical discs, laser discs, optical discs, digital universal optical discs, Blu-ray discs, etc.), magnetic disk storage media, other magnetic storage devices, or any other medium capable of carrying or storing computer programs and capable of being read by a computer, without limitation herein.

[0347] The memory 4003 is used to store computer programs that execute embodiments of the present disclosure, and is controlled by the processor 4001 to execute them. The processor 4001 is used to execute the computer programs stored in the memory 4003 to implement the steps shown in the foregoing method embodiments.

[0348] This disclosure provides a computer-readable storage medium storing a computer program, which, when executed by a processor, can implement the steps and corresponding content of the aforementioned method embodiments.

[0349] This disclosure also provides a computer program product, including a computer program that, when executed by a processor, can implement the steps and corresponding content of the aforementioned method embodiments.

[0350] It should be understood that although arrows indicate various operation steps in the flowcharts of the embodiments of this disclosure, the order in which these steps are implemented is not limited to the order indicated by the arrows. Unless explicitly stated herein, in some implementation scenarios of the embodiments of this disclosure, the implementation steps in each flowchart can be executed in other orders as required. Furthermore, some or all of the steps in each flowchart may include multiple sub-steps or multiple stages based on the actual implementation scenario. Some or all of these sub-steps or stages can be executed at the same time, and each sub-step or stage can also be executed at different times. In scenarios where execution times differ, the execution order of these sub-steps or stages can be flexibly configured as required, and the embodiments of this disclosure do not limit this.

[0351] The above description is only an optional implementation method for some implementation scenarios of this disclosure. It should be noted that for those skilled in the art, other similar implementation methods based on the technical concept of this disclosure without departing from the technical concept of this disclosure also fall within the protection scope of the embodiments of this disclosure.

Claims

1. A control method for a robot chassis, characterized in that, The method is applied to a robot chassis, the robot chassis being equipped with a controller and multiple sensors, the method being executed by the controller, and the method comprising: The robot chassis is acquired from the multiple sensors at the current moment, and the measurement information includes the measurement values ​​of multiple measurement items. Based on the state estimation information of the robot chassis at the previous moment, the state of the robot chassis at the current moment is predicted to obtain the prediction result at the current moment; wherein, the state estimation information includes the estimated values ​​of multiple motion state items and the estimated value of the deviation item of at least one of the multiple measurement items, and the prediction result includes the predicted values ​​of the multiple motion state items and the predicted value of the deviation item of the at least one measurement item. Based on the predicted value of the deviation term of at least one measurement item in the prediction result, the measurement information at the current moment is updated to obtain the updated measurement information; based on the updated measurement information, the predicted values ​​of multiple motion state items in the prediction result are updated; based on the updated predicted values ​​of multiple motion state items and the predicted value of the deviation term of at least one measurement item in the prediction result, the state estimation information of the robot chassis at the current moment is obtained. Based on the estimated values ​​of the multiple motion state items in the current state estimation information of the robot chassis, the control quantity used to control the robot chassis at the current moment is obtained, and the robot chassis is controlled based on the control quantity at the current moment.

2. The method according to claim 1, characterized in that, The step of predicting the current state of the robot chassis based on the state estimation information of the robot chassis at the previous moment includes: Obtain the control quantity used to control the robot chassis in the previous moment; Based on the state estimation information of the robot chassis at the previous moment and the control quantity used to control the robot chassis at the previous moment, the state of the robot chassis at the current moment is predicted.

3. The method according to claim 2, characterized in that, The prediction of the current state of the robot chassis based on the state estimation information of the robot chassis at the previous moment and the control variables used to control the robot chassis at the previous moment includes: Based on the state estimation information of the robot chassis at the previous moment and the control quantity used to control the robot chassis at the previous moment, the state of the robot chassis at the current moment is predicted by a preset state transition equation. The state transition equation describes the correspondence between the state estimation information of the robot chassis at the previous moment and the control quantity used to control the robot chassis at the previous moment, and the state information of the robot chassis at the current moment.

4. The method according to claim 3, characterized in that, The control quantities used to control the robot chassis include: the expected values ​​of the linear velocity of the robot chassis in the horizontal and vertical directions, and the expected values ​​of the angular velocity of the robot chassis. The prediction of the current state of the robot chassis based on the previous state estimation information of the robot chassis and the control variables used to control the robot chassis at the previous moment, through a preset state transition equation, includes: Based on the expected values ​​of the linear velocities of the robot chassis in the horizontal and vertical directions in the control variables of the previous moment, and the estimated values ​​of the linear velocities of the robot chassis in the horizontal and vertical directions in the state estimation information of the previous moment, the estimated values ​​of the linear acceleration of the robot chassis in the horizontal and vertical directions of the previous moment are determined. Based on the expected value of the angular velocity of the robot chassis in the control quantity of the previous moment and the estimated value of the angular velocity of the robot chassis in the state estimation information of the previous moment, the estimated value of the angular acceleration of the robot chassis in the previous moment is determined. Based on the state estimation information of the robot chassis at the previous moment, the estimated values ​​of the linear acceleration of the robot chassis in the horizontal and vertical directions at the previous moment, and the estimated value of the angular acceleration of the robot chassis at the previous moment, the state of the robot chassis at the current moment is predicted.

5. The method according to claim 4, characterized in that, The plurality of sensors include an encoder and an inertial measurement unit (IMU). The encoder measures the linear velocity of the robot chassis in the horizontal and vertical directions. The IMU measures the angular velocity of the robot chassis. The deviation of the at least one measurement includes the deviation of the encoder measurement and the deviation of the IMU measurement. The plurality of motion state items include The deviation term of the at least one measurement item includes ; in, These refer to the horizontal and vertical positions of the robot chassis, respectively. The heading angle of the robot chassis. Let be the linear velocities of the robot chassis in the horizontal and vertical directions. Let be the angular velocity of the robot chassis. The deviation term of the encoder's measurement items. This refers to the deviation term of the measurement items of the IMU.

6. The method according to claim 5, characterized in that, The state transition equation is: in, The time difference between the current moment and the previous moment. and According to The control quantity obtained at time t The linear accelerations of the robot chassis in the horizontal and vertical directions at that moment. According to The control quantity obtained at time t The angular acceleration of the robot chassis at the specified time. For the state estimation information at time k-1, and The prediction result at time k is... and The deviation terms of the encoder measurements at time k-1 are respectively. random perturbation value, The deviation term of the IMU measurement at time k-1 The random perturbation value.

7. The method according to any one of claims 1-6, characterized in that, The step of obtaining the control quantity for controlling the robot chassis at the current moment based on the estimated values ​​of the plurality of motion state items in the current moment state estimation information of the robot chassis includes: Obtain the expected state information at the current moment, wherein the expected state information includes the expected values ​​of the plurality of motion state items; Based on the estimated values ​​of the multiple motion state items in the current state estimation information of the robot chassis and the current desired state information, the control quantity used to control the robot chassis at the current moment is obtained through the Active Disturbance Rejection Control (ADRC) algorithm.

8. The method according to any one of claims 1-6, characterized in that, The step of updating the predicted values ​​of multiple motion state items in the prediction result based on the updated measurement information includes: For each measurement value in the updated measurement information, based on the difference between the measurement value and the predicted value of the corresponding motion state item in the prediction result at the current time, the predicted value of the corresponding motion state item in the prediction result at the current time is updated to obtain the estimated value of the corresponding motion state item at the current time.

9. The method according to claim 8, characterized in that, Each sensor has a preset noise value for its corresponding measurement item; For each measurement value in the updated measurement information, updating the predicted value of the corresponding motion state item in the prediction result at the current time based on the difference between the measurement value and the predicted value of the corresponding motion state item in the prediction result at the current time includes: Based on the difference between the measured value and the predicted value of the corresponding motion state item in the prediction result at the current time, and the noise value corresponding to the measured value, the predicted value of the corresponding motion state item in the prediction result at the current time is updated. The method further includes: The validity of the measured values ​​of each sensor is judged to obtain a validity judgment result, which is either valid or invalid. For each sensor, if the validity judgment result of the measurement item collected by the sensor is determined to be invalid, the noise value corresponding to the sensor is increased by a preset factor.

10. The method according to claim 5, characterized in that, The plurality of sensors also includes: ultra-wideband (UWB) and vision sensors, wherein... The visual sensor measures the position of the robot chassis in the horizontal and vertical directions, as well as the heading angle of the robot chassis. The UWB measurement items are the horizontal and vertical positions of the robot chassis; Each sensor has its own priority, and each sensor is configured with a corresponding acquisition frequency based on its priority. Each sensor acquires the measurement value of its corresponding measurement item at its corresponding acquisition frequency.

11. A control device for a robot chassis, characterized in that, The device is used in the controller of a robot chassis, the robot chassis being equipped with the controller and multiple sensors, and the device includes: The acquisition module is used to acquire the measurement information of the robot chassis collected by the multiple sensors at the current moment, and the measurement information includes the measurement values ​​of multiple measurement items; The state estimation module is used to predict the current state of the robot chassis based on the state estimation information of the previous moment, and obtain the prediction result for the current moment; wherein, the state estimation information includes the estimated values ​​of multiple motion state items and the estimated value of the deviation item of at least one of the multiple measurement items, and the prediction result includes the predicted values ​​of the multiple motion state items and the predicted value of the deviation item of the at least one measurement item. The measurement update module is used to update the measurement information at the current moment based on the predicted value of the deviation term of at least one measurement item in the prediction result, to obtain updated measurement information; update the predicted values ​​of multiple motion state items in the prediction result based on the updated measurement information; and obtain the state estimation information of the robot chassis at the current moment based on the updated predicted values ​​of multiple motion state items and the predicted value of the deviation term of at least one measurement item in the prediction result. The control module is used to obtain the control quantity for controlling the robot chassis at the current moment based on the estimated values ​​of the multiple motion state items in the current moment state estimation information of the robot chassis, and to control the robot chassis based on the control quantity at the current moment.

12. A controller for a robot chassis, characterized in that, The robot chassis is equipped with multiple sensors and the controller, which is configured to perform the method according to any one of claims 1-10.

13. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by a processor, it implements the method described in any one of claims 1-10.

14. A computer program product, comprising a computer program, characterized in that, When the computer program is executed by a processor, it implements the method described in any one of claims 1-10.

Images