Disclosure of Invention
The invention provides a safety control method of a mobile robot, aiming at improving the problems.
The invention is realized in such a way that a safety control method of a mobile robot specifically comprises the following steps:
s1, performing expansion processing on the planned path of the mobile robot according to the size of the vehicle body of the mobile robot;
s2, collecting the point cloud positions of surrounding obstacles in real time by using a laser radar, and removing isolated point clouds;
s3, calculating the distance between the effective point cloud and the outer frame of the mobile robot, and outputting a minimum distance value;
s4 minimum distance dis2And determining the moving speed of the mobile robot according to the position of the expanded planned path.
Further, the method for acquiring the distance between the effective point cloud and the mobile robot outer frame specifically comprises the following steps:
s31, converting the current pose of the mobile robot and the position of the non-isolated point cloud into a vehicle body center coordinate system O1The following steps of (1);
s32, calculating the coordinate system O of each non-isolated point cloud on the vehicle body1Determining a non-isolated point cloud, namely an effective point cloud, in front of the mobile robot based on the angle deviation delta theta of the angle deviation delta theta relative to the laser radar center P;
s33, obtaining each effective point cloud in the vehicle body coordinate system O1Distance dis of the lower part relative to the centre of the vehicle body1;
S34 distance dis1The distance of the effective point cloud relative to the outer frame of the mobile robot is calculated.
Furthermore, each effective point cloud is in the vehicle body coordinate system O1Distance dis of the lower part relative to the centre of the vehicle body1The calculation is made by the following formula:
wherein alpha is the included angle of the diagonal line of the vehicle body, W is the vehicle width of the mobile robot, L is the vehicle width of the mobile robot, and Delta theta is the corresponding non-isolated point cloud in the vehicle body coordinate system O1The angular deviation of the lower phase with respect to the center P of the laser radar.
Further, calculating the distance dis between each effective point cloud and the outer frame of the mobile robot2Calculated by the following formula:
wherein, (x'n,y′n) Representing the nth effective point cloud in the coordinate system O of the center of the vehicle body1Coordinates of (x'p,y′p) Coordinate system O for indicating laser radar center P in vehicle body center1Coordinates of lower, dis1In the vehicle body coordinate system O for the corresponding effective point cloud1The distance of the lower part relative to the center of the vehicle body.
Further, the method for determining the moving speed of the mobile robot is as follows:
s41, defining the corresponding distance dis of the safety zone, the deceleration zone, the stop zone and the interior of the vehicle body of the mobile robot based on the expanded planned path2An interval;
s42, detecting the minimum distance dis2Whether the vehicle is located in the deceleration zone, if the detection result is yes, the step S43 is executed, and if the detection result is no, the minimum distance dis is detected2And if the detection result is negative, the mobile robot is controlled to run at the speed v.
S43, calculating the minimum distance dis2Converting the corresponding point cloud coordinate into a grid map coordinate system, detecting whether the point cloud is positioned on the expanded planned path, and if so, controlling the mobile robot to rotate at the speed v1If the detection result is no, the mobile robot is controlled to move at the speed v2Running, speed v1Less than velocity v2Velocity v2Is less than the speed v, which is the normal driving speed.
Further, if the minimum distance dis2If the corresponding point cloud is located on the expanded planning path, the deceleration proportionality coefficient k is dist21.5, if the minimum distance dis2If the corresponding point cloud is not located on the expanded planning path, the deceleration proportionality coefficient k is dist2/0.5;
The traveling speed of the mobile robot is as follows: v' ═ k × v.
In the running process of the robot, different obstacle avoidance measurements are set according to the relation between information scanned by the robot and a vehicle body, after the radar pose is converted to the center of the vehicle body, the real distance between the point cloud and the vehicle body is calculated according to different directions, and the obstacle threatened to running is subjected to speed reduction processing or stop waiting obstacle avoidance, so that the problem of emergency stop of the vehicle body is effectively solved, and the running reliability and stability of the AGV are ensured.
Detailed Description
The following detailed description of the embodiments of the present invention will be given in order to provide those skilled in the art with a more complete, accurate and thorough understanding of the inventive concept and technical solutions of the present invention.
Fig. 1 is a flowchart of a safety control method for a mobile robot according to an embodiment of the present invention, where the method specifically includes the following steps:
s1, performing expansion processing on the planned path of the mobile robot according to the size of the vehicle body of the mobile robot;
in the global path planning, the mobile robot is regarded as a mass point, and in practical application, the mobile robot has a certain size, so that after the global path is searched, the planned path is expanded according to the size of the mobile robot, and for safety, the expansion size of the planned path needs to be larger than the vehicle width of the mobile robot, so that the ratio of the vehicle width of the mobile robot to the resolution of the grid map is used as the number of grids occupied by the expanded planned path. As shown in fig. 2, the attributes of each raster are [ x, y, value ], where (x, y) represents the center coordinates of the raster, value is the label value of the raster, and if the raster is occupied by the expanded planned path, value is a, and if the raster is not occupied by the expanded planned path, value is b.
S2, collecting the point cloud positions of surrounding obstacles in real time by using a laser radar, and removing isolated point clouds;
in the embodiment of the invention, a laser radar is arranged right in front of the mobile robot, so that the current pose information of the mobile robot and the point cloud information of surrounding obstacles can be acquired in real time; the method comprises the steps that a laser radar screens point clouds under a local window according to scanned information, the size of the local window is a circular window with a vehicle body as a center and a radius of 3m, isolated point clouds caused by point cloud jumping are eliminated, the scanning range of the laser radar is 0-2 pi, 8400 point clouds are arranged in each frame, position information of each point cloud is stored in a number table in a recorded mode, distance values of the current point cloud to be judged and the front point cloud and the rear point cloud in the number table are calculated, if the two distance values are larger than two resolutions of a map, the point clouds are isolated point clouds, the isolated point clouds are removed, and the rest point clouds are called non-isolated point clouds.
S3, calculating the distance between the effective point cloud and the outer frame of the mobile robot, and outputting a minimum distance value;
in the embodiment of the present invention, the method for calculating the distance between the effective point cloud and the outer frame of the mobile robot is specifically as follows:
s31, converting the current pose of the mobile robot and the position of the non-isolated point cloud into a vehicle body center coordinate system O1The following steps of (1);
pose of mobile robot, namely laser radar in world coordinate system O
2A lower position and attitude, if the center position of the laser radar is not coincident with the center position of the mobile robot, the mobile robot is positioned in a world coordinate system O
2The lower pose is converted into a vehicle body center coordinate system O
1Position and pose of the laser radar in a world coordinate system O
2The coordinates of (x) below
p,y
p,θ
p) Calibrating the central coordinate system O of the vehicle body
1And the world coordinate system O
2Has a relative position difference of
Coordinate system O of laser radar center P in vehicle body center line
1Coordinates of (x'
p,y′
p,θ′
p) The calculation formula is as follows:
the point cloud coordinate scanned by the laser radar is relative to the world coordinate system O2Then, based on the principle, the laser point cloud is distributed in the world coordinate system O2Coordinates of lower (x)n,yn) Converted into a vehicle body center coordinate system O1Coordinates of (x'n,y′n)。
S32, calculating the coordinate system O of each non-isolated point cloud on the vehicle body1Determining a non-isolated point cloud, namely an effective point cloud, in front of the mobile robot based on the angle deviation delta theta of the angle deviation delta theta relative to the laser radar center P;
in the embodiment of the present invention, the calculation formula of the angle deviation Δ θ is specifically as follows:
wherein, (x'n,y′n) The coordinate system O of the nth non-isolated point cloud in the center of the vehicle body1Coordinates of (x'p,y′p) Coordinate system O for indicating laser radar center P in vehicle body center1The coordinates of the following.
The point cloud at the rear of the vehicle body has no influence on the forward movement of the mobile robot, so that only the point cloud with the angle of | delta theta | < 90 degrees, namely the non-isolated point cloud positioned in front of the mobile robot, namely the effective point cloud in the invention, is taken.
S33, obtaining each effective point cloud in the vehicle body coordinate system O1Distance dis of the lower part relative to the centre of the vehicle body1;
Where α is an angle between diagonal lines of the vehicle body, that is, α is arctan (W/L), W is the vehicle width of the mobile robot, and L is the vehicle width of the mobile robot.
S34 distance dis1The distance of the effective point cloud relative to the outer frame of the mobile robot is calculated.
Calculating the distance dis between each effective point cloud and the mobile robot frame2Output distance dis2The calculation formula of (1) is specifically as follows;
s4 minimum distance dis2And determining the moving speed of the mobile robot according to the position of the expanded planned path.
In the embodiment of the present invention, the method for determining the moving speed of the mobile robot is specifically as follows:
s41, defining the corresponding distance dis of the safety zone, the deceleration zone, the stop zone and the interior of the vehicle body of the mobile robot based on the expanded planned path2An interval;
s42, detecting the minimum distance dis2Whether the vehicle is located in the deceleration zone, if the detection result is yes, the step S43 is executed, and if the detection result is no, the minimum distance dis is detected2And if the detection result is negative, the mobile robot is controlled to run at the speed v.
S43, calculating the minimum distance dis2Converting the corresponding point cloud coordinate into a grid map coordinate system, detecting whether the point cloud is positioned on the expanded planned path, and if so, controlling the mobile robot to rotate at the speed v1If the detection result is no, the mobile robot is controlled to move at the speed v2Running, speed v1Less than velocity v2Velocity v2And the speed v is smaller than the speed v, and the speed v is the normal running speed, namely the running speed when the obstacle is not avoided.
The traveling speed of the mobile robot will be described with reference to fig. 3, as the minimum distance dis2If the distance is more than 1.5m, the point cloud closest to the mobile robot is located in a safety area of the mobile robot, and the mobile robot runs at the speed v; if the minimum distancedis2If the distance is more than 0.5m and less than 1.5m, the point cloud closest to the mobile robot is positioned in a deceleration area of the mobile robot, the mobile robot is controlled to perform deceleration driving based on a deceleration proportionality coefficient k, and if the distance dis is the minimum distance2If the distance is more than 0 and less than 0.5m, the point cloud closest to the mobile robot is positioned in a stopping area of the mobile robot, and the mobile robot is controlled to stop; if the minimum distance dis2And if the point cloud is less than 0, the point cloud closest to the mobile robot is positioned in the exciting robot, and the mobile robot runs at the speed v.
In the embodiment of the invention, the running speed of the vehicle is determined based on the deceleration proportionality coefficient k, and the calculation formula is as follows:
wherein the dimensions of 1.5 and 0.5 are both the dimension of distance (m); the speed of the trolley is v' ═ k × v, and the dynamic deceleration of the trolley can be realized.
The invention has been described above with reference to the accompanying drawings, it is obvious that the invention is not limited to the specific implementation in the above-described manner, and it is within the scope of the invention to apply the inventive concept and solution to other applications without substantial modification.