Robotics-机器人建模中的坐标系建立 - lvdongjie
Robotics-机器人建模中的坐标系建立 - lvdongjie
机器人建模中的坐标系建立
在机器人建模中，坐标系建立和原点选取看起来像是“画图时的约定”，但它们会直接影响后续的运动学建模、动力学建模、控制算法和程序实现。
同一台机器人，如果坐标系建得清晰，后面的变换矩阵、雅可比矩阵、运动学方程会自然很多；如果坐标系建得混乱，公式会变长，符号容易出错，调试时也很难判断问题到底出在哪里。
所以，机器人建模的第一步，往往不是写公式，而是先把坐标系放对。
1. 为什么机器人建模需要坐标系？#
机器人运动本质上是在空间中描述位置和姿态。
例如我们想回答这些问题：
- 机械臂末端执行器在哪里？
- 末端执行器朝向哪个方向？
- 某个关节转动后，后面的连杆会如何运动？
- 相机坐标系下看到的目标，如何转换到机器人基坐标系？
- 轮式机器人在地图坐标系中的位姿是多少？
这些问题都离不开坐标系。
在机器人建模中，常见坐标系包括：
- 世界坐标系
{W}：描述全局环境，例如地图、地面、工作台。 - 基座坐标系
{B}：固定在机器人底座上，用来描述机器人整体。 - 关节坐标系
{i}：固定在每个关节或连杆上，用来描述相邻连杆之间的关系。 - 末端坐标系
{E}：固定在夹爪、工具或末端执行器上。 - 传感器坐标系
{S}：固定在相机、激光雷达、IMU 等传感器上。
坐标系的作用，是给空间中的点、向量、姿态和运动提供统一的表达标准。
2. 坐标系建立的核心原则#
建立坐标系没有唯一答案，但好的坐标系通常满足几个原则。
2.1 尽量贴合机器人的结构#
坐标轴最好和机器人的自然运动方向一致。
例如：
- 移动机器人中，通常让
X轴指向车体前方； Y轴指向车体左侧；Z轴指向上方；- 机械臂中，关节坐标轴通常与关节转轴重合；
- 末端坐标系通常让某一轴指向工具的工作方向。
这样做的好处是，坐标含义直观。
比如移动机器人中，如果 X 轴指向前方，那么速度 vx 就表示前进速度；如果 Z 轴向上，那么绕 Z 轴旋转就对应平面内的偏航角。
2.2 保持右手坐标系#
机器人建模中通常使用右手坐标系。
判断方法是：
右手四指从 X 轴转向 Y 轴，大拇指指向 Z 轴。
或者记住叉乘关系：
X × Y = Z
Y × Z = X
Z × X = Y
使用右手坐标系可以避免旋转方向、角速度方向和叉乘结果出现符号混乱。
2.3 让相邻坐标系之间的变换尽量简单#
一个好的坐标系选取，应该让变换矩阵尽量简洁。
例如，如果两个坐标系的原点重合，只需要考虑旋转；如果坐标轴方向一致，只需要考虑平移。
建模时不要只看当前坐标系是否好画，还要看它和前后坐标系之间的关系是否好算。
2.4 保持命名和方向一致#
坐标系一旦确定，就要在图纸、公式、代码和文档中保持一致。
例如：
{W}：世界坐标系
{B}：机器人基坐标系
{E}：末端执行器坐标系
{C}：相机坐标系
同时要明确写清楚每个坐标系：
- 原点在哪里；
- X 轴指向哪里；
- Y 轴指向哪里；
- Z 轴如何由右手法则确定；
- 坐标系是固定在世界中，还是跟随机器人运动。
3. 原点选取为什么重要？#
坐标轴决定方向，原点决定位置。
同一个坐标系，如果轴方向不变，只改变原点，那么点的坐标、平移向量、变换矩阵都会发生变化。
在机器人建模中，原点选取会影响：
- 位姿表达是否直观；
- 连杆参数是否简洁；
- 运动学方程是否容易推导；
- 控制点和实际作用点是否一致；
- 传感器数据转换是否方便。
原点选得好，公式会短很多；原点选得不好，后面会不断出现额外的偏移量。
4. 常见原点选取方法#
4.1 世界坐标系原点#
世界坐标系通常固定在环境中。
常见选法包括：
- 地图起点；
- 实验场地角点；
- 工作台中心；
- 机器人初始位置；
- 标定板或外部测量系统的参考点。
世界坐标系的原点应该稳定、可复现，并且便于测量。
例如，在移动机器人导航中，地图坐标系 {map} 的原点通常由建图系统定义；在机械臂工作站中，世界坐标系可能固定在工装夹具或工作台上。
4.2 基座坐标系原点#
基座坐标系通常固定在机器人本体上。
对于机械臂，基座坐标系原点常选在：
- 第一个关节轴线上；
- 底座安装面的中心；
- 厂商定义的机器人 base 坐标系位置。
对于移动机器人，基座坐标系原点常选在：
- 车体几何中心；
- 两个驱动轮轴线的中点；
- 机器人旋转中心；
- IMU 或控制参考点位置。
如果机器人主要做平面运动，原点最好选在运动学模型中的自然参考点上。
例如差速小车中，常把原点选在左右轮连线的中点。这样线速度和角速度模型会非常简洁：
v = (v_r + v_l) / 2
ω = (v_r - v_l) / L
其中 L 是左右轮间距。
4.3 关节坐标系原点#
机械臂建模时，每个关节或连杆通常都要建立局部坐标系。
关节坐标系原点常选在：
- 关节轴线与连杆公共法线的交点；
- 相邻关节轴线的交点；
- 关节转轴上的某个几何中心；
- DH 建模规则规定的位置。
在 Denavit-Hartenberg 建模中，坐标系不是随意放置的，而是遵循一套规则，让相邻连杆之间的变换可以用四个参数描述：
a_i 连杆长度
α_i 连杆扭角
d_i 连杆偏距
θ_i 关节角
这样做的目的，是把复杂的三维连杆关系转化成标准化的矩阵乘法。
4.4 末端执行器坐标系原点#
末端坐标系一般固定在工具或执行器上。
原点常选在：
- 夹爪中心；
- 工具尖端；
- 吸盘中心；
- 焊枪端点；
- TCP，也就是 Tool Center Point。
TCP 的选取非常关键。
机器人真正控制的通常不是法兰盘中心，而是工具实际作用点。例如焊接机器人关心的是焊丝端点，抓取机器人关心的是夹爪中心，点胶机器人关心的是针头出口。
如果末端坐标系原点没有选在实际作用点上，就必须额外补偿工具偏移。
4.5 传感器坐标系原点#
传感器坐标系固定在传感器本体上。
例如相机坐标系通常定义为：
- 原点在相机光心；
Z轴沿光轴向前；X轴沿图像水平方向；Y轴沿图像垂直方向。
激光雷达坐标系通常固定在雷达扫描中心。
IMU 坐标系通常固定在惯性测量单元芯片或模组中心附近。
传感器坐标系和机器人坐标系之间的外参非常重要。所谓外参，本质上就是一个坐标变换：
B_T_C
表示从相机坐标系 {C} 到机器人基坐标系 {B} 的变换关系。
5. 坐标系方向如何确定？#
原点确定之后，还需要确定三个坐标轴方向。
5.1 移动机器人常用方向#
移动机器人中，一个常见约定是：
X：机器人前方
Y：机器人左方
Z：机器人上方
这个约定符合右手坐标系。
在这个坐标系下：
x表示前后位置；y表示左右位置；yaw表示绕Z轴的偏航角；- 正角速度通常表示逆时针旋转。
这也是 ROS 中常见的坐标约定之一。
5.2 机械臂关节坐标系方向#
机械臂中，关节坐标系经常按转轴方向确定。
一般来说：
Z_i轴沿第i个关节的运动轴；- 对转动关节，
Z_i是旋转轴； - 对移动关节，
Z_i是平移方向； X_i轴通常沿相邻关节轴线的公共法线；Y_i轴由右手法则确定。
这类规则可以让机械臂的正运动学表达更系统。
6. 原点选取对公式的影响#
来看一个简单例子。
假设移动机器人在平面上运动，如果基座原点选在车体旋转中心，那么机器人位姿可以写成：
p = [x, y, θ]^T
控制输入可以写成：
u = [v, ω]^T
运动学模型为：
x_dot = v cos θ
y_dot = v sin θ
θ_dot = ω
这个模型非常清晰。
但如果原点选在车头某个偏移点，假设它距离旋转中心有长度 l，那么该点速度还会受到角速度影响：
x_dot = v cos θ - l ω sin θ
y_dot = v sin θ + l ω cos θ
θ_dot = ω
这说明原点不同，模型复杂度会不同。
原点不是随便放的。它应该服务于建模、控制和计算。
7. 坐标系建立中的常见错误#
7.1 左手系和右手系混用#
这是非常常见的问题。
如果某些坐标系使用右手系，某些坐标系不小心变成左手系，旋转矩阵的方向、叉乘结果、姿态角都会出错。
7.2 坐标轴方向只在图上好看，但不利于计算#
有时为了画图方便，把坐标轴随意放置，结果导致后续平移和旋转都很复杂。
建模时更应该关心变换关系是否清楚，而不是图是否看起来对称。
7.3 原点没有放在真实控制点上#
例如机械臂末端工具安装后，如果仍然把法兰盘中心当作末端原点，而实际任务点在工具尖端，就会出现定位误差。
这时必须建立工具坐标系，并标定 TCP。
7.4 忘记坐标系是固定的还是运动的#
有些坐标系固定在世界中，有些坐标系固定在机器人身上，有些坐标系固定在关节或传感器上。
如果没有明确这一点，就容易把“点在动”和“坐标系在动”混在一起。
7.5 变换方向写反#
例如：
A_T_B
通常表示把 B 坐标系下的坐标转换到 A 坐标系下：
A p = A_T_B · B p
如果把它误用成：
B p = A_T_B · A p
结果就会错。
正确做法是使用逆矩阵：
B p = B_T_A · A p
B_T_A = inverse(A_T_B)
8. 建模时的实用流程#
实际建模时，可以按下面的流程进行。
第一步：明确建模目标#
先问清楚：
- 是要做正运动学？
- 是要做逆运动学？
- 是要做动力学？
- 是要做控制？
- 是要做传感器标定？
- 是要做导航定位？
不同目标下，坐标系选取重点不同。
第二步：确定全局参考坐标系#
选择世界坐标系或地图坐标系，明确它的原点和轴方向。
第三步：确定机器人基坐标系#
选择一个和机器人结构、运动模型最匹配的位置作为基座原点。
第四步：为关节、连杆、工具建立局部坐标系#
机械臂可以使用 DH 规则或改进 DH 规则，也可以使用 URDF 中的 link/joint 结构。
移动机器人可以为底盘、轮子、IMU、相机、雷达分别建立坐标系。
第五步：写出相邻坐标系之间的变换#
不要一开始就写复杂的全局公式。
先写清楚每一段：
W_T_B
B_T_1
1_T_2
2_T_3
E_T_tool
B_T_camera
然后再通过矩阵连乘得到整体变换。
第六步：检查方向和单位#
重点检查：
- 是否都是右手坐标系；
- 角度单位是度还是弧度；
- 长度单位是米还是毫米；
- 旋转方向是否符合右手法则；
- 平移向量是在哪个坐标系下表达的。
9. 一个简单案例：机械臂末端工具坐标系#
假设机械臂末端法兰盘坐标系为 {F}，工具坐标系为 {T}。
工具安装在法兰盘前方 l 米的位置，且方向与法兰盘一致。
那么可以写：
F_T_T = [ I [0, 0, l]^T
0 1 ]
如果已知法兰盘相对于基座的位姿：
B_T_F
那么工具相对于基座的位姿就是：
B_T_T = B_T_F · F_T_T
这说明工具坐标系并不是额外的麻烦，而是让控制目标更加准确。
机器人控制末端时，真正应该关注的是 {T}，而不是 {F}。
10. 一句话总结#
机器人建模中的坐标系建立，本质上是在为空间运动建立一套清晰的语言。
原点决定“从哪里开始量”，坐标轴决定“朝哪个方向量”。
好的坐标系应该满足：
- 符合右手法则；
- 贴合机器人结构；
- 原点靠近真实运动中心或控制点；
- 相邻坐标系之间的变换简单；
- 在公式、代码和文档中保持一致。
最终目标不是把坐标系画得复杂，而是让机器人运动关系表达得清楚、稳定、可计算。
坐标系建得好，后面的运动学、动力学和控制都会轻松很多。
评论