Apollo RTK定位原理简介
RTK定位原理
RTK ( Real - time kinematic,实时动态)载波相位差分技术,工作原理如下:
- 卫星把观测数据给基站,也给车端的移动站;
- 基站根据多个卫星的钟差计算出误差项,然后把误差项传递给车端;
- 车端用这个误差项消除观测误差,得到精准的位置。
Apollo RTK定位
该模块是基于GPS和IMU融合后的信息再进行RTK校准,可以得到一个厘米级误差的定位信息,定位信息包含航向角、速度、加速度、经纬度等等。
输入:
GPS-全球定位系统获取的位置相关数据。
header {
timestamp_sec: 1173545122.22
}
localization {
position { //位置
x: 357.51331791372041
y: 96.165912376788725
z: -31.983237908221781
}
orientation { //方向
qx: 0.024015498296453403
qy: 0.0021656820647661572
qz: -0.99072964388722151
qw: -0.13369120534226134
}
linear_velocity { //线速度
x: -1.5626866011382312
y: -5.9852188341040344
z: 0.024798037277423912
}
angular_velocity { //角速度
x: 0
y: 0
z: 0
}
}
IMU-惯性测量单元获取的速度相关数据。
header {
timestamp_sec: 1173545122.2
}
imu {
linear_acceleration { //线性加速度
x: -0.41125867975442232
y: 0.052264811302594541
z: 0.44301818085493777
}
angular_velocity { //角速度
x: 0.0079919345972956756
y: -0.037232397859742021
z: -0.0075406644688951127
}
}
输出:
车辆的位置信息(Planning模块使用)
车辆的姿态,速度信息(Control模块使用)
header {
timestamp_sec: 1173545122.22
module_name: "localization"
sequence_num: 128
}
pose {
position { //位置
x: 357.51331791372041
y: 96.165912376788725
z: -31.983237908221781
}
orientation { //方向
qx: 0.024015498296453403
qy: 0.0021656820647661572
qz: -0.99072964388722151
qw: -0.13369120534226134
}
linear_velocity { //线速度
x: -1.5626866011382312
y: -5.9852188341040344
z: 0.024798037277423912
}
linear_acceleration { //线性加速度
x: -0.68775567663756187
y: -0.079383290718229638
z: -0.43889982872693695
}
angular_velocity { //角速度
x: 0.0035469168217162248
y: -0.039127693784838637
z: -0.0079623083093763921
}
heading: -1.8388082455104939 //航向
}
RTK定位源码解析
目录结构
核心代码解析
rtk_localization_component.cc
RTK定位模块在"Init"中初始化,每当接收到"localization::Gps"消息就触发执行"Proc"函数。
bool Init() override;
bool Proc(const std::shared_ptr<localization::Gps> &gps_msg) override;
rtk_localization.cc
RTK定位模块的核心实现,例如寻找匹配的IMU、IMU线性插值、GPS和IMU数据融合等等。
void InitConfig(const rtk_config::Config &config);
void GpsCallback(const std::shared_ptr<localization::Gps> &gps_msg);
void GpsStatusCallback(
const std::shared_ptr<drivers::gnss::InsStat> &status_msg);
void ImuCallback(const std::shared_ptr<localization::CorrectedImu> &imu_msg);
bool IsServiceStarted();
void GetLocalization(LocalizationEstimate *localization);
void GetLocalizationStatus(LocalizationStatus *localization_status);
模块初始化
RTK定位模块初始化
bool RTKLocalizationComponent::Init() {
tf2_broadcaster_.reset(new apollo::transform::TransformBroadcaster(node_));
if (!InitConfig()) {
AERROR << "Init Config falseed.";
return false;
}
if (!InitIO()) {
AERROR << "Init Interval falseed.";
return false;
}
// get imu to localizaiton transform
if (!GetLocalizationToImuTF()) {
AERROR << "Get IMU to Localization tranform failed.";
return false;
}
return true;
}
初始化IO
bool RTKLocalizationComponent::InitIO() {
// 1.读取IMU信息,每次接收到localization::CorrectedImu消息,则回调执行“RTKLocalization::ImuCallback”
corrected_imu_listener_ = node_->CreateReader<localization::CorrectedImu>(
imu_topic_, std::bind(&RTKLocalization::ImuCallback, localization_.get(),
std::placeholders::_1));
ACHECK(corrected_imu_listener_);
// 2.读取GPS状态信息,每次接收到GPS状态消息,则回调执行
gps_status_listener_ = node_->CreateReader<drivers::gnss::InsStat>(
gps_status_topic_, std::bind(&RTKLocalization::GpsStatusCallback,
localization_.get(), std::placeholders::_1));
ACHECK(gps_status_listener_);
// 3.发布位置信息和位置状态信息
localization_talker_ =
node_->CreateWriter<LocalizationEstimate>(localization_topic_);
ACHECK(localization_talker_);
localization_status_talker_ =
node_->CreateWriter<LocalizationStatus>(localization_status_topic_);
ACHECK(localization_status_talker_);
return true;
}
Proc
bool RTKLocalizationComponent::Proc(
const std::shared_ptr<localization::Gps>& gps_msg) {
// 1. 通过RTKLocalization处理GPS消息回调
localization_->GpsCallback(gps_msg);
if (localization_->IsServiceStarted()) {
LocalizationEstimate localization;
// 2. 获取定位消息
localization_->GetLocalization(&localization);
LocalizationStatus localization_status;
// 3. 获取定位状态
localization_->GetLocalizationStatus(&localization_status);
// set localization pose at rear axle center
CompensateImuLocalizationExtrinsic(&localization);
// publish localization messages
// 4. 发布位置信息
PublishPoseBroadcastTopic(localization);
// 5. 发布位置转换信息
PublishPoseBroadcastTF(localization);
// 6. 发布位置状态信息
PublishLocalizationStatus(localization_status);
ADEBUG << "[OnTimer]: Localization message publish success!";
}
return true;
}
GpsCallback
GPS回调函数中调用了 PrepareLocalizationMsg。
void RTKLocalization::PrepareLocalizationMsg(
const localization::Gps &gps_msg, LocalizationEstimate *localization,
LocalizationStatus *localization_status) {
// find the matching gps and imu message
double gps_time_stamp = gps_msg.header().timestamp_sec();
CorrectedImu imu_msg;
// 1.寻找最匹配的IMU信息
FindMatchingIMU(gps_time_stamp, &imu_msg);
// 2.根据GPS和IMU信息,给位置信息赋值
ComposeLocalizationMsg(gps_msg, imu_msg, localization);
drivers::gnss::InsStat gps_status;
// 3.查找最近的GPS状态信息
FindNearestGpsStatus(gps_time_stamp, &gps_status);
// 4.根据GPS状态信息,给位置状态信息赋值
FillLocalizationStatusMsg(gps_status, localization_status);
}
线性插值
FindMatchingIMU:寻找最匹配的IMU信息
在队列中找到最匹配的IMU消息,其中区分了队列的第一个,最后一个,以及如果在中间位置则进行插值。插值的时候根据距离最近的原则进行反比例插值。
bool RTKLocalization::FindMatchingIMU(const double gps_timestamp_sec,
CorrectedImu *imu_msg) {
if (imu_msg == nullptr) {
AERROR << "imu_msg should NOT be nullptr.";
return false;
}
std::unique_lock<std::mutex> lock(imu_list_mutex_);
auto imu_list = imu_list_;
lock.unlock();
if (imu_list.empty()) {
AERROR << "Cannot find Matching IMU. "
<< "IMU message Queue is empty! GPS timestamp[" << gps_timestamp_sec
<< "]";
return false;
}
// scan imu buffer, find first imu message that is newer than the given
// timestamp
auto imu_it = imu_list.begin();
for (; imu_it != imu_list.end(); ++imu_it) {
if ((*imu_it).header().timestamp_sec() - gps_timestamp_sec >
std::numeric_limits<double>::min()) {
break;
}
}
if (imu_it != imu_list.end()) { // found one
if (imu_it == imu_list.begin()) {
AERROR << "IMU queue too short or request too old. "
<< "Oldest timestamp[" << imu_list.front().header().timestamp_sec()
<< "], Newest timestamp["
<< imu_list.back().header().timestamp_sec() << "], GPS timestamp["
<< gps_timestamp_sec << "]";
*imu_msg = imu_list.front(); // the oldest imu
} else {
// here is the normal case
auto imu_it_1 = imu_it;
imu_it_1--;
if (!(*imu_it).has_header() || !(*imu_it_1).has_header()) {
AERROR << "imu1 and imu_it_1 must both have header.";
return false;
}
if (!InterpolateIMU(*imu_it_1, *imu_it, gps_timestamp_sec, imu_msg)) {
AERROR << "failed to interpolate IMU";
return false;
}
}
} else {
// give the newest imu, without extrapolation
*imu_msg = imu_list.back();
if (imu_msg == nullptr) {
AERROR << "Fail to get latest observed imu_msg.";
return false;
}
if (!imu_msg->has_header()) {
AERROR << "imu_msg must have header.";
return false;
}
if (std::fabs(imu_msg->header().timestamp_sec() - gps_timestamp_sec) >
gps_imu_time_diff_threshold_) {
// 20ms threshold to report error
AERROR << "Cannot find Matching IMU. IMU messages too old. "
<< "Newest timestamp[" << imu_list.back().header().timestamp_sec()
<< "], GPS timestamp[" << gps_timestamp_sec << "]";
}
}
return true;
}
InterpolateIMU:IMU线性插值
根据上述函数得到两个IMU消息分别对角速度、线性加速度、欧拉角进行插值。原则是根据比例,反比例进行插值。
bool RTKLocalization::InterpolateIMU(const CorrectedImu &imu1,
const CorrectedImu &imu2,
const double timestamp_sec,
CorrectedImu *imu_msg) {
if (!(imu1.header().has_timestamp_sec() &&
imu2.header().has_timestamp_sec())) {
AERROR << "imu1 and imu2 has no header or no timestamp_sec in header";
return false;
}
if (timestamp_sec < imu1.header().timestamp_sec()) {
AERROR << "[InterpolateIMU1]: the given time stamp["
<< FORMAT_TIMESTAMP(timestamp_sec)
<< "] is older than the 1st message["
<< FORMAT_TIMESTAMP(imu1.header().timestamp_sec()) << "]";
*imu_msg = imu1;
} else if (timestamp_sec > imu2.header().timestamp_sec()) {
AERROR << "[InterpolateIMU2]: the given time stamp[" << timestamp_sec
<< "] is newer than the 2nd message["
<< imu2.header().timestamp_sec() << "]";
*imu_msg = imu2;
} else {
*imu_msg = imu1;
imu_msg->mutable_header()->set_timestamp_sec(timestamp_sec);
double time_diff =
imu2.header().timestamp_sec() - imu1.header().timestamp_sec();
if (fabs(time_diff) >= 0.001) {
double frac1 =
(timestamp_sec - imu1.header().timestamp_sec()) / time_diff;
if (imu1.imu().has_angular_velocity() &&
imu2.imu().has_angular_velocity()) {
auto val = InterpolateXYZ(imu1.imu().angular_velocity(),
imu2.imu().angular_velocity(), frac1);
imu_msg->mutable_imu()->mutable_angular_velocity()->CopyFrom(val);
}
if (imu1.imu().has_linear_acceleration() &&
imu2.imu().has_linear_acceleration()) {
auto val = InterpolateXYZ(imu1.imu().linear_acceleration(),
imu2.imu().linear_acceleration(), frac1);
imu_msg->mutable_imu()->mutable_linear_acceleration()->CopyFrom(val);
}
if (imu1.imu().has_euler_angles() && imu2.imu().has_euler_angles()) {
auto val = InterpolateXYZ(imu1.imu().euler_angles(),
imu2.imu().euler_angles(), frac1);
imu_msg->mutable_imu()->mutable_euler_angles()->CopyFrom(val);
}
}
}
return true;
}
InterpolateXYZ
根据距离插值,反比例,即Frac1越小,则越靠近P1,Frac1越大,则越靠近P2。
template <class T>
T RTKLocalization::InterpolateXYZ(const T &p1, const T &p2,
const double frac1) {
T p;
double frac2 = 1.0 - frac1;
if (p1.has_x() && !std::isnan(p1.x()) && p2.has_x() && !std::isnan(p2.x())) {
p.set_x(p1.x() * frac2 + p2.x() * frac1);
}
if (p1.has_y() && !std::isnan(p1.y()) && p2.has_y() && !std::isnan(p2.y())) {
p.set_y(p1.y() * frac2 + p2.y() * frac1);
}
if (p1.has_z() && !std::isnan(p1.z()) && p2.has_z() && !std::isnan(p2.z())) {
p.set_z(p1.z() * frac2 + p2.z() * frac1);
}
return p;
}
填充位置信息
ComposeLocalizationMsg
根据GPS和IMU消息对位置信息进行赋值。
void RTKLocalization::ComposeLocalizationMsg(
const localization::Gps &gps_msg, const localization::CorrectedImu &imu_msg,
LocalizationEstimate *localization) {
localization->Clear();
FillLocalizationMsgHeader(localization);
localization->set_measurement_time(gps_msg.header().timestamp_sec());
// combine gps and imu
auto mutable_pose = localization->mutable_pose();
if (gps_msg.has_localization()) {
const auto &pose = gps_msg.localization();
if (pose.has_position()) {
// position
// world frame -> map frame
mutable_pose->mutable_position()->set_x(pose.position().x() -
map_offset_[0]);
mutable_pose->mutable_position()->set_y(pose.position().y() -
map_offset_[1]);
mutable_pose->mutable_position()->set_z(pose.position().z() -
map_offset_[2]);
}
// orientation
if (pose.has_orientation()) {
mutable_pose->mutable_orientation()->CopyFrom(pose.orientation());
double heading = common::math::QuaternionToHeading(
pose.orientation().qw(), pose.orientation().qx(),
pose.orientation().qy(), pose.orientation().qz());
mutable_pose->set_heading(heading);
}
// linear velocity
if (pose.has_linear_velocity()) {
mutable_pose->mutable_linear_velocity()->CopyFrom(pose.linear_velocity());
}
}
if (imu_msg.has_imu()) {
const auto &imu = imu_msg.imu();
// linear acceleration
if (imu.has_linear_acceleration()) {
if (localization->pose().has_orientation()) {
// linear_acceleration:
// convert from vehicle reference to map reference
Vector3d orig(imu.linear_acceleration().x(),
imu.linear_acceleration().y(),
imu.linear_acceleration().z());
Vector3d vec = common::math::QuaternionRotate(
localization->pose().orientation(), orig);
mutable_pose->mutable_linear_acceleration()->set_x(vec[0]);
mutable_pose->mutable_linear_acceleration()->set_y(vec[1]);
mutable_pose->mutable_linear_acceleration()->set_z(vec[2]);
// linear_acceleration_vfr
mutable_pose->mutable_linear_acceleration_vrf()->CopyFrom(
imu.linear_acceleration());
} else {
AERROR << "[PrepareLocalizationMsg]: "
<< "fail to convert linear_acceleration";
}
}
// angular velocity
if (imu.has_angular_velocity()) {
if (localization->pose().has_orientation()) {
// angular_velocity:
// convert from vehicle reference to map reference
Vector3d orig(imu.angular_velocity().x(), imu.angular_velocity().y(),
imu.angular_velocity().z());
Vector3d vec = common::math::QuaternionRotate(
localization->pose().orientation(), orig);
mutable_pose->mutable_angular_velocity()->set_x(vec[0]);
mutable_pose->mutable_angular_velocity()->set_y(vec[1]);
mutable_pose->mutable_angular_velocity()->set_z(vec[2]);
// angular_velocity_vf
mutable_pose->mutable_angular_velocity_vrf()->CopyFrom(
imu.angular_velocity());
} else {
AERROR << "[PrepareLocalizationMsg]: fail to convert angular_velocity";
}
}
// euler angle
if (imu.has_euler_angles()) {
mutable_pose->mutable_euler_angles()->CopyFrom(imu.euler_angles());
}
}
}
查找最近的GPS状态信息
获取最近的GPS状态信息,这里实现的算法是遍历查找离"gps_time_stamp"最近的状态。
bool RTKLocalization::FindNearestGpsStatus(const double gps_timestamp_sec,
drivers::gnss::InsStat *status) {
CHECK_NOTNULL(status);
std::unique_lock<std::mutex> lock(gps_status_list_mutex_);
auto gps_status_list = gps_status_list_;
lock.unlock();
double timestamp_diff_sec = 1e8;
auto nearest_itr = gps_status_list.end();
for (auto itr = gps_status_list.begin(); itr != gps_status_list.end();
++itr) {
double diff = std::abs(itr->header().timestamp_sec() - gps_timestamp_sec);
if (diff < timestamp_diff_sec) {
timestamp_diff_sec = diff;
nearest_itr = itr;
}
}
if (nearest_itr == gps_status_list.end()) {
return false;
}
if (timestamp_diff_sec > gps_status_time_diff_threshold_) {
return false;
}
*status = *nearest_itr;
return true;
}
位置状态赋值
FillLocalizationStatusMsg
void RTKLocalization::FillLocalizationStatusMsg(
const drivers::gnss::InsStat &status,
LocalizationStatus *localization_status) {
apollo::common::Header *header = localization_status->mutable_header();
double timestamp = apollo::cyber::Clock::NowInSeconds();
header->set_timestamp_sec(timestamp);
localization_status->set_measurement_time(status.header().timestamp_sec());
if (!status.has_pos_type()) {
localization_status->set_fusion_status(MeasureState::ERROR);
localization_status->set_state_message(
"Error: Current Localization Status Is Missing.");
return;
}
参考资料
1、RTK原理相关:https://developer.baidu.com/article/detail.html?id=290583
2、解算相关:https://www.zhihu.com/column/c_1114864226103037952
3、插值相关:https://zhuanlan.zhihu.com/p/108853312
4、源码解读1:https://apollo.baidu.com/community/article/15
5、源码解读2:https://www.findmb.top/posts/37997/