阅读说明：本技术 基于多传感器的低速车辆的前向地形三维构建方法 (Forward terrain three-dimensional construction method of low-speed vehicle based on multiple sensors ) 是由 李忠利 杨淑君 高永升 卢耀真 韦宇豪 杨永军 于 2020-07-08 设计创作，主要内容包括：基于多传感器的低速车辆的前向地形三维构建方法,步进电机的旋转轴与雷达组合后与地面呈倾斜角度α设置；上位机的四个串口分别连接并接受单片机、雷达、GPS、陀螺仪数据,分别解析出角度、距离、车速、姿态角信息；雷达实时监测距离信息,将距离信息进行频谱分析,进行低通滤波器滤波并处理得到的距离数据；单片机控制步进电机做摆动运动并实时监测角度信息,将角度信息数据进行线性插值处理,将角度信息与去除噪点的距离信息同步；建立雷达坐标轴和车辆坐标轴,通过坐标变换将雷达坐标轴和车辆坐标轴统一；将X坐标进行修正处理,得到当前位置时物体所有点距离车辆的距离；将该三维坐标导入Matlab后,得到三维模型。(A forward terrain three-dimensional construction method of a low-speed vehicle based on multiple sensors is characterized in that a rotating shaft of a stepping motor is combined with a radar and then forms an inclination angle alpha with the ground; four serial ports of the upper computer are respectively connected with and receive data of the single chip microcomputer, the radar, the GPS and the gyroscope, and angle, distance, vehicle speed and attitude angle information are respectively analyzed; the radar monitors the distance information in real time, performs spectrum analysis on the distance information, performs low-pass filter filtering and processes the obtained distance data; the single chip microcomputer controls the stepping motor to swing and monitors angle information in real time, linear interpolation processing is carried out on angle information data, and the angle information and distance information for removing noise points are synchronized; establishing a radar coordinate axis and a vehicle coordinate axis, and unifying the radar coordinate axis and the vehicle coordinate axis through coordinate transformation; correcting the X coordinate to obtain the distances between all points of the object and the vehicle at the current position; and importing the three-dimensional coordinates into Matlab to obtain a three-dimensional model.)

1. A forward terrain three-dimensional construction method of a low-speed vehicle based on multiple sensors is characterized by comprising the following steps: the system comprises a singlechip, a stepping motor, a radar, a GPS and a gyroscope; the stepping motor is connected with the radar and drives the radar to rotate, a rotating shaft of the stepping motor is perpendicular to the radar, the rotating shaft of the stepping motor is combined with the radar and then forms an inclination angle alpha with the ground, the stepping motor, the radar, the low-pass filter, the GPS and the gyroscope are all connected with the single chip microcomputer, the single chip microcomputer controls the stepping motor to swing, and the single chip microcomputer is connected with an upper computer;

the three-dimensional construction method specifically comprises the following steps:

s1: four serial ports of the upper computer are respectively connected with and receive data of the singlechip, the radar, the GPS and the gyroscope, information of an angle, a distance, a vehicle speed and an attitude angle is respectively analyzed, and then data processing is carried out;

s2: the radar monitors the distance information in real time, performs spectrum analysis on the distance information, and then performs low-pass filter filtering and processing on the obtained distance data;

s3: the single chip microcomputer controls the stepping motor to swing and monitors angle information in real time, linear interpolation processing is carried out on angle information data, and the angle information and distance information for removing noise points are synchronized;

s4: establishing a radar coordinate axis and a vehicle coordinate axis, and unifying the radar coordinate axis and the vehicle coordinate axis through coordinate transformation;

s41: establishing a polar coordinate system which takes a radar as a pole, takes a distance as a polar diameter and takes an angle as a polar angle, and then converting the polar coordinate into a two-dimensional rectangular coordinate system which takes the radar as an origin; then, the whole coordinate system rotates anticlockwise around the Y axis to obtain a three-dimensional rectangular coordinate system;

s5: carrying out compensation attitude angle obtained by RBF neural network on the attitude angle obtained by the gyroscope to obtain a corrected space coordinate system; then correcting the x information through the speed information obtained by the GPS to obtain the three-dimensional coordinate of the object at the current position and the vehicle;

s6: and (4) importing the three-dimensional coordinates of the object at the current position and the vehicle into Matlab to obtain a three-dimensional model.

2. The forward terrain three-dimensional construction method for a multi-sensor based low-speed vehicle of claim 1, characterized in that: the radar is a single line laser radar.

3. The forward terrain three-dimensional construction method for a multi-sensor based low-speed vehicle of claim 1, characterized in that: the low-pass filter is a Butterworth low-pass filter.

4. The forward terrain three-dimensional construction method for a multi-sensor based low-speed vehicle of claim 1, characterized in that: the step S2 specifically includes the following steps:

firstly, carrying out fast Fourier transform on the stored distance information:

wherein F (w) is the image function of f (t), f (t) is the image primitive function of F (w), i.e. the distance information to be input is subjected to spectrum analysis, the frequency wc at the high-energy position is observed, then a filter is carried out to separate out the noise value, and the low-pass filter can be represented by the following formula of the square of the amplitude to the frequency:

where n is the order of the low-pass filter, wc is the cut-off frequency, wp is the sampling frequency; the data passing through the low pass filter will remove some of the noise values to reduce errors in the data.

5. The forward terrain three-dimensional construction method for a multi-sensor based low-speed vehicle of claim 1, characterized in that: the step S1 specifically includes: the four serial ports of the upper computer are respectively connected with and receive data of the singlechip, the radar, the GPS and the gyroscope, and the data of the singlechip, the radar, the GPS and the gyroscope are respectively analyzed in the same timer, so that the data are acquired at the same time and are information of angle, distance, speed and attitude angle of the same position.

6. The forward terrain three-dimensional construction method for a multi-sensor based low-speed vehicle of claim 1, characterized in that: in step S3, the linear interpolation processing on the angle data specifically includes:

firstly, calculating the number of required interpolation in the angle data:

wherein n is_rThe number of the laser radars obtained by sampling is distance data n_mThe number of angle information n obtained by sampling_u: what number of angles needs to be inserted is shown to be the same as the number of distance information, and meanwhile, the interval value of each interpolation needs to be calculated, and is represented by delta n:

where Δ γ represents the step angle of the stepping motor.

7. The forward terrain three-dimensional construction method for a multi-sensor based low-speed vehicle of claim 1, characterized in that: in step S51, the method specifically includes the following steps:

a1: the activation function of the radial basis function neural network can be expressed as:

in the formula, | Lp-ci | | | is an Euclidean norm, ci is the center of a Gaussian function, and is the variance of the Gaussian function;

a2: since we have multiple inputs and multiple outputs, the structure of the radial basis function neural network can obtain the network outputs as:

in the formula, Lp is the P-th input sample, P is 1,2,3,.. the P is the total number of samples, ci is the center of the hidden layer node of the network, Wij is the connection weight from the hidden layer to the output layer, i is 1,2,3,. the h is the number of the hidden layer nodes, and yi is the actual output of the j-th output node of the network corresponding to the input sample;

a3: the method comprises the steps of taking the x-axis acceleration, the y-axis acceleration, the z-axis angular velocity and the speed information of a gyroscope for measuring the attitude angle as input variables of a network, and taking the roll angle, the pitch angle and the yaw angle of a vehicle as output layers of the network, so as to achieve the purpose of compensating the attitude angle.

Technical Field

The invention belongs to the technical field of automobiles, and particularly relates to a forward terrain three-dimensional construction method of a low-speed vehicle based on multiple sensors.

Background

At present, vehicle intellectualization is an important development direction of vehicles, a vehicle intelligent driving technology is one of a representative and core technology of vehicle intellectualization, forward terrain is the premise of vehicle intelligent driving, an intelligent vehicle owner is composed of a plurality of systems such as an environment perception system, a positioning system, a vehicle control system, a decision planning system and the like, among the factors, the environment perception is a medium for obtaining surrounding information, is a pair of eyes in the driving process of the vehicle, can accurately provide forward terrain information for the intelligent vehicle in real time, and provides a reliable basis for path planning and autonomous decision behaviors of the intelligent vehicle. Laser radar has high measurement accuracy and high speed, is not easily influenced by illumination conditions, is one of important sensors in environment perception of unmanned vehicles, is generally adopted by an existing unmanned researcher, has high efficiency and good effect, is adopted by existing unmanned vehicle enterprises in domestic and foreign research, but has certain limitation on large-scale use and research of scholars due to high hardware cost and high data processing difficulty of the multi-line laser radar. The multi-sensor-based forward terrain construction system for the low-speed vehicle can overcome the defect of high price of the multi-line laser radar, and is selected for the low-speed vehicle because the single-line laser radar is low in scanning speed and low in motor rotating speed compared with the multi-line laser radar, so that less road surface information is obtained, and compared with a passenger vehicle, the low-speed vehicle does not need to consider a high-speed road and can acquire forward terrain information more time for the single-line laser radar.

Disclosure of Invention

In view of the above, to solve the above-mentioned deficiencies of the prior art, the present invention provides a method for three-dimensionally constructing forward terrain of a low-speed vehicle based on multiple sensors, which uses multiple sensors to implement three-dimensional construction of an object in front of the vehicle, and can implement construction of a three-dimensional model with less cost.

In order to achieve the purpose, the technical scheme adopted by the invention is as follows:

the forward terrain three-dimensional construction method of the low-speed vehicle based on the multi-sensor comprises a single chip microcomputer, a stepping motor, a radar, a GPS and a gyroscope; the stepping motor is connected with the radar and drives the radar to rotate, a rotating shaft of the stepping motor is perpendicular to the radar, the rotating shaft of the stepping motor is combined with the radar and then forms an inclination angle alpha with the ground, the stepping motor, the radar, the low-pass filter, the GPS and the gyroscope are all connected with the single chip microcomputer, the single chip microcomputer controls the stepping motor to swing, and the single chip microcomputer is connected with an upper computer;

the three-dimensional construction method specifically comprises the following steps:

s1: four serial ports of the upper computer are respectively connected with and receive data of the singlechip, the radar, the GPS and the gyroscope, information of an angle, a distance, a vehicle speed and an attitude angle is respectively analyzed, and then data processing is carried out;

s2: the radar monitors the distance information in real time, performs spectrum analysis on the distance information, and then performs low-pass filter filtering and processing on the obtained distance data;

s3: the single chip microcomputer controls the stepping motor to swing and monitors angle information in real time, linear interpolation processing is carried out on angle information data, and the angle information and distance information for removing noise points are synchronized;

s4: establishing a radar coordinate axis and a vehicle coordinate axis, and unifying the radar coordinate axis and the vehicle coordinate axis through coordinate transformation;

s41: establishing a polar coordinate system which takes a radar as a pole, takes a distance as a polar diameter and takes an angle as a polar angle, and then converting the polar coordinate into a two-dimensional rectangular coordinate system which takes the radar as an origin; then, the whole coordinate system rotates anticlockwise around the Y axis to obtain a three-dimensional rectangular coordinate system;

s5: carrying out compensation attitude angle obtained by RBF neural network on the attitude angle obtained by the gyroscope to obtain a corrected space coordinate system; then correcting the x information through the speed information obtained by the GPS to obtain the three-dimensional coordinate of the object at the current position and the vehicle;

s6: and (4) importing the three-dimensional coordinates of the object at the current position and the vehicle into Matlab to obtain a three-dimensional model.

Further, the radar is a single line laser radar.

Further, the low-pass filter is a butterworth low-pass filter.

Further, the step S2 specifically includes the following steps:

firstly, carrying out fast Fourier transform on the stored distance information:

wherein F (w) is the image function of f (t), f (t) is the image primitive function of F (w), i.e. the distance information to be input is subjected to spectrum analysis, the frequency wc at the high-energy position is observed, then a filter is carried out to separate out the noise value, and the low-pass filter can be represented by the following formula of the square of the amplitude to the frequency:

where n is the order of the low-pass filter, wc is the cut-off frequency, wp is the sampling frequency; the data passing through the low pass filter will remove some of the noise values to reduce errors in the data.

Further, the step S1 specifically includes: the four serial ports of the upper computer are respectively connected with and receive data of the singlechip, the radar, the GPS and the gyroscope, and the data of the singlechip, the radar, the GPS and the gyroscope are respectively analyzed in the same timer, so that the data are acquired at the same time and are information of angle, distance, speed and attitude angle of the same position.

Further, in step S3, the linear interpolation processing on the angle data specifically includes:

firstly, calculating the number of required interpolation in the angle data:

wherein n is_rThe number of the laser radars obtained by sampling is distance data n_mThe number of angle information n obtained by sampling_u: what number of angles needs to be inserted is shown to be the same as the number of distance information, and meanwhile, the interval value of each interpolation needs to be calculated, and is represented by delta n:

where Δ γ represents the step angle of the stepping motor.

Further, the step S51 specifically includes the following steps:

a1: the activation function of the radial basis function neural network can be expressed as:

in the formula, | Lp-ci | | | is an Euclidean norm, ci is the center of a Gaussian function, and is the variance of the Gaussian function;

a2: since we have multiple inputs and multiple outputs, the structure of the radial basis function neural network can obtain the network outputs as:

in the formula, Lp is the P-th input sample, P is 1,2,3,.. the P is the total number of samples, ci is the center of the hidden layer node of the network, Wij is the connection weight from the hidden layer to the output layer, i is 1,2,3,. the h is the number of the hidden layer nodes, and yi is the actual output of the j-th output node of the network corresponding to the input sample;

a3: the method comprises the steps of taking the x-axis acceleration, the y-axis acceleration, the z-axis angular velocity and the speed information of a gyroscope for measuring the attitude angle as input variables of a network, and taking the roll angle, the pitch angle and the yaw angle of a vehicle as output layers of the network, so as to achieve the purpose of compensating the attitude angle.

The invention has the beneficial effects that:

the invention realizes the three-dimensional construction of the object in front of the vehicle by utilizing a plurality of sensors, and can realize the construction of the three-dimensional model with less cost. The invention comprises a singlechip, a stepping motor, a radar, a low-pass filter, a GPS and a gyroscope; the stepping motor is connected with the radar and drives the radar to rotate, a rotating shaft of the stepping motor is perpendicular to the radar, and the rotating shaft of the stepping motor is combined with the radar and then forms an inclination angle alpha with the ground; four serial ports of the upper computer are respectively connected with and receive data of the singlechip, the radar, the GPS and the gyroscope, and information of an angle, a distance, a vehicle speed and an attitude angle is respectively analyzed; the radar monitors the distance information in real time, performs spectrum analysis on the distance information, and then performs low-pass filter filtering and processing on the obtained distance data; the single chip microcomputer controls the stepping motor to swing and monitors angle information in real time, linear interpolation processing is carried out on angle information data, and the angle information and distance information for removing noise points are synchronized; establishing a radar coordinate axis and a vehicle coordinate axis, and unifying the radar coordinate axis and the vehicle coordinate axis through coordinate transformation; correcting the X coordinate to obtain the distances between all points of the object and the vehicle at the current position; and (4) importing the three-dimensional coordinates of the object at the current position and the vehicle into Matlab to obtain a three-dimensional model.

Drawings

In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings used in the description of the embodiments or the prior art will be briefly described below, it is obvious that the drawings in the following description are only some embodiments of the present invention, and for those skilled in the art, other drawings can be obtained according to the drawings without creative efforts.

FIG. 1 is a functional block diagram of the present invention;

FIG. 2 is a schematic diagram of a rectangular coordinate system;

FIG. 3 is a flow diagram of the present invention.

Detailed Description

The following specific examples are given to further clarify, complete and detailed the technical solution of the present invention. The present embodiment is a preferred embodiment based on the technical solution of the present invention, but the scope of the present invention is not limited to the following embodiments.

The forward terrain three-dimensional construction method of the low-speed vehicle based on the multi-sensor comprises a single chip microcomputer, a stepping motor, a radar, a GPS and a gyroscope; the method comprises the steps that a stepping motor is connected with a radar and drives the radar to rotate, a rotating shaft of the stepping motor is perpendicular to the radar, and the rotating shaft of the stepping motor is combined with the radar and then forms an inclination angle alpha with the ground, so that when the ground is a plane, a measuring surface of the radar intersects with the ground to form a straight line, the radar is used as a pole, the measuring surface of the radar is used as a polar coordinate system, the measuring distance is used as a polar diameter, the rotating angle of the stepping motor is used as a polar angle, the obtained polar coordinate system is firstly converted into a rectangular coordinate system with the radar as an origin, and the rectangular coordinate system is marked as Oxy; the stepping motor, the radar, the GPS and the gyroscope are all connected with the single chip microcomputer, the single chip microcomputer controls the stepping motor to do swinging motion, and the single chip microcomputer is connected with an upper computer;

further, in this embodiment, the stepping motor drives the radar to reciprocate at a certain angle γ, the one-dimensional laser radar is changed into a two-dimensional radar with angle information, the swing angle γ is related to the installation position and the installation height of the motor, the reciprocating swing angle of the stepping motor needs to be known, when the vehicle model is selected, the safety distance S and the installation height h are determined, and according to the visual width D of the front of the vehicle, which is actually needed, the vehicle can be calculated by a formula, as shown in fig. 1 and 2

Firstly, the polar coordinates of an ordinal pair (L, beta) with a radar as a pole and a distance as a polar diameter need to be converted into a rectangular coordinate system with a motor as an origin, and an xy coordinate axis is established by taking the laser radar as the origin on a laser radar measurement plane, as shown in fig. 2

x＝L*sin β (1)

y＝L*cos β (2)

Where L denotes the measured distance to the object of the singlet lidar and β denotes the polar angle, resulting in rectangular coordinates x, y in the coordinate system shown in fig. 2.

As shown in fig. 3, the three-dimensional construction method specifically includes the following steps:

s1: writing an upper computer program by vs, respectively connecting four serial ports of the upper computer with and receiving data of a single chip microcomputer, a radar, a GPS and a gyroscope, respectively analyzing information such as an angle, a distance, a vehicle speed and an attitude angle, and then processing the data;

s2: the radar monitors the distance information in real time, performs spectrum analysis on the distance information, and then performs low-pass filter filtering and processing on the obtained distance data;

s3: the single chip microcomputer controls the stepping motor to swing and monitors angle information in real time, linear interpolation processing is carried out on angle information data, and the angle information and distance information for removing noise points are synchronized;

s4: establishing a radar coordinate axis and a vehicle coordinate axis, and unifying the radar coordinate axis and the vehicle coordinate axis through coordinate transformation;

s41: establishing a polar coordinate system which takes a radar as a pole, takes a distance as a polar diameter and takes an angle as a polar angle, and then converting the polar coordinate into a two-dimensional rectangular coordinate system which takes the radar as an origin; then, the whole coordinate system rotates anticlockwise around the Y axis to obtain a three-dimensional rectangular coordinate system;

s5: carrying out compensation attitude angle obtained by RBF neural network on the attitude angle obtained by the gyroscope to obtain a corrected space coordinate system; then correcting the x information through the speed information obtained by the GPS to obtain the three-dimensional coordinate of the object at the current position and the vehicle;

s6: and (4) importing the three-dimensional coordinates of the object at the current position and the vehicle into Matlab to obtain a three-dimensional model.

Further, the radar is a single line laser radar.

Further, the low-pass filter is a butterworth low-pass filter.

Further, the step S2 specifically includes the following steps:

firstly, carrying out fast Fourier transform on the stored distance information:

wherein F (w) is the image function of f (t), f (t) is the image primitive function of F (w), i.e. the distance information to be input is subjected to spectrum analysis, the frequency wc at the high-energy position is observed, then a filter is carried out to separate out the noise value, and the low-pass filter can be represented by the following formula of the square of the amplitude to the frequency:

where n is the order of the low-pass filter, wc is the cut-off frequency, wp is the sampling frequency; the data passing through the low pass filter will remove some of the noise values to reduce errors in the data.

Further, the step S1 specifically includes: the four serial ports of the upper computer are respectively connected with and receive data of the singlechip, the radar, the GPS and the gyroscope, and the data of the singlechip, the radar, the GPS and the gyroscope are respectively analyzed in the same timer, so that the data are acquired at the same time and are information of angle, distance, speed and attitude angle of the same position. And the sampling time is also respectively stored in angle, distance, vehicle speed and attitude angle files so as to ensure the data information synchronization of various sensors after the following data processing is carried out.

Furthermore, because the sending frequency of the single chip microcomputer is far less than that of the single-line laser radar, the quantity of data received and stored every second is different, and linear interpolation processing needs to be carried out on angle data in order that each distance value corresponds to one angle value; in step S3, the linear interpolation processing on the angle data specifically includes:

firstly, calculating the number of required interpolation in the angle data:

wherein n is_rThe number of the laser radars obtained by sampling is distance data n_mThe number of angle information n obtained by sampling_u: what number of angles needs to be inserted is shown to be the same as the number of distance information, and meanwhile, the interval value of each interpolation needs to be calculated, and is represented by delta n:

where Δ γ represents the step angle of the stepping motor.

Further, the step S51 specifically includes the following steps: the method comprises the steps that a gyroscope is used for measuring an attitude angle of a vehicle body, when the vehicle body pitches, rolls and drifts, a rectangular coordinate system of the vehicle deviates, so that a coordinate system is corrected through the measured attitude angle, but the gyroscope deviates from stable output due to interference, so that drift is caused, the attitude angle needs to be compensated, an RBF neural network is used for compensating the attitude angle for the consistent approximation performance of a nonlinear continuous function, the RBF neural network is simple in structure, concise in training and high in learning convergence speed, any nonlinear function can be approximated, and an RBF neural network learning method for selecting a center by self-organization is selected;

a1: the activation function of the radial basis function neural network can be expressed as:

in the formula, | Lp-ci | | | is an Euclidean norm, ci is the center of a Gaussian function, and is the variance of the Gaussian function;

a2: since we have multiple inputs and multiple outputs, the structure of the radial basis function neural network can obtain the network outputs as:

in the formula, Lp is the P-th input sample, P is 1,2,3,.. the P is the total number of samples, ci is the center of the hidden layer node of the network, Wij is the connection weight from the hidden layer to the output layer, i is 1,2,3,. the h is the number of the hidden layer nodes, and yi is the actual output of the j-th output node of the network corresponding to the input sample;

a3: the method comprises the steps of taking the x-axis acceleration, the y-axis acceleration, the z-axis angular velocity and the speed information of a gyroscope for measuring the attitude angle as input variables of a network, and taking the roll angle, the pitch angle and the yaw angle of a vehicle as output layers of the network, so as to achieve the purpose of compensating the attitude angle.

Furthermore, in this embodiment, the rotating shafts of the single-line laser radar and the stepping motor are installed to be inclined at a certain angle α with the ground, and the radar cartesian axis is also inclined at a certain angle with the ground.

Establishing a low-speed vehicle with the vehicle advancing direction as x_oAxis, vertical ground z_oThe axis establishing an Ox_oy_oz_oA three-dimensional coordinate system, which rotates the coordinate axis of the Oxy laser radar by a certain angle theta around the y axis through coordinate transformation_y90-alpha can be unified with the vehicle coordinate axis, so that r can be obtained under the vehicle coordinate system_o＝(x_o,y_o,z_o) Spatial coordinate value of (a):

thereby allowing three-dimensional coordinates r_o＝(x_o,y_o,z_o) Then the compensated attitude angle is subjected to coordinate transformation to obtain corrected space position coordinates, the initial state O system is coincided with the W system, and then the O system firstly rotates around the Zo axisBy an angle psi, then by an angle theta around the Yo-axis, and then by an angle phi around the Xo-axis, an O-system (i.e., the final attitude of the vehicle) is obtained. This Euler angle sequence has been referred to on the book as "aeronautical sequence Euler angles". After three euler angular rotations, the relationship between one vector rW in the world coordinate system (xW, yW, zW) and the corresponding vector ro in the vehicle coordinate system (xo, yo, zo) can be expressed as

Note the bookThe pitch angle, yaw angle and lateral oblique angle obtained by the gyroscope are respectively represented by theta, psi and phi by a simplified writing method

Wherein

Referred to as the transformation matrix from coordinate system W to coordinate system O.

In turnWhereinAlso called direction cosine matrix in the form of euler angles, is known

Then

Further, as the vehicle continuously advances, the distance between the current vehicle and the object is continuously changed, i.e. the distance needs to be continuously corrected, and speed information at each moment is needed. Firstly, the coordinate of the first time after coordinate transformation is recorded as (x)₁ ^w,y₁ ^w,z₁ ^w) The speed is recorded as V1; the coordinate of the second time is (x)₂ ^w,y₂ ^w,z₂ ^w) The speed is denoted as V2 and the time from the first time to the second time is denoted as Δ t₂… …, the coordinate of the nth time is (x)_n ^w,y_n ^w,z_n ^w) The speed is V3, and the time from the n-1 th time to the n th time is Deltat_nHowever, since the vehicle is continuously moving forward, the coordinates of the point before the time n need to be corrected to the current time:

then obtaining the coordinate point after correction, and recording the coordinate point as r again^wCorrected spatial position r^wAnd importing the three-dimensional model into Matlab, and obtaining the current three-dimensional model by using a drawing function.

Further, as shown in fig. 1: a stepping motor and a radar are placed at the point o, the stepping motor and the radar are inclined to the ground by a certain angle alpha, when the vehicle model is selected, the installation height h of the stepping motor and the radar is known, and the safe distance S can be calculated according to the vehicle, namely the distance from the laser radar to the ground is

FIG. 2 shows: in the figure 1, a step motor and a radar are arranged at a point o, a measuring surface of the radar and a rotating surface of the step motor are arranged at a position perpendicular to an OL measuring surface to obtain a figure 2, a triangle in the figure is a measuring surface of the radar and a rotating surface of the step motor, L is a numerical value in the figure 1, and an angle gamma which the motor should swing can be calculated according to a visual width D which is required in front of a vehicle in practice

From the step angle Δ θ of the stepping motor, and the swing angle, the polar angle β per rotation can be found:

a rectangular coordinate axis as shown in fig. 2 is established, and the polar coordinate is converted into a rectangular coordinate:

x＝L*sin β

y＝L*cos β。

in summary, in the present invention, a one-dimensional single-line lidar is used, and we want to obtain the distance and width of an object, where a stepping motor drives the lidar to rotate, so as to functionally implement the function of a two-dimensional lidar, and flexibly control the rotation of the lidar. At this time, the polar coordinates of the ordered pairs (L, θ) using the laser radar as a pole, the distance as a pole diameter, and the rotation angle of the stepping motor as a pole angle are obtained, and first, the polar coordinates are converted into a rectangular coordinate system using the laser radar as an origin, and the rectangular coordinate system is denoted as Oxy. A vehicle rectangular coordinate system with a laser radar as an origin, a vehicle advancing as an x-axis and a vertical ground as a z-axis is established. Namely, the xy two-dimensional rectangular coordinate system is rotated counterclockwise by 90-alpha degrees around the y axis to obtain a three-dimensional coordinate system. The laser radar rotates 90-alpha around the y axis anticlockwise, a rectangular coordinate system of the laser radar can be identical to a vehicle coordinate system, because the vehicle continuously vibrates and jolts during running, the attitude angle of the vehicle in the running process needs to be obtained so as to compensate coordinate axis deviation caused by vehicle running, an adopted gyroscope measures the attitude angle of the vehicle at each moment, then the coordinate system is corrected through rotating coordinate transformation, but the drift caused by deviation stable output generated by the interference of the gyroscope needs to be compensated, and a proper algorithm is selected to correct the attitude angle. As the vehicle continuously advances, the GPS is required to measure the vehicle speed information of the vehicle at every moment so as to correct the distance between an object in front of the vehicle and the current vehicle. The gyroscope is used for measuring the attitude information of the vehicle and correcting the deviation of coordinate axes caused by continuous vibration and bump in the driving process of the vehicle; correcting the X coordinate through the speed information measured by the GPS to obtain the distances between all points of the object and the vehicle at the current position; and importing the three-dimensional coordinates into Matlab to obtain a three-dimensional model.

The principal features, principles and advantages of the invention have been shown and described above. It will be understood by those skilled in the art that the present invention is not limited to the embodiments described above, which are described in the specification and illustrated only to explain the principles of the invention, but that various changes and modifications may be made therein without departing from the spirit and scope of the invention as expressed in the following claims. The scope of the invention is defined by the appended claims and equivalents thereof.