园区场景自动驾驶-利用GPS实现当地地理坐标系下的建图与定位
概述
主要实现如下的功能：
1、利用gps信息，修正题图为当地地理ENU坐标系；
2、在纯定位模式下，利用gps与imu(rtk)信息，实现车辆在地图中的初始定位；
3、根据每个位置点处的卫星信号状态的质量，调整卫星定位的约束方程权重；
1、利用gps建立基于ENU坐标系的map
1.1、利用gps修正map坐标与ENU坐标系重合
在optimization_problem_3d.cc文件中做如下更改。
在函数OptimizationProblem3D::Solve()中增加选择编译的宏定义USE_GPS_ALIGNMENT_LOCAL_FREAME；主要更改的地方包含：
（1） 首个submap的位姿参数不为常数，全部参与被优化；
#if (!USE_GPS_ALIGNMENT_LOCAL_FREAME)
if (first_submap) {
first_submap = false;
// Fix the first submap of the first trajectory except for allowing
// gravity alignment.
// 重力是否需要对齐与此参数是否内优化的关系在哪里体现？ConstantYawQuaternionPlus吗？
C_submaps.Insert(
submap_id_data.id,
CeresPose(submap_id_data.data.global_pose,
translation_parameterization(),
common::make_unique<ceres::AutoDiffLocalParameterization<
ConstantYawQuaternionPlus, 4, 2>>(),
&problem));
// 当利用ENU坐标系时，是否需要调整首节点的位置呢？
// （需要：不仅要对齐x，y轴，也要对齐原点，因为采样时刻的不同步，首节点不一定时首次GPS的位置点）
problem.SetParameterBlockConstant(
C_submaps.at(submap_id_data.id).translation());
}
else
#endif // USE_GPS_ALIGNMENT_LOCAL_FREAME
{
C_submaps.Insert(
submap_id_data.id,
CeresPose(submap_id_data.data.global_pose,
translation_parameterization(),
common::make_unique<ceres::QuaternionParameterization>(),
&problem));
}
（2） 去掉原有的C_fixed_frames约束方程，新建立C_enu_frames约束方程；
伪代码表述：
// 约束方程： I = fixed_frame_pose_in_map.inverse()*node_pose_in_map;
// constraint_pose = C_enu_frames.inverse()*C_nodes.pose;
// 或者：C_enu_frames = I*C_nodes.pose;
源码表述：
#if (USE_GPS_ALIGNMENT_LOCAL_FREAME)
std::map<NodeId, CeresPose> C_enu_frames;
for (auto node_it = node_data_.begin(); node_it != node_data_.end();) {
const int trajectory_id = node_it->id.trajectory_id;
const auto trajectory_end = node_data_.EndOfTrajectory(trajectory_id);
if (!fixed_frame_pose_data_.HasTrajectory(trajectory_id)) {
node_it = trajectory_end;
continue;
}
double last_x(0), last_y(0);
int use_fix_num(0);
double s_sum(0);
bool first_points = true;
double yawl_from_fix(0);
std::unique_ptr<transform::Rigid3d> fixed_frame_pose_last;
for (; node_it != trajectory_end; ++node_it) {
const NodeId node_id = node_it->id;
const NodeSpec3D& node_data = node_it->data;
if (first_points)
{
first_points = false;
last_x = node_data.global_pose.translation().x();
last_y = node_data.global_pose.translation().y();
}
constexpr double ds_min_gps = 0.1;
double ds = hypot(node_data.global_pose.translation().x() - last_x,
node_data.global_pose.translation().y() - last_y);
if (ds < ds_min_gps)
{
// printf("ds:%1.2f, ", ds);
continue;
}
// 基于fixed_frame_pose_data_做插补运算
// const std::unique_ptr<transform::Rigid3d> fixed_frame_pose =
// Interpolate(fixed_frame_pose_data_, trajectory_id, node_data.time);
int8_t gps_status = GPS_STATUS_NO_FIX;
const std::unique_ptr<transform::Rigid3d> fixed_frame_pose =
Interpolate_with_gps_status(fixed_frame_pose_data_, trajectory_id, node_data.time, gps_status);
if (fixed_frame_pose == nullptr) {
// printf("fixed_frame_pose is nullptr, ");
continue;
}
double k_weight_by_gps_status = 1.0;
switch (gps_status)
{
case GPS_STATUS_FIX:
k_weight_by_gps_status = 0.2;
break;
case GPS_STATUS_SBAS_FIX:
k_weight_by_gps_status = 0.5;
break;
case GPS_STATUS_GBAS_FIX:
k_weight_by_gps_status = 1.0;
break;
default:
k_weight_by_gps_status = 0.0;
break;
}
last_x = node_data.global_pose.translation().x();
last_y = node_data.global_pose.translation().y();
if (fixed_frame_pose_last != nullptr)
{
yawl_from_fix = atan2(fixed_frame_pose->translation().y() - fixed_frame_pose_last->translation().y(),
fixed_frame_pose->translation().x() - fixed_frame_pose_last->translation().x());
// printf("yawl_from_fix = %1.4f degree, ",yawl_from_fix*180.0/M_PI);
fixed_frame_pose_last = common::make_unique<transform::Rigid3d>(*fixed_frame_pose);
}
else
{
// printf("fixed_frame_pose_last is nullptr, ");
fixed_frame_pose_last = common::make_unique<transform::Rigid3d>(*fixed_frame_pose);
continue;
}
// 一次有效的约束计算，更新x，y记录值；
use_fix_num ++;
s_sum += ds;
// ----------------------------------------------------------------------
C_enu_frames.emplace(
std::piecewise_construct, std::forward_as_tuple(node_id),
std::forward_as_tuple(
transform::Rigid3d(),
nullptr,
// common::make_unique<ceres::AutoDiffLocalParameterization<
// YawOnlyQuaternionPlus, 4, 1>>(),
// common::make_unique<ceres::AutoDiffLocalParameterization<
// ConstantYawQuaternionPlus, 4, 2>>(),
common::make_unique<ceres::QuaternionParameterization>(),
&problem));
problem.SetParameterBlockConstant(
C_enu_frames.at(node_id).rotation());
problem.SetParameterBlockConstant(
C_enu_frames.at(node_id).translation());
const Constraint::Pose constraint_pose{
transform::Rigid3d(
fixed_frame_pose->translation(),
Eigen::AngleAxisd(
yawl_from_fix,
Eigen::Vector3d::UnitZ())),
options_.fixed_frame_pose_translation_weight()*k_weight_by_gps_status,
options_.fixed_frame_pose_rotation_weight()*k_weight_by_gps_status};
// 约束方程： I = fixed_frame_pose_in_map.inverse()*node_pose_in_map;
// constraint_pose = C_enu_frames.inverse()*C_nodes.pose;
// 或者：C_enu_frames = I*C_nodes.pose;
problem.AddResidualBlock(
SpaCostFunction3D::CreateAutoDiffCostFunction(constraint_pose),
nullptr /* loss function */,
C_enu_frames.at(node_id).rotation(),
C_enu_frames.at(node_id).translation(),
C_nodes.at(node_id).rotation(), C_nodes.at(node_id).translation());
}
printf("\nuse_fix_num:%d, s_sum:%1.3f\n\n", use_fix_num, s_sum);
}
#endif
注：这里包含了根据卫星信号质量调整约束权重的功能，将在后面小节详述。
1.2、保存地图相关参数
地图的原点为首次有效GPS的当地地理坐标系，在实时运行过程中gps要投影到该ENU坐标系下，即地图坐标系下，在定位过程中也需要这样使用，所以要记录ecef_frame_to_enu_frame的值；
纯定位模式下，新的轨迹起点时相对于轨迹0的起点坐标系，在原始的cartographer建图时，轨迹0起点坐标就是map坐标原点，更改后两个存在差异，所以要记录此值，以便定位时可以迅速的寻找起点；
在文件node.cc中做如下修改。
增加函数void Node::rec_map_origin_and_traj_pose(const int trajectory_id)，在停止轨迹的前，将关键变量记录下来：
has_rec_origin_ = false;
if (map_builder_bridge_.sensor_bridge(trajectory_id)->has_ecef_to_local_frame())
{
has_rec_origin_ = true;
rec_origin_fixed_to_local_frame_ =
map_builder_bridge_.sensor_bridge(trajectory_id)->get_ecef_to_local_frame();
rec_traj_pose_ = map_builder_bridge_.GetSubmapList().submap.begin()->pose;
}
增加函数void Node::save_map_yaml_file(const std::string& filename)，在保存地图时，将关键变量保存在同名.yaml后缀的文件中：
if (has_rec_origin_)
{
printf("\n\nfilename:%s\n \n",
filename.c_str());
std::string mapmetadatafile = filename + ".yaml";
printf("Writing map occupancy data to %s", mapmetadatafile.c_str());
FILE* yaml = fopen(mapmetadatafile.c_str(), "w");
fprintf(yaml, "ecef_frame_to_enu_frame:\n");
fprintf(yaml, " translation:\n {x: %1.15f,\n y: %1.15f,\n z: %1.15f}\n",
rec_origin_fixed_to_local_frame_.translation().x(),
rec_origin_fixed_to_local_frame_.translation().y(),
rec_origin_fixed_to_local_frame_.translation().z());
fprintf(yaml, " rotation:\n {x: %1.15f,\n y: %1.15f,\n z: %1.15f,\n w: %1.15f}\n",
rec_origin_fixed_to_local_frame_.rotation().x(),
rec_origin_fixed_to_local_frame_.rotation().y(),
rec_origin_fixed_to_local_frame_.rotation().z(),
rec_origin_fixed_to_local_frame_.rotation().w());
fprintf(yaml, "map_traj0:\n");
fprintf(yaml, " translation:\n {x: %1.15f,\n y: %1.15f,\n z: %1.15f}\n",
rec_traj_pose_.position.x,
rec_traj_pose_.position.y,
rec_traj_pose_.position.z);
fprintf(yaml, " rotation:\n {x: %1.15f,\n y: %1.15f,\n z: %1.15f,\n w: %1.15f}\n",
rec_traj_pose_.orientation.x,
rec_traj_pose_.orientation.y,
rec_traj_pose_.orientation.z,
rec_traj_pose_.orientation.w);
fclose(yaml);
printf("\nDone\n\n\n");
}
else
{
printf("\n\nNO UTM_to_LOCAL when stop traj!!!\n\n\n");
}
2、利用GPS和RTK实现初始定位
2.1、定位模式设定enu坐标系
纯定位模式下，加载map的配置文件正常后，设置地图的enu坐标为本次定位的坐标；
if (base_on_map)
{
node.set_traj_ecef_to_local_frame(traj_id, ecef_to_enu);
}
base_on_map变量为true的条件是：
有地图的yaml文件，且有gps信号；
if ((!FLAGS_load_state_filename.empty())&&(is_gnss_fix_ready))
{
....
base_on_map = true;
}
要执行set_traj_ecef_to_local_frame()函数，需要做如下的更改：
1、更改node中的StartTrajectoryWithDefaultTopics()函数，返回新建的轨迹编号；
2、在node中增加函数set_traj_ecef_to_local_frame()设置坐标变换关系；
3、在sensor_bridge中增加函数set_ecef_to_local_frame()设置坐标变换关系；
2.2、粗略初始定位
订阅到惯组的gps信息、imu（含RTK）的朝向信息后，将初始位置点投影到地图坐标系中；
在node_main.cc中做如下的修改：
根据传递的ros参数set_init_pose_by_gps_pose，需要使用gps进行初始定位时，则
创建订阅话题：
ros::Subscriber fix_sub = node.node_handle()->subscribe("/fix", 1, fix_callback);
ros::Subscriber imu_sub = node.node_handle()->subscribe("/imu", 1, imu_callback);
执行任务
car_in_enu_position = ecef_to_enu*ecef_position_;
car_in_traj0_pose = enu_to_traj0*car_in_enu_pose;
在执行完毕任务后，删除话题订阅；
fix_sub.shutdown();
fix_sub.~Subscriber();
imu_sub.shutdown();
imu_sub.~Subscriber();
2.3、todo：增加精确初始定位
以上的初始定位是基于gps和rtk的数据进行初始定位，精确较差。要实现精确的初始定位，可以结合激光雷达点云与点云地图匹配，做更精确的定位。
3、根据微信信号质量调整约束权重
3.1、更改原有数据结构
在原有的卫星定位数据的结构中增加信号质量的变量；
在…/cartographer/cartographer/sensor/fixed_frame_pose_data.h文件中，对结构体FixedFramePoseData进行修改，增加信号质量状态：
struct FixedFramePoseData {
common::Time time;
common::optional<transform::Rigid3d> pose;
int8_t gps_status;
};
卫星信号质量gps_status的定义如下：
STATUS_NO_FIX = -1, // 无固定解
STATUS_FIX = 0, // 未增强的定位信号
STATUS_SBAS_FIX = 1, // 基于卫星信号增强的定位信号
STATUS_GBAS_FIX = 2, // 基于地面基站增强的定位信号
3.2、接收卫星信号话题时给信号质量赋值
在cartographer_ros/cartographer_ros/cartographer_ros/sensor_bridge.cc文件的SensorBridge::HandleNavSatFixMessage()函数中，更改原有的赋值方式，增加了第三个参数——信号状态。
当不存在固定解时，赋值为单位矩阵：
trajectory_builder_->AddSensorData(
sensor_id, carto::sensor::FixedFramePoseData{
time, carto::common::optional<Rigid3d>(), msg->status.status});
当存在固定解时，赋值为实际位置点+单位四元数：
trajectory_builder_->AddSensorData(
sensor_id,
carto::sensor::FixedFramePoseData{
time, carto::common::optional<Rigid3d>(Rigid3d::Translation(
ecef_to_local_frame_.value() *
LatLongAltToEcef(msg->latitude, msg->longitude,
0.0*msg->altitude))),
msg->status.status});
执行以上步骤，就可以按原有的传递方式，将卫星的信号质量状态传递到程序的内部数据中，供回环优化算法使用。
3.3、在回环优化中考虑卫星信号质量和信号丢失的情况
- 信号丢失段的判断
在cartographer/cartographer/mapping/internal/optimization/optimization_problem_3d.cc文件中增加Interpolate_with_gps_status()函数，相比与原有的Interpolate()函数做了如下更改：
1、增加了插补时刻前后两点的信号质量，当STATUS_NO_FIX状态时，表明传递进来的数据不可靠，则插补返回空；
2、当插补时间前后两点的距离过大，大于lose_gps_distance设定值（默认1.0m）时，认为这个时间段内存在信号丢失，则插补返回空；
// For fixed frame pose with status.
#define GPS_STATUS_NO_FIX (-1)
#define GPS_STATUS_FIX (0)
#define GPS_STATUS_SBAS_FIX (1)
#define GPS_STATUS_GBAS_FIX (2)
std::unique_ptr<transform::Rigid3d> Interpolate_with_gps_status(
const sensor::MapByTime<sensor::FixedFramePoseData>& map_by_time,
const int trajectory_id, const common::Time time, int8_t& gps_status) {
const auto it = map_by_time.lower_bound(trajectory_id, time);
if (it == map_by_time.EndOfTrajectory(trajectory_id) ||
!it->pose.has_value()) {
return nullptr;
}
if (it->gps_status == GPS_STATUS_NO_FIX) return nullptr;
if (it == map_by_time.BeginOfTrajectory(trajectory_id)) {
if (it->time == time) {
gps_status = it->gps_status;
return common::make_unique<transform::Rigid3d>(it->pose.value());
}
return nullptr;
}
const auto prev_it = std::prev(it);
if (prev_it->gps_status == GPS_STATUS_NO_FIX) return nullptr;
if (prev_it->pose.has_value()) {
// 增加距离判断，两点之间的距离大于设定值，则认为这段位置内不存在gps信号：
constexpr double lose_gps_distance = 1.0;
const double l_two_points
= sqrt(pow(prev_it->pose.value().translation().x() - it->pose.value().translation().x(), 2)
+ pow(prev_it->pose.value().translation().y() - it->pose.value().translation().y(), 2)
+ pow(prev_it->pose.value().translation().z() - it->pose.value().translation().z(), 2));
if (l_two_points > lose_gps_distance)
{
printf("\n>>>>>WARN: gps data discard because of l_two_points = %1.2f\n", l_two_points);
printf("gps1[x:%1.2f, y:%1.2f, z:%1.2f]\n",
prev_it->pose.value().translation().x(),
prev_it->pose.value().translation().y(),
prev_it->pose.value().translation().z());
printf("gps2[x:%1.2f, y:%1.2f, z:%1.2f]\n\n",
it->pose.value().translation().x(),
it->pose.value().translation().y(),
it->pose.value().translation().z());
return nullptr;
}
gps_status = std::min(prev_it->gps_status, it->gps_status);
// --- end ---
return common::make_unique<transform::Rigid3d>(
Interpolate(transform::TimestampedTransform{prev_it->time,
prev_it->pose.value()},
transform::TimestampedTransform{it->time, it->pose.value()},
time)
.transform);
}
return nullptr;
}
- 根据信号质量的约束权重调整
在cartographer/cartographer/mapping/internal/optimization/optimization_problem_3d.cc文件的OptimizationProblem3D::Solve()函数中，使用卫星信号时，增加根据信号质量调整权重：
double k_weight_by_gps_status = 1.0;
switch (gps_status)
{
case GPS_STATUS_FIX:
k_weight_by_gps_status = 0.2;
break;
case GPS_STATUS_SBAS_FIX:
k_weight_by_gps_status = 0.5;
break;
case GPS_STATUS_GBAS_FIX:
k_weight_by_gps_status = 1.0;
break;
default:
k_weight_by_gps_status = 0.0;
break;
}
在建立约束方程时，使用该权重：
const Constraint::Pose constraint_pose{
transform::Rigid3d(
fixed_frame_pose->translation(),
Eigen::AngleAxisd(
yawl_from_fix,
Eigen::Vector3d::UnitZ())),
options_.fixed_frame_pose_translation_weight()*k_weight_by_gps_status,
options_.fixed_frame_pose_rotation_weight()*k_weight_by_gps_status};
通过以上的设置，就可以达到不同卫星信号质量下的不同权重赋值。注意，信号质量权重与信号质量对应的误差带大小应该关联。