A laser radar based on a variable field of view stabilized holder and an implementation method thereof

Abstract

The application discloses a laser radar based on a variable field of view stable holder and an implementation method thereof, and relates to the technical field of laser radars, and comprises the laser radar and a variable field of view holder for carrying the laser radar. The variable field of view holder comprises a base, a heading angle platform, a support, a pitch angle motor, a middle frame, a roll angle motor, a carrying platform, a pitch angle rotating bearing, a roll angle rotating bearing and a gyroscope sensor. According to the application, the pitch angle motor and the roll angle motor can compensate for the posture change of a robot caused by ground materials, unevenness, terrain and the like in real time according to the change of the gyroscope sensor. The heading angle platform receives a robot motion instruction and compensates for the mapping distortion caused by the turning of the robot.

Description

Technical Field

[0001] This invention belongs to the field of radar technology and relates to a lidar based on a variable field-of-view stabilized gimbal and its implementation method. Background Technology

[0002] Environmental perception capabilities based on LiDAR are fundamental to the autonomous movement of intelligent robots. Currently, map data acquired by LiDAR is distorted due to robot motion; therefore, there are generally two methods to improve the effectiveness of SLAM:

[0003] 1. Use high-precision sensors.

[0004] 2. Improve the localization and mapping algorithm.

[0005] High-precision sensors are expensive and have low cost-effectiveness, so improvements to positioning and mapping algorithms are generally used to overcome this limitation. However, this method still has certain limitations and cannot avoid errors caused by excessive distortion. Therefore, this invention proposes to improve the hardware of the lidar system by performing real-time corrections on heading angle, pitch angle, and roll angle, thereby overcoming the problem of lidar data distortion caused by robot motion at the hardware level. Summary of the Invention

[0006] The purpose of this invention is to address the shortcomings of existing technologies by providing a lidar based on a variable field-of-view stabilized gimbal.

[0007] A lidar based on a variable field-of-view stabilized gimbal includes:

[0008] LiDAR;

[0009] Variable field-of-view gimbal for mounting lidar;

[0010] The variable field of view gimbal includes: a base (1), a heading platform (2), a bracket (3), a pitch motor (4), a mid-frame (5), a roll motor (6), a mounting platform (7), a pitch rotation bearing (8), a roll rotation bearing (9), and a gyroscope sensor.

[0011] The base (1) is fixedly connected to the robot and moves with the robot.

[0012] The heading angle platform (2) is slidably connected to the base (1); the heading angle platform (2) is driven by a heading angle motor through a rotating shaft to achieve rotation in the horizontal plane.

[0013] The upper surface of the heading angle platform (2) is provided with two supports (3), which can rotate with the platform (2).

[0014] The upper ends of the two brackets (3) are connected to the middle frame (5) through two pitch angle rotating bearings (8); one of the pitch angle rotating bearings (8) is connected to the pitch angle motor drive shaft and is driven by the pitch angle motor (4).

[0015] The middle frame (5) is connected to the mounting platform (7) through two roll angle rotary bearings (9), one of which is connected to the roll angle motor drive shaft and is driven by the roll angle motor (6).

[0016] The gyroscope sensor is used to collect the attitude information of the mounting platform (7).

[0017] Preferably, the base (1) is annular with an annular groove on the inner side; the outer side of the heading angle platform (2) is provided with an annular protrusion that matches the annular groove, so that the heading angle platform (2) can slide along the annular groove;

[0018] Preferably, the heading angle platform (2) is provided with a rotating shaft below it, which is driven by a motor to rotate in the horizontal plane.

[0019] Preferably, when the middle frame (5) is parallel to the heading angle platform (2), the distance between them meets the requirements for the change of the lidar pitch angle.

[0020] Preferably, when the middle frame (5) is parallel to the mounting platform (7), the distance between them meets the requirements for the change of the roll angle of the lidar.

[0021] Preferably, the gyroscope sensor is located between the roll angle motor (6) and the middle frame (5).

[0022] Another object of the present invention is to provide a method for implementing the above-mentioned lidar based on a variable field-of-view stabilized gimbal, specifically:

[0023] Step S1: Level plane calibration

[0024] The middle frame (5) and the mounting platform (7) are placed in a horizontal position and are recorded as the pitch angle reset position and roll angle reset position, respectively. At the same time, the gyroscope sensor is calibrated and the heading angle platform (2) position is preset as the heading angle reset position.

[0025] Step S2: Initialization

[0026] Set the middle frame (5) and the mounting platform (7) to be in a horizontal position or at a certain angle to the horizontal plane and record them as the pitch angle target position and roll angle target position. Record the current heading angle platform (2) position as the heading angle target position.

[0027] Step S3: The robot begins to move, and at the same time, the LiDAR is activated. The LiDAR collects distance data from obstacles to the LiDAR in real time, thereby forming a 3D point cloud map.

[0028] Step S4: The controller determines the road surface flatness in the robot's movement direction based on the acquired 3D point cloud map data, and combines the data collected in real time by the gyroscope sensor to adjust the PID control parameters of the heading angle motor, pitch angle motor (4), and roll angle motor (6) in real time. This enables the control of the swing angle and swing speed of the heading angle platform (2), the middle frame (5), and the mounting platform (7) through the motors when the robot's posture changes, thereby stabilizing the mounting platform (7). Specifically:

[0029] 4-1 Real-time acquisition of the three-axis accelerations AccX(t), AccY(t), and AccZ(t) and the two-axis angular velocities GyroX(t) and GyroY(t) from the gyroscope sensor; where t represents the current time.

[0030] 4-2 Accumulate the angular velocity to obtain the two-axis angles GyroAngleX(t) and GyroAngleY(t);

[0031] GyroAngleX(t)=GyroAngleX(t-1)+GyroX(t)*Δt

[0032] GyroAngleY(t)=GyroAngleY(t-1)+GyroY(t)*Δt

[0033] Where Δt represents the interval time.

[0034] 4-3 Calculate the accelerometer roll angle AccAngleX(t) and pitch angle AccAngleY(t) based on the triaxial acceleration;

[0035]

[0036]

[0037] 4-4 Calculate the current pitch angle P(t) and current roll angle R(t) of the platform (7), and obtain the current heading angle based on the robot's rotation angle;

[0038] P(t)=k1*GyroAngleX(t)+k2*AccAngleX(t)

[0039] R(t)=k1*GyroAngleY(t)+k2*AccAngleY(t)

[0040] Where k1 and k2 are weights;

[0041] 4-5 The controller obtains the robot's angular velocity CmdAngle(t) and calculates the heading angle and the platform's swing angle Yaw(t):

[0042] Yaw(t) = -CmdAngle(t)

[0043] 4-6 The average change j of the height of n point clouds along the robot's motion direction is used to characterize the road surface smoothness along the robot's motion direction:

[0044]

[0045] Where ΔH represents the height difference between adjacent point clouds;

[0046] 4-7 Based on the target position and the current pitch angle P(t), current roll angle R(t), and current heading angle, the pitch angle motor (4), roll angle motor (6), and heading angle motor use an improved PID algorithm to control the output:

[0047]

[0048] Where u q (k) represents the motor output, e q (k) represents the current angle q error, e q (k-1) represents the previous angle q error, Kp, Ki, and Kd are weights, and q = 1, 2, or 3, representing pitch angle, roll angle, and heading angle, respectively.

[0049] The beneficial effects of this invention are:

[0050] 1. This invention employs pitch and roll motors to offset changes in robot posture caused by ground material, unevenness, terrain, etc., in real time based on changes in the gyroscope sensor.

[0051] 2. The heading angle platform of this invention receives robot motion commands and cancels out mapping distortion caused by robot turning.

[0052] 3. The present invention can realize the acquisition of three-dimensional data of lidar by adjusting the middle frame (5) and the mounting platform (7), and the three motors are controlled in real time by PID algorithm, thereby eliminating the distortion problem of the lidar. Attached Figure Description

[0053] Figure 1 This is a schematic diagram of the overall structure of the present invention.

[0054] Figure 2 This is a side view of the bracket of the present invention.

[0055] Figure 3 for Figure 1 Side view. Detailed Implementation

[0056] In the description of this invention, it should be understood that the terms "upper," "lower," "front," "rear," "left," and "right," etc., indicate the orientation or positional relationship based on the orientation or positional relationship shown in the accompanying drawings, and are only for the convenience of describing the invention and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific orientation, or be constructed and operated in a specific orientation, and therefore should not be construed as a limitation of the invention. In this invention, unless otherwise expressly specified and limited, the terms "connection," "fixed," etc., should be interpreted broadly. For example, it can refer to a fixed connection, a detachable connection, or an integral connection; it can refer to a mechanical connection or an electrical connection; it can refer to a direct connection or an indirect connection through an intermediate medium; it can refer to the internal communication between two elements. For those skilled in the art, the specific meaning of the above terms in this invention can be understood according to the specific circumstances.

[0057] The present invention will now be further analyzed with reference to the accompanying drawings.

[0058] like Figure 1-3 As shown, a lidar based on a variable field-of-view stabilized gimbal includes: a lidar and a variable field-of-view gimbal; wherein the variable field-of-view gimbal includes: a base 1, a yaw angle platform 2, a bracket 3, a pitch angle motor 4, a mid-frame 5, a roll angle motor 6, a mounting platform 7, a pitch angle rotation bearing 8, a roll angle rotation bearing 9, and a gyroscope sensor.

[0059] The base 1 is fixedly connected to the robot and moves with the robot.

[0060] The heading angle platform 2 and the base 1 are slidably connected; the base 1 is annular with an annular groove on its inner side; the outer side of the heading angle platform 2 has an annular protrusion that matches the annular groove, so that the heading angle platform 2 can slide along the annular groove. The lower end face of the heading angle platform 2 is provided with a rotating shaft, which is connected to the transmission shaft of the heading angle motor and driven by the heading angle motor to rotate in the horizontal plane.

[0061] Two supports 3 are fixed on the left and right sides of the upper end face of the heading angle platform 2, which can rotate with the platform 2.

[0062] The upper ends of the two brackets 3 are provided with pitch angle rotating bearings 8, and the inner side of the pitch angle rotating bearings 8 is fixed to the middle frame 5; one of the pitch angle rotating bearings 8 is connected to the transmission shaft of the pitch angle motor and is driven by the pitch angle motor 4.

[0063] The middle frame 5 is a circular structure, which is connected to the lower end of the mounting platform 7 through two roll angle rotating bearings 9. One of the roll angle rotating bearings 9 is connected to the roll angle motor drive shaft and is driven by the roll angle motor 6.

[0064] The upper surface of the mounting platform 7 is fixedly connected to the lidar.

[0065] The line connecting the two pitch angle rotary bearings 8 is orthogonal to the line connecting the two roll angle rotary bearings 9.

[0066] The gyroscope sensor is located between the roll angle motor 6 and the mid-frame 5, and is used to collect the attitude information of the mounting platform 7.

[0067] When the middle frame 5 is parallel to the heading angle platform 2, the distance between them meets the requirements for the change of the lidar pitch angle.

[0068] When the middle frame 5 is parallel to the mounting platform 7, the distance between them meets the requirements for the change of the roll angle of the lidar.

[0069] The implementation method of the above-mentioned lidar based on a variable field-of-view stabilized gimbal is as follows:

[0070] Step S1: Level plane calibration

[0071] Position the mid-frame 5 and the mounting platform 7 in a horizontal position, and mark them as the pitch angle reset position and roll angle reset position, respectively; at the same time, calibrate the gyroscope sensor and preset the yaw angle platform 2 position as the yaw angle reset position.

[0072] Step S2: Initialization

[0073] The positions of the middle frame 5 and the mounting platform 7 at a horizontal position or at a certain angle to the horizontal plane are recorded as the target positions of the pitch angle and roll angle, and the current position of the heading angle platform 2 is recorded as the target position of the heading angle.

[0074] Step S3: The robot begins to move, and at the same time, the LiDAR is activated. The LiDAR collects distance data from obstacles to the LiDAR in real time, thereby forming a 3D point cloud map.

[0075] Step S4: The controller determines the road surface flatness in the robot's movement direction based on the acquired 3D point cloud map data, and combines this with real-time data collected by the gyroscope sensor to adjust the PID control parameters of the yaw angle motor, pitch angle motor 4, and roll angle motor 6 in real time. This enables the motors to control the swing angle and swing speed of the yaw angle platform 2, the middle frame 5, and the mounting platform 7 when the robot's posture changes, thereby stabilizing the mounting platform 7. Specifically:

[0076] 4-1 Real-time acquisition of the three-axis accelerations AccX(t), AccY(t), and AccZ(t) and the two-axis angular velocities GyroX(t) and GyroY(t) from the gyroscope sensor; where t represents the current time.

[0077] 4-2 Accumulate the angular velocity to obtain the two-axis angles GyroAngleX(t) and GyroAngleY(t);

[0078] GyroAngleX(t)=GyroAngleX(t-1)+GyroX(t)*Δt

[0079] GyroAngleY(t)=GyroAngleY(t-1)+GyroY(t)*Δt

[0080] Where Δt represents the interval time.

[0081] 4-3 Calculate the accelerometer roll angle AccAngleX(t) and pitch angle AccAngleY(t) based on the triaxial acceleration;

[0082]

[0083]

[0084] 4-4 Calculate the current pitch angle P(t) and current roll angle R(t) of the platform 7, and obtain the current heading angle based on the robot's rotation angle;

[0085] P(t)=k1*GyroAngleX(t)+k2*AccAngleX(t)

[0086] R(t)=k1*GyroAngleY(t)+k2*AccAngleY(t)

[0087] Where k1 and k2 are weights;

[0088] 4-5 The controller obtains the robot's angular velocity CmdAngle(t) and calculates the heading angle and the platform's swing angle Yaw(t):

[0089] Yaw(t) = -CmdAngle(t)

[0090] 4-6 The average change j of the height of n point clouds along the robot's motion direction is used to characterize the road surface smoothness along the robot's motion direction:

[0091]

[0092] Where ΔH represents the height difference between adjacent point clouds;

[0093] 4-7 Based on the target position and the current pitch angle P(t), current roll angle R(t), and current yaw angle, the pitch angle motor 4, roll angle motor 6, and yaw angle motor use an improved PID algorithm to control the output:

[0094]

[0095] Where u q (k) represents the motor output, e q (k) represents the current angle q error, e q (k-1) represents the previous angle q error, Kp, Ki, and Kd are weights, and q = 1, 2, or 3, representing pitch angle, roll angle, and heading angle, respectively.

Claims

1. A method for implementing a lidar based on a variable field-of-view stabilized gimbal, characterized in that, Includes the following steps: Step S1: Level plane calibration The middle frame (5) and the mounting platform (7) are placed in a horizontal position and are recorded as the pitch angle reset position and roll angle reset position, respectively; at the same time, the gyroscope sensor is calibrated and the position of the heading angle platform (2) is preset as the heading angle reset position; Step S2: Initialization Set the middle frame (5) and the mounting platform (7) to be in a horizontal position or at a certain angle to the horizontal plane and record them as the pitch angle target position and roll angle target position. Record the current heading angle platform (2) position as the heading angle target position. Step S3: The robot starts moving, and at the same time the lidar is activated. The lidar collects the distance data from the obstacle to the lidar in real time, and then forms a 3D point cloud map. Step S4: The controller determines the road surface flatness in the robot's movement direction based on the acquired 3D point cloud map data, and combines the data collected in real time by the gyroscope sensor to adjust the PID control parameters of the heading angle motor, pitch angle motor (4), and roll angle motor (6) in real time. This enables the motors to control the swing angle and swing speed of the heading angle platform (2), the middle frame (5), and the mounting platform (7) when the robot's posture changes, thereby stabilizing the mounting platform (7). Specifically: 4-1 Real-time acquisition of the three-axis acceleration (AccX) from the gyroscope sensor AccY AccZ The two-axis angular velocities are GyroX(t) and GyroY(t); where t represents the current time. 4-2 Accumulate the angular velocity to obtain the two-axis angles GyroAngleX(t) and GyroAngleY(t); GyroAngleX(t) = GyroAngleX(t-1) + GyroX(t)* Δt GyroAngleY(t) = GyroAngleY(t-1) + GyroY(t)* Δt Where Δt represents the interval time; 4-3 Calculate the roll angle of the accelerometer based on the triaxial acceleration. accelerometer pitch angle ; 4-4 Calculate the current pitch angle P(t) and current roll angle R(t) of the platform (7), and obtain the current heading angle based on the robot's rotation angle; P(t) = k1 * GyroAngleX(t) + k2 * AccAngleX(t) R(t) = k1 * GyroAngleY(t) + k2 * AccAngleY(t) Where k1 and k2 are weights; 4-5 The controller obtains the robot's rotation angle CmdAngle(t) and calculates the heading angle and the platform's swing angle Yaw(t): Yaw(t) = -CmdAngle(t) 4-6 The average change j of the height of n point clouds along the robot's motion direction is used to characterize the road surface smoothness along the robot's motion direction: in Indicates the height difference between adjacent point clouds; 4-7 Based on the target position and the current pitch angle P(t), current roll angle R(t), and current heading angle, the pitch angle motor (4), roll angle motor (6), and heading angle motor use an improved PID algorithm to control the output: in For motor output, (k) represents the current angle q error. (k-1) represents the previous angle q error, Kp, Ki, and Kd are weights, and q = 1, 2, or 3, representing pitch angle, roll angle, and yaw angle, respectively. The lidar based on a variable field-of-view stabilized gimbal includes: LiDAR; Variable field-of-view gimbal for mounting lidar; The variable field of view gimbal includes: a base (1), a heading platform (2), a bracket (3), a pitch motor (4), a mid-frame (5), a roll motor (6), a mounting platform (7), a pitch rotation bearing (8), a roll rotation bearing (9), and a gyroscope sensor. The base (1) is fixedly connected to the robot; The heading angle platform (2) and the base (1) are slidably connected; the heading angle platform (2) is driven by a heading angle motor through a rotating shaft to achieve rotation in the horizontal plane; The upper surface of the heading angle platform (2) is provided with two supports (3); The upper ends of the two brackets (3) are connected to the middle frame (5) through two pitch angle rotating bearings (8); one of the pitch angle rotating bearings (8) is connected to the pitch angle motor drive shaft and is driven by the pitch angle motor (4); The middle frame (5) is connected to the mounting platform (7) through two roll angle rotary bearings (9), one of which is connected to the roll angle motor drive shaft and is driven by the roll angle motor (6); The gyroscope sensor is used to collect the attitude information of the mounting platform (7).

2. The method according to claim 1, characterized in that... The base (1) is annular with an annular groove on the inner side; the outer side of the heading angle platform (2) is provided with an annular protrusion that matches the annular groove, so that the heading angle platform (2) can slide along the annular groove.

3. The method according to claim 1, characterized in that... When the middle frame (5) is parallel to the heading angle platform (2), the distance between them meets the requirements for the change of the lidar pitch angle.

4. The method according to claim 1, characterized in that... When the middle frame (5) is parallel to the mounting platform (7), the distance between them meets the requirements for the change of the roll angle of the lidar.

5. The method according to claim 1, characterized in that... The gyroscope sensor is located between the roll angle motor (6) and the middle frame (5).

Images