概念

欧拉角其实就是表示向三个方向旋转的角度，比较形象直观。四元数是等价的另一种表示方法，不能直观体现，但是具备一些优点。数学理论实在太复杂，只需要记住以下几条：

• 欧拉角就是绕Z轴、Y轴、X轴的旋转角度，记做ψ、θ、φ，分别为Yaw（偏航）、Pitch（俯仰）、Roll（翻滚）
• 四元数是w,x,y,z，它们的平方和是1
• 欧拉角会出现万向节死锁，比如三维空间中有一个平行于 X 轴的向量，将它绕 Y 轴旋转直到它平行于 Z 轴，这时任何绕 Z 轴的旋转都改变不了该向量的方向

三个旋转方向如图所示：

欧拉角到四元数的转换:

四元数到欧拉角的转换:

描述机器人转弯的角就是绕Z轴的旋转，按照右手法则可以知道机器人向左转为正，右转为负。

ROS中的欧拉角 四元数

geometry_msgs/Quaternion消息

1
2
3
4

float64 x
float64 y
float64 z
float64 w

geometry_msgs/Transform消息

1
2

geometry_msgs/Vector3 translation
geometry_msgs/Quaternion rotation

除了geometry_msgs/Quaternion，还有一个tf::Quaternion类，重要函数：

1
2
3
4
5

Vector3  getAxis () const    //Return the axis of the rotation represented by this quaternion. 
//Set the quaternion using fixed axis RPY. 
void 	setRPY (const tfScalar &roll, const tfScalar &pitch, const tfScalar &yaw)    
//Return an identity quaternion.
tf::Quaternion createIdentityQuaternion()  

还有一个有点特殊的函数，直接返回yaw，但是范围是[0, 2Pi]，所以尽量不要使用

1

tf::Quaternion::getAngle () const

1
2

// [inline, static]
static void tf::quaternionMsgToTF(const geometry_msgs::Quaternion& msg, tf::Quaternion& bt)

四元数——欧拉角

1
2
3
4

tf::Quaternion quat;
double roll, pitch, yaw;
// 赋值给quat，getRPY函数 将quat转为欧拉角
tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);

orientation是表示朝向的消息成员，也就是四元数，类型const geometry_msgs::Quaternion &。 常常配合tf::quaternionMsgToTF函数使用，因为我们一般是先获得geometry_msgs::Quaternion类型

或者用tf2的方式

1
2
3
4
5
6
7

#include “tf2/LinearMath/Matrix3x3.h”

tf2::Matrix3x3 mat(tf2::Quaternion( x, y, z, w) );
mat.getEulerYPR(tf2Scalar&  yaw,
tf2Scalar&  pitch,
tf2Scalar& 	roll,
unsigned int 	solution_number = 1); 

欧拉角 —— 四元数

1
2
3
4
5
6
7
8
9

//只通过yaw计算四元数，用于平面小车
static geometry_msgs::Quaternion  tf::createQuaternionMsgFromYaw(double yaw);

static tf::Quaternion  createQuaternionFromYaw (double yaw)

static tf::Quaternion  createQuaternionFromRPY(double roll, double pitch, double yaw)

static geometry_msgs::Quaternion  createQuaternionMsgFromRollPitchYaw( double roll, 
	double pitch,   double yaw)

python常见的写法:

1
2
3

pos = Pose()
q = tf.transformations.quaternion_from_euler(0, 0, point.z)
pos.orientation = q

1
2
3
4
5
6

pos = Pose()
q = tf.transformations.quaternion_from_euler(0, 0, point.z)
pos.orientation.x = q[0]
pos.orientation.y = q[1]
pos.orientation.z = q[2]
pos.orientation.w = q[3]

报错积累

报错 MSG to TF: Quaternion Not Properly Normalized ， 原因是接受的位姿的四元数不合理。

参考：
四元数与欧拉角（Yaw、Pitch、Roll）的转换

加速度的变化率过大会使机器人底盘电机输出的力矩突变而引起机器人受冲击振荡，为了使TEB生成的轨迹 less jerky, 考虑jerk约束。jerky的意思是有突然的停止和行走现象，也就是trajectory不够平滑稳定。

三台车使用RRT和gmapping联合建图

• 集群机器人可以协作进行地图构建吗？
  两个问题：每个机器人的定位；每个机器人和主节点通信。机器人1把扫描的点加到地图，机器人2把扫描的点加到地图，最后形成完整地图。

• 室内的两个车怎么进行准确定位呢？
  环境不大的话，在天花板上贴二维码，两个机器人都去识别二维码。如果环境很大，初始建图时候，让两个机器人离得近一些，手工量出来二者的相对位置，建图时候在增量定位，这样也可以定位

同事的点云程序必须用PCL-1.8，而ROS默认的是1.7，结果有时出现了版本冲突。

安装了PCL-1.8后，重要的库文件libcostmap_2d.so链接到了pcl-1.8，而不是默认的1.7。此时删除了pcl-1.8之后，再编译会报错:

1
2
3
4

CMake Error at /home/user/catkin_ws/devel/share/costmap_2d/cmake/costmap_2dConfig.cmake:113 (message):
Project 'costmap_2d' specifies '/usr/local/include/pcl-1.8' as an include
dir, which is not found.  It does neither exist as an absolute directory
nor in '/home/user/catkin_ws/src/navigation/costmap_2d//usr/local/include/pcl-1.8'.

我目前的机器人发布tag_detections话题的频率只有0.6Hz，只能换另一台高配置的机器人，频率15Hz。如果机器人走得慢，不必很高。

landmark的观测是 tracking frame，一般是imu或者base_footprint。这个在SensorBridge::HandleLandmarkMessage里已经处理了。

如果关闭后端，landmark机制就无法触发了。

我直接修改apriltag_ros的continuous_detector.cpp中的imageCallback，如果再写一个程序订阅再发布新话题，会增加网络损耗。相机检测到二维码时， 最好有一定的位移或旋转， 这样才能触发landmark机制，rviz里会用MarkerArray标出landmark的位置

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40

markers: 
  - 
    header: 
      seq: 0
      stamp: 
        secs: 1611712040
        nsecs:  13247643
      frame_id: "map"
    ns: "Landmarks"
    id: 0
    type: 2
    action: 0
    pose: 
      position: 
        x: 0.0142477289141
        y: 0.140619658508
        z: 0.337341305453
      orientation: 
        x: 0.987908805705
        y: 0.0191071294953
        z: 0.015915594168
        w: -0.153028765515
    scale: 
      x: 0.2
      y: 0.2
      z: 0.2
    color: 
      r: 0.207129895687
      g: 0.115499980748
      b: 0.769999980927
      a: 1.0
    lifetime: 
      secs: 0
      nsecs:         0
    frame_locked: False
    points: []
    colors: []
    text: ''
    mesh_resource: ''
    mesh_use_embedded_materials: False

参考:
知乎-带Landmark的demo运行和测试
Cartographer——带Landmark的demo的运行和测试

明明安装了库，也在CMake里指定了路径，但还是报这个错误

1. 修改/etc/ld.so.conf 然后刷新

Linux回到这个文件定义的路径里寻找库文件

1
2
3

sudo vim /etc/ld.so.conf
# 添加你的库文件路径
sudo ldconfig

1. 修改环境变量LD_LIBRARY_PATH

1
2

export LD_LIBRARY_PATH=/where/you/install/lib:$LD_LIBRARY_PATH
sudo ldconfig

1. 用ln将需要的so文件链接到/usr/lib或者/lib这两个默认的目录下边

1
2

ln -s /where/you/install/lib/*.so /usr/lib
sudo ldconfig

1. 强行将路径写进去
2. 路径写进去不行的话，在~/.bashrc中，导入export环境变量 LD_LIBRARY_PATH
3. 在/etc/ld.so.conf/目录下，添加该文件的路径，首先命令行输入locate package，定位软件包的位置，然后找个.conf文件，将路径加进去之后，重新sudo ldconfig

运行时动态库的搜索路径的先后顺序是

1. 编译目标代码时指定的动态库搜索路径
2. 环境变量LD_LIBRARY_PATH指定的动态库搜索路径
3. 配置文件/etc/ld.so.conf中指定的动态库搜索路径
4. 默认的动态库搜索路径/lib和/usr/lib

这个顺序是compile gcc时写在程序内的，通常软件源代码自带的动态库不会太多，而我们的/lib和/usr/lib只有root权限才可以修改，如果对环境变量LD_LIBRARY_PATH进行操作就能解决问题，不要改/etc/ld.so.conf，/lib和/usr/lib

设置定位中的初始位姿

使用cartographer的定位模式时， 机器人的初始位姿默认为建图时的起点位姿，机器人运动一段时间后，通过新局部子图与全局地图相匹配，以获取较为理想的位姿，故无需考虑机器人启动位姿，这个定位过程可以在rviz中观察到。

如果不在初始位姿附近，有可能不能定位成功，所以设置启动时的初始位姿，可以加速定位。 初始位姿不能像AMCL那样在Rviz里用箭头给定

定位模式载入的地图在submap中的trajectory id为0。开始定位后，系统默认从起点开始建立子图进行匹配，此时，该子图在submap中的trajectory id为1。要使用start_trajectory服务，必须先终止旧的trajectory为1的建图，调用rosservice call /finish_trajectory， cartographer_ros_msgs/srv/FinishTrajectory.srv如下：

1
2
3

int32 trajectory_id
---
cartographer_ros_msgs/StatusResponse status

然后再开始设定的初始位姿的建图，调用rosservice call /start_trajectory，cartographer_ros_msgs/srv/StartTrajectory.srv如下：

1
2
3
4
5

cartographer_ros_msgs/TrajectoryOptions options
cartographer_ros_msgs/SensorTopics topics
---
cartographer_ros_msgs/StatusResponse status
int32 trajectory_id

更方便的方法是修改源码cartographer_ros/start_trajectory_main.cc，加入#include"cartographer_ros_msgs/FinishTrajectory.h"，在Run()函数开头部分中加入：

1
2
3
4
5
6
7
8
9
10

ros::Duration(0.3).sleep();
ros::NodeHandle node_handle;

ros::ServiceClient finish_trajiectry_client =
    node_handle.serviceClient<cartographer_ros_msgs::FinishTrajectory>(
        kFinishTrajectoryServiceName);

cartographer_ros_msgs::FinishTrajectory f_srv;
f_srv.request.trajectory_id = 1;
finish_trajiectry_client.call(f_srv);

编译完成后，在home_no_odom_localization.launch中添加：

1
2
3
4
5

<node name="cartographer_start_trajectory" pkg="cartographer_ros" type="cartographer_start_trajectory"
      args= "
      -configuration_directory $(find cartographer_ros)/configuration_files
      -configuration_basename home_localization.lua
      -initial_pose {to_trajectory_id=0,relative_pose={translation={0.27,1.12,-0.146},rotation={0,0,1.486,timestamp=0}}}"/>

重点是initial_pose部分： 位姿 is relative to trajectory 0的原点，这是参数to_trajectory_id的作用。ID只能是地图当前所用的，一般是0，最好用/get_trajectory_states服务检查；

relative_pose就是要指定的位姿；timestamp=0非常重要，如果不写这个timestamp，cartographer会默认为当前的时间，结果传入的pose会根据时间做一个位姿变化；rotation是欧拉角（弧度）。

机器人将以initial_pose为起始位姿开辟一条新的trajectory，其id为2。 查看trajectory_node_list话题的ns属性，获知当前运行的trajectory_id。 把机器人开到初始位姿附近，启动launch之后，很快就能定位成功。如果机器人启动的位姿离给定的太远，那么定位过程跟从原点寻找差不多

对于新版本的cartographer_ros，执行

1

rosservice call /start_trajectory "configuration_directory: '' configuration_basename: '' use_initial_pose: false initial_pose: position: {x: 0.0, y: 0.0, z: 0.0} orientation: {x: 0.0, y: 0.0, z: 0.0, w: 0.0} relative_to_trajectory_id: 0"

小强机器人使用 cartographer 在2条trajectory下的定位

要加速定位，需要调整参数POSE_GRAPH.constraint_builder.sampling_ratio， 增大POSE_GRAPH.global_sampling_ratio

纯定位的过程

补充

目前从SLAM里判断是否定位成功是不可能的。

一次纯定位时，无论怎么调整参数都失败，重启机器人后正常了。