CN115639823A - Terrain sensing and movement control method and system for robot under rugged and undulating terrain - Google Patents
Terrain sensing and movement control method and system for robot under rugged and undulating terrain Download PDFInfo
- Publication number
- CN115639823A CN115639823A CN202211329125.XA CN202211329125A CN115639823A CN 115639823 A CN115639823 A CN 115639823A CN 202211329125 A CN202211329125 A CN 202211329125A CN 115639823 A CN115639823 A CN 115639823A
- Authority
- CN
- China
- Prior art keywords
- robot
- terrain
- map
- information
- path
- 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.)
- Pending
Links
Images
Abstract
The invention discloses a terrain sensing and movement control method and system for a robot under rugged and undulating terrain, which relate to the technical field of robots, and comprise the steps of obtaining image feature points, carrying out solution optimization, and establishing a point cloud map based on an optimization result; extracting the traversability information of the point cloud map to obtain a 2.5D cost map; generating a path with the minimum cost of barrier passability information in a 2.5D cost map based on an improved A-x algorithm, and performing global path planning and local path planning; and tracking the track along the local path by an MPC control algorithm with elevation constraint information to form a robot movement control scheme. The invention realizes the autonomous movement of the path planning and the track tracking control of the robot based on the 2.5D map; the method is suitable for large-scale complex environment, and can meet the requirements of intellectualization and practicability.
Description
Technical Field
The invention relates to the technical field of robots, in particular to a terrain sensing and movement control method and system for a robot under rugged and undulating terrain.
Background
In most indoor scenarios, mobile robots are provided with a map of the structured environment, which is divided into traversable and impenetrable cells, cells containing obstacles or walls are marked as non-traversable, and cells without obstacles are marked as traversable. The robot does not need to consider terrain attributes, and only needs to accurately identify obstacles. However, in outdoor scenarios, the mobile robot must traverse rough, rough and rugged natural terrain, the geometric and physical nature of which increases the complexity of the robot navigation problem. To reach the intended destination, the robot must have the ability to evaluate the terrain and determine the cost associated with traversing each terrain segment. One major challenge is to develop a perceptual framework to quickly and accurately distinguish between passable and non-passable regions.
Visual sensors, which are important components of robotic sensing systems, can provide a large amount of information about a scene. At present, the mainstream perception processing is mainly to establish a map through a laser radar, data acquired by the laser radar is sparse and disordered, color and texture information cannot be acquired, and the cost is too high to be applied independently. Among the types of vision sensors, binocular vision has the advantages of non-contact and passive, and is a key technology for a robot to acquire obstacle information and reconstruct a global scene.
The autonomous movement of the robot mainly comprises path planning and track tracking control after a map is established, the conventional ground robot autonomous movement map mainly surrounds 2-dimension and 3-dimension systems, the environmental information is not sufficiently expressed under a 2-dimension system, only passable areas and non-passable areas are distinguished, and the map is not suitable for various terrain expressions under a complex environment; although the 3-dimensional system expresses the surrounding environment in detail, the data volume is too large, the data processing process is too complex, the cost is too high, and the 3-dimensional system is not suitable for large-scale application.
At present, performances of outdoor mobile robots in all aspects are not limited to passable and impassable areas, the outdoor mobile robots can cross or cross irregular complex obstacles, different from a single structural environment, the cost required by the outdoor mobile robots to pass through different areas under complex rugged terrain is different, autonomous moving methods under a relative plane, including a path planning algorithm and a trajectory tracking method, cannot be suitable for autonomous movement of the robots under the rugged terrain, paths calculated by algorithms such as the past path planning and the like are not optimal paths, and the past navigation and trajectory tracking control methods cannot meet the development of obstacle-crossing mobile robots.
Disclosure of Invention
Aiming at the defects in the prior art, the invention aims to provide a terrain sensing and movement control method and a system of a robot under rugged and undulating terrain, and the autonomous movement of the robot is realized by path planning and track tracking control based on a 2.5D map; the method is suitable for large-scale complex environment, and can meet the requirements of intellectualization and practicability.
In order to achieve the purpose, the invention is realized by the following technical scheme:
in a first aspect, embodiments of the present invention provide a method for terrain awareness and movement control of a robot over rough and rough terrain, comprising:
acquiring image characteristic points, solving and optimizing the image characteristic points, and establishing a point cloud map based on an optimization result;
extracting the traversability information of the point cloud map to obtain a 2.5D cost map;
generating a path with the minimum cost and obstacle passability information in a 2.5D cost map based on an improved A-x algorithm, and performing global path planning and local path planning;
and tracking the track along the local path by an MPC control algorithm with elevation constraint information to form a robot movement control scheme.
As a further implementation, feature point extraction and matching are carried out on the images, a translation matrix and a rotation matrix are optimized by pure motion, and each frame of picture is positioned by minimizing a reprojection error;
and optimally managing the local map by solving the observation error and optimizing.
As a further implementation mode, the similarity between the images is calculated through loop detection, the feature points of each image are extracted for similarity scoring, and accumulated drift errors are eliminated through pose graph optimization;
after errors are eliminated in the pose graph optimization, the optimal structure and the motion result of the whole system are calculated based on a global BA algorithm, and pose information of the robot in a camera coordinate system and sparse point cloud information in the global are output.
As a further implementation mode, the point cloud is subjected to coordinate transformation to a map coordinate system, and filtering and down-sampling are performed; and obtaining corresponding point cloud global information according to the relative relation between the camera coordinate system and the map coordinate system.
As a further implementation mode, the terrain sunken below the horizontal plane is used as a negative obstacle, and whether the terrain can pass or not is judged according to the sunken depth and gradient of the terrain, so that the robot is controlled to move under the complex terrain;
carrying out voxel filtering on the three-dimensional dynamic point cloud map to obtain a three-dimensional octree map, and carrying out uniform sampling according to the size of voxels to discretize the point cloud set to generate a sampling point cloud set; and extracting local topographic features from the sampling point set, and projecting the results obtained by setting weights into a corresponding xy grid map as cost.
As a further implementation, the a-path planning algorithm under the 2.5D map with the terrain cost information is:
combining the performance of the mobile robot in a 2.5D cost map to generate a path with the minimum cost of barrier passability information, performing global path planning and local path planning, and generating a track to enable the robot to bypass or cross the barrier; and adding the terrain cost information into the path planning, and controlling the robot to judge whether to directly move a path which is relatively long in path but relatively flat in ground information through a rugged road or by bypassing the rugged road according to the cost.
As a further implementation, the MPC control algorithm with elevation constraint information includes:
acquiring the state of the current robot and a reference path generated by path planning, converting a map coordinate system into a robot coordinate system, taking the initial position of the robot as an origin, and taking the direction of the mobile robot as the direction of an x axis; establishing a matrix equation, decomposing and solving by using QR to obtain position parameters, and planning a tracked path by using polynomial fitting;
predicting a track according to the current state and the path, and dividing the track into N control points with interval time dt; output y at time t +1 after the processing t+1 Control quantity u generated at time t of the previous stage t Combining the two to be input as a new state quantity to form a prediction equation to obtain a prediction output y t+2 ；
From the first control point, the process is repeated recursively from the first step until y is reached t+N And (4) ending, wherein N output results form a prediction step length, and N control quantities form a control step length.
In a second aspect, embodiments of the present invention further provide a terrain awareness and movement control system for a robot over rough and rough terrain, comprising:
the point cloud map building module is used for obtaining image characteristic points, carrying out solution optimization and building a point cloud map based on an optimization result;
the 2.5D cost map generation module is used for extracting the traversability information of the point cloud map and generating a 2.5D cost map;
the path planning module is used for generating a path with the minimum cost and obstacle passability information in the 2.5D cost map based on an improved A-x algorithm, and performing global path planning and local path planning;
and the robot movement control scheme generation module is used for tracking the track along the local path by an MPC control algorithm with elevation constraint information to form a robot movement control scheme.
In a third aspect, embodiments of the present invention also provide a terrain awareness and movement control device for a robot over rough and rough terrain, comprising a memory and a processor;
the memory is used for storing relevant program codes;
the processor is used for calling the program code and executing the robot terrain perception and movement control method.
In a fourth aspect, an embodiment of the present invention further provides a computer-readable storage medium for storing a computer program, where the computer program is used to execute the robot terrain awareness and movement control method.
The invention has the following beneficial effects:
(1) The method is based on a 2.5D cost map, and realizes the autonomous movement of the robot in path planning and trajectory tracking control; the method is suitable for large-scale complex environment, and can meet the requirements of intellectualization and practicability.
(2) The negative obstacle is added in the mapping mode, namely the terrain sunken below the horizontal plane is used as the negative obstacle, whether the terrain can pass or not is judged according to the sunken depth and the gradient of the terrain, and then the robot is controlled to move under the complex terrain.
(3) According to the invention, on the basis of the traditional MPC, the cost of fusing the map information variation and the speed is increased, when the robot moves through the terrain with changed height, the map information variation becomes larger, and the map information variation and the speed fusion information are fixed in the constraint condition, so that the speed is reduced to buffer the variation of the map information, and when the robot passes through the road with changed height, the passing speed is reduced, and the robot moves more stably.
Drawings
The accompanying drawings, which are incorporated in and constitute a part of this specification, are included to provide a further understanding of the invention, and are incorporated in and constitute a part of this specification, illustrate exemplary embodiments of the invention and together with the description serve to explain the invention and not to limit the invention.
FIG. 1 is a navigation stack architecture diagram in accordance with one or more embodiments of the present invention;
FIG. 2 is a schematic diagram of a coordinate system variation in accordance with one or more embodiments of the invention;
FIG. 3 is a schematic diagram of an improved A-star algorithm path planning according to one or more embodiments of the present disclosure;
FIG. 4 is a schematic representation of a differential robot coordinate system in accordance with one or more embodiments of the present disclosure;
FIG. 5 is a flow diagram of a navigation process in accordance with one or more embodiments of the invention.
Detailed Description
The first embodiment is as follows:
the embodiment provides a terrain sensing and movement control method of a robot under rugged and undulating terrain, which comprises the following steps:
acquiring image characteristic points, solving and optimizing the image characteristic points, and establishing a point cloud map based on an optimization result;
extracting the traversability information of the point cloud map to obtain a 2.5D cost map;
generating a path with the minimum cost and barrier passability information in a 2.5D cost map based on an improved A-star algorithm, and performing global path planning and local path planning;
and tracking the track along the local path by an MPC control algorithm with elevation constraint information to form a robot movement control scheme.
In the embodiment, in an ROS (robot operating system), firstly, a binocular RGB (red, green and blue) camera is used for collecting image information, feature point transformation and camera pose information are resolved, a sparse point cloud map is established, point cloud information is transformed into a map coordinate system in real time, cost calculation is carried out on point cloud data features under the global coordinate system, and the point cloud data features are projected to a two-dimensional plane to obtain a global 2.5D cost map; the Navigation stack provided by the ROS is used for construction, an improved A star algorithm is used as a global planner, and a global path is generated in a 2.5D map with cost information.
In order to realize the rough and rough terrain path tracking by using the nonlinear MPC, a local planner part is realized as an MPC path tracker, and a local path is generated around a robot for tracking control, as shown in FIG. 1; by doing so, positioning (localization), mapping services, global planning, etc. provided by the existing Navigation stack can be directly used.
Specifically, as shown in fig. 5, the method includes the following steps:
step A: the images transmitted by the Realsense D435i camera are subjected to ORB feature point extraction and matching, and a translation matrix T of the camera is optimized by utilizing pure motion BA (Bundle Adjustment) c And a rotation matrix R c By minimizing reprojectionAnd (4) positioning the camera of each frame of picture by the error, and optimizing the camera pose and the positions of the extracted characteristic points in the picture by considering the positions of the optimization variables.
After the three-dimensional coordinate points are calculated to obtain pixel coordinates, local BA optimization is executed in order to reduce accumulated errors, the pose and the road signs are adjusted simultaneously by solving and optimizing observation errors, some main key frames and all visible points in the key frames are optimized, and a local map is managed and optimized.
Calculating the similarity between the images by loop detection, extracting the characteristic points of each image for similarity scoring, and taking a priori similarity s (v) t ,v t-Δt ) The similarity between the key point in the image at a certain moment and the key point in the image at the previous moment is shown, and the other points are normalized according to the score.
The step is carried out to detect a loop and eliminate accumulated drift errors through pose graph optimization.
After the error of the loop detection pose image is optimized and eliminated, the 4 th thread is started to execute the global BA algorithm, the optimal structure and the motion result of the whole system are calculated, and finally pose and sparse point cloud information of the robot in a camera coordinate system are output.
As shown in fig. 2, real-time TF (Transform Frame) coordinate transformation is performed, the point cloud is transformed into a map coordinate system, filtering and downsampling are performed, and the camera point cloud P is obtained according to the relative relationship between the camera coordinate system and the map coordinate system C To the map coordinate system P m The transformation of (a) may be expressed as:
P m ＝R×P C +T；
wherein R is a rotation matrix, T is a translation vector (dynamic coordinate transformation), and point cloud information in a camera coordinate system is converted into a map coordinate system through a formula, so that corresponding point cloud global information is obtained.
And B: and extracting the traversability information of the established point cloud map, and embedding the information and the elevation information into a state space to form a 2.5D cost map.
The traditional Octomap generates a three-dimensional octree map and a two-dimensional grid map, and the three-dimensional octree map and the two-dimensional grid map are combined, so that three-dimensional information is reserved, and the operability of the two-dimensional map is considered.
The embodiment also adds a negative obstacle to the traditional mapping mode, namely the terrain sunken below the horizontal plane is used as the negative obstacle, and whether the terrain can pass or not is judged according to the sunken depth and the gradient of the terrain, so that the robot is controlled to move under the rugged and undulating terrain.
The environment is represented by a point cloud consisting of n points, this point cloud set being referred to as the original set of points Q O Performing voxel filtering on the three-dimensional dynamic point cloud map to obtain a three-dimensional octree map (Octomap) according to the voxel size R S Performing uniform sampling to discretize the point cloud set to generate a sampled point cloud set Q S . We set Q from the sampling points S Local topographic features such as roughness, gradient and geometric relation with adjacent point sets are extracted, and the extracted local topographic features are used as a cost and are projected into a corresponding xy grid map with a certain weight. These features are used to derive a traversability metric for the map. For a set of sample points Q S The ith voxel unit of (1), selecting several evaluation factors to determine the cost value of this voxel unit:
z-axis mean mu of point clouds within voxels Z And is used for indicating the height of the obstacle and judging the type of the obstacle (positive/negative obstacle).
Z-axis variance D of all point clouds within a voxel Z For determining the roughness of the terrain.
Surface reconstruction is carried out on the point cloud in the voxel to calculate the normal vector N of the tangent line of the z axis of the surface z And calculating the surface gradient.
Taking the ith voxel unit as the center and five unit lengths as the radius, calculating the variance sigma of the average value mu of all the voxel units in the region on the z axis Z And determining the topographic relief complexity.
Then, each evaluation factor is linearly weighted to obtain a cost function:
J c ＝w μ μ z +w D D Z +w N N Z +w σ σ Z ；
wherein, w μ ,w D ,w N ,w σ Is an adjustable parameter that scales each evaluation factor according to the desired behavior. In the default setting, the value of each weight is 0.25. In practical applications, the most influential of these factors is μ Z If μ Z The ground clearance is larger than the ground clearance of the mobile robot, and the area is directly judged to be the region which can not pass; passing through mu Z The obstacle attribute can be judged to be a convex obstacle or a concave obstacle, and for the obstacles with different attributes, the influence of the other three evaluation factors on the robot movement is different, so that the follow-up robot complex movement processing is carried out.
The cost of each voxel unit is projected into a grid Map (Portable Gray Map) with Gray values of different color depths (0-100) to form a 2.5D cost Map. Cost per voxel unit
The concrete implementation formula mapped into the map is as follows:
wherein J M Data representing maximum cost threshold, filled in map i ∈[0,100]。
The map is updated and new measurements from the distance sensors are processed as points in space and mapped to a high-level map. This will produce a new height measurement p (assuming a cell is updated for each measurement point) on the cell (x, y) in the elevation map. In the map coordinate system, the height measurements are approximately Gaussian probability distributions of
Mean p and variance
Position r is given in the sensor coordinate system SP Can be converted into a corresponding height measurement p:
p＝P(Φ -1 (r SP )-r SM )；
the projection matrix P = [0 1] maps the three-dimensional measurement to a scalar height measurement P (under the map coordinate system).
And C: the pose of the current mobile robot is obtained through the fusion of a camera and an IMU, then a path with the minimum cost of barrier passability information is generated in a 2.5D cost map through an improved A-star algorithm in 2.5D and by combining the performance of the mobile robot, global path planning and local path planning are carried out, and a track is generated to enable the robot to bypass or cross the barrier.
Further, the improved a global path planning algorithm comprises the following steps:
the improved A-algorithm has the calculation formula as follows:
f(n)＝g(n)+h(n)+c(n)；
wherein f (n) is used as an evaluation function to evaluate the cost of each node to the target point; g (n) is the moving cost of moving from the initial state to the state n, and the cost required for moving n unit distances in the plane; h (n) is the estimated cost of the best path from state n to the target state, i.e., the heuristic function of the a-star algorithm.
For the heuristic function, if the map in grid form is only allowed to move in four directions, up, down, left, and right, manhattan distance may be used
If eight directions of movement are allowed in the grid-formed map, diagonal distances may be used, and if any directions of movement are allowed in the grid-formed map, euclidean distances may be used,
the method adopts Euclidean distance as heuristic function distance to express the advance distance between two nodes.
c (n) is the terrain cost of moving from the initial state to the state n under the 2.5D elevation map, information representing the terrain cost is arranged in each grid in the map, the terrain cost information is added into the path planning, the robot is controlled to judge whether to directly move on a rugged road surface or bypass the rugged road surface to select a path which is relatively long in path but relatively flat in ground information according to the cost, and c (n) depends on the size of the terrain trafficability information represented in each node.
And giving a global target and a pose according to the transmitted 2.5D elevation map, and acquiring current pose information through a visual odometer and an IMU (inertial measurement Unit). As shown in fig. 3, two empty tables, open _ set and Close _ set, are initialized, the algorithm first adds all passable nodes around the starting point into Open _ set, sets the cost of the starting point to 0, determines whether Open _ set is empty, if yes, path planning fails, ends the operation, if not empty, selects the minimum cost point n of all passable nodes around the node where the current is located from Open _ set, determines whether n is the end point, if yes, searches the preamble nodes in reverse direction, generates the optimal path, path planning succeeds, if not, deletes node n from Open _ set, adds Close _ set, then traverses all passable nodes around node n, if passable node m around node n is not in Close _ set, sets parent of m to n, calculates cost of m point with the above formula, adds m point into Open _ set, performs the above determination, and performs loop operation until finding the end point. In each search, the algorithm always selects the point with the minimum total cost f (n) in the current Open _ set for expansion, and repeats the process until a node sequence with the minimum cost, namely the optimal path plan, is generated.
Step D: since the local planner, including the MPC in this embodiment, moves across the obstacle without considering the problem of identifying and avoiding the obstacle, the global planner uses the above-mentioned modified a-x algorithm with elevation cost to reflect the map of the obstacle and update the generated path to the target location at frequent time intervals (1 second). And the local planner generates a local planning track on a path 5 meters in front of the robot according to the moving position of the robot in the path generated by the global planner, and calculates the acceleration and the steering speed of the robot. The input required by the actual robot control, namely the advancing speed v, is obtained by integrating the acceleration a, and finally the speed of each driving wheel is regulated by the motor controller by using a rotating speed formula.
First, a kinematic model of the mobile robot is established, as shown in fig. 4, in a global coordinate system, a motion equation of the differential wheeled robot is expressed as follows:
yaw rate ω = (v) of yaw angle R -v L ) W is the wheel track, and the rotation speed required for solving the robot driving motor according to the formula is omega R And ω L It can be calculated according to the formula:
keeping the path planning of the A star consistent with the previous path planning of the A star, and representing the Euclidean distance between the position of the robot on the two-dimensional coordinate and the path by the transverse track error
For application to the path tracking problem, a lateral trajectory error and a direction error are added to the state variables, the direction error measuring the difference between the advancing direction of the mobile robot and the tangential direction of the path.
The method comprises the steps of obtaining a state x of a current robot and a reference path generated by path planning, firstly, converting a coordinate system, converting a map coordinate system into a robot coordinate system, taking an initial position of the robot as an original point and a direction of moving the robot as an x-axis direction, then establishing a matrix equation, resolving to obtain position parameters by using QR, and planning a motion track of the robot by using cubic polynomial curve fitting.
And predicting the state at the next moment according to the state of the robot at the previous moment to realize the establishment of a model, wherein the discrete post model is as follows:
x t+1 ＝f(x t ,u t )；
using the state variable x = (x y θ v d η c) T Robot motion x modeled as a discrete system t+1 ＝f(x t ,u t ) The non-linear equation of (a) is as follows:
x t+1 ＝x t +v t cos(θ)·dt；
y t+1 ＝y t +v t sin(θ)·dt；
θ t+1 ＝θ t +ω t ·dt；
v t+1 ＝v t +a t ·dt；
d t+1 ＝g(x t )-y t +v t sin(η)·dt；
c t+1 ＝Δdata[m x ×x t+1 +y t+1 ]×v t+1 。
dt is the time interval for discretization, g (x) t ) Is one with x t For input, a polynomial curve fitting function of the y coordinate is output,
represents a curve fitting function g (x) t ) D represents a lateral trajectory error, η direction error, d and η represent a distance error from the robot position to the predicted path and a direction error of the robot direction from a tangent of the predicted path, Δ data [ ]]Is the absolute value of the difference between the map information of the current state and the previous state, and c is the fusion quantity of the map information and the speed of the robot.
The MPC algorithm includes the following steps: 1. the method comprises the steps of obtaining a state x of a current robot and a reference path sent by a path planning algorithm, firstly, carrying out coordinate system transformation, converting a map coordinate system into a robot coordinate system, taking an initial position of the robot as an origin, and taking the direction of a moving robot as an x-axis direction 2, predicting a track according to the current state and the path, dividing the track into N control points, wherein the interval time is dt, and the total time T = N × dt.3. The control driver inputs the data of the first control point 4. After completion from the first control point, recursion proceeds again from the first step.
The optimization problem of MPC is represented by a cost function of formula and a constraint of formula:
wherein, the first and the second end of the pipe are connected with each other,
x t ＝x(t) (a)
the expression (a) takes the current state variable x (t) as an initial value of a motion prediction interval in the optimization process, the expression (b) indicates that the robot follows a motion equation in the movement interval, the expression (c) indicates a state constraint condition, and the expression (d) indicates the speed for moving the control robot and the limit range of each speed input.
The first part of the constraint condition is to make the error between the vehicle track and the reference path be minimum and make the speed of the vehicle be as close to the reference speed as possible, the difference between the speed and the reference speed, the error of the transverse track and the error of the direction are minimum, the second part is to make the vehicle run more smoothly by making the angular acceleration and the acceleration be as small as possible, the input size of each driver is reduced as small as possible, the third part is to make the robot move more smoothly and reduce the difference between the angular acceleration and the linear acceleration input by the two adjacent drivers as small as possible, and the fourth part is to control the mobile robot to pass stably when passing through the terrain rugged road surface, so that the fusion amount change of the map information and the robot speed information is reduced as small as possible, and when the map information changes, the speed is reduced.
TABLE 1MPC optimization weights
The optimized weights for MPC are set to table 1. Giving priority to path following performance, and placing priority on transverse track errors and direction errors; for fast driving, the weight is reflected on the forward speed error. To prevent sudden changes, the weighting of yaw acceleration and damping (jerk) is considered. Most importantly, the influence of the map on the mobile robot is considered to be reflected on the map speed fusion error.
The traditional MPC algorithm controls the mobile robot to track the track on a two-dimensional map plane, and does not consider the situations of ground fluctuation and need to cross obstacles, so that the robot cannot be controlled to move along a path stably and accurately in practical application; this embodiment has increased the cost that map information variation and speed fuse on the basis of traditional MPC, and map information variation can grow when the robot removes the topography through altitude variation, and map information variation is fixed unchangeable with speed fusion information in the constraint condition, so speed can reduce the variation that buffers the map information that falls for when the robot is through altitude variation road surface, reduces the speed of passing through and then makes the robot remove more steadily.
Example two:
the present embodiment provides a terrain awareness and movement control system for a robot over rough and rough terrain, comprising:
the point cloud map building module is used for obtaining image characteristic points, carrying out solution optimization and building a point cloud map based on an optimization result;
the 2.5D cost map generation module is used for extracting the traversability information of the point cloud map and generating a 2.5D cost map;
the route planning module is used for generating a route with the minimum cost and obstacle passability information in the 2.5D cost map based on an improved A-star algorithm, and performing global route planning and local route planning;
and the robot movement control scheme generation module is used for tracking the track along the local path by an MPC control algorithm with elevation constraint information to form a robot movement control scheme.
Example three:
the embodiment provides a robot terrain sensing and movement control device under rugged and rough terrain, which comprises a memory and a processor;
the memory is used for storing relevant program codes;
the processor is used for calling the program code and executing the robot terrain perception and movement control method of the first embodiment.
Example four:
the embodiment provides a computer-readable storage medium for storing a computer program for executing the terrain awareness and movement control method of the robot according to the first embodiment.
The above description is only a preferred embodiment of the present application and is not intended to limit the present application, and various modifications and changes may be made by those skilled in the art. Any modification, equivalent replacement, or improvement made within the spirit and principle of the present application shall be included in the protection scope of the present application.
Claims (10)
1. A terrain sensing and movement control method for a robot under rugged and undulating terrain is characterized by comprising the following steps:
acquiring image characteristic points, solving and optimizing the image characteristic points, and establishing a point cloud map based on an optimization result;
extracting the traversability information of the point cloud map to obtain a 2.5D cost map;
generating a path with the minimum cost of barrier passability information in a 2.5D cost map based on an improved A-x algorithm, and performing global path planning and local path planning;
and tracking the track along the local path by an MPC control algorithm with elevation constraint information to form a robot movement control scheme.
2. The terrain awareness and movement control method for the robot over the rugged topography as claimed in claim 1, wherein feature points are extracted and matched for the images, and each frame of image is positioned by minimizing the reprojection error by using pure motion to optimize a translation matrix and a rotation matrix;
and optimally managing the local map by solving the observation error and optimizing.
3. The terrain awareness and movement control method for the robot over the rugged rough terrain as claimed in claim 2, wherein the loop detection calculates similarity between images, extracts feature points of each image for similarity scoring, and eliminates accumulated drift errors through pose graph optimization;
after errors are eliminated in the pose graph optimization, the optimal structure and the motion result of the whole system are calculated based on a global BA algorithm, and pose information of the robot in a camera coordinate system and sparse point cloud information in the global are output.
4. The terrain awareness and movement control method for the robot over the rough and rough terrain as recited in claim 3, wherein the point cloud is coordinate transformed to a map coordinate system, filtered and down-sampled; and obtaining corresponding point cloud global information according to the relative relation between the camera coordinate system and the map coordinate system.
5. The terrain awareness and movement control method for the robot over the rough and rough terrain as claimed in claim 1, wherein the terrain sunken below the horizontal plane is used as a negative obstacle, and whether the terrain can pass or not is judged according to the sunken depth and gradient of the terrain, so as to control the robot to move under the complex terrain;
carrying out voxel filtering on the three-dimensional dynamic point cloud map to obtain a three-dimensional octree map, carrying out uniform sampling according to the size of a voxel to discretize the point cloud set, and generating a sampling point cloud set; and extracting local topographic features from the sampling point set, and projecting the results obtained by setting weights into a corresponding xy grid map as cost.
6. The method for terrain awareness and movement control of a robot over rough and rough terrain according to claim 1, wherein the a-path planning algorithm under a 2.5D map with terrain cost information is:
combining the performance of the mobile robot in a 2.5D cost map, generating a path with barrier passability information and minimum cost, performing global path planning and local path planning, and generating a track to enable the robot to bypass or cross the barrier; and adding the terrain cost information into the path planning, and controlling the robot to judge whether to directly move through a rugged road or bypass the rugged road according to the cost and select a path which is relatively long in path but relatively flat in ground information to move.
7. The method for terrain awareness and movement control of a robot over rough and rough terrain as claimed in claim 1, wherein the MPC control algorithm with elevation constraint information comprises:
acquiring the state of the current robot and a reference path generated by path planning, converting a map coordinate system into a robot coordinate system, taking the initial position of the robot as an origin, and taking the direction of the mobile robot as an x-axis direction; establishing a matrix equation, decomposing and solving by using QR to obtain position parameters, and planning a tracked path by using polynomial fitting;
predicting a track according to the current state and the path, and dividing the track into N control points with interval time dt; output y at time t +1 after the processing t+1 Control quantity u generated at time t of previous stage t Combining the two to be input as a new state quantity to form a prediction equation to obtain a prediction output y t+2 ；
From the first control pointThen recursion is performed again from the first step until y is reached t+N And (4) ending, wherein N output results form a prediction step length, and N control quantities form a control step length.
8. Robotic terrain awareness and movement control system under rugged and undulating terrain, comprising:
the point cloud map building module is used for obtaining image characteristic points, carrying out solution optimization and building a point cloud map based on an optimization result;
the 2.5D cost map generation module is used for extracting the traversability information of the point cloud map and generating a 2.5D cost map;
the path planning module is used for generating a path with the minimum cost and obstacle passability information in the 2.5D cost map based on an improved A-x algorithm, and performing global path planning and local path planning;
and the robot movement control scheme generation module is used for tracking the track along the local path through an MPC control algorithm with elevation constraint information to form a robot movement control scheme.
9. A robot terrain awareness and movement control device under rough and rough terrain, comprising a memory and a processor;
the memory is used for storing relevant program codes;
the processor is used for calling the program code and executing the robot terrain awareness and movement control method according to any one of claims 1-7.
10. A computer-readable storage medium for storing a computer program for executing the method of terrain awareness and movement control of a robot of any of claims 1-8.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211329125.XA CN115639823A (en) | 2022-10-27 | 2022-10-27 | Terrain sensing and movement control method and system for robot under rugged and undulating terrain |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211329125.XA CN115639823A (en) | 2022-10-27 | 2022-10-27 | Terrain sensing and movement control method and system for robot under rugged and undulating terrain |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN115639823A true CN115639823A (en) | 2023-01-24 |
Family
ID=84947532
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202211329125.XA Pending CN115639823A (en) | 2022-10-27 | 2022-10-27 | Terrain sensing and movement control method and system for robot under rugged and undulating terrain |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115639823A (en) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116147642A (en) * | 2023-04-12 | 2023-05-23 | 中国科学技术大学 | Terrain and force integrated four-foot robot accessibility map construction method and system |
| CN116339336A (en) * | 2023-03-29 | 2023-06-27 | 北京信息科技大学 | Electric agricultural machinery cluster collaborative operation method, device and system |
-
2022
- 2022-10-27 CN CN202211329125.XA patent/CN115639823A/en active Pending
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116339336A (en) * | 2023-03-29 | 2023-06-27 | 北京信息科技大学 | Electric agricultural machinery cluster collaborative operation method, device and system |
| CN116147642A (en) * | 2023-04-12 | 2023-05-23 | 中国科学技术大学 | Terrain and force integrated four-foot robot accessibility map construction method and system |
| CN116147642B (en) * | 2023-04-12 | 2023-08-29 | 中国科学技术大学 | Terrain and force integrated four-foot robot accessibility map construction method and system |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112859859B (en) | Dynamic grid map updating method based on three-dimensional obstacle object pixel object mapping | |
| JP6644742B2 (en) | Algorithms and infrastructure for robust and efficient vehicle positioning | |
| Wolf et al. | Autonomous terrain mapping and classification using hidden markov models | |
| WO2018126228A1 (en) | Sign and lane creation for high definition maps used for autonomous vehicles | |
| CN111429574A (en) | Mobile robot positioning method and system based on three-dimensional point cloud and vision fusion | |
| JP6855524B2 (en) | Unsupervised learning of metric representations from slow features | |
| CN112639502A (en) | Robot pose estimation | |
| CN115639823A (en) | Terrain sensing and movement control method and system for robot under rugged and undulating terrain | |
| CN113110455B (en) | Multi-robot collaborative exploration method, device and system for unknown initial state | |
| CN110986945B (en) | Local navigation method and system based on semantic altitude map | |
| Liu et al. | Real-time 6d lidar slam in large scale natural terrains for ugv | |
| CN112762957A (en) | Multi-sensor fusion-based environment modeling and path planning method | |
| Li et al. | Hybrid filtering framework based robust localization for industrial vehicles | |
| Farag | Real-time autonomous vehicle localization based on particle and unscented kalman filters | |
| Pang et al. | Low-cost and high-accuracy LiDAR SLAM for large outdoor scenarios | |
| CN113686347A (en) | Method and device for generating robot navigation path | |
| Guo et al. | Occupancy grid based urban localization using weighted point cloud | |
| CN112907625A (en) | Target following method and system applied to four-footed bionic robot | |
| Li et al. | An overview on sensor map based localization for automated driving | |
| Devy et al. | On autonomous navigation in a natural environment | |
| Song et al. | Critical rays self-adaptive particle filtering SLAM | |
| Deusch et al. | Improving localization in digital maps with grid maps | |
| Marks et al. | Gamma‐SLAM: Visual SLAM in unstructured environments using variance grid maps | |
| Rangan et al. | Improved localization using visual features and maps for Autonomous Cars | |
| Perla et al. | Implementation of Autonomous Cars using Machine Learning |
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 |