Stair gradient angle determination method and device, electronic equipment and storage medium
By performing projection plane mapping and slope fitting processing on stair point cloud data, the problem of difficulty in accurately measuring the stair slope angle in the prior art is solved, and the high-precision perception of stairs by intelligent driving vehicles and the improvement of climbing capabilities.
Patent Information
- Application Number
- CN202411235089.X
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-09-04
- Publication Date
- 2024-12-06
- Estimated Expiration
- 2044-09-04
AI Technical Summary
The prior art is difficult to accurately determine the slope angle of the stairs, which affects the stair climbing ability of intelligent driving vehicles. Especially in the applications of millimeter wave radar, inertial measurement units and stereo vision cameras, there are measurement accuracy and noise problems.
By obtaining multiple point cloud data of the stairs, establishing the projection plane perpendicular to the plane where the stairs are located, mapping the point cloud data to the two-dimensional plane, and performing slope fitting processing to determine the slope angle of the stairs.
Accurate measurement of the stair slope angle is achieved, the intelligent driving vehicle's perception of stairs is improved, the measurement accuracy and noise problems in related technologies are avoided, and the vehicle's stair climbing ability is enhanced.
Smart Images

Figure CN119087456A_ABST
Abstract
Description
Technical Field
[0001] The present application relates to the field of vehicle control technology, and in particular to a method, device, electronic device and storage medium for determining the slope angle of stairs. Background Art
[0002] Slope prediction is one of the key technologies in advanced automotive technology and an important component of smart cars and autonomous driving technology. For example, accurate slope information is required in anti-lock braking systems (ABS), automatic transmission technology (AMT), adaptive cruise control system (ACC), direct shift transmission technology (DSG), hill assist system (HAC) and hill descent control system (HDC).
[0003] Stairs are a type of slope. Currently, there are several technical routes for determining the slope angle of stairs:
[0004] (1) Use millimeter-wave radar to detect stair parameters. The millimeter-wave radar point cloud is sparse and can only provide information in three dimensions: distance, direction, and speed. The height information is very limited and cannot obtain complete environmental information. The width of the stairs can be identified, but accurate height information cannot be obtained, so the slope information of the stairs cannot be processed and obtained; (2) Use an inertial measurement unit to detect stair parameters. Simply using an inertial measurement unit to obtain the pitch angle to infer the current slope angle of the stairs, but for the stair environment, the vehicle body will vibrate greatly during operation, resulting in large noise in the IMU measurement results, and its measurement precision and accuracy are low; (3) Use a stereo vision camera to detect stair parameters. Although the stereo vision camera can obtain three-dimensional environmental information, it has high requirements on the computing power of the control module, and in the case of dim light or obstruction, inaccurate detection may occur, resulting in the inability to correctly detect and identify the stair scene.
[0005] Therefore, how to determine the slope angle of the stairs to provide a stair climbing foundation for intelligent driving vehicles has become a problem that continues to need to be solved urgently. Summary of the invention
[0006] In view of this, the embodiments of the present application provide a method, device, electronic device and storage medium for determining the slope angle of stairs to solve the problem in the prior art that the slope angle of stairs cannot be accurately determined.
[0007] According to a first aspect of an embodiment of the present application, a method for determining the slope angle of stairs is provided, the method comprising: obtaining multiple point cloud data corresponding to the stairs, and establishing a projection plane according to the plane where the stairs are located, the projection plane being perpendicular to the plane where the stairs are located; mapping the multiple point cloud data to the projection plane, and obtaining the two-dimensional point cloud coordinates of each point cloud data after being mapped to the projection plane; performing slope fitting processing on the multiple two-dimensional point cloud coordinates to obtain the stair slope corresponding to the stairs, and determining the slope angle of the stairs according to the stair slope.
[0008] According to a second aspect of an embodiment of the present application, a device for determining the slope angle of stairs is provided, the device comprising: an acquisition module, for acquiring multiple point cloud data corresponding to the stairs, and establishing a projection plane according to the plane where the stairs are located, the projection plane being perpendicular to the plane where the stairs are located; a mapping module, for mapping multiple point cloud data to the projection plane, and obtaining the two-dimensional point cloud coordinates after each point cloud data is mapped to the projection plane; a calculation module, for performing slope fitting processing on the multiple two-dimensional point cloud coordinates to obtain the stair slope corresponding to the stairs, and determining the slope angle of the stairs according to the stair slope.
[0009] According to a third aspect of an embodiment of the present application, an electronic device is provided, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor implements the steps of the above method when executing the computer program.
[0010] According to a fourth aspect of an embodiment of the present application, a computer-readable storage medium is provided, which stores a computer program, and when the computer program is executed by a processor, the steps of the above method are implemented.
[0011] Compared with the prior art, the beneficial effects of the embodiments of the present application are as follows: the method in the embodiments of the present application obtains multiple point cloud data corresponding to the stairs, and establishes a projection plane according to the plane where the stairs are located, and the projection plane is perpendicular to the plane where the stairs are located; maps multiple point cloud data to the projection plane, and obtains the two-dimensional point cloud coordinates after each point cloud data is mapped to the projection plane; performs slope fitting processing on the multiple two-dimensional point cloud coordinates to obtain the stair slope corresponding to the stairs, and determines the slope angle of the stairs according to the slope of the stairs, wherein the three-dimensional point cloud data is converted into two-dimensional point cloud coordinates by mapping multiple three-dimensional point cloud coordinates to a projection plane perpendicular to the plane where the stairs are located. Since multiple two-dimensional point cloud coordinates can express the fitting straight line corresponding to the stairs, this example can obtain the slope corresponding to the straight line fitted by multiple two-dimensional point cloud coordinates by performing slope fitting processing on multiple two-dimensional point cloud coordinates, and subsequently accurately determines the slope angle of the stairs according to the slope of the stairs, avoiding the problem of being unable to accurately determine the slope angle of the stairs in the related art. BRIEF DESCRIPTION OF THE DRAWINGS
[0012] In order to more clearly illustrate the technical solutions in the embodiments of the present application, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative work.
[0013] Figure 1 It is a flow chart of a method for determining the slope angle of stairs provided in an embodiment of the present application;
[0014] Figure 2 This is a basic schematic diagram of a laser radar for identifying stairs provided in an embodiment of the present application;
[0015] Figure 3 It is a basic schematic diagram of mapping a two-dimensional point cloud coordinate to a projection plane provided by an embodiment of the present application;
[0016] Figure 4 It is a flowchart of another method for determining the slope angle of stairs provided in an embodiment of the present application;
[0017] Figure 5 It is a basic schematic diagram of an initial coordinate system of a laser radar provided in an embodiment of the present application;
[0018] Figure 6 1 is a flow chart of another method for determining the slope angle of stairs provided in an embodiment of the present application;
[0019] Figure 7 It is a flowchart of another method for determining the slope angle of stairs provided in an embodiment of the present application;
[0020] Figure 8 It is a flowchart of another method for determining the slope angle of stairs provided in an embodiment of the present application;
[0021] Fig. 9 It is a flowchart of another optional method for determining the slope angle of stairs provided in an embodiment of the present application;
[0022] Fig.10 is a flow chart of another optional method for determining the slope angle of stairs provided in an embodiment of the present application;
[0023] Fig.11 It is a basic structural schematic diagram of a vehicle provided in an embodiment of the present application;
[0024] Fig.12 It is a flowchart of another optional method for determining the slope angle of stairs provided in an embodiment of the present application;
[0025] Fig.13It is a structural schematic diagram of a device for determining the slope angle of stairs provided in an embodiment of the present application;
[0026] Fig.14 It is a structural schematic diagram of an electronic device provided in an embodiment of the present application. DETAILED DESCRIPTION
[0027] In the following description, specific details such as specific system structures, technologies, etc. are provided for the purpose of illustration rather than limitation, so as to provide a thorough understanding of the embodiments of the present application. However, it should be clear to those skilled in the art that the present application may also be implemented in other embodiments without these specific details. In other cases, detailed descriptions of well-known systems, devices, circuits, and methods are omitted to prevent unnecessary details from obstructing the description of the present application.
[0028] A method and device for determining the slope angle of stairs according to an embodiment of the present application will be described in detail below with reference to the accompanying drawings.
[0029] Figure 1 A method for determining the slope angle of a staircase provided in an embodiment of the present application is as follows: Figure 1 As shown, the method includes:
[0030] S101, obtaining a plurality of point cloud data corresponding to the stairs, and establishing a projection plane according to the plane where the stairs are located, wherein the projection plane is perpendicular to the plane where the stairs are located;
[0031] S102, mapping a plurality of point cloud data to a projection plane, and obtaining a two-dimensional point cloud coordinate after each point cloud data is mapped to the projection plane;
[0032] S103, performing slope fitting processing on the multiple two-dimensional point cloud coordinates to obtain the stair slope corresponding to the stairs, and determining the slope angle of the stairs according to the stair slope.
[0033] It can be understood that the method for determining the slope angle of the stairs provided in this example is applied to vehicles equipped with laser radars, and the above-mentioned vehicles are new energy vehicles using hybrid power, and the above-mentioned vehicles include vehicles with automatic driving or intelligent driving (including vehicles with manned functions (such as cars, buses, large buses, minibuses, etc.), vehicles with cargo functions (such as ordinary trucks, vans, trailers, closed trucks, tank trucks, flatbed trucks, container trucks, dump trucks, trucks with special structures), special vehicles (such as logistics distribution vehicles, automatic guided transport vehicles AGV, patrol cars, cranes, cranes, excavators, bulldozers, forklifts, rollers, loaders, off-road engineering vehicles, armored engineering vehicles, sewage treatment vehicles, sanitation vehicles, vacuum trucks, floor washing vehicles, sprinkler trucks, sweeping robots, food delivery robots, shopping guide robots, lawn mowers, golf carts, etc.), vehicles with entertainment functions (such as entertainment vehicles, amusement park automatic driving devices, balance cars, etc.), rescue vehicles (such as fire trucks, ambulances, power repair vehicles, engineering rescue vehicles, etc.)), etc. It can be understood that the method for determining the slope angle of the stairs provided in this example is also applicable to unmanned vehicles and wheeled / tracked / legged robots equipped with laser radar functions.
[0034] For better explanation, this example will be described later by applying the above-mentioned method for determining the slope angle of stairs to a vehicle equipped with a laser radar. For example, the laser radar is installed on the roof of the vehicle, and the laser radar is connected to the intelligent driving domain system on chip (System on Chip, SOC). After the intelligent driving domain SOC collects the point cloud data of the stairs through the laser radar, it determines the slope angle of the stairs based on the above-mentioned steps S101 and S103.
[0035] The method provided in this example first uses a laser radar to perform point cloud recognition on the stairs to obtain multiple point cloud data corresponding to the stairs. The point cloud data is three-dimensional data, and each point cloud data has (x, y, z) coordinates. In some examples, the above-mentioned laser radar is a mechanical rotating multi-line laser radar. The characteristic of the mechanical rotating multi-line laser radar is that the optical-mechanical structure is driven by a motor to rotate as a whole, and the transmitting system and the receiving system rotate in a macro sense. By continuously rotating the transmitter head, the laser changes from a "line" to a "surface", and multiple laser beams are arranged in the vertical direction to form multiple surfaces, so as to achieve the purpose of dynamic scanning and dynamic reception of information. A mechanical structure is used for 360° rotation scanning, and directional distributed scanning is used in the vertical direction. Both the transmitter and the receiver rotate with the scanning component, thereby realizing all-round environmental perception of the stairs and obtaining point cloud data distributed on the surface of the stairs.
[0036] In some examples, this example will also establish a projection plane based on the plane where the stairs are located, and the projection plane is perpendicular to the plane where the stairs are located; specifically, a plane can be represented by an equation in three-dimensional space, usually in the form of Ax+By+Cz+D=0, where A, B, and C are the components of the plane's normal vector (i.e., the vector perpendicular to the plane), and D is a constant used to adjust the position of the plane. This example uses a laser radar to obtain point cloud data distributed on the surface of the stairs. Each point cloud data has (x, y, z) coordinates, and then determines the plane where the stairs are located based on the above (x, y, z) coordinates.
[0037] Specifically, take the laser radar facing the stairs, and the laser radar takes the center of the laser radar itself as the center of the circle, and establish the radar coordinate system according to the right-hand rule as an example, assuming that the plane where the stairs are located is (Ax+By+Cz+D=0), for each point cloud data (x i ,y i ,z i ) is adjusted so that the x value of each point cloud data is the same, and then each point cloud data (x i ,y i ,z i ) calculates the distance to the plane (Ax+By+Cz+D=0) and finds the sum of the squares of these distances. Minimize this sum to get the values of A, B, C, and D, and then determine the plane where the stairs are located.
[0038] After determining the projection plane, in this example, after determining the plane where the stairs are located, a projection plane is established according to the plane where the stairs are located, and the projection plane is perpendicular to the plane where the stairs are located; for example, Figure 2 As shown, Figure 2 As shown in the figure, when the laser radar is facing the stairs (the projection plane formed by the X-axis and the Z-axis in the three-dimensional coordinate system corresponding to the laser radar is perpendicular to the plane where the stairs are located, and the Y-axis is parallel to the plane where the stairs are located, it is determined that the laser radar is facing the stairs), the steps of the stairs are identified, and a series of point cloud data are generated on the steps. The coordinates P of any point cloud data can be obtained through the PCL point cloud library algorithm origin (x, y, z). Since the laser radar is facing the stairs, the projection plane formed by the laser radar's X-axis and the X-axis is perpendicular to the plane where the stairs are located. Therefore, this example uses the laser radar's X-axis and Z-axis as reference axes to establish the projection plane.
[0039] After establishing the projection plane, this example will map multiple point cloud data to the projection plane, and obtain the 2D point cloud coordinates corresponding to each point cloud data. In the projection plane, each 2D point cloud coordinate corresponds to one point cloud data. The schematic diagram after mapping multiple point cloud data to the projection plane is as follows: Figure 3As shown in the figure, although point cloud noise will appear due to the laser radar measurement error and the existence of interference, after mapping multiple point cloud data to the projection plane, the point cloud data finally presented on the projection plane will converge to a certain straight line, that is, the point cloud data can be fitted into a straight line, where the projection plane has the horizontal direction as the X direction and the vertical direction as the Y direction. Obviously, the fitting straight line can be expressed as y=kx+b, where the x coordinate represents the laser radar point cloud data P origin The x data component of (x,y,z), the y coordinate represents the lidar point cloud data P origin The z data component of (x, y, z). The parameter k represents the slope of the fitted line, which actually means the slope of the stairs. The slope angle of the stairs can be accurately determined based on the slope.
[0040] Based on the above principle, this example performs slope fitting processing on multiple two-dimensional point cloud coordinates to obtain the slope corresponding to the straight line fitted by the multiple two-dimensional point cloud coordinates, and finally determines the slope angle of the stairs according to the slope of the stairs.
[0041] According to the technical solution provided in the embodiment of the present application, multiple point cloud data corresponding to the stairs are obtained, and a projection plane is established according to the plane where the stairs are located, and the projection plane is perpendicular to the plane where the stairs are located; the multiple point cloud data are mapped to the projection plane, and the two-dimensional point cloud coordinates after each point cloud data is mapped to the projection plane are obtained; the multiple two-dimensional point cloud coordinates are subjected to slope fitting processing to obtain the stair slope corresponding to the stairs, and the slope angle of the stairs is determined according to the slope of the stairs, wherein the three-dimensional point cloud data are converted into two-dimensional point cloud coordinates by mapping multiple three-dimensional point cloud coordinates to a projection plane perpendicular to the plane where the stairs are located. Since the multiple two-dimensional point cloud coordinates can express the fitting straight line corresponding to the stairs, this example can obtain the slope corresponding to the straight line fitted by the multiple two-dimensional point cloud coordinates by performing slope fitting processing on the multiple two-dimensional point cloud coordinates, and subsequently accurately determine the slope angle of the stairs according to the slope of the stairs, thereby avoiding the problem in the related art that the slope angle of the stairs cannot be accurately determined.
[0042] In some embodiments, Figure 4 As shown, a projection plane is established based on the plane where the stairs are located, including:
[0043] S401, obtaining an initial coordinate system of a laser radar, where the laser radar is used to perform point cloud recognition on stairs;
[0044] S402, determining the calibration axis and the reference axis from the initial coordinate system, and obtaining the positional relationship between the calibration axis and the plane where the stairs are located;
[0045] S403, if the calibration axis intersects with the plane where the stairs are located, adjusting the position of the laser radar according to the plane of the stairs so that the calibration axis is parallel to the plane where the stairs are located;
[0046] S404: construct a projection plane according to the adjusted reference axis of the laser radar.
[0047] Specifically, take the laser radar facing the stairs, and the laser radar uses the center of the laser radar itself as the center of the circle, and establishes the radar coordinate system according to the right-hand rule. At this time, the projection plane formed by the X-axis and Z-axis of the laser radar is perpendicular to the plane where the stairs are located. Therefore, the projection plane can be directly established with the X-axis and Z-axis of the laser radar as the reference axis. However, in the daily use of the vehicle, the position relationship between the laser radar and the stairs is often not a direct relationship. At this time, the projection plane formed by the X-axis and Z-axis of the laser radar is not perpendicular to the plane where the stairs are located. Therefore, the projection plane cannot be directly established with the X-axis and Z-axis of the laser radar as the reference axis.
[0048] For the above reasons, when the laser radar and the stairs are not directly opposite (that is, when the projection plane formed by the X-axis and Z-axis of the laser radar is not perpendicular to the plane where the stairs are located), this example will obtain the initial coordinate system of the laser radar. For example, the laser radar uses its own center as the center of the circle and establishes the initial coordinate system according to the right-hand rule, such as Figure 5 shown.
[0049] It can be understood that this example does not limit the laser radar to establish the initial coordinate system only in the above-mentioned manner, and relevant personnel can flexibly set the method for establishing the initial coordinate system of the laser radar according to actual needs.
[0050] After obtaining the initial coordinate system, this example determines the calibration axis and the reference axis from the initial coordinate system. Specifically, the initial coordinate system contains three coordinate axes, X, Y, and Z, of which one is the calibration axis and the other two are reference axes. In the initial coordinate system, the Z axis must be a reference axis, and the axis facing the stairs is another reference axis in the X and Y axes. In the X and Y axes, the other axis is another calibration axis. For example, taking the laser radar as the center of the circle and establishing the initial coordinate system according to the right-hand rule, the Y axis is used as the calibration axis, and the X and Z axes are used as reference axes.
[0051] It can be understood that when the laser radar is facing the stairs, the calibration axis is parallel to the plane where the stairs are located; therefore, if the calibration axis and the plane where the stairs are located intersect, the positional relationship between the laser radar and the stairs is not facing each other. At this time, this example will adjust the position of the laser radar according to the plane of the stairs (at this time, the adjustment is a virtual adjustment, so that the laser radar after the virtual adjustment is facing the stairs), so that the calibration axis in the initial coordinate system corresponding to the virtually adjusted laser radar is parallel to the plane where the stairs are located, and the plane formed by the two reference axes in the initial coordinate system corresponding to the virtually adjusted laser radar is perpendicular to the plane where the stairs are located (achieving the effect of the laser radar facing the stairs).
[0052] Finally, this example constructs a projection plane based on the adjusted lidar reference axis so that the projection plane is perpendicular to the plane of the stairs.
[0053] According to the technical solution provided in the embodiment of the present application, an initial coordinate system of the laser radar is obtained, and the laser radar is used to perform point cloud recognition on the stairs; the calibration axis and the reference axis are determined from the initial coordinate system, and the positional relationship between the calibration axis and the plane where the stairs are located is obtained; if the calibration axis and the plane where the stairs are located intersect, the position of the laser radar is adjusted according to the plane of the stairs so that the calibration axis is parallel to the plane where the stairs are located; a projection plane is constructed according to the adjusted reference axis of the laser radar; this example determines whether the projection plane can be directly constructed based on the reference axis through the positional relationship between the calibration axis and the plane where the stairs are located, and when the calibration axis and the plane where the stairs are located intersect, the laser radar is adjusted so that the projection plane constructed based on the reference axis can be parallel to the plane where the stairs are located.
[0054] In some embodiments, Figure 6 As shown, the two-dimensional point cloud coordinates of each point cloud data after being mapped to the projection plane are obtained, including:
[0055] S601, determining the three-dimensional coordinates of each point cloud data in the adjusted initial coordinate system;
[0056] S602 , remove the calibration axis coordinates of the three-dimensional coordinates corresponding to each point cloud data to obtain the two-dimensional point cloud coordinates corresponding to each point cloud data.
[0057] Specifically, after adjusting the initial coordinate system, this example adjusts the coordinates of each point cloud data according to the adjustment of the initial coordinate system, so as to determine the three-dimensional coordinates of each point cloud data in the adjusted initial coordinate system.
[0058] Since the projection plane is constructed based on two reference axes, after the three-dimensional point cloud data is projected onto the projection plane, the three-dimensional point cloud data no longer has coordinates corresponding to the calibration axis. Therefore, this example removes the calibration axis coordinates from the three-dimensional coordinates corresponding to each point cloud data to obtain the two-dimensional point cloud coordinates corresponding to each point cloud data.
[0059] First, the forward FOV range of the laser radar is selected as (-β°, β°) as the region of interest ROI, which contains the stairs, and the three-dimensional coordinates of all point cloud data of the stairs in the ROI are obtained as P roi (x, y, z), project it to the XZ plane of the laser radar, and get the two-dimensional point cloud coordinates on the XZ plane as P CZ (x,y), where P XZ The component x is equal to P roi The component x,PXz The component y is equal to P roi The component z.
[0060] In some examples, it is also possible to first obtain an initial projection plane, then map the point cloud data to the initial projection plane, and obtain the initial two-dimensional point cloud coordinates after each point cloud data is mapped to the projection plane, and then adjust the initial projection plane so that the initial projection plane is perpendicular to the plane where the stairs are located, and finally adjust the initial two-dimensional point cloud coordinates according to the adjustment of the initial projection plane to obtain the two-dimensional point cloud coordinates.
[0061] According to the technical solution provided in the embodiment of the present application, the three-dimensional coordinates of each point cloud data in the adjusted initial coordinate system are determined; the calibration axis coordinates are removed for the three-dimensional coordinates corresponding to each point cloud data to obtain the two-dimensional point cloud coordinates corresponding to each point cloud data. After mapping the point cloud data to the projection plane, the method determines the coordinate relationship of the point cloud data in the projection plane, and based on the coordinate relationship, the calibration axis coordinates are removed for the three-dimensional coordinates corresponding to the point cloud data, thereby achieving accurate acquisition of the two-dimensional point cloud coordinates corresponding to each point cloud data.
[0062] In some embodiments, Figure 7 As shown, slope fitting processing is performed on multiple two-dimensional point cloud coordinates to obtain the stair slope corresponding to the stairs, including:
[0063] S701, obtaining a preset slope fitting formula, where the slope fitting formula is obtained by performing a slope partial derivative on an objective function corresponding to the straight line fitting formula;
[0064] S702, bringing the coordinates of the multiple two-dimensional point clouds into the slope fitting formula to perform slope fitting processing to obtain the slope of the stairs.
[0065] Specifically, the preset slope fitting formula is as follows:
[0066]
[0067] Among them, K is the slope of the stairs, n is the number of two-dimensional point cloud coordinates, x i is the horizontal coordinate corresponding to the i-th two-dimensional point cloud coordinate, y i is the ordinate corresponding to the i-th two-dimensional point cloud coordinate.
[0068] After obtaining the above slope fitting formula, this example brings multiple two-dimensional point cloud coordinates into the slope fitting formula for slope fitting processing to obtain the slope of the stairs.
[0069] It can be understood that the above slope fitting formula is obtained by taking the slope partial derivative of the objective function corresponding to the straight line fitting formula; specifically, for the two-dimensional point cloud coordinates P corresponding to the projected point cloud data,XZ (x i ,y i ), (i=1,2,…n), the straight line fitting formula after straight line fitting based on the coordinates of the two-dimensional point cloud is f(x i )=y i =kx i +b, where k is the slope of the line (the slope of the line is also the slope of the staircase), and b is the intercept of the line (the intercept of the line can represent the intercept of the staircase). The goal is to solve the parameters k and b so that y i =kx i +b is established, based on the above straight line fitting formula, the objective function is constructed, and the objective function is as follows:
[0070]
[0071] By transforming the objective function, we can get:
[0072]
[0073] By taking the partial derivative of the above equation and setting it equal to zero, we get
[0074]
[0075] The slope fitting formula can be obtained as:
[0076]
[0077] According to the technical solution provided in the embodiment of the present application, a pre-set slope fitting formula is obtained, and the slope fitting formula is obtained by performing a slope partial derivative on the objective function corresponding to the straight line fitting formula; multiple two-dimensional point cloud coordinates are substituted into the slope fitting formula for slope fitting processing to obtain the slope of the stairs, wherein the slope angle of the stairs is converted into the slope of the straight line through the above method, and this method can accurately and quickly obtain the slope of the straight line, thereby improving the efficiency of the slope angle calculation of the stairs.
[0078] In some embodiments, Figure 8 As shown, the slope angle of the stairs is determined according to the slope of the stairs, including:
[0079] S801, obtaining a preset conversion function of slope and angle;
[0080] S802: Convert the slope of the stairs using a conversion function to obtain a slope angle.
[0081] It can be understood that the slope is the ratio of the vertical distance of the straight line rising (or falling) to the horizontal distance, and the angle is the angle between the straight line and the positive direction of the x-axis in the coordinate system. According to the above records, the slope and the angle have a correlation. This example is based on the correlation and is provided with a conversion function of the slope and the angle. The conversion function is as follows: α = arctan (k), where α is the angle and k is the slope.
[0082] After obtaining the slope of the stairs, this example substitutes the slope of the stairs into the conversion function to obtain the corresponding slope angle, as follows: slope angle = arctan (slope of the stairs).
[0083] According to the technical solution provided in the embodiment of the present application, a pre-set conversion function of slope and angle is obtained; the slope of the stairs is converted by the conversion function to obtain the slope angle, wherein the conversion function can reflect the correlation between the slope and the angle. Therefore, this example accurately converts the slope of the stairs into the slope angle through the conversion function.
[0084] In some embodiments, Fig. 9 As shown, the method also includes:
[0085] S901, obtaining a preset intercept fitting formula, where the intercept fitting formula is obtained by performing partial derivative of the intercept on the objective function corresponding to the straight line fitting formula;
[0086] S902, bringing the multiple two-dimensional point cloud coordinates into the intercept fitting formula to perform intercept fitting processing to obtain the stair intercept, where the stair intercept represents the distance between the stair and the origin corresponding to the two-dimensional point cloud coordinates.
[0087] Specifically, the preset intercept fitting formula is as follows:
[0088]
[0089] Where b is the staircase intercept, n is the number of two-dimensional point cloud coordinates, x i is the horizontal coordinate corresponding to the i-th two-dimensional point cloud coordinate, y i is the ordinate corresponding to the i-th two-dimensional point cloud coordinate.
[0090] After obtaining the above intercept fitting formula, this example brings multiple two-dimensional point cloud coordinates into the intercept fitting formula for intercept fitting processing to obtain the stair intercept.
[0091] It can be understood that the above intercept fitting formula is obtained by performing intercept partial derivative of the objective function corresponding to the straight line fitting formula; specifically, for the two-dimensional point cloud coordinates P corresponding to the projected point cloud data XZ (x i ,y i), (i=1,2,…n), the straight line fitting formula after straight line fitting based on the coordinates of the two-dimensional point cloud is f(x i )=y i =kx i +b, where k is the slope of the line (the slope of the line is also the slope of the staircase), and b is the intercept of the line (the intercept of the line can represent the intercept of the staircase). The goal is to solve the parameters k and b so that y i =kx i +b is established, based on the above straight line fitting formula, the objective function is constructed, and the objective function is as follows:
[0092]
[0093] By transforming the objective function, we can get:
[0094]
[0095] By taking the partial derivative of the above equation and setting it equal to zero, we get
[0096]
[0097] The slope fitting formula can be obtained as:
[0098]
[0099] According to the technical solution provided in the embodiment of the present application, a pre-set intercept fitting formula is obtained, and the intercept fitting formula is obtained by performing partial derivative of the intercept on the objective function corresponding to the straight line fitting formula; multiple two-dimensional point cloud coordinates are substituted into the intercept fitting formula for intercept fitting processing to obtain the stair intercept, and the stair intercept represents the distance between the stairs and the origin corresponding to the two-dimensional point cloud coordinates, thereby realizing accurate determination of the stair intercept and providing a basis for subsequent vehicle control.
[0100] In some embodiments, Fig.10 As shown, the method also includes:
[0101] S1001, determining a power distribution strategy according to the slope angle, and determining a starting point of the stairs according to the stair intercept;
[0102] S1002: Generate a vehicle control strategy according to the slope angle and the starting point of the stairs, and control the vehicle according to the vehicle control strategy.
[0103] Specifically, different slope angles correspond to different power distribution strategies. For example, the higher the slope angle, the higher the required power output; the lower the slope angle, the lower the required power output; and the higher the slope angle, the slower the speed, to ensure that the vehicle can maintain sufficient traction without slipping. Relevant personnel can flexibly set the power distribution strategy corresponding to the slope angle based on the above principle, which will not be repeated here.
[0104] In some examples, the vehicle can also determine the starting point of the stairs based on the above-mentioned stair intercept, which enables the vehicle to determine the location to start climbing the stairs. Therefore, this example generates a vehicle control strategy based on the slope angle and the starting point of the stairs, and controls the vehicle according to the vehicle control strategy.
[0105] In order to better understand this method, this embodiment provides a more specific example for illustration:
[0106] The vehicle consists of four parts: LiDAR, intelligent driving domain system on chip (System on Chip, SOC), intelligent driving domain microcontroller unit (Microcontroller Unit, MCU), and cockpit domain. Intelligent driving vehicles are equipped with multi-line LiDAR (such as Robosense Ruby plus 128-line LiDAR), such as Fig.11 As shown, the laser radar is connected to the intelligent driving domain SOC through the ETH network, and the SOC can execute the above steps 101 to S103. When the vehicle needs to perform stair slope recognition, the multi-line laser radar inputs the original point cloud data to the intelligent driving domain control SOC, and the SOC can output the slope angle, which can be directly output to the stair slope UI display module and the stair slope voice broadcast module in the cockpit domain, and directly interact with the driver. Based on the slope angle of the stairs, the driver can objectively know the slope angle of the stairs on the road ahead (in some examples, the stair intercept can also be output), providing a decision basis for whether off-road climbing can be performed; on the other hand, the stair slope recognition result can also be input into the intelligent driving SOC decision planning module, and further input into the control execution module, providing perception result information for autonomous stair climbing of future intelligent driving vehicles.
[0107] Continuing from the above example, the method for SOC to calculate the slope angle based on point cloud data is as follows: Fig.12As shown, this example first obtains point cloud data through the laser radar set on the vehicle, and each point cloud data has three-dimensional coordinates. Then, the point cloud data corresponding to the stairs is obtained through the point cloud ROI interception algorithm, and then a projection plane is generated. The intercepted point cloud data is mapped to the projection plane, and the two-dimensional point cloud coordinates corresponding to each point cloud data in the projection plane are obtained. Then, the two-dimensional point cloud coordinates are fitted (the fitting can be performed based on the Ceres fitting algorithm), and the final fitting result is obtained by setting a suitable fitting cost value, and the final stair slope is obtained. The slope angle is determined based on the stair slope, and finally the slope angle corresponding to the stairs is output; in this method, the laser radar provides high-precision three-dimensional point cloud data, making the detection of the stair slope more accurate; and this method is applicable to various indoor and outdoor environments, and can cope with complex environments with dim light, reducing the impact of external factors on the detection accuracy. And this method is applicable to functional unmanned vehicles, wheeled / tracked robots, and intelligent driving passenger cars, which expands the application scope of unmanned vehicles. In this method, the slope angle and stair intercept can be directly output to the display module and voice broadcast module to assist the driver in making decisions; they can also be input to the intelligent driving controller to provide perception information for future intelligent driving vehicles to climb stairs autonomously. This method uses a monomial fitting algorithm and optimization based on the ceres library to improve the calculation efficiency and fitting accuracy; this method makes the technology more economical and practical as the cost of lidar hardware decreases; this method is simple and reliable, greatly saving settlement resources and development costs, and is easy to implement.
[0108] According to the technical solution provided in the embodiment of the present application, the power distribution strategy is determined according to the slope angle, and the starting point of the stairs is determined according to the stair intercept; the vehicle control strategy is generated according to the slope angle and the starting point of the stairs, and the vehicle is controlled according to the vehicle control strategy. The slope angle and stair intercept in this example provide a basis for determining the power distribution strategy, so that the determined vehicle control strategy is more in line with the actual situation, thereby improving the experience of the vehicle climbing stairs.
[0109] All the above optional technical solutions can be arbitrarily combined to form optional embodiments of the present application, which will not be described one by one here.
[0110] The following is an embodiment of the device of the present application, which can be used to execute the embodiment of the method of the present application. For details not disclosed in the embodiment of the device of the present application, please refer to the embodiment of the method of the present application.
[0111] This embodiment also provides a device for determining the slope angle of stairs. Fig.13 As shown, the device comprises:
[0112] The acquisition module 1301 is used to acquire a plurality of point cloud data corresponding to the stairs, and establish a projection plane according to the plane where the stairs are located, and the projection plane is perpendicular to the plane where the stairs are located;
[0113] A mapping module 1302 is used to map multiple point cloud data to a projection plane, and obtain the two-dimensional point cloud coordinates of each point cloud data after being mapped to the projection plane;
[0114] The calculation module 1303 is used to perform slope fitting processing on the coordinates of the multiple two-dimensional point clouds to obtain the stair slope corresponding to the stairs, and determine the slope angle of the stairs according to the stair slope.
[0115] In some examples, the acquisition module 1301 is also used to obtain the initial coordinate system of the laser radar, which is used to perform point cloud recognition on the stairs; determine the calibration axis and the reference axis from the initial coordinate system, and obtain the positional relationship between the calibration axis and the plane where the stairs are located; if the calibration axis and the plane where the stairs are located intersect, adjust the position of the laser radar according to the plane of the stairs so that the calibration axis is parallel to the plane where the stairs are located; and construct a projection plane according to the adjusted reference axis of the laser radar.
[0116] In some examples, the mapping module 1302 is also used to determine the three-dimensional coordinates of each point cloud data in the adjusted initial coordinate system; the calibration axis coordinates are removed from the three-dimensional coordinates corresponding to each point cloud data to obtain the two-dimensional point cloud coordinates corresponding to each point cloud data.
[0117] In some examples, the calculation module 1303 is also used to obtain a preset slope fitting formula, which is obtained by performing a slope partial derivative on the objective function corresponding to the straight line fitting formula; multiple two-dimensional point cloud coordinates are substituted into the slope fitting formula for slope fitting processing to obtain the slope of the stairs.
[0118] In some examples, the calculation module 1303 is further used to obtain a preset conversion function of slope and angle; the slope of the stairs is converted by the conversion function to obtain the slope angle.
[0119] In some examples, the calculation module 1303 is also used to obtain a preset intercept fitting formula, which is obtained by taking the intercept partial derivative of the objective function corresponding to the straight line fitting formula; multiple two-dimensional point cloud coordinates are substituted into the intercept fitting formula for intercept fitting processing to obtain the stair intercept, which represents the distance between the stairs and the origin corresponding to the two-dimensional point cloud coordinates.
[0120] In some examples, the calculation module 1303 is also used to determine a power distribution strategy based on the slope angle, determine the starting point of the stairs based on the stair intercept; generate a vehicle control strategy based on the slope angle and the starting point of the stairs, and control the vehicle according to the vehicle control strategy.
[0121] According to the technical solution provided in the embodiment of the present application, the device for determining the slope angle of the stairs provided in the embodiment obtains multiple point cloud data corresponding to the stairs, and establishes a projection plane according to the plane where the stairs are located, and the projection plane is perpendicular to the plane where the stairs are located; maps the multiple point cloud data to the projection plane, and obtains the two-dimensional point cloud coordinates after each point cloud data is mapped to the projection plane; performs slope fitting processing on the multiple two-dimensional point cloud coordinates to obtain the stair slope corresponding to the stairs, and determines the slope angle of the stairs according to the slope of the stairs, wherein the three-dimensional point cloud data is converted into two-dimensional point cloud coordinates by mapping multiple three-dimensional point cloud coordinates to a projection plane perpendicular to the plane where the stairs are located. Since the multiple two-dimensional point cloud coordinates can express the fitting straight line corresponding to the stairs, this example can obtain the slope corresponding to the straight line fitted by the multiple two-dimensional point cloud coordinates by performing slope fitting processing on the multiple two-dimensional point cloud coordinates, and subsequently accurately determines the slope angle of the stairs according to the slope of the stairs, thereby avoiding the problem of being unable to accurately determine the slope angle of the stairs in the related art.
[0122] Fig.14 Schematic diagram of an electronic device 14 provided in an embodiment of the present application. Fig.14 As shown, the electronic device 14 of this embodiment includes: a processor 1401, a memory 1402, and a computer program 1403 stored in the memory 1402 and executable on the processor 1401. When the processor 1401 executes the computer program 1403, the steps in the above-mentioned method embodiments are implemented. Alternatively, when the processor 1401 executes the computer program 1403, the functions of the modules / units in the above-mentioned device embodiments are implemented.
[0123] The electronic device 14 may be a desktop computer, a notebook, a PDA, a cloud server, or other electronic device. The electronic device 14 may include, but is not limited to, a processor 1401 and a memory 1402. Those skilled in the art will appreciate that Fig.14 The electronic device 14 is merely an example and does not limit the electronic device 14 , and may include more or less components than those shown in the figure, or different components.
[0124] Processor 1401 can be a central processing unit (CPU), or other general-purpose processors, digital signal processors (DSP), application-specific integrated circuits (ASIC), field-programmable gate arrays (FPGA) or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc.
[0125] The memory 1402 may be an internal storage unit of the electronic device 14, for example, a hard disk or memory of the electronic device 14. The memory 1402 may also be an external storage device of the electronic device 14, for example, a plug-in hard disk, a smart media card (SMC), a secure digital (SD) card, a flash card (FlashCard), etc. equipped on the electronic device 14. The memory 1402 may also include both an internal storage unit of the electronic device 14 and an external storage device. The memory 1402 is used to store computer programs and other programs and data required by the electronic device.
[0126] Those skilled in the art can clearly understand that for the convenience and simplicity of description, only the division of the above-mentioned functional units and modules is used as an example. In actual applications, the above-mentioned functions can be assigned to different functional units and modules as needed, that is, the internal structure of the device can be divided into different functional units or modules to complete all or part of the functions described above. The functional units and modules in the embodiments can be integrated into one processing unit, or each unit can exist physically separately, or two or more units can be integrated into one unit. The above-mentioned integrated unit can be implemented in the form of hardware or in the form of software functional units.
[0127] If the integrated module / unit is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the present application implements all or part of the processes in the above-mentioned embodiment method, and can also be completed by instructing the relevant hardware through a computer program. The computer program can be stored in a computer-readable storage medium, and the computer program can implement the steps of the above-mentioned various method embodiments when executed by the processor. The computer program may include computer program code, and the computer program code may be in source code form, object code form, executable file or some intermediate form. The computer-readable medium may include: any entity or device capable of carrying computer program code, recording medium, U disk, mobile hard disk, magnetic disk, optical disk, computer memory, read-only memory (ROM), random access memory (RAM), electric carrier signal, telecommunication signal and software distribution medium. It should be noted that the content contained in the computer-readable medium can be appropriately increased or decreased according to the requirements of regional requirements and patent practice. For example, in some areas, according to regional requirements and patent practice, the computer-readable medium does not include electric carrier signals and telecommunication signals.
[0128] The above embodiments are only used to illustrate the technical solutions of the present application, rather than to limit them. Although the present application has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or make equivalent replacements for some of the technical features therein. These modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the embodiments of the present application, and should all be included in the protection scope of the present application.
Claims
1. A method for determining the slope angle of stairs, characterized in that: The method comprises: Acquire a plurality of point cloud data corresponding to the stairs, and establish a projection plane according to the plane where the stairs are located, wherein the projection plane is perpendicular to the plane where the stairs are located; Mapping the plurality of point cloud data to the projection plane, and obtaining the two-dimensional point cloud coordinates of each point cloud data after being mapped to the projection plane; Slope fitting processing is performed on the plurality of two-dimensional point cloud coordinates to obtain the stair slope corresponding to the stair, and the slope angle of the stair is determined according to the stair slope.
2. The method according to claim 1, characterized in that Establishing a projection plane according to the plane where the stairs are located, including: Acquire an initial coordinate system of a laser radar, wherein the laser radar is used to perform point cloud recognition on the stairs; Determine a calibration axis and a reference axis from the initial coordinate system, and obtain a positional relationship between the calibration axis and the plane where the stairs are located; If the calibration axis intersects with the plane where the stairs are located, adjusting the position of the laser radar according to the plane of the stairs so that the calibration axis is parallel to the plane where the stairs are located; The projection plane is constructed according to the adjusted reference axis of the laser radar.
3. The method according to claim 2, characterized in that Obtaining the two-dimensional point cloud coordinates of each point cloud data after being mapped to the projection plane, comprising: Determine the three-dimensional coordinates of each point cloud data in the adjusted initial coordinate system; The calibration axis coordinates are removed from the three-dimensional coordinates corresponding to each of the point cloud data to obtain the two-dimensional point cloud coordinates corresponding to each of the point cloud data.
4. The method according to claim 1, characterized in that Performing slope fitting processing on the plurality of two-dimensional point cloud coordinates to obtain the stair slope corresponding to the staircase includes: Obtaining a preset slope fitting formula, wherein the slope fitting formula is obtained by performing a slope partial derivative on an objective function corresponding to a straight line fitting formula; The coordinates of the two-dimensional point clouds are brought into the slope fitting formula to perform slope fitting processing to obtain the slope of the stairs.
5. The method according to claim 1, characterized in that Determining the slope angle of the stairs according to the slope of the stairs includes: Get the preset slope and angle conversion function; The stair slope is converted by the conversion function to obtain the slope angle.
6. The method according to claim 5, characterized in that The method further comprises: Obtaining a preset intercept fitting formula, where the intercept fitting formula is obtained by performing partial derivative of the intercept on the objective function corresponding to the straight line fitting formula; The plurality of two-dimensional point cloud coordinates are brought into the intercept fitting formula to perform intercept fitting processing to obtain a stair intercept, wherein the stair intercept represents the distance between the stair and the origin corresponding to the two-dimensional point cloud coordinate.
7. The method according to claim 6, characterized in that The method further comprises: Determine a power distribution strategy according to the slope angle, and determine a starting point of the staircase according to the staircase intercept; A vehicle control strategy is generated according to the slope angle and the starting point of the stairs, and the vehicle is controlled according to the vehicle control strategy.
8. A device for determining the slope angle of stairs, characterized in that: The device comprises: An acquisition module, used for acquiring a plurality of point cloud data corresponding to the stairs, and establishing a projection plane according to the plane where the stairs are located, wherein the projection plane is perpendicular to the plane where the stairs are located; A mapping module, used for mapping the plurality of point cloud data to the projection plane, and obtaining the two-dimensional point cloud coordinates of each point cloud data after being mapped to the projection plane; The calculation module is used to perform slope fitting processing on the plurality of two-dimensional point cloud coordinates to obtain the stair slope corresponding to the stair, and determine the slope angle of the stair according to the stair slope.
9. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that: When the processor executes the computer program, the steps of the method according to any one of claims 1 to 7 are implemented.
10. A computer-readable storage medium storing a computer program, characterized in that: When the computer program is executed by a processor, the steps of the method according to any one of claims 1 to 7 are implemented.
Citation Information
Patent Citations
Three-dimension design method for side slope gentling for surface loose bed of open-pit mine
CN110700285A
Road edge detection system and method based on laser radar and fan-shaped space segmentation
CN110781827A
All-terrain vehicle and automatic stair detection and climbing method thereof
CN112099494A
Road edge detection method and device, terminal equipment and storage medium
CN115015964A
Stair detection method of robot, robot and storage medium
CN116009016A
