Chassis structure of stair climbing robot and first step reaching method of chassis structure

Abstract

The invention discloses a chassis structure of a stair-climbing robot and a first step arriving method thereof.The chassis structure comprises a bearing body and a track assembly at the bottom of the bearing body, a scissor-type lifting mechanism is arranged at the front end of the bottom of the bearing body, and the scissor-type lifting mechanism provides lifting power through an electric linear actuator; universal wheels are arranged at the bottom of the shear fork type lifting mechanism; a telescopic wheel mechanism is arranged at the rear end of the bottom of the bearing body and comprises a shell, a pair of telescopic legs is arranged in a telescopic groove in the bottom of the shell, straight wheels with driving motors are arranged at the bottoms of the telescopic legs, and the telescopic legs are driven by a driving mechanism in the shell to move up and down synchronously. According to the invention, through the cooperation of the scissor-fork type lifting mechanism and the telescopic wheel mechanism, the front end of the crawler belt assembly can be tilted, so that the capability of reaching first steps with different heights is stronger, and the trafficability and the adaptability are improved.

Description

Technical Field

[0001] The present invention relates to a wheel-track composite robot, and particularly to a chassis structure of a stair-climbing robot and a method for reaching the first step. Background Art

[0002] The wheel-track composite robot can switch between wheels and tracks according to different terrains, enabling both high-speed movement and obstacle-crossing capabilities. Stairs are one of the obstacles that are difficult for robots to cross, and the difficulty lies in how to make the track reach the first step. Existing wheel-track composite robots have limited ability to reach the first step. Specifically, they are only applicable to the case where the step height is lower than the track. Summary of the Invention

[0003] Object of the Invention: The first object of the present invention is to provide a chassis structure of a stair-climbing robot with stronger passability to meet the need to cross stairs with different step heights, especially when the step height is higher than the track. The second object of the present invention is to provide a method for reaching the first step of the chassis structure.

[0004] Technical Solution: A chassis structure of a stair-climbing robot according to the present invention includes a load-bearing main body and a track assembly at its bottom. A scissor lift mechanism is provided at the front end of the bottom of the load-bearing main body. The scissor lift mechanism is powered by an electric linear actuator for lifting, and a universal wheel is provided at the bottom of the scissor lift mechanism. A telescopic wheel mechanism is provided at the rear end of the bottom of the load-bearing main body. The telescopic wheel mechanism includes a housing. A pair of telescopic legs are provided in a telescopic groove at the bottom of the housing. A straight wheel with a built-in drive motor is provided at the bottom of the telescopic legs. The pair of telescopic legs are driven by a drive mechanism inside the housing to move synchronously up and down.

[0005] Further, the scissor lift mechanism has a pair of scissor arms, which are connected by a connecting rod. The electric linear actuator drives the connecting rod to achieve the lifting function of the scissor lift mechanism. The universal wheel is installed on a cross beam at the bottom of the scissor lift mechanism.

[0006] Further, the drive mechanism includes a motor, an output gear, and a driven gear. A rack is installed at the upper end of the telescopic leg. The output gear is installed at the output end of the motor. The output gear meshes with the driven gear, and at the same time, the output gear and the driven gear respectively mesh with the two racks.

[0007] Further, the load-bearing main body is made of an aluminum plate.

[0008] Further, a mobile robot system assembly is installed on the load-bearing main body. The mobile robot system assembly includes a micro control unit MCU and multiple sensors connected to the micro control unit MCU. The multiple sensors include an AI camera, a 2D radar, a lidar, a cliff sensor, and an ultrasonic sensor;

[0009] The ultrasonic sensor is used to detect the distance to the obstacle in front, and the cliff sensor is used to detect the height from the ground; the 2D radar and lidar are used to project the point cloud data generated by scanning the obstacle onto the image plane of the AI camera, and further combine with the color information of the AI camera to generate a fused data representation;

[0010] The microcontroller unit MCU receives the data of multiple sensors, obtains the information of the front environment and the current position of the robot, and judges whether it is in a safe working state; the microcontroller unit MCU also controls the crawler assembly, the electric linear actuator of the scissor lift mechanism, and the telescopic wheel mechanism to act.

[0011] Furthermore, the AI camera, 2D radar and lidar are installed at the front end of the load-bearing body, the ultrasonic sensor is installed at the front end of the scissor lift mechanism, and the cliff sensor is installed at the bottom of the scissor lift mechanism.

[0012] The method for the first step of the chassis structure of the stair-climbing robot according to the present invention includes:

[0013] 1) The robot moves. When the distance detected by the ultrasonic sensor to the obstacle in front reaches the set value, the robot stops;

[0014] 2) The 2D radar and lidar work together to scan the obstacle in front to preliminarily judge whether the obstacle in front is a stair;

[0015] 3) Use the radar data combined with the color information of the AI camera to generate a fused data representation, and further accurately judge whether the obstacle in front is a stair according to the fused data representation. If it is a stair, go to step 4);

[0016] 4) Use the scan data of the 2D radar and lidar to estimate the height of the first step of the stair;

[0017] 5) First, the robot chassis advances to a stop 2 cm away from the stair, then the scissor lift mechanism unfolds, and according to the estimated height of the first step, judge whether the initial crawler is higher than the first step. If not, lift the front end of the chassis 5 cm higher than the first step, and at the same time, the unfolding speed of the scissor lift mechanism is constant. The vertical height of the crawler from the ground can be obtained through the distance measured by the cliff sensor and the tilt angle of the chassis to ensure that the front end of the crawler can be on the first step; subsequently, the telescopic wheel mechanism retracts, the scissor lift mechanism retracts, the front end of the crawler assembly reaches the first step, and the crawler assembly works to start climbing.

[0018] Furthermore, step 2) includes:

[0019] The lidar obtains all horizontal plane sets P = {S in the global point cloud map generated during the preprocessing process P,i, i = 0, …, n}, and filter out the connected planes, that is, two planes at similar heights with at least a certain number of connection points within a certain distance range, to form a set T = {S T,i , i = 0, …, m}; n and m represent the maximum number of layers;

[0020] Then, for each candidate step surface S in the set T T , construct a step surface coordinate system S according to the eigenvectors (v i1 , v i2 , v i3 ) obtained by the principal component analysis method. The eigenvector v oi corresponding to the maximum eigenvalue points to the step width direction w, the second eigenvector v i1 corresponds to the step depth direction d, and the eigenvector v i2 corresponding to the minimum eigenvalue points to the step height direction h; i3

[0021] Calculate the included angle g o in the x - direction and the included angle g x in the y - direction for every two candidate step surface coordinate systems S y . If g x , g y are both less than the preset angle threshold, then these two step surfaces may belong to the same staircase, and the next - layer step surface S T,i+1 should be within a certain distance range of the upper - layer step surface S T,i ; Calculate the centroid T,i+1 of S in the coordinate system S T,i of the step oi position

[0022]

[0023] where, is the transformation matrix, obtained according to the coordinate system S oi of each layer of step surface; G is the global coordinate system, used to establish the description of the working environment space of the mobile robot;

[0024] If is located within a certain area in front - upper or rear - upper of S T,i , then a staircase is formed; otherwise, this step surface does not belong to this staircase.

[0025] Further, step 4) includes:

[0026] Scan the staircase in front of the robot horizontally through a 2D radar, and obtain the width of the obstacle according to Equation (1):

[0027]

[0028] where D is the width of the obstacle, and θ a and θ b are the maximum values of the angles at the left and right ends of the obstacle, L a and L b are respectively the distances corresponding to reaching the obstacle at the angular positions of θ a and θ b ;

[0029] For the distance information collected by the lidar, it is saved in the form of an array M[i] in the order from bottom to top, where i represents the arrangement number of the depth data scan; by scanning the stairs in front of the robot vertically with the lidar, the depth and height of the obstacle are obtained; point H is the boundary point of the first step of the measured stairs; h 0 represents the camera mounting height, which is a fixed value; h S and h S0 represent respectively the height of the first step and the height difference between the boundary point of the first step and the camera in the vertical direction; θ S is the angle between the line connecting the lidar and H and the horizontal direction, and we can get:

[0030] θ S = (H - 360) * 0.08 (2)

[0031] h S0 = M[H] * tanθ S (3)

[0032] h S = h S0 + h 0 (4)

[0033] According to equations (2) to (4), the height parameter of the first step can be estimated using the depth value M[H] and the arrangement number value H corresponding to the selected first-layer boundary point.

[0034] Furthermore, in step 5), during the stair climbing process, after obtaining the data of each sensor, the robot chassis obtains the real-time position of the robot through a motion model, which is a combination of position estimation and direction estimation, and both use a constant velocity / constant angular velocity model;

[0035] The state vector includes position, velocity, Euler angles, and the velocity of Euler angles. The Euler angles are the roll angle α, pitch angle β, and yaw angle γ of the robot;

[0036] The rotation matrix composed of Euler angles

[0037] where R α is the rotation matrix of the roll angle, Rβ The rotation matrix for the pitch angle, R γ is the rotation matrix for the yaw angle;

[0038] Convert the angular velocity of the Euler angles to the linear velocity in the local coordinate system of the robot through the rotation matrix

[0039] V local = R α,β,γ [α,β,γ] T ;

[0040]

[0041] where R ′ represents the covariance matrix for velocity and direction estimation in the robot motion model, used to quantify the uncertainty of these estimations; the variance and represent the quality of the velocity and direction constraints of the robot.

[0042] Advantageous effects: Compared with the prior art, the present invention has the following remarkable advantages: Through the cooperation of the scissor lift mechanism and the telescopic wheel mechanism, the present invention can lift the front end of the crawler assembly, so that it has a stronger ability to reach the first step at different heights, improving the passability and adaptability; By configuring multiple sensors and control algorithms, the present invention can achieve adaptive staircase modeling and robot environmental perception, improving the accuracy and efficiency of step detection, and at the same time can ensure that the robot safely and accurately reaches the first step and realizes autonomous climbing, and this process does not require any existing map or prior knowledge of the staircase. BRIEF DESCRIPTION OF THE DRAWINGS

[0043] Figure 1 is a schematic diagram of the chassis structure of the stair-climbing robot provided by an embodiment of the present invention;

[0044] Figure 2 is Figure 1 the rear view of;

[0045] Figure 3 is Figure 1 the side perspective view of;

[0046] Figure 4 is Figure 1 the bottom view of;

[0047] Figure 5 is a schematic diagram of the structure of the scissor lift mechanism in an embodiment of the present invention;

[0048] Figure 6 is a schematic diagram of the structure of the telescopic wheel mechanism in an embodiment of the present invention;

[0049] Figure 7It is a flow chart of the method for reaching the first step of the chassis structure provided by the embodiment of the present invention;

[0050] Figure 8 It is a schematic diagram of the process of climbing stairs in the embodiment of the present invention. Specific embodiments

[0051] The present invention will be further described below with reference to the accompanying drawings.

[0052] Appendix Figures 1 to 8 The reference numerals in the accompanying drawings are as follows:

[0053] 1, load-bearing main body; 11, AI camera; 12, 2D radar; 13, lidar;

[0054] 2, crawler assembly;

[0055] 3, scissor lift mechanism; 31, universal wheel; 32, electric linear actuator; 33, connecting rod; 34, cross beam; 35, cliff sensor; 36, ultrasonic sensor; 37, scissor arm;

[0056] 4, telescopic wheel mechanism; 41, housing; 42, telescopic leg; 43, straight wheel; 44, output gear; 45, driven gear; 46 / 47, rack; 48, battery;

[0057] 5, mobile robot system assembly.

[0058] As Figures 1 to 4 shown, the embodiment of the present invention provides a chassis structure of a stair-climbing robot, including a load-bearing main body 1 made of aluminum plate. The bottom of the load-bearing main body 1 is provided with a crawler assembly 2, a scissor lift mechanism 3, a telescopic wheel mechanism 4 and a mobile robot system assembly 5. Among them, the scissor lift mechanism 3 is located at the front end of the bottom of the load-bearing main body 1, and the telescopic wheel mechanism 4 is located at the rear end of the bottom of the load-bearing main body 1.

[0059] As Figure 5 shown, the scissor lift mechanism 3 has an electric linear actuator 32 and a pair of scissor arms 37. The pair of scissor arms 37 are connected by a connecting rod 33. The electric linear actuator 32 drives the connecting rod 33 to realize the lifting function of the scissor lift mechanism 3. Two universal wheels 31 are installed on the cross beam 34 at the bottom of the scissor lift mechanism 3.

[0060] As Figure 6 shown, the telescopic wheel mechanism 4 includes a housing 41. A pair of telescopic legs 42 are arranged in the telescopic groove at the bottom of the housing 41. The bottom of the telescopic legs 42 is provided with straight wheels 43 with a built-in drive motor. The pair of telescopic legs 42 are driven by a drive mechanism inside the housing 41 to move up and down synchronously. Specifically,

[0061] The driving mechanism includes a motor, an output gear 44 and a driven gear 45. Rack bars 46 and 47 are respectively installed at the upper ends of the two telescopic legs 42. The output gear 44 is installed at the output end of the motor, and the battery 48 arranged inside the housing 41 powers the motor and the straight wheels 43. The output gear 44 meshes with the driven gear 45, and at the same time, the output gear 44 and the driven gear 45 respectively mesh with the two rack bars.

[0062] The mobile robot system assembly 5 includes a micro control unit MCU and multiple sensors connected to the micro control unit MCU. The multiple sensors include an AI camera 11, a 2D radar 12, a lidar 13, a cliff sensor 35 and a ultrasonic sensor 36. Among them, the AI camera 11, the 2D radar 12 and the lidar 13 are embedded at the front end of the load-bearing body 1, the ultrasonic sensor 36 is installed at the front end of the scissor lift mechanism 3, and the cliff sensor 35 is installed at the bottom of the scissor lift mechanism 3.

[0063] The ultrasonic sensor 36 is used to detect the distance to the obstacle in front, and the cliff sensor 35 is used to detect the height from the ground; the 2D radar 12 and the lidar 13 are used to project the point cloud data generated by scanning the obstacle onto the image plane of the AI camera 11, and further combine the color information of the AI camera 11 to generate a fused data representation. In this way, depth information can be added to each pixel point in the image to form a three-dimensional point cloud image with depth and color, enabling the robot to more comprehensively understand the environment. Even in low light, limited vision or complex backgrounds, it can maintain a high navigation accuracy and ensure the stable operation of the robot.

[0064] The micro control unit MCU receives the data of multiple sensors, obtains the information of the front environment, and based on the environmental information, obtains the current position of the robot and judges whether it is in a safe working state; the micro control unit MCU also controls the crawler assembly 2, the electric linear actuator 32 of the scissor lift mechanism 3, and the telescopic wheel mechanism 4 to act.

[0065] As Figure 7 shown, the embodiment of the present invention also provides a method for the first step reaching of the chassis structure of the stair-climbing robot described in the embodiment of the present invention, including the following steps:

[0066] 1) The robot moves. When the distance detected by the ultrasonic sensor 36 from the obstacle in front reaches a set value, for example, 10 cm, the robot stops;

[0067] 2) The 2D radar 12 and the lidar 13 work together to scan the obstacle in front to preliminarily judge whether the obstacle in front is a stair;

[0068] The lidar 13 obtains all horizontal plane sets P = {S in the global point cloud map generated in the preprocessing process P,i, i = 0, …, n}, and filter out the connected planes, that is, two planes at similar heights with at least a certain number of connection points within a certain distance range, to form a set T = {S T,i , i = 0, …, m}; n and m represent the maximum number of layers;

[0069] Then, for each candidate step surface S in the set T T , according to the eigenvector (v i1 , v i2 , v i3 ) obtained by the principal component analysis method, construct a step surface coordinate system S oi . The eigenvector v i1 corresponding to the maximum eigenvalue points to the step width direction w, the second eigenvector v i2 corresponds to the step depth direction d, and the eigenvector v i3 corresponding to the minimum eigenvalue points to the step height direction h;

[0070] Calculate the included angle g o in the x - direction and the included angle g x in the y - direction for every two candidate step surface coordinate systems S y . If g x , g y are both less than the preset angle threshold, then these two step surfaces may belong to the same staircase, and the next - layer step surface S T,i+1 should be within a certain distance range of the upper - layer step surface S T,i ; Calculate the position of the centroid T,i+1 of S in the coordinate system S T,i of the step S oi

[0071]

[0072] where, is the transformation matrix, obtained according to the coordinate system S oi of each layer of step surface; G is the global coordinate system, used to establish the description of the working environment space of the mobile robot.

[0073] If is within a certain area in front - upper or rear - upper of S T,i , then a staircase is formed; otherwise, this step surface does not belong to this staircase.

[0074] 3) The AI camera 11 acquires the front image information, combines the radar data with the color information of the AI camera 11 to generate a fused data representation, which can add depth information to each pixel point in the image, forming a three-dimensional point cloud image with depth and color, enabling the robot to understand the environment more comprehensively. Thus, the robot chassis can determine whether there is a staircase ahead. Based on the fused data representation, it further accurately determines whether the obstacle ahead is a staircase. If it is a staircase, proceed to step 4).

[0075] 4) Use the scan data of the 2D radar 12 and the lidar 13 to estimate the height of the first step of the staircase;

[0076] The 2D radar 12 scans the staircase in front of the robot horizontally, and the width of the obstacle can be obtained according to Equation (1):

[0077]

[0078] where D is the width of the obstacle, θ a and θ b are the maximum values of the angles at the left and right ends of the obstacle, L a and L b are the distances corresponding to the positions of the angles θ a and θ b where the obstacle is reached respectively;

[0079] The vertical field of view of the lidar 13 is 58°, the viewing angle difference between adjacent two pixel points in the vertical direction is about 0.08°, and the vertical field of view is bounded by the horizontal line, with 29° above and below the horizontal line, positive above the horizontal line and negative below the horizontal line. Therefore, the value range of the vertical field of view is -29° to 29°. The distance information collected by the lidar 13 is saved in the form of an array M[i] in ascending order from bottom to top, where i represents the serial number of the depth data scan, and its range of change is 0 to 719; thus, M[0] saves the distance value collected in the -29° direction, M

[719] saves the distance value collected in the 29° direction, and M

[359] and M

[360] can be approximated as the distance values collected in the horizontal direction. By scanning the staircase in front of the robot vertically with the lidar 13, the depth and height of the obstacle are obtained; point H is the boundary point of the first step of the measured staircase; h 0 = 305mm represents the camera mounting height, which is a fixed value; h S , h S0 represent the height of the first step and the height difference between the boundary point of the first step and the camera in the vertical direction respectively; θ S is the angle between the connection line of the lidar 13 and H and the horizontal direction, and it can be obtained that:

[0080] θ S= (H - 360) * 0.08 (2)

[0081] h S0 = M[H] * tanθ S (3)

[0082] h S = h S0 + h 0 (4)

[0083] According to equations (2) to (4), the height parameter of the first step can be estimated using the depth value M[H] and the arrangement number value H corresponding to the first-layer boundary points selected.

[0084] 5) As Figure 8 shown, first, the robot chassis advances to a stop when it is 2 cm away from the stairs. Then, the scissor lift mechanism 3 unfolds, and based on the estimated height of the first step, it determines whether the initial track is higher than the first step. If not, the front end of the chassis is lifted to about 5 cm higher than the first step. At the same time, since the unfolding speed of the scissor lift mechanism 3 is constant, the micro control unit MCU can obtain the vertical height of the track from the ground through the distance measured by the cliff sensor 35 and the tilt angle of the chassis, ensuring that the front end of the track can be on the first step. Subsequently, the telescopic wheel mechanism 4 retracts, the scissor lift mechanism 3 retracts, the front end of the track assembly 2 reaches the first step, and the track assembly 2 starts to work and begins to climb.

[0085] During the process of climbing the stairs, the robot needs to adjust its own posture and the actions of each part in real time according to the positioning information. Therefore, after obtaining the data of each sensor, the robot chassis obtains the real-time position of the robot through the motion model. The motion model is a combination of position estimation and direction estimation, both of which use the constant velocity / constant angular velocity model; the state vector includes position, velocity, Euler angles, and the velocity of Euler angles. The Euler angles are the roll angle α, pitch angle β, and yaw angle γ of the robot;

[0086] The rotation matrix composed of Euler angles

[0087] where, R α is the rotation matrix of the roll angle, R β is the rotation matrix of the pitch angle, R γ is the rotation matrix of the yaw angle;

[0088] Convert the angular velocity of Euler angles (roll angular velocity, pitch angular velocity, yaw angular velocity) to the linear velocity V in the local coordinate system of the robot through the rotation matrix local = R α,β,γ [α, β, γ] T ;

[0089]

[0090] Among them, R ′ represents the covariance matrix of the speed and direction estimates in the robot motion model, which is used to quantify the uncertainty of these estimates. The variance and represent the quality of the speed and direction constraints of the robot.

Claims

1. A chassis structure of a stair climbing robot, comprising a load-bearing body (1) and a crawler assembly (2) at its bottom, characterized in that: A scissor-type lifting mechanism (3) is arranged at the front end of the bottom of the load-bearing body (1), the scissor-type lifting mechanism (3) provides lifting power through an electric linear actuator (32), and a universal wheel (31) is arranged at the bottom of the scissor-type lifting mechanism (3); a telescopic wheel mechanism (4) is arranged at the rear end of the bottom of the load-bearing body (1), the telescopic wheel mechanism (4) comprises a shell (41), a pair of telescopic legs (42) are arranged in a telescopic groove at the bottom of the shell (41), and a straight-running wheel (43) with a self-contained driving motor is arranged at the bottom of the telescopic legs (42), and the pair of telescopic legs (42) are driven by a driving mechanism inside the shell (41) to achieve synchronous up and down movement.

2. The chassis structure of the stair climbing robot according to claim 1, characterized in that: The scissor-type lifting mechanism (3) has a pair of scissor arms (37), the pair of scissor arms (37) are connected via a connecting rod (33), an electric linear actuator (32) drives the connecting rod (33) to realize the lifting function of the scissor-type lifting mechanism (3); and a universal wheel (31) is installed on a crossbeam (34) at the bottom of the scissor-type lifting mechanism (3).

3. The chassis structure of the stair climbing robot according to claim 1, characterized in that: The driving mechanism comprises a motor, an output gear (44) and a driven gear (45); a rack is installed on the upper end of the telescopic leg (42); the output gear (44) is installed on the output end of the motor; the output gear (44) is meshed with the driven gear (45); and the output gear (44) and the driven gear (45) are respectively meshed with two racks.

4. The chassis structure of the stair climbing robot according to claim 1, characterized in that: The load-bearing body (1) is made of an aluminum plate.

5. The chassis structure of the stair climbing robot according to any one of claims 1 to 4, characterized in that: A mobile robot system assembly (5) is installed on the load-bearing body (1), and the mobile robot system assembly (5) includes a microcontroller unit (MCU) and a plurality of sensors connected to the microcontroller unit (MCU), wherein the plurality of sensors include an AI camera (11), a 2D radar (12), a laser radar (13), a cliff sensor (35), and an ultrasonic sensor (36); The ultrasonic sensor (36) is used to detect the distance of the obstacle ahead, and the cliff sensor (35) is used to detect the height from the ground; the 2D radar (12) and the laser radar (13) are used to project the point cloud data generated by scanning the obstacle onto the image plane of the AI ​​camera (11), and further combine the color information of the AI ​​camera (11) to generate a fused data representation; The microcontroller unit MCU receives data from multiple sensors, obtains information about the front environment and the current position of the robot, and determines whether it is in a safe working state; the microcontroller unit MCU also controls the movement of the crawler assembly (2), the electric linear actuator (32) of the scissor-type lifting mechanism (3), and the telescopic wheel mechanism (4).

6. The chassis structure of the stair climbing robot according to claim 5, characterized in that: The AI ​​camera (11), the 2D radar (12) and the laser radar (13) are installed at the front end of the load-bearing body (1), the ultrasonic sensor (36) is installed at the front end of the scissor-type lifting mechanism (3), and the cliff sensor (35) is installed at the bottom of the scissor-type lifting mechanism (3).

7. A method for reaching the first step of the chassis structure of the stair climbing robot according to claim 5, characterized in that: include: 1) The robot moves, and when the ultrasonic sensor (36) detects that the distance to the obstacle in front reaches a set value, the robot stops; 2) The 2D radar (12) and the laser radar (13) work together to scan the obstacle ahead to preliminarily determine whether the obstacle ahead is a staircase; 3) Using the radar data in combination with the color information of the AI ​​camera (11), a fused data representation is generated, and the fused data representation is used to further accurately determine whether the obstacle ahead is a staircase. If it is a staircase, proceed to step 4); 4) using the scanning data of the 2D radar (12) and the laser radar (13), the height of the first step of the stairs is estimated; 5) First, the robot chassis moves forward until it stops at a distance of 2 cm from the stairs, then the scissor-type lifting mechanism (3) is deployed, and based on the estimated height of the first step, it is determined whether the initial track is higher than the first step. If not, the front end of the chassis is lifted to 5 cm above the first step. At the same time, the deployment speed of the scissor-type lifting mechanism (3) is constant. The distance measured by the cliff sensor (35) and the chassis inclination angle can be used to obtain the vertical height of the track from the ground, ensuring that the front end of the track can be on the first step; then, the telescopic wheel mechanism (4) is retracted, the scissor-type lifting mechanism (3) is retracted, the front end of the track assembly (2) reaches the first step, and the track assembly (2) starts to work and begins climbing.

8. The method for reaching the first step according to claim 7, characterized in that: Step 2) includes: The laser radar (13) obtains all horizontal plane sets P = {S P,i , i=0,…,n}, and screen out the planes with connectivity, that is, two planes at similar heights that have at least a certain number of connection points within a certain distance range, forming a set T={S T,i ,i=0,…,m}; n,m represent the maximum number of layers; Then, for each candidate step surface S in the set T T , according to the eigenvector (v i1 ,v i2 ,v i3 ) Construct the step surface coordinate system S oi , the eigenvector v corresponding to the maximum eigenvalue i1 Pointing in the direction of the step width w, the second eigenvector v i2 Corresponding to the step depth direction d, the eigenvector v corresponding to the minimum eigenvalue i3 Pointing in the direction of step height h; Calculate the coordinate system S of each two candidate step surfaces o The angle g in the x direction x The angle g with the y direction y , if g x ,g y If both are smaller than the preset angle threshold, then these two step surfaces may belong to the same staircase, and the next step surface S T,i+1 Should be located on the upper step surface S T,i Within a certain distance; calculate S T,i+1 The centroid In S T,i The coordinate system S of the step oi The position below in, is the transformation matrix, according to the coordinate system S of each step surface oi Get; G is the global coordinate system, which is used to establish the description of the working environment space of the mobile robot; if Located in S T,i If it is within a certain area in the upper front or upper rear, it constitutes a staircase; otherwise, the step surface does not belong to this staircase.

9. The method for reaching the first step according to claim 8, characterized in that: Step 4) includes: The stairs in front of the robot are scanned horizontally by the 2D radar (12), and the width of the obstacle is obtained according to formula (1): Where D is the width of the obstacle, θ a and θ b is the maximum value of the angles at the left and right ends of the obstacle, L a and L b They are θ a and θ b The distance to the obstacle corresponding to the angular position; The distance information collected by the laser radar (13) is stored in the form of an array M[i] in order from bottom to top, where i represents the arrangement number of the depth data scan; the laser radar (13) scans the stairs in front of the robot in the vertical direction to obtain the depth and height of the obstacle; point H is the boundary point of the first step of the staircase being measured; h0 represents the camera installation height, which is a fixed value; h S 、h S0 Respectively represent the height of the first step and the height difference between the first step boundary point and the camera in the vertical direction; θ S is the angle between the line connecting the laser radar (13) and H and the horizontal direction, we can get: i S =(H-360)*0.08 (2) h S0 =M[H]*tanθ S (3) h S =h S0 +h0 (4) According to equations (2) to (4), the height parameter of the first step can be estimated by using the depth value M[H] and the arrangement number value H corresponding to the first layer boundary point screened out.

10. The method for reaching the first step according to claim 9, characterized in that: In step 5), during the stair climbing process, after acquiring the data from each sensor, the robot chassis obtains the real-time position of the robot through the motion model. The motion model is a combination of position estimation and direction estimation, both of which use a constant velocity / angular velocity model; The state vector contains position, velocity, Euler angles and velocity of Euler angles. The Euler angles are the roll angle α, pitch angle β and yaw angle γ of the robot. Rotation matrix composed of Euler angles Among them, R α is the rotation matrix of the roll angle, R β is the rotation matrix of the pitch angle, R γ is the rotation matrix of the yaw angle; The angular velocity of the Euler angle is converted to the linear velocity V in the robot's local coordinate system through the rotation matrix local =R α,β,γ [α,β,γ] T ; Among them, R ′ represents the covariance matrix of the velocity and orientation estimates in the robot motion model, and is used to quantify the uncertainty of these estimates; variance and Represents the mass of the robot's velocity and orientation constraints.

Images