Autonomous stair climbing control method for small wheel-track switchable mobile robot

Abstract

The invention discloses an autonomous stair climbing control method for a small wheel-track switchable mobile robot, which belongs to the field of mobile robots, and comprises the following steps: measuring geometric parameters of steps and the relative position of the robot through a visual sensor, and controlling the robot to approach in a wheel type mode and correct the direction under the fusion of the sensor; unfolding the track after judging that the front end of the track is contacted with the step according to the pitching angle change, and adjusting the support angle and completely unfolding the track after judging that the tail support rod is contacted with the ground; a robot Euler angle is obtained based on inertial sensor data, a step edge normal vector is combined to calculate an angle relation between a track and a step plane, and accordingly a driving motor and a supporting rod motor are controlled to adjust postures; a driving motor is controlled to enable the robot to climb along the sideline of the step, and the advancing direction is corrected in real time through fusion of vision and inertial sensors; and after the robot reaches the last step according to visual detection, the driving motor and the supporting rod motor are cooperatively controlled to enable the robot to leave the steps. The method is accurate in control and stable in operation.

Description

Technical Field

[0001] This invention belongs to the field of mobile robots, and in particular relates to a control method for autonomous stair climbing of a small wheel-track switchable mobile robot. Background Technology

[0002] Due to the rapid development of electronic information technology, mobile robots are widely used in fields such as reconnaissance, search and rescue, mine clearance, and inspection. Compared to humans, mobile robots are better able to adapt to extreme environments such as high temperatures, extreme cold, and radiation. With the continuous advancement of sensor technology, communication technology, and artificial intelligence technology, the environmental perception and autonomous decision-making capabilities of mobile robots are constantly improving. They have evolved from initial wire-controlled systems to short-range wireless communication control, and then to space-based long-range wireless communication control, and are currently developing towards autonomous decision-making and multi-robot collaborative cluster control. Mobile robots can be classified according to their locomotion mechanism into wheeled robots, tracked robots, and legged robots. Combining these robots results in various configurations, such as wheeled-tracked robots and wheeled-legged robots. Ground-based wheel-tracked unmanned platforms can switch between wheeled and tracked modes to adapt to different types of environments. For example, wheeled mode can be used to increase speed on smooth surfaces with high adhesion coefficients, such as asphalt and cement, while tracked mode can be used on unpaved surfaces, steep slopes, or in the presence of trenches. By adding a tail strut, autonomous stair-climbing functionality can be achieved.

[0003] Therefore, in terms of mobile platform configuration, the single wheeled or tracked structure has evolved into a hybrid form that can switch between wheel and track. This configuration can switch between wheeled and tracked driving modes according to different terrain environments, taking into account both high-speed mobility on flat roads and high passability in complex terrain. The addition of a rear strut mechanism further expands the potential for climbing stairs.

[0004] However, small wheel-tracked mobile robots are limited by their size and center of gravity when facing large obstacles such as stairs, and cannot directly traverse them using either wheeled or tracked modes. Secondly, the limited payload capacity of small platforms restricts the types and functions of sensors that can be mounted, making it difficult to acquire complete and accurate environmental information, leading to high uncertainty in the obstacle-crossing process. Current technologies primarily rely on skilled operators for remote control based on visual information. However, in extreme environments where humans cannot access and communication may be interrupted, this human-intervention-dependent control method cannot meet mission requirements. Therefore, there is an urgent need for a control method that enables small wheel-tracked mobile robots to autonomously traverse stairs. Summary of the Invention

[0005] To address the aforementioned technical problems, this invention provides an autonomous stair-climbing control method for a small wheel-track switchable mobile robot, comprising:

[0006] The robot measures the geometric dimensions of the steps using a visual sensor and obtains the relative position information between the robot and the steps.

[0007] Based on the relative position information, the robot is controlled to approach the steps in a wheeled manner, and the direction of travel is corrected in real time through sensor fusion during the approach.

[0008] Based on the change in the robot's pitch angle, once the front end of the track contacts the edge of the step, the robot controls the deformation mechanism to unfold the wheel structure into track mode.

[0009] Based on the change in the robot's pitch angle, after the tail support rod contacts the ground, the robot controls the support angle of the support rod and fully deploys the tracks.

[0010] The robot's Euler angles are obtained based on inertial sensor data. The angular relationship between the track plane and the step edge plane is calculated by combining the normal vector of the step edge plane. The robot's posture is adjusted by controlling the drive motor and the strut motor according to the angular relationship.

[0011] The robot climbs along the edge of the steps by controlling the speed of the drive motors on both sides, and corrects its direction of travel in real time by using fused data from visual and inertial sensors during the climbing process.

[0012] Based on the step edge information detected by the vision sensor, once the robot reaches the last step, the drive motor and the strut motor work together to make the robot leave the step.

[0013] Preferably, the process of measuring the geometric dimensions of the steps and obtaining the relative position information between the robot and the steps using a vision sensor includes:

[0014] Staircase point cloud data is acquired using a binocular camera, and voxel filtering preprocessing is performed on the information point cloud data.

[0015] The preprocessed point cloud data is segmented into multiple planar clusters using the RANSAC algorithm. Each planar cluster is characterized by its center point, normal vector, and planar dimensions.

[0016] The step height is obtained by calculating the distance between adjacent horizontal plane clusters, the step depth is obtained by calculating the distance between adjacent vertical plane clusters, and the step width is obtained by calculating the average size of the planar clusters.

[0017] The shortest distance between the robot's front plane and the segmented vertical plane cluster is calculated to obtain the relative position distance between the robot's coordinate system origin and the first step.

[0018] Preferably, the process of controlling the robot to approach the steps in a wheeled manner based on the relative position information, and correcting the direction of travel in real time through sensor fusion during the approach, includes:

[0019] Establish the state-space equation of the robot in the world coordinate system, and the input of the state-space equation is the rotational speed of the two drive motors;

[0020] The distance between the relative positions and the integral value of the robot's longitudinal velocity is used to determine whether it is close to the step;

[0021] During the approach process, a state observer is designed based on the fusion information of the camera and the inertial sensor to obtain the track slip rate and the robot's center of mass sideslip angle;

[0022] The travel direction is corrected by adjusting the speed difference between the two drive motors in real time based on the output value of the state observer.

[0023] Preferably, the robot's relative position, step geometry parameters, and step edge information are acquired using a binocular camera;

[0024] The robot's Euler angles, pitch angles, and travel distance are obtained using inertial sensors.

[0025] The rotation matrix estimate obtained by the visual sensor is fused with the angular velocity integral value of the inertial sensor, and the cumulative error of the inertial measurement is corrected by the visual estimate.

[0026] Preferably, the process of controlling the deformation mechanism to unfold the wheeled structure into a tracked mode includes:

[0027] The control motor drives the four-bar linkage to deploy the tracks;

[0028] The robot's pitch angle is obtained in real time using inertial sensors;

[0029] When the pitch angle exceeds the preset threshold, it is determined that the front end of the track has contacted the edge of the step, and the deformation motor stops rotating.

[0030] Preferably, the process of controlling the strut support angle and fully deploying the tracks includes:

[0031] The control strut motor drives the four-bar linkage mechanism to rotate the strut;

[0032] The robot's pitch angle is monitored in real time by inertial sensors. When the pitch angle change is greater than a preset threshold, it is determined that the support rod has made contact with the ground and the support rod motor stops rotating.

[0033] The control motor continues to rotate until the tracks are fully extended, the fully extended position is defined by an internal Hall sensor.

[0034] Preferably, the process of controlling the drive motor and the strut motor to adjust the robot's posture according to the angular relationship includes:

[0035] The robot's angular velocity is obtained by inertial sensors and integrated to obtain Euler angle measurements.

[0036] The robot's Euler angles are estimated by inversely calculating the rotation matrix from the camera coordinate system to the world coordinate system, and the integral cumulative error of the inertial sensor is corrected based on the estimated Euler angles.

[0037] Calculate the coordinates of the normal vector of the step edge plane in the world coordinate system based on the geometric dimensions of the step, and transform the unit vector of the z-axis of the vehicle coordinate system to the world coordinate system to obtain the normal vector of the track plane.

[0038] Calculate the direction cosine difference between the normal vector of the step edge plane and the normal vector of the track plane, and control the speed difference of the drive motors on both sides and the rotation angle of the strut motor based on the direction cosine difference.

[0039] Preferably, the process of real-time correction of the direction of travel by fusing data from visual and inertial sensors during the ascent includes:

[0040] The anti-overturning limit angle is calculated based on the projection of the robot's center of mass in the world coordinate system and the coordinates of the contact point; the anti-slip limit angle is calculated based on the elastic modulus of the track.

[0041] The minimum value of the anti-overturning limit angle and the anti-slip limit angle is taken as the limit value of the yaw angle in the direction of travel;

[0042] Canny edge detection and directional filtering are performed using a visual sensor to extract the step edge.

[0043] When there is no step edge in the visual image that meets the orientation condition, it is determined that the robot has reached the last step.

[0044] Preferably, the process of controlling the coordinated action of the drive motor and the strut motor includes:

[0045] The drive motor is controlled to make the robot continue to travel a preset distance, which is determined based on the camera's field of view and the robot's body size.

[0046] During the movement, the rotation angle of the strut motor is controlled to reduce the robot's pitch angle. The robot completes the stair climbing process after it has completely crossed the last step.

[0047] Preferably, the small wheel-track switchable mobile robot includes a mobile robot body, a binocular camera, an inertial sensor, and a small industrial computer;

[0048] The mobile robot body includes a body, wheel-track deformable hubs, and a strut system;

[0049] The machine body is equipped with a strut motor and its controller, a reduction gear set, a power battery, a voltage regulator module, a main controller, and a remote communication module.

[0050] The wheel-track deformable hub includes a drive motor and its controller and a reduction gear set, a deformable motor and its controller and a reduction gear set, a track assembly, and a four-bar deformable mechanism;

[0051] The strut system includes struts and a four-bar linkage that drives their rotation.

[0052] Compared with the prior art, the present invention has the following advantages and technical effects:

[0053] This invention enables a small wheel-tracked mobile robot to autonomously acquire stair geometry parameters and robot pose information through the fusion of binocular cameras and inertial sensors without relying on external communication or human intervention. Based on a multi-stage cooperative control strategy, it completes the entire process of approaching, entering, climbing, and leaving the staircase autonomously, effectively improving the autonomous obstacle-crossing capability and task execution reliability of small mobile robots in extreme environments. Attached Figure Description

[0054] The accompanying drawings, which form part of this application, are used to provide a further understanding of this application. The illustrative embodiments and descriptions of this application are used to explain this application and do not constitute an undue limitation of this application. In the drawings:

[0055] Figure 1 This is a schematic diagram of the perception and control system assembly of a wheeled deformable mobile robot with a tail strut according to an embodiment of the present invention.

[0056] Figure 2 This is a schematic diagram illustrating the staircase parameters during the binocular camera measurement process according to an embodiment of the present invention.

[0057] Figure 3 This is a schematic diagram of the kinematic relationship of the track mode trajectory control of the wheel-track deformable robot according to an embodiment of the present invention;

[0058] Figure 4 This is a schematic diagram of the overall framework for the autonomous hill-climbing control of the wheel-track deformable robot according to an embodiment of the present invention;

[0059] The components include: 1. Robot body; 2. Binocular camera; 3. Inertial sensor; 4. Small industrial computer; 1-1. Body; 1-2. Wheel-track deformable hub; 1-3. Support system. Detailed Implementation

[0060] It should be noted that, unless otherwise specified, the embodiments and features described in this application can be combined with each other. This application will now be described in detail with reference to the accompanying drawings and embodiments.

[0061] It should be noted that the steps shown in the flowchart in the accompanying drawings can be executed in a computer system such as a set of computer-executable instructions, and although a logical order is shown in the flowchart, in some cases the steps shown or described may be executed in a different order than that shown here.

[0062] This embodiment provides a method for controlling the autonomous stair climbing of a small wheel-track switchable mobile robot, including:

[0063] The robot measures the geometric dimensions of the steps using a visual sensor and obtains the relative position information between the robot and the steps.

[0064] The robot is controlled to approach the steps in a wheeled manner based on relative position information, and its direction of travel is corrected in real time through sensor fusion during the approach.

[0065] Based on the change in the robot's pitch angle, once the front end of the track contacts the edge of the step, the robot controls the deformation mechanism to unfold the wheel structure into track mode.

[0066] Based on the change in the robot's pitch angle, after the tail support rod contacts the ground, the robot controls the support angle of the support rod and fully deploys the tracks.

[0067] The robot's Euler angles are obtained based on data from inertial sensor 3. The angular relationship between the track plane and the step edge plane is calculated by combining the normal vector of the step edge plane. The robot's posture is adjusted by controlling the drive motor and the strut motor according to the angular relationship.

[0068] The robot climbs along the edge of the steps by controlling the speed of the drive motors on both sides, and corrects its direction of travel in real time by using the fusion data of the vision sensor and the inertial sensor 3 during the climbing process.

[0069] Based on the step edge information detected by the vision sensor, once the robot reaches the last step, the drive motor and the strut motor work together to make the robot leave the step.

[0070] Furthermore, the process of measuring the geometric dimensions of the steps and obtaining the relative position information between the robot and the steps using visual sensors includes:

[0071] Staircase point cloud data is acquired using binocular camera 2, and voxel filtering preprocessing is performed on the information point cloud data.

[0072] The preprocessed point cloud data is segmented into multiple planar clusters using the RANSAC algorithm. Each planar cluster is characterized by its center point, normal vector, and planar dimensions.

[0073] The step height is obtained by calculating the distance between adjacent horizontal plane clusters, the step depth is obtained by calculating the distance between adjacent vertical plane clusters, and the step width is obtained by calculating the average size of the planar clusters.

[0074] The shortest distance between the robot's front plane and the segmented vertical plane cluster is calculated to obtain the relative position distance between the robot's coordinate system origin and the first step.

[0075] Furthermore, the process of controlling the robot to approach the steps in a wheeled manner based on relative position information, and correcting the direction of travel in real time through sensor fusion during the approach, includes:

[0076] Establish the state-space equation of the robot in the world coordinate system. The input of the state-space equation is the rotational speed of the two drive motors.

[0077] The distance between the relative positions and the integral value of the robot's longitudinal velocity is used to determine whether it is close to the step;

[0078] During the approach process, a state observer is designed based on the fusion information of the camera and inertial sensor 3 to obtain the track slip rate and the robot's center of mass side slip angle;

[0079] The travel direction is corrected by adjusting the speed difference between the two drive motors in real time based on the output value of the state observer.

[0080] Furthermore, the robot's relative position, step geometry parameters, and step edge information are acquired through the binocular camera 2;

[0081] The robot's Euler angles, pitch angles, and travel distance are obtained through inertial sensor 3.

[0082] The rotation matrix estimate obtained by the visual sensor is fused with the angular velocity integral value of the inertial sensor 3, and the cumulative error of the inertial measurement is corrected by the visual estimate.

[0083] Furthermore, the process by which the deformation mechanism unfolds the wheeled structure into a tracked mode includes:

[0084] The control motor drives the four-bar linkage to deploy the tracks;

[0085] The robot's pitch angle is acquired in real time using inertial sensor 3;

[0086] When the pitch angle exceeds the preset threshold, it is determined that the front end of the track has contacted the edge of the step, and the deformation motor stops rotating.

[0087] Furthermore, the process of controlling the strut support angle and fully deploying the tracks includes:

[0088] The control strut motor drives the four-bar linkage mechanism to rotate the strut;

[0089] The robot's pitch angle is monitored in real time by inertial sensor 3. When the pitch angle change is greater than a preset threshold, it is determined that the support rod has made contact with the ground and the support rod motor stops rotating.

[0090] The control motor continues to rotate until the tracks are fully extended, the fully extended position is defined by an internal Hall sensor.

[0091] Furthermore, the process of adjusting the robot's posture by controlling the drive motor and the strut motor according to the angular relationship includes:

[0092] The robot's angular velocity is obtained by inertial sensor 3 and integrated to obtain the Euler angle measurement value;

[0093] The robot's Euler angles are estimated by inversely calculating the rotation matrix from the camera coordinate system to the world coordinate system, and the integral cumulative error of inertial sensor 3 is corrected based on the Euler angle estimates.

[0094] Calculate the coordinates of the normal vector of the step edge plane in the world coordinate system based on the geometric dimensions of the step, and transform the unit vector of the z-axis of the vehicle coordinate system to the world coordinate system to obtain the normal vector of the track plane.

[0095] Calculate the direction cosine difference between the normal vector of the step edge plane and the normal vector of the track plane, and control the speed difference of the drive motors on both sides and the rotation angle of the strut motor based on the direction cosine difference.

[0096] Furthermore, the process of real-time correction of the direction of travel during the ascent by fusing data from the visual sensor and the inertial sensor 3 includes:

[0097] The anti-overturning limit angle is calculated based on the projection of the robot's center of mass in the world coordinate system and the coordinates of the contact point; the anti-slip limit angle is calculated based on the elastic modulus of the track.

[0098] The minimum value of the anti-overturning limit angle and the anti-slip limit angle is taken as the limit value of the yaw angle in the direction of travel;

[0099] Canny edge detection and directional filtering are performed using a visual sensor to extract the step edge.

[0100] When there is no step edge in the visual image that meets the orientation condition, it is determined that the robot has reached the last step.

[0101] Furthermore, the process of controlling the coordinated action of the drive motor and the strut motor includes:

[0102] The drive motor is controlled to make the robot continue to travel a preset distance, which is determined based on the camera's field of view and the size of the robot body 1.

[0103] During the movement, the rotation angle of the strut motor is controlled to reduce the robot's pitch angle. The robot completes the stair climbing process after it has completely crossed the last step.

[0104] Furthermore, such as Figure 1As shown, the small wheel-track switchable mobile robot includes a mobile robot body 1, a binocular camera 2, an inertial sensor 3, and a small industrial control computer 4;

[0105] The mobile robot body 1 includes a body 1-1, a wheel-track deformable hub 1-2, and a strut system 1-3;

[0106] The machine body 1-1 is equipped with a strut motor and its controller, a reduction gear set, a power battery, a voltage regulator module, a main controller, and a remote communication module.

[0107] The wheel-track deformable hub 1-2 includes a drive motor and its controller and a reduction gear set, a deformable motor and its controller and a reduction gear set, a track assembly, and a four-bar deformable mechanism;

[0108] The strut system 1-3 includes struts and a four-bar linkage that drives their rotation.

[0109] A Realsense D415 binocular camera 2 is mounted on top of the robot to measure the distance between the robot body 1 and the step and to detect and identify the step edge. The step edge information is used for robot-assisted pose estimation and step geometry estimation. A host computer integrating an Nvidia Jetson Nano and a Wheeltech 9-axis IMU are mounted on the back of the robot. The IMU is used to measure the robot's real-time pose, including the Euler angles of the robot's coordinate system relative to the world coordinate system and its travel distance. The host computer is used to process the visual information from the binocular camera 2 and the IMU information in real time.

[0110] The wheel-tracked deformable robot involved in this embodiment uses a wheel-driven mode to approach the steps and unfold its tracks to place them on the steps to climb the stairs. Since the size of the climbing stairs is limited by the wheel hub diameter and the position of the robot's center of gravity, this embodiment uses a tail support rod mechanism to support the ground and adjust the height of the center of gravity, thereby removing the limitation of the center of gravity position on the height of the climbable steps. During the climbing process, the robot's real-time posture is obtained through the fusion perception of the camera and the inertial sensor 3, and this is used as input to control the direction of travel and the output speed of the drive motor. The tail support rod serves as a support redundancy to maintain the stability of the climbing process. Near the end of the stairs, the angle of the support rod is adjusted to raise the height of the center of gravity and coordinate with the control of the drive wheel speed to achieve the final crossing.

[0111] Furthermore, in this embodiment, the active stair climbing process of the wheeled deformable robot, which uses a camera and an inertial sensor 3 to fuse the pose perception and adjust the speed of the support rod and the drive wheel to complete the four processes of approaching, entering, climbing, and leaving, includes the following 7 steps: the approaching process includes steps 1 and 2, the entering process includes steps 3, 4, and 5, the climbing process includes step 6, and the leaving process includes step 7.

[0112] Specifically, step 1: The robot stops at a suitable distance from the stairs and uses binocular camera 2 to measure the geometric dimensions of the steps to be climbed, including the height and depth of the steps. If the height of the steps is greater than the robot's maximum climbing height, the robot abandons the climb. If the height of the steps to be climbed is less than or equal to the maximum climbing height, the robot continues to step 2.

[0113] In this step, as an additional implementation method, such as Figure 2 As shown, the robot stops at a suitable distance from the stairs and uses binocular camera 2 to measure the geometric dimensions of the steps to be climbed, including the height and depth of the steps. The geometric feature parameters of the stairs can be represented as a quintuple. .in, These represent the height, depth, and width of the staircase, respectively. Representing the camera coordinate system With world coordinate system The rotation matrix and displacement vector.

[0114] The measurement process includes three main steps: data preprocessing, plane segmentation, and feature extraction and computation. Data preprocessing involves removing point cloud data outside the staircase area using body beam filtering to reduce the amount of data processing. The plane segmentation method uses the RANSAC algorithm, which divides the point cloud data into Np clusters, each cluster representing a plane. For each segmented plane i, a triplet is used to represent it. , where c i n represents the center point of the plane. i This represents the normal vector of the plane. This represents the planar dimensions, which can be obtained through principal component analysis (PCA).

[0115] The height h of a stair step is obtained by calculating the distance between two adjacent horizontal planes; the depth of a stair step is obtained by calculating the distance d between two adjacent vertical planes; and the width w of the plan can be obtained by calculating the plan dimensions. The mean was obtained.

[0116] It should be noted that the normal vector of the horizontal plane of the stairs is along the world coordinate system. The vertical upward unit vector Its representation in the camera coordinate system is as unit vector, Rotation matrix It can be obtained by inverse calculation. Similarly. This represents the center coordinates of plane i in the camera coordinate system, and the world coordinate system. The origin is located at this point, and the vector can be obtained by inverse calculation. If the step height is greater than the robot's maximum climbable height, the robot abandons the climb; if the step height is less than or equal to the maximum climbable height, the robot continues to step 2.

[0117] Step 2: The binocular camera 2 measures the distance to determine the relative position of the robot's coordinate system origin and the step in the world coordinate system. The speed of the left and right drive motors is controlled to make the robot approach the step. During the approach process, the robot controls the direction of travel by fusing information from the camera and inertial sensor 3. When the wheel hub contacts or gets close enough to the top edge of the step, the robot stops and proceeds to step 3.

[0118] In this step, as an optional implementation, binocular camera 2 measures the distance to determine the robot's relative position to the origin of the robot's coordinate system in the world coordinate system and the first step. The robot's front-end plane information is prior knowledge, obtained by calculating the shortest distance between it and the vertical plane obtained from clustering in step 1, and is represented as... After obtaining this distance, control the speed of the left and right drive motors to bring the robot closer to the step, such as... Figure 3 As shown, the state-space equation controlling the trajectory in the world coordinate system can be expressed as:

[0119] ;

[0120] in , , These represent the longitudinal velocity, lateral velocity, and yaw rate of the robot's center of mass, respectively. , These represent the speeds of the drive motors on the low-speed and high-speed sides, respectively. , These represent the slip ratios of the low-speed and high-speed tracks, respectively, which can be calculated based on the relationship between relative speed and coupling speed. , v l and v h Let these represent the entrapment velocities for low-speed and high-speed measurements, respectively, which can be expressed as: , , B represents the sideslip angle of the robot body 1, and B represents the distance between the center lines of the two tracks. This indicates the transmission ratio from the drive motor to the drive wheel hub in tracked drive mode. This indicates the hub radius. The parameter... , , The state observer is designed by fusing information from a camera and an inertial sensor. When the longitudinal velocity... integral and distance When the difference is less than the set threshold, it can be considered that the wheel hub is close enough to the upper edge of the first step, and proceed to step 3.

[0121] Step 3: Control the rotation angle of the deformation motor to drive the four-bar deformation mechanism to spread the track. The robot pitch angle is obtained through the inertial sensor 3. When the pitch angle increases significantly, it can be determined that the front end of the track contacts the edge of the step, and the deformation is stopped to proceed to step 4. Among them, when the pitch angle is greater than a given threshold, it is considered that the robot has undergone a significant pitch change.

[0122] Step 4: Control the rotation angle of the strut motor to drive the four-bar linkage to rotate the strut until it contacts the ground. When the pitch angle obtained by the inertial sensor 3 decreases significantly, it can be determined that the strut is in contact with the ground. The contact point between the robot and the ground is transferred from the rear end of the track to the strut. Stop the strut rotation and control the rotation angle of the track deformation motor until the track is fully extended. The termination position is limited by the internal Hall sensor. When the track is fully extended, the track deformation process is automatically terminated, and proceed to step 5.

[0123] Step 5: Determine the Euler angles of the robot's world coordinate system based on the inertial sensor 3, and determine the normal vector of the plane containing the stair edge based on the step dimensions measured in Step 1. This determines the positional relationship between the track plane and the plane containing the step edge. If the normal vector of the track plane is parallel to the normal vector of the plane containing the step edge, no operation is required and Step 6 can be performed directly. If there is a significant angle between the normal vector of the track plane and the normal vector of the plane containing the step edge, the angle is reduced to less than the threshold by controlling the speed difference of the drive motors on both sides and the rotation angle of the strut motor before proceeding to Step 6.

[0124] In this step, as an optional implementation, inertial sensor 3 is used to measure the robot's Euler angles in the world coordinate system in real time. Since this angle value is obtained by integrating the angular velocity, the accumulated measurement error of the angular velocity over a certain period of time will lead to an unacceptable deviation in the angle measurement. According to step 1, the rotation matrix from the camera coordinate system to the world coordinate system can be obtained by using the coordinates of the stair plane normal vector in the world coordinate system and the camera coordinate system. This matrix can be expressed as... ,in Indicates from the camera coordinate system To the robot body coordinate system 1 The rotation matrix, determined by the camera's position, is prior knowledge, allowing estimation based on camera information. This allows for the indirect calculation of the robot's Euler angles in the world coordinate system. Due to limitations in camera image processing speed, this measurement cannot be used for real-time control feedback, but it can serve as a correction for the inertial sensor 3 to reduce accumulated errors. Based on the step dimensions measured in step 1, the inclination angle of the plane containing the stair edge can be determined. This allows us to determine the normal vector of the plane containing the edge of the staircase. The normal vector of the track plane can be considered as the unit vector of the z-axis of the vehicle coordinate system. ,according to Its coordinates in the world coordinate system can be calculated. This allows us to determine the positional relationship between the track plane and the plane containing the step edge. If the normal vector of the track plane is parallel to the normal vector of the plane containing the step edge, then no operation is required and step 6 can be performed directly. If there is a significant angle between the normal vector of the track plane and the normal vector of the plane containing the step edge, a PID controller is designed to control the speed difference between the two drive motors and the rotation angle of the strut motor by using the cosine difference of the directions of the two vectors in the world coordinate system, so as to reduce the angle to less than the threshold, and then step 6 is performed.

[0125] Step 6: Control the speed of the drive motors on both sides to make the robot move upward along the plane of the step edge. The robot travels the shortest distance when traveling in a direction perpendicular to the step edge, but the pitch angle is larger and the motor driving force required is larger. If the robot's travel direction is not perpendicular to the step edge, the travel distance is larger but the pitch angle is smaller and the driving force required is smaller. The tilt angle of this travel direction should not be too large, as an excessive tilt angle will cause the robot to overturn or the tracks to be subjected to excessive lateral force and disengage. The robot's pose is observed in real time by the inertial sensor 3 and the binocular camera 2, and the travel direction is corrected until the robot reaches the last step. The binocular camera 2 obtains the step edge by adding a filter to the edge line. When there is no step edge line in the visual image, it can be considered that the last step has been reached. At this time, the robot stops moving and proceeds to step 7.

[0126] In this step, as an additional implementation, the rotational speeds of the two drive motors are controlled according to the state-space equation in step 2 to make the robot move upwards along the plane of the step edge. The robot travels the shortest distance in a direction perpendicular to the step edge, but the pitch angle is larger, requiring greater motor driving force. If the robot's direction of travel is not perpendicular to the step edge, the travel distance is larger, but the pitch angle is smaller, requiring less driving force. The tilt angle in this direction should not be too large, as an excessively large tilt angle can cause the robot to overturn or the tracks to become dislodged due to excessive lateral force. The angle at which the robot overturns can be obtained by comparing the projection of the center of mass in the world coordinate system with the coordinates of the contact point. The lateral force that causes the track to detach can be calculated based on the elastic modulus of the rubber track, and thus the angle can be determined through force analysis. The minimum value of the two is taken. This serves as the limit for deviation from the direction of travel.

[0127] The robot's pose is detected in real time according to the method in step 5, and the direction of travel is corrected until the robot reaches the last step. The binocular camera 2 obtains the edge line through the edge line Canny edge line detection method, and obtains the edge line with a small deviation from the horizontal axis of the world coordinate system through directional filtering as the step edge line. When there is no step edge line in the visual image, it can be considered that the last step has been reached. At this time, the robot stops moving and proceeds to step 7.

[0128] Step 7: Control the rotation speed of the drive motors on both sides to allow the robot to continue traveling a certain distance so that the front end of the track can pass the last step of the stairs. The traveling distance is determined by the camera's field of view and the dimensions of the robot body 1. The traveling distance can be taken as half the longitudinal length of the robot. Then, control the rotation angle of the support motor and use the data from the inertial sensor 3 to measure the robot's pitch angle in real time, so as to reduce the robot's pitch angle and continue traveling to pass the last step, completing the entire stair climbing process. The overall control framework is as follows: Figure 4 As shown.

[0129] In this embodiment, the distance between the robot entering the stage and the first step of the stairs, the geometric dimensions of the stairs to be climbed, and the direction of the normal vector of the plane containing the edge of the step are observed by binocular camera 2 to confirm the position of the last step of the exit stage.

[0130] In this embodiment, the robot's pose angle during the climbing process is observed in real time by fusing binocular camera 2 and inertial sensor 3, thereby obtaining the relationship between the normal vector of the plane where the track is located and the normal vector angle of the plane where the stair edge is located during the entry phase, the traveling direction of the robot body 1 during the climbing phase, and the pitch angle of the robot body 1 during the exit phase; the entry phase of the stair climbing process is realized by controlling the deformation of the wheeled to tracked travel mode.

[0131] In this embodiment, the pitch angle of the robot body 1 is adjusted by controlling the rotation angle of the tail support rod to eliminate the height limitation of the center of gravity on the staircase to be climbed during the robot's entry phase of climbing stairs. During the exit phase of the climbing process, the pitch angle of the robot body 1 is adjusted so that the robot leaves the steps stably without tipping over.

[0132] This embodiment achieves three autonomous stair-climbing processes—approaching, climbing, and leaving—by controlling the output speed of the drive motors on both sides of the robot. During these processes, the direction of travel is controlled by the speed difference between the drive motors on both sides. The control method of this invention uses dual-sensor information fusion feedback to control the rotation angle of the tail strut motor, the rotation angle of the track deformation motor, and the output speed of the drive motors on both sides to complete the complete autonomous stair-climbing process of approaching, entering, climbing, and leaving. The control method of this embodiment is clearly structured, precise, and stable in operation.

[0133] The above are merely preferred embodiments of this application, but the scope of protection of this application is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in this application should be included within the scope of protection of this application. Therefore, the scope of protection of this application should be determined by the scope of the claims.

Claims

1. A method for controlling the autonomous stair climbing of a small wheel-track switchable mobile robot, characterized in that, include: The robot measures the geometric dimensions of the steps using a visual sensor and obtains the relative position information between the robot and the steps. Based on the relative position information, the robot is controlled to approach the steps in a wheeled manner, and the direction of travel is corrected in real time through sensor fusion during the approach. Based on the change in the robot's pitch angle, once the front end of the track contacts the edge of the step, the robot controls the deformation mechanism to unfold the wheel structure into track mode. Based on the change in the robot's pitch angle, after the tail support rod contacts the ground, the robot controls the support angle of the support rod and fully deploys the tracks. The robot's Euler angles are obtained based on inertial sensor data. The angular relationship between the track plane and the step edge plane is calculated by combining the normal vector of the step edge plane. The robot's posture is adjusted by controlling the drive motor and the strut motor according to the angular relationship. The robot climbs along the edge of the steps by controlling the speed of the drive motors on both sides, and corrects its direction of travel in real time by using fused data from visual and inertial sensors during the climbing process. Based on the step edge information detected by the vision sensor, once the robot reaches the last step, the drive motor and the strut motor work together to make the robot leave the step.

2. The method according to claim 1, characterized in that, The process of measuring the geometric dimensions of the steps and obtaining the relative position information of the robot and the steps using vision sensors includes: Staircase point cloud data is acquired using a binocular camera, and voxel filtering preprocessing is performed on the information point cloud data. The preprocessed point cloud data is segmented into multiple planar clusters using the RANSAC algorithm. Each planar cluster is characterized by its center point, normal vector, and planar dimensions. The step height is obtained by calculating the distance between adjacent horizontal plane clusters, the step depth is obtained by calculating the distance between adjacent vertical plane clusters, and the step width is obtained by calculating the average size of the planar clusters. The shortest distance between the robot's front plane and the segmented vertical plane cluster is calculated to obtain the relative position distance between the robot's coordinate system origin and the first step.

3. The method according to claim 1, characterized in that, The process of controlling the robot to approach the steps in a wheeled mode based on the relative position information, and correcting the direction of travel in real time through sensor fusion during the approach, includes: Establish the state-space equation of the robot in the world coordinate system, and the input of the state-space equation is the rotational speed of the two drive motors; The distance between the relative positions and the integral value of the robot's longitudinal velocity is used to determine whether it is close to the step; During the approach process, a state observer is designed based on the fusion information of the camera and the inertial sensor to obtain the track slip rate and the robot's center of mass sideslip angle; The travel direction is corrected by adjusting the speed difference between the two drive motors in real time based on the output value of the state observer.

4. The method according to claim 1, characterized in that, The sensor fusion includes: The robot's relative position, step geometry parameters, and step edge information are obtained using a binocular camera. The robot's Euler angles, pitch angles, and travel distance are obtained using inertial sensors. The rotation matrix estimate obtained by the visual sensor is fused with the angular velocity integral value of the inertial sensor, and the cumulative error of the inertial measurement is corrected by the visual estimate.

5. The method according to claim 1, characterized in that, The process by which the control deformation mechanism unfolds the wheeled structure into tracked mode includes: The control motor drives the four-bar linkage to deploy the tracks; The robot's pitch angle is obtained in real time using inertial sensors; When the pitch angle exceeds the preset threshold, it is determined that the front end of the track has contacted the edge of the step, and the deformation motor stops rotating.

6. The method according to claim 1, characterized in that, The process of controlling the strut support angle and fully deploying the tracks includes: The control strut motor drives the four-bar linkage mechanism to rotate the strut; The robot's pitch angle is monitored in real time by inertial sensors. When the pitch angle change is greater than a preset threshold, it is determined that the support rod has made contact with the ground and the support rod motor stops rotating. The control motor continues to rotate until the tracks are fully extended, the fully extended position is defined by an internal Hall sensor.

7. The method according to claim 1, characterized in that, The process of adjusting the robot's posture by controlling the drive motor and the strut motor according to the aforementioned angular relationship includes: The robot's angular velocity is obtained by inertial sensors and integrated to obtain Euler angle measurements. The robot's Euler angles are estimated by inversely calculating the rotation matrix from the camera coordinate system to the world coordinate system, and the integral cumulative error of the inertial sensor is corrected based on the estimated Euler angles. Calculate the coordinates of the normal vector of the step edge plane in the world coordinate system based on the geometric dimensions of the step, and transform the unit vector of the z-axis of the vehicle coordinate system to the world coordinate system to obtain the normal vector of the track plane. Calculate the direction cosine difference between the normal vector of the step edge plane and the normal vector of the track plane, and control the speed difference of the drive motors on both sides and the rotation angle of the strut motor based on the direction cosine difference.

8. The method according to claim 1, characterized in that, The process of correcting the direction of travel in real time by fusing data from visual and inertial sensors during the ascent includes: The anti-overturning limit angle is calculated based on the projection of the robot's center of mass in the world coordinate system and the coordinates of the contact point; the anti-slip limit angle is calculated based on the elastic modulus of the track. The minimum value of the anti-overturning limit angle and the anti-slip limit angle is taken as the limit value of the yaw angle in the direction of travel; Canny edge detection and directional filtering are performed using a visual sensor to extract the step edge. When there is no step edge in the visual image that meets the orientation condition, it is determined that the robot has reached the last step.

9. The method according to claim 1, characterized in that, The process of controlling the coordinated movement of the drive motor and the strut motor includes: The drive motor is controlled to make the robot continue to travel a preset distance, which is determined based on the camera's field of view and the robot's body size. During the movement, the rotation angle of the strut motor is controlled to reduce the robot's pitch angle. The robot completes the stair climbing process after it has completely crossed the last step.

10. The method according to claim 1, characterized in that, The small wheel-track switchable mobile robot includes a mobile robot body, a binocular camera, an inertial sensor, and a small industrial control computer; The mobile robot body includes a body, wheel-track deformable hubs, and a strut system; The machine body is equipped with a strut motor and its controller, a reduction gear set, a power battery, a voltage regulator module, a main controller, and a remote communication module. The wheel-track deformable hub includes a drive motor and its controller and a reduction gear set, a deformable motor and its controller and a reduction gear set, a track assembly, and a four-bar deformable mechanism; The strut system includes struts and a four-bar linkage that drives their rotation.

Images