An active camouflage patrol navigation method of an intelligent robot in a complex scene

Abstract

The application discloses an active camouflage patrol navigation method of an intelligent robot in a complex scene. The method comprises the following steps: (1) constructing a camouflage patrol semantic map; (2) fusing RTK and IMU positioning: fusing RTK and IMU sensor data by means of an ESKF to estimate the pose of the robot in the complex scene; and (3) DPPC motion control: designing an active camouflage patrol motion control method based on a geometric model for a differential robot motion model. According to the active camouflage patrol navigation method, the robot can determine a decision-making camouflage patrol manner according to prior map semantic information, and different methods are executed according to scene characteristics, so that the patrol efficiency is improved as much as possible while ensuring that the robot is not damaged by illegal persons, and finally, the patrol with the consideration of rapidity and safety is realized.

Description

Technical Field

[0001] This invention belongs to the field of robot planning and navigation technology. Specifically, it relates to an active camouflage patrol navigation method for intelligent robots in complex scenarios. Background Technology

[0002] To address the common problem of security patrol robots lacking proactive self-protection awareness and being easily detected or even destroyed by tracking targets during missions, Chinese patent CN113536934A proposes a method for actively concealing patrol robots. This method can reduce the visibility of their actions, but it suffers from low patrol efficiency, with patrols taking a long time when the patrol area is large.

[0003] Experience shows that criminals rarely commit acts of vandalism in open environments or densely populated areas, where the security threat to robots is relatively low. Therefore, robots can perform conventional patrol methods in these scenarios to ensure efficient mission completion. However, in sparsely populated and relatively confined spaces, criminals can easily damage robots. In such environments, employing camouflage patrol methods can effectively reduce the probability of the robot being detected or even destroyed, thus ensuring its own safety. In diverse, large-scale outdoor patrol areas, if robots can perform different patrol behaviors according to the characteristics of different scenarios, they can balance patrol efficiency and safety, completing patrol missions more efficiently and safely. Summary of the Invention

[0004] To address the shortcomings of existing technologies, this invention proposes an active camouflage patrol navigation method for intelligent robots in complex scenarios. A camouflage patrol semantic map is constructed based on an Open Street Map (OSM) to provide prior information such as patrol paths and scene attributes. The robot is equipped with sensors such as RTK and IMU, and uses an Error State Kalman Filter (ESKF) algorithm to estimate its pose in the map coordinate system. Based on its own pose and the map's prior information, the robot determines the scene type and decides on the patrol method. If the scene is determined to be a camouflage patrol scenario, active camouflage patrol is executed (see Chinese Patent CN113536934A for details); otherwise, a conventional patrol method is used. The motion control of the conventional patrol method is implemented using Differential Pure Pursuit Control (DPPC). For diverse, large-scale outdoor patrol scenarios, this method can achieve camouflage patrol tasks more safely and efficiently.

[0005] Firstly, a proactive camouflage patrol navigation method for intelligent robots in complex scenarios is provided, specifically including the following steps:

[0006] Step S1: Construct a camouflage patrol semantic map based on Open Street Map (OSM);

[0007] Step S2: Estimate the pose of the patrol robot based on the state error Kalman filter algorithm;

[0008] Step S3: Patrol route generation and patrol mode decision

[0009] Based on the estimated pose of the patrol robot obtained in step S2, the nearest path point N of the current patrol robot in the camouflage patrol semantic map is obtained. i ;

[0010] All path points in the camouflage patrol semantic map are parsed into an undirected graph, and a patrol path is planned using Dijkstra's algorithm.

[0011] Obtain the stored road semantic information of each path point in the patrol route, determine whether the semantic information of the current path point is a disguised and hidden road, if so, perform active disguised patrol, if it is other semantic information, perform step S4, and repeat the steps after reaching the next path point.

[0012] Step S4, Patrol Motion Control

[0013] Based on the estimated pose of the patrol robot obtained in step S2, the angular velocity of the patrol robot is obtained; the patrol path following function is realized based on the real-time updated angular velocity of the patrol robot.

[0014] Secondly, a robotic active camouflage patrol navigation system is provided, comprising:

[0015] The Disguise Patrol Semantic Map Building Module is used to build patrol semantic maps based on the Open Street Map (OSM).

[0016] The patrol robot pose estimation module is used to estimate the pose of the patrol robot based on the state error Kalman filter algorithm, and to obtain the nearest path point in the camouflage patrol semantic map based on the estimated pose of the patrol robot.

[0017] The patrol path generation module is used to obtain the patrol path in the disguised patrol semantic map based on the nearest path point and the patrol target point;

[0018] The patrol mode decision module is used to decide the patrol mode based on the road semantic information stored at each path point in the patrol route;

[0019] The patrol motion control module enables the patrol robot to follow the patrol path based on its real-time updated angular velocity.

[0020] Thirdly, a computer-readable storage medium is provided having a computer program stored thereon, which, when executed in a computer, causes the computer to perform the method described thereon.

[0021] Fourthly, a robot is provided, characterized in that it includes a processor and a memory, the memory storing machine-executable instructions executable by the processor, the processor executing the machine-executable instructions to implement the method.

[0022] The present invention has the following beneficial effects:

[0023] 1. This invention constructs a large-scale camouflage patrol semantic map based on OSM, which can provide path prior information for navigation and make camouflage patrol strategies based on semantic information.

[0024] 2. This invention uses ESKF to fuse RTK and IMU sensor data. This algorithm can effectively reduce the linearization error of nonlinear systems.

[0025] 3. This invention proposes a DPPC path tracking control method, which is better suited for motion control of differential model robots.

[0026] 4. This invention comprehensively considers robot patrol strategies in complex scenarios. The conventional patrol navigation and camouflage patrol navigation methods proposed in this invention can be used in combination to simultaneously ensure patrol efficiency and robot safety. Attached Figure Description

[0027] Figure 1 This is a flowchart of the method of the present invention;

[0028] Figure 2 Visualize the topographical plot of the OSM map;

[0029] Figure 3 Visualize a local map as a plan view;

[0030] Figure 4 Image showing the path filtering results;

[0031] Figure 5 (a)-(b) are the OSM map and the corresponding camouflage patrol semantic map, respectively;

[0032] Figure 6 This is a DPPC motion control model. Detailed Implementation

[0033] The present invention will be further explained below with reference to the accompanying drawings;

[0034] This invention utilizes a PowerBot robot equipped with a RealSense D435 depth camera, RTK, and IMU as an experimental platform. The computer system is started, relevant nodes are run, and the patrol robot begins performing camouflage patrol tasks. This presents an active camouflage patrol navigation method for intelligent robots in complex scenarios. Figure 1 The specific steps are as follows:

[0035] Step S1: Construct a camouflage patrol semantic map based on Open Street Map (OSM)

[0036] In OSM platform map data, pathpoints are described in terms of latitude and longitude, which are based on a spherical coordinate system. However, autonomous mobile platforms typically describe their own pose information using a Cartesian coordinate system. Therefore, it is necessary to convert the latitude and longitude information in the OSM map data into planar coordinates first.

[0037] 1-1 Since pathpoints in the Open Street Map (OSM) road data are described using latitude and longitude, and these latitude and longitude are based on a spherical coordinate system, the latitude and longitude information of the Open Street Map (OSM) is converted into planar coordinates; specifically:

[0038] Calculate the planar distance between the sampling point (lon2, lat2) and the reference point (lon1, lat1) in the Open Street Map (OSM):

[0039]

[0040] Among them, R earth The radius of the Earth;

[0041] The distances between all sampling points and the reference point are calculated sequentially to obtain the planar coordinate positions of all sampling points relative to the reference point, thus converting the spherical coordinate system map into a planar coordinate system map; the final visualized image converted into planar coordinates is shown below. Figure 2 As shown.

[0042] 1-2 Due to observation of local visualization ( Figure 3 As can be seen, the distribution of path points in the Open Street Map (OSM) road data is uneven, with some roads having excessively sparse path point distribution. Therefore, the planar coordinate system map processed in step 1-1 needs to be preprocessed; specifically:

[0043] To ensure the accuracy of the robot's position estimation in the camouflage patrol semantic map, the distribution of nodes in the camouflage patrol map should be as uniform and dense as possible. For road segments with overly sparse pathpoints, interpolation is used to supplement them. Let node1 and node2 be two pathpoints on a road in the planar coordinate system map, and let d be the maximum tolerable distance between the two pathpoints. iter_maxThen the number of interpolation path points that need to be inserted between the two path points is num. node =d 12 / d iter_max , where d 12 This represents the distance between path points node1 and node2;

[0044] interpolation generates path points (x) i ,y i Position:

[0045] x i =((num) node -i)*node1.x+i*node2.x) / num node (2)

[0046] y i =((num) node -i)*node1.y+i*node2.y) / num node

[0047] Where i is the i-th interpolation path point, (node1.x, node1.y) represents the planar coordinates of path point node1, and (node2.x, node2.y) represents the planar coordinates of path point node2;

[0048] 1-3 Filtering of effective roads and acquisition of semantic information in a planar coordinate system map. Figure 4 :

[0049] Since the map data provided by the Open Street Map (OSM) includes road data and regional landmarks (such as buildings, lakes, and parks), and these regional landmarks cannot provide prior information for navigation and occupy storage space, they are deleted as redundant data; specifically:

[0050] ① Distinguish based on the label information of Way elements in the map data of the plane coordinate system map. If the first-level label of a Way element is "highway", the road where the current Way element is located is considered a valid road. For other labels, delete the current Way element. Since the Way element contains multiple Node elements, it is also necessary to clear the Nodes contained in the current Way element.

[0051] To ensure the accessibility of the selected paths and the necessity of patrolling, the remaining Way elements are further filtered, retaining the paths with the secondary labels "unclassified" and "path" to construct the final disguised patrol map.

[0052] ② Based on the mapping of the secondary labels "unclassified" and "path" to the semantic information "NorimalRoad" and "CamouflageRoad" of the camouflage patrol map respectively, a camouflage patrol semantic map is constructed as follows: Figure 5 As shown in (b);

[0053] Figure 6 This is a DPPC motion control model.

[0054] Step S2: Estimate the pose of the patrol robot based on the state error Kalman filter algorithm and obtain the nearest path point.

[0055] 2-1 Estimation of State Variable Initialization

[0056] Assuming the {B} coordinate system, IMU coordinate system, and RTK coordinate system coincide, the estimated state variables of the patrol robot are:

[0057] X = [P, V, R, ba, bω] T (3)

[0058] Where P∈R 3 [m]、V∈R 3 [m / s] and R∈R 3 [rad] represents the position, velocity, and attitude of the patrol robot's body coordinate system in the global coordinate system, respectively; ba∈R 3 [m / s 2 ] and bω∈R 3 [rad / s 2 The superscript T indicates the offset of the IMU acceleration and angular velocity, respectively;

[0059] The error state quantity is:

[0060] δX=[δP, δV, δR, δba, δbω] T (4)

[0061] Taking the initial position of the patrol robot as the origin of the global coordinate system, P is initialized to (0,0,0); the initial velocity is zero, so V is initialized to (0,0,0); R is mainly obtained based on the uniformity of the magnitude of gravity g in different coordinate systems. Assuming that the z-axis of the world coordinate system is parallel to the direction of gravity, the measured value of IMU acceleration is regarded as the projection of g in the vehicle coordinate system. Therefore, the initial attitude R of the patrol robot is deduced from the two sets of IMU accelerations; if the IMU does not have zero bias, the measured value of angular velocity when the IMU is stationary is 0, so the bias of angular velocity is taken as:

[0062]

[0063] Where, ωi ωi represents the angular velocity measurement value when the IMU is stationary at time i; n represents the total number of frames for angular velocity measurement.

[0064] The accelerometer bias is the measured static acceleration value a. i The difference between the gravity projection and the gravity projection:

[0065]

[0066] Among them, a i The acceleration measurement value of the IMU at time i when it is stationary;

[0067] At this point, the initialization of all estimated state variables is complete;

[0068] 2-2 State prediction based on IMU data;

[0069] After the initialization of the estimated state variables is completed, state prediction needs to be performed based on IMU data. The prediction process is mainly based on the kinematic equations, but as will be discussed later in this study, the prediction function is implemented based on the median integral.

[0070] First, calculate the median of acceleration and angular velocity based on the IMU data:

[0071]

[0072] Where R t The update is achieved through the multiplication of the rotation matrices:

[0073] R t =R t-1 R{Δφ} (8)

[0074] Where R{} is the rotation matrix obtained by the Rodriguez formula; Δφ is the angular increment during the state period, Δφ=(ω mid -bω)Δt; Δt represents the state period time interval;

[0075] Position and speed updated as follows:

[0076]

[0077] Therefore, the error state is updated as follows:

[0078]

[0079] in,[] × For rotation update operations, V noise R noise ba noise bw noise These are random Gaussian white noise;

[0080] The above equation (10) is integrated into a compact form:

[0081]

[0082] in, Let represent the prior error state quantity at time t. F is the posterior error state quantity of the previous time step. noise Let F be the noise transition matrix. x Here is the error state transition matrix, and its values ​​are respectively

[0083]

[0084] Where I represents the identity matrix;

[0085] The prior prediction covariance at time t is updated as follows:

[0086]

[0087] in, Let Q be the posterior covariance of the previous time step. n Here is the Gaussian white noise matrix, and its value is:

[0088]

[0089] Where, σ a σ ω The variances σ of the noise in acceleration and angular velocity measurements are respectively represented. ba σ bω These represent the variances of the random walk noise due to acceleration bias and the random walk noise due to angular velocity bias, respectively.

[0090] 2-3 State updates based on RTK data;

[0091] To eliminate errors caused by long-term integration, RTK (Real-Time Kinematic) data is used to update the prior estimates through observation; the observation model for RTK data is set as follows:

[0092] Y = HX t +v (15)

[0093] Where H is the observation state transition matrix, X t Given the current real-world state, where v is the sensor's observation noise, which follows a Gaussian distribution v ~ N{0,V}; and since only RTK observation of position information is used, H is:

[0094] H = [I 0 0 0 0] (16)

[0095] Then, the Kalman gain is obtained:

[0096]

[0097] The posterior state error is updated based on the Kalman gain:

[0098]

[0099] Finally, the posterior state covariance is updated:

[0100]

[0101] The nominal state is updated based on the state error, and finally the estimated pose of the patrol robot is obtained.

[0102] 2-4 Obtain the nearest path point of the patrol robot's estimated pose in the camouflage patrol semantic map:

[0103] Based on the estimated pose of the patrol robot obtained in step S2, the nearest path point N of the current patrol robot in the camouflage patrol semantic map is obtained according to formula (20). i :

[0104] N i ←min(d) (20)

[0105] Where d={d i ,i∈N*},d i The distance between a waypoint in the camouflaged patrol semantic map and the patrol robot's current pose; N* represents a positive integer;

[0106] Step S3: Patrol route generation and patrol mode decision

[0107] 3-1 Generate patrol path

[0108] All path points in the camouflage patrol semantic map are parsed into an undirected graph G = (N, E), where N is a path point and E is the distance between adjacent path points. Based on the undirected graph G, Dijkstra's algorithm is used to plan a patrol path way = (node ​​... s ,node e ), where node s To node e This indicates the start and end points of the path. The start point is the nearest path point of the current patrol robot (x) from step 3-1. c ,y c );

[0109] 3-2 Patrol Method Decision

[0110] The stored road semantic information of each path point in the patrol path is obtained. It is determined whether the semantic information of the current path point is a disguised and hidden road. If so, active disguise patrol is performed. If it is other semantic information, step S4 is performed. After reaching the next path point, step 3-3 is repeated. The active disguise patrol method is implemented using the active hiding method of a patrol robot when performing a tracking task, as described in Chinese patent CN202110671091.

[0111] Step S4, Patrol Motion Control

[0112] Based on the estimated pose of the patrol robot obtained in step S2, the angle α between the current pose of the patrol robot and the tracking target point is calculated according to formula (21):

[0113]

[0114] Where (x) g ,y g () indicates the location of the target point being tracked by the patrol robot along the patrol path. This represents the heading angle; the following derivation can be made using the law of cosines:

[0115]

[0116] Where L represents the distance between the patrol robot's current location and the target point it is tracking; R p Indicates the turning radius;

[0117] The turning radius can be obtained from equation (22) above.

[0118] Let the linear velocity of the patrol robot be v. s Then the angular velocity of the patrol robot is:

[0119]

[0120] The patrol path following function is implemented based on the real-time updated angular velocity w of the patrol robot.

Claims

1. A method for active camouflage patrol navigation of an intelligent robot in complex scenarios, characterized in that... The method includes the following steps: Step S1: Construct a camouflage patrol semantic map based on Open Street Map (OSM); Step S2: Estimate the pose of the patrol robot based on the state error Kalman filter algorithm and obtain the nearest path point; 2-1 Estimation of State Variable Initialization Assuming the {B} coordinate system, IMU coordinate system, and RTK coordinate system coincide, the estimated state variables of the patrol robot are: （3） in , and These represent the position, velocity, and attitude of the patrol robot's body coordinate system in the global coordinate system, respectively. and These represent the biases of IMU acceleration and angular velocity, respectively; the superscript T indicates transpose. The error state quantity is: （4） Taking the position of the patrol robot at the initial moment of its movement as the origin of the global coordinate system, P is initialized to... The initial velocity is zero, so V is initialized to... R is obtained based on the uniformity of the value of gravity g in different coordinate systems. Assuming that the z-axis of the world coordinate system is parallel to the direction of gravity, the measured value of IMU acceleration is regarded as the projection of g in the vehicle coordinate system. Therefore, the attitude R of the patrol robot at the initial moment can be deduced from the two sets of IMU accelerations. If the IMU does not have zero bias, the measured value of angular velocity when the IMU is stationary is 0. Therefore, the bias of angular velocity is taken as: （5） in, ωi represents the angular velocity measurement value when the IMU is stationary at time i; n represents the total number of frames for angular velocity measurement. The accelerometer bias is the measured static acceleration value a. i The difference between the gravity projection and the gravity projection: （6） in, The acceleration measurement value of the IMU at time i when it is stationary; At this point, the initialization of all estimated state variables is complete; 2-2 State prediction based on IMU data; First, calculate the median of acceleration and angular velocity based on the IMU data: （7） Where R t The update is achieved through the multiplication of the rotation matrices: （8） in, This is the rotation matrix obtained through the Rodriguez formula transformation; The angular increment within the state period. ; Indicates the time interval of the state cycle; Position and speed updated as follows: （9） Therefore, the error state is updated as follows: （10） in, For rotation update operations, V noise R noise ba noise bw noise These are random Gaussian white noise; The above equation (10) can be integrated into a compact form: （11） in, Let represent the prior error state quantity at time t. This represents the posterior error state quantity from the previous time step. Let F be the noise transition matrix. x Here is the error state transition matrix, and its values ​​are respectively ， （12） Where I represents the identity matrix; The prior prediction covariance at time t is updated as follows: （13） in, The posterior covariance of the previous time step. Here is the Gaussian white noise matrix, and its value is: （14） in, , These represent the variances of the noise in acceleration and angular velocity measurements, respectively. , These represent the variances of the random walk noise due to acceleration bias and the random walk noise due to angular velocity bias, respectively. 2-3 State updates based on RTK data; To eliminate errors caused by long-term integration, RTK (Real-Time Kinematic) data is used to update the prior estimates; the observation model for RTK data is set as follows: （15） Where H is the observation state transition matrix, Let v be the current true state, and v be the sensor's observation noise, which follows a Gaussian distribution. Since only RTK observation location information is used, H is: （16） Then, the Kalman gain is obtained: （17） The posterior state error is updated based on the Kalman gain: （18） Finally, the posterior state covariance is updated: （19） The nominal state is updated based on the state error, and finally the estimated pose of the patrol robot is obtained. 2-4 Obtain the nearest path point of the patrol robot's estimated pose in the camouflage patrol semantic map: Based on the estimated pose of the patrol robot obtained in step S2, the nearest path point N of the current patrol robot in the camouflage patrol semantic map is obtained according to formula (20). i : （20） in, d i The distance between waypoints in the camouflaged patrol semantic map and the current posture of the patrol robot; Represents positive integers; Step S3: Patrol route generation and patrol mode decision 3-1 Generate patrol path All waypoints in the camouflage patrol semantic map are parsed into an undirected graph. Where N represents the path points and E represents the distance between adjacent path points; based on the undirected graph G, Dijkstra's algorithm is used to plan a patrol path. , where node s To node e This indicates the start and end points of the path, with the start point being the nearest path point for the current patrol robot as described in step 3-1. ; 3-2 Patrol Method Decision Obtain the stored road semantic information of each path point in the patrol route, determine whether the semantic information of the current path point is a disguised and hidden road, if so, perform active disguised patrol, if it is other semantic information, perform step S4, and repeat step 3-3 after reaching the next path point. Step S4, Patrol Motion Control Based on the estimated pose of the patrol robot obtained in step S2, the angle between the current pose of the patrol robot and the tracking target point is calculated according to formula (21). : （21） in This indicates the location of the target point being tracked by the patrol robot along the patrol path. Indicates the heading angle; The following derivation can be made using the Law of Cosines: (22) Where L represents the distance between the patrol robot's current location and the target point it is tracking; Indicates the turning radius; The turning radius can be obtained from the above equation (22). ; Let the linear velocity of the patrol robot be v. s Then the angular velocity of the patrol robot is: （23） Based on the real-time updated angular velocity of the patrol robot Implement patrol path following function.

2. The method according to claim 1, characterized in that... Step S1 is as follows: 1-1 Since the path points in the Open Street Map (OSM) road data are described in terms of latitude and longitude, and the latitude and longitude are based on the spherical coordinate system, the latitude and longitude information of the Open Street Map (OSM) is converted into planar coordinates. 1-2 Because the distribution of path points in the OSM road data of the open street map is uneven and the distribution of path points in some roads is too sparse, the planar coordinate system map processed in step 1-1 is preprocessed. 1-3 Filtering of effective roads and acquisition of semantic information in a planar coordinate system map Since the map data provided by the Open Street Map (OSM) includes road data and regional landmarks, and these regional landmarks cannot provide prior information for navigation and occupy storage space, they are deleted as redundant data.

3. The method according to claim 2, characterized in that... Step 1-1 specifically involves: Calculate sampling points in Open Street Map OSM With reference point Planar distance: （1） Among them, R earth The radius of the Earth; The distances between all sampling points and the reference point are calculated sequentially to obtain the planar coordinate positions of all sampling points relative to the reference point, thus realizing the conversion of the spherical coordinate system map into a planar coordinate system map.

4. The method according to claim 3, characterized in that... Step 1-1 specifically involves: Let node1 and node2 be two path points on a road in a planar coordinate system map, and let d be the maximum tolerance distance between the two path points. iter_max Then the number of interpolation path points that need to be inserted between the two path points is ,in This represents the distance between path points node1 and node2; Path points generated by interpolation Location: （2） Where i is the i-th interpolation path point, ( , ) represents the planar coordinates of the path point node1, ( , ) represents the planar coordinates of the path point node2.

5. The method according to claim 4, characterized in that... Step 1-1 specifically involves: ① Distinguish based on the label information of Way elements in the map data of the plane coordinate system map. If the first-level label of a Way element is "highway", the road where the current Way element is located is considered a valid road. For other labels, delete the current Way element. Since the Way element contains multiple Node elements, it is also necessary to clear the Nodes contained in the current Way element. To ensure the accessibility of the selected paths and the necessity of patrolling, the remaining Way elements are further filtered, retaining the paths with the secondary labels "unclassified" and "path" and constructing the final disguised patrol map. ② Based on the secondary labels "unclassified" and "path" respectively mapped to the semantic information "NorimalRoad" and "CamouflageRoad" of the disguised patrol map, the disguised patrol semantic map is constructed.

6. A robot active camouflage patrol navigation system implementing the method of any one of claims 1-5, characterized in that... include: The Disguise Patrol Semantic Map Building Module is used to build patrol semantic maps based on the Open Street Map (OSM). The patrol robot pose estimation module is used to estimate the pose of the patrol robot based on the state error Kalman filter algorithm, and to obtain the nearest path point in the camouflage patrol semantic map based on the estimated pose of the patrol robot. The patrol path generation module is used to obtain the patrol path in the disguised patrol semantic map based on the nearest path point and the patrol target point; The patrol mode decision module is used to decide the patrol mode based on the road semantic information stored at each path point in the patrol route; The patrol motion control module enables the patrol robot to follow the patrol path based on its real-time updated angular velocity.

7. A computer-readable storage medium having a computer program stored thereon, which, when executed in a computer, causes the computer to perform the method of any one of claims 1-5.

8. A robot, characterized in that, The method includes a processor and a memory, the memory storing machine-executable instructions that can be executed by the processor, the processor executing the machine-executable instructions to implement the method of any one of claims 1-5.

Images