SE(3) constraints for robotics

2021-09-08

This document summarizes some common maths used for state estimation of rigid bodies such as in robotics.

1 Transformation parameterisation

Rigid transformations in 3 dimensions are known as the special Euclidean group, $SE(3)$, and can be written in the homogeneous form.

The rotation matrix $\mathbf{R}$ is in the special orthogonal group $SO(3)$, which means that it is orthogonal (its columns are normal and orthogonal to each other) and it has determinant $+1$.

1.1 Transforming a point

An element $\mathbf{T}\in SE(3)$ may transform a 3D point $\mathbf{p}\in {\mathbb{R}}^3$:

In this document we use it interchangeably with the homogeneous representation:

so that points may be transformed rigidly

1.2 The Lie algebra

Each element in $SE(3)$ is associated with an element on the corresponding Lie algebra, $\mathfrak{s}\mathfrak{e}(3)$:

where $[\mathit{\omega }{]}_{\times }$ is the skew symmetric form of the cross product by $\omega$. We may also write it as a $6\times 1$ vector.

We will use the $6\times 1$ and $4\times 4$ representations interchangeably depending on context.

Note that some other textbooks put the translational part on top and the rotational part below. It doesn’t matter much, but it will affect our notation for things like the adjoint action, differentials, etc below.

1.3 The exponential map

The group $SE(3)$ and its algebra $\mathfrak{s}\mathfrak{e}(3)$ are related by the exponential map:

The definition of $\exp$ is based on the Taylor series:

Closed forms exist.See: J. L. Blanco’s report jlblanco.

Note that $SE(3)$ is not commutative. The adjoint action relates things in different orders:

The adjoint is a $6\times 6$ matrix:

1.4 Notation summary

In general,

• bold lowercase refers to vectors (e.g. translation $\mathbf{t}$)
• bold uppercase refers to matrices (e.g. transformation $\mathbf{T}$)
• non-bold lowerase refers to scalars (e.g. time $t$)

Here we define and summarise some notation for the following sections.

Description	Notation
skew-symmetric cross product matrix $\left[\begin{array}{ccc}0& -t_3& t_2\\ t_3& 0& -t_1\\ -t_2& t_1& 0\end{array}\right]$	$[\mathbf{t}{]}_{\times }$
translational part of $\mathbf{T}\in SE(3)$	$\mathbf{t}(\mathbf{T})$
rotational part of $\mathbf{T}\in SE(3)$	$\mathbf{R}(\mathbf{T})$
translational part of $\mathit{\xi }\in \mathfrak{s}\mathfrak{e}(3)$	$\mathit{\tau }(\mathit{\xi })$
rotational part of $\mathit{\xi }\in \mathfrak{s}\mathfrak{e}(3)$	$\mathit{\omega }(\mathit{\xi })$
compose ${\mathbf{T}}_1,{\mathbf{T}}_2\in SE(3)$	$T_1{T}_2$
exp $\mathit{\xi }\in \mathfrak{s}\mathfrak{e}(3)$	$\exp (\mathit{\xi })$
log of $\mathbf{T}\in SE(3)$	$\log (\mathbf{T})$
exp $\mathit{\omega }\in \mathfrak{s}\mathfrak{o}(3)$	$\exp (\mathit{\omega })$
log of $\mathbf{R}\in SO(3)$	$\log (\mathbf{R})$
inverse of $\mathit{\xi }\in \mathfrak{s}\mathfrak{e}(3)$	$-\mathit{\xi }$
inverse of $\mathbf{T}\in SE(3)$	${\mathbf{T}}^{-1}$
inverse of $\mathit{\omega }\in \mathfrak{s}\mathfrak{o}(3)$	$-\mathit{\omega }$
inverse of $\mathbf{R}\in SO(3)$	${\mathbf{R}}^T$
adjoint of $\mathbf{T}\in SE(3)$	$\mathrm{Ad}(\mathbf{T})$
$i$ th element of $\{\mathit{\xi }\mathrm{\mid }\mathit{\xi }\in \mathfrak{s}\mathfrak{e}(3)\}$	${\mathit{\xi }}_i$
$i$ th element of $\{\mathbf{T}\mathrm{\mid }\mathbf{T}\in SE(3)\}$	${\mathbf{T}}_i$
transform $\mathbf{p}\in {\mathbb{R}}^3$ by $\mathbf{T}\in SE(3)$	$\mathbf{T}\mathbf{p}$
transform $\mathbf{p}\in {\mathbb{R}}^3$ by $\mathit{\xi }\in \mathfrak{s}\mathfrak{e}(3)$	$\exp (\mathit{\xi })\mathbf{p}$

2 Derivatives

Here, we only differentiate with respect to $\mathit{\delta }\in \mathfrak{s}\mathfrak{e}(3)$ around the point $\mathit{\delta }=\mathbf{0}$. In other words, we have a function $\mathbf{F}(\mathbf{T})$ where $\mathbf{T}\in SE(3)$ and we would like to perturb $\mathbf{T}$ by a very small $\mathit{\delta }$.

Suppose $\mathit{\delta }={\left[\begin{array}{cc}\mathit{\omega }& \mathit{\tau }\end{array}\right]}^T$.If $\mathbf{F}$ is of dimension $n$, then the resulting derivative is an $n\times 6$ Jacobian.

where $\mathbf{F}(\mathbf{T}\oplus \mathit{\delta })=\mathbf{F}(\exp (\mathit{\delta })\mathbf{T})$ in the case of left-perturbations and $\mathbf{F}(\mathbf{T}\exp (\mathit{\delta }))$ in the case of right-perturbations.

The adjoint action can be used to relate the left and right derivatives by using the chain rule.

We also have a derivative of the log function:

where

The exact derivation is in Ethan Eade’s report eade.

Let $\mathbf{T}\in SE(3)$, $\mathbf{p}\in {\mathbb{R}}^3$, then the following table lists some useful derivatives.

As you will see later, these are basically all the derivatives you need, and all other derivatives can be easily derived from these, often together with using the adjoint.

Function	Derivative
$\mathbf{F}(\mathit{\delta })$	${\frac{\mathrm{\partial }\mathbf{F}}{\mathrm{\partial }\mathit{\delta }}]}_{\mathit{\delta }=\mathbf{0}}$
$\exp (\mathit{\delta })\mathbf{T}\mathbf{p}$	$\left[\begin{array}{cc}-[\mathbf{T}\mathbf{p}{]}_{\times }& \mathbf{I}\end{array}\right]$
$\mathbf{T}\exp (\mathit{\delta })\mathbf{p}$	$\mathbf{R}(\mathbf{T})\left[\begin{array}{cc}-[\mathbf{p}{]}_{\times }& \mathbf{I}\end{array}\right]$
$\log (\exp (\mathit{\delta })\mathbf{T})$	${\mathbf{D}}_{\text{log}}(\mathbf{T})$

3 Optimisation

Suppose we have a trajectory $\mathbf{S}(t)\in SE(3)$ that is a smooth curve. We would like to minimise some objective function:

We seek to reduce the cost as much as possible. This is a nonlinear least squares problem. During the optimisation process, we perturb it by a small perturbation $\mathit{\delta }(t)\in \mathfrak{s}\mathfrak{e}(3)$:

where the $\oplus$ operator can either be the left-update:

or the right-update:

Both are valid depending on numerical properties of the problem.

In any case, we linearise the problem around $\mathit{\delta }=\mathbf{0}$.

Let the Jacobian matrix be:

Then we can solve $\mathit{\delta }$ using Gauss-Newton:

or steepest descent:

or more sophisticated algorithms. In practice, we use Powell’s Dog Leg. The update is

where scalar weights $c_{\text{GN}}$, $c_{\text{s}}$ are chosen such that $\mathrm{\parallel }\mathit{\delta }\mathrm{\parallel }\le \mathrm{\Delta }$ where the scalar parameter $\mathrm{\Delta }$ is the radius of the trust region. When $\mathrm{\Delta }$ is small, the problem is behaving badly and the quadratic approximation that Gauss-Newton uses is not very valid. In this case, $c_{\text{GN}}$ is zero, allowing the optimiser to take timid steps along the steepest descent direction. When $\mathrm{\Delta }$ is large, the quadratic approximation is good and we take bigger steps in the Gauss-Newton direction.

Heuristics are used to determine when to increase $\mathrm{\Delta }$ and when to decrease it.

3.1 Optimisation under uncertainty

When uncertainty is present, the data is associated with some covariance matrix $\mathbf{\Sigma }$ which is an $n\times n$ matrix where $n$ is the sise of the residual. The Gauss-Newton update then becomes:

The matrix ${\mathbf{\Sigma }}^{-1}$ is also sometimes called the information matrix. In practice, it is useful to factor this matrix, for example, by taking the matrix square root. Let $\mathbf{W}={\mathbf{\Sigma }}^{-\frac{1}{2}}$, then,

In other words, we just pre-multiply the Jacobian and residual by a weight matrix. This is a form of whitening. In the common case where $\mathbf{\Sigma }$ is a diagonal matrix, we simply divide each row of the Jacobian and the residual by the standard deviation.

Another common factorisation is the eigendecomposition of the covariance matrix $\mathbf{\Sigma }$:

where $\mathbf{Q}\in SO(n)$ is the square matrix whose columns are the orthonormal eigenvectors of $\mathbf{\Sigma }$ and $\mathbf{\Lambda }$ is the diagonal matrix whose entries are the eigenvalues. Then,

This is useful, for example, in the case of surfel matches. Recall that the covariance matrix is always symmetric positive semidefinite, allowing for easy eigendecomposition.

3.2 Robust loss functions

A least squares problem can be thought of as minimising the negative log likelihood function of a Gaussian. The likelihood function is of the form

Then, the log likelihood is of the least squares form:

However, in many cases, the random variable is not Gaussian. The Gaussian has very thin tails, making it incredibly unlikely to have outliers. In the real life, there are often many outliers that necessitate a more thick-tailed distribution. To model such situations, we need robust loss functions.

Instead of optimising $\sum f_i^2$, we optimise:

where $\rho$ is a robust loss function. We can then weigh each row or block of the Jacobian $\mathbf{J}$ and the residual $\mathbf{F}$ with a robust reweighting factor:

Name	Definition
TrivialLoss	$\rho (x)=x$
CauchyLoss	$\rho (x)=\log (1+x)$
HuberLoss	$\rho (x)=\{\begin{array}{ll}x& x<k\\ 2k\sqrt{x}-k^2& x\ge k^2\end{array}$

4 Trajectory representation

The goal is to recover the trajectory of a vehicle as a parametric curve $\mathbf{S}(t)\in SE(3)$ as a function of time $t$.

We may assume the curve does not oscillate faster than some frequency (say, 10~Hz).

Now we should find a trajectory representation that satisfies the following properties:

• For some finite period of time, the trajectory should be approximately correct and differentiable.
• For any given real number $t$ within the domain of the curve, we can efficiently evaluate $\mathbf{S}(t)$ in constant time.
• Local perturbations $\mathit{\delta }(t)$ may be applied to the curve, so that ${\mathbf{S}}_{\text{new}}(t)=\mathbf{S}(t)\oplus \mathit{\delta }(t)$, such that the perturbation $\mathit{\delta }(t)$ may be parameterised with a finite number of parameters, each with compact support.
• The trajectory must be independent of the choice of the inertial reference frame.

Our solution to satisfy these requirements is a piecewise linear trajectory.The trajectory is represented as a sequence of elements ${\mathbf{S}}_i$ that represent $\mathbf{S}(t_i)$ for some $t_i$ sampled with even spacing at a high frequency (say, 100~Hz).To evaluate $\mathbf{S}(t)$, we use a geodesic interpolation for $t_i\le t<t_{i+1}$.

To perturb this spline with a curve $\mathit{\delta }(t)$, we update each ${\mathbf{S}}_i$, like so:

Alternative approaches for parameterising trajectories include:

• Cumulative cubic B-splines.
• Hermite splines.

5 Parameterisation of the perturbation

When applying perturbations, the curve $\mathit{\delta }$ is a smooth curve which may be thought of as a vector of infinite dimension.

To ensure that the problem is computationally tractable and that ${\mathbf{S}}_{\text{new}}$ remains twice-differentiable, we parameterise the perturbation $\mathit{\delta }$ by a finite vector $\mathit{\xi }$.The vector $\mathit{\xi }$ is the concatenation many six-dimensional vectors ${\mathit{\xi }}_i\in \mathfrak{s}\mathfrak{e}(3)$, such that

Notice that, since $\mathit{\delta }(t)$ only applies small local perturbations, it is possible to add together the ${\mathit{\xi }}_i$ treating them as vectors in ${\mathbb{R}}^6$.The scalar-valued function $\beta$ is a basis function with compact support, which means that it is nonzero for a finite contiguous segment of $t$ and zero everywhere else.A good choice is the piecewise polynomial for a cardinal cubic B-spline:

We can now redefine the Jacobian to be with respect to the parameters:

Since $\beta$ is a constant,

The elements ${\mathit{\xi }}_i\in \mathfrak{s}\mathfrak{e}(3)$ are known as the spline knots. Since each knot is a $6\times 1$ vector, the number of columns of $\mathbf{J}$ is $6\times n_{\text{knots}}$ where $n_{\text{knots}}$ is the number of spline knots.

As you can see, the derivative of the trajectory at any point $t$ in time is a linear combination of the derivatives of up to four spline knots.

In the next sections we will compute derivatives

knowing that each of these blocks will contribute up to four blocks, weighted with scalar weights $\beta$, to the actual Jacobian where we are optimizing the spline knots $\mathit{\xi }$.

6 Constraints

The function $\mathbf{F}$ is known as the residual. It consists of many constraints:

For people familiar with the Ceres library, each constraint is a residual block.

6.1 Position constraint

The position constraint seeks to penalise the distance between the pose’s translational component and a 3D point.

For example, the 3D point may be the GPS position $\mathbf{p}(t)$ measured at time $t$.

6.1.1 Residual

The residual is a $3\times 1$ vector:

Recall from the notation section that $\mathbf{t}(\mathbf{T})$ is the translational component of the $SE(3)$ element $\mathbf{T}$.

6.1.2 Left Jacobian

The left Jacobian is $3\times 6$:

The trick is to view $\mathbf{t}(\mathbf{T})=\mathbf{T}\mathbf{p}$ where $\mathbf{p}=\mathbf{0}$.Then we can apply the Jacobian for transforming a point in the derivatives table.

6.1.3 Right Jacobian

The right Jacobian is $3\times 6$:

6.2 Loop closure constraint

Suppose that we have aligned the poses from two points in time along a trajectory, e.g. using ICP.

This produces a relative transformation $\mathbf{A}$.

6.2.1 Residual

The residual is a $6\times 1$ vector:

6.2.2 Left Jacobians

6.2.3 Right Jacobians

6.3 Gravity constraint

The gravity constraint seeks to penalise the distance between up vector of the car $\mathbf{R}(\mathbf{T}(t))\mathbf{u}$ and the true up vector ${\mathbf{u}}_{\text{true}}=[0,0,1{]}^T$.

For example, if the sensor were mounted perfectly upright, then $\mathbf{u}={\mathbf{u}}_{\text{true}}$.

We can also use the accelerometer reading to obtain a different $\mathbf{u}$ at each point in time, as long as you remember to subtract the inertial acceleration.

6.3.1 Residual

The residual is a $3\times 1$ vector:

6.3.2 Left Jacobian

Since the gravity constraint only depends on rotation and not on translation, we only care about the derivative with respect to $\mathit{\omega }(\mathit{\delta })$, which we hereafter write as $\mathit{\omega }$.

6.3.3 Right Jacobian

Recall that $\mathbf{R}(\mathbf{T})$ is the rotational component of the pose $\mathbf{T}$.

6.4 Point constraint

We are aligning a “moving” or “map” point $\mathbf{m}$ to a “static” or “scene” point $\mathbf{s}$ by transforming the moving point with the pose $\mathbf{T}$.

The matrix $\mathbf{A}$ can be used to store the uncertainty of the point $\mathbf{s}$, i.e. an information matrix (the inverse of a covariance matrix).

• If you are registering the point to a line, you can let $\mathbf{A}$ be $2\times 3$. A common choice is to compute an orthonormal basis from the direction of the ray, e.g. using Duff et al’s approach. This is commonly used in visual odometry.
• If you are registering the point to a plane, you can let $\mathbf{A}$ be $1\times 3$.

6.4.1 Residual

The residual is a $3\times 1$ vector:

6.4.2 Left Jacobian

The left Jacobian is $k\times 6$, where $\mathbf{A}$ is $k\times 3$.

6.4.3 Right Jacobian

The right Jacobian is $k\times 6$, where $\mathbf{A}$ is $k\times 3$.

Recall that $\mathbf{R}(\mathbf{T})$ is the rotational component of the pose $\mathbf{T}$.

6.5 Velocity constraint

The velocity constraint seeks to penalise deviations in vehicle velocity from the 6 degree of freedom velocity estimates from another source.

For ease of implementation, the vehicle velocity is obtained by numerical differentiation, e.g. by evaluating the pose at times $t_1$ and $t_2$.Let $h=1\mathrm{/}(t_2-t_1)$.

6.5.1 Residual

The residual is a $6\times 1$ vector.For concise notation let us define the $6\times 6$ matrix $\mathbf{\Delta }$ such that

6.5.2 Left Jacobian

Consider differentiating with respect to left-updates of $\mathbf{T}(t_1)$.

The Jacobian is $6\times 6$.

Now, consider differentiating with respect to left-updates to $\mathbf{T}(t_2)$.

6.6 Regularisation constraint

The regularisation constraint implements a basic Tikhonov regulariser.It seeks to dampen the problem to avoid divergent oscillations, overfitting, or other types of poor convergence.

6.6.1 Residual

The residual is $6\times 1$:

6.6.2 Jacobian

The Jacobian is the identity:

7 References

• jlblanco Blanco, J.L. (2020) A tutorial on SE(3) transformation parameterizations and on-manifold optimization. link
• eade Eade, E. (2018) Derivative of the Exponential map. link

#include    <iostream>
#include    <cmath>

#include    <Eigen/Dense>
#include    <sophus/common.hpp>
#include    <sophus/geometry.hpp>

using namespace  std;
using namespace  Eigen;



int main(int argc, const char *argv[])
{
    Matrix3d R = AngleAxisd(M_PI / 2, Vector3d(0, 0, 1)).toRotationMatrix();
    Quaterniond q(R);
    // ************  SO(3) ************
    Sophus::SO3d SO3d_R(R); 
    Sophus::SO3d SO3d_q(q); // Result should be the same as SO3d_R
    cout << "SO(3) from rotation matrix = \n" << SO3d_R.matrix() << endl;
    cout << "SO(3) from quaternion = \n" << SO3d_q.matrix() << endl;
    // Logarithmic map to get the lie algebra
    Vector3d so3 = SO3d_R.log();
    cout << "so3 = \n" << so3.transpose() << endl;
    // Hat is from vector to skew-symmetric matrix
    cout << "so3 hat = \n" << Sophus::SO3d::hat(so3) << endl;
    // Vee is from skew-symmetric matrix to vector
    cout << "so3 vee = \n" << Sophus::SO3d::vee(Sophus::SO3d::hat(so3)).transpose() << endl;
    // Update by perturbation model
    Vector3d update_so3(1e-4, 0, 0);
    Sophus::SO3d SO3d_updated = Sophus::SO3d::exp(update_so3) * SO3d_R;
    cout << "SO3 updated = \n" << SO3d_updated.matrix() << endl;
    cout << "****************************" << endl;
    return 0;
}

https://www.cgal.org/index.html

1.2D 和 3D 几何：CGAL 提供了各种数据结构和算法，用于处理二维和三维的点、线段、多边形、曲线、曲面等几何对象。它支持凸包计算、点定位、包围盒计算、空间分割等操作。

2.2D 和 3D 三角剖分：CGAL 实现了多种高质量的、高效的三角剖分算法。它支持 Delaunay 三角剖分、Voronoi 图计算、网格重构、约束三角剖分等操作。

3.2D 和 3D 网格生成与处理：CGAL 提供了用于生成和处理网格的算法和数据结构。它支持网格生成、网格布尔运算、网格修复、网格优化、封闭表面重构等操作。

4.几何优化：CGAL 实现了多个几何优化算法，用于求解几何优化问题，如最小凸包、最小旋转包、最长空间线段等。

5.多边形和非封闭曲线处理：CGAL 支持进行多边形布尔运算、多边形修复、多边形拟合、轮廓计算等操作。它还提供了对非封闭曲线的操作和处理。

6.曲面重建：CGAL 提供了多个用于重建曲面的算法，包括点云重建、隐函数重建、流形重建等。这些算法可用于从离散的点集生成平滑的曲面模型。

7.拓扑关系和空间搜索：CGAL 支持计算几何对象之间的拓扑关系，如相交、包含、相交点等。它还提供了用于空间搜索的数据结构和算法，如 kd-树、R 树等。

很多人学完高数，依然分不清导数和微分的区别，觉得反正计算时

好像是一回事。其实，它们在数学定义和几何意义上有着本质的不同。

.

一、 导数 (Derivative)：关注的是“比值”

1. 核心定义：瞬时变化率

导数描述的是函数在某一点的变化快慢。

.

2. 几何意义：切线斜率

一句话总结：导数是一个“率”（Rate），是除法的结果。

.

二、 微分 (Differential)：关注的是“增量”

1. 核心定义：线性近似

微分的本质是“以直代曲”。

2. 几何意义：切线上的增量

一句话总结：微分是一个“量”（Quantity），是乘法的结果。

.

三、 两者的联系与区别（关键！）

这个公式说明：微分 = 导数 × 自变量增量。

2. 区别对照表：

四、 避坑指南（考试常考点）

①连续 ≠ 可导

但函数可导，一定连续。

记忆口诀：光滑才可导，尖点不可导。

想知道变化有多快？求导数。

想估算变了多少数值？用微分。

近期很多人问：“我想学机器人，是不是先买个树莓派跑跑ROS？”

听我说句大实话：快停下，你那是在浪费生命。

90%的人搞机器人都在做无用功。他们看了几百个演示视频，跑通了几个GitHub上的开源Demo，就以为自己懂机器人了。结果呢？遇到真实环境，参数一变，机器人立马“智障”。

为什么？因为你只摸到了皮毛，底下全是空的。

机器人不是单一技能，它是一个深不见底的技术栈。它是应用数学、物理学、软件工程和硬件工程的暴力美学，而“控制”是把它们粘合在一起的胶水。

不想再当个只会跑Demo的“调包侠”？听好了，这才是如果不浪费3年时间的正确学习顺序。

---

第一步：先认清现实，别在那自我感动

别一上来就整什么ROS教程、炫酷仿真、AI大模型。

那些是最后才干的事。

如果你跳过基础去搞上层建筑，你造出来的不是机器人，是脆弱的电子玩具。稍微碰点干扰，系统就崩了。

记住：机器人 = 数学 + 物理 + 软硬件 + 控制。缺一不可。

第二步：啃下数学这块硬骨头

别被吓跑，你不需要像数学博士那样推导公式，但你需要“能用的数学”。

这是你的底线，没得商量：

• 线性代数：向量、矩阵、特征值。不懂这个，你连机器人现在的姿态和坐标变换都搞不明白。
• 微积分：搞不清梯度和速率，你怎么做连续系统？
• 概率论：真实世界充满了噪声和不确定性，不懂概率，你的机器人就是个瞎子。
• 微分方程：动力学、运动、控制全靠它。

你的目标很简单：看到方程，脑子里能立马浮现出它描述的物理动作。

第三步：像工程师那样学物理，而不是理论家

机器人活在真实世界里，而真实世界是充满恶意的。

理论上完美的运动，现实中会被摩擦力、惯性、发热教做人。你需要关注：

• 运动学：位置、速度、加速度。
• 动力学：质量、惯性、扭矩。
• 能量：功率限制、效率、散热。
• 约束：齿轮间隙、材料柔性。

物理学，就是那些“天真”的机器人死掉的地方。

第四步：控制算法，机器人的心脏

没有控制算法，机器人就是一堆会动的废铁。

这一块的学习路径很清晰：

1. PID：先搞懂反馈直觉。
2. 状态空间（State-space）：建立系统思维。
3. 卡尔曼滤波：学会估计那些你无法直接测量的东西。
4. LQR / MPC：在约束条件下实现最优行为。

一句话：控制就是让数学触碰金属，让死物拥有“灵魂”。

第五步：带着系统思维写代码

别把Web开发那一套带进来。机器人软件开发不是写网页。

这里没有撤回键，甚至可能会炸机。

• 核心技能：C++是必须的，Python是辅助的。不懂并发（Concurrency）和时序（Timing），你的程序就是垃圾。
• 工具链：Linux玩得转吗？Git用得溜吗？
• ROS2：看到没？这时候才轮到ROS。只有当你搞懂了基础，ROS才是利器，否则它就是你的拐杖。

软件是机器人的神经系统，任何一个Bug都可能导致肢体瘫痪。

第六步：感知，在混乱世界里找秩序

即使你装了最好的摄像头，机器人其实并没有“看见”。它是在推断。

你需要学什么？

• 摄像头、激光雷达（LiDAR）、IMU的数据处理。
• 坐标系转换（这块能把你绕晕）。
• SLAM（同步定位与地图构建）基础。
• 过滤噪声：传感器一定会骗你，你要学会去伪存真。

感知的本质就是概率。如果你追求100%的确定性，那你入错行了。

第七步：规划，想好了再动

没有规划的运动就是发疯。

怎么从A点到B点不撞墙？

• 图搜索。
• 采样方法（RRT）。
• 基于优化的规划。

规划，就是把你的“意图”转化为具体的“动作”。

第八步：硬件，越早碰越好

仿真器是会骗人的，它掩盖了太多错误。

尽早去摸真实的电机、编码器、驱动器、单片机。去感受接线时的抓狂，去体会机械公差带来的绝望。

硬件会教你做人，那是让你学会谦卑的最快途径。

第九步：集成，地狱模式开启

这是大多数人放弃的地方。

把上面所有东西凑在一起时，真正的噩梦开始了：延迟Bug、控制环路不稳定、传感器漂移、时序混乱……

别以为代码写完就结束了，集成调试才是真正的课程开始。

---

拒绝“虚假勤奋”

别再刷那些无脑教程了，也别再只会Copy别人的Github代码改个名字当自己的。

正确的姿势是：

建立一个小系统 -> 把它搞崩 -> 测量数据 -> 记录失败 -> 逐层修复。

机器人是通过“失败”来教会你东西的。

这条路很慢，进步是非线性的，建立直觉可能需要几年。但只有当你能把这个技术栈从底层的数学到上层的硬件全部打通时，你才不再是一个“写代码的”，而是一个真正的机器人工程师。

这就是玩票和专家的区别。