CN106441275A - Method and device for updating planned path of robot - Google Patents

Abstract

The invention is applicable to the technical field of robots, and provides a method and a device for updating a planned path of a robot. The method comprises the following steps of obtaining a current carrier coordinate in real time in the movement process of the robot, converting the current carrier coordinate into a current global coordinate of the robot, calculating a movement speed estimation and a position estimation that the robot moves to a next site according to the current global coordinate and a current movement speed, obtaining a three-dimensional coordinate point a pixel point in a depth image representing an obstacle in a surrounding environment of the robot, transforming the three-dimensional coordinate point into a two-dimensional coordinate point, further obtaining the angular point of the obstacle in the surrounding environment according to the two-dimensional coordinate point, and finally updating the planned path of the robot in combination with the position estimation and the angular point of the obstacle. Thus, the accurate positioning and the avoidance of the obstacle are realized by utilizing speed and position information which is obtained by a sensor of the robot; the calculation in the path updating process is decreased through transforming the three-dimensional coordinate into the two-dimensional coordinate; the response speed of updating the path is improved.

Description

A method and device for updating a planned path of a robot

technical field

The invention belongs to the technical field of robots, and in particular relates to a method and device for updating a planned path of a robot.

Background technique

Existing robots usually use a Global Positioning System (Global Positioning System, abbreviated as GPS) for positioning, and use lasers to detect the surrounding environment, so as to realize autonomous path planning. GPS is the global positioning system dominated by the United States. It is a satellite navigation system with all-round, all-weather, full-time and high precision. The laser sensor uses the principle of laser ranging to record a large number of dense The three-dimensional coordinates, reflectivity and texture information of the points are used to reconstruct the three-dimensional model of the measured target and various map data such as lines, surfaces, and volumes.

However, there are some problems when using GPS to locate objects in a small area. On the one hand, GPS signals are easily blocked by objects such as buildings and mountains. On the other hand, it is difficult for GPS to obtain important geographical information such as the moving angle of the robot, and it takes a long time to calculate the surrounding environment using laser sensors, which makes When the moving range of the robot is relatively small, it is difficult to realize the real-time and precise positioning of the robot.

Contents of the invention

The purpose of the present invention is to provide a method and device for updating a robot's planned path, aiming at solving the problem that the existing technology cannot provide an effective method for updating the robot path, which makes it difficult to realize real-time and precise positioning of the robot when the robot moves in a small range The problem.

In one aspect, the present invention provides a method for updating a robot's planned path, the method comprising the following steps:

Obtaining the current carrier coordinates during the movement of the robot in real time, and transforming the current carrier coordinates into the current global coordinates of the robot;

calculating a moving speed estimate and a position estimate for the robot to move to a next location according to the current global coordinates and the current moving speed;

Obtaining three-dimensional coordinate points representing pixel points in the depth image of obstacles in the surrounding environment of the robot, converting the three-dimensional coordinate points into two-dimensional coordinate points to obtain the projected outline of obstacles in the surrounding environment of the robot;

Acquiring corner points of obstacles in the surrounding environment according to the two-dimensional coordinate points;

Updating the planned path of the robot according to the position estimate and the corner points of the obstacles.

In another aspect, the present invention provides a device for updating a robot's planned path, the device comprising:

A coordinate transformation unit, configured to acquire the current carrier coordinates during the movement of the robot in real time, and transform the current carrier coordinates into the current global coordinates of the robot;

A parameter acquisition unit, configured to calculate a moving speed estimate and a position estimate for the robot to move to a next location according to the current global coordinates and the current moving speed;

A coordinate conversion unit, configured to obtain three-dimensional coordinate points representing pixels in the depth image of obstacles in the surrounding environment of the robot, and convert the three-dimensional coordinate points into two-dimensional coordinate points, so as to obtain the coordinates of obstacles in the surrounding environment of the robot projection profile;

a corner point acquiring unit, configured to acquire corner points of obstacles in the surrounding environment according to the two-dimensional coordinate points; and

a path updating unit, configured to update the planned path of the robot according to the estimated position and the corner points of the obstacles.

The present invention acquires the current carrier coordinates during the robot's moving process in real time, transforms the current carrier coordinates into the current global coordinates of the robot, and calculates the moving speed estimation and position estimation of the robot moving to the next location according to the current global coordinates and the current moving speed. Obtain the three-dimensional coordinate points representing the pixel points in the depth image of obstacles in the surrounding environment of the robot, convert the three-dimensional coordinate points into two-dimensional coordinate points, so as to obtain the projected outline of obstacles in the surrounding environment of the robot, and then obtain the surrounding environment according to the two-dimensional coordinate points The corner points of the obstacles in the center, and finally update the robot's planned path by combining the position estimation and the corner points of the obstacles, so that the speed and position information obtained by the robot's own sensors can be used to realize the precise positioning and avoidance of obstacles, and through the three-dimensional coordinates to two The dimensional coordinates reduce the computational complexity in the path update process and improve the response speed of the path update.

Description of drawings

Fig. 1 is the implementation flowchart of the update method of the robot planning path provided by the embodiment of the present invention;

FIG. 2 is a schematic structural diagram of an updating device for a robot planning path provided by an embodiment of the present invention; and

Fig. 3 is a schematic diagram of a preferred structure of an updating device for a robot planned path provided by an embodiment of the present invention.

detailed description

In order to make the object, technical solution and advantages of the present invention clearer, the present invention will be further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described here are only used to explain the present invention, not to limit the present invention.

The specific realization of the present invention is described in detail below in conjunction with specific embodiment:

Figure 1 shows the implementation process of the method for updating the robot planning path provided by the embodiment of the present invention. For the convenience of description, only the parts related to the embodiment of the present invention are shown, and the details are as follows:

In step S101, the current carrier coordinates during the movement of the robot are obtained in real time, and the current carrier coordinates are transformed into the current global coordinates of the robot.

The embodiment of the present invention is applicable to a robot, and is used for autonomous path planning during the moving process of the robot. Corresponding sensors are configured on the robot, such as acceleration sensors, gyroscopes, etc., to obtain data related to the movement of the robot during the movement process, such as acceleration, deflection aspects of the movement process, and the like. The current carrier coordinates are the corresponding coordinates in the carrier coordinate system when the robot is at the current position, and the carrier coordinate system is the coordinate system fixed to the robot. As an example, the origin O _b of the coordinate system may be located at the center of mass of the robot. x _b points to the moving direction of the robot, y _b points to the right-hand side of the robot's forward direction, and z _b is perpendicular to the O _b x _b y _b plane and satisfies the right-hand rule. The current global coordinates are the corresponding coordinates in the global coordinate system when the robot is at the current position, and the global coordinate system is the global coordinate system set in a specific application (for example, a navigation application).

Therefore, preferably, the carrier coordinate system and the global coordinate system of the robot should be constructed in advance, thereby using the coordinates on the carrier coordinate system (robot) to locate the robot, without using a dedicated positioning system or global system to locate the robot, improving the The positioning accuracy of the robot when moving in a small range. When transforming the current carrier coordinates into the current global coordinates of the robot, the transformation relationship between the carrier coordinate system and the global coordinate system can be used for conversion. Specifically, the formula can be used Calculate the current global coordinates of the robot to obtain the precise global coordinates of the robot. in, is the current vector coordinates, Where ψ is the heading angle (the rotation angle of the robot around the z-axis), θ is the pitch angle (the rotation angle of the robot around the y-axis), γ is the roll angle (the rotation angle of the robot around the x-axis), and λ is a preset constant .

In step S102, according to the current global coordinates and the current moving speed, calculate the estimated moving speed and position of the robot moving to the next location.

In the embodiment of the present invention, the current moving speed of the robot needs to be obtained, so as to calculate the moving speed estimation and position estimation of the robot moving to the next location according to the current global coordinates and the current moving speed. That is, from the current position or coordinates, move along the path determined by the robot at the previous position at this speed to determine the next possible moving speed and position or coordinates of the robot.

Preferably, according to the formula

Compute the velocity estimate and position estimate for the robot moving to the next location, respectively. Among them, V _xW represents the speed of the robot in the direction of the x-axis in the global coordinate system, V _yW represents the speed in the direction of the y-axis in the global coordinate system, V _zW represents the speed in the direction of the z-axis in the global coordinate system, Δω _xw (j ) represents the acceleration change value between time point j and j-1 time point in the x-axis direction in the global coordinate system, and Δω _yw (j) represents the acceleration change value between time point j and j-1 time point in the y-axis direction in the global coordinate system Δω _zw (j) represents the acceleration change value between j time point and j-1 time point in the z-axis direction in the global coordinate system, k and k+1 represent the current time point and the next time point, and x _w represents the global coordinate The position in the x-axis direction of the global coordinate system, y _w represents the position in the y-axis direction of the global coordinate system, z _w represents the position in the z-axis direction of the global coordinate system, T ₂ represents the calculation cycle of navigation and positioning, and T ₁ is expressed as Sensor sampling period, n is a constant, and T ₂ =n*T ₁ . so, Then respectively represent the current time point (k time point) and the next time point (k+1 time point) the speed of the robot in each direction under the global coordinate system, Then respectively represent the coordinates of the robot in each direction under the global coordinate system at the current time point (k time point) and the next time point (k+1 time point).

In step S103, the three-dimensional coordinate points representing the pixel points in the depth image of the obstacle in the surrounding environment of the robot are obtained, and the three-dimensional coordinate points are converted into two-dimensional coordinate points to obtain the projected outline of the obstacle in the surrounding environment of the robot.

In the embodiment of the present invention, the Kinect depth sensor can be used to obtain the map or obstacle image on the moving path of the robot, and then convert the three-dimensional coordinate point representing the obstacle image into a two-dimensional coordinate point, and then convert the depth of the pixel point in the depth image Information, horizontal axis information, and vertical axis information are converted into point cloud data. According to the preset conversion relationship and point cloud data, calculate the three-dimensional coordinate points representing obstacles in the surrounding environment of the robot. According to the relationship between the three-dimensional coordinates and the two-dimensional coordinates The preset projection relationship converts the three-dimensional coordinate points into two-dimensional coordinate points, so as to obtain the two-dimensional projection outline of the obstacles in the surrounding environment of the robot. In this way, while the two-dimensional map or obstacle image converted from the depth image does not reduce the accuracy of the two-dimensional map or obstacle image, the complexity of subsequent calculations can be effectively reduced, and the update efficiency of the robot path is improved.

In step S104, corner points of obstacles in the surrounding environment are obtained according to the two-dimensional coordinate points.

In the embodiment of the present invention, when obtaining the corner points of obstacles in the surrounding environment, the correlation matrix of each two-dimensional coordinate point is calculated using a preset window function, and the Harris corner point of each two-dimensional coordinate point is calculated according to the correlation matrix Response, and then do a non-maximum value suppression for the Harris corner point response of each two-dimensional coordinate point, to find the maximum value point in the two-dimensional coordinate points in the preset window, when the two-dimensional coordinate point in the preset window The Harris corner point response of the coordinate point is greater than the preset threshold, and the Harris corner point response of the two-dimensional coordinate point is the local maximum value in the preset window, and the two-dimensional coordinate point is determined as the obstacle in the surrounding environment. Corner points, thereby identifying obstacles or obstacle boundaries in the obstacle image.

In step S105, the planned path of the robot is updated according to the position estimation and the corner points of the obstacles.

In the embodiment of the present invention, according to the next location where the robot may move to, combined with the identified obstacles, the path determined by the robot at the previous location can be further corrected and updated to prevent the robot from moving Collision with obstacles.

The embodiment of the present invention uses the speed and position information obtained by the robot's own sensor to realize the precise positioning and avoidance of obstacles, and reduces the computational complexity in the path update process by converting three-dimensional coordinates to two-dimensional coordinates, and improves the response speed of path update .

Fig. 2 shows the structure of the update device for the robot planning path provided by the embodiment of the present invention. For the convenience of description, only the parts related to the embodiment of the present invention are shown, including:

The coordinate transformation unit 21 is used to obtain the current carrier coordinates during the movement of the robot in real time, and transform the current carrier coordinates into the current global coordinates of the robot;

The parameter acquisition unit 22 is used to calculate the moving speed estimation and position estimation of the robot moving to the next location according to the current global coordinates and the current moving speed;

The coordinate conversion unit 23 is used to obtain three-dimensional coordinate points representing pixels in the depth image of obstacles in the surrounding environment of the robot, and convert the three-dimensional coordinate points into two-dimensional coordinate points to obtain the projected outline of obstacles in the surrounding environment of the robot;

A corner point obtaining unit 24, configured to obtain the corner points of obstacles in the surrounding environment according to the two-dimensional coordinate points; and

The path update unit 25 is configured to update the planned path of the robot according to the position estimation and the corner points of the obstacles.

As shown in Figure 3, preferably, the update device also includes:

The coordinate system construction unit 20 is used to pre-construct the carrier coordinate system and the global coordinate system of the robot.

Coordinate transformation unit 21 includes:

The coordinate calculation subunit 211 is used to calculate according to the formula Calculate the current global coordinates of the robot, where, is the current vector coordinates, Where ψ is the heading angle (the rotation angle of the robot around the z-axis), θ is the pitch angle (the rotation angle of the robot around the y-axis), γ is the roll angle (the rotation angle of the robot around the x-axis), and λ is a preset constant .

The parameter acquisition unit 22 includes:

The parameter calculation subunit 221 is used for according to the formula:

Compute the velocity estimate and position estimate for the robot moving to the next location, respectively. Among them, V _xW represents the speed of the robot in the direction of the x-axis in the global coordinate system, V _yW represents the speed in the direction of the y-axis in the global coordinate system, V _zW represents the speed in the direction of the z-axis in the global coordinate system, Δω _xw (j ) represents the acceleration change value between time point j and j-1 time point in the x-axis direction in the global coordinate system, and Δω _yw (j) represents the acceleration change value between time point j and j-1 time point in the y-axis direction in the global coordinate system Δω _zw (j) represents the acceleration change value between j time point and j-1 time point in the z-axis direction in the global coordinate system, k and k+1 represent the current time point and the next time point, and x _w represents the global coordinate The position in the x-axis direction of the global coordinate system, y _w represents the position in the y-axis direction of the global coordinate system, z _w represents the position in the z-axis direction of the global coordinate system, T ₂ represents the calculation cycle of navigation and positioning, and T ₁ is expressed as Sensor sampling period, n is a constant, and T ₂ =n*T ₁ . so, Then respectively represent the current time point (k time point) and the next time point (k+1 time point) the speed of the robot in each direction under the global coordinate system, Then respectively represent the coordinates of the robot in each direction under the global coordinate system at the current time point (k time point) and the next time point (k+1 time point).

In the embodiment of the present invention, each unit of the update device can be realized by corresponding hardware or software units, and each unit can be an independent software and hardware unit, or can be integrated into a software and hardware unit of a robot, which is not limited here. this invention.

The above descriptions are only preferred embodiments of the present invention, and are not intended to limit the present invention. Any modifications, equivalent replacements and improvements made within the spirit and principles of the present invention should be included in the protection of the present invention. within range.

Claims (10)

1. A method for updating a robot planning path, characterized in that the method comprises the steps of: Obtaining the current carrier coordinates during the movement of the robot in real time, and transforming the current carrier coordinates into the current global coordinates of the robot; calculating a moving speed estimate and a position estimate for the robot to move to a next location according to the current global coordinates and the current moving speed; Obtaining three-dimensional coordinate points representing pixel points in the depth image of obstacles in the surrounding environment of the robot, converting the three-dimensional coordinate points into two-dimensional coordinate points to obtain the projected outline of obstacles in the surrounding environment of the robot; Acquiring corner points of obstacles in the surrounding environment according to the two-dimensional coordinate points; Updating the planned path of the robot according to the position estimate and the corner points of the obstacles. 2. The method according to claim 1, wherein, before the step of obtaining the current carrier coordinates in the moving process of the robot in real time, the method also includes: The carrier coordinate system and the global coordinate system of the robot are constructed in advance. 3. The method according to claim 2, wherein the step of transforming the current carrier coordinates into the current global coordinates of the robot comprises: According to the formula Calculate the current global coordinates of the robot, where, is the current vector coordinates, Where ψ is the heading angle, θ is the pitch angle, γ is the roll angle, and λ is a preset constant. 4. The method of claim 3, wherein the step of calculating the velocity estimate and position estimate for the robot moving to the next location comprises: According to the formula: Calculate the moving speed estimate and position estimate of the robot moving to the next location respectively, wherein, V x _{W represents the speed on the x-axis direction under the global coordinate system, V yW represents} the speed on the y-axis direction under the global coordinate system, and V _zW Indicates the velocity in the direction of the z-axis in the global coordinate system, Δω _xw (j) indicates the acceleration change value between time point j and j-1 time point in the x-axis direction in the global coordinate system, Δω _yw (j) indicates the acceleration change value in the global coordinate system The acceleration change value between j time point and j-1 time point in the y-axis direction, Δω _zw (j) represents the acceleration change value between j time point and j-1 time point in the z-axis direction in the global coordinate system, k, k+ 1 represents the current time point and the next time point, T ₂ represents the calculation cycle of navigation and positioning, T ₁ represents the sensor sampling cycle, T ₂ =n*T ₁ . 5. The method according to claim 1, characterized in that, the step of obtaining a three-dimensional coordinate point representing a pixel point in the depth image of an obstacle in the surrounding environment of the robot, and converting the three-dimensional coordinate point into a two-dimensional coordinate point, include: converting the depth information, horizontal axis information, and vertical axis information of pixels in the depth image into point cloud data; Calculating the three-dimensional coordinate points representing obstacles in the surrounding environment of the robot according to a preset conversion relationship and the point cloud data; The three-dimensional coordinate points are converted into two-dimensional coordinate points according to the preset projection relationship between the three-dimensional coordinates and the two-dimensional coordinates. 6. The method according to claim 1, wherein the step of obtaining corner points of obstacles in the surrounding environment according to the two-dimensional coordinate points comprises: Use the preset window function to calculate the correlation matrix of each two-dimensional coordinate point; Calculate the Harris corner response of each two-dimensional coordinate point according to the correlation matrix; Perform a non-maximum suppression on the Harris corner point response of each two-dimensional coordinate point to find the maximum value point among the two-dimensional coordinate points in the preset window; When the Harris corner point response of the two-dimensional coordinate point in the preset window is greater than the preset threshold value, and the Harris corner point response of the two-dimensional coordinate point is a local maximum value in the preset window, the The two-dimensional coordinate point is determined as a corner point of an obstacle in the surrounding environment. 7. A device for updating a robot planning path, characterized in that the device comprises: A coordinate transformation unit, configured to acquire the current carrier coordinates during the movement of the robot in real time, and transform the current carrier coordinates into the current global coordinates of the robot; A parameter acquisition unit, configured to calculate a moving speed estimate and a position estimate for the robot to move to a next location according to the current global coordinates and the current moving speed; A coordinate conversion unit, configured to obtain three-dimensional coordinate points representing pixels in the depth image of obstacles in the surrounding environment of the robot, and convert the three-dimensional coordinate points into two-dimensional coordinate points, so as to obtain the coordinates of obstacles in the surrounding environment of the robot projection profile; a corner point acquiring unit, configured to acquire corner points of obstacles in the surrounding environment according to the two-dimensional coordinate points; and a path updating unit, configured to update the planned path of the robot according to the estimated position and the corner points of the obstacles. 8. The device of claim 7, further comprising: The coordinate system construction unit is used to pre-construct the carrier coordinate system and the global coordinate system of the robot. 9. The device according to claim 8, wherein the coordinate transformation unit comprises: Coordinate Calculation Subunit for use according to the formula Calculate the current global coordinates of the robot, where, is the current vector coordinates, 10. The device according to claim 9, wherein the parameter obtaining unit comprises: Parameter calculation subunit for use according to the formula: A moving speed estimate and a position estimate for the robot moving to the next location are calculated separately.