CN106406338B - Autonomous navigation device and method of omnidirectional mobile robot based on laser range finder - Google Patents

Abstract

The invention relates to the technical field of autonomous navigation of mobile robots, in particular to an autonomous navigation device of an omnidirectional mobile robot based on a laser range finder and a method thereof. The utility model provides an autonomous navigation device of omnidirectional mobile robot based on laser rangefinder, includes the encoder of measuring mobile robot wheel's travel distance, measures mobile robot's rotation angular velocity and acceleration's inertial measurement unit, and encoder and inertial measurement unit connect the controller respectively, still include laser rangefinder, and the host computer is connected to the laser rangefinder, and the host computer is connected the controller, and the execution unit is connected to the controller. The invention relates to a method for automatically avoiding obstacles in the travelling process, which is characterized in that a laser range finder is used for positioning a mobile robot, a two-dimensional plane map of an environment is established, and autonomous navigation is performed according to a target task.

Description

An autonomous navigation device of an omnidirectional mobile robot based on a laser range finder and its method

technical field

The present invention relates to the technical field of autonomous navigation of mobile robots, and more particularly, relates to an autonomous navigation device and method of an omnidirectional mobile robot based on a laser range finder.

Background technique

Intelligent mobile robots can move autonomously in unknown or partially unknown environments, and have functions such as environmental perception, behavior planning, dynamic decision-making, and executive decision-making. Robots usually need to solve three problems in the process of autonomous movement, namely, the positioning problem of the robot, the task planning problem of the robot and the behavior planning problem of the robot. Due to the lack of prior map information in the unknown environment, mobile robots need to obtain surrounding environmental information through sensors such as vehicle-mounted laser rangefinders, monocular, and binocular cameras to construct an environmental map and estimate the current position of the robot, that is, synchronous positioning and Mapping (Simultaneous Localization and Mapping, SLAM). Based on the constructed map information, the feasible path of the robot is planned according to the target task; the robot needs accurate positioning information in the process of traveling along the planned path, and realizes dynamic behavior decision-making based on real-time location information and environmental information to complete the goal Task.

SLAM is divided into two aspects of positioning and mapping, and is the core technology for mobile robots to realize autonomous navigation in unknown environments. The traditional dead reckoning method uses information such as the on-board odometer to locate the robot, but the odometer has accumulated errors, and its positioning accuracy decreases with the movement of the robot. When the environment map is known, the observed environment information can be compared with the known map information, so as to correct the pose estimation of the robot in real time and reduce the cumulative error of positioning. SLAM considers the situation where the environmental information is unknown. On the one hand, the pose estimation of the robot requires prior map information. On the other hand, the establishment of the map requires accurate robot pose information. Positioning and map building complement and influence each other. , accurate positioning provides accurate maps, and accurate maps provide accurate positioning. At present, SLAM is mainly based on Kalman filter and particle filter-based SLAM methods, but with the movement of the robot and the expansion of the map, the Kalman filter needs to maintain a huge state variable matrix, and the particle number of the particle filter also needs to be Therefore, the computational complexity of the SLAM algorithm based on Kalman filter and particle filter increases with the movement of the robot. At the same time, when both the kinematics model and the observation model of the robot are nonlinear, especially when the accurate observation model cannot be obtained, the accuracy of the SLAM algorithm based on Kalman filter and particle filter will be affected. The present invention considers the SLAM algorithm based on the iterative closest point algorithm (ICP). Due to the limited number of point clouds obtained by the laser range finder, the calculation overhead of using the point matching iterative calculation method is fixed. Compared with the SLAM algorithm based on Kalman filter and particle filter SLAM algorithm, ICPSLAM algorithm has reduced computational complexity and better real-time performance.

Global path planning algorithms mainly include methods such as fuzzy planning algorithm, ant colony optimization algorithm, particle swarm optimization algorithm, A* algorithm, etc. These methods usually require exact modeling and description of obstacles in a certain space. In addition, when the environment tends to be complex or dynamically changing, how to make the robot's path globally optimal or suboptimal, and how to avoid oscillation and deadlock of the planned path are still problems to be solved. The Rapidly Extended Random Tree (RRT) algorithm is a sampling-based single-query motion planning method. Through random sampling points in the state space, the search is directed to a blank area, so as to find a feasible path from the starting point to the target point, which is suitable for complex environments and changes. Scenario path planning.

At present, common obstacle avoidance algorithms include visual map method, artificial potential field method, and vector field histogram method. The present invention considers using the obstacle avoidance algorithm based on the obstacle area. When the expected speed of the mobile robot falls in the established obstacle area, the expected speed is corrected, and the obstacle is avoided through the minimum correction amount while ensuring the progress towards the target point.

Contents of the invention

In order to overcome at least one of the above-mentioned defects in the prior art, the present invention provides an autonomous navigation device and method for an omni-directional mobile robot based on a laser range finder. The laser range finder is used to locate the mobile robot, and at the same time establish A two-dimensional planar map of the environment, and autonomous navigation according to the target task, and a method that can autonomously avoid obstacles during the travel process.

In order to solve the above-mentioned technical problems, the technical solution adopted in the present invention is: an autonomous navigation device for an omnidirectional mobile robot based on a laser rangefinder, which includes an encoder for measuring the moving distance of the wheels of the mobile robot, and an encoder for measuring the rotation of the mobile robot. The inertial measurement unit for angular velocity and acceleration, the encoder and the inertial measurement unit are respectively connected to the controller, and a laser range finder is also included. The laser range finder is connected to the host computer, the host computer is connected to the controller, and the controller is connected to the execution unit.

The present invention uses a three-wheeled omnidirectional mobile robot as a robot motion platform, uses a vehicle-mounted encoder and an inertial measurement unit to obtain position information and attitude information (referred to as position and posture information) of the mobile robot, and simultaneously uses a vehicle-mounted laser range finder to obtain all directions of the mobile robot. Point cloud information of the nearest obstacle. The iterative closest point algorithm (ICP) is used to match the robot's current observation data with the previous observation data to correct the pose estimation of the mobile robot and simultaneously build a planar map of the environment. When the environment map is known, set the target task point, use the rapid search random tree algorithm (RRT) to plan the feasible path, and control the navigation of the mobile robot. In the process of autonomous navigation, the robot detects obstacles based on real-time laser data, performs local path planning, and realizes autonomous obstacle avoidance.

In the present invention, the vehicle-mounted encoder measures the moving distance of the wheels of the mobile robot, the inertial measurement unit measures the rotational angular velocity and acceleration of the mobile robot, and the vehicle-mounted embedded controller acquires the measured values of the encoder and the inertial measurement unit, combined with the kinematics model of the mobile robot Calculate its position and orientation information and send it to the host computer system; the host computer system simultaneously obtains the laser point cloud information of the laser rangefinder, calculates the expected value of the mobile robot's motion speed (including translation speed and rotation speed) through algorithm calculation, and sends the motion The speed expectation value is sent to the embedded controller on the vehicle; the embedded controller inversely solves the movement speed of the wheel through the kinematics model of the mobile robot, and then applies the PID algorithm to perform closed-loop control on the speed of the wheel.

Further, the chassis of the mobile robot is provided with three omnidirectional wheels. Preferably, the three omnidirectional wheels are distributed in an equilateral triangle.

Utilize the control method of the autonomous navigation device of the omnidirectional mobile robot based on the described laser range finder, wherein, comprise the following steps:

S1. Simultaneous positioning and map construction;

S2. Path planning;

S3. Position tracking and autonomous obstacle avoidance.

This device includes three main technical modules: synchronous positioning and mapping, path planning and autonomous obstacle avoidance. Synchronous positioning and mapping adopt the ICP-SLAM algorithm. First, the odometer and inertial sensor of the mobile robot are used, combined with the kinematics model of the robot to estimate the pose information of the mobile robot, and then the ICP algorithm is used to analyze the laser point cloud information and map information. Make a comparison, correct the pose, and update the map information. The global path planning module is to set the target position when the map and the initial position are known, and use the RRT algorithm to find an optimized feasible path under the premise of ensuring that the robot does not collide with obstacles such as walls. The autonomous obstacle avoidance module uses the ICP algorithm to obtain accurate positioning information, and calculates the expected movement speed according to the planned global path. At the same time, it uses the laser point cloud information to judge obstacles in the forward direction, and calculates the expected robot movement speed through local path planning. Dynamic correction to realize the robot's autonomous obstacle avoidance during travel.

The present invention proposes a relatively complete solution to the problem of autonomous navigation of a mobile robot based on a laser range finder in an unknown indoor environment, specifically including simultaneous positioning and mapping, path planning, position tracking and obstacle avoidance algorithms. First, the position and orientation information of the mobile robot is obtained through the encoder and the inertial measurement unit, the environmental information is obtained by the laser rangefinder, and the ICP-SLAM algorithm is used for synchronous positioning and mapping. Then use the RRT algorithm to plan the global path according to the target task, and design the obstacle avoidance strategy according to the real-time position and environmental information of the mobile robot, and make local path planning.

Compared with the prior art, the beneficial effect is that the present invention designs a global ICP algorithm that matches the currently acquired laser point cloud information with the constructed map, and a local ICP algorithm that matches the current and previous laser point cloud information. CP algorithm, and integrated global ICP algorithm and local ICP algorithm to correct the pose information of the mobile robot, which improves the accuracy of pose estimation and reduces the failure of matching.

A position tracking algorithm including two parts, the inner loop and the outer loop, is designed. The inner loop uses the encoder and the inertial measurement unit to estimate the pose information of the mobile robot; in the outer loop, the estimated pose information of the inner loop is used , and combine the laser point cloud and the known map to further correct the pose information of the robot. The position tracking strategy based on the inner and outer rings ensures accurate positioning while improving the real-time performance of the system and avoiding control delays caused by time-consuming calculations.

According to the four different situations of the distance between the mobile robot and the obstacle, the corresponding obstacle avoidance strategies are designed respectively, so that the mobile robot can move forward along the shortest path as far as possible while avoiding the obstacle.

Description of drawings

Figure 1 is a block diagram of the mobile robot system.

Figure 2 is a schematic diagram of the kinematics model of the three-wheeled omnidirectional mobile robot.

Figure 3 is a flowchart of the ICP-SLAM algorithm.

Figure 4 is a flow chart of position tracking and autonomous obstacle avoidance algorithm.

Figure 5 is a schematic diagram of the obstacle avoidance algorithm.

Detailed ways

The accompanying drawings are for illustrative purposes only, and should not be construed as limitations on this patent; in order to better illustrate this embodiment, certain components in the accompanying drawings will be omitted, enlarged or reduced, and do not represent the size of the actual product; for those skilled in the art It is understandable that some well-known structures and descriptions thereof may be omitted in the drawings. The positional relationship described in the drawings is for illustrative purposes only, and should not be construed as a limitation on this patent.

As shown in Figure 1, an autonomous navigation device for an omnidirectional mobile robot based on a laser rangefinder, including an encoder for measuring the moving distance of the wheels of the mobile robot, an inertial measurement unit for measuring the rotational angular velocity and acceleration of the mobile robot, The encoder and the inertial measurement unit are respectively connected to the controller, and a laser range finder is also included. The laser range finder is connected to the upper computer, the upper computer is connected to the controller, and the controller is connected to the execution unit.

In this embodiment, the vehicle-mounted encoder measures the moving distance of the wheels of the mobile robot, and the inertial measurement unit measures the rotational angular velocity and acceleration of the mobile robot. The model calculates its pose information and sends it to the host computer system; the host computer system obtains the laser point cloud information of the laser rangefinder at the same time, calculates the expected value of the mobile robot's motion speed (including translation speed and rotation speed) through algorithm calculation, and sends The expected value of the movement speed is sent to the on-board embedded controller; the embedded controller inversely solves the movement speed of the wheel through the kinematics model of the mobile robot, and then applies the PID algorithm to perform closed-loop control on the speed of the wheel.

As shown in Figure 2, the chassis of the mobile robot is composed of three omnidirectional wheels distributed in an equilateral triangle. By controlling the speed of the three omnidirectional wheels, the omnidirectional mobile robot can achieve 360° translation and rotation on the plane. The motion of an omnidirectional mobile robot is divided into two parts: rotation and translation. Let the speeds of the three wheels be (v ₁ , v ₂ , v ₃ ), and the moving speeds of the mobile robot body coordinate system along the x-axis and y-axis be (v _x , v _y ). In the case of only translation without rotation, the kinematic equation of the omnidirectional mobile robot is:

When there is only rotation and no translation, the angular velocity of rotation is w, then:

v ₁ =v ₂ =v ₃ =L*w. (2)

When there is rotation and translation at the same time, the speed of each omnidirectional wheel is the superposition of the speed of translation and rotation. If the moving speed of each omni-directional wheel is known, the translation speed and rotation speed of the mobile robot can be deduced. Due to the strong coupling of translation and rotation, it is necessary to make the robot not perform translation and rotation at the same time during the control process, so as to improve the accuracy of the pose estimation of the mobile robot.

The synchronous positioning and map building module is shown in Figure 3. According to the onboard odometer and inertial sensor, the movement information of the mobile robot is obtained, and the pose information of the mobile robot is estimated in combination with the kinematics model. Obtain the point cloud information around the estimated position in the current existing map as a virtual model point set, and use the real point cloud information of the current environment obtained by the laser rangefinder as a data point set, and perform ICP algorithm matching between the two , to get the global correction value of the pose information. On the other hand, the local correction value of the pose information is obtained by using the laser point cloud information acquired last time and the currently acquired point cloud information for ICP matching. According to the matching error value, the two sets of correction values are weighted to obtain the final corrected pose information. At the same time, update the map information and add the current laser observation value.

The iterative closest point algorithm is the core algorithm of the synchronous positioning and map building module. It finds the closest point between the data point set and the model point set through an iterative cycle to form a matching pair, and solves the rotation matrix R of the data point set relative to the model point set _k and the translation vector T _k such that the similarity measure S _LTF is minimized in each iteration. The classification of ICP and its various improvements can be classified into the following steps: 1. Select a subset of data point set P and model point set M. 2. Find matching point-set pairs. 3 Assign weights to each matching pair. 4. Eliminate certain matching pairs. 5 Find the geometric transformation that minimizes the similarity measure S _LTF . Specific steps are as follows:

1: Initialize the rotation matrix R ₀ and the translation vector T ₀ , which are randomly selected or obtained through other conditions.

2: For each point of the data point set P, find the nearest point in the model point set to form a matching pair, and remove the wrong matching pair at the same time to form a matching point set pair:

N＝{(i,j)|x _i ∈P,y _j ∈M,y _j ＝argmin(||(R _k - ₁ x _i +T _k - ₁ -y _j )|| ² )}. (3 )

The sum of squared matching distances is

3: Find the optimal geometric transformation (that is, the rotation matrix R _k and the translation vector T _k ) to minimize the sum of squared distances between the transformed data point set P' and the model point set M (ie, the similarity measure):

4: If the stop condition is satisfied or the number of iterations reaches the upper limit, the matching ends, otherwise, return to step 2. There are two stopping conditions of the ICP algorithm: (1) the standard deviation e=S _LTS /|N _p | is small enough; (2) the change of the standard deviation |e-e'|/e between two times before and after is small enough.

In step 2, the methods for finding the nearest point are commonly used based on the k-d tree and the nearest point search algorithm based on Delaunary triangulation. Experimental research results show that the k-d tree is suitable for point search of high-dimensional point sets, and the algorithm based on Delaunay triangulation is more suitable for low-dimensional point sets, so the present invention considers using the nearest point search algorithm based on Delaunay triangulation. The algorithm divides the set of scattered points into uneven triangular grids, and the grids meet two basic criteria. One is the empty circle feature, that is, there will be no Other points exist. The second is to maximize the minimum angle property, that is, in the possible triangulations formed by the scatter set, the minimum angle of the triangle formed by the Delaunay triangulation is the largest. In the ICP algorithm, firstly, the model point set M is delaunary triangulated, and then the triangulation search strategy is used to find the closest point of the data point set P in the model point set as the corresponding point to form a matching pair.

Ideally, the data point set P and the model point set M can be completely matched. If a point x _p in the data point set finds the corresponding point y _pm in the model point set M, then the corresponding point in the model point set y _pm found in the data point set It should also be x _p . This can be used as a criterion for judging matching pairs. Let the corresponding point of P _x1 in the data point set in the model point set be M _y1 , the corresponding point of M _y1 in the model point set in the data point set be P _x2 , and the corresponding point of P _x2 be M _y2 . When the following relationship is satisfied, we believe that the search for the corresponding point is correct, otherwise the corresponding relationship is eliminated.

where ε is a constant related to the standard deviation.

Common methods for finding the best geometric transformation in step 3 include SVD singular value decomposition, quaternion, orthogonal matrix, double quaternion method, etc. This scheme adopts the method of SVD decomposition.

In the process of synchronous positioning and mapping, the estimated pose information of the mobile robot is used as the initial estimate of the ICP algorithm. There is only a small error between the estimated value and the true value, which can greatly reduce the number of iterations of the ICP algorithm and improve the convergence speed. At the same time, the point cloud information around the estimated mobile robot position on the existing map is used as the model point set to further reduce the size of the model point set and improve the calculation speed of the ICP algorithm.

When the environment map and the initial position P _s are known, the target position P _e is set, and the global path planning is carried out by using the rapid expansion random tree (RRT) algorithm. The RRT algorithm uses an initial point in the state space as the root node, gradually increases the leaf nodes through random sampling and expansion, and finally generates a random expansion tree. When the leaf nodes of the random tree contain the target point or points in the target area, a path composed of leaf nodes of the random tree from the initial point to the target point is the planned path. The planned path is given in the form of a series of discrete nodes, there are no obstacles between adjacent nodes, and the target location point can be directly reached according to the planned path. Considering that in the actual environment, due to the inertia of the mobile robot and the existence of unknown obstacles, the mobile robot cannot strictly follow the planned path. In the planned path given in the form of nodes, the mobile robot can use each node as a sub-movement target, and perform local path planning according to the actual situation, reducing the need to re-plan the global path due to deviation from the trajectory or unknown obstacles. , which improves the robustness of the navigation process.

Considering the size of the mobile robot, the environment map is expanded before using the RRT algorithm to find the path. The expanded area is called the collision area S, so that the volume of the mobile robot is transferred to the obstacle. In the process of RRT algorithm, the mobile robot is treated as a particle. When the particle representing the mobile robot is located in the collision area, in the actual environment, the mobile robot has collided with the obstacle.

The RRT algorithm assumes that the search tree is T, the number of current nodes is N, its nodes are ti _j (t _j ), j is the number of the current node, and i is the number of its parent node. Taking the initial position as the root node t ₀₁ of the search tree, the line segment formed by two points on the map is L(p ₁ ,p ₂ ). Generate a random point p in the blank area of the map (that is, the mobile robot does not collide with obstacles when it is at a random point). Find the nearest directly connected node p _k following the random point in the existing random tree, that is, there is no obstacle blocking and no collision area between the node and the random point.

Take this node as the parent node, expand the search tree along the direction of the connection between the parent node and the random point, and generate a new child node t _k(N+1) , and the distance between the child node and the parent node is the step size s.

t _k(N+1) ＝{t|t∈L(t _k ,p),||tt _k ||＝s}. (9)

Continuously generate random points to expand the random tree until the child node of the random tree reaches the target point, and backtracking from the node and its parent node is the found path path=(p ₁ ,p ₂ ,...,p _n ), n is The number of nodes in the path.

The path composed of a series of nodes is obtained through the RRT algorithm, but it is often not the optimal path. There is a possibility of direct arrival of non-adjacent nodes, which can be further optimized. The optimization steps are as follows:

Starting from the initial node, judge whether node i and node i+2 are connected, that is, whether the mobile robot can directly move from node i to node i+2 in a straight line; if possible, remove node i+1, and then judge node i and node i+ 3; If they cannot be connected directly, the next step is to judge node i+1 and node i+3. and so on. Similarly, it can also be judged from the position of the target point at the same time.

Position tracking and autonomous obstacle avoidance, this module includes two parts: position tracking and obstacle avoidance. Position tracking obtains accurate pose information as the input of the obstacle avoidance algorithm. The obstacle avoidance algorithm calculates the expected moving speed based on the planned path and the current pose information, and corrects the expected speed by combining the laser sensor information. Finally, the corrected moving speed Send to the mobile robot. As shown in Figure 4.

The tracking and positioning process is similar to the positioning and mapping process. The odometer and inertial sensor are used to estimate the pose of the mobile robot, and the ICP algorithm is used to correct the pose information. Since the acquisition frequency of the laser information is only 10HZ at the highest, the calculation frequency of the ICP algorithm is 3HZ in the worst case. In order to ensure the real-time control of the robot, two tracking algorithms, the inner loop and the outer loop, are designed. The inner loop uses the pose information estimated by the dead reckoning method as the input of the obstacle avoidance algorithm, and the control frequency is 15Hz. The outer loop uses the ICP algorithm to correct the pose, and uses the corrected pose information as the input of the obstacle avoidance algorithm. Input, the minimum control frequency is 3HZ. As shown in Figure 3, the estimated pose of the mobile robot is obtained at time t, including the positions of the x-axis and y-axis in the world coordinates and the yaw angle of the mobile robot X _t =(x _t ,y _t ,α _t ) , at the same time, the acquired laser information is matched with the known map information by the ICP algorithm, and the estimated pose X _t is used as the input of the obstacle avoidance algorithm. At t+1, t+2,..., t+k-1, use the estimated pose information X _t+1 , X _t+2 ,..., X _t+k-1 at the current moment as obstacle avoidance input to the algorithm. The estimated pose of the mobile robot at time t+k is X _t+k . At this time, the ICP algorithm that starts computing at time t completes the matching and obtains the corrected pose at time t. Considering the sampling frequency of the laser rangefinder, we set the time interval between time t and time t+k to be greater than 0.1s. Therefore, the corrected pose of the mobile robot is:

The pose we will correct As the input of the obstacle avoidance algorithm, the laser information at time t+k is obtained at the same time, and the ICP algorithm is matched again according to the above steps to correct the pose information of the robot.

As shown in Figure 4, after obtaining the pose information of the mobile robot, the obstacle avoidance algorithm makes local path planning based on the real-time environment information. First determine the current moving target. Since the planned path is given in the form of nodes, each node is a sub-moving target. When it reaches the vicinity of the current target node or judges the position of the mobile robot, it can move to the next node along a straight line across the current target node. When , switch the target node, that is, the next path node will be the target node. Then calculate the expected direction of motion of the mobile robot based on the current position and the position of the target node, use the obstacle avoidance algorithm to correct the direction of motion, and send the calculated translation and rotation speeds to the mobile robot to control its motion.

The RRT algorithm plans a feasible path, but in the actual control process, due to the kinematics characteristics of the mobile robot and unknown obstacles in the environment, the mobile robot will not move completely according to the expected path, and needs to be based on the current real-time position. Attitude information, moving speed and environmental information are used for local path planning to avoid obstacles and approach the target at the same time.

The local path planning algorithm is shown in Figure 5. The safe distance is set to d. Let the current position be S, the target position be E, and the ideal moving direction be V _SE . The predicted position P in the next control cycle is predicted by using the above information. The obstacle avoidance process is divided into the following four situations:

(1) As shown in Figure 5(a), the point closest to the current position S among the obstacles is O1. If the distance between the two is less than d, it indicates that the mobile robot is about to collide. At this time, the position of the target point is not considered. The mobile robot moves away from the point O1.

(2) If SO1 is greater than d, then find the obstacle point O closest to the predicted position P, and establish the obstacle area VO with point O as the center and radius d as the shaded part in Figure 5(b). If the desired motion direction V _SE does not fall within this area, it means that the robot will not collide with this point, and the desired speed can be kept unchanged.

(3) As shown in Figure 5(c), if V _SE falls within the obstacle zone VO, the mobile robot will collide within a limited time, and the velocity direction needs to be corrected at this time. At this time, if the point P is outside the obstacle area, find the tangent point T between the point S and the circle VO, then the final movement direction will be the tangent direction:

(4) As shown in Figure 5(d), if the point P is located within the obstacle area VO, the correction amount of the velocity V _SE at this time is direction, the corrected velocity is:

Since the measurement range of the laser rangefinder used is 240°, when the final direction of the robot is far from the current direction, it should be rotated so that the mobile robot can fully measure the environmental information in the direction of progress and avoid the Collision occurs without reaching an obstacle. In the remaining cases, the mobile robot is controlled to do translational motion only, avoiding the increase of positioning error due to simultaneous rotation and translational motion.

Apparently, the above-mentioned embodiments of the present invention are only examples for clearly illustrating the present invention, rather than limiting the implementation of the present invention. For those of ordinary skill in the art, other changes or changes in different forms can be made on the basis of the above description. It is not necessary and impossible to exhaustively list all the implementation manners here. All modifications, equivalent replacements and improvements made within the spirit and principles of the present invention shall be included within the protection scope of the claims of the present invention.

Claims (4)

1. A control method based on the autonomous navigation device of the omnidirectional mobile robot of laser range finder, it is characterized in that, the autonomous navigation device comprises the encoder of the moving distance of measuring mobile robot wheel, the rotational angular velocity of measurement mobile robot and acceleration The inertial measurement unit, the encoder and the inertial measurement unit are respectively connected to the controller, and a laser rangefinder is also included. The laser rangefinder is connected to the host computer, the host computer is connected to the controller, and the controller is connected to the execution unit; The chassis of the mobile robot is equipped with three omnidirectional wheels; The three omnidirectional wheels are distributed in an equilateral triangle; Utilize the control method of the autonomous navigation device of the omnidirectional mobile robot based on the described laser range finder, comprise the following steps: S1. Simultaneous positioning and map construction; S2. Path planning; S3. Position tracking and autonomous obstacle avoidance; Wherein, in the step S3, the position tracking obtains accurate pose information as the input of the obstacle avoidance algorithm, and the obstacle avoidance algorithm calculates the expected moving speed according to the planned path and the current pose information, and combines the information of the laser sensor to calculate the expected speed Make corrections, and finally send the corrected motion speed to the mobile robot; The tracking and positioning process is similar to the positioning and mapping process. The odometer and inertial sensor are used to estimate the pose of the mobile robot, and the ICP algorithm is used to correct the pose information; since the highest frequency of laser information acquisition is only 10HZ, the ICP algorithm is at the worst The calculation frequency of the situation is 3HZ; in order to ensure the real-time control of the robot, the tracking algorithm of the inner loop and the outer loop is designed; the inner loop uses the pose information estimated by the dead reckoning method as the input of the obstacle avoidance algorithm, and the control The frequency is 15Hz, and the outer loop uses the ICP algorithm to correct the pose, and the corrected pose information is used as the input of the obstacle avoidance algorithm. The minimum control frequency is 3HZ; the estimated pose of the mobile robot is obtained at time t, including the world The position of the x-axis and y-axis under the coordinates and the yaw angle X _t of the mobile robot = (x _t , y _t , α _t ), and at the same time, the acquired laser information is matched with the information of the known map by the ICP algorithm. At this time, The estimated pose X _t is used as the input of the obstacle avoidance algorithm; at the time t+1, t+2,...,t+k-1, the estimated pose information X _t+1 , X _t+2 , ..., X _t+k-1 is used as the input of the obstacle avoidance algorithm; the estimated pose of the mobile robot at time t+k is X _t+k , and the ICP algorithm that starts to calculate at time t completes the matching, Get the corrected pose at time t Considering the sampling frequency of the laser rangefinder, the time interval between time t and time t+k is set to be greater than 0.1s; thus the corrected pose of the mobile robot is: The pose to be corrected As the input of the obstacle avoidance algorithm, the laser information at the time t+k is obtained at the same time, and the ICP algorithm is matched again according to the above steps to correct the pose information of the robot; After obtaining the pose information of the mobile robot, the obstacle avoidance algorithm makes local path planning based on the real-time environment information; firstly, the current moving target is determined. Since the planned path is given in the form of nodes, each node is a sub-moving target. When it reaches the vicinity of the current target node or judges that the position of the mobile robot can cross the current target node and move along a straight line to the next node, then switch the target node, that is, the next path node will be used as the target node; then calculate according to the current position and the position of the target node The expected direction of motion of the mobile robot is corrected using the obstacle avoidance algorithm, and the calculated translation and rotation speeds are sent to the mobile robot to control its motion; The RRT algorithm plans a feasible path, but in the actual control process, due to the kinematics characteristics of the mobile robot and the unknown obstacles in the environment, the mobile robot will not move completely according to the expected path, and needs to be based on the current real-time pose Information, moving speed and environmental information for local path planning to avoid obstacles and approach the target at the same time; The specific implementation of the local path planning algorithm is as follows: Set the safety distance as d; let the current position be S, the target position be E, and the ideal direction of motion be V _SE ; use the above information to predict the predicted position P in the next control cycle; divide the obstacle avoidance process into the following four situations : (1) Among the obstacles, the point closest to the current position S is O1. If the distance between the two is less than d, it indicates that the mobile robot is about to collide. At this time, regardless of the position of the target point, the mobile robot moves away from the point O1 sports; (2) If SO1 is greater than d, then find the obstacle point O closest to the predicted position P, use point O as the center of the circle, and establish the obstacle area VO with d as the radius. If the desired movement direction V _SE does not fall within this area, then Indicates that the robot will not collide with this point and can keep the desired speed unchanged; (3) If V _SE falls within the VO interval of the obstacle area, the mobile robot will collide within a limited time, and the speed direction needs to be corrected at this time; at this time, if the point P is outside the obstacle area, find the point S and the circle The tangent point T of VO, then the final motion direction will be the tangent direction: (4) If the point P is located within the obstacle area VO, the correction amount of the velocity V _SE at this time is direction, the corrected velocity is: Since the measurement range of the laser rangefinder used is 240°, when the final direction of the robot is far from the current direction, it should be rotated so that the mobile robot can fully measure the environmental information in the direction of progress and avoid the In the rest of the cases, the mobile robot is controlled to only do translational motions to avoid the increase in positioning errors due to simultaneous rotation and translational motions. 2. the control method of the autonomous navigation device of the omnidirectional mobile robot based on laser rangefinder according to claim 1, is characterized in that: in described step S1, obtains the moving of mobile robot according to encoder and inertial measurement unit Information, combined with the kinematics model to estimate the pose information of the mobile robot; obtain the point cloud information around the estimated position in the current existing map as a virtual model point set, and use the laser rangefinder to obtain the real environment of the current environment The point cloud information of the laser is used as a data point set, and the two are matched by the ICP algorithm to obtain the global correction value of the pose information; on the other hand, the last acquired laser point cloud information and the currently acquired point cloud information are used for ICP matching to obtain the pose information The local correction value; according to the matching error value, the two sets of correction values are weighted to obtain the final corrected pose information; at the same time, the map information is updated and the current laser observation value is added. 3. the control method of the autonomous navigation device of the omnidirectional mobile robot based on laser range finder according to claim 2, it is characterized in that: in described step S1, The iterative closest point algorithm is the core algorithm of the synchronous positioning and map building module. It finds the closest point between the data point set and the model point set through an iterative cycle to form a matching pair, and solves the rotation matrix R of the data point set relative to the model point set _k and the translation vector T _k , so that the similarity measure S _LTF is the smallest in each iteration; ICP and its various improved classifications can be classified into the following steps: 1. Select a subset of the data point set P and the model point set M ; 2. Find matching point set pairs; 3. Assign weights to each matching pair; 4. Eliminate some matching pairs; 5. Find geometric transformations to minimize the similarity measure S _LTF ; the specific steps are as follows: 1: Initialize the rotation matrix R ₀ and the translation vector T ₀ , which are randomly selected or obtained through other conditions; 2: For each point of the data point set P, find the nearest point in the model point set to form a matching pair, and remove the wrong matching pair at the same time to form a matching point set pair: N＝{(i,j)|x _i ∈P,y _j ∈M,y _j ＝argmin(||(R _k-1 x _i +T _k-1 -y _j )|| ² )}. (3 ) The sum of squared matching distances is 3: Find the optimal geometric transformation (that is, the rotation matrix R _k and the translation vector T _k ) to minimize the sum of squared distances between the transformed data point set P' and the model point set M (ie, the similarity measure): 4: If the stop condition is met or the number of iterations reaches the upper limit, the matching ends, otherwise return to step 2. There are two stop conditions for the ICP algorithm: (1) the standard deviation e=S _LTS /|N _p | is small enough; (2 ) The standard deviation change |e-e'|/e is small enough before and after two times; In step 2, the method of finding the nearest point uses the nearest point search algorithm based on Delaunary triangulation; this algorithm divides the set of scattered points into an uneven triangular grid, and the grid satisfies two basic criteria, one is an empty circle feature, that is, there will be no other points within the circumscribed circle of any triangular mesh in the Delaunay triangular network; the second is to maximize the minimum angle feature, that is, in the triangulation that may be formed by the scattered point set, the Delaunay triangulation In the ICP algorithm, the model point set M is first subjected to Delaunary triangulation, and then the triangulation search strategy is used to find the nearest point of the data point set P in the model point set as the corresponding point to form a matching right; Ideally, the data point set P and the model point set M can be completely matched. If a point x _p in the data point set finds the corresponding point y _pm in the model point set M, then the corresponding point in the model point set y _pm found in the data point set It should also be x _p ; it can be used as a criterion for judging matching pairs; let the corresponding point of P _x1 in the data point set in the model point set be M _y1 , and the corresponding point of M _y1 in the model point set in the data point set be P _x2 , The corresponding point of P _x2 is M _y2 ; when the following relationship is satisfied, the search for the corresponding point is correct, otherwise the corresponding relationship is eliminated; where ε is a constant related to the standard deviation; The method of finding the best geometric transformation in step 3 adopts the method of SVD decomposition. 4. the control method of the autonomous navigation device of the omnidirectional mobile robot based on laser range finder according to claim 1, it is characterized in that: in described step S2, When the environment map and the initial position P _s are known, set the target position P _e , and use the rapid expansion random tree algorithm for global path planning; the RRT algorithm uses an initial point in the state space as the root node, through random sampling The way of expansion is to gradually increase the leaf nodes, and finally generate a random expansion tree; when the leaf nodes of the random tree contain the target point or the point in the target area, a line from the initial point to the target point is in the direction of the random tree The path composed of leaf nodes is the planned path; the planned path is given in the form of a series of discrete nodes, there are no obstacles between adjacent nodes, and the target location point can be directly reached according to the planned path; considering that in the actual environment, due to the movement Due to the robot's motion inertia and the existence of unknown obstacles, the mobile robot cannot strictly follow the planned path; in the planned path given in the form of nodes, the mobile robot can use each node as a sub-movement target, and perform local path according to the actual situation. Planning, reducing the need to re-plan the global path due to deviation from the trajectory or unknown obstacles, and improving the robustness of the navigation process; Considering the size of the mobile robot, the environment map is expanded before using the RRT algorithm to find the path. The expanded area is called the collision area S, so that the volume of the mobile robot is transferred to the obstacle; in the process of the RRT algorithm, the The mobile robot is regarded as particle processing. When the particle representing the mobile robot is located in the collision area, in the actual environment, the mobile robot has collided with the obstacle; The RRT algorithm assumes that the search tree is T, the number of current nodes is N, and its node is t _ij (t _j ), j is the number of the current node, and i is the number of its parent node; the initial position is taken as the root node t ₀₁ of the search tree, and the map The line segment formed by the above two points is L(p ₁ ,p ₂ ); generate a random point p in the blank area of the map (that is, the mobile robot does not collide with obstacles when it is at a random point); find the follower in the existing random tree The nearest directly connected node p _k , that is, there is no obstacle occlusion and no collision area between the node and the random point; Take this node as the parent node, expand the search tree along the direction of the connection between the parent node and the random point, and generate a new child node t _k(N+1) , and the distance between the child node and the parent node is the step size s; t _k(N+1) ＝{t|t∈L(t _k ,p),||tt _k ||＝s}. (9) Continuously generate random points to expand the random tree until the child node of the random tree reaches the target point, and backtracking from the node and its parent node is the found path path=(p ₁ ,p ₂ ,...,p _n ), n is the number of nodes in the path; The path composed of a series of nodes is obtained through the RRT algorithm, but it is often not the optimal path. There is a possibility of direct arrival of non-adjacent nodes, which can be further optimized; the optimization steps are as follows: Starting from the initial node, judge whether node i and node i+2 are connected, that is, whether the mobile robot can directly move from node i to node i+2 in a straight line; if possible, remove node i+1, and then judge node i and node i+ 3; If they cannot be connected directly, the next step is to judge node i+1 and node i+3; and so on; similarly, the judgment can also start from the position of the target point at the same time.