A control method for a quadruped robot in complex terrain

Abstract

The present invention discloses a control method for a quadruped robot applied to complex terrain, comprising the following steps: S1, after the image processing device on the quadruped robot obtains visual signals, the signals are input to the perception host unit to obtain the robot posture, scoring map and height map; S2, the scoring map and height map are converted to the world coordinate system of motion control; S3, after the motion host obtains the information of step S1 and step S2, the initial foothold of the quadruped robot is estimated; S4, the foothold optimization module comprehensively compares the factors of score, height and distance from the initial foothold in the scoring map and height map near the initial foothold, and determines the final foothold; S5, the expected reference speed of the quadruped robot is reversely calculated according to the final foothold position. The present invention is not only suitable for regular staircase road conditions, but also can overcome complex road conditions, and has good adaptability; the foothold selection method enables a more suitable foothold to be selected, and the walking stability of the quadruped robot is higher.

Description

Technical Field

[0001] The present invention belongs to the technical field of quadruped robot control methods, and in particular relates to a quadruped robot control method applied to complex terrain. Background Art

[0002] At present, the following institutions have made in-depth research progress in the field of quadruped robots: Internationally, there are Boston Dynamics, ETH Switzerland, MIT in the United States, etc., and domestically, there are Shandong University, Zhejiang University and Harbin Institute of Technology, etc. In complex terrain, due to its own characteristics, legged robots are very likely to slip on the edge of the terrain with a high drop (such as the edge of stairs or stones), resulting in the body losing control. There are many ideas to solve this problem, which can be roughly divided into the following according to the development timeline: 1. Planning the body movement trajectory through the ZMP stability criterion without visual perception, mostly walk or crawl gaits with only one leg swinging at the same time. The effect achieved is that even if one leg slips, it can remain stable due to the large ZMP stability margin supported by three legs; 2. Trot gait without visual perception, by combining model predictive control and whole-body dynamics control to achieve better control effect, trot step frequency is relatively fast, which is conducive to quickly recovering balance after slipping; 3. Model predictive control combined with vision, planning the reference speed according to the size of the stairs, and optimizing the footing point combined with visual perception to avoid slipping on the edge of the stairs.

[0003] For the third idea, the latest existing technology can plan the reference speed according to the size of the stairs, and optimize the foothold by combining visual information. The defect of the existing technology is that it has low adaptability and is only suitable for regular staircase conditions, and it is difficult to overcome complex road conditions. After obtaining the target foothold by calculating the reference speed, a more suitable foothold is selected in the surrounding area. The range of foothold changes is relatively limited, and exceeding a certain limit will affect the stability of the robot body. Summary of the invention

[0004] In order to overcome the shortcomings of the prior art, the present invention provides a control method for a quadruped robot applied to complex terrains, which has good adaptability, can cope with complex road surfaces, and has high walking stability.

[0005] The technical solution adopted by the present invention to solve the technical problem is: a quadruped robot control method applied to complex terrain, comprising the following steps:

[0006] The image processing device on the S1 quadruped robot obtains the visual signal and inputs it into the perception host unit to obtain the robot's position, scoring map and height map;

[0007] S2 converts the score map and height map to the world coordinate system of motion control;

[0008] S3 After the motion host obtains the information of step S1 and step S2, it estimates the initial landing point of the quadruped robot;

[0009] The S4 landing point optimization module comprehensively compares the score map and height map near the initial landing point, and determines the final landing point based on the score, height and distance from the initial landing point;

[0010] S5 reversely calculates the expected reference speed of the quadruped robot according to the final foothold position.

[0011] Furthermore, the step S1 includes the following sub-steps:

[0012] S11 determines the position of the quadruped robot in the environment;

[0013] S12 analyzes and calculates the acquired visual signals to form a scoring map and a height map, wherein the scoring map is formed based on the following formula: λ 1 ,λ 2 ,λ 3 is the weight coefficient, s(h) is the slope, σ(s(h)) and are the standard deviation and mean of the slope, is the average value of height.

[0014] The scoring map can help you gain an understanding of the terrain, making it easier to judge the suitability of a location as a base.

[0015] Furthermore, in step S3, the initial foothold is calculated using a capture point method according to a reference speed input by the control module and an estimated speed of the quadruped robot itself.

[0016] Furthermore, the step S4 includes the following sub-steps:

[0017] S41 selects a regional map around the initial landing point, wherein the regional map includes a scoring map and a height map of the region;

[0018] S42 divides the regional map into several sub-regions, each of which corresponds to an independent score;

[0019] S43: if the score of the initial landing point location area meets a preset threshold, the initial landing point is determined as the final landing point;

[0020] S44: If the score of the initial foothold position area does not meet the preset threshold, foothold selection is performed, and the foothold selection meets the maximum height constraint and the leg interference constraint.

[0021] Furthermore, in step S44, the maximum height constraint refers to satisfying:

[0022] h obstacle (p i , p i，stance )-p i，z <h max

[0023] h obstacle (p i , p i，stance )-P i，stance，z <h max ·, p i Indicates the location of the alternative landing point, p i，stance Indicates the previous standing position, h obstacle (p i , p i，stance ) indicates p i With p i，stance The maximum obstacle height between them; z represents the height of pi or pi stance position;

[0024] In step S44, the leg interference constraint means that the distance between the footholds of two adjacent legs is greater than a set threshold.

[0025] It ensures that the swinging leg will not exceed the height limit and that there will be no interference between the two adjacent legs, which will affect the movement effect.

[0026] Furthermore, the optimization cost function of the foothold includes and p*i, prev is the last optimization result, Si is the selectable range, i swing refers to the swinging process, and grounded refers to after landing.

[0027] While minimizing the foothold score, the difference between the two optimization results is minimized, so that there is no significant change in the results of two adjacent optimizations, ensuring the linearity and stability of the swing leg trajectory.

[0028] Furthermore, the calculation formula of the initial final landing point position in step S5 is:

[0029]

[0030] in represents the foothold position of the i-th leg in the x direction in the world coordinate system, p w，b Represents the position of the body center in the world coordinate system, Represents the position of the hip joint in the body coordinate system, and the position in the world coordinate system is obtained by the rotation matrix R, T remmain is the remaining swing time of the swing leg, T stance is the support phase time, v x is the current estimated velocity in the x direction, vref is the target speed, c is the correction coefficient;

[0031] Assume that the target speed is equal to the current estimated speed, so that c*(v x -v ref ) is zero, we get

[0032] Ensure that the changed landing point matches the current speed to improve movement stability.

[0033] Furthermore, the scoring map has a size of 3m*3m, with a total of 100*100 cells, each cell is 0.03m*0.03m, and is centered on the center of mass of the quadruped robot. Each cell is scored between 0 and 1, and the score of the inaccessible area is 1.

[0034] Furthermore, the selection strategy in step S4 is to search in a spiral order or select columns from far to near according to the lateral distance; search and compare the scoring map scores of the candidate cells, and record the cell with the lowest current score at any time as the current optimal result; if the latest searched cell score is less than or equal to the previously recorded cell score, update the record and the current optimal result.

[0035] The beneficial effects of the present invention are: 1) it can not only adapt to regular staircase conditions, but also overcome complex road conditions, and has good adaptability; 2) the method for selecting a foothold makes it possible to select a more suitable foothold, and the walking stability of the quadruped robot is higher; 3) after selecting a suitable foothold, the reference speed is back-calculated so that the reference speed matches the selected foothold, reducing the impact of changing the foothold on the stability of the robot body; 4) the range of changing the foothold is less restricted. BRIEF DESCRIPTION OF THE DRAWINGS

[0036] Figure 1 It is a schematic block diagram of the present invention.

[0037] Figure 2 Schematic diagram of the search strategy for the spiral sequence in step S4 of the present invention.

[0038] Figure 3 Schematic diagram of the search strategy for selecting columns from far to near according to lateral distance in step 4 of the present invention.

[0039] Figure 4 It is a schematic diagram of a specific implementation of the present invention. DETAILED DESCRIPTION

[0040] In order to enable those skilled in the art to better understand the solutions of the present invention, the following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work should fall within the scope of protection of the present invention.

[0041] D435 depth cameras are installed on the front and back of the quadruped robot as image processing devices.

[0042] A quadruped robot control method for complex terrain includes the following steps:

[0043] The image processing device on the S1 quadruped robot obtains the visual signal and inputs it into the perception host unit to obtain the robot's position, scoring map and height map;

[0044] The specific steps include the following:

[0045] S11 determines the position of the quadruped robot in the environment. The visual odometer estimates the position of the quadruped robot in the environment by analyzing the environment. The visual odometer is based on the world coordinate system of visual positioning, and the leg and foot odometer is based on the world coordinate system of motion control.

[0046] S12 point cloud data processing, analyzes and calculates the acquired visual signals to form a scoring map and a height map, where the scoring map is formed based on the following formula: λ 1 ,λ 2 ,λ 3 is the weight coefficient, s(h) is the slope, σ(s(h)) and are the standard deviation and mean of the slope, is the average value of height.

[0047] The scoring map is 3m*3m in size, with a total of 100*100 cells, each cell is 0.03m*0.03m, and the quadruped robot's body mass center is used as the center. Each cell is scored between 0-1, and the score of the inaccessible area is 1. The scoring map comprehensively considers the height change, slope change, and distance from the edge of the cell, and scores each cell between 0 and 1. The smaller the score, the safer it is, and the inaccessible area is 1. The size and resolution of the height map are consistent with the scoring map. The data of each cell is the physical height of the place.

[0048] S2 converts the scoring map and height map to the world coordinate system of motion control; there is a general calculation method for coordinate system conversion, which will not be repeated here;

[0049] S3 After the motion host obtains the information of step S1 and step S2, the position information, scoring map and height map of the quadruped robot in the environment are input into the motion host to estimate the initial landing point of the quadruped robot;

[0050] Specifically, in step S3, the capture point method is used to calculate the initial landing point based on the reference speed input by the control module and the estimated speed of the quadruped robot itself. The reference speed here is input by the upper control module, such as navigation or handle instructions; the estimated speed is provided by the leg odometer. The odometer is a computing module that can provide information such as the position and speed of the robot.

[0051] S4 selects the most suitable landing point. The landing point optimization module comprehensively compares the score map, height and distance from the initial landing point in the scoring map and height map near the initial landing point to determine the final landing point;

[0052] The specific steps include the following:

[0053] S41 selects a regional map around the initial landing point, wherein the regional map includes a scoring map and a height map of the region;

[0054] S42 divides the regional map into several sub-regions, each of which corresponds to an independent score;

[0055] S43: if the score of the initial landing point location area meets a preset threshold, the initial landing point is determined as the final landing point;

[0056] S44: if the score of the initial foothold position area does not meet the preset threshold, foothold selection is performed, and the foothold selection meets the maximum height constraint and the leg interference constraint;

[0057] In the above step S44, the maximum height constraint refers to satisfying:

[0058] h obstacle (p i , P i，stace )-p i，z <h max

[0059] h obstacle (p i , p i，stance )-p i，stance，z <h max ., p i Indicates the location of the alternative landing point, p i，stance Indicates the previous standing position, h obstacle (p i , p i，stance ) indicates p i With pi，stance The maximum obstacle height between them; z represents the height of pi or pi stance position;

[0060] In the above step S44, the leg interference constraint means that the distance between the footholds of two adjacent legs is greater than a set threshold, that is, the foothold of a certain leg should not fall within a certain range around the foothold of the adjacent leg.

[0061] In the above step S44, the optimization cost function of the landing point includes and p*i, prev is the last optimization result, Si is the selectable range, i swing refers to the swinging process, and grounded refers to after landing.

[0062] The foothold will be continuously updated as time goes by. The optimization result obtained in the previous unit time is used before the optimization result in the current unit time. Therefore, when there is a large difference between the optimization results of two consecutive times, the sudden change of the swing leg trajectory will cause jitter and oscillation. The foothold optimization module records the cell with the lowest score at any time as the current optimal result. If the score of the cell most recently searched is less than or equal to the previously recorded cell score, the record and the current optimal result are updated.

[0063] The selection strategy in step S4 is to search in spiral order or select columns from far to near according to lateral distance; search and compare the scoring map scores of candidate cells, and record the cell with the lowest current score at any time as the current optimal result; if the latest searched cell score is less than or equal to the previously recorded cell score, update the record and the current optimal result.

[0064] like Figure 2 As shown, searching in the spiral order shown in the figure tends to select the cell closest to the initial landing point, and there is no significant difference between the rows and columns before, after, left and right.

[0065] like Figure 3 As shown, the search order is to select columns from far to near according to the lateral distance. In the same column, the backward direction is searched first and then the forward direction is searched. It is manifested in a tendency to change the lateral position as little as possible or to change it less, and in the same column, it tends to change forward.

[0066] The default search range in the foothold selection module is 11*11 cells. The foothold selection module first determines the height and map score of the initial foothold to determine whether the initial foothold is suitable. If it exceeds a certain threshold, the foothold selection function is activated, and if it does not exceed, it is considered suitable. The height of the initial foothold is used as part of the selection constraint to narrow the selection range to cells with a height close to the initial foothold. Then, according to Figure 2 , Figure 3The selection order shown searches and compares the score map scores of candidate cells in turn, and records the cell with the lowest current score at any time as the current optimal result. If the score of the cell most recently searched is less than or equal to the previously recorded cell score, the record and the current optimal result are updated. The figure above shows two search strategies.

[0067] S5 reversely calculates the expected reference speed of the quadruped robot according to the final foothold position.

[0068] The calculation formula of the initial final landing point position in step S5 is:

[0069]

[0070] in represents the foothold position of the i-th leg in the x direction in the world coordinate system, p w，b Represents the position of the body center in the world coordinate system, Represents the position of the hip joint in the body coordinate system, and the position in the world coordinate system is obtained by the rotation matrix R, T remain is the remaining swing time of the swing leg, T stance is the support phase time, v x is the current estimated velocity in the x direction, v ref is the target speed, c is the correction coefficient;

[0071] Assume that the target speed is equal to the current estimated speed, so that c*(v x -v ref ) is zero, we get

[0072] The motion host includes multiple modules, such as leg and foot odometer, swing leg planning, foothold optimization and model predictive control. The reference speed is initially input by the upper control module, such as navigation or handle instructions. The swing leg planning module uses the capture point method to calculate the initial foothold according to the reference speed and the estimated speed of the robot itself (provided by the leg and foot odometer). The foothold optimization module searches for the most suitable foothold by comprehensively comparing the score, height and distance from the initial foothold in the surrounding cells near the initial foothold. In some uncommon extreme cases, there may be no suitable foothold within the search range, and the initial foothold will be selected as the final target. After selecting a suitable foothold, due to the difference in distance between the initial foothold, setting the target foothold to the optimized foothold will cause the robot's expected speed to not match the support state, affecting the stability of the robot. The present invention adopts the method of back-calculating the expected reference speed based on the selected foothold as a reference after selecting a suitable foothold, so that the reference speed matches the expected foothold position to be stepped on, solving the problem of affecting stability analyzed before.

[0073] like Figure 4 As shown in the figure, the quadruped robot walks to the front of the stairs, and the front image processing device picks up the terrain features to form the following Figure 4 The map shown. The edge of the stairs is displayed in dark colors, indicating that it has a higher score in the scoring map; the middle area of ​​the stairs and the ground are displayed in light colors, and the score is close to 0 in the scoring map, indicating that it can be selected as a landing point. The landing point optimization module finally selected the position in the middle of the stairs, and the small black square represents the final selection result. Then, based on the final selected position, the expected reference speed for the landing point to fall accurately here is calculated and input into the model predictive control and swing leg planning modules for execution.

[0074] The above specific implementation modes are used to explain the present invention rather than to limit the present invention. Any modification and change made to the present invention within the spirit of the present invention and the protection scope of the claims shall fall within the protection scope of the present invention.

Claims

1. A quadruped robot control method for complex terrain, It is characterized in that The following steps are involved: The image processing device on the S1 quadruped robot obtains the visual signal and inputs it into the perception host unit to obtain the robot's position, scoring map and height map; S2 converts the score map and height map to the world coordinate system of motion control; S3 After the motion host obtains the information of step S1 and step S2, it estimates the initial landing point of the quadruped robot; The S4 landing point optimization module comprehensively compares the score map and height map near the initial landing point, and determines the final landing point based on the score, height and distance from the initial landing point; Step S4 includes the following sub-steps, S41 selects a regional map around the initial landing point, wherein the regional map includes a scoring map and a height map of the region; S42 divides the regional map into several sub-regions, each of which corresponds to an independent score; S43: if the score of the initial landing point location area meets a preset threshold, the initial landing point is determined as the final landing point; S44: if the score of the initial foothold position area does not meet the preset threshold, foothold selection is performed, and the foothold selection meets the maximum height constraint and the leg interference constraint; In step S44, the maximum height constraint refers to satisfying: p i Indicates the location of the alternative landing point, p i，stance Indicates the previous standing position, h obstacle (p i ,p i，stance ) indicates p i With p i，stance The maximum obstacle height between them; z represents the height of pi or pi stance position; In step S44, the leg interference constraint means that the distance between the footholds of two adjacent legs is greater than a set threshold; S5 reversely calculates the expected reference speed of the quadruped robot according to the final foothold position.

2. The control method according to claim 1, It is characterized in that The step S1 comprises the following sub-steps: S11 determines the position of the quadruped robot in the environment; S12 analyzes and calculates the acquired visual signals to form a scoring map and a height map, wherein the scoring map is formed based on the following formula: λ 1 ,λ 2 ,λ 3 is the weight coefficient, s(h) is the slope, σ(s(h)) and are the standard deviation and mean of the slope, is the average value of height.

3. The control method according to claim 1, Features: In step S3, the initial landing point is calculated using the capture point method according to the reference speed input by the control module and the estimated speed of the quadruped robot itself.

4. The control method according to claim 1, Features: The optimization cost function of the foothold includes and p*i,prev is the last optimization result, Si is the selectable range, i swing refers to the corresponding foothold position during the swing process, and i grounded refers to the foothold position after landing.

5. The control method according to claim 1, Features: The calculation formula of the initial final landing point position in step S5 is: in represents the foothold position of the i-th leg in the x direction in the world coordinate system, p w，b Represents the position of the body center in the world coordinate system, Represents the position of the hip joint in the body coordinate system, and the position in the world coordinate system is obtained by the rotation matrix R, T remain is the remaining swing time of the swing leg, T stance is the support phase time, v x is the current estimated velocity in the x direction, v ref is the target speed, c is the correction coefficient; Assume that the target speed is equal to the current estimated speed, so that c*(v x -v ref ) is zero, we get 6. The control method according to claim 1, Features: The scoring map has a size of 3m*3m, with a total of 100*100 cells, each cell is 0.03m*0.03m, and is centered on the center of mass of the quadruped robot. Each cell is scored between 0 and 1, and the score of the inaccessible area is 1.

7. The control method according to claim 1, Features: The selection strategy in step S4 is to search in spiral order or select columns from far to near according to lateral distance; search and compare the scoring map scores of candidate cells, and record the cell with the lowest current score at any time as the current optimal result; If the most recently searched cell score is less than or equal to the previously recorded cell score, update the record and the current best result.

Images