Chassis control system and method for stair climbing robot based on posture adjustment

Abstract

This invention discloses a chassis control system and method for a stair-climbing robot vehicle based on attitude adjustment, belonging to the field of electric vehicle control technology. The system includes: acquiring multi-sensor fusion data of the stair-climbing robot vehicle chassis and initial state parameters of the actuators; calculating attitude-step adaptation data and determining the motion trajectory and attitude adjustment amount of the chassis actuators accordingly; iteratively optimizing the vehicle's center of gravity projection to fall into a stable support polygon with acceptable attitude error, and outputting an attitude adaptation completion signal; controlling the actuators' actions according to hierarchical collaborative logic, and dynamically correcting the attitude adjustment amount by collecting attitude feedback data in real time during stair climbing; this invention improves the chassis attitude stability and environmental adaptability during stair climbing through multi-sensor fusion, iterative attitude optimization, and hierarchical collaborative control, adapting to steps of different sizes.

Description

Technical Field

[0001] This invention belongs to the field of electric vehicle control technology, specifically a chassis control system and method for a stair-climbing robot vehicle based on attitude adjustment. Background Technology

[0002] With the expansion of special mobile robots in complex scenarios, stair-climbing robots, as typical unstructured environment navigators, directly impact operational safety due to the accuracy and stability of their chassis attitude control. Existing stair-climbing equipment often employs fixed structures or simple wheeled designs, resulting in several technical shortcomings: First, the sensors are limited and cannot integrate multi-dimensional information such as inertia, displacement, contour, and suspension, making it difficult to accurately calculate the vehicle's attitude and step geometry parameters, leading to poor step adaptability. Second, the attitude adjustment lacks an iterative optimization mechanism, making it easy for the center of gravity to exceed the stable support range, resulting in tilting and overturning during stair climbing. Third, the actuator control lacks hierarchical collaborative logic, leading to chaotic timing of the swing arm, suspension, and wheel assembly movements, resulting in delayed attitude response and low adjustment accuracy.

[0003] Meanwhile, traditional chassis control does not consider mechanical errors such as tire deformation and swing arm kinematic slippage, resulting in large deviations in displacement calculation and further reducing the reliability of attitude control. Furthermore, it cannot perform real-time closed-loop correction of adjustments during stair climbing, making it difficult to cope with fluctuations in step size and road surface disturbances, leading to insufficient smoothness and safety during passage. Therefore, developing a chassis control method for stair-climbing robotic vehicles that can accurately sense, iteratively optimize, and collaboratively control, in order to improve attitude stability and environmental adaptability, has become an urgent technical problem to be solved in this field. Summary of the Invention

[0004] To address the shortcomings of existing technologies, this invention proposes a chassis control system and method for a stair-climbing robot vehicle based on attitude adjustment. The system acquires multi-sensor fusion data of the stair-climbing robot vehicle chassis and initial state parameters of the actuators. After calculation, attitude-step adaptation data is obtained, and the motion trajectory and attitude adjustment amount of the chassis actuators are determined accordingly. Through iterative optimization, the vehicle's center of gravity projection falls into a stable support polygon, and the attitude error meets the standard, outputting an attitude adaptation completion signal. The actuator actions are controlled according to hierarchical collaborative logic, and attitude feedback data is collected in real time during stair climbing to dynamically correct the attitude adjustment amount. This invention improves the chassis attitude stability and environmental adaptability during stair climbing through multi-sensor fusion, iterative attitude optimization, and hierarchical collaborative control, adapting to steps of different sizes.

[0005] To achieve the above objectives, the present invention provides the following technical solution:

[0006] A chassis control method for a stair-climbing robot based on attitude adjustment includes:

[0007] Acquire multi-sensor fusion data of the stair-climbing robot chassis and initial state parameters of the chassis actuators;

[0008] Based on the multi-sensor fusion data and the initial state parameters of the chassis actuator, the real-time attitude data of the vehicle body and the geometric parameters of the step are calculated to obtain attitude-step adaptation data; the attitude-step adaptation data includes the vehicle body pitch angle, roll angle, center of gravity height, center of gravity projection position, and step height and width.

[0009] Based on the attitude-step adaptation data, the motion trajectory and attitude adjustment amount of the chassis actuator are determined, and the motion trajectory and attitude adjustment amount are iteratively optimized until the center of gravity projection of the vehicle body falls into the chassis stable support polygon and the attitude error is less than the first preset threshold, and the attitude adaptation completion signal is output.

[0010] Based on the received attitude adaptation completion signal, the chassis actuator is controlled to perform actions according to the hierarchical collaborative logic. During the stair climbing process, chassis attitude feedback data is collected in real time, and the attitude adjustment amount is dynamically corrected based on the chassis attitude feedback data for iterative optimization.

[0011] Specifically, the acquisition of multi-sensor fusion data of the stair-climbing robot chassis and initial state parameters of the chassis actuators includes:

[0012] The inertial measurement unit (IMU) collects triaxial acceleration and angular velocity data of the vehicle body, and a complementary filtering algorithm is used to fuse the triaxial acceleration and angular velocity data to generate inertial tilt angle data. A magnetic encoder mounted on the wheel drive motor shaft collects the number of motor rotations, calculates the actual linear displacement after mechanical deformation compensation, and generates wheel displacement data. Laser rangefinder arrays mounted at the front and rear of the chassis collect the reflection time difference between the vertical and horizontal planes of the steps, calculate the vertical and horizontal distances from the sensor probes to the step edges, and generate step contour data. A Hall effect position sensor collects the rotation angle of the swing arm joint and converts it into… Linear travel generates swing arm travel data; based on force-sensitive resistors and spring compression displacement sensors, the compression and rebound of the suspension springs under static and dynamic conditions are collected, and the equivalent elastic coefficient is calculated to generate suspension stiffness data; based on the number of pulses per unit time collected by the magnetic encoder, the wheel set speed data is generated through counting and time window averaging; the inertial tilt angle data, wheel set displacement data, step profile data, swing arm travel data, suspension stiffness data, and wheel set speed data are collected into a data structure in the chassis domain controller through the controller local area network bus, and the data structure stores the timestamp and valid flag of each data according to a predefined memory mapping table.

[0013] Specifically, the calculation process for the actual linear displacement after mechanical deformation compensation includes:

[0014] The nominal displacement is obtained by multiplying the original pulse count output by the magnetic encoder by the wheel set calibration circumference. The strain values ​​output by strain gauges mounted on the wheel set suspension are collected, and the current vertical load is obtained by querying the pre-stored calibration curve of the strain value and the wheel set vertical load. The tire compression radius is calculated based on the current vertical load and the tire radial stiffness coefficient, where the tire compression radius is equal to the tire unloaded radius minus the ratio of the current vertical load to the tire radial stiffness coefficient. The nominal displacement is corrected using the tire compression radius by multiplying the nominal displacement by the ratio of the wheel set calibration circumference to the compressed circumference, where the compressed circumference is equal to twice pi multiplied by the tire compression radius. The swing arm travel data and wheel set deflection angle data are collected, and the longitudinal slip ratio caused by the wheel set kinematic deformation is calculated based on the geometric relationship between the swing arm travel and the wheel set deflection angle. The first corrected displacement is corrected using the longitudinal slip ratio by multiplying the first corrected displacement by a factor minus the longitudinal slip ratio to obtain the actual linear displacement.

[0015] Specifically, based on the multi-sensor fusion data and the initial state parameters of the chassis actuator, the real-time attitude data of the vehicle body and the geometric parameters of the steps are calculated to obtain attitude-step adaptation data, including:

[0016] The pitch and roll axis angle components from the inertial tilt angle data are directly assigned as the vehicle pitch and roll angles, respectively. The displacements of the left front wheel, right front wheel, left rear wheel, and right rear wheel from the wheel set displacement data are input into a four-point support plane fitting algorithm. This algorithm uses the least squares method to solve the spatial plane equations of the four wheel contact points, obtaining attitude verification values. These values ​​are then cross-validated with the inertial tilt angle data. When the deviation exceeds a second preset threshold, a Kalman filter is used to weight and fuse the vehicle pitch and roll angles to obtain the corrected vehicle pitch. The angle and the corrected body roll angle; based on the relative height difference of each wheel set and the compression of each suspension spring, the torque distribution of the body around the longitudinal and transverse axes is solved, and the center of gravity height and center of gravity projection position are calculated, where the center of gravity projection position is represented by the abscissa and ordinate in the chassis plane coordinate system; the vertical distance sequence and horizontal distance sequence in the step contour data are input into the step boundary detection algorithm, which uses the slope change rate threshold segmentation method to extract the rising edge and horizontal edge of each step, calculates the height difference of the rising edge as the step height, and calculates the length of the horizontal edge as the step width.

[0017] Specifically, the step boundary detection algorithm uses a slope change rate threshold segmentation method to extract the rising edge and horizontal edge of each step, calculates the height difference of the rising edge as the step height, and calculates the length of the horizontal edge as the step width, including:

[0018] The vertical and horizontal distance sequences in the step contour data are constructed into discrete two-dimensional point sets, each consisting of horizontal and vertical distance coordinates. The discrete two-dimensional point sets are smoothed to obtain a smoothed step contour curve. The slope of the tangent line at each point on the smoothed step contour curve is calculated to obtain a slope sequence. The slope difference between two adjacent points in the slope sequence is calculated to obtain a slope change rate sequence. Rising edge threshold and horizontal edge threshold are set. When the slope change rate exceeds the rising edge threshold and the current slope value is positive, the current point is marked as the rising edge start point. When the slope change rate is lower than the horizontal edge threshold and the absolute value of the current slope is less than 0.01, the current point is marked as the horizontal edge start point. The vertical distance difference between the rising edge start point and the horizontal edge start point is taken as the step height, and the horizontal distance difference between the horizontal edge start point and the next rising edge start point is taken as the step width.

[0019] Specifically, the process of generating the attitude adaptation completion signal includes:

[0020] The inclusion relationship between the center of gravity projection position and the chassis stability support polygon is determined, and the shortest distance vector is calculated. The inverse direction of the shortest distance vector is decomposed into a swing arm adjustment component and a wheel set height adjustment component to generate the initial attitude adjustment amount. The chassis stability support polygon is a convex quadrilateral formed by connecting the four wheel set contact points in a clockwise order. The step height and step width are used as the constraint boundaries of the motion trajectory, and the change curves of the vehicle pitch angle and the change curves of the vehicle roll angle are planned to generate the motion trajectory. The initial attitude adjustment amount is applied to the current chassis actuator model, and the center of gravity projection position and attitude error after each step of adjustment are iteratively calculated. In each iteration, the nonlinear mapping relationship of the suspension stiffness is updated according to the adjusted swing arm stroke, and the predicted value of the wheel set displacement data is updated according to the adjusted wheel set speed. When the center of gravity projection position enters the interior of the chassis stability support polygon and the attitude error is less than the first preset threshold during the iteration process, the iteration stops, and an attitude adaptation completion signal is generated. The attitude adaptation completion signal includes the final target value of the swing arm stroke, the target value of the suspension stiffness, and the target value of the wheel set speed.

[0021] Specifically, the process of determining the inclusion relationship between the projected position of the center of gravity and the chassis stability support polygon and calculating the shortest distance vector includes:

[0022] A ray is emitted horizontally from the center of gravity projection point. The number of intersections between the ray and each side of the chassis stabilization support polygon is calculated. If the number of intersections is odd, the center of gravity projection point is determined to be inside the chassis stabilization support polygon. If the number of intersections is zero or even, the center of gravity projection point is determined to be outside the chassis stabilization support polygon. When the center of gravity projection point is outside the chassis stabilization support polygon, the shortest perpendicular from the center of gravity projection point to the line containing each side of the polygon is calculated in turn. If the perpendicular falls within the line segment interval, the perpendicular distance is recorded. Otherwise, the distance from the center of gravity projection point to the endpoint of the line segment is recorded. The minimum value among all recorded distances is taken as the shortest distance vector.

[0023] Specifically, the step of controlling the chassis actuators to perform actions according to the hierarchical collaborative logic based on the received attitude adaptation completion signal includes:

[0024] The target value of the swing arm stroke in the attitude adaptation completion signal is sent to the swing arm execution layer. The swing arm execution layer uses a proportional-integral-derivative controller to drive the electric push rod to move to the target stroke according to the trapezoidal velocity curve. The target value of the suspension stiffness is sent to the suspension adjustment layer. The suspension adjustment layer adjusts the oil volume of the suspension air spring by controlling the solenoid valve to change the equivalent stiffness. The target value of the wheel set speed is sent to the wheel set drive layer. The wheel set drive layer uses a field-oriented control algorithm to drive the brushless DC motor to reach the target speed. In the hierarchical collaborative logic, the swing arm execution layer takes precedence over the suspension adjustment layer, the suspension adjustment layer takes precedence over the wheel set drive layer, and the layers handshake their status with synchronization semaphores. After the previous layer completes its action and locks its position, it triggers the action of the next layer.

[0025] Specifically, the real-time acquisition of chassis attitude feedback data during the stair-climbing process, and the dynamic correction of attitude adjustment based on the chassis attitude feedback data for iterative optimization, includes:

[0026] During the stair climbing process, inertial tilt angle data, wheel displacement data, and swing arm travel data are collected at a fixed sampling period to generate chassis attitude feedback data. The real-time attitude value in the chassis attitude feedback data is compared with the expected attitude value at the current moment in the motion trajectory to calculate the attitude deviation vector. The attitude deviation vector includes pitch angle deviation, roll angle deviation, center of gravity height deviation, and center of gravity projection position deviation. The attitude deviation vector is input to the model predictive controller. The prediction time domain length of the model predictive controller is an integer multiple of the step period, and the control time domain length is a single step period. The rolling optimization method is used to solve the control increment sequence that minimizes the quadratic cost function between the predicted attitude and the reference trajectory. The first control increment of the control increment sequence is extracted as the correction value of the attitude adjustment. The correction value is superimposed on the current attitude adjustment to generate the corrected attitude adjustment, which is used as the initial attitude adjustment for the next iteration.

[0027] The attitude-adjustable stair-climbing robot chassis control system includes:

[0028] The data acquisition module is used to collect multi-sensor fusion data and initial state parameters of the chassis actuators of the stair climbing robot. After complementary filtering, mechanical deformation compensation and slip ratio correction, the data is collected to the chassis domain controller via CAN bus and stored according to a predefined memory mapping table.

[0029] The parameter calculation module takes multi-sensor fusion data and initial state parameters of chassis actuators as input to calculate real-time attitude data, and obtains attitude-step adaptation data through cross-validation and weighted fusion.

[0030] The attitude iteration optimization module determines the motion trajectory and attitude adjustment of the chassis actuator based on the attitude-step adaptation data. It updates the nonlinear mapping relationship of suspension stiffness and the predicted value of wheel group displacement through iterative calculation. After the iteration reaches the target, it outputs the attitude adaptation completion signal.

[0031] The dynamic correction module receives the attitude adaptation completion signal and drives the actuator to move according to the hierarchical collaborative logic of the swing arm execution layer first, the suspension adjustment layer next, and the wheel group drive layer last. At the same time, it collects attitude feedback data, uses the model predictive controller to solve the control increment, and dynamically corrects the attitude adjustment amount.

[0032] Compared with the prior art, the beneficial effects of the present invention are:

[0033] 1. This invention proposes a chassis control method for a stair-climbing robot based on attitude adjustment. Through multi-sensor fusion and attitude-step joint calculation, it can accurately acquire data on vehicle pitch angle, roll angle, center of gravity position, and step height and width. Combined with iterative optimization, it ensures that the center of gravity projection is always within the stable support polygon of the chassis, thereby improving the attitude stability and anti-overturning ability of the stair-climbing robot. It effectively solves the problems of easy center of gravity shift and insufficient attitude adaptation accuracy of traditional equipment, and can quickly adapt to steps of different specifications, improving environmental adaptability and traffic safety.

[0034] 2. This invention proposes a chassis control method for a stair-climbing robot based on attitude adjustment. It adopts a layered collaborative control logic for the swing arm, suspension, and wheel components, executes actions in priority order, and locks them synchronously. Combined with model predictive control, it corrects the attitude adjustment amount in real time, realizing dynamic closed-loop adjustment during stair climbing, improving attitude response speed and control accuracy. At the same time, it optimizes displacement calculation through algorithms, reduces sensor and structural errors, improves the accuracy of chassis motion control, makes the stair climbing process smoother and more efficient, and enhances the overall reliability and operational continuity of the equipment. Attached Figure Description

[0035] Figure 1 This is a schematic diagram of the chassis control method for the stair-climbing robot vehicle based on attitude adjustment according to the present invention;

[0036] Figure 2 This is a flowchart illustrating the principle of the chassis control method for a stair-climbing robot vehicle based on attitude adjustment according to the present invention.

[0037] Figure 3 This is a diagram of the chassis control system for the stair-climbing robot vehicle based on attitude adjustment, as described in this invention. Detailed Implementation

[0038] Example 1:

[0039] Please see Figure 1 and Figure 2 The present invention provides an embodiment of a chassis control method for a stair-climbing robot vehicle based on attitude adjustment. In this embodiment, a lithium iron phosphate battery is configured for the stair-climbing robot vehicle. The method includes steps S1 to S4, namely:

[0040] S1: Acquire multi-sensor fusion data of the stair-climbing robot chassis and initial state parameters of the chassis actuators;

[0041] Furthermore, in this embodiment, the chassis of the stair-climbing robot is initially parked horizontally and stationary, with all four wheel sets in full contact with the ground, the swing arm in the initial retracted position, the suspension spring in the natural static compression state, the chassis domain controller completes power-on initialization, and all actuators return to zero and feed back parameters such as initial position, angle, and stroke.

[0042] The acquisition of multi-sensor fusion data of the stair-climbing robot chassis and initial state parameters of the chassis actuators includes:

[0043] S1.1: The inertial measurement unit collects the three-axis acceleration and three-axis angular velocity of the vehicle body, and the complementary filtering algorithm is used to fuse the three-axis acceleration and three-axis angular velocity to generate inertial tilt angle data;

[0044] Furthermore, the inertial measurement unit (IMU) is fixedly installed at the geometric center of the stair-climbing robot chassis. This geometric center position minimizes measurement interference caused by local vibrations of the vehicle body and wheel jolts, ensuring the stability and accuracy of attitude data acquisition. After the IMU is activated, it acquires the three-axis acceleration and three-axis angular velocity signals of the vehicle body in three-dimensional space in real time at a high-frequency sampling rate of 1000 Hz. The three-axis accelerations correspond to the accelerations of the vehicle body in the forward / backward, left / right, and vertical / upward directions, respectively, while the three-axis angular velocities correspond to the rotational speeds of the vehicle body in pitch, roll, and yaw rotation, respectively.

[0045] Furthermore, after acquiring the raw triaxial acceleration and triaxial angular velocity signals, the signals first pass through a hardware low-pass filter circuit to remove high-frequency noise, such as electromagnetic interference and mechanical vibration noise generated by motor operation and road bumps. Then, the signals are transmitted to the signal processing unit of the chassis domain controller. The chassis domain controller calls a pre-stored complementary filtering algorithm program to dynamically fuse the acceleration and angular velocity data. The complementary filtering algorithm automatically allocates the fusion weights of the triaxial acceleration and angular velocity data according to the vehicle's motion state. When the vehicle is stationary or moving at a low, stable speed... During movement, the weight of the three-axis acceleration data is increased, and the vehicle tilt angle is accurately calculated using the gravitational acceleration vector. When the vehicle is in dynamic motion such as rapid attitude adjustment or switching between climbing stairs, the weight of the three-axis angular velocity data is increased to avoid dynamic interference to the acceleration signal, which could lead to distortion in the tilt angle calculation. The final output is stable, continuous, and non-jumping inertial tilt angle data. The inertial tilt angle data includes real-time values ​​of the vehicle pitch axis angle, roll axis angle, and yaw axis angle. The complementary filtering is a prior art in this field and is not an inventive solution of this application, so it will not be described in detail here.

[0046] Furthermore, in this embodiment, the stair-climbing robot is initially stationary on a horizontal ground. The vertical acceleration of the three-axis acceleration collected by the inertial measurement unit is close to the standard gravitational acceleration, while the acceleration in the front-back and left-right directions is close to zero. The angular velocities of all three axes are zero. After being fused by the complementary filtering algorithm, the pitch angle and roll angle of the output inertial tilt angle data are both 0 degrees, and the yaw angle is consistent with the initial orientation of the vehicle body, which serves as the initial inertial tilt angle reference.

[0047] S1.2: Based on the magnetic encoder installed on the shaft end of the wheel set drive motor, the number of motor rotations is collected, the actual linear displacement after mechanical deformation compensation is calculated, and wheel set displacement data is generated;

[0048] It should be emphasized that the wheel set displacement data is generated by combining the actual linear displacements of the right front wheel, left front wheel, left rear wheel, and right rear wheel after double mechanical deformation compensation. Therefore, the wheel set displacement data includes the displacements of the left front wheel, right front wheel, left rear wheel, and right rear wheel.

[0049] S1.3: Based on the laser ranging sensor arrays installed at the front and rear of the chassis to collect the reflection time difference between the vertical and horizontal surfaces of the steps, the vertical and horizontal distances from the sensor probes to the edge of the steps are calculated using the triangulation principle to generate step contour data.

[0050] Furthermore, the laser ranging sensor array is divided into a front-end array and a rear-end array. The front-end array contains three sets of laser ranging probes, which are installed on the left, middle and right sides of the front of the chassis, respectively. The rear-end array contains two sets of laser ranging probes, which are installed on the left and right sides of the rear of the chassis. All probes adopt an integrated design of infrared laser emission and reception, which can adapt to the needs of step detection under different indoor and outdoor lighting conditions.

[0051] As the stair-climbing robot slowly moves towards the steps, the laser ranging sensor array continuously emits infrared laser signals towards the step surface. The infrared laser signals are reflected after contacting the vertical and horizontal surfaces of the steps. The probe receives the reflected signals in real time and records the time difference between emission and reception. The chassis domain controller calculates the straight-line distance between the sensor and the step surface based on the time difference and the constant value of the speed of light. Then, the straight-line distance is decomposed into vertical and horizontal distances using the triangulation principle. The vertical distance is the vertical distance between the probe and the step surface, and the horizontal distance is the front-back distance between the probe and the step surface. The triangulation principle is existing technology in this field and is not an inventive solution of this application, so it will not be described in detail here.

[0052] A laser ranging sensor array continuously acquires multiple sets of vertical and horizontal distance data, forming a continuous distance sequence. The chassis domain controller performs smoothing filtering on the distance sequence to remove random errors from single measurements and extract stable and effective step contour feature data, including the inflection point position of the step edge, the vertical extension length, and the horizontal extension length, ultimately generating complete step contour data. In this embodiment, the stair-climbing robot is facing a standard residential building staircase. The distance sequence acquired by the laser ranging sensor array exhibits a clear step-like change, with the vertical distance increasing rapidly along the rising edge of the step and the horizontal distance remaining stable along the horizontal edge. This feature data can clearly characterize the overall contour of the step.

[0053] S1.4: The rotation angle of the swing arm joint is collected by the Hall effect position sensor and converted into linear stroke to generate swing arm stroke data;

[0054] Furthermore, each swing arm of the stair-climbing robot is equipped with an independent Hall effect position sensor. The Hall effect position sensor is installed at the rotary joint connecting the swing arm and the chassis. It uses the Hall effect to detect the rotation angle of the swing arm joint in real time. It is not affected by environmental factors such as mechanical friction, dust, and humidity. It has high measurement accuracy and fast response speed, and can track the movement status of the swing arm in real time.

[0055] After the Hall effect position sensor acquires the real-time rotation angle of the swing arm joint, it converts the angle signal into an electrical signal and transmits it to the chassis domain controller. The chassis domain controller converts the rotation angle linearly into the linear travel of the swing arm based on the mechanical structural parameters of the swing arm, including the swing arm length, the position of the joint rotation center, and the coordinates of the wheel assembly connection point. The linear travel represents the actual length of the swing arm extending or retracting from its initial position, reflecting the swing arm's adjustment range for the wheel assembly.

[0056] In this embodiment, in the initial state of the stair-climbing robot, all swing arms are in the retracted initial position. The rotation angle collected by the Hall effect position sensor is the initial reference angle, and the converted linear travel is zero. When the stair-climbing robot approaches the step, the chassis domain controller issues a swing arm extension command. The rotation angle of the swing arm joint gradually increases, the Hall effect position sensor collects the angle change in real time, and the chassis domain controller synchronously converts it into linear travel data. The linear travel data of the four sets of swing arms are collected and transmitted independently, and finally integrated to generate swing arm travel data. This data provides a direct basis for subsequent swing arm motion control and attitude adjustment calculation.

[0057] S1.5: Based on force-sensitive resistors and spring compression displacement sensors, the compression and rebound of suspension springs under static and dynamic conditions are collected, and the equivalent elastic coefficient is calculated using Hooke's law to generate suspension stiffness data.

[0058] Furthermore, the four-wheel independent suspension of the stair-climbing robot is equipped with force-sensitive resistors and spring compression displacement sensors. The force-sensitive resistors are installed between the upper and lower support seats of the suspension springs, which can sense the pressure on the springs in real time. The spring compression displacement sensors adopt a non-contact structure to accurately detect the compression and rebound of the springs under static loads and dynamic impacts. The two sensors work together to collect complete data on the force and deformation of the suspension springs.

[0059] Throughout the stair-climbing operation, the suspension springs experience various states, including static support, impact from the steps, and dynamic deformation during the climb. Force-sensitive resistors collect real-time spring pressure values, while spring compression displacement sensors simultaneously collect compression and rebound values ​​and transmit them to the chassis domain controller in real time. Based on Hooke's Law and factory-calibrated parameters such as spring material properties and structural dimensions, the chassis domain controller calculates the real-time equivalent spring coefficient of the suspension springs. The equivalent spring coefficient reflects the suspension's stiffness characteristics; a larger equivalent spring coefficient indicates a stiffer suspension with stronger resistance to deformation, while a smaller equivalent spring coefficient indicates a softer suspension with better damping and shock absorption.

[0060] Furthermore, the calculation process for the equivalent elasticity coefficient includes:

[0061] (1) The chassis domain controller reads the factory calibration parameters of the suspension spring from the local non-volatile memory. These factory calibration parameters include the wire diameter of the spring, the mean diameter of the spring, the effective number of coils of the spring, and the shear modulus of the spring material. The wire diameter value read by the chassis domain controller is recorded as 12mm, the mean diameter value as 80mm, the effective number of coils as 6, and the shear modulus of elasticity as 80,000 N / mm². 2 ;

[0062] (2) The chassis domain controller calculates the theoretical equivalent spring coefficient of the suspension spring based on the theoretical formula of the equivalent spring coefficient in Hooke's Law and the factory calibration parameters read. This includes: firstly, calculating the fourth power of the wire diameter, i.e., 12mm multiplied by itself to obtain 20736mm. 4 Then calculate the cube of the spring's mean diameter, which is 80mm multiplied by itself three times to get 512000mm. 3 Next, the molecular part was calculated, with a shear modulus of 80,000 N / mm². 2 Multiply by 20736 to get 165,888,000 N / mm 2 Then calculate the denominator: first, multiply 8 by the effective number of revolutions (6 revolutions) to get 48, then multiply 48 by 512000mm. 3 The result is 24576000 mm. 3 Dividing the numerator by the denominator, we get 67.5 N / mm, which is the theoretical equivalent spring constant of the suspension spring.

[0063] (3) The chassis domain controller reads the voltage signal output by the force-sensitive resistor, and uses linear interpolation to query the current support force value based on the pre-stored calibration curve of voltage and force. The current support force value is 1350 Newtons.

[0064] (4) The chassis domain controller collects the spring compression data at the current moment from the spring compression displacement sensor in real time. The spring compression displacement sensor is installed on the side of the suspension spring. The chassis domain controller reads the displacement signal output by the spring compression displacement sensor. The value of the displacement signal is 20mm, which represents the amount of change of the spring from its free length to its current length, i.e. the current compression value.

[0065] (5) The chassis domain controller calculates the real-time equivalent elastic coefficient of the suspension spring based on the force-displacement relationship in Hooke's Law, combined with the current support force value and the current compression value. The chassis domain controller divides the current support force value of 1350N by the current compression value of 20mm to obtain 67.5N / mm, which is the real-time equivalent elastic coefficient of the suspension spring.

[0066] (6) The chassis domain controller compares the obtained theoretical equivalent elastic coefficient with the obtained real-time equivalent elastic coefficient. In this embodiment, the chassis domain controller calculates the difference between the theoretical equivalent elastic coefficient and the real-time equivalent elastic coefficient and obtains zero N / mm. The chassis domain controller determines whether the absolute value of the difference exceeds the preset deviation threshold. The preset deviation threshold is 5% of the theoretical equivalent elastic coefficient, i.e. 3.375 N / mm. Since the difference is 0 N / mm, which is less than 3.375 N / mm, the chassis domain controller determines that the real-time equivalent elastic coefficient of the current suspension spring is accurate and reliable, and outputs the obtained 67.5 N / mm as the final suspension stiffness data.

[0067] (7) If the absolute value of the difference between the theoretical equivalent elastic coefficient and the real-time equivalent elastic coefficient exceeds 3.375 N / mm, the chassis domain controller will determine that the suspension spring has fatigue deformation or the sensor has measurement deviation. At this time, the chassis domain controller will use the weighted average method to fuse the theoretical equivalent elastic coefficient and the real-time equivalent elastic coefficient, set the weight coefficient of the theoretical equivalent elastic coefficient to 0.3, set the weight coefficient of the real-time equivalent elastic coefficient to 0.7, calculate the fused equivalent elastic coefficient, and output it as the final suspension stiffness data.

[0068] In this embodiment, when the stair-climbing robot is stationary on a level surface, the suspension springs only bear the weight of the vehicle body. The pressure collected by the force-sensitive resistor is stable, and the compression amount collected by the spring compression displacement sensor is constant. The chassis domain controller calculates the static equivalent elastic coefficient. When the wheel set contacts the edge of the step, the spring is subjected to an instantaneous impact, the compression amount increases rapidly, the pressure value of the force-sensitive resistor rises, and the chassis domain controller calculates the dynamic equivalent elastic coefficient in real time to respond promptly to the deformation changes of the suspension. When the center of gravity of the vehicle body shifts as it climbs the step, the compression amount and pressure of the suspension of different wheel sets differ. The chassis domain controller calculates the equivalent elastic coefficient of the four-wheel suspension separately, and finally integrates all the data to generate suspension stiffness data.

[0069] S1.6: The number of pulses per unit time collected by the magnetic encoder is counted and averaged over a time window to generate wheel set speed data;

[0070] Furthermore, the specific steps in S1.6 include:

[0071] (1) Confirm the inherent output parameters of the magnetic encoder and the system sampling parameters. The magnetic encoder adopts the factory-calibrated parameter of 10240 pulses per revolution and does not make any modifications during use. The system sets a fixed signal sampling frequency for the magnetic encoder. The sampling frequency is set to 100 Hz, which corresponds to completing one signal acquisition every 10 milliseconds. This sampling frequency can match the response requirements of motor operation and wheel speed change, ensuring the real-time performance and stability of signal acquisition.

[0072] (2) The original pulse signal output by the magnetic encoder is collected in real time according to the real-time sampling frequency. The magnetic encoder rotates synchronously with the shaft of the wheel drive motor and outputs a pulse signal corresponding to the rotation state of the motor.

[0073] (3) Receive the original pulse signal, determine the rotation direction of the motor based on the phase difference of the original pulse signal, and then determine the movement direction of the wheel set to obtain the pulse signal with direction marking;

[0074] (4) A fixed counting time window with a length of 50 milliseconds is preset. This time window is used to uniformly count the number of pulses per unit time. Each time the time window starts, the system first clears the pulse counting register, and then continuously accumulates the pulse signal with direction mark. When the time window ends, the counting stops and the total number of pulses in the current time window is latched.

[0075] (5) Based on the total number of pulses within the time window, combined with the time window length of 50 milliseconds, first calculate the number of pulses corresponding to each millisecond, then convert the number of pulses corresponding to each millisecond into the number of pulses per second to obtain the original pulse frequency data without smoothing.

[0076] (6) Set the overlap ratio of the sliding time window to 80% so that there is a 40-millisecond overlap area between two adjacent time windows. Take the original pulse frequency data of the current time window and the original pulse frequency data of the previous four consecutive historical time windows as the calculation objects. After summing the five sets of data by the arithmetic mean method, take the average value to obtain the smoothed stable pulse frequency data and eliminate the data deviation caused by instantaneous fluctuations.

[0077] (7) Based on the parameter of 10240 pulses per revolution, combined with the rolling radius parameter calibrated by the wheel set at the factory, the number of pulses per second after smoothing is converted into the number of revolutions per second of the wheel set, and the linear velocity of the wheel set forward per second is obtained, generating the initial data of the unverified wheel set speed.

[0078] (8) The effective range of the wheel set speed is set to 0 to 2 meters per second in advance. This range covers all speed scenarios of the stair climbing robot. The initial data of the wheel set speed is compared with the effective range. Data within the effective range is determined to be effective speed data, and data outside the effective range is determined to be abnormal data. Abnormal data is replaced with the effective speed data of the previous moment to obtain the final usable effective data of the wheel set speed.

[0079] (9) The collection time is marked for the valid data of wheel speed using the millisecond-level timestamp generation rule. The timestamp is stored synchronously with the valid data of wheel speed, so that the data can be time-aligned with other sensor data to obtain the wheel speed data.

[0080] S1.7: The inertial tilt angle data, wheel set displacement data, step profile data, swing arm travel data, suspension stiffness data, and wheel set speed data are collected into the data structure in the chassis domain controller through the controller local area network bus. The data structure stores the timestamp and valid flag of each data according to a predefined memory mapping table.

[0081] Furthermore, the data structure adopts a nested union structure. The structure header includes a four-byte timestamp field to record the system time at the moment of data acquisition, a one-byte data source identifier field to distinguish six data sources: inertial measurement unit, magnetic encoder, laser rangefinder, Hall effect position sensor, force-sensitive resistor, and spring compression displacement sensor, and a one-byte valid flag field. The lower six bits of the eight binary bits of this valid flag field correspond to the reception status of inertial tilt angle data, wheel set displacement data, step profile data, swing arm travel data, suspension stiffness data, and wheel set speed data, respectively. When a data point is not updated within a preset time window, the corresponding valid flag position is zero. In this embodiment, the preset time window is twice the system sampling period, and the sampling period is 20ms to 100ms. Before calling the data calculation function, the main loop program of the chassis domain controller first reads the value of the valid flag field and checks whether the valid flags of the six required data points are all logic one. If the valid flag of any required data point is zero, a data re-acquisition request is triggered. This request is broadcast to each sensor node through the controller local area network bus, requiring all sensors to resend data in the next sampling period.

[0082] The calculation process for the actual linear displacement after mechanical deformation compensation includes:

[0083] S1.2.1: Collect the original pulse count output by the magnetic encoder, multiply the original pulse count by the wheel set calibration circumference to obtain the nominal displacement of the wheel set under ideal conditions of no load and no deformation. The wheel set calibration circumference is the one calibrated at the time of manufacture.

[0084] S1.2.2: Collect the strain values ​​output by the strain gauges installed on the wheel assembly suspension. Based on the strain values ​​and the pre-stored calibration curve of the vertical load of the wheel assembly, the current vertical load is obtained. The strain gauges can accurately sense the stress deformation of the suspension in the vertical direction. There is a one-to-one correspondence between the strain values ​​and the vertical load of the wheel assembly calibration curve. This calibration curve has undergone multiple load tests and is stored in the chassis domain controller before the stair climbing robot leaves the factory.

[0085] S1.2.3: Calculate the tire compression radius based on the current vertical load and the tire radial stiffness coefficient; the tire compression radius is equal to the tire unloaded radius minus the ratio of the current vertical load to the tire radial stiffness coefficient;

[0086] S1.2.4: The nominal displacement is corrected by using the tire compression radius, that is, by multiplying the nominal displacement by the ratio of the wheel set's calibrated circumference to the circumference after compression to obtain the first corrected displacement; the circumference after compression is equal to twice pi multiplied by the tire compression radius;

[0087] S1.2.5: Collect swing arm stroke data and wheel set deflection angle data, and calculate the longitudinal slip ratio caused by the kinematic deformation of the wheel set based on the geometric relationship between the swing arm stroke and the wheel set deflection angle.

[0088] Further, the specific steps of S1.2.5 can be understood as follows: Collect swing arm stroke data and wheel set deflection angle data; based on the geometric relationship between swing arm stroke and wheel set deflection angle, calculate the longitudinal displacement deviation of the wheel set driven by the swing arm movement. This longitudinal displacement deviation is caused by the non-pure rolling motion generated by the swing arm rotation driving the wheel set, which belongs to the inherent kinematic deformation brought about by the chassis structure; convert the longitudinal displacement deviation in a unit time, that is, divide the total longitudinal displacement deviation in a unit time by the unit time length to obtain the speed deviation; determine the theoretical forward speed, calculate the sum of the theoretical forward speed and the speed deviation to obtain the actual forward speed, and then divide the difference between the actual forward speed and the theoretical forward speed by the theoretical forward speed to obtain the longitudinal slip ratio caused by the kinematic deformation of the wheel set.

[0089] It needs to be explained that the swing arm is a mechanical structure connecting the chassis and the wheel assembly. Every time the swing arm extends or retracts a certain length, the wheel assembly will deflect around the rotation center of the swing arm. The swing arm stroke refers to the straight-line distance the swing arm extends or retracts from its reference position, and the wheel assembly deflection angle refers to the rotation angle of the wheel assembly relative to the vertical axis of the chassis. In the chassis mechanical structure design, key dimensions such as the swing arm length and the position of the hinge point are fixed at the factory. When the swing arm extends or retracts, it will drive the wheel assembly to perform a fixed-length linkage motion around the hinge point, thus forming a uniquely determined constraint relationship. Specifically, given the change in the swing arm stroke, the wheel assembly deflection angle can be uniquely calculated through the mechanism's geometric dimensions; given the change in the wheel assembly deflection angle, the change in the swing arm stroke can also be uniquely calculated in reverse.

[0090] Furthermore, the calculation process for longitudinal displacement deviation includes: collecting current swing arm travel data to determine the actual length of the swing arm extension or retraction; collecting current wheel set deflection angle data to determine the deflection angle of the wheel set relative to the chassis; using the geometric relationship between the swing arm travel and the wheel set deflection angle, calculating the actual arc trajectory length traveled by the wheel set under the drive of the swing arm; projecting this arc trajectory length onto the longitudinal straight line direction of the vehicle's forward movement to obtain the theoretically expected linear displacement; calculating the difference between the linear displacement and the ideal displacement that the wheel set should produce when rolling purely, and the resulting difference is the longitudinal displacement deviation caused by the kinematic deformation of the wheel set.

[0091] S1.2.6: The first corrected displacement is corrected by using the longitudinal slip ratio, that is, the first corrected displacement is multiplied by the difference between the longitudinal slip ratio and the first corrected displacement to obtain the actual linear displacement.

[0092] S2: Based on the multi-sensor fusion data and the initial state parameters of the chassis actuator, calculate the real-time attitude data of the vehicle body and the geometric parameters of the step to obtain attitude-step adaptation data; the attitude-step adaptation data includes the vehicle body pitch angle, roll angle, center of gravity height, center of gravity projection position, and step height and width;

[0093] Based on the multi-sensor fusion data and the initial state parameters of the chassis actuator, the real-time attitude data of the vehicle body and the geometric parameters of the steps are calculated to obtain attitude-step adaptation data, including:

[0094] S2.1: Directly assign the pitch axis angle component and roll axis angle component in the inertial tilt angle data as the vehicle pitch angle and vehicle roll angle;

[0095] It should be noted that, as shown in S1.1, the inertial tilt angle data includes three real-time values: vehicle pitch axis angle, roll axis angle, and yaw axis angle. Among them, the yaw axis angle reflects the horizontal steering angle of the vehicle body and has no direct impact on the climbing attitude adjustment. Therefore, only the pitch axis angle component and the roll axis angle component are extracted as the initial values ​​of the vehicle pitch angle and vehicle roll angle, respectively. The vehicle pitch angle represents the degree of tilt of the vehicle body in the front-rear direction. When climbing stairs, the vehicle body will lift up with the steps, and the pitch angle will gradually increase. The vehicle roll angle represents the degree of tilt of the vehicle body in the left-right direction. When the height of the left and right wheel sets is inconsistent, the roll angle will change.

[0096] In this embodiment, the stair-climbing robot is initially stationary on a horizontal surface, and the pitch axis and roll axis angle components in the inertial tilt angle data are both 0 degrees. Therefore, the initial pitch angle and roll angle of the vehicle are both 0 degrees. When the front wheel assembly of the stair-climbing robot contacts the step and slowly lifts up, the pitch axis angle component in the inertial tilt angle data gradually increases, the vehicle pitch angle is updated synchronously, the roll axis angle component remains at 0 degrees, and the vehicle roll angle remains stable.

[0097] S2.2: Input the left front wheel displacement, right front wheel displacement, left rear wheel displacement and right rear wheel displacement from the wheel set displacement data into the spatial four-point support plane fitting algorithm. The spatial four-point support plane fitting algorithm uses the least squares method to solve the spatial plane equations of the four wheel set contact points to obtain the attitude verification value, and performs cross-verification with the inertial tilt angle data. When the deviation exceeds the second preset threshold, the Kalman filter is used to perform weighted fusion of the vehicle pitch angle and the vehicle roll angle to obtain the corrected vehicle pitch angle and the corrected vehicle roll angle.

[0098] Furthermore, the calculation process for the attitude verification value is as follows:

[0099] (1) Establish a fixed chassis plane coordinate system with the geometric center of the stair climbing machine chassis as the origin of the coordinate system, set the longitudinal forward direction of the vehicle as the longitudinal axis of the coordinate system, set the left and right lateral direction of the vehicle as the lateral axis of the coordinate system, and set the direction perpendicular to the ground upward as the vertical axis of the coordinate system.

[0100] (2) According to the chassis factory calibration parameters, read the fixed lateral coordinates and fixed longitudinal coordinates of the four wheel sets in the chassis plane coordinate system. The fixed longitudinal coordinates of the left front wheel and the right front wheel are consistent, the fixed longitudinal coordinates of the left rear wheel and the right rear wheel are consistent, the fixed lateral coordinates of the left front wheel and the left rear wheel are consistent, and the fixed lateral coordinates of the right front wheel and the right rear wheel are consistent.

[0101] (3) Read the displacement of the left front wheel, the right front wheel, the left rear wheel and the right rear wheel in the wheel set displacement data, and use them as the vertical coordinates of the left front wheel contact point, the right front wheel contact point, the left rear wheel contact point and the right rear wheel contact point respectively. The vertical coordinates reflect the height change of each wheel set contact point relative to the chassis reference.

[0102] (4) Combine the fixed lateral coordinates and fixed longitudinal coordinates of each wheel group with the real-time vertical coordinates of each wheel group to generate the three-dimensional coordinates of the contact point of the left front wheel, the contact point of the right front wheel, the contact point of the left rear wheel, and the contact point of the right rear wheel. These three-dimensional coordinates completely describe the spatial position of the contact point of each wheel group in the chassis plane reference coordinate system.

[0103] (5) Based on the expression rules of the plane equation, establish corresponding constraint equations for the three-dimensional coordinates of each wheel contact point. Four constraint equations are constructed for the four wheel contact points. The four constraint equations are combined to form an overdetermined equation system for solving the plane parameters. The variables to be solved in the overdetermined equation system are the three normal vector components of the supporting plane. The number of equations in the overdetermined equation system is greater than the number of variables to be solved.

[0104] Furthermore, the process of constructing the corresponding constraint equations for the three-dimensional coordinates of each generated wheel contact point includes: numerically associating the fixed lateral coordinates of the current wheel contact point with the first direction parameter of the supporting plane; then numerically associating the fixed longitudinal coordinates of the current wheel contact point with the second direction parameter of the supporting plane; and finally numerically associating the vertical coordinates of the current wheel contact point with the third direction parameter of the supporting plane. The first, second, and third direction parameters are the parameters to be solved for on the supporting plane, satisfying the general form of the plane equation. After the association is completed, the values ​​obtained from these three associations are combined to ensure that the combined result satisfies the constant numerical relationship required by the spatial position, thereby forming constraint equations applicable only to the current wheel contact point.

[0105] Furthermore, the same construction operation is performed sequentially on the contact points of the left front wheel, right front wheel, left rear wheel, and right rear wheel. Each wheel contact point generates an independent constraint equation. The four constraint equations are combined to form an overdetermined set of equations for solving the planar direction parameters.

[0106] (6) Perform singular value decomposition on the overdetermined equation system to obtain the least square solution of the overdetermined equation system. The least square solution is the three normal vector components of the four wheel support plane of the vehicle body. The three normal vector components together determine the spatial orientation of the support plane and obtain the support plane normal vector.

[0107] (7) Perform a dot product operation between the normal vector of the supporting plane and the vertically upward unit vector in the chassis plane coordinate system to obtain a unique dot product value, which is used to characterize the relative relationship between the plane normal vector and the vertical direction;

[0108] (8) Perform an inverse cosine operation on the dot product value to obtain the deflection angle of the plane normal vector relative to the vertical direction. This deflection angle can intuitively reflect the spatial tilt of the four-wheel support plane. This deflection angle is determined as the attitude verification value output by the spatial four-point support plane fitting algorithm.

[0109] Furthermore, the calculation process for the corrected vehicle pitch angle and the corrected vehicle roll angle includes:

[0110] (1) Read the attitude verification value output by the spatial four-point support plane fitting algorithm. The attitude verification value includes the pitch direction verification value and the roll direction verification value. Simultaneously read the inertial tilt angle data output by the inertial measurement unit. The inertial tilt angle data includes the vehicle pitch angle component and the vehicle roll angle component.

[0111] (2) The deviation judgment threshold used for cross-validation is used as the second preset threshold and is set to a fixed value of 0.5 degrees to determine whether the deviation between the attitude verification value and the inertial tilt angle data is within a reasonable range, and the second preset threshold remains constant during system operation.

[0112] (3) The pitch direction verification value and the vehicle pitch angle component are used to calculate the difference to obtain the pitch direction deviation value, and the pitch direction deviation value is compared with the second preset threshold to determine whether the pitch direction deviation exceeds the limit range.

[0113] (4) The difference between the roll direction verification value and the roll angle component of the vehicle body is calculated to obtain the roll direction deviation value. The calculated roll direction deviation value is compared with the second preset threshold to determine whether the roll direction deviation exceeds the limit range.

[0114] (5) When the judgment results of (3) and (4) do not exceed the limit, the attitude verification value and the inertial tilt angle data are both in a valid state. The obtained vehicle pitch angle component and vehicle roll angle component are directly output as valid attitude data, and the Kalman filter operation is not started.

[0115] (6) When the judgment result of (3) or (4) is outside the limit range, start the Kalman filter fusion process, including: initializing a one-dimensional Kalman filter, setting the state transition matrix of the Kalman filter to an identity matrix with the internal elements of the matrix being a fixed value of 1, setting the observation matrix to an identity matrix with the internal elements of the matrix being a fixed value of 1, setting the process noise covariance matrix to a fixed constant of 0.0001 based on the factory calibration value of the random walk noise of the inertial measurement unit angular velocity, and setting the observation noise covariance matrix to a fixed constant of 0.0004 based on the joint calibration value of the quantization error of the wheel displacement sensor and the plane fitting error caused by wheel slippage.

[0116] (7) Using the pitch direction verification value as the predicted value input and the vehicle pitch angle component as the observed value input, within the current sampling period, firstly, based on the posterior estimate obtained from the previous sampling period and the configured state transition matrix, the prior state estimate of the current period is calculated; then, based on the posterior error covariance obtained from the previous sampling period, after state transition mapping, it is superimposed with the process noise covariance to obtain the prior error covariance of the current period. However, since the state transition matrix is ​​an identity matrix, the sum of the posterior error covariance and the process noise covariance of the previous sampling period is used as the prior error covariance of the current period; subsequently, based on the prior error covariance and the observation noise covariance... The Kalman gain for the current period is calculated by considering the numerical relationship between the differences. Then, the deviation value is obtained by subtracting the prior state estimate from the vehicle pitch angle component of the current sampling period. The deviation value is multiplied by the Kalman gain and then added to the prior state estimate to complete the superposition correction of the prior state estimate. Finally, the posterior estimate for the current sampling period is obtained, which is the corrected vehicle pitch angle. Finally, the posterior error covariance for the current sampling period is updated, and the updated value is stored and used as the input for the next sampling period. The calculation formula for the prior state estimate is prior art in this field and is not an inventive solution of this application, so it will not be described in detail here.

[0117] (8) Using all the filter parameters configured in (6), with the roll direction verification value as the prediction value input and the vehicle roll angle component as the observation value input, execute the process in (7) to obtain the corrected vehicle roll angle;

[0118] (9) The corrected vehicle pitch angle and the corrected vehicle roll angle are integrated to form complete corrected vehicle attitude data and transmitted to the chassis domain controller for vehicle center of gravity calculation and attitude adjustment. At the same time, the obtained posterior estimate and posterior error covariance are stored as the pre-data for the next sampling period verification and filtering fusion.

[0119] S2.3: Based on the relative height difference of each wheel set and the compression of each suspension spring, the torque distribution of the vehicle body around the longitudinal and transverse axes is solved using the torque balance equation, and the center of gravity height and center of gravity projection position are calculated. The center of gravity projection position is represented by the abscissa and ordinate in the chassis plane coordinate system.

[0120] Furthermore, the specific steps in S2.3 include:

[0121] (1) Read the displacement of the left front wheel, the right front wheel, the left rear wheel and the right rear wheel in the wheel set displacement data, calculate the displacement difference between adjacent wheel sets respectively, and obtain the lateral relative height difference between the left front wheel and the right front wheel, the lateral relative height difference between the left rear wheel and the right rear wheel, the longitudinal relative height difference between the left front wheel and the left rear wheel, and the longitudinal relative height difference between the right front wheel and the right rear wheel. All relative height differences are used to characterize the tilt state of the current four-wheel support plane.

[0122] (2) Read the spring compression of the left front wheel suspension, right front wheel suspension, left rear wheel suspension and right rear wheel suspension in the suspension stiffness data. The spring compression is collected by force-sensitive resistor and spring compression displacement sensor, which can directly reflect the magnitude of the vertical load borne by the corresponding wheel set.

[0123] (3) Based on the spring coefficient specified by the manufacturer of the suspension spring, calculate the spring compression and spring coefficient corresponding to each wheel group to obtain the vertical support force of the left front wheel, right front wheel, left rear wheel and right rear wheel. The vertical support forces of the four wheels together constitute the load system of the vehicle body.

[0124] Further explanation is needed: the spring compression corresponding to each wheel group is multiplied by the elastic coefficient to obtain the axial support force generated by the suspension spring of the current wheel group. This force is the real-time vertical support force borne by the corresponding wheel group. The system performs the same operation sequentially on the left front wheel suspension, right front wheel suspension, left rear wheel suspension, and right rear wheel suspension to obtain the vertical support force of each of the four wheel groups.

[0125] (4) Taking the chassis geometric center as the torque reference point, and combining the wheel track parameters calibrated by the chassis factory, half of the wheel track value is determined as the lateral distance between the left and right wheel sets and the longitudinal axis of the chassis plane coordinate system. The vertical support force of the two left wheels is multiplied by the corresponding lateral distance to obtain the left longitudinal torque generated by the two left wheels. The vertical support force of the two right wheels is multiplied by the corresponding lateral distance to obtain the right longitudinal torque generated by the two right wheels. The left longitudinal torque and the right longitudinal torque are combined to form the torque balance equation of the vehicle body around the longitudinal axis.

[0126] It needs to be further explained that the longitudinal axis is the left and right center lines in the middle of the chassis, the transverse axis is the front and rear center lines in the middle of the chassis, and the wheelbase is the total width between the left and right wheels. Therefore, the distance from the center of the left wheel to the left and right center lines is half of the wheelbase, which is the lateral distance from the left and right wheel sets to the longitudinal axis.

[0127] To further explain, for the vehicle body to remain tilted or rollover-free, the total torque generated on the left and the total torque generated on the right must be balanced. Expressing this balance condition, where the longitudinal torque on the left equals the longitudinal torque on the right, in words is the torque balance equation of the vehicle body around its longitudinal axis.

[0128] (5) Solve the torque balance equation of the vehicle body around the longitudinal axis to obtain the torque distribution state of the vehicle body around the longitudinal axis, which reflects the load balance of the vehicle body in the left and right directions. According to the torque balance condition, calculate the projection value of the center of gravity on the horizontal axis of the chassis plane coordinate system, which is used to characterize the offset position of the center of gravity in the left and right directions of the vehicle body, that is, the horizontal coordinate of the projection position of the center of gravity.

[0129] What needs to be understood first is that the purpose of balancing the torque of the car body around the longitudinal axis is to determine how much the center of gravity is offset in the left and right directions. The left and right directions are the horizontal axis in the coordinate system, so the value obtained is the horizontal coordinate of the center of gravity projection.

[0130] It is also important to understand that the torque distribution state refers to the magnitude of the torque on the left and right sides, which side is larger, and by how much. It is not a single number, but rather a comparison of the torques on the left and right sides. This includes: if the torque on the left is greater than the torque on the right, the center of gravity is shifted to the left; if the torque on the right is greater than the torque on the left, the center of gravity is shifted to the right; if both sides are equal, the center of gravity is in the middle.

[0131] It is also important to understand that the torque balance condition is that when the vehicle body remains stable from left to right without tilting, the values ​​of the longitudinal torque on the left and right sides remain equal, and the vehicle body has no additional tendency to rotate around the longitudinal axis.

[0132] Further explanation is needed: based on the obtained left and right longitudinal moments, substitute them into the moment balance equation of the vehicle body around the longitudinal axis, and use the horizontal coordinate of the center of gravity projection as the unknown quantity to perform reverse derivation. By satisfying the balance condition that the left and right moments are equal, the unique value of the center of gravity position that meets the stable state is obtained.

[0133] (6) Using the chassis geometric center as the torque reference point, and combined with the chassis factory-calibrated wheelbase parameters, determine the longitudinal distance between the front and rear wheel sets and the transverse axis in the chassis plane coordinate system. Multiply the vertical support force of the front and rear wheel sets with the corresponding longitudinal distance to obtain the lateral torque generated by the two sets of front wheels and the two sets of rear wheels respectively. Combine the two sets of lateral torques to form the torque balance equation of the vehicle body around the transverse axis. The specific process is as described in (4).

[0134] (7) Solve the torque balance equation of the vehicle body around the transverse axis to obtain the torque distribution state of the vehicle body around the transverse axis, which reflects the load balance in the front and rear directions of the vehicle body. According to the torque balance condition, calculate the projection value of the center of gravity on the longitudinal axis of the chassis plane coordinate system, that is, the longitudinal coordinate of the center of gravity projection position. The specific process is as described in (5).

[0135] (8) Based on the relative height difference of the four wheels, obtain the real-time height value of each wheel group. According to the obtained vertical support force of the four wheels, calculate the proportion of the vertical support force of each wheel group to the total vertical support force of the four wheels. Use this proportion as the weighted calculation weight coefficient of the corresponding wheel group height. Then multiply the height value of each wheel group with the corresponding weight coefficient to obtain the weighted height value of each wheel group. Then sum the weighted height values ​​of the four wheel groups to obtain the vertical height value of the center of gravity from the chassis reference plane. After the vertical height value is corrected by the chassis reference height, wheel group unloaded radius, suspension free length and other system factory geometric calibration parameters, it is determined as the real-time center of gravity height of the vehicle body.

[0136] (9) Integrate the real-time center of gravity height of the vehicle body, the horizontal coordinate of the center of gravity projection position and the vertical coordinate of the center of gravity projection position to form complete center of gravity status data, and transmit the center of gravity status data to the chassis domain controller. At the same time, store the calculation results as the pre-data for the center of gravity update in the next sampling cycle.

[0137] S2.4: Input the vertical distance sequence and horizontal distance sequence in the step contour data into the step boundary detection algorithm. The step boundary detection algorithm uses the slope change rate threshold segmentation method to extract the rising edge and horizontal edge of each step, calculates the height difference of the rising edge as the step height, and calculates the length of the horizontal edge as the step width.

[0138] The stepped boundary detection algorithm uses a slope change rate threshold segmentation method to extract the rising edge and horizontal edge of each step, calculates the height difference of the rising edge as the step height, and calculates the length of the horizontal edge as the step width, including:

[0139] S2.4.1: Construct a discrete two-dimensional point set from the vertical distance sequence and horizontal distance sequence in the step contour data. Each point consists of horizontal distance coordinates and vertical distance coordinates. The discretization process of the sequence is prior art in this field and is not an inventive solution of this application. It will not be described in detail here.

[0140] S2.4.2: Smooth the discrete two-dimensional point set to obtain the smoothed step contour curve, calculate the tangent slope of each point on the smoothed step contour curve to obtain the slope sequence. The tangent slope is calculated by dividing the difference in vertical distance between two adjacent points by the difference in horizontal distance. Smoothing is a common technique in this field and is not an inventive solution of this application. It will not be described in detail here.

[0141] S2.4.3: Calculate the slope difference between two adjacent points in the slope sequence to obtain the slope change rate sequence. At the same time, set the rising edge threshold and the horizontal edge threshold. The rising edge threshold is 0.5 and the horizontal edge threshold is 0.05. When the slope change rate exceeds the rising edge threshold and the current slope value is positive, mark the current point as the rising edge start point. When the slope change rate is lower than the horizontal edge threshold and the absolute value of the current slope is less than 0.01, mark the current point as the horizontal edge start point.

[0142] S2.4.4: The difference in vertical distance between the starting point of the rising edge and the starting point of the horizontal edge is taken as the step height, and the difference in horizontal distance between the starting point of the horizontal edge and the starting point of the next rising edge is taken as the step width.

[0143] S3: Based on the attitude-step adaptation data, determine the motion trajectory and attitude adjustment amount required by the chassis actuator, and perform iterative optimization on the motion trajectory and attitude adjustment amount until the projection of the vehicle body center of gravity falls into the chassis stable support polygon and the attitude error is less than the first preset threshold, and output the attitude adaptation completion signal.

[0144] The process of generating the attitude adaptation completion signal includes:

[0145] S3.1: Determine the inclusion relationship between the center of gravity projection position and the chassis stability support polygon and calculate the shortest distance vector. Decompose the shortest distance vector in the opposite direction into the swing arm adjustment component and the wheel set lifting adjustment component to generate the initial attitude adjustment amount; the chassis stability support polygon is a convex quadrilateral formed by connecting the four wheel set contact points in a clockwise order.

[0146] The process of determining the inclusion relationship between the projected position of the center of gravity and the chassis stability support polygon and calculating the shortest distance vector includes:

[0147] S3.1.1: Draw a ray from the center of gravity projection point in the horizontal direction and calculate the number of intersections between the ray and each side of the chassis stabilization support polygon. If the number of intersections is odd, the center of gravity projection point is determined to be inside the chassis stabilization support polygon. If the number of intersections is zero or even, the center of gravity projection point is determined to be outside the chassis stabilization support polygon.

[0148] S3.1.2: When located outside the chassis stability support polygon, calculate the shortest perpendicular from the center of gravity projection point to the line containing each side of the polygon. If the perpendicular falls within the line segment interval, record the perpendicular distance; otherwise, record the distance from the center of gravity projection point to the line segment endpoint. Take the minimum value among all recorded distances as the shortest distance vector. The formulas for calculating the distance from a point to a line and from a point to a point are existing technologies in this field and are not inventive solutions of this application, and will not be elaborated here.

[0149] Furthermore, the inverse direction of the shortest distance vector is decomposed into a swing arm adjustment component and a wheel assembly lifting adjustment component to generate the initial attitude adjustment amount, including:

[0150] (1) Obtain the shortest distance vector, reverse the shortest distance vector to obtain the shortest distance in the opposite direction vector. This inverse direction is the target direction for vehicle posture adjustment, which can ensure that the center of gravity moves into the stable support area after adjustment.

[0151] Among them, the shortest distance vector is reversed, that is, the original direction of the shortest distance vector is reversed by 180 degrees, so that the vector direction is changed from pointing to the outside of the stable support area to pointing to the inside of the stable support area. The reversed vector direction is the target direction for vehicle posture adjustment.

[0152] (2) Obtain the chassis plane coordinate system and pre-set the decomposition weights of the swing arm adjustment component and the wheel set lifting adjustment component. The weights are factory-calibrated according to the chassis structure and attitude adjustment requirements and remain unchanged throughout the adjustment process. The weight of the swing arm adjustment component is set to 0.4 and the weight of the wheel set lifting adjustment component is set to 0.6. This ensures that the total adjustment effect of the two vector components after decomposition is consistent with the adjustment effect in the opposite direction of the original shortest distance vector.

[0153] (3) Project the shortest distance reverse direction vector in the horizontal and vertical planes of the chassis plane coordinate system. During the decomposition process, with the movement direction of the swing arm as a reference, extract the component in the shortest distance reverse direction vector used to adjust the swing arm angle. Then multiply the extracted component with the set swing arm adjustment component weight of 0.4 to obtain the swing arm adjustment component. The swing arm adjustment component can clearly characterize the direction and amplitude reference value of each swing arm to be rotated.

[0154] (4) Project the shortest distance reverse direction vector in the vertical axis direction and in the horizontal and longitudinal planes of the chassis plane coordinate system. During the decomposition process, with the lifting direction of the wheelset as a reference, extract the relevant components in the shortest distance reverse direction vector used to adjust the height of the wheelset. Then multiply the extracted components with the set weight of the lifting adjustment component of the wheelset by 0.6 to obtain the lifting adjustment component of the wheelset. The lifting adjustment component of the wheelset can clearly characterize the direction and amplitude reference value of each wheelset that needs to be lifted.

[0155] (5) Set the attitude adjustment ratio coefficient to 5. Its function is to convert the values ​​of the decomposed swing arm adjustment component and wheel set lifting adjustment component into actual executable control values ​​that conform to the swing arm rotation range and wheel set lifting range, so as to ensure that the generated adjustment amount is accurate and controllable.

[0156] (6) Multiply the swing arm adjustment component with the attitude adjustment ratio coefficient to obtain the initial adjustment amount of the swing arm. The swing arms of the left front wheel, right front wheel, left rear wheel and right rear wheel correspond to their respective swing arm adjustment components. The multiplication operation is performed independently to obtain their respective initial adjustment amounts of the swing arm, ensuring that the adjustment direction and amplitude of each swing arm can be accurately matched with the center of gravity offset direction.

[0157] (7) Multiply the wheel set lifting adjustment component with the attitude adjustment ratio coefficient to obtain the initial lifting adjustment amount of the wheel set. The four wheel sets correspond to their respective wheel set lifting adjustment components, and the multiplication operation is performed independently to obtain their respective initial lifting adjustment amounts of the wheel set, ensuring that the lifting direction and amplitude of each wheel set can assist in adjusting the center of gravity position.

[0158] (8) Integrate the initial adjustment amounts of the four swing arms with the initial adjustment amounts of the four wheel sets to form a complete initial attitude adjustment amount. This initial attitude adjustment amount includes the rotation adjustment parameters of each swing arm and the lifting adjustment parameters of each wheel set, which clarifies the specific direction and amplitude of the attitude adjustment. At the same time, store the initial attitude adjustment amount generated this time.

[0159] S3.2: Using the step height and step width as the constraint boundaries of the motion trajectory, the fifth-order polynomial interpolation method is used to plan the change curves of the vehicle pitch angle and the change curves of the vehicle roll angle to generate the motion trajectory;

[0160] Furthermore, the specific steps in S3.2 include:

[0161] (1) Read the step height and step width and determine them as the constraint boundary parameters for vehicle motion trajectory planning. The step height is used to constrain the maximum adjustment range of the vehicle pitch angle to avoid the vehicle from tilting forward or backward and becoming unbalanced due to excessive pitch angle adjustment. The step width is used to constrain the adjustment range of the vehicle roll angle to prevent the roll angle from exceeding the safety threshold and causing the vehicle to roll over.

[0162] (2) Read the current actual pitch angle and the current actual roll angle of the vehicle body, and use them as the reference values ​​for the initial moment of pitch angle and roll angle trajectory planning, respectively, to ensure that the trajectory planning starts from the current actual attitude of the vehicle body and avoid trajectory misalignment caused by initial attitude deviation;

[0163] (3) Based on the step height constraint boundary parameters and the current actual pitch angle value of the vehicle body, set the complete boundary conditions for pitch angle trajectory planning, including: First, set the current actual pitch angle value of the vehicle body to the initial pitch angle value; at the same time, set the first derivative value of the pitch angle at the initial moment to zero, which represents that the instantaneous velocity of the vehicle body pitch motion at the initial moment is zero, and the second derivative value of the pitch angle at the initial moment to zero, which represents that the instantaneous acceleration of the vehicle body pitch motion at the initial moment is zero; then, combine the step height constraint boundary and the vehicle body attitude stability The requirements are defined, the target pitch angle value at the termination time is determined, and the first derivative value of the pitch angle at the termination time and the second derivative value of the pitch angle at the termination time are set to zero to ensure that the vehicle pitch motion stops smoothly at the termination time without impact or vibration. The vehicle pitch angle value at the initial time, the first derivative value of the pitch angle at the initial time, the second derivative value of the pitch angle at the initial time, the target pitch angle value at the termination time, the first derivative value of the pitch angle at the termination time, and the second derivative value of the pitch angle at the termination time together constitute the complete boundary conditions for pitch angle trajectory planning.

[0164] (4) Using the set pitch angle trajectory planning boundary conditions as input, the preset fifth-order polynomial coefficient solution formula is called to calculate the six polynomial coefficients of the pitch angle change curve. During the solution process, the determined step height constraint boundary requirements are strictly followed to ensure that the six polynomial coefficients obtained can make the generated pitch angle change curve meet the constraint requirements. Subsequently, based on the six polynomial coefficients obtained, a pitch angle trajectory curve that changes continuously with time is generated. The pitch angle trajectory curve clearly represents the smooth change process of the vehicle pitch angle from the actual value at the initial moment to the target value at the final moment. The fifth-order polynomial coefficient solution formula and its solution process are existing technical contents in this field and are not the inventive solution of this application. They will not be described in detail here.

[0165] (5) Based on the step width constraint boundary parameters and the current actual roll angle value of the vehicle body, set the complete boundary conditions for roll angle trajectory planning, including: First, set the current actual roll angle value of the vehicle body to the actual roll angle value of the vehicle body at the initial moment; at the same time, set the first derivative value of the roll angle at the initial moment to zero, which represents that the instantaneous velocity of the vehicle body roll motion at the initial moment is zero, and the second derivative value of the roll angle at the initial moment to zero, which represents that the instantaneous acceleration of the vehicle body roll motion at the initial moment is zero; then, in combination with the step width constraint boundary and the vehicle body attitude stability requirements, determine the target roll angle value at the termination moment, and set the first derivative value of the roll angle at the termination moment to zero and the second derivative value of the roll angle at the termination moment to zero, so as to ensure that the vehicle body roll motion stops smoothly at the termination moment without impact or shaking, and obtain the complete boundary conditions for roll angle trajectory planning.

[0166] (6) The six polynomial coefficients of the roll angle change curve are calculated by using the fifth-order polynomial coefficient solution formula. Based on the six polynomial coefficients obtained, a roll angle trajectory curve that changes continuously with time is generated. This roll angle trajectory curve represents the smooth change process of the vehicle roll angle from the actual value at the initial moment to the target value at the final moment.

[0167] (7) Align the pitch angle trajectory curve and the roll angle trajectory curve precisely along the same time axis to ensure that the time parameters of the two curves are completely consistent at each time point, and avoid trajectory confusion caused by time misalignment. After alignment, merge the pitch angle trajectory curve and the roll angle trajectory curve to form a six-dimensional motion trajectory vector. The six-dimensional motion trajectory vector contains the corresponding pitch angle expectation value and roll angle expectation value at each time point, which fully represents the attitude change trajectory of the vehicle body in the step scene.

[0168] (8) The obtained six-dimensional motion trajectory vector is transmitted to the chassis domain controller as the control basis of the vehicle body posture adjustment actuator, which is used to control the rotation of the swing arm and the lifting of the wheel set to realize the smooth passage of the vehicle body over the step; the vehicle body posture adjustment actuator includes the swing arm and the wheel set.

[0169] S3.3: Apply the initial attitude adjustment to the current chassis actuator model, and iteratively calculate the center of gravity projection position and attitude error after each adjustment. In each iteration, update the nonlinear mapping relationship of suspension stiffness according to the adjusted swing arm stroke, and update the predicted value of wheel set displacement data according to the adjusted wheel set speed.

[0170] Furthermore, the process of iteratively calculating the adjusted center of gravity projection position and attitude error at each step using the Newton-Euler inverse dynamics algorithm includes:

[0171] (1) Call the current chassis actuator model. The chassis actuator model includes the body rigid body, left front swing arm rigid body, right front swing arm rigid body, left rear swing arm rigid body, right rear swing arm rigid body, left front wheel assembly rigid body, right front wheel assembly rigid body, left rear wheel assembly rigid body and right rear wheel assembly rigid body. The rigid bodies are connected to the spring damping unit through rotational hinges to form a complete chassis multi-rigid body structure.

[0172] (2) Select the current swing arm stroke value, suspension stiffness value and wheel set speed value as system state variables, where the swing arm stroke value corresponds to the motion stroke of each swing arm rigid body, the suspension stiffness value corresponds to the stiffness parameter of the spring damping unit between each rigid body, and the wheel set speed value corresponds to the rotation parameter of each wheel set rigid body.

[0173] (3) Based on the chassis multi-rigid body structure, combined with the determined system state variables, a chassis multi-rigid body dynamic model is constructed. The chassis multi-rigid body dynamic model can accurately characterize the spatial position relationship, motion characteristics and interaction between components of each rigid body, clarify the connection logic of each rotating hinge and spring damping unit, and fully reflect the dynamic characteristics of the chassis actuator.

[0174] Furthermore, it can be understood that the chassis multi-rigid-body structure includes the body rigid body, the left front / right front / left rear / right rear swing arm rigid bodies, and the left front / right front / left rear / right rear wheel set rigid bodies; the connection relationship is: the rigid bodies are hinged through rotational hinges and elastically connected through suspension spring damping units; the chassis multi-rigid-body dynamics model is configured hierarchically, including a rigid body layer, a constraint connection layer, and a dynamics calculation layer. The rigid body layer contains nine rigid body units, including the body rigid body, four sets of swing arm rigid bodies, and four sets of wheel set rigid bodies, and each rigid body defines a local coordinate system. The system pre-stores inherent parameters of the rigid body, including mass, center of mass position, moment of inertia, and geometric dimensions. The geometric dimensions include the swing arm length, wheel radius, and hinge point coordinates; all parameters are factory-calibrated. The constraint connection layer includes rotary hinge units and suspension spring damping units. Each of the four swing arm rigid bodies has one rotary hinge, a single-degree-of-freedom revolute joint, allowing only rotation of the swing arm around the vertical axis of the hinge point. This constrains the translational degree of freedom between the swing arm and the body, transmitting hinge constraint forces and moments. Each control arm rigid body is connected to its corresponding wheel assembly rigid body by a suspension spring damping unit, forming an elastic damping connection pair containing a linear spring and a viscous damper, transmitting vertical elastic force and damping force. The dynamic calculation layer is constructed based on the Newton-Euler dynamic equations, inputting the forces acting on each rigid body, such as hinge constraint force, spring damping force, gravity, and wheel assembly driving force, and outputting the rigid body motion state. The Newton-Euler dynamic equations are existing technology in this field and are not an inventive solution of this application, so they will not be elaborated here. The chassis multi-rigid-body dynamic model takes as input system state quantities such as control arm travel, suspension stiffness, wheel assembly speed and torque, and outputs the spatial pose of each rigid body, hinge constraint force, vehicle center of gravity position, and pitch / roll attitude angle. The chassis multi-rigid-body dynamic model characterizes the hinge constraint relationship between rigid bodies through rotational hinge units and characterizes the elastic support and damping characteristics through suspension spring damping units, accurately characterizing the spatial position relationship, motion characteristics, and inter-component interaction of each rigid body, and fully reflecting the dynamic characteristics of the chassis actuator.

[0175] (4) Apply the initial attitude adjustment amount to the chassis actuator model to drive each swing arm rigid body and wheel assembly rigid body to produce initial action, so that the chassis attitude is initially adjusted. At the same time, set the threshold for the change in the center of gravity projection position to 0.5 mm, and the threshold for the pitch angle error and the threshold for the roll angle error to 0.1 degrees to determine whether the iteration has converged. Initialize the number of iterations to zero, and synchronously read the center of gravity projection position, vehicle pitch angle and vehicle roll angle at the beginning of the current iteration.

[0176] (5) In each iteration, based on the current applied attitude adjustment amount and combined with the constructed chassis multi-rigid-body dynamics model, update the position coordinates and attitude angles of each rigid body in the chassis actuator model in space to ensure that the position and attitude of each rigid body can accurately reflect the current adjustment state. The updated spatial position coordinates and attitude angles of each rigid body are directly used as the input for the next step of calculating the hinge point constraint force and constraint torque.

[0177] (6) Using the updated spatial position coordinates and attitude angles of each rigid body as input, the Newton-Euler recursive method is used to calculate from the base rigid body to the end rigid body in sequence, that is, from the body rigid body to each wheel group rigid body in sequence, to solve the constraint force and constraint moment at each rotational hinge. The recursive process uses Newton's equation and Euler's equation simultaneously. Newton's equation for each rigid body is used to describe the translational dynamics of its center of mass, and Euler's equation for each rigid body is used to describe its rotational dynamics about its center of mass, to ensure that the calculated constraint force and constraint moment can accurately reflect the force state between each component. Newton's equation and Euler's equation are existing technologies in this field and are not the inventive solution of this application, and will not be elaborated here.

[0178] (7) Using the constraint forces and constraint moments of each hinge point as input, and combining the structural characteristics of the constructed chassis multi-rigid body dynamics model, the center of gravity height and center of gravity projection coordinates of the entire system under static equilibrium conditions are re-solved. At the same time, the vehicle pitch angle and vehicle roll angle under the current state are calculated simultaneously. The re-solved center of gravity parameters and attitude parameters are used as target values ​​for iterative comparison.

[0179] (8) Compare the re-solved center of gravity projection position with the center of gravity projection position read at the beginning of the current iteration, and calculate the change in center of gravity projection position; compare the re-solved vehicle pitch angle with the read initial vehicle pitch angle, and calculate the pitch angle error; compare the re-solved vehicle roll angle with the read initial vehicle roll angle, and calculate the roll angle error; then, compare the change in center of gravity projection position with the set center of gravity projection position change threshold of 0.5mm, and compare the pitch angle error and roll angle error with the pitch angle error threshold of 0.1 degrees and the roll angle error threshold of 0.1 degrees, respectively. If the change in center of gravity projection position is less than the center of gravity projection position change threshold, and the pitch angle error and roll angle error are both less than the pitch angle error threshold and the roll angle error threshold, then the current iteration is determined to be converged and the iteration calculation is stopped; otherwise, the iteration is determined to be unconverged.

[0180] (9) If the iteration is determined to be non-convergent, the current attitude adjustment amount is fine-tuned based on the calculated change in the center of gravity projection position, pitch angle error and roll angle error, combined with the characteristics of the constructed chassis multi-rigid body dynamics model, to obtain the adjusted attitude adjustment amount. Then, the adjusted attitude adjustment amount is used as the new input, and (5)-(8) are repeated to update the rigid body position, calculate the constraint force, solve the center of gravity and attitude parameters, and determine the convergence state until the iteration convergence condition is met.

[0181] (10) When the iteration converges, the final coordinates of the center of gravity projection position, the center of gravity height, the vehicle pitch angle, and the roll angle are output as the final reference for vehicle attitude adjustment and transmitted to the chassis domain controller.

[0182] S3.4: When the center of gravity projection position enters the interior of the chassis stable support polygon and the attitude error is less than the first preset threshold during the iteration process, the iteration stops and an attitude adaptation completion signal is generated; the attitude adaptation completion signal includes the final target value of the swing arm stroke, the target value of the suspension stiffness and the target value of the wheel set speed.

[0183] Furthermore, the first preset threshold is dynamically adjusted based on the ratio of the step height to the chassis length. Specifically, this includes: real-time acquisition of the current step height and chassis length, and calculation of the ratio of step height to chassis length; when the ratio is greater than 0.15, the current staircase is determined to be a steep staircase, the first preset threshold is set to 0.5 degrees, and a high-precision mode is used for attitude control; when the ratio is less than 0.05, the current staircase is determined to be a gentle staircase, the first preset threshold is set to 2.0 degrees, and a fast-passing mode is used for attitude control; when the ratio is greater than or equal to 0.05 and less than or equal to 0.15, the current staircase is determined to be a normal staircase, the first preset threshold is set to 1.0 degrees, and a standard mode is used for attitude control.

[0184] S4: Based on the received attitude adaptation completion signal, control the chassis actuator to perform actions according to the hierarchical collaborative logic, collect chassis attitude feedback data in real time during the climbing process, and dynamically correct the attitude adjustment amount based on the chassis attitude feedback data for iterative optimization.

[0185] The step of controlling the chassis actuators to perform actions according to the hierarchical collaborative logic based on the received attitude adaptation completion signal includes:

[0186] The target value of the swing arm stroke in the attitude adaptation completion signal is sent to the swing arm execution layer. The swing arm execution layer uses a proportional-integral-derivative controller to drive the electric push rod to move to the target stroke according to the trapezoidal velocity curve. The target value of the suspension stiffness is sent to the suspension adjustment layer. The suspension adjustment layer adjusts the oil volume of the suspension air spring by controlling the solenoid valve to change the equivalent stiffness. The target value of the wheel set speed is sent to the wheel set drive layer. The wheel set drive layer uses a field-oriented control algorithm to drive the brushless DC motor to reach the target speed. In the hierarchical collaborative logic, the swing arm execution layer takes precedence over the suspension adjustment layer, the suspension adjustment layer takes precedence over the wheel set drive layer, and the layers handshake their status with synchronization semaphores. After the previous layer completes its action and locks its position, it triggers the action of the next layer.

[0187] Furthermore, the specific process of using a proportional-integral-derivative controller to drive the electric linear actuator to move along a trapezoidal velocity curve to the target stroke includes:

[0188] (1) Read the target value of the swing arm stroke and the actual value of the current swing arm stroke corresponding to the electric push rod collected in real time by the swing arm stroke sensor. The actual value of the current swing arm stroke reflects the current actual position of the swing arm.

[0189] (2) Calculate the difference between the target value of the swing arm stroke and the actual value of the current swing arm stroke, and determine the difference as the error input of the proportional-integral-derivative controller;

[0190] (3) Enable the preset proportional-integral-derivative controller. The proportional-integral-derivative controller is used to generate the control signal of the electric push rod according to the error input. It consists of three parts: proportional link, integral link and derivative link. Each link adopts the fixed gain parameters calibrated by the factory. The proportional gain of the proportional link is calibrated to 5, the integral gain of the integral link is calibrated to 0.2, and the derivative gain of the derivative link is calibrated to 0.1.

[0191] (4) Input the obtained error input to the proportional-integral-derivative controller, and calculate the corresponding control quantity through the three links of the controller. The proportional link multiplies the error input by the proportional gain to generate the proportional control quantity, which is used to quickly respond to the swing arm stroke deviation. The integral link multiplies the cumulative amount of the error input over time by the integral gain to generate the integral control quantity, which is used to eliminate the long-term stroke deviation. The derivative link multiplies the rate of change of the error input by the derivative gain to generate the derivative control quantity, which is used to suppress the fluctuation during the swing arm movement.

[0192] (5) The proportional control quantity, integral control quantity and derivative control quantity are summed to obtain the torque command of the electric push rod motor. This torque command is the power control signal of the electric push rod and is directly used to drive the electric push rod to move the swing arm to the target stroke position.

[0193] (6) A trapezoidal speed curve is preset for controlling the operation of the electric actuator. The trapezoidal speed curve is divided into three continuous stages: a uniform acceleration stage, a uniform speed stage, and a uniform deceleration stage. The parameters of each stage are fixed values ​​calibrated at the factory to ensure the smooth operation of the electric actuator. The acceleration of the uniform acceleration stage is preset based on the ratio of the maximum thrust of the electric actuator to the load mass and is set to 50 mm / s. 2 The absolute value of the deceleration in the uniformly decelerated section is equal to the absolute value of the acceleration in the uniformly accelerated section, which is 50 mm / s². 2 To ensure symmetrical and smooth acceleration and deceleration, the speed of the constant speed section is set as the maximum safe speed of the electric actuator, calibrated to 100 mm / s. At the same time, a deceleration threshold is set, which is calculated based on the relationship between the current running speed of the electric actuator and the square of the deceleration in the constant speed section. It represents the minimum remaining travel distance required for the electric actuator to smoothly decelerate from the constant speed state to a stop, and is used to determine when the electric actuator switches to the constant speed section to ensure that the electric actuator can smoothly decelerate to a stop.

[0194] (7) Based on the generated torque command, the electric push rod is controlled to start running according to the set trapezoidal speed curve. In the initial stage, the electric push rod accelerates uniformly according to the acceleration of the uniform acceleration section until the running speed reaches the maximum safe speed of the uniform speed section. Then it enters the uniform speed section, and the electric push rod runs at a constant speed of 100 mm / s, continuously approaching the target value of the swing arm stroke. During the operation, the remaining swing arm stroke distance is calculated in real time. When the remaining swing arm stroke distance is less than the set deceleration threshold, the control logic switches from the uniform speed section to the uniform deceleration section, and the electric push rod runs at a constant speed of 50 mm / s. 2 The deceleration is uniform to avoid impact caused by sudden stopping;

[0195] (8) Real-time acquisition of the actual value of the current swing arm stroke corresponding to the electric push rod, comparison of the actual value of the current swing arm stroke with the read target value of the swing arm stroke, and reference to the preset stroke error threshold, which is set to 0.1mm in this embodiment. If the difference between the actual value of the current swing arm stroke and the target value of the swing arm stroke is less than the stroke error threshold, it is determined that the electric push rod has moved to the target stroke, and the electric push rod is immediately controlled to stop running; if the target stroke is not reached, (4)-(7) are repeated until the swing arm stroke reaches the target requirement.

[0196] (9) After the electric push rod moves to the target stroke and stops running, it outputs a control completion signal and feeds it back to the chassis domain controller to inform that the swing arm has been successfully adjusted to the target position of the swing arm stroke.

[0197] Furthermore, the specific process by which the wheel drive layer uses a field-oriented control algorithm to drive the brushless DC motor to achieve the target speed includes:

[0198] (1) The three-phase current signal of the brushless DC motor is collected in real time through the current sampling circuit. The three-phase current signal directly reflects the current load state and operating current of the motor. At the same time, the rotor position signal of the motor is collected in real time through the rotor position sensor built into the brushless DC motor. The rotor position signal is used to provide the rotation angle required during the coordinate transformation process.

[0199] (2) The three-phase current signal is converted into the first current component and the second current component in the two-phase stationary coordinate system by the Clark transformation, so as to realize the dimension reduction conversion of the three-phase electrical quantity to the two-phase electrical quantity. The Clark transformation is the prior art in this field and is not an inventive solution of this application. It will not be described in detail here.

[0200] (3) Combining the rotation angle corresponding to the rotor position signal, the first current component and the second current component in the two-phase stationary coordinate system are converted into the direct-axis current component and the quadrature-axis current component in the two-phase rotating coordinate system by Park transformation. The direct-axis current component corresponds to the excitation component of the brushless DC motor, and the quadrature-axis current component corresponds to the torque component of the motor. The Park transformation is a prior art in this field and is not an inventive solution of this application. It will not be elaborated here.

[0201] (4) Read the target value of the wheel set speed, and at the same time collect the current actual speed value of the wheel set corresponding to the output terminal of the brushless DC motor. Calculate the speed deviation between the target value of the wheel set speed and the current actual speed value, and use the speed deviation as the input of the speed loop proportional integral derivative controller.

[0202] (5) Activate the preset speed loop proportional-integral-derivative controller, which takes the speed deviation as input and outputs the quadrature-axis current reference value after internal calculation;

[0203] (6) Set the direct-axis current reference value to 0. At the same time, construct two independent current control structures, namely the direct-axis current control branch and the quadrature-axis current control branch. The first proportional-integral controller is used for the closed-loop regulation of the direct-axis current, and the second proportional-integral controller is used for the closed-loop regulation of the quadrature-axis current. All parameters of the two controllers are fixed values ​​calibrated at the factory and are not adjusted throughout the process.

[0204] (7) The direct-axis current component and the direct-axis current reference value are input to the first proportional-integral controller. The direct-axis current component tracks the direct-axis current reference value through closed-loop control, and finally outputs the direct-axis voltage command. At the same time, the quadrature-axis current component and the quadrature-axis current reference value are input to the second proportional-integral controller. The quadrature-axis current component tracks the quadrature-axis current reference value through closed-loop control, and finally outputs the quadrature-axis voltage command.

[0205] (8) Using the obtained direct-axis voltage command and quadrature-axis voltage command as input, and combining the rotation angle corresponding to the acquired rotor position signal, the direct-axis voltage command and quadrature-axis voltage command in the two-phase rotating coordinate system are converted into the first voltage component and the second voltage component in the two-phase stationary coordinate system through the inverse Park transformation. This conversion process is the inverse operation of the Park transformation in (3). The inverse Park transformation is the prior art in this field and is not an inventive solution of this application. It will not be described in detail here.

[0206] (9) The first voltage component and the second voltage component in the two-phase stationary coordinate system are used as inputs, and the signal is converted by the vector pulse width modulation method to generate the pulse width modulation signal of the six switching transistors inside the three-phase inverter. The pulse width modulation signal is directly used to control the working state of the three-phase inverter, thereby driving the brushless DC motor to rotate, so that the speed of the wheel set gradually approaches the target speed. The vector pulse width modulation method is the prior art in this field and is not an inventive solution of this application, so it will not be described in detail here.

[0207] (10) Repeatedly execute (1)-(9) to continuously collect feedback signals such as the three-phase current, rotor position and actual speed of the brushless DC motor, and dynamically adjust the speed deviation, current reference value, voltage command and pulse width modulation signal to ensure that the speed of the wheel set is stably tracked by the target speed.

[0208] The process of collecting chassis attitude feedback data in real time during stair climbing, and dynamically correcting the attitude adjustment amount based on the chassis attitude feedback data for iterative optimization, includes:

[0209] S4.1: During the stair climbing process, inertial tilt angle data, wheel set displacement data and swing arm stroke data are collected at a fixed sampling period to generate chassis attitude feedback data;

[0210] Furthermore, the duration of the fixed sampling period is dynamically set according to the ratio of the response speed of the chassis actuator to the step period. Specifically, it includes: real-time acquisition of the total response time of the electric push rod of the swing arm actuator layer from zero stroke to full stroke, dividing the total response time by 10 to obtain the basic sampling period; acquisition of the acceleration time of the brushless DC motor of the wheel drive layer from zero speed to maximum speed, dividing the acceleration time by 8 to obtain the alternative sampling period; acquisition of the action time of the solenoid valve of the suspension adjustment layer from fully closed to fully open, dividing the action time by 6 to obtain the compensation sampling period; taking the maximum value of the basic sampling period, the alternative sampling period, and the compensation sampling period as the fixed sampling period, and setting the upper limit of the fixed sampling period to 50 milliseconds and the lower limit to 10 milliseconds.

[0211] S4.2: Compare the real-time attitude value in the chassis attitude feedback data with the expected attitude value at the current moment in the motion trajectory to calculate the attitude deviation vector; the attitude deviation vector includes pitch angle deviation, roll angle deviation, center of gravity height deviation and center of gravity projection position deviation. A positive deviation value indicates that the real-time attitude value is greater than the expected attitude value, and a negative deviation value indicates that the real-time attitude value is less than the expected attitude value. The larger the absolute value of the deviation, the more serious the attitude deviation.

[0212] S4.3: Input the attitude deviation vector into the model predictive controller. The prediction time domain length of the model predictive controller is an integer multiple of the step period, and the control time domain length is a single step period. The rolling optimization method is used to solve the control increment sequence that minimizes the quadratic cost function between the predicted attitude and the reference trajectory. The first control increment of the control increment sequence is extracted as the correction value of the attitude adjustment.

[0213] Furthermore, the specific steps of S4.3 include:

[0214] (1) Select the state variable vector; the state variable vector includes seven state variables: vehicle pitch angle, vehicle pitch rate, vehicle roll angle, vehicle roll rate, center of gravity height, center of gravity height change rate, and wheel set speed deviation.

[0215] (2) Select the control variable vector; the control variable vector includes three control variables: the swing arm travel adjustment rate, the suspension stiffness adjustment rate, and the wheel set torque command;

[0216] (3) Using the state variable vector and the control variable vector as input, construct a linearized state space equation; the state space equation includes a state transition matrix and a control matrix. The state transition matrix is ​​used to describe the linear mapping relationship between the state variables at the current time and the state variables at the next time, and the control matrix is ​​used to describe the linear mapping relationship between the control variables and the rate of change of the state variables.

[0217] (4) Set the prediction time domain length of the model predictive controller to three complete step cycles, and dynamically calculate the duration of a single step cycle based on the ratio of the step width to the current wheel speed. This prediction time domain can cover the operation process of three consecutive steps.

[0218] (5) Set the control time domain length of the model predictive controller to a single step cycle to keep the control time domain consistent with the running time of a single step. Then construct a quadratic cost function, which consists of two weighted calculation contents. The first part is the weighted sum of squares of the state deviation vectors at each moment in the prediction time domain. The state deviation vectors are obtained by the difference operation between the state variable vector and the constructed reference trajectory vector. The second part is the weighted sum of squares of the control increment vectors at each moment in the control time domain. The control increment vectors are obtained by the difference operation between the control variable at the current moment and the control variable at the previous moment. The weighting coefficient of the state deviation is 10 as specified by the factory and the weighting coefficient of the control increment is 0.1 as specified by the factory.

[0219] Furthermore, the reference trajectory vector is constructed based on the motion trajectory generated in S3.2. In S3.2, the variation curves of vehicle pitch angle and vehicle roll angle are planned using a fifth-order polynomial interpolation method with step height and step width as constraint boundaries. The reference trajectory vector includes the reference values ​​of pitch angle and roll angle corresponding to these two curves. At the same time, it supplements the reference values ​​of the other five physical quantities in the state variable vector, namely, vehicle pitch rate, vehicle roll rate, center of gravity height, rate of change of center of gravity height, and wheel set speed deviation. The reference values ​​of the other five physical quantities are all factory calibrated according to the vehicle attitude stability requirements. Among them, the reference values ​​of vehicle pitch rate and vehicle roll rate are calibrated to 0 rad / s, the reference value of center of gravity height is calibrated to a fixed value of 500 mm, the reference value of center of gravity height change rate is calibrated to 0 mm / s, and the reference value of wheel set speed deviation is calibrated to 0 r / min, ensuring that the reference trajectory vector corresponds one-to-one with the seven state variables selected in the first step.

[0220] (6) Set the physical operation constraints of the chassis actuator, wherein the upper limit of the swing arm stroke is set to 150mm and the lower limit is set to 0mm, the upper limit of the suspension stiffness is set to 10kN per meter and the lower limit is set to 5kN per meter, and the upper limit of the wheel set speed is set to 120r / min and the lower limit is set to 0r / min.

[0221] (7) At each sampling time, based on the state space equation, with the quadratic cost function as the optimization objective and the set actuator constraints as the solution boundary, the quadratic programming solver is used to calculate online to obtain the optimal control increment sequence that minimizes the quadratic cost function. The quadratic programming solver is the prior art in this field and is not an inventive solution of this application. It will not be described in detail here.

[0222] (8) Extract the first control increment from the obtained optimal control increment sequence as the control variable output at the current moment. The output value is the correction value of the attitude adjustment amount. Send the correction value to the chassis actuator and update the state variable vector and control variable vector at the current moment.

[0223] S4.4: Add the correction value to the current attitude adjustment to generate the corrected attitude adjustment, and use it as the initial attitude adjustment for the next iteration.

[0224] Furthermore, the specific steps of S4.4 include:

[0225] (1) Establish an iteration counter and an iteration cache queue. The depth of the iteration cache queue is 5, which is used to store the attitude adjustment vector and the corresponding attitude error scalar value generated in the last 5 iterations.

[0226] (2) When the model predicts the controller outputs the corrected attitude adjustment amount, the corrected attitude adjustment amount is weighted and averaged with the attitude adjustment amount used in the current iteration step. The weighted average coefficient is calculated as follows: extract the attitude error values ​​of the two most recent ones in the iteration buffer queue. If the attitude error value of the latter is less than the attitude error value of the former, it is determined that the attitude error is in a continuous decreasing trend. The weight coefficient of the corrected attitude adjustment amount is set to 0.2 and the weight coefficient of the current attitude adjustment amount is set to 0.8. If the attitude error value of the latter is greater than the attitude error value of the former, it is determined that the attitude error is in an increasing or oscillating trend. The weight coefficient of the corrected attitude adjustment amount is set to 0.8 and the weight coefficient of the current attitude adjustment amount is set to 0.2.

[0227] (3) Write the attitude adjustment vector obtained after weighted average calculation to the tail of the iterative cache queue, and remove the oldest data at the head of the queue. Use the attitude adjustment vector obtained after weighted average calculation as the initial attitude adjustment input value when calling the iterative optimization step next time.

[0228] Example 2:

[0229] Please see Figure 3 Another embodiment of the present invention provides: a chassis control system for a stair-climbing robot vehicle based on attitude adjustment, comprising:

[0230] The data acquisition module 10 is used to collect multi-sensor fusion data and initial state parameters of the actuator of the stair climbing robot chassis through devices such as inertial measurement unit, magnetic encoder, laser rangefinder sensor array, Hall effect position sensor, force-sensitive resistor and spring compression displacement sensor. After complementary filtering, mechanical deformation compensation and slip ratio correction, the data is collected to the chassis domain controller through CAN bus and stored according to the predefined memory mapping table.

[0231] The parameter calculation module 20 takes multi-sensor fusion data and initial state parameters of the chassis actuator as input, and uses algorithms such as spatial four-point support plane fitting, least squares method, and step boundary detection to calculate real-time attitude data such as vehicle pitch angle, roll angle, center of gravity height, and center of gravity projection position, as well as geometric parameters such as step height and width. After cross-validation and weighted fusion, attitude-step adaptation data is obtained.

[0232] The attitude iteration optimization module 30 determines the motion trajectory and attitude adjustment amount of the chassis actuator based on the attitude-step adaptation data. With the goal of the center of gravity projection falling into the chassis stable support polygon and the attitude error being less than the first preset threshold, it updates the nonlinear mapping relationship of suspension stiffness and the predicted value of wheel set displacement through iterative calculation. After the iteration reaches the target, it outputs an attitude adaptation completion signal containing the target value of swing arm travel, the target value of suspension stiffness, and the target value of wheel set speed.

[0233] The dynamic correction module 40 is used to receive the attitude adaptation completion signal and drive the actuator to move according to the hierarchical collaborative logic of the swing arm execution layer first, the suspension adjustment layer next, and the wheel group drive layer last. Each layer realizes state handshake and position locking through synchronous semaphores. During the stair climbing process, attitude feedback data is collected at a fixed sampling period. The model predictive controller is used to solve the control increment and dynamically correct the attitude adjustment amount to realize closed-loop optimization control throughout the stair climbing process.

[0234] The embodiments of the present invention have been described above with reference to the accompanying drawings. However, the present invention is not limited to the specific embodiments described above. The specific embodiments described above are merely illustrative and not restrictive. Those skilled in the art can make changes, modifications, substitutions and variations to the above embodiments under the guidance of the present invention without departing from the spirit and scope of the present invention. All of these variations are within the protection scope of the present invention.

Claims

1. A chassis control method for a stair-climbing robot vehicle based on attitude adjustment, characterized in that, include: Acquire multi-sensor fusion data of the stair-climbing robot chassis and initial state parameters of the chassis actuators; Based on the multi-sensor fusion data and the initial state parameters of the chassis actuator, the real-time attitude data of the vehicle body and the geometric parameters of the step are calculated to obtain attitude-step adaptation data; the attitude-step adaptation data includes the vehicle body pitch angle, roll angle, center of gravity height, center of gravity projection position, and step height and width. Based on the attitude-step adaptation data, the motion trajectory and attitude adjustment amount of the chassis actuator are determined, and the motion trajectory and attitude adjustment amount are iteratively optimized until the center of gravity projection of the vehicle body falls into the chassis stable support polygon and the attitude error is less than the first preset threshold, and the attitude adaptation completion signal is output. Based on the received attitude adaptation completion signal, the chassis actuator is controlled to perform actions according to the hierarchical collaborative logic. During the stair climbing process, chassis attitude feedback data is collected in real time, and the attitude adjustment amount is dynamically corrected based on the chassis attitude feedback data for iterative optimization.

2. The chassis control method for a stair-climbing robot vehicle based on attitude adjustment as described in claim 1, characterized in that, The acquisition of multi-sensor fusion data of the stair-climbing robot chassis and initial state parameters of the chassis actuators includes: The inertial measurement unit (IMU) collects triaxial acceleration and angular velocity data of the vehicle body, and a complementary filtering algorithm is used to fuse the triaxial acceleration and angular velocity data to generate inertial tilt angle data. A magnetic encoder mounted on the wheel drive motor shaft collects the number of motor rotations, calculates the actual linear displacement after mechanical deformation compensation, and generates wheel displacement data. Laser rangefinder arrays mounted at the front and rear of the chassis collect the reflection time difference between the vertical and horizontal planes of the steps, calculate the vertical and horizontal distances from the sensor probes to the step edges, and generate step contour data. A Hall effect position sensor collects the rotation angle of the swing arm joint and converts it into… Linear travel generates swing arm travel data; based on force-sensitive resistors and spring compression displacement sensors, the compression and rebound of the suspension springs under static and dynamic conditions are collected, and the equivalent elastic coefficient is calculated to generate suspension stiffness data; based on the number of pulses per unit time collected by the magnetic encoder, the wheel set speed data is generated through counting and time window averaging; the inertial tilt angle data, wheel set displacement data, step profile data, swing arm travel data, suspension stiffness data, and wheel set speed data are collected into a data structure in the chassis domain controller through the controller local area network bus, and the data structure stores the timestamp and valid flag of each data according to a predefined memory mapping table.

3. The chassis control method for a stair-climbing robot vehicle based on attitude adjustment as described in claim 2, characterized in that, The calculation process for the actual linear displacement after mechanical deformation compensation includes: The nominal displacement is obtained by multiplying the original pulse count output by the magnetic encoder by the wheel set calibration circumference. The strain values ​​output by strain gauges mounted on the wheel set suspension are collected, and the current vertical load is obtained by querying the pre-stored calibration curve of the strain value and the wheel set vertical load. The tire compression radius is calculated based on the current vertical load and the tire radial stiffness coefficient, where the tire compression radius is equal to the tire unloaded radius minus the ratio of the current vertical load to the tire radial stiffness coefficient. The nominal displacement is corrected using the tire compression radius by multiplying the nominal displacement by the ratio of the wheel set calibration circumference to the compressed circumference, where the compressed circumference is equal to twice pi multiplied by the tire compression radius. The swing arm travel data and wheel set deflection angle data are collected, and the longitudinal slip ratio caused by the wheel set kinematic deformation is calculated based on the geometric relationship between the swing arm travel and the wheel set deflection angle. The first corrected displacement is corrected using the longitudinal slip ratio by multiplying the first corrected displacement by a factor minus the longitudinal slip ratio to obtain the actual linear displacement.

4. The chassis control method for a stair-climbing robot vehicle based on attitude adjustment as described in claim 2, characterized in that, Based on the multi-sensor fusion data and the initial state parameters of the chassis actuator, the real-time attitude data of the vehicle body and the geometric parameters of the steps are calculated to obtain attitude-step adaptation data, including: The pitch and roll axis angle components from the inertial tilt angle data are directly assigned as the vehicle pitch and roll angles, respectively. The displacements of the left front wheel, right front wheel, left rear wheel, and right rear wheel from the wheel set displacement data are input into a four-point support plane fitting algorithm. This algorithm uses the least squares method to solve the spatial plane equations of the four wheel contact points, obtaining attitude verification values. These values ​​are then cross-validated with the inertial tilt angle data. When the deviation exceeds a second preset threshold, a Kalman filter is used to weight and fuse the vehicle pitch and roll angles to obtain the corrected vehicle pitch. The angle and the corrected body roll angle; based on the relative height difference of each wheel set and the compression of each suspension spring, the torque distribution of the body around the longitudinal and transverse axes is solved, and the center of gravity height and center of gravity projection position are calculated, where the center of gravity projection position is represented by the abscissa and ordinate in the chassis plane coordinate system; the vertical distance sequence and horizontal distance sequence in the step contour data are input into the step boundary detection algorithm, which uses the slope change rate threshold segmentation method to extract the rising edge and horizontal edge of each step, calculates the height difference of the rising edge as the step height, and calculates the length of the horizontal edge as the step width.

5. The chassis control method for a stair-climbing robot vehicle based on attitude adjustment as described in claim 4, characterized in that, The stepped boundary detection algorithm uses a slope change rate threshold segmentation method to extract the rising edge and horizontal edge of each step, calculates the height difference of the rising edge as the step height, and calculates the length of the horizontal edge as the step width, including: The vertical and horizontal distance sequences in the step contour data are constructed into discrete two-dimensional point sets, each consisting of horizontal and vertical distance coordinates. The discrete two-dimensional point sets are smoothed to obtain a smoothed step contour curve. The slope of the tangent line at each point on the smoothed step contour curve is calculated to obtain a slope sequence. The slope difference between two adjacent points in the slope sequence is calculated to obtain a slope change rate sequence. Rising edge threshold and horizontal edge threshold are set. When the slope change rate exceeds the rising edge threshold and the current slope value is positive, the current point is marked as the rising edge start point. When the slope change rate is lower than the horizontal edge threshold and the absolute value of the current slope is less than 0.01, the current point is marked as the horizontal edge start point. The vertical distance difference between the rising edge start point and the horizontal edge start point is taken as the step height, and the horizontal distance difference between the horizontal edge start point and the next rising edge start point is taken as the step width.

6. The chassis control method for a stair-climbing robot vehicle based on attitude adjustment as described in claim 4, characterized in that, The process of generating the attitude adaptation completion signal includes: The inclusion relationship between the center of gravity projection position and the chassis stability support polygon is determined, and the shortest distance vector is calculated. The inverse direction of the shortest distance vector is decomposed into a swing arm adjustment component and a wheel set height adjustment component to generate the initial attitude adjustment amount. The chassis stability support polygon is a convex quadrilateral formed by connecting the four wheel set contact points in a clockwise order. The step height and step width are used as the constraint boundaries of the motion trajectory, and the change curves of the vehicle pitch angle and the change curves of the vehicle roll angle are planned to generate the motion trajectory. The initial attitude adjustment amount is applied to the current chassis actuator model, and the center of gravity projection position and attitude error after each step of adjustment are iteratively calculated. In each iteration, the nonlinear mapping relationship of the suspension stiffness is updated according to the adjusted swing arm stroke, and the predicted value of the wheel set displacement data is updated according to the adjusted wheel set speed. When the center of gravity projection position enters the interior of the chassis stability support polygon and the attitude error is less than the first preset threshold during the iteration process, the iteration stops, and an attitude adaptation completion signal is generated. The attitude adaptation completion signal includes the final target value of the swing arm stroke, the target value of the suspension stiffness, and the target value of the wheel set speed.

7. The chassis control method for a stair-climbing robot vehicle based on attitude adjustment as described in claim 6, characterized in that, The process of determining the inclusion relationship between the projected position of the center of gravity and the chassis stability support polygon and calculating the shortest distance vector includes: A ray is emitted horizontally from the center of gravity projection point. The number of intersections between the ray and each side of the chassis stabilization support polygon is calculated. If the number of intersections is odd, the center of gravity projection point is determined to be inside the chassis stabilization support polygon. If the number of intersections is zero or even, the center of gravity projection point is determined to be outside the chassis stabilization support polygon. When the center of gravity projection point is outside the chassis stabilization support polygon, the shortest perpendicular from the center of gravity projection point to the line containing each side of the polygon is calculated in turn. If the perpendicular falls within the line segment interval, the perpendicular distance is recorded. Otherwise, the distance from the center of gravity projection point to the endpoint of the line segment is recorded. The minimum value among all recorded distances is taken as the shortest distance vector.

8. The chassis control method for a stair-climbing robot vehicle based on attitude adjustment as described in claim 6, characterized in that, The step of controlling the chassis actuators to perform actions according to the hierarchical collaborative logic based on the received attitude adaptation completion signal includes: The target value of the swing arm stroke in the attitude adaptation completion signal is sent to the swing arm execution layer. The swing arm execution layer uses a proportional-integral-derivative controller to drive the electric push rod to move to the target stroke according to the trapezoidal velocity curve. The target value of the suspension stiffness is sent to the suspension adjustment layer. The suspension adjustment layer adjusts the oil volume of the suspension air spring by controlling the solenoid valve to change the equivalent stiffness. The target value of the wheel set speed is sent to the wheel set drive layer. The wheel set drive layer uses a field-oriented control algorithm to drive the brushless DC motor to reach the target speed. In the hierarchical collaborative logic, the swing arm execution layer takes precedence over the suspension adjustment layer, the suspension adjustment layer takes precedence over the wheel set drive layer, and the layers handshake their status with synchronization semaphores. After the previous layer completes its action and locks its position, it triggers the action of the next layer.

9. The chassis control method for a stair-climbing robot vehicle based on attitude adjustment as described in claim 1, characterized in that, The process of collecting chassis attitude feedback data in real time during stair climbing, and dynamically correcting the attitude adjustment amount based on the chassis attitude feedback data for iterative optimization, includes: During the stair climbing process, inertial tilt angle data, wheel displacement data, and swing arm travel data are collected at a fixed sampling period to generate chassis attitude feedback data. The real-time attitude value in the chassis attitude feedback data is compared with the expected attitude value at the current moment in the motion trajectory to calculate the attitude deviation vector. The attitude deviation vector includes pitch angle deviation, roll angle deviation, center of gravity height deviation, and center of gravity projection position deviation. The attitude deviation vector is input to the model predictive controller. The prediction time domain length of the model predictive controller is an integer multiple of the step period, and the control time domain length is a single step period. The rolling optimization method is used to solve the control increment sequence that minimizes the quadratic cost function between the predicted attitude and the reference trajectory. The first control increment of the control increment sequence is extracted as the correction value of the attitude adjustment. The correction value is superimposed on the current attitude adjustment to generate the corrected attitude adjustment, which is used as the initial attitude adjustment for the next iteration.

10. A chassis control system for a stair-climbing robot vehicle based on attitude adjustment, used to implement the chassis control method for a stair-climbing robot vehicle based on attitude adjustment as described in any one of claims 1-9, characterized in that, include: The data acquisition module is used to collect multi-sensor fusion data and initial state parameters of the chassis actuators of the stair climbing robot. After complementary filtering, mechanical deformation compensation and slip ratio correction, the data is collected to the chassis domain controller via CAN bus and stored according to a predefined memory mapping table. The parameter calculation module takes multi-sensor fusion data and initial state parameters of chassis actuators as input to calculate real-time attitude data, and obtains attitude-step adaptation data through cross-validation and weighted fusion. The attitude iteration optimization module determines the motion trajectory and attitude adjustment of the chassis actuator based on the attitude-step adaptation data. It updates the nonlinear mapping relationship of suspension stiffness and the predicted value of wheel group displacement through iterative calculation. After the iteration reaches the target, it outputs the attitude adaptation completion signal. The dynamic correction module receives the attitude adaptation completion signal and drives the actuator to move according to the hierarchical collaborative logic of the swing arm execution layer first, the suspension adjustment layer next, and the wheel group drive layer last. At the same time, it collects attitude feedback data, uses the model predictive controller to solve the control increment, and dynamically corrects the attitude adjustment amount.

Images