Localization - ArcturusNavigation/all_seaing_vehicle GitHub Wiki

Back: Documentation

The core part of our localization system consists of two sensors, the GPS and the IMU(+Compass).

RTK GPS Setup

The GPS includes a rover (the GPS on the boat), which computes the boat's global position, and a base station (a GPS that's stationary on the shore), that's providing corrections to the rover using a technology called RTK (Real-Time Kinematic positioning). They are both simpleRTK2B models, with the appropriate antenna (using other antennas is possible), equipped with XBee radio modules that enable the communication of corrections between them.

The rover GPS is connected to the Pixhawk via the TELEM1 port (using the daa connector that's on the GPS and an appropriate cable), and is powered via USB (connected to any port of the Jetson), from the GPS+Power micro-USB port (the XBee+Power port should also work, but GPS+Power can also be used for debugging, details below). The base station GPS can be powered using any USB power supply, in the same way as the rover, without the need to pass any data to it after setting it up.

Configuring the GPS is done using the UBlox Center, and configuration on the Pixhawk side is done using the Ardupilot Mission Planner.

Rover Setup

There are two aspects to setting up the whole system to work correctly. We first need to set up the rover GPS to get satellite signals and send the computed global position solution to the Pixhawk. The configuration for that is described here: simpleRTK2B Rover Setup. This should be enough for GPS positioning without RTK (achieving ~50cm global accuracy when it doesn't drift, which may happen for various reasons, so its local precision is not that great).

Note: Depending on the firmware version on the ZED F9P, the maximum frequency that it can run in might be 7GHz, and the Pixhawk requires a minimum of 5GHz to reliably use the GPS data (its internal EKF doesn't work when the GPS is set to 1Hz, and sends a message saying "EKF3 waiting for GPS config data").

Note 2: In the link above, in the Pixhawk configuration section, it sets the parameters COMPASS_ENABLE, COMPASS_USE, COMPASS_USE2, COMPASS_USE3 to 0 and EK3_SRC1_YAW to 2, disabling the compass and instead making the Pixhawk's internal EKF (more details in the IMU+Compass section) use a GPS moving baseline for yaw measurement (using two GPSs some distance apart, on the boat, can provide heading measurements), which we are not doing, since we are using the Pixhawk's compass, so don't change those parameters. Also the GPS_RATE_MS and GPS_RATE_MS2 should be 200 to use 5Hz GPS measurements.

Base Station Setup

The second part is configuring the shoreside GPS as a base station, and making it send connections to the rover over radio link, which is a bit trickier. Some tutorials that describe that process are: Configuring simpleRTK2B Base Station Setup, Radio Link Setup. It is possible to use the survey-in method with a reasonable accuracy threshold to switch to TIME mode, which, combined with the GPS->XBee and XBee->GPS LEDs flashing in the base station and the rover respectively, means it's sending corrections (1.5m should be enough, and it takes 2-5min, set it to be smaller for greater global accuracy and maybe local precision, but takes a ton of time to survey). However, using the fixed position mode with some coordinates (provided by Google Maps :)) that are close enough to the actual base station position (because otherwise it doesn't switch to TIME) should be enough for achieving some local precision, since we don't care that much about global accuracy.

To receive RTCM corrections on the rover side, the messages being sent from the base station (RTCM 1005, 1074, 1084, 1094, 1230) need to be enabled in UART2, at the UBX-CFG-MSG configuration message, both in the base station and the rover GPS. When the rover receives corrections, aside from the LED blinking, the mode displayed in UCenter is 3D/DGNSS/Fix (or Float, which is suboptimal), and when RTK is possible (needs 30-50 satellites, and many of them need to be common between the base station and the rover GPS), it will show RTK Fix, which can, in theory, reach 2-5cm of accuracy, but that depends on environmental conditions, multipath propagation (satellite signals bouncing off neighboring buildings or mountains), and other things.

It is possible to use Mission Planner to configure the base station to use either a fixed position or the survey-in method, and also store previously surveyed positions and use those instead (or set arbitrary ones), by connecting to the GPS+Power port of the simpleRTK2B board and following this guide: Mission Planner RTK/GPS Inject, also: ArduPilot RTK Setup. Some more details on that from the UBlox side are described here: UBlox Mission Planner Autoconfig. However, any configuration done like that doesn't save on the board, so the base station needs to be constantly powered by a computer to not reset the config.

We can also see the rover's position if we connect to the XBee+Power microUSB port of the base station: Rover position from Base Station.

GPS Resources

A guide on how to configure the UBlox ZED F9P (the chip that the Ardusimple RTK2B GPS has) for various use cases (including the respective configuration files) is this: UBlox ZED F9P Configuration, while a simpleRTK2B user guide is provided here: simpleRTK2B User Guide. The ZED F9P datasheet is this: ZED F9P Datasheet. Some more interesting resources are these ones: RTK Base & Rover, Sparkfun Rover Base RTK.

Pixhawk Setup

We are using the Pixhawk 6X for odometry and for controlling the thrusters. The Pixhawk has an INS (Inertial Navigation System), which includes an accelerometer, a gyroscope, and a magnetometer (compass), which are part of an Inertial Measurement Unit (IMU), as well as some interfaces for computing velocities, as well as position and altitude estimates, using those.

Since the linear accelerations from the raw IMU data need to be double-integrated to compute an estimated position, errors accumulate really quickly (especially under the influence of vibrations) they are causing really large drift (moving at 1m/s while stationary) and are not very useful on their own. Thus, a common standard across marine autonomy is to fuse them with the GPS data, providing much more accurate position estimates that are faster than the GPS itself (using the IMU in between GPS updates at 30Hz, and correcting based on GPS measurements at 1-2Hz), and generally pretty good velocity estimates.

This can either be done on the ROS side using the robot_localization package (using the ekf_robot_localization and navsat_transform nodes, integrated as described here: GPS EKF setup), or using the Pixhawk's internal EKF fuser, EKF3. The latter option seems to be more stable, since it also deals better with the reference frames related to the IMU (and compass) measurements, and is easier to setup, changing the following parameters: EKF3 Sources. It is highly recommended to (and no obvious reason not to, except from potential EM interference from the thruster ESCs) use the compass as a yaw source instead of the gyroscope (or a moving baseline, which relies on GPS signals which might be unreliable), since it doesn't drift at all and is really accurate and reliable.

ROS Interface & TF frames

To interface the Pixhawk's IMU (and fused EKF3) data, as well as the GPS positions received from the ArduSimple GPS, with ROS, we are using the mavros node from the respective package, which can provide us with various types of data using multiple plugins, which we can enable and disable, and configure, as needed, using the mavros.yaml config file. The global_position plugin provides the GPS positions (raw in the mavros/global_position/raw/fix topic, and possibly fused in mavros/global_position/global), while the local_position plugin gives us the EKF3 outputs in a local coordinate frame, with the mavros/local_position/odom and mavros/local_position/pose topics. We can get the raw IMU data from the mavros/imu/data topic, but those are not really usable (except perhaps from the compass orientation, if it's included in those, which I'm not sure about).

While the mavros/local_position/odom and the mavros/global_position/raw/fix topics contain all the information we need from the Pixhawk, it is important that we incorporate that into the TF tree of our robot. That way, we can convert between local (robot's reference frame) and global coordinates really easily. REP105 provides guidelines on how this should be done. The way odometry information is incorporated into the TF tree is usually using the odom frame, by providing the odom->base_link transform (fused positions and the GPS position are in this case still considered odometry, abusing the standard a bit, because we are using SLAM to refine those position estimates, and having multiple map-like frames would be both confusing and inconvenient). To do that directly, we are using a simple odometry_publisher node, which combines the raw GPS positions with the fused Pixhawk velocities into a single odometry message, odometry/gps (with the option of using another, or the same, odometry topic for position estimate), and publishes the respective position to the TF tree, enabling the functionality of other nodes relying on that odometry.

LiDAR-Inertial Odometry

raise NotImplementedError