Crawler ground unmanned platform autonomous stair climbing heading adjustment system and method thereof

Abstract

This invention belongs to the field of robot / unmanned vehicle control technology, and relates to a heading adjustment system and method for an autonomous stair climbing system of a tracked ground unmanned platform. It includes: an information acquisition module that outputs point cloud information via LiDAR, outputs velocity and acceleration information via an inertial measurement unit, and obtains track wheel speed and current curvature via a chassis controller; an identification module that uses LiDAR to identify and model the stairs; a positioning module that uses tightly coupled LIO to estimate the vehicle's pose and obtain its yaw angle; an instantaneous LiDAR point cloud to obtain the distance between the vehicle and the adjacent wall, thus obtaining the lateral distance; a LQR-based controller module that establishes a linear model of the ground unmanned platform and applies LQR control to minimize yaw angle and lateral distance errors, ultimately outputting a yaw rate signal to the actuator module; and an actuator module that adjusts the heading of the tracked ground unmanned platform during autonomous stair climbing to maintain straight-line travel in the middle of the stairs.

Description

Technical Field

[0001] This invention belongs to the field of robot / unmanned vehicle control technology, specifically relating to a heading adjustment system and method for an autonomous stair climbing system of a tracked ground unmanned platform. Background Technology

[0002] Currently, the solution to the problem of robots slipping when climbing stairs mostly involves using data from an IMU (Inertial Measurement Unit) as the control feedback and employing PID control to ensure the robot maintains straight-line movement. (Cui Junhan) [1] To address the issue of robot slippage while climbing stairs, yaw angle and angular velocity data from IMU sensors are used as feedback parameters to implement cascaded PID control on the two-wheel differential track platform. This improves the response speed and steady-state accuracy of the control system, thus mitigating slippage during stair climbing.

[0003] Using IMU data alone can lead to cumulative errors. During the stair climbing process, due to the special structure of the stairs, the unmanned platform on the ground will be in a state of constant shaking, which will exacerbate the drift error of the IMU and result in low reliability of the measurement values.

[0004] PID control is a model-free control system. The control effect depends entirely on the selection of parameters, and multiple trials and adjustments are required to achieve a good control effect.

[0005] Most current solutions can only guarantee that the robot travels in a straight line, but cannot guarantee that the robot will stay in the middle of the stairs while climbing them. Summary of the Invention

[0006] To address the aforementioned technical problems, this invention provides a heading adjustment system and method for an autonomous stair climbing system of a tracked ground unmanned platform, ensuring that the robot remains in the center of the stairs during the climbing process.

[0007] First, the present invention provides a heading adjustment system for autonomous stair climbing of tracked ground unmanned platforms, which mainly consists of an information acquisition module, an identification module, a positioning module, a controller module based on LQR (linear quadratic regulator), and an actuator module.

[0008] The information acquisition module includes a lidar and an inertial measurement unit; the information acquisition module outputs point cloud information through the lidar, outputs velocity and acceleration information through the inertial measurement unit, and obtains track wheel speed and current curvature through the chassis controller.

[0009] The identification module uses LiDAR to identify and model the stairs.

[0010] The positioning module uses a tightly coupled LIO (a system combining lidar and inertial measurement unit) to estimate the vehicle's pose, thereby obtaining the vehicle's yaw angle; it uses instantaneous lidar point cloud to obtain the distance between the vehicle and the adjacent wall, thereby obtaining the lateral distance.

[0011] The LQR-based controller module obtains the direction angle and width information of the stairs based on the identification module, the yaw angle and lateral distance of the vehicle based on the positioning module, and the track wheel speed and current curvature based on the information acquisition module. It establishes a linear model of the ground unmanned platform and applies LQR control to minimize the yaw angle error and lateral distance error. Finally, it outputs the yaw rate signal to the actuator module.

[0012] The actuator module refers to the actuator of the ground unmanned platform, which is used to output the final left and right wheel speeds to achieve the input yaw rate, realize the heading adjustment of the tracked ground unmanned platform during the autonomous climbing of stairs, and thus ensure that the ground unmanned platform maintains a straight line in the middle of the stairs during the climbing process.

[0013] Secondly, this invention discloses a heading adjustment method for autonomous stair climbing of tracked unmanned ground platforms. Utilizing the aforementioned heading adjustment system for autonomous stair climbing of tracked unmanned ground platforms, the method includes the following steps:

[0014] (1) The information acquisition module outputs point cloud information through lidar, outputs speed and acceleration information through inertial measurement unit, and obtains track wheel speed and current curvature through chassis controller.

[0015] (2) The recognition module uses LiDAR to recognize and model the stairs. The positioning module uses a tightly coupled LIO (a system combining LiDAR and inertial measurement unit) to estimate the vehicle's pose, thereby obtaining the vehicle's yaw angle; it uses instantaneous LiDAR point cloud to obtain the distance between the vehicle and the adjacent wall, thereby obtaining the lateral distance.

[0016] (3) The LQR-based controller module obtains the direction angle and width information of the stairs based on the identification module, obtains the yaw angle and lateral distance of the vehicle based on the positioning module, obtains the track wheel speed and current curvature based on the information acquisition module, establishes a linear model of the ground unmanned platform, and applies LQR control to minimize the yaw angle error and lateral distance error, and finally outputs the yaw angle velocity signal to the actuator module.

[0017] (4) The actuator module outputs the final left and right wheel speeds to achieve the input yaw rate, thereby adjusting the heading of the tracked unmanned ground platform during the autonomous stair climbing process, thus ensuring that the unmanned ground platform maintains a straight line in the middle of the staircase during the stair climbing process.

[0018] In step (2), the positioning module obtains the lateral distance using the following method:

[0019] The collected lidar point cloud contains points of the wall. By using the Random Sample Consensus (RANSAC) algorithm, the wall points can be extracted from these points, and the wall can be fitted to a plane. After fitting the plane equation of the wall, the distance from the platform position point to the fitted plane is calculated, and the lateral distance between the platform and the wall is obtained.

[0020] The specific method for establishing a linear model of a ground-based unmanned platform is as follows:

[0021] Lateral error y e and yaw angle error θ e The calculation formula is:

[0022]

[0023] In the formula b w The width of the stair treads is obtained by the identification module, d(t) is the lateral distance between the platform and the wall obtained by the positioning module, and θ is the width of the stair treads. w The direction angle of the staircase is obtained by the identification module, and θ(t) is the yaw angle of the vehicle obtained by the positioning module.

[0024] The linear model of a ground-based unmanned platform can be represented as:

[0025]

[0026] In the formula u(t)=ω(t), v r ρ is the speed of the right track, L is the aiming distance, and ρ is the current curvature.

[0027] The technical effects of this invention are as follows:

[0028] 1. This invention uses tightly coupled LIO to estimate the yaw angle of the vehicle, and uses instantaneous LiDAR point cloud to obtain the distance between the vehicle and the adjacent wall, which is used as the control variable.

[0029] 2. This invention uses LiDAR to identify and model stairs, obtaining the direction angle and width information of the stairs, which are then used as ideal values ​​for variables.

[0030] 3. The control feedback variables used in this invention are lateral error and yaw angle error, which can ensure that the unmanned ground platform travels in a straight line while climbing stairs, and also ensure that it stays in the center of the stairs.

[0031] 4. The information acquisition module adopted in this invention includes a lidar and an inertial measurement unit. The combination of the two can improve measurement accuracy, reliability, and reduce cumulative error, and performs better in stairwell environments.

[0032] 5. This invention adopts LQR control. This control strategy applies a system model, which can achieve closed-loop optimization and reduce the workload of parameter tuning. Attached Figure Description

[0033] Figure 1 This is a schematic diagram of the system structure of the present invention;

[0034] Figure 2 This is a block diagram of the LQR control system of the present invention. Detailed Implementation

[0035] The specific technical solution of the present invention will be described in conjunction with the accompanying drawings.

[0036] like Figure 1 As shown, this invention discloses a heading adjustment system for autonomous stair climbing of a tracked ground unmanned platform, which mainly consists of an information acquisition module, an identification module, a positioning module, a controller module based on LQR (linear quadratic regulator), and an actuator module.

[0037] The information acquisition module includes a lidar and an inertial measurement unit; the information acquisition module outputs point cloud information through the lidar, outputs velocity and acceleration information through the inertial measurement unit, and obtains track wheel speed and current curvature through the chassis controller.

[0038] The identification module uses LiDAR to identify and model the stairs.

[0039] The positioning module uses a tightly coupled LIO (a system combining lidar and inertial measurement unit) to estimate the vehicle's pose, thereby obtaining the vehicle's yaw angle; it uses instantaneous lidar point cloud to obtain the distance between the vehicle and the adjacent wall, thereby obtaining the lateral distance.

[0040] The LQR-based controller module obtains the direction angle and width information of the stairs based on the identification module, the yaw angle and lateral distance of the vehicle based on the positioning module, and the track wheel speed and current curvature based on the information acquisition module. It establishes a linear model of the ground unmanned platform and applies LQR control to minimize the yaw angle error and lateral distance error. Finally, it outputs the yaw rate signal to the actuator module.

[0041] The actuator module refers to the actuator of the ground unmanned platform, which is used to output the final left and right wheel speeds to achieve the input yaw rate, realize the heading adjustment of the tracked ground unmanned platform during the autonomous climbing of stairs, and thus ensure that the ground unmanned platform maintains a straight line in the middle of the stairs during the climbing process.

[0042] The above-mentioned heading adjustment method for an autonomous stair-climbing system for tracked unmanned ground platforms includes the following steps:

[0043] (1) The information acquisition module outputs point cloud information through lidar, outputs speed and acceleration information through inertial measurement unit, and obtains track wheel speed and current curvature through chassis controller.

[0044] (2) The recognition module uses LiDAR to recognize and model the stairs. The positioning module uses a tightly coupled LIO (a system combining LiDAR and inertial measurement unit) to estimate the vehicle's pose, thereby obtaining the vehicle's yaw angle; it uses instantaneous LiDAR point cloud to obtain the distance between the vehicle and the adjacent wall, thereby obtaining the lateral distance.

[0045] In step (2), the positioning module obtains the lateral distance using the following method:

[0046] The acquired LiDAR point cloud contains points representing the walls. Using the RANSAC (Random Sample Consensus) algorithm, wall points can be extracted from these points, and a plane fit can be performed on the walls. RANSAC is a robust algorithm that effectively identifies and removes noise or outliers from the point cloud, thus accurately fitting the plane equation of the walls. After fitting the plane equation of the walls, because the LiDAR point cloud contains distance information, the distance from the platform's location point to the fitted plane can be calculated, thus obtaining the lateral distance between the platform and the walls.

[0047] (3) The LQR-based controller module obtains the direction angle and width information of the stairs based on the identification module, obtains the yaw angle and lateral distance of the vehicle based on the positioning module, obtains the track wheel speed and current curvature based on the information acquisition module, establishes a linear model of the ground unmanned platform, and applies LQR control to minimize the yaw angle error and lateral distance error, and finally outputs the yaw angle velocity signal to the actuator module.

[0048] (4) The actuator module outputs the final left and right wheel speeds to achieve the input yaw rate, thereby adjusting the heading of the tracked unmanned ground platform during the autonomous stair climbing process, thus ensuring that the unmanned ground platform maintains a straight line in the middle of the staircase during the stair climbing process.

[0049] The specific method for establishing a linear model of a ground-based unmanned platform is as follows:

[0050] Lateral error y e and yaw angle error θ e The calculation formula is:

[0051]

[0052] In the formula b w The width of the stair treads is obtained by the identification module, d(t) is the lateral distance between the platform and the wall obtained by the positioning module, and θ is the width of the stair treads. wThe direction angle of the staircase is obtained by the identification module, and θ(t) is the yaw angle of the vehicle obtained by the positioning module.

[0053] The linear model of a ground-based unmanned platform can be represented as:

[0054]

[0055] In the formula u(t)=ω(t), v r ρ is the speed of the right track, L is the aiming distance, and ρ is the current curvature.

[0056] LQR control methods based on LQR controller modules, such as Figure 2 As shown.

[0057] The linear state-space equations of the controlled system can be simplified to:

[0058]

[0059] Define the quadratic performance index for the state vector and control vector as follows:

[0060]

[0061] In the formula, Q and R are:

[0062] R = r

[0063] In the formula, q1 and q2 represent the lateral deviation y, respectively. e , heading angle deviation θ e The weighting coefficient, r, represents the weighting coefficient for the yaw rate ω.

[0064] For convenience, the following matrix is ​​defined:

[0065] D = C T QC

[0066] Performance metrics can be abbreviated as:

[0067]

[0068] To find optimal control, the Hamiltonian function is introduced:

[0069]

[0070] In the formula, λ is the multiplier to be determined. Since the control is optimal, the partial derivative of the Hamiltonian function with respect to u should be zero, which gives:

[0071]

[0072] By transforming the formula, we can obtain the expression for the control quantity u during optimal control:

[0073] u = -R -1 B T λ

[0074] Continuing to take partial derivatives with respect to the Hamiltonian function, we can obtain the canonical equation:

[0075]

[0076] Introduce a new matrix P to represent λ:

[0077] λ=Px

[0078] Substituting the formula, we get:

[0079]

[0080] Differentiating the formula yields:

[0081]

[0082] Substituting the state-space equations, we get:

[0083]

[0084] By comparison, we can obtain the Riccati equation:

[0085] PA+A T P-PBR -1 B T P+D=0

[0086] Solving the Riccati equations, the feedback matrix K can be expressed as:

[0087] K = -R -1 B T P

[0088] At this point, the optimal control law u * (t) is represented as:

[0089] u * (t)=-K(t)x(t).

Claims

1. A heading adjustment system for autonomous stair climbing on a tracked unmanned ground platform, characterized in that, It includes an information acquisition module, an identification module, a positioning module, an LQR-based controller module, and an actuator module; The information acquisition module outputs point cloud information through lidar, outputs velocity and acceleration information through inertial measurement unit, and obtains track wheel speed and current curvature through chassis controller; The identification module uses LiDAR to identify and model the stairs; The positioning module uses a system combining a tightly coupled LIO lidar and an inertial measurement unit to estimate the vehicle's pose and obtain the vehicle's yaw angle; it uses instantaneous lidar point clouds to obtain the distance between the vehicle and the adjacent wall, thus obtaining the lateral distance. The LQR-based controller module obtains the direction angle and width information of the stairs based on the identification module, the yaw angle and lateral distance of the vehicle based on the positioning module, and the track wheel speed and current curvature based on the information acquisition module. It establishes a linear model of the ground unmanned platform and applies LQR control to minimize the yaw angle error and lateral distance error. Finally, it outputs the yaw rate signal to the actuator module. The actuator module refers to the actuator of the ground unmanned platform, which is used to output the final left and right wheel speeds to achieve the input yaw rate, realize the heading adjustment of the tracked ground unmanned platform during the autonomous climbing of stairs, and thus ensure that the ground unmanned platform maintains a straight line in the middle of the stairs during the climbing process.

2. A heading adjustment method for autonomous stair climbing on a tracked unmanned ground platform, characterized in that, The heading adjustment system for autonomous stair climbing of a tracked ground unmanned platform as described in claim 1 includes the following steps: (1) The information acquisition module outputs point cloud information through lidar, outputs velocity and acceleration information through inertial measurement unit, and obtains track wheel speed and current curvature through chassis controller; (2) The recognition module uses LiDAR to recognize and model the stairs; the positioning module uses a system combining tightly coupled LIO LiDAR and inertial measurement unit to estimate the vehicle's pose and obtain the vehicle's yaw angle; the instantaneous LiDAR point cloud is used to obtain the distance between the vehicle and the adjacent wall and obtain the lateral distance. (3) The LQR-based controller module obtains the direction angle and width information of the stairs based on the identification module, obtains the yaw angle and lateral distance of the vehicle based on the positioning module, obtains the track wheel speed and current curvature based on the information acquisition module, establishes a linear model of the ground unmanned platform, and applies LQR control to minimize the yaw angle error and lateral distance error, and finally outputs the yaw angle velocity signal to the actuator module. (4) The actuator module outputs the final left and right wheel speeds to achieve the input yaw rate, thereby adjusting the heading of the tracked unmanned ground platform during the autonomous stair climbing process, so as to ensure that the unmanned ground platform maintains a straight line in the middle of the stair climbing process.

3. The heading adjustment method for autonomous stair climbing of a tracked ground unmanned platform according to claim 2, characterized in that, In step (2), the positioning module obtains the lateral distance using the following method: The collected lidar point cloud contains points of the wall. By using the Random Sample Consensus (RANSAC) algorithm, the wall points are extracted from these points, and the wall is fitted to a plane. After fitting the plane equation of the wall, the distance from the platform position point to the fitted plane is calculated, and the lateral distance between the platform and the wall is obtained.

4. The heading adjustment method for autonomous stair climbing of a tracked ground unmanned platform according to claim 2, characterized in that, The specific method for establishing a linear model of a ground-based unmanned platform is as follows: lateral error and yaw angle error The calculation formula is: ； In the formula To identify the width of the stair steps obtained by the identification module, The lateral distance between the platform and the wall is obtained by the positioning module. To identify the direction angle of the stairs obtained by the identification module, The yaw angle of the vehicle obtained by the positioning module; The linear model of the ground unmanned platform is represented as follows: ； In the formula , , The speed of the right track. Pre-aiming distance, This represents the current curvature.

Images