CN113867333B - Four-foot robot stair climbing planning method based on visual perception and application thereof - Google Patents
Four-foot robot stair climbing planning method based on visual perception and application thereof Download PDFInfo
- Publication number
- CN113867333B CN113867333B CN202111034406.8A CN202111034406A CN113867333B CN 113867333 B CN113867333 B CN 113867333B CN 202111034406 A CN202111034406 A CN 202111034406A CN 113867333 B CN113867333 B CN 113867333B
- Authority
- CN
- China
- Prior art keywords
- stair
- target
- target robot
- preset
- robot
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 230000009194 climbing Effects 0.000 title claims abstract description 117
- 238000000034 method Methods 0.000 title claims abstract description 59
- 230000016776 visual perception Effects 0.000 title claims abstract description 39
- 206010034701 Peroneal nerve palsy Diseases 0.000 claims description 28
- 230000005021 gait Effects 0.000 claims description 11
- 238000004364 calculation method Methods 0.000 claims description 9
- 238000012937 correction Methods 0.000 claims description 9
- 230000004438 eyesight Effects 0.000 claims description 6
- 238000005457 optimization Methods 0.000 claims description 4
- 230000008447 perception Effects 0.000 claims description 3
- 210000003371 toe Anatomy 0.000 description 27
- 210000002683 foot Anatomy 0.000 description 23
- 239000011159 matrix material Substances 0.000 description 13
- 230000033001 locomotion Effects 0.000 description 8
- 238000004422 calculation algorithm Methods 0.000 description 6
- 230000006870 function Effects 0.000 description 6
- 210000000689 upper leg Anatomy 0.000 description 6
- 238000010586 diagram Methods 0.000 description 5
- 230000005540 biological transmission Effects 0.000 description 4
- 230000008569 process Effects 0.000 description 4
- 230000011218 segmentation Effects 0.000 description 4
- 230000001052 transient effect Effects 0.000 description 4
- 230000009286 beneficial effect Effects 0.000 description 3
- 238000006243 chemical reaction Methods 0.000 description 3
- 210000002414 leg Anatomy 0.000 description 3
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical compound C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 description 2
- 230000000386 athletic effect Effects 0.000 description 2
- 244000309466 calf Species 0.000 description 2
- 230000002493 climbing effect Effects 0.000 description 2
- 238000004891 communication Methods 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 239000000284 extract Substances 0.000 description 2
- 239000004973 liquid crystal related substance Substances 0.000 description 2
- 239000000463 material Substances 0.000 description 2
- 238000005259 measurement Methods 0.000 description 2
- 239000000203 mixture Substances 0.000 description 2
- 230000003287 optical effect Effects 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 238000005070 sampling Methods 0.000 description 2
- 238000000638 solvent extraction Methods 0.000 description 2
- 230000009466 transformation Effects 0.000 description 2
- 230000000007 visual effect Effects 0.000 description 2
- PBGKNXWGYQPUJK-UHFFFAOYSA-N 4-chloro-2-nitroaniline Chemical compound NC1=CC=C(Cl)C=C1[N+]([O-])=O PBGKNXWGYQPUJK-UHFFFAOYSA-N 0.000 description 1
- 230000009471 action Effects 0.000 description 1
- 230000006399 behavior Effects 0.000 description 1
- 238000004590 computer program Methods 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 230000001419 dependent effect Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000018109 developmental process Effects 0.000 description 1
- 239000003814 drug Substances 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 238000009434 installation Methods 0.000 description 1
- 230000003993 interaction Effects 0.000 description 1
- 238000013178 mathematical model Methods 0.000 description 1
- 238000010295 mobile communication Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 239000007787 solid Substances 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
- 230000007723 transport mechanism Effects 0.000 description 1
- 210000001364 upper extremity Anatomy 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
- G05D1/0246—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means
- G05D1/0251—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means extracting 3D information from a plurality of images taken from different locations, e.g. stereo vision
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
Abstract
The invention discloses a four-foot robot stair climbing planning method based on visual perception and application thereof, and relates to the field of robots, wherein the four-foot robot stair climbing planning method based on visual perception comprises the following steps: obtaining stair geometric parameters in the space three-dimensional point cloud; calculating a preset reference speed of the target robot according to the stair geometric parameters and the target robot size; calculating a preset foot falling position of the target robot according to the stair geometric parameters and the preset reference speed; correcting the pose of the target robot according to the geometric parameters of the stairs, and calculating the target pose of the target robot in real time; and planning a stair climbing track of the target robot according to the preset reference speed, the preset foot falling position and the target gesture. According to the visual perception-based four-foot robot stair climbing planning method, the optimal climbing track can be planned autonomously, the climbing efficiency is improved, the calculated amount is greatly reduced, and the method can be suitable for various stairs in a real environment.
Description
Technical Field
The invention relates to the technical field of computers, in particular to a four-foot robot stair climbing planning method based on visual perception and application thereof.
Background
Robots are commonly known as robots, which include all machines that simulate human behavior or ideas and other living things, and are of great use in industry, medicine, agriculture, construction and even military. For example, four-legged robots, which have made long-legged progress in athletic ability thanks to recent hardware designs and rapid developments of motion controllers, have been commercialized, and many have exhibited remarkable athletic ability under uneven terrain.
At present, stairways are often studied as a special case in uneven terrain. In the research of the motion of the quadruped robot in uneven terrain, two planning schemes of non-visual perception and visual perception can be roughly divided. The planning scheme without visual perception, namely 'blind climbing', can actually realize climbing of stairs by accident, but has very limited robustness, reliability and performance; in the planning scheme with visual perception, the control of external instructions is often needed to complete the climbing of stairs, the condition that the same step is invalid to step for many times usually occurs when climbing is performed in the stair environment, and the climbing action is unnatural.
Disclosure of Invention
The present invention aims to solve at least one of the technical problems existing in the prior art. Therefore, the embodiment of the invention provides the four-foot robot stair climbing planning method based on visual perception, which can autonomously plan the optimal climbing track, improve the climbing efficiency, greatly reduce the calculated amount and can be suitable for various stairs in a real environment.
The embodiment of the invention also provides a stair climbing planning device.
The embodiment of the invention also provides electronic equipment.
The embodiment of the invention also provides a computer readable storage medium.
According to an embodiment of the first aspect of the invention, a four-foot robot stair climbing planning method based on visual perception comprises the following steps:
obtaining stair geometric parameters in the space three-dimensional point cloud;
calculating a preset reference speed of the target robot according to the stair geometric parameters and the target robot size;
calculating a preset foot drop position of the target robot according to the stair geometric parameters and the preset reference speed;
correcting the pose of the target robot according to the stair geometric parameters, and calculating the target pose of the target robot in real time;
and planning a stair climbing track of the target robot according to the preset reference speed, the preset foot falling position and the target gesture.
According to the visual perception-based four-foot robot stair climbing planning method provided by the embodiment of the first aspect of the invention, the method has at least the following beneficial effects: firstly, stair geometric parameters in a space three-dimensional point cloud are obtained, the preset reference speed of a target robot is calculated, then the preset foot falling position of the target robot is calculated according to the stair geometric parameters and the preset reference speed, then the pose of the target robot is corrected according to the stair geometric parameters to obtain the target pose, finally the stair climbing track of the target robot is planned according to the preset reference speed, the preset foot falling position and the target pose, the optimal climbing track can be planned autonomously, the climbing efficiency is improved, the calculated amount is greatly reduced, and the stair climbing device can be suitable for various stairs in a real environment.
According to some embodiments of the invention, the planning the climbing trajectory of the target robot according to the preset reference speed, the preset foot-drop position and the target gesture includes: calculating a target control variable according to the preset reference speed and the preset landing position; calculating joint moment of the target robot according to the target control variable; and controlling the stair climbing track of the target robot according to the joint moment and the target gesture plan.
According to some embodiments of the invention, the obtaining stair geometry parameters in the three-dimensional point cloud comprises: denoising the space three-dimensional point cloud to obtain a target three-dimensional point cloud; partitioning the target three-dimensional point cloud into Ping Miandian clouds; and extracting the geometric parameters of the stairs according to the plane point cloud and the structural information of the stairs.
According to some embodiments of the invention, the calculating the preset reference speed of the target robot according to the stair geometric parameter and the target robot size includes: defining a preset toe length of the target robot according to the stair geometric parameters; and calculating the preset reference speed according to a preset gait cycle, the target robot size and the preset toe length.
According to some embodiments of the invention, the calculating the preset landing position of the target robot according to the stair geometric parameter and the preset reference speed includes: acquiring the central real-time position of the target robot; calculating a nominal foot drop position according to the center real-time position and the preset reference speed; correcting the nominal landing position according to the geometric parameters of the stairs to obtain the preset landing position.
According to some embodiments of the invention, the correcting the nominal landing position according to the stair geometric parameter to obtain the preset landing position includes: acquiring a real-time toe position of the target robot; extracting the position of the central line of the stair from the geometric parameters of the stair; and correcting the nominal landing position according to the centerline position of the stair and the real-time toe position by using a quadratic programming optimization method to obtain the preset landing position.
According to some embodiments of the invention, the correcting the pose of the target robot according to the stair geometric parameter, calculating the target pose of the target robot in real time, includes: establishing a first local coordinate system, a second local coordinate system and a third local coordinate system corresponding to the real-time position of the target robot according to the stair geometric parameters; calculating according to the first local coordinate system to obtain a first position, calculating according to the second local coordinate system to obtain a second position, and calculating according to the third local coordinate system to obtain a third position; and correcting the pose of the target robot according to the first position, the second position and the third position to obtain the target pose.
According to an embodiment of the second aspect of the invention, a stair climbing planning device comprises:
the first acquisition module is used for acquiring stair geometric parameters in the space three-dimensional point cloud;
the first calculation module is used for calculating the preset reference speed of the target robot according to the stair geometric parameters and the target robot size;
the second calculation module is used for calculating the preset foot falling position of the target robot according to the stair geometric parameters and the preset reference speed;
the correcting module is used for correcting the pose of the target robot according to the stair geometric parameters and calculating the target pose of the target robot in real time;
and the planning module is used for planning a stair climbing track of the target robot according to the preset reference speed, the preset foot falling position and the target gesture.
According to the stair climbing planning device provided by the embodiment of the second aspect of the invention, the stair climbing planning device has at least the following beneficial effects: by executing the visual perception-based four-foot robot stair climbing planning method provided by the embodiment of the first aspect of the invention, the optimal climbing track can be planned autonomously, the climbing efficiency is improved, the calculated amount is greatly reduced, and the method can be suitable for various stairs in a real environment.
An electronic device according to an embodiment of a third aspect of the present invention includes: at least one processor, and a memory communicatively coupled to the at least one processor; the memory stores instructions that are executed by the at least one processor to cause the at least one processor to implement the vision perception-based four-foot robot stair climbing planning method of the first aspect when executing the instructions.
The electronic equipment according to the embodiment of the third aspect of the invention has at least the following beneficial effects: by executing the visual perception-based four-foot robot stair climbing planning method provided by the embodiment of the first aspect of the invention, the optimal climbing track can be planned autonomously, the climbing efficiency is improved, the calculated amount is greatly reduced, and the method can be suitable for various stairs in a real environment.
A computer readable storage medium according to an embodiment of the fourth aspect of the present invention stores computer executable instructions for causing a computer to perform the vision perception based four-legged robot stair climbing planning method of the first aspect.
The computer-readable storage medium according to the embodiment of the fourth aspect of the present invention has at least the following advantageous effects: by executing the visual perception-based four-foot robot stair climbing planning method provided by the embodiment of the first aspect of the invention, the optimal climbing track can be planned autonomously, the climbing efficiency is improved, the calculated amount is greatly reduced, and the method can be suitable for various stairs in a real environment.
Additional aspects and advantages of the invention will be set forth in part in the description which follows, and in part will be obvious from the description, or may be learned by practice of the invention.
Drawings
The foregoing and/or additional aspects and advantages of the invention will become apparent and may be better understood from the following description of embodiments taken in conjunction with the accompanying drawings in which:
fig. 1 is a flow diagram of a stair climbing planning method for a four-foot robot based on visual perception according to an embodiment of the present invention;
FIG. 2 is a schematic illustration of stair geometry according to an embodiment of the present invention;
FIG. 3 is a schematic diagram of the structure of a first local coordinate system, a second local coordinate system, and a third local coordinate system according to an embodiment of the present invention;
FIG. 4 is a schematic diagram of a stair climbing planning apparatus according to an embodiment of the present invention;
fig. 5 is a functional block diagram of an electronic device according to an embodiment of the present invention.
Reference numerals:
the system comprises a first acquisition module 400, a first calculation module 410, a second calculation module 420, a correction module 430, a planning module 440, a processor 500, a memory 510, a data transmission module 520, a camera 530 and a display screen 540.
Detailed Description
Embodiments of the present invention are described in detail below, examples of which are illustrated in the accompanying drawings, wherein like or similar reference numerals refer to like or similar elements or elements having like or similar functions throughout. The embodiments described below by referring to the drawings are illustrative only and are not to be construed as limiting the invention.
In the description of the present application, it should be understood that references to orientation descriptions such as upper, lower, front, rear, left, right, etc. are based on the orientation or positional relationship shown in the drawings, are merely for convenience of description of the present application and to simplify the description, and do not indicate or imply that the apparatus or elements referred to must have a particular orientation, be constructed and operated in a particular orientation, and thus should not be construed as limiting the present application.
In the description of the present application, unless explicitly defined otherwise, terms such as arrangement, installation, connection, etc. should be construed broadly and the specific meaning of the terms in the present application can be reasonably determined by a person skilled in the art in combination with the specific contents of the technical scheme.
First, several nouns involved in the present application are parsed:
1. depth camera: the depth camera is also called a 3D camera, and compared with the traditional camera, the depth camera is functionally added with a depth measurement, so that the surrounding environment and changes can be perceived more conveniently and accurately. A depth camera is a camera that can acquire the physical distance of an object in a scene from a camera. Depth cameras are generally composed of various lenses and optical sensors, and according to the measurement principle, the mainstream depth cameras are generally divided into the following methods: time of flight, structured light, binocular stereo vision.
2. Model predictive control: model predictive control, MPC for short, is a special class of control. Its current control action is obtained by solving a finite time domain open loop optimal control problem at each sampling instant. The current state of the process is used as the initial state of the optimal control problem, and the solved optimal control sequence only implements the first control effect. This is the biggest difference from those algorithms that use pre-computed control laws. Essentially model predictive control solves an open loop optimal control problem. Its ideas are independent of the specific model, but implementation is model dependent.
3. PD control: PD is the power down mode bit of the power control register PCON in the singlechip. The bit write 1 initiates a power down mode when the clock is frozen.
4. Random sample consensus algorithm: random sample consensus, RANSAC for short, it can estimate the parameters of a mathematical model in an iterative manner from a set of observation data sets containing "outliers". It is an uncertain algorithm-it has a certain probability to get a reasonable result; the number of iterations must be increased in order to increase the probability.
At present, stairways are often studied as a special case in uneven terrain. In the research of the motion of the quadruped robot in uneven terrain, two planning schemes of non-visual perception and visual perception can be roughly divided. The planning scheme without visual perception, namely 'blind climbing', can actually realize climbing of stairs by accident, but has very limited robustness, reliability and performance; in a planning scheme with visual perception, external instructions are often required to control to finish climbing stairs, the condition that the same step is invalid to step for many times usually occurs when climbing in a stair environment, and climbing actions are unnatural
Based on the above, the embodiment of the invention provides a four-foot robot stair climbing planning method, a device, electronic equipment and a storage medium based on visual perception, which can autonomously plan an optimal climbing track, improve climbing efficiency, greatly reduce calculated amount and adapt to various stairs in a real environment.
Referring to fig. 1, a four-foot robot stair climbing planning method based on visual perception according to an embodiment of the first aspect of the present invention includes:
step S100, stair geometric parameters in the space three-dimensional point cloud are obtained.
The space three-dimensional point cloud can be a space three-dimensional point cloud of stairs acquired by a depth camera; stair geometry parameters may include: the height h, depth d, width w, rotation matrix (relative to the depth camera) and distance (from a vertical surface of the stairs to the depth camera) of the stairs. Optionally, in order to complete the task of climbing stairs, a spatial three-dimensional point cloud of stairs may be obtained from the depth camera, and then the spatial three-dimensional point cloud is processed, for example, denoising and segmentation are performed on the spatial three-dimensional point cloud, and the required geometric parameters of stairs are extracted from the processed point cloudReferring to FIG. 2, FIG. 2 shows a stair geometry wherein the normal vector of the vertical plane of the stair where the o-point is the center point of the stair is n r Normal vector n of tread t The length w and z points of the stair plane are the midpoints of the plane length of the stair, and can be lifted by the space three-dimensional point cloud of the depth camera DTaking out the height h, depth D, width w, rotation matrix of the stairs relative to the depth camera D>And distance->The stair geometry parameters can be used for planning stair climbing tracks of the target robot. The structural information of the stairs can be fully utilized, and a complex grid map is not required to be established, so that the calculated amount is greatly reduced.
Step S110, calculating the preset reference speed of the target robot according to the stair geometric parameters and the target robot size.
Wherein the target robot may be a robot that needs to perform a stair climbing task, such as a quadruped robot; the preset reference speed may be a preset travel speed of the target robot when performing a stair climbing task. Alternatively, the preset reference speed may be determined from the time consumption of each step of the target robot and the length of the stair step, assuming that the preset reference speed is calculated as v ref By presetting a reference speed v ref The reliability of the target robot for climbing stairs is guaranteed.
Step S120, calculating a preset foot drop position of the target robot according to the stair geometric parameters and the preset reference speed.
The preset landing position may be a landing point of the target robot on the stair, for example, a preset landing coordinate. Optionally, the center real-time position and the preset reference speed of the middle target robot can be v ref Substituting a preset formula to obtain the theoretical foot falling position of the robot. Because the foot drop position of the target robot should be as close to the central line of the stair as possible, the situation that the foot drop position steps on the edge of the stair is reduced, therefore, the theoretical foot drop position of the target robot needs to be subjected to secondary planning, for example, the foot drop position of the target robot can be corrected in real time according to the real-time toe position of the target robot and the central line position of the stair in the geometric parameters of the stair, and the target robot can be obtainedThe preset foot drop position of the person.
And step S130, correcting the pose of the target robot according to the stair geometric parameters, and calculating the target pose of the target robot in real time.
The target gesture may be a gesture of the corrected target robot relative to the stairs. Optionally, the inertial sensor and the encoder may perform state estimation on the target robot, but since the state estimation of the inertial sensor and the encoder tends to continuously accumulate errors, the accumulated errors are more serious especially for the quadruped robot moving on uneven ground; the real-time pose correction can greatly reduce the error of falling feet, avoid tripping and stepping on the air caused by inaccurate positioning, and enhance the stability of movement, so that the pose of the target robot needs to be corrected in real time. Alternatively, the posture of the target robot in different states may be corrected in real time based only on the real-time surrounding terrain where the target robot is located, rather than the global terrain, to obtain the target posture of the target robot.
Step S140, planning a stair climbing track of the target robot according to the preset reference speed, the preset foot falling position and the target gesture.
Optionally, based on the preset reference speed, the preset foot falling position and the target gesture of the target robot planned above, the model prediction controller is utilized to control 12 joint motors of the target robot, the joint moment at the current moment is calculated, then the stair climbing track of the target robot is automatically planned according to the joint moment, so that the target robot can climb stairs along the planned track, and the stair climbing task is completed.
According to the stair climbing planning method for the four-legged robot based on visual perception, firstly, stair geometric parameters in the space three-dimensional point cloud are obtained, the preset reference speed of the target robot is calculated, then the preset foot falling position of the target robot is calculated according to the stair geometric parameters and the preset reference speed, then pose correction is carried out on the target robot according to the stair geometric parameters, the target pose is obtained, finally, the stair climbing track of the target robot is planned according to the preset reference speed, the preset foot falling position and the target pose, the optimal climbing track can be planned autonomously, the climbing efficiency is improved, the calculated amount is greatly reduced, and the stair climbing planning method can be suitable for various stairs in a real environment.
In some embodiments of the present invention, a climbing track of a target robot is planned according to a preset reference speed, a preset landing position and a target gesture, including:
and calculating a target control variable according to the preset reference speed and the preset landing position. The target control variable can be an optimal control variable of the target robot at the current moment corresponding to the target robot when climbing stairs. Optionally, the control may be performed by a model prediction controller, assuming that the target robot is a quadruped robot, predicting a state of the target robot within a certain time by using a simplified model of the quadruped robot, and optimizing a cost equation obtained based on the predicted state to obtain an optimal control quantity u= [ f ] at the current moment 1 f 2 f 3 f 4 ] T Wherein f 1 、f 2 、f 3 And f 4 The ground reaction force of each foot of the four-foot robot is respectively represented by a superscript T, and the transpose of the matrix is represented by specific formulas (1) to (4) as follows:
subject to x k+1 ＝A k x k +B k u k +g……(2)
D k u k ＝0……(4)
wherein,representing the states of the four-foot robot (including attitude angle, center point position, angular velocity and center point line velocity); x is x k+1,ref Is a reference state; q (Q) k And R is k Representing a parameter matrix, C k And D k All are constant matrices; a is that k 、B k For two matrices, A k 、B k The following is shown:
the above formula (1) describes a cost equation describing the following performance in model predictive control, (2) describes a state space equation based on a simplified model, (3) describes a constraint equation of friction cone versus control amount, and (4) describes a constraint equation of target robot torque balance. The quadratic programming can be performed by the above formulas (1) to (4), and the objective is to find the optimal solution that satisfies the constraint conditions of formulas (2) to (4) and at the same time minimizes formula (1) as much as possible.
And calculating the joint moment of the target robot according to the target control variable and the target gesture. The joint torque may be a driving torque of a certain joint of the target robot. Optionally, the joint moment τ of the target robot i Can be calculated from the Jacobian matrix and the target control variable, and can be based on the ground reaction force f in the target control variable according to the following equation (5), for example i And Jacobian matrix calculation to obtain tau i ：
Wherein J is i For the jacobian matrix, the superscript T denotes the transpose of the matrix.
And controlling the stair climbing track of the target robot according to the joint moment and the target gesture plan. Optionally, in order to make the climbing gesture of the target robot more natural and ensure the climbing stability, the method can be based on the gait planning of combination and setting of the state estimation of the sensor information, and based on the joint moment of the target robot, the target gesture is added for optimization, namely the target robot is controlled by the joint PD through the target gesture, so as to obtain the stair climbing track of the target robot. The target control variable is calculated through the preset reference speed and the preset foot falling position, then the joint moment of the target robot is calculated according to the target control variable, finally the stair climbing track of the target robot is planned according to the joint moment and the target gesture, the calculated amount can be reduced, the automatic planning of the stair climbing track is realized, and the stair climbing method is suitable for various stairs.
In some embodiments of the invention, obtaining stair geometry parameters in a spatial three-dimensional point cloud comprises:
and denoising the space three-dimensional point cloud to obtain a target three-dimensional point cloud. The target three-dimensional point cloud may be a three-dimensional point cloud obtained by denoising the space three-dimensional point cloud. Optionally, in order to accurately extract the geometric parameters of the stairs, a filter can be used to remove depth noise of the space three-dimensional point cloud and simultaneously maintain edge characteristics, namely, denoising the space three-dimensional point cloud, so as to obtain a target three-dimensional point cloud after denoising.
And dividing the target three-dimensional point cloud into planar point clouds. Alternatively, a clustering algorithm based on Euclidean distance can be utilized to segment a target three-dimensional point cloud, a random sampling coincidence algorithm is utilized to extract Ping Miandian cloud from the segmented target three-dimensional point cloud, and a characteristic center point c of the segmented plane point cloud is extracted i Normal vector n of vertical plane i Feature vector v i 。
And extracting the geometric parameters of the stairs according to Ping Miandian cloud and stair structure information. Alternatively, as shown in fig. 2, the required stair geometry parameters may be extracted from the planar point cloud, for example, the height h and depth d of the stair may be obtained by projecting the central point line of the adjacent vertical planes (treads) to the corresponding normal vector, and the feature vector v extracted from each plane may be used i Obtaining the range of the plane point cloud in the direction of the maximum characteristic vector to obtain the length w of the plane, and utilizing the normal vector n of the vertical plane i Normal vector n of tread t And the cross vector of both (n) i ×n t ) The composition matrix is the transformation matrix of the stair coordinate system relative to the depth camera coordinate systemThe position of the center point of the first vertical plane obtained by segmentation in the camera coordinate system is marked as +.>Thereby obtaining the desired stair set parameter +.>By extracting the geometric parameters of the stairs simply and effectively from the space three-dimensional point cloud, the structural information of the stairs is fully utilized, a complex grid map is not required to be established, and the calculated amount is greatly reduced while the geometric parameters of the stairs with high precision are ensured.
In some embodiments of the invention, calculating the preset reference speed of the target robot based on the stair geometry and the target robot size comprises:
and defining the preset toe length of the target robot according to the stair geometric parameters. The preset toe length may be the maximum length of the target robot's one-step toe. Alternatively, it is assumed that the preset toe length of the target robot is defined as L (L H ,l K ) Wherein l H And l K The lengths of the thigh and the shank, respectively, may be a predetermined toe length L (L H ,l K ) For calculating a preset reference speed.
And calculating a preset reference speed according to the preset gait cycle, the target robot size and the preset toe length. The preset gait cycle may be one gait cycle of the preset target robot, and the gait cycle may be a time elapsed from heel-off of the same foot to heel-on of the same foot when walking. Optionally, the preset minor period may be set as desired, assuming a preset gait period of T G The length of the toe is preset to be L (L) H ,l K )，l H And l K The lengths of the thigh and the calf are respectively, and the integer programming can be performed on the number of stairs which the target robot should cross for one gait cycle according to the following formula (6), formula (7) and formula (8):
subject to L(l H ,l K )∈s·k±Δ (7)
wherein,representing the inclined length of each step of the stair, d is the depth of the stair, h is the height of the stair, L 0 Representing the distance between the front and rear thigh joints of the target robot, < ->Is the maximum length of the front and rear toes on the same side, and delta represents the offset of the actual foothold point relative to the center line. In the above formula, the formula (7) requires that the calculated length of the landing point of the target robot does not break through the kinematic constraint of the maximum swing leg (the length of the thigh is related to the length of the shank), and the formula (8) avoids that the front leg and the rear leg on the same side of the body are stepped on the same step and the front landing point and the rear landing point break through the kinematic constraint of the maximum length. Although this is an integer programming, the above is only one-dimensional and k can vary from 0 to 3 depending on the actual hardware and stairs. When the value range of k is 0-3, in order to ensure that the toe of each step of the target robot is stepped in the middle of the tread as much as possible, the reliability of stair climbing is ensured, and the preset reference speed v can be solved according to the following formula (9) ref ：
The preset reference velocity v can be calculated by the above formula (9) ref . By defining the preset toe length of the target robot, and then calculating the preset reference speed according to the preset gait cycle and the preset toe lengthThe optimal reference speed planning based on stair parameters can be realized, and the climbing efficiency of the target robot is improved.
In some embodiments of the present invention, calculating a preset landing position of a target robot according to a stair geometry parameter and a preset reference speed includes:
and acquiring the center real-time position of the target robot. The center real-time position may be a position corresponding to a center of the target robot detected in real time, and the center real-time position may be a center of the robot with respect to a local frame (local frame) coordinate system. Alternatively, since the center position of the target robot changes in real time during the stair climbing process, the center position of the target robot can be detected in real time, for example, at time t 0 When the position of the central embodiment of the target robot is c 0 。
And calculating a nominal foot falling position according to the central real-time position and a preset reference speed. The nominal landing position may be the theoretical landing coordinate of the target machine when performing the stair climbing task. Alternatively, assume the center real-time position is c, assume the preset reference velocity is v ref Can be usedRepresenting a preset reference velocity v ref The center real-time position and the preset reference speed can be expressed as +.>Can be +.>Substituting the following formulas (10) and (11) to obtain the nominal falling foot position
Wherein p is hip,i Indicating the thigh joint position corresponding to the ith leg of the target robot,representing a preset reference speed, T st Representing the length of time, K, of the support state v Representing the adjustable parameter. The nominal foot drop position can be calculated according to the above formula (10) and formula (11)>
Correcting the nominal landing position according to the geometric parameters of the stairs to obtain a preset landing position. Optionally, since the landing position of the target robot needs to be as close to the stair midline as possible, and the situation that the landing position steps on the stair edge is reduced, the nominal landing position needs to be corrected, so that the landing point of the target robot is aligned with the stair midline. Specifically, the nominal landing position can be subjected to secondary planning according to the geometric parameters of the stair, so that the nominal landing position is close to the central line of the stair, namely, the nominal landing position is corrected, and the corrected nominal landing position is used as a preset landing position. The method comprises the steps of obtaining the central real-time position of the target robot, calculating a first foot drop point according to the central real-time position and a preset reference speed, and finally correcting the nominal foot drop position according to the geometric parameters of the stairs to obtain the preset foot drop position, so that the optimal foot drop point planning of the target robot can be realized in different stair environments, and the optimal track of the target robot can be automatically planned.
In some embodiments of the present invention, correcting a plurality of nominal landing positions according to stair geometry parameters to obtain a preset landing position includes:
and acquiring the real-time toe position of the target robot. The real-time toe position may be a toe position of the target robot detected in real time. Optionally, an optionalBecause the toe position of the target robot changes in real time during the stair climbing process, the toe position of the target robot can be detected in real time, for example, the time t i In this case, the position of the center embodiment of the target robot can be obtained as p i 。
And extracting the position of the central line of the stair from the geometric parameters of the stair. Alternatively, the position of the centerline of the stair may be extracted from geometric parameters of the stair, for example, referring to fig. 2, in the stair shown in fig. 2, the position of the centerline of the stair may be a line (not shown in the figure) connecting a midpoint z of a length w of a first step of the stair and a center o-point of the stair, so as to obtain the position of the middle of the stair.
Correcting the nominal landing position according to the centerline position and the real-time toe position of the stair by using a quadratic programming optimization method to obtain a preset landing position. Alternatively, assume the real-time toe position is p i The nominal foot drop position p can be calculated by the following formula (12) n,i And (3) correcting:
the nominal foot drop position p can be calculated according to the above formula (12) n,i And correcting to obtain a preset falling position. The real-time toe position of the target robot is obtained, the stair center line position is extracted from the stair geometric parameters, the nominal foot drop position is corrected according to the stair center line position and the real-time toe position, the preset foot drop position is obtained, and the optimal foot drop planning of the target robot can be realized in different stair environments, so that the optimal track of the target robot can be automatically planned.
In some embodiments of the present invention, performing pose correction on a target robot according to stair geometry parameters, calculating a target pose of the target robot in real time, including:
and establishing a first local coordinate system, a second local coordinate system and a third local coordinate system corresponding to the real-time position of the target robot according to the stair geometric parameters. The first local coordinate system can be a first local coordinate system established according to the real-time gesture of the target robot; the second local coordinate system may be a second local coordinate system established according to the real-time pose of the target robot; the third local coordinate system may be a third local coordinate system established from the real-time pose of the target robot. Optionally, as shown in fig. 3, assuming that the horizontal rightward direction is the x direction, assuming that the target robot is a quadruped robot, three local coordinate systems may be established according to different states of the quadruped robot when performing a stair climbing task, for example, in a state that the quadruped robot is "approaching a stair", the coordinate systems are established at the lower edge of the first stage step, so as to obtain a first local coordinate system; under the condition that the quadruped robot is on the stairs, the coordinate system is built on the upper edge of the vertical surface of the corresponding stairs projected by the central point of the quadruped robot body, and a second local coordinate system is obtained; under the condition that the four-legged robot is close to the stair end point, the coordinate system is built at the upper edge of the last stage of the steps, and then a third local coordinate system is obtained.
And calculating according to the first local coordinate system to obtain a first position, calculating according to the second local coordinate system to obtain a second position, and calculating according to the third local coordinate system to obtain a third position. The first position may be a position of the target robot in the x direction in the first local coordinate; the second position may be a position of the target robot in the x-direction in the second local coordinates; the third position may be a position of the target robot in the x-direction in the third local coordinate. Optionally, assuming that the horizontal rightward direction is the x direction, in different local coordinate systems, the positions of the target robot in the x direction are different, so as to respectively obtain the first positions C x1 、C x2 And C x3 Comprehensively obtaining the position C of the target robot in the x direction x The method comprises the following steps:
in the above formulas (13), (14) and (15), d p And h p Can be made byCalculated according to simple geometric relation, and concretely, d p And h p Obtainable from before->(position of center point of corresponding stair surface under camera coordinate system) utilize +.>Conversion of (rotation matrix of camera coordinate system relative to stair coordinate system) to stair coordinate system. Equation (13) is used to calculate the first position, equation (14) is used to calculate the second position, and equation (15) is used to calculate the third position.
And correcting the pose of the target robot according to the first position, the second position and the third position to obtain the target pose. Alternatively, it may be composed ofAnd correcting the pose of the target robot relative to the stairs, wherein the correction can be operated in real time at the frequency of 20Hz, so that the corrected target pose of the target robot is obtained. The method comprises the steps of establishing a first local coordinate system, a second local coordinate system and a third local coordinate system of a target robot according to stair geometric parameters, calculating a first position, a second position and a third position of the target robot according to different local coordinate systems, and finally correcting the pose of the target robot according to the first position, the second position and the third position to obtain a target pose, so that the real-time pose correction of the target robot can be carried out, the falling foot error of the target robot is greatly reduced, tripping and stepping-over of the target robot due to inaccurate positioning are avoided, and the motion stability is enhanced.
The following describes in detail the process of the stair climbing planning method for a four-legged robot based on visual perception according to an embodiment of the present invention. It is to be understood that the following description is exemplary only and is not intended to limit the invention in any way.
The four-foot robot stair climbing planning method based on visual perception comprises the following steps:
firstly, extracting geometric parameters of stairs.
In order to complete the task of climbing stairs, the quadruped robot firstly extracts geometric parameters of stairs from the space three-dimensional point cloud acquired by the depth camera, as shown in fig. 2, for the space three-dimensional point cloud acquired by the depth camera, firstly, a filter can be utilized to remove depth noise of the space three-dimensional point cloud and simultaneously maintain edge characteristics; then, partitioning the point cloud by using a clustering algorithm based on Euclidean distance, and extracting Ping Miandian cloud by using a RANSAC algorithm; finally, extracting a characteristic center point c from the planar point cloud obtained by segmentation i Normal vector n of vertical plane i Feature vector v i The geometric information theta of the stairs can be extracted by utilizing the separated plane characteristics: the depth d and the height h of the stairs can be obtained by utilizing the projection of the central point connecting line of the adjacent vertical planes (treads) to the corresponding normal vector, and the characteristic vector v extracted by each plane is utilized i Obtaining the range of the plane point cloud in the direction of the maximum characteristic vector to obtain the length w of the plane, and utilizing the normal vector n of the vertical plane i Normal vector n of tread t And the cross vector of both (n) i ×n t ) The composition matrix is the transformation matrix of the stair coordinate system relative to the depth camera coordinate systemThe position of the center point of the first vertical plane obtained by segmentation in the camera coordinate system is marked as +.>Thereby obtaining the required stair set parameters
And secondly, planning the optimal reference speed of the quadruped robot.
Because the reference speed and the position of the landing point of the quadruped robot are the key points for reliably and efficiently completing the climbing of the autonomous stairs, the motion capability of the quadruped robot and parameters of different stairs can be consideredAnd automatically generating the optimal reference speed of the four-foot robot by the test speed. Alternatively, the reference speed of the quadruped robot may be approximately determined by the time of each step and the length of the stair steps. Based on this, the maximum length of the one-step toe of the four-legged robot can be defined as L (L) H ,l K ) Wherein l H And l K The length of the thigh and the calf, respectively. In order to ensure the reliability of stair climbing, the toe of each step of the quadruped robot is stepped on the middle of the tread of the stair as much as possible. In order not to lose generality, Δ represents the offset of the actual landing point relative to the stair centerline. Calculating the reference speed of the quadruped robot according to the formulas (6), (7), (8) and (9) to obtain the preset reference speed v of the quadruped robot ref 。
And thirdly, planning the optimal foot falling position of the four-foot robot.
Preset reference speed v based on stair geometric information theta and planning ref The theoretical foot falling position of the four-foot robot can be obtained. First, according to the real-time positions c and v of the center of the quadruped robot ref (use ofRepresenting a preset reference speed), the center real-time position and the preset reference speed are represented as +.>Can be +.>Substituting the above formula (10) and formula (11) to obtain the theoretical landing position p of the quadruped robot n,i . Since the foot drop position of the quadruped robot is required to be as close to the central line of the stair as possible, the condition that the foot drop position steps on the edge of the stair is reduced, the nominal foot drop position is required to be corrected, so that the foot drop point of the quadruped robot is aligned with the central line of the stair, and the theoretical foot drop position p of the quadruped robot can be calculated according to the formula (12) n,i And performing secondary planning to obtain the optimal foot falling position of the quadruped robot.
And fourthly, correcting the pose of the quadruped robot based on the visual parameters.
Because the state estimation of the inertial sensor and the encoder always accumulates errors, the accumulated errors are more serious especially for the motion of the quadruped robot on uneven ground; the real-time pose correction can greatly reduce the error of falling feet, avoid tripping and stepping on the air caused by inaccurate positioning, and enhance the stability of movement, so that the pose of the target robot needs to be corrected in real time. Specifically, the position of the quadruped robot on the stairs can be divided into three local coordinate systems corresponding to three states, as shown in fig. 3, the coordinate system is built at the lower edge of the first-stage steps in the state that the quadruped robot is close to the stairs, and then the first local coordinate system is obtained; under the condition that the quadruped robot is on the stairs, the coordinate system is built on the upper edge of the vertical surface of the corresponding stairs projected by the central point of the quadruped robot body, and a second local coordinate system is obtained; under the condition that the four-legged robot is close to the stair end point, the coordinate system is built at the upper edge of the last stage of the steps, and then a third local coordinate system is obtained. In different states, the position C of the robot in the x-direction x Can be obtained according to the above-described formula (13), formula (14) and formula (15). Finally, it can be composed ofAnd correcting the pose of the target robot relative to the stair, wherein the correction can be operated in real time at the frequency of 20Hz, so that the corrected target pose of the four-foot robot is obtained.
And fifthly, planning a stair climbing track of the quadruped robot based on the optimal foot falling position, the preset reference speed and the target gesture of the quadruped robot.
The model predictive controller can be used for controlling, the state of the target robot in a certain time is predicted by using a simplified model of the target robot, and the optimal control quantity u= [ f ] at the current moment can be obtained by optimizing a cost equation obtained based on the predicted state 1 f 2 f 3 f 4 ] T As shown in the above formulas (1) to (5), the joint force of the quadruped robot is obtainedMoment τ i Can be based on the joint moment tau of the quadruped robot i And controlling the quadruped robot to execute a stair climbing task, and obtaining a climbing track of the quadruped robot.
According to the visual perception-based four-foot robot stair climbing planning method, the optimal climbing track can be planned autonomously, the climbing efficiency is improved, the calculated amount is greatly reduced, and the method can be suitable for various stairs in a real environment.
Referring to fig. 4, a stair climbing planning device according to an embodiment of the second aspect of the present invention includes:
a first obtaining module 400, configured to obtain stair geometry parameters in the spatial three-dimensional point cloud;
a first calculation module 410, configured to calculate a preset reference speed of the target robot according to the stair geometric parameter and the target robot size;
the second calculation module 420 is configured to calculate a preset foot drop position of the target robot according to the stair geometric parameter and a preset reference speed;
the correcting module 430 is configured to correct the pose of the target robot according to the stair geometric parameters, and calculate the target pose of the target robot in real time;
the planning module 440 is configured to plan a stair climbing track of the target robot according to the preset reference speed, the preset landing position and the target gesture.
According to the stair climbing planning device, by executing the four-foot robot stair climbing planning method based on visual perception, which is disclosed by the embodiment of the invention, the optimal climbing track can be planned autonomously, the climbing efficiency is improved, the calculated amount is greatly reduced, and the stair climbing planning device can be suitable for various stairs in a real environment.
Referring to fig. 5, an embodiment of the third aspect of the present invention further provides a functional block diagram of an electronic device, including: at least one processor 500, and a memory 510 communicatively coupled to the at least one processor 500; a data transmission module 520, a camera 530, and a display 540 may also be included.
Wherein the processor 500 is configured to execute the visual perception based four-foot robot stair climbing planning method in the first aspect embodiment by invoking a computer program stored in the memory 510.
The data transmission module 520 is connected with the processor 500, so as to realize data interaction between the data transmission module 520 and the processor 500.
The cameras 530 may include front cameras and rear cameras. Typically, the front camera is disposed on the front panel of the terminal and the rear camera is disposed on the rear surface of the terminal. In some embodiments, the at least two rear cameras are any one of a main camera, a depth camera, a wide-angle camera and a tele camera, so as to realize that the main camera and the depth camera are fused to realize a background blurring function, and the main camera and the wide-angle camera are fused to realize a panoramic shooting and Virtual Reality (VR) shooting function or other fusion shooting functions. In some embodiments, camera 530 may also include a flash. The flash lamp can be a single-color temperature flash lamp or a double-color temperature flash lamp. The dual-color temperature flash lamp refers to a combination of a warm light flash lamp and a cold light flash lamp, and can be used for light compensation under different color temperatures.
The display 540 may be used to display information entered by a user or information provided to a user. The display 540 may include a display panel, which may optionally be configured in the form of a liquid crystal display (Liquid Crystal Display, LCD) or an Organic Light-Emitting Diode (OLED) or the like. Further, the touch panel may cover the display panel, and when the touch panel detects a touch operation thereon or thereabout, the touch panel is transferred to the processor 500 to determine the type of touch event, and then the processor 500 provides a corresponding visual output on the display panel according to the type of touch event. In some embodiments, the touch panel may be integrated with the display panel to implement input and output functions.
The memory is used as a non-transient storage medium and can be used for storing a non-transient software program and a non-transient computer executable program, such as the four-foot robot stair climbing planning method based on visual perception in the embodiment of the first aspect of the invention. The processor executes the non-transient software program and instructions stored in the memory, so as to realize the four-foot robot stair climbing planning method based on visual perception in the embodiment of the first aspect.
The memory may include a memory program area and a memory data area, wherein the memory program area may store an operating system, at least one application program required for a function; the storage data area may store a four-legged robot stair climbing planning method based on visual perception as in the embodiment of the first aspect described above. In addition, the memory may include high-speed random access memory, and may also include non-transitory memory, such as at least one magnetic disk storage device, flash memory device, or other non-transitory solid state storage device. In some embodiments, the memory optionally includes memory remotely located relative to the processor, the remote memory being connectable to the terminal through a network. Examples of such networks include, but are not limited to, the internet, intranets, local area networks, mobile communication networks, and combinations thereof.
The non-transitory software programs and instructions required to implement the vision-based four-legged robot stair climbing planning method in the first aspect embodiment are stored in a memory, which when executed by one or more processors, performs the vision-based four-legged robot stair climbing planning method in the first aspect embodiment.
The fourth aspect embodiment of the present invention also provides a computer-readable storage medium storing computer-executable instructions for: the visual perception-based four-foot robot stair climbing planning method in the embodiment of the first aspect is executed.
In some embodiments, the storage medium stores computer-executable instructions that are executed by one or more control processors, for example, by one processor in an electronic device of an embodiment of the third aspect, which may cause the one or more processors to perform the vision-aware four-legged robot stair climbing planning method of the embodiment of the first aspect.
The embodiments of the present invention have been described in detail with reference to the accompanying drawings, but the present invention is not limited to the above embodiments, and various changes can be made within the knowledge of one of ordinary skill in the art without departing from the spirit of the present invention.
The above described apparatus embodiments are merely illustrative, wherein the units illustrated as separate components may or may not be physically separate, i.e. may be located in one place, or may be distributed over a plurality of network elements. Some or all of the modules may be selected according to actual needs to achieve the purpose of the solution of this embodiment.
Those of ordinary skill in the art will appreciate that all or some of the steps, systems, and methods disclosed above may be implemented as software, firmware, hardware, and suitable combinations thereof. Some or all of the physical components may be implemented as software executed by a processor, such as a central processing unit, digital signal processor, or microprocessor, or as hardware, or as an integrated circuit, such as an application specific integrated circuit. Such software may be distributed on computer readable media, which may include computer storage media (or non-transitory media) and communication media (or transitory media). The term computer storage media includes both volatile and nonvolatile, removable and non-removable media implemented in any method or technology for storage of information such as computer readable instructions, data structures, program modules or other data, as known to those skilled in the art. Computer storage media includes, but is not limited to, RAM, ROM, EEPROM, flash memory or other memory technology, CD-ROM, digital Versatile Disks (DVD) or other optical disk storage, magnetic cassettes, magnetic tape, magnetic disk storage or other magnetic storage devices, or any other medium which can be used to store the desired information and which can be accessed by a computer. Furthermore, as is well known to those of ordinary skill in the art, communication media typically embodies computer readable instructions, data structures, program modules or other data in a modulated data signal such as a carrier wave or other transport mechanism and includes any information delivery media.
In the description of the present specification, reference to the terms "one embodiment," "some embodiments," "illustrative embodiments," "examples," "specific examples," or "some examples," etc., means that a particular feature, structure, material, or characteristic described in connection with the embodiment or example is included in at least one embodiment or example of the invention. In this specification, schematic representations of the above terms do not necessarily refer to the same embodiments or examples. Furthermore, the particular features, structures, materials, or characteristics described may be combined in any suitable manner in any one or more embodiments or examples.
While embodiments of the present invention have been shown and described, it will be understood by those of ordinary skill in the art that: many changes, modifications, substitutions and variations may be made to the embodiments without departing from the spirit and principles of the invention, the scope of which is defined by the claims and their equivalents.
Claims (8)
1. A four-foot robot stair climbing planning method based on visual perception is characterized by comprising the following steps:
obtaining stair geometric parameters in the space three-dimensional point cloud;
calculating a preset reference speed of the target robot according to the stair geometric parameters and the target robot size;
Calculating a preset foot drop position of the target robot according to the stair geometric parameters and the preset reference speed;
correcting the pose of the target robot according to the stair geometric parameters, and calculating the target pose of the target robot in real time;
planning a stair climbing track of the target robot according to the preset reference speed, the preset foot falling position and the target gesture;
the step of calculating the preset reference speed of the target robot according to the stair geometric parameters and the target robot size comprises the following steps:
defining a preset toe length of the target robot according to the stair geometric parameters;
calculating the preset reference speed according to a preset gait cycle, the target robot size and the preset toe length;
the step of calculating the preset landing position of the target robot according to the stair geometric parameter and the preset reference speed comprises the following steps:
acquiring the central real-time position of the target robot;
calculating a nominal foot drop position according to the center real-time position and the preset reference speed;
correcting the nominal landing position according to the geometric parameters of the stairs to obtain the preset landing position.
2. The visual perception-based four-foot robot stair climbing planning method according to claim 1, wherein the planning the stair climbing trajectory of the target robot according to the preset reference speed, the preset landing position and the target gesture comprises:
calculating a target control variable according to the preset reference speed and the preset landing position;
calculating joint moment of the target robot according to the target control variable;
and realizing stair climbing control of the target robot according to the joint moment and the target gesture plan.
3. The visual perception-based four-foot robot stair climbing planning method according to claim 1, wherein the step of obtaining stair geometry parameters in a spatial three-dimensional point cloud comprises:
denoising the space three-dimensional point cloud to obtain a target three-dimensional point cloud;
dividing the target three-dimensional point cloud into plane point clouds;
and extracting the geometric parameters of the stairs according to the plane point cloud and the structural information of the stairs.
4. The visual perception-based four-foot robot stair climbing planning method according to claim 1, wherein the correcting the nominal landing position according to the stair geometric parameter to obtain the preset landing position comprises:
Acquiring a real-time toe position of the target robot;
extracting the position of the central line of the stair from the geometric parameters of the stair;
and correcting the nominal landing position according to the centerline position of the stair and the real-time toe position by using a quadratic programming optimization method to obtain the preset landing position.
5. The stair climbing planning method for a quadruped robot based on visual perception according to claim 1, wherein the performing pose correction on the target robot according to the stair geometric parameters, calculating the target pose of the target robot in real time, comprises:
establishing a first local coordinate system, a second local coordinate system and a third local coordinate system corresponding to the real-time position of the target robot according to the stair geometric parameters;
calculating according to the first local coordinate system to obtain a first position, calculating according to the second local coordinate system to obtain a second position, and calculating according to the third local coordinate system to obtain a third position;
and correcting the pose of the target robot according to the first position, the second position and the third position to obtain the target pose.
6. Four sufficient robot stair climbing planning device based on visual perception, its characterized in that includes:
The first acquisition module is used for acquiring stair geometric parameters in the space three-dimensional point cloud;
the first calculation module is used for calculating the preset reference speed of the target robot according to the stair geometric parameters and the target robot size;
the second calculation module is used for calculating the preset foot falling position of the target robot according to the stair geometric parameters and the preset reference speed;
the correcting module is used for correcting the pose of the target robot according to the stair geometric parameters and calculating the target pose of the target robot in real time;
the planning module is used for planning a stair climbing track of the target robot according to the preset reference speed, the preset foot falling position and the target gesture;
the step of calculating the preset reference speed of the target robot according to the stair geometric parameters and the target robot size comprises the following steps:
defining a preset toe length of the target robot according to the stair geometric parameters;
calculating the preset reference speed according to a preset gait cycle, the target robot size and the preset toe length;
the step of calculating the preset landing position of the target robot according to the stair geometric parameter and the preset reference speed comprises the following steps:
Acquiring the central real-time position of the target robot;
calculating a nominal foot drop position according to the center real-time position and the preset reference speed;
correcting the nominal landing position according to the geometric parameters of the stairs to obtain the preset landing position.
7. An electronic device, comprising:
at least one processor, and,
a memory communicatively coupled to the at least one processor; wherein,
the memory stores instructions that are executed by the at least one processor to cause the at least one processor to implement the vision-aware four-legged robot stair climbing planning method according to any one of claims 1 to 5 when the instructions are executed.
8. A computer readable storage medium having stored thereon computer executable instructions for causing a computer to perform the vision perception based four-legged robot stair climbing planning method according to any one of claims 1 to 5.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111034406.8A CN113867333B (en) | 2021-09-03 | 2021-09-03 | Four-foot robot stair climbing planning method based on visual perception and application thereof |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111034406.8A CN113867333B (en) | 2021-09-03 | 2021-09-03 | Four-foot robot stair climbing planning method based on visual perception and application thereof |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113867333A CN113867333A (en) | 2021-12-31 |
| CN113867333B true CN113867333B (en) | 2023-11-17 |
Family
ID=78989571
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111034406.8A Active CN113867333B (en) | 2021-09-03 | 2021-09-03 | Four-foot robot stair climbing planning method based on visual perception and application thereof |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113867333B (en) |
Families Citing this family (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114536341B (en) * | 2022-03-21 | 2023-08-08 | 合肥工业大学 | Laser scanning joint angle pre-control method for climbing of large dog robot |
| CN114633825B (en) * | 2022-05-16 | 2022-09-16 | 深圳鹏行智能研究有限公司 | Movement control method, legged robot, and computer-readable storage medium |
| CN115503850B (en) * | 2022-11-17 | 2023-03-31 | 国网智能科技股份有限公司 | Foot end touchdown detection method and system for foot type robot |
Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109202901A (en) * | 2018-08-29 | 2019-01-15 | 厦门理工学院 | A kind of biped robot's stair climbing gait planning method, apparatus and robot |
| CN110231035A (en) * | 2019-06-27 | 2019-09-13 | 北京克莱明科技有限公司 | Climb mobile robot path guide method |
| CN112529903A (en) * | 2021-02-03 | 2021-03-19 | 德鲁动力科技(成都)有限公司 | Stair height and width visual detection method and device and robot dog |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR20130049029A (en) * | 2011-11-03 | 2013-05-13 | 삼성전자주식회사 | Walking robot and control method for thereof |
-
2021
- 2021-09-03 CN CN202111034406.8A patent/CN113867333B/en active Active
Patent Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109202901A (en) * | 2018-08-29 | 2019-01-15 | 厦门理工学院 | A kind of biped robot's stair climbing gait planning method, apparatus and robot |
| CN110231035A (en) * | 2019-06-27 | 2019-09-13 | 北京克莱明科技有限公司 | Climb mobile robot path guide method |
| CN112529903A (en) * | 2021-02-03 | 2021-03-19 | 德鲁动力科技(成都)有限公司 | Stair height and width visual detection method and device and robot dog |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113867333A (en) | 2021-12-31 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN113867333B (en) | Four-foot robot stair climbing planning method based on visual perception and application thereof | |
| CN109816696A (en) | A kind of robot localization and build drawing method, computer installation and computer readable storage medium | |
| US8019145B2 (en) | Legged locomotion robot | |
| JP6108818B2 (en) | Moving device and position recognition method of moving device | |
| WO2022042184A1 (en) | Method and apparatus for estimating position of tracking target, and unmanned aerial vehicle | |
| Miki et al. | Elevation mapping for locomotion and navigation using gpu | |
| CN103003031B (en) | Biped walking robot and method of determining the foot-landing timing for the biped walking robot | |
| Wang et al. | Drc-hubo walking on rough terrains | |
| CN107192375B (en) | A kind of unmanned plane multiple image adaptive location bearing calibration based on posture of taking photo by plane | |
| CN112596534A (en) | Gait training method and device for quadruped robot based on deep reinforcement learning, electronic equipment and medium | |
| WO2022000713A1 (en) | Augmented reality self-positioning method based on aviation assembly | |
| Belter et al. | Optimization-based legged odometry and sensor fusion for legged robot continuous localization | |
| Dai et al. | Hsc4d: Human-centered 4d scene capture in large-scale indoor-outdoor space using wearable imus and lidar | |
| CN116619382A (en) | Robot motion control method and system and electronic equipment | |
| CN111489392A (en) | Single target human motion posture capturing method and system in multi-person environment | |
| CN106959051A (en) | The automatized calibration method of weapon-aiming system based on spatial perception location technology | |
| WO2024037155A1 (en) | State estimation method and apparatus for legged robot, and computer device and computer-readable storage medium | |
| Kubelka et al. | Improving multimodal data fusion for mobile robots by trajectory smoothing | |
| CN113610932A (en) | Binocular camera external parameter calibration method and device | |
| CN111844013B (en) | Robot gait planning method and device, robot and storage medium | |
| CN113379850B (en) | Mobile robot control method, device, mobile robot and storage medium | |
| CN110502857B (en) | Terrain roughness online estimation method for quadruped robot | |
| CN114046800A (en) | High-precision mileage estimation method based on double-layer filtering framework | |
| CN112074873A (en) | Information processing device, information processing method, program, and floor modeling system | |
| Goulding | Biologically-inspired image-based sensor fusion approach to compensate gyro sensor drift in mobile robot systems that balance |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |