CN114527753A

CN114527753A - Man-machine integrated building path planning method, computer device and program product

Info

Publication number: CN114527753A
Application number: CN202210098967.2A
Authority: CN
Inventors: 余伶俐; 罗嘉威; 赵于前; 周开军
Original assignee: Central South University
Current assignee: Central South University
Priority date: 2022-01-25
Filing date: 2022-01-25
Publication date: 2022-05-24

Abstract

The invention discloses a man-machine co-integrated building path planning method, a computer device and a program product.A three-dimensional grid map is constructed through environment information acquired by an airborne laser radar, two-dimensional projection is carried out based on the difference of motion characteristics of pedestrians and robots, two-dimensional grid maps facing the pedestrians and the robots are respectively generated, and the global positioning information of the robots is obtained by adopting an AMCL algorithm according to the two-dimensional grid maps facing the robots; the pedestrian prediction module generates a global path with time information and capable of crossing over special environments such as short obstacles by adopting an A-star algorithm on a two-dimensional grid map facing pedestrians, and inputs the global path to the path planning module; the path planning module is divided into global planning and local planning, wherein the global planning generates a global path based on pedestrian prediction and space-time consistency constraint according to the obtained robot position information, target position information and pedestrian path information. The invention realizes the full intelligent navigation.

Description

Man-machine integrated building path planning method, computer device and program product

Technical Field

The invention relates to the field of path planning in buildings, in particular to a man-machine integrated path planning method between buildings, a computer device and a program product.

Background

Path planning in a building aiming at a man-machine co-fusion environment is one of important contents in the field of indoor robot navigation planning, and two-dimensional grid maps facing pedestrians and robots are respectively generated by establishing a three-dimensional grid map and performing two-dimensional projection based on the difference of the motion characteristics of the pedestrians and the robots. The pedestrian-oriented two-dimensional grid map adopts the A-star algorithm for introducing time information to realize the navigation technology that the pedestrian can cross the obstacle indoors, and plays a good navigation guiding role for the pedestrians which are unfamiliar with the large indoor environment. The indoor robot has a wide application prospect at present, and in consideration of man-machine safety in the driving process of the indoor robot, an A-x algorithm suitable for a man-machine co-fusion environment is adopted for a two-dimensional grid map facing the robot to generate a global path based on pedestrian prediction and space-time consistency constraint, so that the robot can predict the driving track of pedestrians in a period of time in advance and avoid the pedestrian in advance in the driving process. However, in the process of implementing the a-x algorithm, the time predicted by the pedestrian is an estimated value, and has a certain difference with the actual running time of the pedestrian, so that the functions of dynamic pedestrian trajectory prediction and obstacle avoidance need to be implemented by adopting a local planning algorithm based on background cancellation and DWA; the background cancellation method can identify static obstacles and pedestrians in the environment through a series of coordinate transformation according to laser radar point cloud data between two adjacent frames, track prediction is carried out on the pedestrians, then obstacle avoidance is carried out on the pedestrians in advance by combining a DWA local planning algorithm, so that the driving speed, the direction and the like of the pedestrians are not influenced, and the safety in the driving process is improved. The cross-floor navigation in the invention can realize that the autonomous transport robot moves from the starting floor to the target floor without artificial interference by combining the A-star algorithm and the elevator dispatching module, and has higher intelligent degree. However, in an actual scene, the following problems still restrict the application of the robot within a building scene.

(1) Aiming at the problem that pedestrians are unfamiliar in large indoor scenes, a global path suitable for pedestrian reference needs to be designed; considering the difference of the pedestrian motion characteristics and the robot: pedestrians can cross short obstacles, pedestrians can pass through gate channels in environments such as airports and the like, a two-dimensional grid map facing the pedestrians is required to be generated according to the motion characteristics of the pedestrians, a global planning algorithm is adopted based on the map to generate a global pedestrian path, and the pedestrians can cross the short obstacles, pass through the gate channels and the like when driving according to the global path.

(2) The next difficulty is how to generate a global path between buildings, which is constrained based on pedestrian prediction and space-time consistency and can cross floors, aiming at the problem that an indoor robot needs to travel across floors to realize navigation between different floors. The robot is transported to a target floor from an initial floor by combining the robot and an elevator dispatching module to realize a fully intelligent elevator transportation technology, and then navigates to a target floor to a target point. The multi-layer two-dimensional grid map has high expandability, less occupation of program running resources and faster running time.

(3) Aiming at the problem of obstacle avoidance of an indoor robot in the navigation process, the invention provides a pedestrian track prediction and dynamic obstacle avoidance method based on background cancellation and DWA local planning algorithm. According to the method, the resolution of static obstacles and pedestrians can be realized through the point cloud data of the laser radar between two adjacent frames, meanwhile, the track of the pedestrians is predicted, and then the pedestrians are avoided in advance by combining with a DWA local planning algorithm, so that the people and the safety in the driving process are greatly improved.

The invention patent application, namely an omnidirectional mobile robot autonomous navigation system based on a VFH local path planning method (application number: 202010043476.9), discloses an omnidirectional mobile robot autonomous navigation system based on the VFH local path planning method, which constructs a grid map through a laser radar and introduces the idea of an A algorithm into a VFH obstacle avoidance method for improvement, so that a mobile robot can select the obstacle avoidance direction as excellent as possible. However, the method is only limited to autonomous navigation of the robot without considering pedestrian prediction under a two-dimensional plane, and has great hidden danger in driving under a man-machine co-fusion environment; meanwhile, the local track generated by the method has discontinuity, so that the tracking and driving of the robot are not suitable.

The invention patent application 'an indoor mobile robot man-machine co-fusion navigation device and method' (application number: 201910588551.7) discloses an indoor mobile robot man-machine co-fusion navigation device and method, which only realize indoor single-floor navigation and only carry out simple track prediction for pedestrians.

A human-ware navigation method is provided by a Master academic thesis 'multi-layer cost map-based mobile robot man-machine co-fusion navigation technology research', and the method can improve the performance of path planning, improve the comfort of human beings and obey certain social rules. Meanwhile, although a multi-layer cost map is established, the multi-layer cost map is only used for navigation and pedestrian prediction of a single floor.

Disclosure of Invention

The invention aims to solve the technical problem that the prior art is insufficient, provides a man-machine integrated building path planning method, a computer device and a program product, and improves man-machine safety when a robot in a man-machine integrated environment is oriented to driving.

In order to solve the technical problems, the technical scheme adopted by the invention is as follows: a man-machine integrated building road path planning method comprises the following steps:

s1, when the robot with the laser radar is driven between buildings, collecting laser radar environment information in the driving process, constructing a three-dimensional grid map, performing two-dimensional projection based on the difference of the motion characteristics of pedestrians and the robot, and generating a two-dimensional grid map facing pedestrians and a two-dimensional grid map facing the robot;

s2, generating a pedestrian-oriented global path with time-space information by adopting an A-algorithm introducing time information to the two-dimensional grid map facing to the pedestrian;

s3, acquiring the global position of the robot, and generating a global path facing the robot based on pedestrian prediction and space-time consistency constraint by adopting an A-star algorithm to the two-dimensional grid map facing the robot according to the global path facing the pedestrian;

and S4, according to the environmental information collected by the laser radar, dynamically predicting the pedestrian track based on a background cancellation method, and dynamically avoiding obstacles by adopting a DWA local planning algorithm according to the predicted track and the global path facing the robot.

The invention discloses a man-machine integrated building path planning method, a computer device and a program product, wherein a step S1 is used for carrying out two-dimensional projection based on the difference of the motion characteristics of pedestrians and robots to generate a two-dimensional grid map facing to pedestrians and a two-dimensional grid map facing to robots; the pedestrian-oriented two-dimensional grid map sets short obstacles which can be crossed by pedestrians as idle grids, and the global paths which are generated in the subsequent step S2, have temporal-spatial information and are oriented to the pedestrians, may pass through the short obstacles, but all belong to pedestrian passable areas; however, for the robot, even a short obstacle cannot cross the area, so that the robot needs to avoid the area because the part is the obstacle grid in the two-dimensional grid map facing the robot. Meanwhile, in the step S3, not only global position information and target position information of the robot are required, but also global path information which is generated in the step S2, has time-space information and faces to pedestrians, an A-star algorithm is adopted to generate a global path which is based on pedestrian prediction and space-time consistency constraint and faces to the robot, and the path realizes the function of avoiding in advance through prediction of the running track of the pedestrians; however, the uncertainty of the running speed of the pedestrian is considered, so that the reliability of the global path generated in the step S3 is not sufficient, and in order to guarantee the man-machine safety in the running process, the invention carries out dynamic pedestrian trajectory prediction based on a background cancellation method, and adopts a DWA local planning algorithm to carry out dynamic obstacle avoidance, so that the man-machine safety in the running process of the robot is improved.

The specific implementation process of step S1 includes:

s1.1, obtaining a three-dimensional point cloud map by adopting a point cloud registration algorithm for point cloud data detected by a laser radar;

s1.2, filtering the three-dimensional point cloud map, and converting the filtered three-dimensional point cloud map into a three-dimensional grid map with resolution ratio resolution;

s1.3, setting a height range h_{pedestrianmin}≤z_gridoctreeWherein h is_{pedestrianmin}Projecting the three-dimensional grid map to the xoy plane on the Z axis for the maximum height of the pedestrian capable of crossing the obstacle to obtain a two-dimensional grid map facing the pedestrian; if the horizontal and vertical coordinates in the three-dimensional grid map are (x)_gridoctree,y_gridoctree) Z axis satisfies h_{pedestrianmin}≤z_gridoctreeIn the range of (2), if there is no three-dimensional grid, then two-dimensional grid (x)_gridoctree,y_gridoctree) Is a free grid; if a three-dimensional grid is present, the two-dimensional grid is an obstacle grid, where (x)_gridoctree,y_gridoctree,z_gridoctree) Coordinates of a central point of the three-dimensional grid are obtained;

s1.4, setting the height range z_gridrobot≤h_robotmaxWherein h is_robotmaxProjecting the three-dimensional grid map into the xoy plane on the Z axis for the maximum height of the robot to obtain a two-dimensional grid map facing the robot; if the horizontal and vertical coordinates in the three-dimensional grid map are (x)_gridrobot,y_gridrobot) Z axis satisfies 0. ltoreq. Z_gridrobot≤h_robotmaxIn the range of (2), if there is no three-dimensional grid, then two-dimensional grid (x)_gridrobot,y_gridrobot) Is a free grid; if a three-dimensional grid is present, the two-dimensional grid is an obstacle grid, where (x)_gridrobot,y_gridrobot,z_gridrobot) Coordinates of a central point of the three-dimensional grid are obtained;

and S1.5, returning to the step S1.1, constructing three-dimensional grid maps of other floors, performing two-dimensional projection based on the difference of the motion characteristics of the pedestrians and the robot, and generating a two-dimensional grid map facing the pedestrians and a two-dimensional grid map facing the robot.

In consideration of the difference of the running characteristics of the pedestrians and the robot, a two-dimensional grid map which is suitable for both the pedestrians and the robot navigation cannot be generated simultaneously through one two-dimensional grid map.

The specific implementation process of step S2 includes:

s2.1, inputting target goal_pedestrian(x_{pedestriangoal},y_{pedestriangoal},level_{pedestriangoal}) The current position start of the pedestrian_pedestrian(x_{pedestrianstart},y_{pedestrianstart},level_{pedestrianstart}) Wherein x is_{pedestriangoal}And y_{pedestriangoal}Is a two-dimensional coordinate, level, of the target point in the xoy plane_{pedestriangoal}Is the floor of the target, x_{pedestrianstart}And y_{pedestrianstart}Is a two-dimensional coordinate, level, of the pedestrian in the xoy plane_{pedestrianstart}The floor where the pedestrian is currently located;

s2.2, judging whether level is met_{pedestriangoal}＝level_{pedestrianstart}(ii) a If yes, indicating that the target point and the pedestrian are currently on the same floor, and turning to the step S2.3; if not, the target point and the pedestrian are currently positioned at different floors, and the step S2.4 is carried out;

s2.3, if the target point and the pedestrian are located on the same floor, generating a global path with time-space information and facing the pedestrian by adopting an A-algorithm introducing time information, wherein the starting point is (x)_{pedestrianstart},y_{pedestrianstart}) The target point is (x)_{pedestriangoal},y_{pedestriangoal})；

S2.4, if the target point and the pedestrian are currently positioned at different floors, generating a level by adopting an A-star global path planning algorithm introducing time information_{pedestrianstart}Of a layerGlobal path, where the starting point is (x)_{pedestrianstart},y_{pedestrianstart}) The target point is the level_{pedestrianstart}Position of elevator on floor_elevator(x_elevator,y_elevator) (ii) a When the pedestrian takes the elevator to go to the target floor level_goalWhen the two-dimensional grid map is updated to the level_goalA two-dimensional grid map of layers; generate the level_{pedestriangoal}Global path of floor with the starting point being the level_{pedestriangoal}Position of floor elevator_elevator(x_elevator,y_elevator) The target point is (x)_{pedestriangoal},y_{pedestriangoal})。

According to the pedestrian-oriented two-dimensional grid map, the global path which accords with the motion characteristics of the pedestrian is generated by adopting an A-global path planning method introducing time information to the two-dimensional grid map for the pedestrian, the pedestrian possibly encounters an obstacle when running according to the guidance of the path, but the pedestrian belongs to the classes of low and short obstacles which can be crossed by the pedestrian and the like, and the pedestrian can be regarded as a driving area for the flexible pedestrian.

The specific implementation process for generating the pedestrian-oriented global path with the time-space information by adopting the A-algorithm introducing the time information comprises the following steps:

1) reception start (x)_start,y_start) Goal Point, goal (x)_goal,y_goal) Information defining two empty sets Open and Close;

2) calculate the Start (x)_start,y_start) Heuristic value of F_startAnd its arrival at the start (x) of origin_start,y_start) At time t_startStore the set Open { (x)_start,y_start,F_start,t _start0,0) }; heuristic value F_startThe calculation formula of (2) is as follows:

3) judging whether the set Open is an empty set, if not, turning to the step 4); if so, finishing the global planning, and not finding a feasible driving path which has time-space information and faces to the pedestrian from the starting point to the target point;

4) selecting the grid point with the minimum heuristic value from the set Open as the grid point grid of the next step_n(x_n,y_n) Acquiring its time information t_nGrid the grid points_nMove from set Open into set Close;

5) judging grid points grid_nWhether the target point is goal; if yes, grid is determined according to grid points_nThe father nodes are traced back in sequence to find a feasible driving path which has time-space information and faces to the pedestrian from the starting point to the target point; if not, turning to the step 6);

6) finding grid points grid_nNon-obstacle grids in eight neighborhood orientations and not in aggregate Close, denoted as near reachable grids grid_nc(x_nc,y_nc) Calculating the near reachable grid_nc(x_nc,y_nc) Heuristic value of F_ncAnd time t of arrival at the nearby reachable grid_nc＝t_n+t_fSetting a parent node adjacent to the reachable grid as grid_nStore the set Open { (x)_start,y_start,F_start,t_start,0,0)、…、(x_nc,y_nc,F_nc,t_nc,x_n,y_n) In (1) }; wherein t is_fTo grid from grid point_nTo grid_ncPredicted time consumption of (2); heuristic value F_ncThe calculation formula of (2) is as follows:

in which I_NFor expansion costs, the grid can be reached if it is close_ncIs a free grid, I_N0; reachable grid if close_ncIn order to expand the grid, the grid is expanded,

wherein p is₁、p₂For a given parameter, distance _ obstacle is the distance of the expanded grid from the nearest barrier grid; wherein the non-obstacle grid comprisesA free grid and an expansion grid; the determination process of the free grid and the expansion grid comprises the following steps: marking the elevator Position of each floor in the two-dimensional grid map as Position according to the generated two-dimensional grid map facing the pedestrian_elevator(x_elevator,y_elevator) Marking the elevator door barrier grid as an idle grid in a two-dimensional grid map; performing obstacle expansion operation on the two-dimensional grid map, namely taking the obstacle grid as an expansion center and the expansion radius as radius_{pedestrianinflation}If the expansion area of a certain barrier grid is less than half of the area of the barrier grid, the barrier grid is an idle grid, otherwise, the barrier grid is an expansion grid;

7) and skipping to the step 3) until the global planning is finished.

The global path generated by the global planning algorithm comprises the predicted pedestrian driving time, so that the A-star algorithm which is subsequently applicable to the man-machine co-fusion environment can be conveniently realized.

The specific implementation process of step S3 includes:

s3.1, the robot obtains the position (x) of the robot in the xoy plane through AMCL positioning_robotstart,y_robotstart) The floor where the robot is currently located is level_robotstart；

S3.2, inputting a robot navigation target goal_robot(x_robotgoal,y_robotgoal,level_robotgoal) Wherein x is_robotgoalAnd y_robotgoalIs a two-dimensional coordinate, level, of the target point in the xoy plane_robotgoalIs the floor where the target is located;

s3.3, judging whether level is met_robotgoal＝level_robotstart(ii) a If yes, the target point and the robot are located on the same floor currently, and the step S3.4 is carried out; if not, the target point and the robot are currently positioned on different floors, and the step S3.5 is carried out;

s3.4, if the target point and the robot are located on the same floor, generating a global path facing the robot based on pedestrian prediction and space-time consistency constraint by adopting an A-star algorithm, wherein the starting point is (x)_robotstart,y_robotstart) The target point is (x)_robotgoal,y_robotgoal)；

S3.5, if the target point and the robot are currently located on different floors, performing inter-floor navigation by combining an A-algorithm and an elevator dispatching module, and specifically comprising the following steps:

1) firstly, the first level is generated by adopting A-algorithm_robotstartGlobal path of layers, where the starting point is (x)_robotstart,y_robotstart) The target point is the level_robotstartPosition of elevator on floor_elevator(x_elevator,y_elevator)；

2) Calculating the time t consumed by the robot to reach the target point according to the global path generated in the step 1)_rs，

Wherein s is_robotglobalGlobal path length v for arriving at the elevator from the current position of the robot_robotassignFor a given speed;

3) and the robot acquires the level of the floor where the elevator is located currently_elecurrentCalculating the level of the floor where the elevator is located_elecurrentReach the floor level of the robot_robotstartTime t of_es，

Wherein s is_{elevatorstart}For the level of the floor where the elevator is located_elecurrentThe distance from the floor level of the robot_robotstartHeight of (v)_elevatorThe running speed of the elevator can be measured in advance and is a fixed value;

4) judging whether s is satisfied_robotglobal≤s_thresoldWherein s is_thresoldA given distance threshold; if yes, turning to step 5); if not, the robot continues to run, and meanwhile, the time t consumed when the robot reaches the target point is updated_rsAnd returning to the step 3);

5) judging whether | t is satisfied_rs-t_es|≤t_thresoldWherein t is_thresoldIs a given time threshold; if yes, the elevator reaches the floor level of the robot after the robot reaches the target point_robotstartIssuing an elevator calling instruction to an elevator dispatching module; if not, indicating that the robot delays in the running process due to the existence of the obstacle, and sending an elevator calling command after reaching the target point;

6) after the elevator reaches the floor where the robot is located, opening the door of the elevator; meanwhile, the robot judges whether a pedestrian enters or exits the elevator, if so, the robot retreats for a certain distance, and the robot enters the elevator after the pedestrian enters or exits the elevator; if no pedestrian enters or exits the elevator, the robot directly enters the elevator;

7) after the robot enters the elevator, an instruction of successfully entering the elevator is issued to the elevator dispatching module, after the elevator dispatching module receives the instruction of successfully entering the elevator issued by the robot, the elevator dispatching module issues an instruction of closing the elevator door, and the elevator goes to the target floor level_robotgoalSimultaneously, issuing an instruction to the robot to switch the two-dimensional grid map;

8) after the elevator reaches the target floor, the elevator dispatching module issues an instruction to open the door of the elevator, and simultaneously issues an instruction to start the target floor navigation for the robot;

9) after the robot receives a target floor arrival instruction from the elevator dispatching module, the robot starts target floor navigation and generates a level by adopting an A-star algorithm_robotgoalGlobal path of layer, where the starting point is the level_robotgoalPosition of floor elevator_elevator(x_elevator,y_elevator) The target point is (x)_robotgoal,y_robotgoal)。

The invention adopts an A-algorithm applicable to a man-machine co-fusion environment to a two-dimensional grid map facing a robot to generate a global path based on pedestrian prediction and space-time consistency constraint, wherein the path can predict the running track of a pedestrian in a certain period in advance and avoid the pedestrian in advance without influencing the running speed, direction and the like of the pedestrian; meanwhile, in order to realize the cross-floor navigation of the robot, the elevator dispatching module is combined with the A-star algorithm, so that the autonomous transport robot can move from the starting floor to the target floor without human interference, and the intelligent degree is higher.

The specific implementation process for generating a robot-oriented global path based on pedestrian prediction and space-time consistency constraint by adopting an A-star algorithm comprises the following steps:

A) receive target goal_robot(x_robotgoal,y_robotgoal) Two-dimensional information, pedestrian-oriented global path reference points, and two empty sets Open_rAnd Close_r；

B) Calculating a robot starting point start_robot(x_robotstart,y_robotstart) Heuristic value of F_robotstartAnd its arrival at the start of arrival_robotTime t of_robotstartTo store into the set Open_r＝{(x_robotstart,y_robotstart,F_robotstart,t _robotstart0,0) }; wherein the heuristic value F_startThe calculation formula of (a) is as follows:

C) determine set Open_rWhether the collection is an empty collection or not, if not, turning to the step D); if so, finishing the global planning, and finding no robot-oriented global path from the starting point to the target point based on the pedestrian prediction and the space-time consistency constraint;

D) from the set Open_rSelecting the grid point with the minimum heuristic value as the grid point grid of the next step_rn(x_rn,y_rn) Get grid_rn(x_rn,y_rn) Time information t of_rnGrid points grid_rnFrom the set Open_rMove into aggregate Close_rPerforming the following steps;

E) judging grid points grid_rnWhether it is the target goal_robot(ii) a If yes, grid point grid is determined according to grid points_rnThe father nodes are traced back in sequence, and a global path facing the robot from a starting point to a target point based on pedestrian prediction and space-time consistency constraint is found; if not, go to step F) The preparation method comprises the following steps of (1) performing;

F) grid point by grid_rnTime information t of_rnGet from t_rnT after the time_simAnd marking the two-dimensional information of the global path reference Point of the pedestrian in seconds as Point_pedestrian(x_pedestrian,y_pedestrian) Wherein t is_simFor a preset time, marking the grid occupied by the global pedestrian path reference point as an obstacle grid, and simultaneously performing obstacle expansion, wherein the expansion center is the obstacle grid, and the expansion radius is radius_pedestrian；

G) Finding grid points grid_rnEight neighborhood orientations and not in aggregate Close_rNon-obstacle grid in (1), marked as a near reachable grid_rnc(x_rnc,y_rnc) Calculating a heuristic value F of the near reachable grid_rncAnd time t of arrival at the nearby reachable grid_rnc＝t_rn+t_rfSetting a parent node adjacent to the reachable grid as grid_rnTo store into the set Open_r＝{(x_robotstart,y_robotstart,F_robotstart,t_robotstart,0,0)、…、(x_rnc,y_rnc,F_rnc,t_rnc,x_rn,y_rn) In (1) }; wherein t is_rfTo grid from grid point_rnTo grid_rncPredicted time consumption of (2); wherein the heuristic value F_rncThe calculation formula of (a) is as follows:

wherein I_rNFor expansion costs, the grid can be reached if it is close_rncIs a free grid, I_rN0; reachable grid if close_rncIn order to expand the grid, the grid is expanded,

wherein p is_r1、p_r2For a given parameter, distance _ obstacle _ r is the distance of the expanded grid from the nearest barrier grid; wherein the non-obstacle grid includes a free grid and an expanded grid; the determination process of the free grid and the expansion grid comprises the following steps: in the robotThe elevator Position marked out each floor in the dimension grid map is marked as Position_elevator(x_elevator,y_elevator) Converting the elevator door barrier grid into an idle grid in a two-dimensional grid map; performing obstacle expansion on the two-dimensional grid map, namely taking the obstacle grid as an expansion center and radius of expansion_{robotinflation}If the expansion area of a certain barrier grid is less than half of the area of the barrier grid, the barrier grid is an idle grid, otherwise, the barrier grid is an expansion grid;

H) and C) jumping to the step C) until the global planning is finished.

Since the robot needs to predict the running track of the pedestrian in advance and avoid the pedestrian, the running path and the time information of the pedestrian need to be known in advance and corresponding processing is carried out, and the running track of the pedestrian in a certain period of time is predicted and is taken as an obstacle to carry out avoidance processing.

The specific implementation process of step S4 includes:

i) defining a local cost map of the robot as a length and a width respectively by taking the current position of the robot as a center_robotlocal、width_robotlocal；

ii) from the two-dimensional coordinates (x) of the dynamic pedestrian_rpc,t,y_rpc,t) And its running speed v_doHeading angle theta_doPredicting a dynamic pedestrian driving track:

wherein, t_predict＝1,2,…,t_dopredict，t_dopredictPredicting a time duration for the trajectory; dynamic pedestrian speed v_doAnd a heading angle theta_doThe calculation formula is as follows:

(x_rpc,t,y_rpc,t) As a clustering point pointcluster_rt(x) of (C)_{rkmeanstranspc,t-1},y_{rkmeanstranspc,t-1}) As a clustering point pointcluster_{rkmeanstrans,t-1}Coordinate of (1), Pointcloud_rt(x_rpc,t,y_rpc,t) As a cluster point Pointcloud_t(x_pc,t,y_pc,t) Clustering points within the local cost map; pointcloud_t(x_pc,t,y_pc,t) Two-dimensional coordinates of current frame point cloud data; pointcloud_{rkmeanstrans,t-1}(x_{rkmeanstranspc,t-1},y_{rkmeanstranspc,t-1}) As a clustering point pointcluster_{kmeanstrans,t-1}(x_{kmeanstranspc,t-1},y_{kmeanstranspc,t-1}) Clustering points, Pointcloud, within the local cost map_{kmeanstrans,t-1}(x_{kmeanstranspc,t-1},y_{kmeanstranspc,t-1}) As a pair of clustering points Pointclosed_kmeans,t-1(x_kmeanspc,t-1,y_kmeanspc,t-1) Carrying out coordinate conversion to obtain clustering points; pointcloud_kmeans,t-1(x_kmeanspc,t-1,y_kmeanspc,t-1) For the two-dimensional coordinate Pointcloud of the last frame point cloud data_t-1(x_pc,t-1,y_pc,t-1) Carrying out K-means clustering to obtain clustering points;

iii) to predict the resulting pedestrian trajectory (x)_{pedestrianpredict},y_{pedestrianpredict}) As a circle center with radius of radius_{pedestrianpredict}Performing obstacle inflation; traversing all obstacles in the local cost map and calculating a track traj_robotDistance between the last track point and the nearest barrier in the local cost map_{localtoobstacle}As a collision cost; and judging whether the distance is satisfied_{localtoobstacle}≥2×radius_{robotinflation}If yes, distance_{localtoobstacle}＝2×radius_{robotinflation}；

Wherein radius_{robotinflation}Expansion radius for obstacle expansion for two-dimensional grid maps, (x)_robot,y_robot) Position information of the robot at the current moment;

iv) selecting the trajectory traj_robotTaking the absolute speed of the last trace point as the speed cost distance_vel；traj_robotFor traversing each speed v of the robot_robotAngular velocity ω_robotOne of the combined products is at t_robotsimA sequence of trace points in seconds;

v) order set

Wherein num_trajrobotDistance for the total number of all sampling traces_{localtoglobal,1}Tracking cost, distance, for the global path of the 1 st trace_{localtoobstacle,1}Distance for the collision cost of the 1 st trace_vel,1The velocity cost for the 1 st trace; distance_{localtoglobal,1}＝D-distance_{mintoglobal，1}；distance_{mintoglobal，1}The minimum distance from the last track point in the 1 st track to the global path is obtained; d is a given value; distance_vel，1Taking the absolute speed value of the last track point in the 1 st track as the speed cost;

vi) normalizing each type of data in the set Evaluate:

vii) calculating the total cost value of each sampling track, wherein the specific calculation formula is as follows: evaluate_traj,k＝α·normal_distance_{localtoglobal,k}+β·normal_distance_{localtoobstacle,k}+γ·normal_distance_vel,k(ii) a Wherein, alpha, beta and gamma are given weighted values;

viii) selecting the track with the maximum total cost value as the local obstacle avoidance track.

The global path generated by the A-x algorithm applicable to the man-machine co-fusion environment can estimate the running time of the pedestrian, but the running speed of the pedestrian is uncertain, so that the estimation of the running time of the pedestrian cannot be accurate, dynamic obstacle avoidance needs to be carried out according to the environment information detected by the laser radar, a background cancellation method is adopted, static obstacles and dynamic obstacles in the environment can be well identified through two frames of laser radar point cloud data, the running track of the dynamic obstacles is estimated, and the effects of track prediction and dynamic obstacle avoidance are achieved.

Calculating dynamic pedestrian travel speed v_doAnd a heading angle theta_doPreviously, the following determination was also made: to clustering point pointcluster_rt(x_rpc,t,y_rpc,t) And clustering point pointclosed_{rkmeanstrans,t-1}(x_{rkmeanstranspc,t-1},y_{rkmeanstranspc,t-1}) Performing difference processing if the difference meets the requirement

Then the coordinate is said to be (x)_rpc,t,y_rpc,t) The obstacle of (2) is a static obstacle, wherein range_thresoldIs a given threshold value; if not satisfied with

Then the coordinate is said to be (x)_rpc,t,y_rpc,t) The obstacle is a dynamic pedestrian, and the running speed v of the dynamic pedestrian is calculated_doAnd a course angle theta_do。

The dynamic pedestrian track prediction based on the background cancellation method can be used for distinguishing dynamic and static obstacles only by using the laser radar point cloud data of two adjacent frames, and meanwhile, the driving speed and the course angle of the dynamic pedestrian track are calculated, so that the dynamic pedestrian track prediction based on the background cancellation method occupies less calculation resources and is easier to realize.

The invention also provides a computer device, comprising a memory, a processor and a computer program stored on the memory; the processor executes the computer program to implement the steps of the method of the present invention.

The present invention also provides a computer program product comprising a computer program/instructions; which when executed by a processor implement the steps of the method of the present invention.

Compared with the prior art, the invention has the following beneficial effects:

1) the method comprises the steps of firstly establishing a three-dimensional grid map according to environmental information acquired by a laser radar in the driving process of the robot, and carrying out two-dimensional projection based on the difference of the motion characteristics of pedestrians and the robot to generate a two-dimensional grid map facing the pedestrians and the robot. And adopting different two-dimensional grid maps according to different path requirements to generate a global path according with the motion characteristics of the pedestrian/robot. The pedestrian-oriented two-dimensional grid map adopts an A-global path planning algorithm introducing time information to generate a global path capable of crossing short obstacles, and pedestrians can quickly reach a target position in an unfamiliar large indoor scene based on the path and avoid the obstacles in the way. An A-x algorithm applicable to a man-machine co-fusion environment is adopted for a two-dimensional grid map facing a robot, a global path based on pedestrian prediction and space-time consistency constraint is generated, and the fact that the running track of pedestrians in a certain period can be predicted in advance and avoided in advance when the robot runs indoors is guaranteed.

2) Aiming at the problem of prediction time deviation caused by uncertainty of the running speed of the pedestrian, a background cancellation and DWA local trajectory planning algorithm is provided to realize the functions of pedestrian prediction and dynamic obstacle avoidance, and the man-machine safety of the robot in the running process is ensured. Aiming at the navigation problem among multiple floors of a robot, a fully intelligent elevator transportation technology is realized by combining the robot and an elevator dispatching module, a multi-layer two-dimensional grid map can be used for navigation among the multiple floors in a building, pedestrian prediction can be carried out without establishing a multi-layer cost map, less resources are occupied, the function is stronger, the robot can be transported to a target floor from an initial floor without artificial interference, and the labor cost is greatly reduced.

3) The method realizes two-dimensional projection by establishing the three-dimensional grid map and based on the difference of the motion characteristics of the pedestrians and the robot, the generated two-dimensional grid map facing the pedestrians and the robot can not only guide the pedestrians unfamiliar with large indoor scenes, but also can be used for generating the global path of the robot together with the generated two-dimensional grid map facing the pedestrians and the robot due to the fact that the two-dimensional grid map facing the pedestrians and the robot are consistent in size, the global path based on pedestrian prediction and space-time consistency constraint is generated by combining time space information, and the man-machine safety of the robot in the man-machine co-fusion environment during driving is improved.

Drawings

FIG. 1 is a flow chart of the path planning between buildings in man-machine integration;

FIG. 2 is a two-dimensional grid map facing a pedestrian;

FIG. 3 is a two-dimensional grid map for a robot;

FIG. 4 is a pedestrian global path that may traverse a short obstacle;

FIG. 5 is a robot global path without consideration of pedestrian predictions;

FIG. 6 is a robot global path based on pedestrian predictions and spatiotemporal consistency constraints;

FIG. 7(a) shows two adjacent frames of laser radar point cloud data cluster points; FIG. 7(b) shows two transformed frame cluster points;

fig. 8 is a dynamic obstacle avoidance based on the DWA local planning algorithm.

Detailed Description

The man-machine integrated building path planning method, the computer device and the program product provided by the embodiment of the invention, as shown in fig. 1, comprise the following steps:

the method comprises the following steps: when the robot with the laser radar runs between buildings, collecting laser radar environment information in the running process, constructing a three-dimensional grid map, performing two-dimensional projection based on the difference of motion characteristics of pedestrians and the robot, and generating a two-dimensional grid map facing the pedestrians and the robot:

step 1, when a robot equipped with a laser radar runs on a single floor between buildings, the laser radar is adopted to collect environmental information in the running process;

step 2, constructing a three-dimensional grid map, which comprises the following specific steps:

step 2.1, obtaining a three-dimensional point cloud map by adopting a point cloud registration algorithm according to the perception information obtained from the upper layer;

step 2.2, filtering the three-dimensional point cloud, and converting the filtered three-dimensional point cloud map into a three-dimensional grid map with resolution of resolution, wherein resolution is 0.05;

step 3, settingA height range h_{pedestrianmin}≤z_gridoctreeWherein h is_{pedestrianmin}And (3) when the maximum height of the obstacle which can be crossed by the pedestrian is 0.8 m, projecting the three-dimensional grid map on the Z axis into the xoy plane to obtain a two-dimensional grid map facing the pedestrian, and if the horizontal and vertical coordinates in the three-dimensional grid map are (x)_gridoctree,y_gridoctree) Z axis satisfies h_{pedestrianmin}≤z_gridoctreeDoes not exist in the range of (a), it is projected as a two-dimensional grid (x) in the xoy plane_gridoctree,y_gridoctree) Is a free grid; if a three-dimensional grid is present, the two-dimensional grid whose projection is in the xoy plane is an obstacle grid, where (x)_gridoctree,y_gridoctree,z_gridoctree) Coordinates of a central point of the three-dimensional grid are obtained;

step 4, setting a height range z_gridrobot≤h_robotmaxWherein h is_robotmaxAnd (3) projecting the three-dimensional grid map on a xoy plane on a Z axis to obtain a robot-oriented two-dimensional grid map, wherein the maximum height of the robot is 0.5 m, and if the horizontal and vertical coordinates in the three-dimensional grid map are (x)_gridrobot,y_gridrobot) Z axis satisfies 0. ltoreq. Z_gridrobot≤h_robotmaxDoes not exist in the range of (b), it is projected as a two-dimensional grid (x) in the xoy plane_gridrobot,y_gridrobot) Is a free grid; if a three-dimensional grid is present, the two-dimensional grid whose projection is in the xoy plane is an obstacle grid, where (x)_gridrobot,y_gridrobot,z_gridrobot) Coordinates of a central point of the three-dimensional grid are obtained;

and 5, returning to the step 1, constructing three-dimensional grid maps of other floors, performing two-dimensional projection based on the difference of the motion characteristics of the pedestrians and the robots, and generating two-dimensional grid maps facing the pedestrians and the robots.

As shown in fig. 2 and 3, fig. 2 is a generated pedestrian-oriented two-dimensional grid map, and fig. 3 is a robot-oriented two-dimensional grid map, in which a dotted frame is a short obstacle that a pedestrian can cross, but the obstacle region cannot pass through for the robot.

Step two: and (3) generating a global path capable of crossing short obstacles by adopting an A-global path planning algorithm on the two-dimensional grid map facing the pedestrians:

step 1, marking the elevator Position of each floor in the map as Position according to the generated two-dimensional grid map facing the pedestrian_elevator(x_elevator,y_elevator) Converting the elevator door barrier grid into an idle grid in the two-dimensional grid map;

step 2, performing obstacle expansion on the two-dimensional grid map, wherein the obstacle grid is taken as an expansion center, and the expansion radius is radius_{pedestrianinflation}If the expansion area of a certain cell is less than half of the cell area, the cell is a vacant cell, otherwise, the cell is an expanded cell, wherein radius_{pedestrianinflation}0.2 m;

step 3, the pedestrian inputs the goal point_pedestrian(x_{pedestriangoal},y_{pedestriangoal},level_{pedestriangoal}) The current position start of the pedestrian_pedestrian(x_{pedestrianstart},y_{pedestrianstart},level_{pedestrianstart}) Wherein x is_{pedestriangoal}And y_{pedestriangoal}Is a two-dimensional coordinate, level, of the target point in the xoy plane_{pedestriangoal}Is the floor of the target, x_{pedestrianstart}And y_{pedestrianstart}Is a two-dimensional coordinate, level, of a pedestrian in an xoy plane_{pedestrianstart}The floor where the pedestrian is currently located;

step 4, judging whether the level is met_{pedestriangoal}＝level_{pedestrianstart}(ii) a If yes, indicating that the target point and the pedestrian are currently located on the same floor, and turning to the step 4.1; if not, the target point and the pedestrian are positioned at different floors, and the step 4.2 is carried out;

and 4.1, if the target point and the pedestrian are currently positioned on the same floor, generating a global path which has time-space information and can cross over short obstacles by adopting an A-star algorithm introducing time information, wherein the starting point is (x)_{pedestrianstart},y_{pedestrianstart}) The target point is (x)_{pedestriangoal},y_{pedestriangoal})；

Step 4.2, if the target point and the pedestrian are currently positioned at different floors, generating a level by adopting an A-x global path planning algorithm introducing time information_{pedestrianstart}Global path of layers, where the starting point is (x)_{pedestrianstart},y_{pedestrianstart}) The target point is the level_{pedestrianstart}Position of elevator on floor_elevator(x_elevator,y_elevator) (ii) a Then, the pedestrian takes the elevator to go to the target floor level_goalAnd updating the two-dimensional grid map to the level_goalA two-dimensional grid map of layers; finally, a level is generated_{pedestriangoal}Global path of floor with the starting point being the level_{pedestriangoal}Position of floor elevator_elevator(x_elevator,y_elevator) The target point is (x)_{pedestriangoal},y_{pedestriangoal})。

The A-global path planning algorithm for introducing the time information is specifically realized as follows:

2) calculating the starting point start (x)_start,y_start) Heuristic value of F_startAnd its arrival at the start (x) of origin_start,y_start) At time t_startWhen 0, the set Open { (x) is stored_start,y_start,F_start,t _start0,0) }; wherein the heuristic value F_startThe calculation formula of (a) is as follows:

3) judging whether the set Open is an empty set, if not, turning to 4); if so, finishing the global planning, and finding no feasible driving path from the starting point to the target point;

4) selecting the grid point with the minimum heuristic value from the set OpenAs the next step grid point grid_n(x_n,y_n) Acquire its time information t_nGrid points grid_nMove from set Open into set Close;

5) and judging grid points grid_nWhether the target point is goal; if yes, grid point grid is determined according to grid points_nThe father nodes are traced back in sequence to find a travelable path from the starting point to the target point; if not, go to 6);

6) finding grid point grid_nNon-obstacle grids in eight neighborhood orientations and not in aggregate Close, denoted as near reachable grids grid_nc(x_nc,y_nc) Calculating heuristic values F of these grids_ncAnd the time t of arrival at these grids_nc＝t_n+t_fSetting parent nodes of these grids to grid_nStore the set Open { (x)_start,y_start,F_start,t_start,0,0)、…、(x_nc,y_nc,F_nc,t_nc,x_n,y_n) In (1) }; wherein t is_f1 second is from grid point grid_nTo grid_ncPredicted time consumption of (2); wherein the heuristic value F_ncThe calculation formula of (a) is as follows:

wherein I_NFor expansion cost, if approaching, the grid can be reached_ncWhen it is a free grid, I_N0; reachable grid if close_ncIn the case of the expansion of the grid,

wherein p is₁、p₂For given parameters, 20 and 5 respectively, and distance _ obstacle is the distance between the expansion grid and the nearest barrier grid;

7) and turning to 3) until the global planning is finished.

As shown in fig. 4, where the dotted line is a global pedestrian path, it can be seen that the generated path can well avoid the obstacle to reach the target position.

Step three: a star algorithm applicable to a man-machine co-fusion environment is adopted for a robot-oriented two-dimensional grid map to generate a global path based on pedestrian prediction and space-time consistency constraint, and the specific steps are as follows:

step 1, the robot obtains the position (x) of the robot in the xoy plane through AMCL positioning_robotstart,y_robotstart) The floor where the robot is currently located is level_robotstart；

Step 2, marking the elevator Position of each floor in the map as Position according to the generated two-dimensional grid map facing the robot_elevator(x_elevator,y_elevator) Converting the elevator door barrier grid into an idle grid in the two-dimensional grid map;

and 3, performing obstacle expansion on the two-dimensional grid map, wherein the obstacle grid is taken as an expansion center, and the expansion radius is radius_{robotinflation}If the expansion area of a certain cell is less than half of the cell area, the cell is a vacant cell, otherwise, the cell is an expanded cell, wherein radius_{robotinflation}0.5 m;

step 4, inputting a robot navigation target goal_robot(x_robotgoal,y_robotgoal,level_robotgoal) Wherein x is_robotgoalAnd y_robotgoalIs a two-dimensional coordinate, level, of the target point in the xoy plane_robotgoalIs the floor where the target is located;

step 5, judging whether the level is met_robotgoal＝level_robotstart(ii) a If yes, the target point and the robot are located on the same floor currently, and the step 5.1 is carried out; if not, the target point and the robot are currently positioned on different floors, and the step 5.2 is carried out;

step 5.1, if the target point and the robot are currently positioned on the same floor, generating a global path based on pedestrian prediction and space-time consistency constraint by adopting an A-star algorithm, wherein the starting point is (x)_robotstart,y_robotstart) The target point is (x)_robotgoal,y_robotgoal)；

And 5.2, if the target point and the robot are currently positioned on different floors, performing inter-floor navigation by combining an A-algorithm with an elevator dispatching module, and specifically comprising the following steps:

3) the robot acquires the level of the floor where the elevator is located currently_elecurrentCalculating the level of the floor where the elevator is located_elecurrentReach the floor level of the robot_robotstartTime t of_es，

4) judging whether s is satisfied_robotglobal≤s_thresoldWherein s is_thresoldA given distance threshold; if yes, turning to step 5); if not, the robot continues to run, and meanwhile, the time t consumed by the robot to reach the target point is updated_rsAnd returning to the step 3);

5) judging whether | t is satisfied_rs-t_es|≤t_thresoldWherein t is_thresoldIs a given time threshold; if yes, the robot arrives at the target pointThen the elevator will reach the floor level of the robot_robotstartIssuing an elevator calling instruction to an elevator dispatching module; if not, indicating that the robot delays in the driving process due to the existence of the obstacle, and sending an elevator calling command after reaching the target point;

The algorithm a applicable to the human-machine co-fusion environment is specifically implemented as follows:

1) receive the goal_robot(x_robotgoal,y_robotgoal) Two-dimensional information, pedestrian-oriented global path reference points, and two empty sets Open_rAnd Close_r；

2) Calculating the starting point start of the robot_robot(x_robotstart,y_robotstart) Heuristic value of F_robotstartAnd its arrival at the start of arrival_robotAt time t_robotstartStore in the set Open at 0_r＝{(x_robotstart,y_robotstart,F_robotstart,t _robotstart0,0) }; wherein the heuristic value F_startThe calculation formula of (c) is as follows:

3) determine set Open_rWhether the collection is empty or not, if not, turning to 4); if so, finishing the global planning, and finding no feasible driving path from the starting point to the target point;

4) from the set Open_rSelecting the grid point with the minimum heuristic value as the grid point grid of the next step_rn(x_rn,y_rn) Acquiring its time information t_rnGrid points grid_rnFrom the set Open_rMove into aggregate Close_rPerforming the following steps;

5) and judging grid points grid_rnWhether it is the target goal_robot(ii) a If yes, grid point grid is determined according to grid points_rnThe father nodes are traced back in sequence to find a travelable path from the starting point to the target point; if not, go to 6);

6) according to grid point grid_rnTime information t of_rnGet from t_rnT after the time_simAnd marking the two-dimensional information of the global path reference Point of the pedestrian in seconds as Point_pedestrian(x_pedestrian,y_pedestrian) Wherein t is_sim3 is the time given in advance, and marks the grid occupied by the reference points as the barrier grid, and simultaneously, the barrier expansion is carried out, the expansion center is the barrier grid, and the expansion radius is radius_pedestrian0.5 m;

7) finding grid point grid_rnEight neighborhood orientations and not in aggregate Close_rNon-obstacle grid in (1), marked as a near reachable grid_rnc(x_rnc,y_rnc) Calculating heuristic values F of these grids_rncAnd the time t of arrival at these grids_rnc＝t_rn+t_rfSetting parent nodes of these grids to grid_rnTo store into the set Open_r＝{(x_robotstart,y_robotstart,F_robotstart,t_robotstart,0,0)、…、(x_rnc,y_rnc,F_rnc,t_rnc,x_rn,y_rn) In (1) }; wherein t is_rf1 second is from grid point grid_rnTo grid_rncPredicted time consumption of (2); wherein the heuristic value F_rncThe calculation formula of (a) is as follows:

in which I_rNFor expansion costs, the grid can be reached if it is close_rncWhen it is a free grid, I_rN0; reachable grid if close_rncIn the case of the expansion of the grid,

wherein p is_r1、p_r2For given parameters, 20 and 5 respectively, and distance _ obstacle _ r is the distance between the expansion grid and the nearest barrier grid;

8) and turning to 3) until the global planning is finished.

As shown in fig. 5 and 6, the reason why the robot trajectories in fig. 5 and 6 are different is that fig. 6 is a global path based on pedestrian prediction and spatio-temporal consistency constraint generated by a global path planning algorithm applicable to a human-computer co-fusion environment, and fig. 5 does not consider pedestrian trajectory prediction. Since the set pedestrian speed is faster than the robot, the pedestrian is traveling from behind the robot in fig. 6, and the robot deviates from the original trajectory in order to give way to the pedestrian.

Step four: the method specifically comprises the following steps of carrying out dynamic pedestrian track prediction and obstacle avoidance based on a background cancellation and DWA local planning algorithm:

step 1, defining a local cost map of the robot as a length and a width respectively by taking the current position of the robot as a center_robotlocal＝12、width_robotlocal＝12；

And 2, predicting the dynamic pedestrian track based on the background cancellation method, which comprises the following steps:

step 2.1, when the robot equipped with the laser radar runs in an autonomous navigation mode, the laser radar is adopted to obtain point cloud data of adjacent frames, two-dimensional coordinates of the point cloud data are obtained, the time difference is recorded as delta t, and the two-dimensional coordinates of the two frames of point cloud data are respectively recorded as Pointcloud_t-1(x_pc,t-1,y_pc,t-1)、Pointcloud_t(x_pc,t,y_pc,t) Recording the current frame point cloud data at the time of t, and recording the previous frame point cloud data at the time of t-1;

step 2.2, respectively adopting a K-means clustering method to obtain clustering points for the two frames of point cloud data, and respectively recording the clustering points as pointcluster_kmeans,t-1(x_kmeanspc,t-1,y_kmeanspc,t-1)、Pointcloud_kmeans,t(x_kmeanspc,t,y_kmeanspc,t)；

Step 2.3, acquiring the running speed v of the robot according to the chassis data of the robot_robotAnd course angle increment delta theta_robotWhere Δ θ_robotThe increment of the robot course angle from t-1 to t;

step 2.4, calculating the travel distance of the robot in the x and y directions within the time delta t, wherein the specific calculation formula is as follows:

wherein (x)_t-1,y_t-1) Two-dimensional coordinate information of the robot in the global coordinate system at the moment t-1;

step 2.5, clustering point Pointcloud_kmeans,t-1(x_kmeanspc,t-1,y_kmeanspc,t-1) Coordinate conversion is carried out by adopting the formula in the step 2.4, and the converted clustering point Pointcluster is obtained_{kmeanstrans,t-1}(x_{kmeanstranspc,t-1},y_{kmeanstranspc,t-1})；

Step 2.6, clustering point Pointcloud_t(x_pc,t,y_pc,t)、Pointcloud_{kmeanstrans,t-1}(x_{kmeanstranspc,t-1},y_{kmeanstranspc,t-1}) Respectively limiting the range, acquiring clustering points in the local cost map, and respectively recording the clustering points as pointclosed_rt(x_rpc,t,y_rpc,t)、Pointcloud_{rkmeanstrans,t-1}(x_{rkmeanstranspc,t-1},y_{rkmeanstranspc,t-1})；

Step 2.7, clustering points Pointcloud are respectively aligned_rt(x_rpc,t,y_rpc,t) And clustering point pointclosed_{rkmeanstrans,t-1}(x_{rkmeanstranspc,t-1},y_{rkmeanstranspc,t-1}) Performing difference processing if the difference meets the requirement

Then the coordinate is said to be (x)_rpc,t,y_rpc,t) The obstacle of (a) is a static obstacle, wherein range_thresold0.05 is a given threshold; if not satisfied with

Then the coordinate is said to be (x)_rpc,t,y_rpc,t) The barrier of (2) is a dynamic pedestrian and goes to step 2.8;

step 2.8, calculating the dynamic pedestrian running speed v_doAnd a heading angle theta_doThe specific calculation formula is as follows:

step 2.9, according to the two-dimensional coordinates (x) of the dynamic pedestrian_rpc,t,y_rpc,t) And its running speed v_doHeading angle theta_doPredicting the driving track, wherein the specific calculation formula is as follows:

wherein, t_predict＝1,2,…,t_dopredict，t_dopredictPredicting a time duration for the trajectory;

and 3, combining the predicted track of the pedestrian to dynamically avoid the obstacle based on the DWA local planning algorithm, and specifically comprising the following steps of:

step 3.1, limiting a robot speed and angular speed sampling space according to the robot state information;

step 3.2, performing expansion processing on each obstacle in the local cost map, wherein the circle center is the center of the obstacle, and the expansion radius is radius_{robotinflation}＝0.5；

Step 3.3, traversing each speed v of the robot_robotAngular velocity omega_robotCombining to generate a bar at t_robotsimSequence of trace points traj within 3 seconds_robotThe specific calculation formula is as follows:

wherein (x)_robot,y_robot) Position information of the robot at the current moment;

step 3.4, evaluate trajectory traj_robotThe method comprises the following specific steps of:

1) calculating the track traj_robotMinimum distance between last trace point and global path_mintoglobalSubsequently calculating distance_{localtoglobal}＝D-distance_mintoglobalAs the tracking global path cost, where D ═ 20 is a previously given value;

2) to predict the resulting pedestrian trajectory (x)_{pedestrianpredict},y_{pedestrianpredict}) As a circle center with radius of radius_{pedestrianpredict}Performing obstacle inflation, wherein radius_{pedestrianpredict}Is calculated as follows:

then traversing all the obstacles in the local cost map, and calculating the track traj_robotDistance between the last track point and the nearest barrier in the local cost map_{localtoobstacle}As a collision cost; and judges whether distance is satisfied_{localtoobstacle}≥2×radius_{robotinflation}If yes, distance_{localtoobstacle}＝2×radius_{robotinflation}；

3) Selecting a track traj_robotTaking the absolute speed of the last trace point as the speed cost distance_vel；

4) The three cost values are stored in a set of { (distance)_{localtoglobal},distance_{localtoobstacle},distance_vel) … };

5) and normalizing each type of data in the set Evaalurate, wherein the specific calculation formula is as follows:

wherein, num_trajrobotFor all traces traj of a sample_robot；

6) And calculating the total cost value of each sampling track, wherein the specific calculation formula is as follows:

evaluate_traj,k＝α·normal_distance_{localtoglobal,k}+β·normal_distance_{localtoobstacle,k}+γ·normal_distance_vel,k

wherein, alpha, beta and gamma are given weighted values in advance and are respectively 0.1, 0.1 and 0.05;

7) and selecting the track with the maximum total cost value as a local obstacle avoidance track.

Fig. 7(a) shows two adjacent frames of laser radar point cloud data cluster points, where a hollow circle is a cluster point of a previous frame and a solid circle is a cluster point of a current frame, and the two frames of cluster points do not coincide with each other due to a time difference between the two frames of point cloud data; fig. 7(b) shows two frames of transformed clustering points, it can be found that 24 clustering points among 25 clustering points are basically overlapped, and are not completely overlapped due to the existence of noise, and the position deviation is large as seen from the square frame part in the figure, which indicates that the obstacle corresponding to the clustering point is a dynamic obstacle, i.e. a pedestrian. And then, the pedestrian track prediction can be carried out according to the clustering points of the two frames.

Fig. 8 is a simulation diagram of dynamic obstacle avoidance using a DWA local planning algorithm, in which dynamic and static obstacles exist at the same time, and the result shows that the robot can perform pedestrian trajectory prediction and advance avoidance.

Claims

1. A man-machine integrated building path planning method is characterized by comprising the following steps:

2. The human-computer integrated building-to-building path planning method according to claim 1, wherein the specific implementation process of step S1 includes:

s1.2, filtering the three-dimensional point cloud map, and converting the filtered three-dimensional point cloud map into a three-dimensional grid map with resolution;

s1.3, setting a height range h_{pedestrianmin}≤z_gridoctreeWherein h is_{pedestrianmin}Projecting the three-dimensional grid map to the xoy plane on the Z axis for the maximum height of the pedestrian capable of crossing the barrier to obtain a two-dimensional grid map facing the pedestrian; if the horizontal and vertical coordinates in the three-dimensional grid map are (x)_gridoctree,y_gridoctree) Z axis satisfies h_{pedestrianmin}≤z_gridoctreeIn the range of (2), if there is no three-dimensional grid, then two-dimensional grid (x)_gridoctree,y_gridoctree) Is a free grid; if a three-dimensional grid is present, the two-dimensional grid is an obstacle grid, where (x)_gridoctree,y_gridoctree,z_gridoctree) Coordinates of a central point of the three-dimensional grid are obtained;

3. The human-computer integrated building-to-building path planning method according to claim 1, wherein the specific implementation process of step S2 includes:

S2.4, if the target point and the pedestrian are currently positioned at different floors, generating a level by adopting an A-star global path planning algorithm introducing time information_{pedestrianstart}Global path of layers, where the starting point is (x)_{pedestrianstart},y_{pedestrianstart}) The target point is the level_{pedestrianstart}Position of elevator on floor_elevator(x_elevator,y_elevator) (ii) a When the pedestrian takes the elevator to go to the target floor level_goalWhen the two-dimensional grid map is updated to the level_goalA two-dimensional grid map of layers; generate the level_{pedestriangoal}Global path of floor with the starting point being the level_{pedestriangoal}Position of floor elevator_elevator(x_elevator,y_elevator) The target point is (x)_{pedestriangoal},y_{pedestriangoal})。

4. The human-computer co-integrated building-to-building path planning method according to claim 3, wherein the specific implementation process of generating the pedestrian-oriented global path with the time-space information by adopting an A-algorithm introducing time information comprises the following steps:

2) calculate the Start (x)_start,y_start) Heuristic value of F_startAnd its arrival at the start (x) of origin_start,y_start) At time t_startStore the set Open { (x)_start,y_start,F_start,t_start0,0) }; heuristic value F_startThe calculation formula of (2) is as follows:

4) selecting the grid point with the minimum heuristic value from the set Open as the grid point grid of the next step_n(x_n,y_n) Acquiring its time information t_nGrid points grid_nMove from set Open into set Close;

5) judging grid points grid_nWhether the target point is goal; if yes, grid point grid is determined according to grid points_nThe father nodes are traced back in sequence to find a travelable path which has time-space information and faces to the pedestrian from the starting point to the target point; if not, turning to the step 6);

6) finding grid points grid_nNon-obstacle grids in eight neighborhood orientations and not in aggregate Close, denoted as near reachable grids grid_nc(x_nc,y_nc) Computing a near reachable grid_nc(x_nc,y_nc) Heuristic value of F_ncAnd time t of arrival at the nearby reachable grid_nc＝t_n+t_fSetting a parent node adjacent to the reachable grid as grid_nStore the set Open { (x)_start,y_start,F_start,t_start,0,0)、…、(x_nc,y_nc,F_nc,t_nc,x_n,y_n) In (1) }; wherein t is_fTo grid from grid point_nTo grid_ncPredicted time consumption of (2); heuristic value F_ncThe calculation formula of (2) is as follows:

wherein I_NFor expansion costs, the grid can be reached if it is close_ncIs a free grid, I_N0; reachable grid if close_ncIn order to expand the grid, the grid is expanded,

wherein p is₁、p₂For a given parameter, distance _ obstacle is the distance of the expanded grid from the nearest barrier grid; wherein the non-obstacle grid includes a free grid and an expanded grid; the determination process of the free grid and the expansion grid comprises the following steps: marking the elevator Position of each floor in the two-dimensional grid map as Position according to the generated two-dimensional grid map facing the pedestrian_elevator(x_elevator,y_elevator) Marking the elevator door barrier grid as an idle grid in a two-dimensional grid map; performing obstacle expansion operation on the two-dimensional grid map, namely taking the obstacle grid as an expansion center and the expansion radius as radius_{pedestrianinflation}If the expansion area of a certain barrier grid is less than half of the area of the barrier grid, the barrier grid is an idle grid, otherwise, the barrier grid is an expansion grid;

7) and skipping to the step 3) until the global planning is finished.

5. The human-computer integrated building-to-building path planning method according to claim 1, wherein the specific implementation process of step S3 includes:

S3.5, if the target point and the robot are currently positioned on different floors, performing inter-floor navigation by combining an A-algorithm with an elevator dispatching module; preferably, the step S3.5 is implemented by:

1a) firstly, generating a level by adopting an A-algorithm_robotstartGlobal path of layers, where the starting point is (x)_robotstart,y_robotstart) The target point is the level_robotstartPosition of elevator on floor_elevator(x_elevator,y_elevator)；

2a) Calculating the time t taken for the robot to reach the target point according to the global path generated in the step 1a)_rs，

3a) robot obtains elevator present floor level_elecurrentCalculating the level of the floor where the elevator is located_elecurrentReach the floor level of the robot_robotstartTime t of_es，

Wherein s is_{elevatorstart}For the level of the floor where the elevator is located_elecurrentThe distance from the floor level of the robot_robotstartHeight of (v)_elevatorThe detected running speed of the elevator is a fixed value;

4a) judging whether s is satisfied_robotglobal≤s_thresoldWherein s is_thresoldA given distance threshold; if yes, go to step 5 a); if not, the robot continues to run, and meanwhile, the time t consumed when the robot reaches the target point is updated_rsAnd returning to the step 3 a);

5a) judging whether | t is satisfied_rs-t_es|≤t_thresoldWherein t is_thresoldIs a given time threshold; if yes, the elevator reaches the floor level of the robot after the robot reaches the target point_robotstartIssuing a calling elevator instruction to an elevator dispatching module; if not, sending an elevator calling command after reaching the target point;

6a) after the elevator reaches the floor where the robot is located, the robot sends an elevator door opening instruction to the elevator dispatching module; meanwhile, the robot judges whether a pedestrian enters or exits the elevator, if yes, the robot retreats, and the robot enters the elevator after the pedestrian enters or exits the elevator; if no pedestrian enters or exits the elevator, the robot directly enters the elevator;

7a) after the robot enters the elevator, an instruction of successfully entering the elevator is issued to the elevator dispatching module, and after the elevator dispatching module receives the instruction of successfully entering the elevator issued by the robot, the elevator dispatching module sends the instructionThe elevator door is closed by a command, and the elevator is controlled to go to the target floor level_robotgoalSimultaneously, issuing an instruction to the robot to switch the two-dimensional grid map;

8a) after the elevator reaches the target floor, an elevator dispatching module issues an elevator door opening instruction and issues a target floor navigation instruction to the robot;

9a) after receiving a target floor arrival instruction from the elevator dispatching module, the robot starts target floor navigation and generates a level by adopting an A-x algorithm_robotgoalGlobal path of layer, where the starting point is the level_robotgoalPosition of floor elevator_elevator(x_elevator,y_elevator) The target point is (x)_robotgoal,y_robotgoal)。

6. The human-computer co-integrated building-to-building path planning method according to claim 5, wherein the specific implementation process for generating a robot-oriented global path based on pedestrian prediction and space-time consistency constraint by adopting an A-x algorithm comprises the following steps:

B) Calculating a robot starting point start_robot(x_robotstart,y_robotstart) Heuristic value of F_robotstartAnd its arrival at the start_robotAt time t_robotstartTo store into the set Open_r＝{(x_robotstart,y_robotstart,F_robotstart,t_robotstart0,0) }; wherein the heuristic value F_startThe calculation formula of (a) is as follows:

C) determine set Open_rWhether the collection is an empty collection or not, if not, turning to the step D); if yes, then the global planning is carried outEnding, and not finding a global path facing the robot from a starting point to a target point based on pedestrian prediction and space-time consistency constraint;

D) from the set Open_rSelecting the grid point with the minimum heuristic value as the grid point grid of the next step_rn(x_rn,y_rn) Get grid_rn(x_rn,y_rn) Time information t of_rnGrid the grid points_rnFrom the set Open_rMove into aggregate Close_rThe preparation method comprises the following steps of (1) performing;

E) judging grid points grid_rnWhether it is the target goal_robot(ii) a If yes, grid point grid is determined according to grid points_rnThe father nodes are traced back in sequence, and a global path facing the robot from a starting point to a target point based on pedestrian prediction and space-time consistency constraint is found; if not, turning to the step F);

wherein p is_r1、p_r2For a given parameter, distance _ obstacle _ r is the distance of the expanded grid from the nearest barrier grid; wherein the non-obstacle grid includes a free grid and an expanded grid; the determination process of the free grid and the expansion grid comprises the following steps: marking the elevator Position of each floor in the two-dimensional grid map facing the robot as Position_elevator(x_elevator,y_elevator) Converting the elevator door barrier grid into an idle grid in a two-dimensional grid map; performing obstacle expansion on the two-dimensional grid map, namely taking the obstacle grid as an expansion center and the expansion radius as radius_{robotinflation}If the expansion area of a certain barrier grid is less than half of the area of the barrier grid, the barrier grid is an idle grid, otherwise, the barrier grid is an expansion grid;

H) and C) jumping to the step C) until the global planning is finished.

7. The human-computer integrated building-to-building path planning method according to claim 1, wherein the specific implementation process of step S4 includes:

i) defining the local cost map of the robot as the length and the width of the local cost map of the robot by taking the current position of the robot as the center_robotlocal、width_robotlocal；

(x_rpc,t,y_rpc,t) As a clustering point pointcluster_rt(x) of (C)_{rkmeanstranspc,t-1},y_{rkmeanstranspc,t-1}) As a clustering point pointcluster_{rkmeanstrans,t-1}Coordinate of (1), Pointcloud_rt(x_rpc,t,y_rpc,t) As a clustering point pointcluster_t(x_pc,t,y_pc,t) Clustering points within the local cost map; pointcloud_t(x_pc,t,y_pc,t) Two-dimensional coordinates of current frame point cloud data; pointcloud_{rkmeanstrans,t-1}(x_{rkmeanstranspc,t-1},y_{rkmeanstranspc,t-1}) As a clustering point pointcluster_{kmeanstrans,t-1}(x_{kmeanstranspc,t-1},y_{kmeanstranspc,t-1}) Clustering points, Pointcloud, within the local cost map_{kmeanstrans,t-1}(x_{kmeanstranspc,t-1},y_{kmeanstranspc,t-1}) To cluster point pointclosed_kmeans,t-1(x_kmeanspc,t-1,y_kmeanspc,t-1) Carrying out coordinate conversion to obtain clustering points; pointcloud_kmeans,t-1(x_kmeanspc,t-1,y_kmeanspc,t-1) For the two-dimensional coordinate Pointcloud of the last frame point cloud data_t-1(x_pc,t-1,y_pc,t-1) Carrying out K-means clustering to obtain clustering points;

iii) to predict the resulting pedestrian trajectory (x)_{pedestrianpredict},y_{pedestrianpredict}) As a circle center with radius of radius_{pedestrianpredict}Performing obstacle inflation; traversing all obstacles in the local cost map and calculating a track traj_robotDistance between the last track point and the nearest barrier in the local cost map_{localtoobstacle}As a collision cost; and judgeWhether distance is satisfied_{localtoobstacle}≥2×radius_{robotinflation}If yes, distance_{localtoobstacle}＝2×radius_{robotinflation}；

iv) selecting a trajectory traj_robotTaking the absolute speed of the last track point as the speed cost distance_vel；traj_robotFor traversing each speed v of the robot within the speed and angular speed threshold_robotAngular velocity omega_robotOne of the combined products is at t_robotsimA sequence of trace points in seconds;

v) order set

vi) normalizing each type of data in the set Evaluate:

vii) calculating the total cost value of each sampling track, wherein the specific calculation formula is as follows:

wherein, alpha, beta and gamma are given weighted values;

8. The human-computer integrated building-to-building path planning method according to claim 7, wherein a dynamic pedestrian traveling speed v is calculated_doAnd a heading angle theta_doPreviously, the following judgment was also made:

to clustering point pointcluster_rt(x_rpc,t,y_rpc,t) And clustering point pointclosed_{rkmeanstrans,t-1}(x_{rkmeanstranspc,t-1},y_{rkmeanstranspc,t-1}) Performing difference processing if the difference meets the requirement

Then the coordinate is said to be (x)_rpc,t,y_rpc,t) The obstacle of (a) is a static obstacle, wherein range_thresoldIs a given threshold value; if not satisfied with

Then the coordinate is said to be (x)_rpc,t,y_rpc,t) The obstacle is a dynamic pedestrian, and the running speed v of the dynamic pedestrian is calculated_doAnd a heading angle theta_do。

9. A computer apparatus comprising a memory, a processor and a computer program stored on the memory; characterized in that the processor executes the computer program to carry out the steps of the method according to one of claims 1 to 8.

10. A computer program product comprising a computer program/instructions; characterized in that the computer program/instructions, when executed by a processor, performs the steps of the method according to one of claims 1 to 8.