4、运动学模型：轮式机器人运动学模型（差速、阿克曼）、无人机运动学模型、机械臂运动学模型
运动学模型，说白了就是研究机器人「怎么动」的数学工具。它不关心力有多大、扭矩够不够，只关心位置、速度和几何约束之间的关系。我个人觉得，这是做轨迹跟踪控制最基础、也最容易被忽视的一环。
你想想看，如果你连机器人怎么走都描述不清楚，那还谈什么控制它？所以这一章，咱们把几种主流机器人的运动学模型掰开揉碎了讲清楚。
4.1 差速轮式机器人运动学模型
差速驱动，最常见的就是扫地机器人、AGV小车那种。两个驱动轮独立控制，一个万向轮随动。它的运动学模型其实很简单，但坑不少。
4.1.1 模型推导
先定义几个变量：
- v：机器人质心的线速度
- ω：机器人绕质心的角速度
- v_L、v_R：左轮和右轮的线速度
- L：左右轮之间的距离（轮距）
- R：车轮半径
核心关系就两个公式：
v = (v_R + v_L) / 2
ω = (v_R - v_L) / L然后通过积分，就能得到机器人在全局坐标系下的位姿：
x_dot = v * cos(θ)
y_dot = v * sin(θ)
θ_dot = ω嗯，这里要注意：这个模型假设车轮与地面之间是纯滚动，没有滑动。我在项目中遇到过，地面稍微有点油或者灰尘，这个模型就开始不准了。
我曾经在瓷砖地面上跑差速小车，模型算出来的轨迹和实际差了快20%。后来发现是打滑导致的。解决办法有两个：要么加IMU做融合，要么降低加速度限制。
4.1.2 代码实现
写一个简单的运动学更新函数：
def diff_drive_update(x, y, theta, v_L, v_R, dt, L):
# 计算线速度和角速度
v = (v_R + v_L) / 2.0
omega = (v_R - v_L) / L
# 更新位姿
x += v * np.cos(theta) * dt
y += v * np.sin(theta) * dt
theta += omega * dt
return x, y, theta我一般会在更新前加一个限幅，防止电机指令突变导致模型震荡。尤其是角速度，限幅在±2 rad/s以内比较安全。
4.2 阿克曼转向运动学模型
阿克曼模型就是汽车那种转向方式。前轮转向，后轮驱动。它的运动学约束比差速复杂一些，因为存在一个「瞬时转动中心」的概念。
4.2.1 模型推导
关键参数：
- δ：前轮转向角
- L_wb：轴距（前后轮之间的距离）
- v：后轴中心的速度
运动学方程：
x_dot = v * cos(θ)
y_dot = v * sin(θ)
θ_dot = (v / L_wb) * tan(δ)你会发现，角速度和转向角之间是非线性关系。tan(δ)这个东西，在小角度时可以近似为δ，但大角度时千万别偷懒。
阿克曼模型的转弯半径 R = L_wb / tan(δ)。当δ接近90°时，R趋近于0，但实际机械结构根本做不到。所以阿克曼机器人的最小转弯半径是有限制的。
4.2.2 代码实现
def ackerman_update(x, y, theta, v, delta, dt, L_wb):
# 计算角速度
omega = (v / L_wb) * np.tan(delta)
# 更新位姿
x += v * np.cos(theta) * dt
y += v * np.sin(theta) * dt
theta += omega * dt
return x, y, theta我曾经在阿克曼小车上做轨迹跟踪，发现倒车时模型完全反了。原因是倒车时v为负，tan(δ)不变，但实际的运动方向会变。所以倒车时记得把δ取反，或者单独处理。
4.3 无人机运动学模型
四旋翼无人机的运动学，比轮式机器人多了一个维度——高度。而且它的姿态变化直接影响水平运动，耦合性很强。
4.3.1 模型推导
无人机的位置用(x, y, z)表示，姿态用欧拉角(φ, θ, ψ)表示，分别是横滚角、俯仰角、偏航角。
运动学方程（简化版）：
x_dot = v_x
y_dot = v_y
z_dot = v_z
φ_dot = p + q * sin(φ) * tan(θ) + r * cos(φ) * tan(θ)
θ_dot = q * cos(φ) - r * sin(φ)
ψ_dot = (q * sin(φ) + r * cos(φ)) / cos(θ)其中(p, q, r)是机体角速度。说实话，这个公式看着挺吓人，但实际用的时候，小角度假设下可以大大简化。
做无人机轨迹跟踪时，我一般把姿态控制和外环位置控制分开。外环只关心位置和速度，内环负责把姿态指令转成电机转速。这样调试起来清晰很多。
4.3.2 代码实现
def drone_kinematics_update(pos, euler, vel, omega, dt):
# 位置更新
pos[0] += vel[0] * dt
pos[1] += vel[1] * dt
pos[2] += vel[2] * dt
# 姿态更新（简化版，小角度假设）
euler[0] += omega[0] * dt # 横滚角
euler[1] += omega[1] * dt # 俯仰角
euler[2] += omega[2] * dt # 偏航角
return pos, euler我曾经在风大的天气飞无人机，小角度假设完全失效。姿态角到了30°以上，简化模型和实际差了很大。后来我改用四元数表示姿态，避免了欧拉角的万向锁问题。
4.4 机械臂运动学模型
机械臂的运动学，核心就是正解和逆解。正解：已知关节角度，求末端位置。逆解：已知末端位置，求关节角度。
4.4.1 正运动学
用DH参数法建立坐标系变换矩阵。每个关节对应一个齐次变换矩阵：
T_i = [[cos(θ_i), -sin(θ_i)*cos(α_i), sin(θ_i)*sin(α_i), a_i*cos(θ_i)],
[sin(θ_i), cos(θ_i)*cos(α_i), -cos(θ_i)*sin(α_i), a_i*sin(θ_i)],
[0, sin(α_i), cos(α_i), d_i ],
[0, 0, 0, 1 ]]把所有变换矩阵乘起来，就得到末端位姿：
T_0n = T_1 * T_2 * ... * T_n正运动学是唯一的，给定一组关节角度，末端位姿是确定的。但逆运动学可能有多个解，甚至无解。
4.4.2 逆运动学
逆解没有通用公式，得根据机械臂的具体构型来推导。对于6轴工业机器人，常用的方法是解析法或数值法。
解析法：通过几何关系直接求解。速度快，但只适用于特定构型。
数值法：用雅可比矩阵迭代求解。通用性强，但计算量大，可能不收敛。
def inverse_kinematics_numerical(target_pose, initial_q, max_iter=100, tol=1e-6):
q = initial_q
for i in range(max_iter):
# 计算当前末端位姿
current_pose = forward_kinematics(q)
# 计算误差
error = target_pose - current_pose
if np.linalg.norm(error) < tol:
break
# 用雅可比矩阵更新关节角度
J = compute_jacobian(q)
dq = np.linalg.pinv(J) @ error
q += dq
return q我一般先用解析法求一个初始解，再用数值法微调。这样既保证了速度，又提高了精度。另外，逆解时一定要考虑关节限位，不然电机可能会撞到机械限位。
4.5 知识体系总览
为了让你更直观地理解这几种运动学模型的关系，我画了一张图：
4.6 总结
运动学模型是轨迹跟踪的基石。差速模型简单但容易打滑，阿克曼模型非线性强但更接近真实车辆，无人机模型维度高且耦合，机械臂模型则要处理正逆解的复杂性。
我个人建议，刚开始做轨迹跟踪时，先从差速模型入手。它简单、直观，能让你快速理解核心概念。等把差速模型吃透了，再扩展到阿克曼、无人机和机械臂。
嗯，这一章的内容就到这里。记住，模型只是工具，关键是你怎么用它。下一章咱们会讲轨迹规划，到时候这些模型就派上用场了。