教程
輪式里程計視頻講解
里程計分類
ros--odometry
什么是里程計
里程計是一種利用從移動傳感器獲得的數據來估計物體位置隨時間的變化而改變的方法。該方法被用在許多機器人系統來估計機器人相對于初始位置移動的距離。
注意：里程計是一套算法，不是物理硬件。
作用
機器人相對于初始位置移動的位姿信息。
在ros中，通過發布里程計話題odom，其他模塊可以訂閱這個話題的數據，獲取機器人實時的位姿信息。
里程計和TF
既然里程計話題發布機器人的位姿信息，TF也是發布機器人位姿信息，為什么不發布里程計話題就可以了，還要計算出里程之后再發布tf？
| 特性 | 里程計話題 | TF（Transform） |
|---|---|---|
| 數據內容 | 包含位姿（pose）和速度（twist），通常帶協方差 | 僅包含坐標系間的剛性變換（平移+旋轉） |
| 主要用途 | 提供里程計的完整運動狀態（供算法使用） | 提供坐標系間的空間關系（供可視化、傳感器融合等使用） |
| 消費者 | 導航算法（如EKF、路徑規劃） | RViz、傳感器數據處理節點、多坐標系協作模塊 |
| 更新頻率 | 通常較低（10-50Hz，取決于傳感器） | 通常較高（與TF廣播頻率一致，可能100Hz+） |
| 話題名稱 | 不固定，不同的機器人可以使用不同的里程計話題名稱，創建話題時需要質地的那個話題名稱。 訂閱的時候也通過話題名稱訂閱。 | 固定話題名稱，/tf和/tf_static。所有的TF變化都通過這兩個話題發布，再通過looup_transform訂閱指定兩個坐標系的TF。 |
| RVIZ插件 | Odometry插件 | TF插件 |
RVIZ對TF的依賴要更強些。
里程計在ros中的作用
ros2中的里程計odometry：
# 包含父ID
std_msgs/Header header# 子ID，即姿勢所在的坐標系
string child_frame_id# 通常相對于父坐標的估計姿態.
geometry_msgs/PoseWithCovariance pose# 在子坐標系中的線速度和角速度.
geometry_msgs/TwistWithCovariance twist
ros2中的里程計是需要通過傳感器比如編碼器，IMU等獲取機器人的線速度，角速度，加速度等信息來完成填充odometry接口中的數據，之后再以話題的方式發布出去。
可見ros2中的里程計不僅可以發布機器人的當前位姿，還包括機器人的線速度和角速度等信息。
里程計的分類
輪式里程計
輪式里程計是通過電機編碼器反饋電機轉速，再利用電機轉速和減速比等物理參數計算出小車輪子的轉速，再通過運動學正解計算小車整體的線速度和角速度，也就是里程計需要發布的參數的。
角度累計
不斷累計機器人繞Y軸轉動的角度yam，其實根據三角函數計算式可知，超過180度就是計算-180度內的角度，超過-180度就是計算180度內的角度，所以，機器人轉一圈的范圍也就是0~180或者0~-180之間，為了防止角度溢出，我們只需要控制角度累計的角度再-180~180之間即可。
兩輪差速位置累計
delta_th = vth * dt;
odom_th_ += delta_th; // 角度累計--姿態累計
delta_x = vx * cos(odom_th_) * dt; // 位姿累計
delta_y = vx * sin(odom_th_) * dt;
適用對象： 專為 兩輪差分驅動機器人 設計（如TurtleBot），假設機器人 只有X方向線速度（vx）和Z軸角速度（vth），Y方向速度始終為0。
對于差分驅動機器人：
-
Y方向速度恒為0（不能橫向移動），因此
Y * sin(pZ)和Y * cos(pZ)項為0。 -
若
dt很小，角度變化delta_th對位移的影響可忽略，此時方法1是方法2的近似。
通用2D位置累計
pZ += Z * Sampling_Time; // 角度累計
pX += (X * cos(pZ) - Y * sin(pZ)) * Sampling_Time; // 位置累計
pY += (X * sin(pZ) + Y * cos(pZ)) * Sampling_Time;適用對象： 適用于 任何2D平面運動的機器人（如全向輪、麥克納姆輪機器人），支持 X/Y方向線速度（X, Y）和Z軸角速度（Z）。
哪種更準確？
-
如果機器人 嚴格兩輪差分驅動，且
dt足夠小： 兩種方法等效（因為Y=0，方法2退化為方法1）。 -
如果機器人 有Y方向速度 或
dt較大： 必須用方法2，否則會忽略旋轉對位移的耦合影響。 -
如果機器人 快速旋轉（
vth較大）： 方法2更準確，方法1會引入誤差。
里程累計
里程累計包括位置，即X軸位置和Y軸位置累計和角度累計，三者共同表示機器人的位姿。
注意：一定要線角度累計，再利用累計的角度計算位置累計。
慣性里程計
視覺里程計
激光里程計
ros中的實現--odometry
odometry:
# 包含父ID
std_msgs/Header header# 子ID，即姿勢所在的坐標系
string child_frame_id# 通常相對于父坐標的估計姿態.
geometry_msgs/PoseWithCovariance pose# 在子坐標系中的線速度和角速度.
geometry_msgs/TwistWithCovariance twist
odom.child_frame_id = robot_frame_id; #里程計TF子坐標
odom.twist.twist.linear.x = X; # X方向速度
odom.twist.twist.linear.y = Y; # Y方向速度
odom.twist.twist.angular.z = Z; # 繞Z軸角速度
角速度可以通過IMU獲取，或者通過編碼器和運動學正解獲取機器人整體角速度和線速度，那么XY方向的線速度怎么獲取呢？
在輪式里程計中，計算出機器人整體的角速度和線速度之后，可以利用角速度計算出這段時間內的總的旋轉角度，再通過總的旋轉角度計算出線速度再XY軸上各自的分量，各自分速度再和時間積分就是位置里程。
delta_th = vth * dt;
odom_th_ += delta_th; // 角度累計--姿態累計
delta_x = vx * cos(odom_th_) * dt; // 位姿累計
delta_y = vx * sin(odom_th_) * dt;里程計和TF廣播
里程計是如何完成TF廣播的？
里程計代表機器人的位置相對于初始位置的累加變化，即機器人當前位姿。
TF(動態)廣播器記錄和發布的是兩個坐標系之間的實時平移和旋轉的坐標系變換關系。
tf2::Quaternion q;q.setRPY(0, 0, odom_th_); // odom_th_是yam旋轉角累計// 發布里程計話題，用于算法odom_publisher_->publish(odom_msg);// 之后發布TF，用于RVIZ坐標變換geometry_msgs::msg::TransformStamped t;t.header.stamp = this->get_clock()->now();t.header.frame_id = "odom";t.child_frame_id = "base_footprint";t.transform.translation.x = odom_x_;t.transform.translation.y = odom_y_;t.transform.translation.z = 0.0;t.transform.rotation.x = q[0];t.transform.rotation.y = q[1];t.transform.rotation.z = q[2];t.transform.rotation.w = q[3];if (pub_odom_) {// 廣播里程計TFtf_broadcaster_->sendTransform(t);}里程計話題發布
1，創建話題
2，創建話題消息
消息類型：
nav_msgs/msg/Odometry
# 消息中的 pose（位姿） 應該由 header.frame_id 給出的坐標系指定。
# 消息中的 twist（速度信息） 應該由 child_frame_id 給定的坐標系來指定。# 包含位姿的父坐標系的 frame_id。
- 這是一個標準的 ROS 2 消息頭（Header），包含時間戳和父坐標系 ID。
- `header.frame_id` 表示 **位姿（pose）所在的參考系**，也就是“父坐標系”。
std_msgs/Header headerbuiltin_interfaces/Time stampint32 secuint32 nanosecstring frame_idstring child_frame_id# 估計的位姿，通常是相對于一個固定的全局世界坐標系。
geometry_msgs/PoseWithCovariance posePose posePoint positionfloat64 xfloat64 yfloat64 zQuaternion orientationfloat64 x 0float64 y 0float64 z 0float64 w 1float64[36] covariance# 估計的線速度和角速度是相對于子坐標系的。
geometry_msgs/TwistWithCovariance twistTwist twistVector3 linearfloat64 xfloat64 yfloat64 zVector3 angularfloat64 xfloat64 yfloat64 zfloat64[36] covariance
1. std_msgs/Header header
-
作用：消息的頭部信息，包含時間戳和參考坐標系。
-
stamp(builtin_interfaces/Time)：-
sec：時間戳的秒部分（Unix時間）。 -
nanosec：時間戳的納秒部分（0~999,999,999）。 -
用途：標識里程計數據的時間點，用于與其他傳感器數據同步。
-
-
frame_id(string)：-
值示例：
"odom"。 -
用途：既是里程計的父坐標系，也是pose的參考坐標系，也就是pose的父坐標系，也就是pose是child_frame_id這個坐標系相對于frame_id這個坐標系的pose。
-
-
2. string child_frame_id
-
作用：指定機器人本體的坐標系。
-
值示例：
"base_footprint"或"base_link"。 -
用途：里程計子坐標系，也就是機器人本體坐標系，也是pose和速度信息所在坐標系。
-
3. geometry_msgs/PoseWithCovariance pose
-
作用：機器人相對于全局坐標系的位姿（位置+朝向）及其不確定性。
-
pose(geometry_msgs/Pose)：-
position(Point)：-
x,y,z：3D位置坐標（單位：米）。 -
注意：對于地面機器人，
z通常為0（除非有高度變化）。
-
-
orientation(Quaternion)：-
x,y,z,w：四元數表示朝向（需歸一化）。 -
默認值：
(0, 0, 0, 1)表示無旋轉（朝向與父坐標系一致）。
-
-
-
covariance(float64[36])：-
位姿的6x6協方差矩陣（按行優先排列），表示不確定性：
-
-
[x_var, xy, xz, x_roll, x_pitch, x_yaw,xy, y_var, yz, y_roll, y_pitch, y_yaw,...]4. geometry_msgs/TwistWithCovariance twist
-
作用：機器人本體的線速度和角速度及其不確定性。
-
twist(geometry_msgs/Twist)：-
linear(Vector3)：-
x：前進速度（單位：米/秒，正值為前進）。 -
y：橫向速度（正值為左移，差分驅動機器人通常為0）。 -
z：垂直速度（地面機器人通常為0）。
-
-
angular(Vector3)：-
x：橫滾角速度（繞X軸旋轉，地面機器人通常為0）。 -
y：俯仰角速度（繞Y軸旋轉，地面機器人通常為0）。 -
z：偏航角速度（繞Z軸旋轉，單位：弧度/秒，正值為逆時針）。
-
-
-
covariance(float64[36])：-
速度的6x6協方差矩陣（排列方式同
pose.covariance）。 -
用途：描述速度測量的噪聲特性（如IMU誤差）。
-
-
協方差的選擇
const double odom_pose_covariance[36] = {1e-3, 0, 0, 0, 0, 0, 0, 1e-3, 0, 0, 0, 0,0, 0, 1e6, 0, 0, 0,0, 0, 0, 1e6, 0, 0,0, 0, 0, 0, 1e6, 0,0, 0, 0, 0, 0, 1e3 };const double odom_pose_covariance2[36] = {1e-9, 0, 0, 0, 0, 0, 0, 1e-3, 1e-9, 0, 0, 0,0, 0, 1e6, 0, 0, 0,0, 0, 0, 1e6, 0, 0,0, 0, 0, 0, 1e6, 0,0, 0, 0, 0, 0, 1e-9 };const double odom_twist_covariance[36] = {1e-3, 0, 0, 0, 0, 0, 0, 1e-3, 0, 0, 0, 0,0, 0, 1e6, 0, 0, 0,0, 0, 0, 1e6, 0, 0,0, 0, 0, 0, 1e6, 0,0, 0, 0, 0, 0, 1e3 };const double odom_twist_covariance2[36] = {1e-9, 0, 0, 0, 0, 0, 0, 1e-3, 1e-9, 0, 0, 0,0, 0, 1e6, 0, 0, 0,0, 0, 0, 1e6, 0, 0,0, 0, 0, 0, 1e6, 0,0, 0, 0, 0, 0, 1e-9} ;
什么是協方差矩陣？
在概率機器人學中，協方差矩陣用來表示某個狀態變量的不確定性。數值越大，說明該維度上的測量或估計越不可靠。
位姿協方差結構：
[ x_x, x_y, x_z, x_roll, x_pitch, x_yaw, // x可靠性y_x, y_y, y_z, y_roll, y_pitch, y_yaw, // y可靠性z_x, z_y, z_z, z_roll, z_pitch, z_yaw, // z可靠性roll_x, roll_y, roll_z, roll_roll, roll_pitch, roll_yaw, // roll可靠性pitch_x, pitch_y, pitch_z, pitch_roll, pitch_pitch, pitch_yaw, // pitch可靠性yaw_x, yaw_y, yaw_z, yaw_roll, yaw_pitch, yaw_yaw ] // yaw可靠性{1e-3, 0, 0, 0, 0, 0, 0, 1e-3, 0, 0, 0, 0,0, 0, 1e6, 0, 0, 0,0, 0, 0, 1e6, 0, 0,0, 0, 0, 0, 1e6, 0,0, 0, 0, 0, 0, 1e3}| 維度 | 值 | 含義說明 |
|---|---|---|
x,?y | 1e-3 | X、Y 位置估計較準確 |
z | 1e6 | Z 軸方向不關心或不可靠（如地面機器人） |
roll,?pitch | 1e6 | 不依賴或無法測量 |
yaw | 1e3 | 偏航角有一定誤差 |
-
x和y方向：高精度（方差=0.001，置信度高）。 -
z方向：極低精度（方差=1e6，表示忽略此方向，適用于地面機器人）。 -
旋轉（
roll、pitch）：極低精度（方差=1e6，通常地面機器人不關心俯仰/橫滾）。 -
yaw方向：中等精度（方差=1000，適用于航向角估計）。
這是一個典型的 2D 地面機器人位姿協方差矩陣
{1e-9, 0, 0, 0, 0, 0, 0, 1e-3,1e-9, 0, 0, 0,0, 0, 1e6, 0, 0, 0,0, 0, 0, 1e6, 0, 0,0, 0, 0, 0, 1e6, 0,0, 0, 0, 0, 0, 1e-9}-
移動的x和旋轉的yaw方向：極高精度（方差=1e-9，幾乎完全信任）。
位姿協方差結構類似，但表示速度的不確定性（數據可靠性）。
-- 機器人禁止使用高可靠性協方差；運動姿態使用低可靠性協方差。
)
