Multi-legged robot slope marching control method and system based on double-peak Gaussian fusion

Abstract

The invention provides a multi-legged robot slope advancing control method and system based on double-peak Gaussian fusion, and belongs to the technical field of legged robot control. Comprising the following steps: constructing a foot end plane equation by utilizing foot end three-dimensional coordinates of the multi-foot robot at two grounding moments, and obtaining a pitch angle and a roll angle; performing feed-forward calculation based on a plane angle memory update algorithm to obtain a feed-forward pitch angle and a feed-forward roll angle; joint angle difference calculation is carried out based on a joint rotation angle feedback algorithm, and then the real-time correction value of the pitch angle is determined; determining a final pitch angle of the multi-legged robot by adopting a double-peak Gaussian fusion method; and converting the final pitch angle and the feed-forward roll angle into joint torque, an expected joint position and an expected joint speed in combination with a gait and a speed instruction input by a user so as to control the multi-legged robot to advance on the slope surface. According to the invention, on the basis of ensuring the light weight of the control framework, the terrain sudden change can be quickly coped, so that the multi-legged robot can be controlled to stably advance on the rugged slope.

Description

Technical Field

[0001] The present invention belongs to the technical field of legged robot control, and particularly relates to a multi-legged robot slope traveling control method and system based on bimodal Gaussian fusion. Background Art

[0002] The statements in this part merely provide background technical information related to the present invention and do not necessarily constitute prior art.

[0003] In recent years, multi-legged robots have become an important research direction in the field of robotics due to their excellent terrain adaptability and motion flexibility. With the expansion of application scenarios from structured environments to complex unstructured terrains, the limitations of traditional wheeled / caterpillar motion mechanisms have become increasingly prominent, while the bionic motion mechanism based on discrete foot-end touchdown provides an innovative solution for moving on irregular terrains. With the continuous progress of technology, simply achieving stable walking on flat ground can no longer meet the high-order requirements for autonomy and robustness in actual application scenarios. Therefore, the research on adaptive control technology in complex terrains, especially slope environments, is particularly important. Moreover, the current technological evolution shows a trend from static gait to dynamic gait, which poses higher technical requirements for motion control algorithms.

[0004] Currently, the research on current dynamic gait slope control technology mainly adopts control methods based on model-free deep reinforcement learning and environment perception based on 3D lidar. However, these control methods all face some technical problems, such as: (1) Control method based on model-free deep reinforcement learning: The domain difference between simulation and reality easily leads to the decay of policy transfer performance. A small deviation in terrain physical parameters may cause catastrophic failure, and the black-box decision-making mechanism results in the lack of system interpretability. Therefore, when foot-end slipping or attitude mutation occurs, it is difficult to trace and debug this control method in a timely manner.

[0005] (2) Control method based on environment perception of 3D lidar: By constructing a terrain digital elevation model through point cloud registration and then performing offline gait planning, this leads to an exponential growth in its computational complexity, and the average delay from environment scanning to gait generation is difficult to meet the real-time control requirements of dynamic gait. Summary of the Invention

[0006] To overcome the deficiencies of the above-mentioned prior art, the present invention provides a multi-legged robot slope traveling control method and system based on bimodal Gaussian fusion, which can quickly respond to terrain mutations on the basis of ensuring the lightweight of the control architecture, so as to control the multi-legged robot to travel stably on rugged slopes.

[0007] To achieve the above object, one or more embodiments of the present invention provide the following technical solutions: The first aspect of the present invention provides a control method for a multi-legged robot to travel on a slope based on bimodal Gaussian fusion.

[0008] The control method for a multi-legged robot to travel on a slope based on bimodal Gaussian fusion includes: Obtain the three-dimensional coordinates of the foot end of the multi-legged robot at two touchdown moments, and construct the foot end plane equation; obtain the pitch angle and roll angle of the multi-legged robot on the slope according to the foot end plane equation; Based on the plane angle memory update algorithm including the attenuation coefficient, perform feedforward calculation on the obtained pitch angle and roll angle to obtain the feedforward pitch angle and feedforward roll angle of the multi-legged robot on the slope; Perform joint angle difference calculation based on the joint angle feedback algorithm, and determine the real-time correction amount of the pitch angle of the multi-legged robot on the slope according to the difference calculation result; Use the bimodal Gaussian fusion method to determine the final pitch angle of the multi-legged robot, that is: if the multi-legged robot is at any one of the two touchdown moments, use the feedforward pitch angle as the final pitch angle; otherwise, use the value obtained by weighted fusion of the feedforward pitch angle and the real-time correction amount of the pitch angle as the final pitch angle; Combine the obtained final pitch angle and feedforward roll angle with the gait and speed commands input by the user, and convert them into joint torques, desired joint positions, and desired joint speeds through NMPC-WBC solution to control the multi-legged robot to travel on the slope.

[0009] Further, the two touchdown moments of the multi-legged robot are: at the 0 moment and the T / 2 moment when all legs of the multi-legged robot touch the ground simultaneously within a complete gait cycle T in the trot gait.

[0010] Further, obtaining the pitch angle and roll angle of the multi-legged robot on the slope according to the foot end plane equation includes: setting a yaw angle threshold and a terrain flatness threshold, and when the yaw angle of the multi-legged robot is less than the set yaw angle threshold and the terrain flatness is less than the set terrain flatness threshold, use the first composite control strategy combined with the foot end plane equation to calculate the pitch angle and roll angle; otherwise, use the second composite control strategy combined with the foot end plane equation to calculate the pitch angle and roll angle.

[0011] Further, the first composite control strategy is: regard the direction of the multi-legged robot when going uphill as facing the slope directly, that is, keep the yaw angle zero, and calculate the pitch angle and roll angle according to the foot end plane equation; the second composite control strategy is: first, use the IMU to read the yaw angle and calculate the slope normal vector, and then calculate the pitch angle and roll angle according to the foot end plane equation.

[0012] Further, the plane angle memory update algorithm is: ; Among them, and respectively represent the feedforward roll angle and the feedforward pitch angle of the updated multi-legged robot on the slope surface, and respectively represent the roll angle and the pitch angle of the multi-legged robot on the slope surface obtained through the foot-end plane equation at the current moment, and respectively represent the roll angle and the pitch angle of the multi-legged robot on the slope surface obtained through the foot-end plane equation and saved in the previous gait cycle; represents the attenuation coefficient.

[0013] Furthermore, based on the joint angle feedback algorithm, the joint angle difference is calculated, including: calculating the difference between the sum of the front knee joint angles and the sum of the rear knee joint angles, and generating a real-time correction amount for the pitch angle through a PID controller.

[0014] Furthermore, when the weighted fusion algorithm takes the value obtained by weighted fusion of the feedforward pitch angle and the real-time correction amount of the pitch angle as the final pitch angle: ; In the formula, represents the final pitch angle of the multi-legged robot on the slope surface after weighted fusion, represents the feedforward pitch angle of the multi-legged robot on the slope surface updated by the plane angle memory update algorithm, represents the real-time correction amount of the pitch angle generated by the PID controller; and respectively represent the weights corresponding to and ; among them, The value of

[0015] The second aspect of the present invention provides a multi-legged robot slope traveling control system based on bimodal Gaussian fusion.

[0016] The multi-legged robot slope traveling control system based on bimodal Gaussian fusion includes: A touchdown moment detection module, configured to: obtain the three-dimensional coordinates of the foot ends of the multi-legged robot at two touchdown moments, and construct a foot-end plane equation; obtain the pitch angle and the roll angle of the multi-legged robot on the slope surface according to the foot-end plane equation; A feedforward module, configured to: perform feedforward calculation on the obtained pitch angle and roll angle based on the plane angle memory update algorithm including the attenuation coefficient, so as to obtain the feedforward pitch angle and the feedforward roll angle of the multi-legged robot on the slope surface; The joint angle feedback module is configured to calculate the joint angle difference based on the joint angle feedback algorithm and determine the real-time correction amount of the pitch angle of the multi-legged robot on the slope according to the difference calculation result; The bimodal Gaussian fusion control module is configured to determine the final pitch angle of the multi-legged robot by using the bimodal Gaussian fusion method, that is, if the multi-legged robot is at any one of the two touchdown moments, the feedforward pitch angle is used as the final pitch angle; otherwise, the value obtained by weighted fusion of the feedforward pitch angle and the real-time pitch angle correction amount is used as the final pitch angle; The traveling control module is configured to convert the obtained final pitch angle and the feedforward roll angle, combined with the gait and speed commands input by the user, into joint torques, desired joint positions, and desired joint speeds after NMPC-WBC calculation to control the multi-legged robot to travel on the slope. The third aspect of the present invention provides a computer-readable storage medium, on which a program is stored, and when the program is executed by a processor, the steps in the method for controlling the traveling of a multi-legged robot on a slope based on bimodal Gaussian fusion as described in the first aspect of the present invention are implemented.

[0017] The fourth aspect of the present invention provides an electronic device, including a memory, a processor, and a program stored on the memory and executable on the processor. When the processor executes the program, the steps in the method for controlling the traveling of a multi-legged robot on a slope based on bimodal Gaussian fusion as described in the first aspect of the present invention are implemented.

[0018] The above one or more technical solutions have the following beneficial effects: (1) The present invention combines the foot-end plane equation and proposes a composite strategy of dual-method fusion, which can quickly and accurately estimate the terrain pitch angle and roll angle; at the same time, the present invention combines the adaptive decay coefficient in the plane angle memory update algorithm to dynamically fuse the current estimated angle and historical angle data, and uses the obtained attitude angle as feedforward control; and combines the bimodal Gaussian weights. At the moment when all legs touch the ground, it completely relies on the feedforward attitude angle generated by terrain estimation. During the swing phase, the joint angle feedback module calculates the angle difference between the front and rear knee joints in real time, and uses the PID controller to generate the pitch angle correction amount to form a transparent decision-making link. Therefore, the present invention can effectively compensate for the attitude deviation caused by terrain dynamic changes and external disturbances, achieve a rapid response to terrain mutations, and thus significantly improve the interpretability on the basis of avoiding the attitude instability problem caused by historical data lag in the prior art.

[0019] (2) The present invention realizes the lightweight and real-time optimization of the control architecture, that is: through the construction of the foot-end plane equation (based on the coordinates at the moment when multiple feet touch the ground simultaneously) and the analytical design of the double-peak Gaussian function, the present invention can avoid complex terrain modeling and iterative optimization calculations, reduce the calculation time of the algorithm, and thus meet the real-time control requirements for multi-legged robots; at the same time, the feedforward-feedback phased cooperation mechanism further reduces the conflict of redundant control instructions and can effectively reduce the system resource occupancy rate.

[0020] The advantages of the additional aspects of the present invention will be partially given in the following description, partially become obvious from the following description, or be understood through the practice of the present invention. BRIEF DESCRIPTION OF THE DRAWINGS

[0021] The accompanying drawings forming a part of the present invention are used to provide a further understanding of the present invention. The schematic embodiments of the present invention and their descriptions are used to explain the present invention and do not constitute an improper limitation to the present invention.

[0022] Figure 1 It is a flowchart of the multi-legged robot slope traveling control method based on double-peak Gaussian fusion in Embodiment 1 of the present invention.

[0023] Figure 2 It is a gait schematic diagram of the trot gait in Embodiment 1 of the present invention.

[0024] Figure 3 It is a schematic diagram of the coordinate system of the multi-legged robot in Embodiment 1 of the present invention.

[0025] Figure 4 It is a schematic diagram of the roll angle adjustment of the multi-legged robot when climbing a slope in Embodiment 1 of the present invention.

[0026] Figure 5 It is a schematic diagram of the pitch angle adjustment of the multi-legged robot when climbing a slope in Embodiment 1 of the present invention.

[0027] Figure 6 It is a schematic diagram of the control relationship of the multi-legged robot slope traveling control system based on double-peak Gaussian fusion in Embodiment 2 of the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0028] It should be noted that the following detailed descriptions are all exemplary and are intended to provide further explanations of the present invention. Unless otherwise specified, all technical and scientific terms used herein have the same meaning as commonly understood by those of ordinary skill in the technical field to which the present invention belongs.

[0029] It should be noted that the terms used herein are only for describing specific embodiments and are not intended to limit the exemplary embodiments according to the present invention.

[0030] Without conflict, the embodiments in the present invention and the features in the embodiments may be combined with each other.

[0031] Overall idea proposed by the present invention: The present invention provides a multi-legged robot slope walking control method based on bimodal Gaussian fusion. Combining the trot gait characteristics, this method can regard all the legs of the multi-legged robot as supporting legs at the 0 moment and near the T / 2 moment in each gait cycle T, and construct a plane equation through the foot end positions. At the same time, a hybrid strategy based on the fusion of two methods is proposed for fast and high-precision terrain pitch angle and roll angle estimation; and a plane angle memory update formula is designed, taking the obtained attitude angles as feedforward control. When far from these two moments, knee joint angle feedback control is added to correct the pitch angle of the multi-legged robot in real time. Finally, by designing a bimodal Gaussian function to cover two touchdown events, calculating the weights, and combining the terrain estimation (feedforward control) and knee joint angle feedback (feedback control) through weighting, fast adaptation to the terrain is achieved, with higher robustness.

[0032] Embodiment 1 This embodiment discloses a multi-legged robot slope walking control method based on bimodal Gaussian fusion.

[0033] As Figure 1 shown, the multi-legged robot slope walking control method based on bimodal Gaussian fusion includes: Step S1: Obtain the three-dimensional coordinates of the foot ends of the multi-legged robot at two touchdown moments, and construct a foot end plane equation; obtain the pitch angle and roll angle of the multi-legged robot on the slope according to the foot end plane equation; Step S2: Based on the plane angle memory update algorithm including the attenuation coefficient, perform feedforward calculation on the obtained pitch angle and roll angle to obtain the feedforward pitch angle and feedforward roll angle of the multi-legged robot on the slope; Step S3: Calculate the joint angle difference based on the joint angle feedback algorithm, and determine the real-time correction amount of the pitch angle of the multi-legged robot on the slope according to the difference calculation result; Step S4: Use the bimodal Gaussian fusion method to determine the final pitch angle of the multi-legged robot, that is: if the multi-legged robot is at any of the two touchdown moments, use the feedforward pitch angle as the final pitch angle; otherwise, use the value obtained by weighted fusion of the feedforward pitch angle and the real-time correction amount of the pitch angle as the final pitch angle; Step S5: Combine the obtained final pitch angle and feedforward roll angle, and convert them into joint torques, desired joint positions, and desired joint velocities through NMPC-WBC calculation in combination with the gait and speed commands input by the user to control the multi-legged robot to walk on the slope.

[0034] Based on the above process, the present invention can quickly respond to sudden changes in terrain while ensuring the lightweight control architecture, so as to control the multi-legged robot to move stably on a rugged slope. To facilitate the understanding of the technical solution of the present invention, the specific implementation steps in the technical solution of the present invention are further explained and illustrated below.

[0035] In step S1, the three-dimensional coordinates of the foot end of the multi-legged robot at two ground contact moments are obtained, and a foot end plane equation is constructed; the pitch angle and roll angle of the multi-legged robot on the slope are obtained according to the foot end plane equation.

[0036] The moment t=0 and the moment t=T / 2 in a trot gait cycle T are regarded as all the legs of the multi-legged robot touching the ground at the same time. The three-dimensional coordinates of the foot ends of all the touching legs at these two moments are obtained to construct the foot end plane equation; and the advantages of two methods for estimating the pitch angle and roll angle of the terrain (i.e., the dual-method composite control strategy: the first composite control strategy and the second composite control strategy) are combined to achieve fast and accurate estimation of the pitch angle and roll angle in the slope terrain. Among them, the trot gait is a common gait of quadruped robots, that is, the diagonal trotting gait; its characteristic is that the two diagonal legs of the robot (such as the left front leg and the right hind leg) move synchronously, and then the other pair of diagonal legs (the right front leg and the left hind leg) move alternately; this gait strikes a balance between speed, stability and energy efficiency.

[0037] like Figure 2 As shown in the figure, in the trot gait, the left front leg and the right hind leg are swing legs before T / 2, and the right front leg and the left hind leg are swing legs after T / 2. At time T, the left front leg and the right hind leg will become swing legs again. This time T is a complete gait cycle. In more popular terms: the time it takes for all legs to start from a touchdown / swing mode, go through a full round of movement, and return to this mode is called a complete gait cycle.

[0038] Step S1 can be specifically implemented by the following steps: Step S1-1, constructing a foot end plane equation by acquiring the three-dimensional coordinates of the foot end of the multi-legged robot in the world coordinate system.

[0039] by Figure 2Taking the trot gait diagram of the quadruped robot shown as an example, LF, RF, RH, and LH represent the left front leg, right front leg, right hind leg, and left hind leg of the multi-legged robot respectively. In a gait cycle T, the left front leg and the right hind leg are the swing legs in the first T / 2 time period, and the right front leg and the left hind leg are the swing legs in the second T / 2 time period. Only at the moment of 0 and around T / 2 can all legs be regarded as the touchdown legs. By obtaining the three-dimensional coordinates of all foot tips at these two moments, rather than storing the three-dimensional coordinates of the touchdown legs in the previous gait cycle and combining them with the three-dimensional coordinates of the touchdown legs in the current gait cycle, the plane equation that conforms to the real terrain at the current moment can be constructed more precisely. Among them, the foot tip plane equation is defined as: (1) Wherein, represents the height offset of the plane; represents the slope of the terrain in the x direction, that is, the height change generated by the unit x axis displacement; represents the slope of the terrain in the y direction, that is, the height change generated by the unit y axis displacement; is the coordinate of a point on the terrain plane. In this embodiment, let represent the foot tip coordinate of the i th leg of the multi-legged robot, and substituting it into the foot tip plane equation, we can get: (2) Wherein, represents the height of the foot tip of the i th leg of the robot in the z-axis direction. Since the above system of equations contains four equations and three unknowns, it needs to be solved by the least squares method. Therefore, equation (2) is denoted as: (3) Wherein, is the set of the left column of equation (2) , representing the height of the foot tip of each leg of the robot in the z axis direction, M represents the four-row and three-column matrix shown in the right column of equation (2), X represents the set of , , in the right column of equation (2). The ultimate goal of the solution is to find a X such that the sum of the squares of the residual vector is minimized, that is, to minimize the following objective function: (4) Simplified to get: (5) Thus, the final solution can be expressed as: (6) At this time, in the foot-end plane equation a, b, c have all been obtained. Next, the slope pitch angle and roll angle can be solved according to the foot-end plane equation. For this, the present invention provides a dual-method composite control strategy. Specifically: set a yaw angle threshold and a terrain flatness threshold. When the yaw angle of the multi-legged robot is less than the set yaw angle threshold and the terrain flatness is less than the set terrain flatness threshold, use the first composite control strategy combined with the foot-end plane equation to calculate the pitch angle and roll angle; otherwise, use the second composite control strategy combined with the foot-end plane equation to calculate the pitch angle and roll angle. The dual-method composite control strategy can be implemented by the following method: A. The first composite control strategy.

[0040] By default, when the multi-legged robot is going uphill, it faces the slope directly, that is, the yaw angle remains zero, and the influence of the yaw angle is not considered. Calculate the pitch angle and roll angle according to the foot-end plane equation. According to the definition of b, c in the plane equation ( b represents the height change caused by the unit x axis displacement, c represents the height change caused by the unit y axis displacement), the slope pitch angle and roll angle can be calculated simply by the following formula: (7) Where, represents the pitch angle of the slope, represents the roll angle of the slope.

[0041] B. The second composite control strategy: Considering the influence of the yaw angle, first use the IMU to read out the yaw angle , and obtain the slope normal vector as through Equation (1). Among them, the IMU is a sensor that can measure the self-acceleration and angular velocity of an object. Through the above information, the attitude angles (roll angle, yaw angle, pitch angle) of the robot can be calculated. Normalize the slope normal vector to obtain: (8) Where, represents the slope normal vector after normalization, that is, the slope unit normal vector. The slope unit normal vector obtained by Equation (8) is the third column of the rotation matrix from the slope coordinate system to the world coordinate system. Let , and the others are similar, then there are the following equalities: (9) Where, respectively represent the roll angle, pitch angle, and yaw angle of the terrain estimated by the plane equation. The yaw angle is consistent with the forward direction of the robot and is directly read from the IMU. Therefore is known, and has also been calculated in Equation (8). Thus can also be calculated. Therefore, the pitch angle and roll angle of the slope estimated by the plane equation are: (10) where represents the pitch angle estimated by the plane equation, represents the roll angle estimated by the plane equation; represents the matrix in the row value, that is represents , represents , represents .

[0042] In the actual application process, the first composite control strategy is simple to calculate and does not depend on IMU data, and is more suitable for straight-line climbing, low-dynamic scenarios, or resource-constrained embedded platforms; the second composite control strategy considers the influence of the yaw angle, corrects the coordinate system through the rotation matrix, can accurately reflect the actual attitude of the robot in three-dimensional space, has strong adaptability to complex terrains, and is more suitable for complex terrain exploration and dynamic steering tasks. The present invention adopts a composite control strategy that combines the two, that is: set the yaw angle threshold and the terrain flatness threshold. When the yaw angle of the multi-legged robot is less than the set yaw angle threshold and the terrain flatness is less than the set terrain flatness threshold, the first composite control strategy is adopted in combination with the foot-end plane equation to calculate the pitch angle and the roll angle; otherwise, the second composite control strategy is adopted in combination with the foot-end plane equation to calculate the pitch angle and the roll angle.

[0043] As an optional embodiment, the yaw angle threshold can be set to 10 0 , and the terrain flatness threshold is 0.05. That is, when the yaw angle is less than 10 0 and the terrain is flat , the first composite control strategy is adopted, and the second composite control strategy is switched to at other times to better balance efficiency and accuracy.

[0044] In step S2, based on the plane angle memory update algorithm including the attenuation coefficient, feedforward calculation is performed on the obtained pitch angle and roll angle to obtain the feedforward pitch angle and feedforward roll angle of the multi-legged robot on the slope.

[0045] The plane angle memory update algorithm can be expressed as: (11) Among them, and respectively represent the feedforward roll angle and the feedforward pitch angle of the updated multi-legged robot on the slope, and respectively represent the roll angle and the pitch angle of the multi-legged robot on the slope obtained through the foot-end plane equation at the current moment, and respectively represent the roll angle and the pitch angle of the multi-legged robot on the slope obtained through the foot-end plane equation saved in the previous gait cycle; represents the attenuation coefficient. Among them, the attenuation coefficient The calculation formula is: (12) Among them, k is a control parameter that is as large as possible and greater than zero, used to adjust the steepness of the curve; is the time weight based on the gait phase, obtained from the double-peak Gaussian fusion function. When approaches 1, that is, at the moment when the four legs of the multi-legged robot touch the ground simultaneously; also approaches 1, completely adopting new data to ensure a fast response when the terrain changes suddenly, and ensuring that the proportion of new data is not less than 0.7.

[0046] The feedforward roll angle obtained in formula (11) is the final expected roll angle. As Figure 4 shown in the schematic diagram of the roll angle adjustment of the multi-legged robot when going uphill. For the pitch angle, since it is significantly affected by terrain dynamic disturbances during slope movement, it is necessary to further compensate through a feedforward-feedback hybrid control strategy to improve the body attitude stability; it should be noted additionally that Figure 4 shown in represents the body coordinate system, represents the world coordinate system.

[0047] In step S3, based on the joint angle feedback algorithm, the joint angle difference is calculated, and the real-time correction amount of the pitch angle of the multi-legged robot on the slope is determined according to the calculation result of the difference.

[0048] Calculating the joint angle difference based on the joint angle feedback algorithm includes: calculating the difference between the sum of the front knee joint angles and the sum of the rear knee joint angles, and generating the real-time correction amount of the pitch angle through the PID controller . As Figure 3 shown in the schematic diagram of the coordinate system of the multi-legged robot, They are the knee joint angles of the left front leg, right front leg, left hind leg, and right hind leg respectively. When the multi-legged robot moves forward on a horizontal plane or with its torso parallel to the slope surface, due to the symmetry of its trot gait, the theoretical difference between the sum of the front knee joint angles and the sum of the rear knee joint angles is zero. Since using only the plane equation causes inaccurate terrain estimation when far from the 0 and T / 2 moments in each gait cycle, this feedback algorithm is introduced to dynamically compensate for the attitude deviation caused by terrain changes and foot-end slippage to obtain the pitch angle correction amount. , that is: (13) Among them, represents the difference between the sum of the front knee joint angles and the sum of the rear knee joint angles, , and represent the gain coefficients of proportional, integral, and differential in the PID formula respectively.

[0049] In step S4, the bimodal Gaussian fusion method is used to determine the final pitch angle of the multi-legged robot, that is: if the multi-legged robot is at any of the two touchdown moments, the feedforward pitch angle is used as the final pitch angle; otherwise, the value obtained by weighted fusion of the feedforward pitch angle and the real-time pitch angle correction amount is used as the final pitch angle.

[0050] In this step, the designed bimodal Gaussian fusion formula can be expressed as: (14) Among them, is the time weight based on the gait phase, is the weight decay coefficient, is the current normalized phase. Among them, The calculation formula of (15) Among them, mod represents the remainder operation, T represents the gait cycle.

[0051] In each gait cycle T at the 0 and T / 2 moments, all legs touch the ground simultaneously, and the weight = 1. At this time, the value of the desired pitch angle completely depends on the slope pitch angle obtained in step S2 to achieve fast terrain tracking; when far from the 0 moment and the T / 2 moment, the weight exponentially decays. At this time, a feedback correction link is added to improve the anti-interference ability. At this time, the desired pitch angle is obtained by weighting the terrain estimation (feedforward control) in step S2 and the joint angle feedback (feedback control) in step S3, that is, the weighted fusion algorithm when the value obtained by weighted fusion of the feedforward pitch angle and the real-time pitch angle correction amount is used as the final pitch angle is: (16) In the formula, represents the final pitch angle of the multi-legged robot on the slope after weighted fusion, represents the feedforward pitch angle of the multi-legged robot on the slope after being updated by the plane angle memory update algorithm, represents the real-time pitch angle correction amount generated by the PID controller; and respectively represent the weights corresponding to and ; among them, The value of gradually decays as it moves away from the 0 moment and the T / 2 moment within the gait cycle T of the multi-legged robot. The schematic diagram of the pitch angle adjustment of the multi-legged robot when going uphill is as shown in Figure 5 shown.

[0052] In step S5, the obtained final pitch angle and feedforward roll angle, combined with the gait and speed commands input by the user, are converted into joint torques, desired joint positions, and desired joint velocities after being solved by NMPC-WBC to control the multi-legged robot to move on the slope.

[0053] The final pitch angle and the feedforward roll angle are combined with the gait and speed commands input by the user and sent to the trajectory planning module. After the trajectory planning module generates the desired trajectory, it is sent to the NMPC module. The NMPC will calculate the optimal system state and system input and provide them to the WBC. The WBC will send the solved joint torques, desired joint positions, and desired joint velocities to the multi-legged robot, thereby controlling the movement of the robot on the slope. It should be noted that how to convert the obtained final pitch angle and feedforward roll angle into the joint torques, desired joint positions, and desired joint velocities that can control the multi-legged robot to move on the slope is not the main innovation point of the present invention. Therefore, as long as the technical means used here can achieve the conversion required by the present invention, the present invention does not specifically limit the specific conversion method.

[0054] Embodiment 2 This embodiment discloses a multi-legged robot slope traveling control system based on bimodal Gaussian fusion.

[0055] A multi-legged robot slope traveling control system based on bimodal Gaussian fusion includes: A touchdown moment detection module, configured to: obtain the three-dimensional coordinates of the foot end of the multi-legged robot at two touchdown moments and construct a foot end plane equation; obtain the pitch angle and roll angle of the multi-legged robot on the slope according to the foot end plane equation; A feedforward module, configured to: perform feedforward calculation on the obtained pitch angle and roll angle based on a plane angle memory update algorithm including an attenuation coefficient to obtain the feedforward pitch angle and feedforward roll angle of the multi-legged robot on the slope; The joint angle feedback module is configured to calculate the joint angle difference based on the joint angle feedback algorithm, and determine the real-time correction amount of the pitch angle of the multi-legged robot on the slope according to the difference calculation result; The bimodal Gaussian fusion control module is configured to embed a bimodal Gaussian fusion algorithm, and is used to determine the final pitch angle of the multi-legged robot through the bimodal Gaussian fusion method, that is: if the multi-legged robot is in any of the two touchdown moments, the feedforward pitch angle is used as the final pitch angle; otherwise, the value obtained by weighted fusion of the feedforward pitch angle and the real-time correction amount of the pitch angle is used as the final pitch angle; The traveling control module is configured to combine the obtained final pitch angle and the feedforward roll angle, and convert them into joint torques, desired joint positions, and desired joint velocities through NMPC-WBC calculation in combination with the gait and speed commands input by the user, so as to control the multi-legged robot to travel on the slope. The control relationship of the control system framework adopted by the present invention is as Figure 6 shown, that is: the user inputs a trot gait and a speed command, which are combined with the roll angle obtained by the feedforward module and the pitch angle obtained by the bimodal Gaussian fusion control module and the current system state to generate a desired trajectory and send it to the NMPC. The NMPC will calculate the optimal system state and system input and then provide them to the WBC. Finally, the solved joint torques, desired joint positions, and desired joint velocities are sent to the multi-legged robot. The multi-legged robot sends the current sensor data and joint state to the state estimator, thereby realizing the control of the multi-legged robot during the traveling process on the slope.

[0056] Through the above module design, the attitude adjustment during climbing can be realized only relying on proprioceptors, and a rapid response to terrain mutations can be achieved. It not only avoids the attitude instability problem caused by the lag of historical data in the traditional method, but also avoids complex terrain modeling and iterative optimization calculations, can meet the real-time control requirements of the multi-legged robot, and has strong robustness.

[0057] Embodiment III The purpose of this embodiment is to provide a computer-readable storage medium.

[0058] A computer-readable storage medium, on which a computer program is stored, and when the program is executed by a processor, it realizes the steps in the method for controlling the traveling of a multi-legged robot on a slope based on bimodal Gaussian fusion as described in Embodiment I of the present disclosure.

[0059] Embodiment IV The purpose of this embodiment is to provide an electronic device.

[0060] An electronic device, comprising a memory, a processor, and a program stored on the memory and executable on the processor, wherein when the processor executes the program, the steps in the multi-legged robot slope traveling control method based on bimodal Gaussian fusion as described in Embodiment 1 of the present disclosure are implemented.

[0061] In the devices of the above Embodiments 2, 3, and 4, the steps involved correspond to those in Method Embodiment 1. For the specific implementation manners, reference may be made to the relevant description part of Embodiment 1. The term "computer-readable storage medium" should be understood to include a single medium or multiple media including one or more instruction sets; it should also be understood to include any medium that can store, encode, or carry an instruction set for execution by a processor and enable the processor to execute any method in the present invention.

[0062] Those skilled in the art should understand that the above-mentioned modules or steps of the present invention can be implemented by a general-purpose computer device. Optionally, they can be implemented by program codes executable by a computing device, so that they can be stored in a storage device and executed by the computing device, or they can be separately fabricated into individual integrated circuit modules, or multiple modules or steps among them can be fabricated into a single integrated circuit module for implementation. The present invention is not limited to any specific combination of hardware and software.

[0063] Although the specific implementation manners of the present invention have been described above in conjunction with the accompanying drawings, it is not a limitation to the protection scope of the present invention. Those skilled in the art should understand that, based on the technical solutions of the present invention, various modifications or deformations that can be made without creative efforts by those skilled in the art are still within the protection scope of the present invention.

Claims

1. A multi-legged robot slope control method based on bimodal Gaussian fusion, characterized in that: include: Obtain the three-dimensional coordinates of the foot end of the multi-legged robot at two ground contact moments and construct the foot end plane equation; obtain the pitch angle and roll angle of the multi-legged robot on the slope according to the foot end plane equation; Based on the plane angle memory update algorithm including the attenuation coefficient, the obtained pitch angle and roll angle are feedforward calculated to obtain the feedforward pitch angle and feedforward roll angle of the multi-legged robot on the slope. The joint angle difference is calculated based on the joint angle feedback algorithm, and the real-time correction amount of the pitch angle of the multi-legged robot on the slope is determined according to the difference calculation result; The bimodal Gaussian fusion method is used to determine the final pitch angle of the multi-legged robot, that is, if the multi-legged robot is at any of the two ground contact moments, the feedforward pitch angle is used as the final pitch angle; otherwise, the value obtained by weighted fusion of the feedforward pitch angle and the real-time correction of the pitch angle is used as the final pitch angle; The final pitch angle and feedforward roll angle are combined with the gait and speed commands input by the user and converted into joint torque, expected joint position and expected joint speed after NMPC-WBC solution to control the multi-legged robot to move on the slope.

2. The multi-legged robot slope control method based on bimodal Gaussian fusion as claimed in claim 1 is characterized in that: The two ground contact moments of the multi-legged robot are: the moment 0 and the moment T / 2 when all legs of the multi-legged robot touch the ground simultaneously within a complete gait cycle T in the trot gait.

3. The multi-legged robot slope control method based on bimodal Gaussian fusion as claimed in claim 1 is characterized in that: The pitch angle and roll angle of a multi-legged robot on a slope are obtained according to a foot-end plane equation, including: setting a yaw angle threshold and a terrain flatness threshold, when the yaw angle of the multi-legged robot is less than the set yaw angle threshold and the terrain flatness is less than the set terrain flatness threshold, a first composite control strategy is used in combination with the foot-end plane equation to calculate the pitch angle and the roll angle; otherwise, a second composite control strategy is used in combination with the foot-end plane equation to calculate the pitch angle and the roll angle.

4. The multi-legged robot slope control method based on bimodal Gaussian fusion as claimed in claim 3 is characterized in that: The first composite control strategy is: the direction of the multi-legged robot when going uphill is regarded as facing the slope, that is, the yaw angle is kept at zero, and the pitch angle and roll angle are calculated according to the foot-end plane equation; the second composite control strategy is: first, use the IMU to read the yaw angle and calculate the slope normal vector, and then calculate the pitch angle and roll angle according to the foot-end plane equation.

5. The multi-legged robot slope control method based on bimodal Gaussian fusion as claimed in claim 1 is characterized in that: The plane angle memory update algorithm is: ； in, and They represent the updated feedforward roll angle and feedforward pitch angle of the multi-legged robot on the slope, and They represent the rolling angle and pitch angle of the multi-legged robot on the slope obtained by the foot end plane equation at the current moment, and They respectively represent the roll angle and pitch angle of the multi-legged robot on the slope obtained by the foot end plane equation saved in the previous gait cycle; Represents the attenuation coefficient.

6. The multi-legged robot slope travel control method based on double-peak Gaussian fusion as claimed in claim 1, characterized in that: The joint angle difference calculation is performed based on the joint angle feedback algorithm, including: calculating the difference between the sum of the front knee joint angles and the sum of the rear knee joint angles, and generating a real-time correction value for the pitch angle through a PID controller.

7. The multi-legged robot slope travel control method based on bimodal Gaussian fusion as claimed in claim 1, characterized in that: The weighted fusion algorithm when the value after weighted fusion of the feedforward pitch angle and the real-time pitch angle correction is used as the final pitch angle is: ； In the formula, represents the final pitch angle of the multi-legged robot on the slope after weighted fusion, represents the feedforward pitch angle of the multi-legged robot on the slope after being updated by the plane angle memory update algorithm, Represents the real-time correction of the pitch angle generated by the PID controller; and Respectively represent and The corresponding weights; among them, The value of gradually decays as it moves away from time 0 and time T / 2 within the gait cycle T of the multi-legged robot.

8. A multi-legged robot slope travel control system based on bimodal Gaussian fusion, characterized in that: include: The ground contact moment detection module is configured to: obtain the three-dimensional coordinates of the foot end of the multi-legged robot at two ground contact moments, and construct a foot end plane equation; Obtain the pitch angle and roll angle of the multi-legged robot on the slope according to the foot end plane equation; The feedforward module is configured to: perform feedforward calculation on the obtained pitch angle and roll angle based on a plane angle memory update algorithm including an attenuation coefficient, so as to obtain a feedforward pitch angle and a feedforward roll angle of the multi-legged robot on the slope; The joint angle feedback module is configured to: calculate the joint angle difference based on the joint angle feedback algorithm, and determine the real-time correction amount of the pitch angle of the multi-legged robot on the slope according to the difference calculation result; The bimodal Gaussian fusion control module is configured to: determine the final pitch angle of the multi-legged robot by using the bimodal Gaussian fusion method, that is, if the multi-legged robot is at any one of the two ground contact moments, the feedforward pitch angle is used as the final pitch angle; otherwise, the value obtained by weighted fusion of the feedforward pitch angle and the real-time correction amount of the pitch angle is used as the final pitch angle; The travel control module is configured to: convert the obtained final pitch angle and feedforward roll angle into joint torque, expected joint position and expected joint speed after NMPC-WBC solution in combination with the gait and speed instructions input by the user, so as to control the multi-legged robot to move on the slope.

9. A computer-readable storage medium having a program stored thereon, characterized in that: When the program is executed by a processor, the steps in the multi-legged robot slope travel control method based on bimodal Gaussian fusion as described in any one of claims 1 to 7 are implemented.

10. An electronic device comprising a memory, a processor, and a program stored in the memory and executable on the processor, characterized in that: When the processor executes the program, the steps in the multi-legged robot slope travel control method based on bimodal Gaussian fusion as described in any one of claims 1 to 7 are implemented.

Images