Enable real-time high-precision mapping by using a Mid-40 LiDAR with a detection distance of 260m, an accuracy of 2cm, and a non-repetitive scanning pattern, combined with the high-precision position and attitude data provided by the APX-15 inertial navigation module.
The following two pictures show the real-time renderings:
Below is the overall rendering :
The figure below shows the setup of the real-time high-precision mapping system, including the connection and data interaction between modules.
Detailed explanations ( or requirements ) of each module are as follows:
GNSS signal: GPS antenna, providing satellite signals to APX-15;
NTRIP: Network interface, used for NTRIP data of the network RTK service between the wireless network module and APX-15;
PPS: TTL level interface, provides hardware time synchronization signal to Mid-40;
Point Data: Manifold and Livox Converter 2.0 box transmit Mid-40 point cloud data through the network port;
GNRMC: USB to RS232 level, connect to Manifold, provide Mid-40 with the time information corresponding to each PPS pulse;
GSOF: USB to TTL level, connect to Manifold, transmit the attitude and position data output by APX-15;
Power: 24V DC power supply to power Manifold, Mid-40, APX-15;
First of all, you need to prepare the following hardware modules. The remarks have descriptions and links of related products.
Name | Number | Remark |
---|---|---|
APX-15 | 1 | APX-15 UAV |
GNSS Antenna | 1 | Support frequency band: GPS+GLONASS+BeiDou+Galileo |
USB to TTL/RS232 module | 2 | Support USB to TTL level or RS232 level, CP2102 multi-function module |
Manifold 2 | 1 | NVIDIA Jetson TX2 core, Manifold 2-G |
Mid-40 | 1 | It is recommended to use the short cable version, Mid-40 |
Livox Converter 2.0 | 1 | 9~30v power input, TTL level synchronization interface, non-Mid-40 original converter box, Included in Horizon products |
Wireless Network Module | 1 | Connect outdoor 3G/4G signal with RJ45 interface, Huawei WiFi 2 Pro |
Remarks:
The whole system includes 3 core modules:
One end of the Livox Converter box is connected to Mid-40, and the other end has 3 interfaces:
Power supply: connect 24v power supply;
Network port: connect to Manifold 2;
Synchronization interface: the blue part of the synchronization signal line is connected to the APX PPS signal output port (PIN13), and the black part is connected to the APX GND port (PIN12);
Power supply: connect 24v power supply;
Network port: connect to Livox Converter 2.0;
USB to RS232 module: RS232 Rx is connected to APX-15 COM1 Tx (PIN20), RS232 GND is connected to APX-15 GND (PIN12);
USB to TTL module: TTL Rx is connected to APX-15 COM3 Tx (PIN22), TTL GND is connected to APX-15 GND (PIN12);
MMCX interface: connect GNSS antenna
PIN12: GND
PIN13: output PPS signal
PIN20: COM1 Tx, output GNRMC data
PIN22: COM3 Tx, output GSOF INS Full Navigation Info data
PIN27 / 29/31/33: RJ45 wired network interface, used to transmit network RTK service data
PIN42: Power GND
PIN44: power +24v input
In order to enable APX-15 COM1 to output GNRMC data and COM3 to output GSOF data, it is necessary to configure the type and frequency of the corresponding IO output data in the APX-15 management interface (login through a browser). At the same time, in order to use the network RTK service, the NTRIP client account of the network RTK service needs to be added on the corresponding page.
COM1 output configuration:
COM3 (displayed as COM2 in the management interface) output configuration:
Add NTRIP client account:
After the hardware connection and interface configuration, integrate all the modules as compactly as possible in the following manner:
Relationship between imu coordinate and LiDAR coordinate
Picture of the system after installed on the drone:
The calibration between Lidar and imu is unnecessary as they are close to each other. But we do need to compensate the displacement between imu and GNSS by configuring the translation parameter as follows:
The following test runs in Ubuntu 64-bit 16.04 environment.
# Install Livox_SDK
git clone https://github.com/Livox-SDK/Livox-SDK.git
cd Livox-SDK
sudo ./third_party/apr/apr_build.sh
cd build && cmake ..
make
sudo make install
# Install livox_ros_driver
git clone https://github.com/Livox-SDK/livox_ros_driver.git ws_livox/src
cd ws_livox
catkin_make
cd ws_livox/src
git clone https://github.com/Livox-SDK/livox_high_precision_mapping.git
cd ws_livox
catkin_make
source ./devel/setup.sh
LiDAR Configuration
In the livox_ros_driver/config/livox_lidar_config.json file, add Mid-40's SN number in broadcast_code
. The rest configurations are as follows:
"lidar_config": [
{
"broadcast_code": "your device SN",
"enable_connect": true,
"enable_fan": true,
"return_mode": 0,
"coordinate": 0,
"imu_rate": 0,
"extrinsic_parameter_source": 0
}
],
Port Configuration
After connecting the above hardware, there will be two more devices in the /dev/
directory of Manifold 2:
/dev/ttyUSB0
/dev/ttyUSB1
Find the corresponding device name that sends GSOF data and GNRMC data. Suppose /dev/ttyUSB0
is GSOF data and /dev/ttyUSB1
is GNRMC data.
In the gnss_module/apx15/launch/apx15.launch file, the configuration parameter port is /dev/ttyUSB0 and baud is 230400:
<param name="port" value="/dev/ttyUSB0" />
<param name="baud" value="230400" />
In the livox_ros_driver/config/livox_lidar_config.json file, set the configuration parameter enable_timesync to true, device_name to /dev/ttyUSB1, and baudrate_index to 6 (corresponding to 115200 baud rate):
"timesync_config": {
"enable_timesync": true,
"device_name": "/dev/ttyUSB1",
"comm_device_type": 0,
"baudrate_index": 6,
"parity_index": 0
}
Livox-Mapping is a mapping program for Livox LiDAR. The project uses rtk / imu information to stitch together the information output by LiDAR to form a complete point cloud.
First, set a path to save the point cloud in the livox_mapping.launch file.
Modify the extrinsic parameters in livox_mapping_case.cpp if your coordinates are different from ours.
Modify the lidar_delta_time in livox_mapping_case.cpp if your LiDAR data frequency is not 100Hz.;
Directly run the mapping_online.launch file to generate point cloud data in the pointcloud2 format that combines the imu pose and gnss position:
roslaunch livox_mapping mapping_online.launch
In apx_lidar_raw.launch, set rosbag_enable
to true and configure the saving path for the bag file. It will automatically store the three raw data of imu, gnss and point cloud after execution.
roslaunch livox_mapping apx_lidar_raw.launch
After data collection, run livox_mapping.launch to complete the offline mapping.
#Play the rosbag file recorded above
rosbag play xxxxxxx.bag
#Run the mapping program
roslaunch livox_mapping livox_mapping.launch
Imu data is saved in format sensor_msgs::Imu, including quaternion, roll, pitch, yaw data with corresponding accuracy, and the angular velocity and acceleration along x/y/z axis.
#geometry_msgs/Quaternion orientation
float64 x
float64 y
float64 z
float64 w
#float64[9] orientation_covariance
float64[0] # roll, unit: degree
float64[1] # pitch, unit: degree
float64[2] # yaw, unit: degree
float64[3] # roll RMS, unit: degree
float64[4] # pitch RMS, unit: degree
float64[5] # yaw RMS, unit: degree
#geometry_msgs/Vector3 angular_velocity
float64 x # unit: rad/s
float64 y # unit: rad/s
float64 z # unit: rad/s
#geometry_msgs/Vector3 linear_acceleration
float64 x # unit: m/s^2
float64 y # unit: m/s^2
float64 z # unit: m/s^2
Location and navigation data is saved in format sensor_msgs::NavSatFix, including GPS and IMU status, and latitude, longitude, altitude with corresponding accuracy.
#sensor_msgs/NavSatStatus status
int8 status # apx GPS status
uint16 service # apx IMU status
float64 latitude # unit: degree
float64 longitude # unit: degree
float64 altitude # unit: m
#float64[9] position_covariance
float64[0] # North Position RMS, unit: m
float64[1] # East Position RMS, unit: m
float64[2] # Down Position RMS, unit: m