机器人 / 系统课时
用平面双连杆核对位置计算
用平面双连杆核对位置计算
已知两段连杆长度和两个关节角时,可以把每段连杆看成一个平面向量,逐段累加得到末端相对基座的坐标。本篇使用具体数值、Python 计算和特殊姿态交叉核验结果,重点避免角度单位混用、相对角误用和坐标方向错误。
学习目标
- 明确平面双连杆的基座坐标系、关节角定义和正方向。
- 根据两段连杆长度与关节角,计算肘关节和末端的二维坐标。
- 使用特殊角度、几何距离和 Python 程序验证正运动学结果。
统一坐标与关节角约定
考虑一个具有两个转动关节的平面机械臂。第一段连杆连接基座与肘关节,第二段连杆连接肘关节与末端执行器。两个关节都绕垂直于运动平面的轴旋转。
采用以下约定:
- 基座位于原点 , 轴水平向右, 轴竖直向上。
- 逆时针旋转为正,顺时针旋转为负。
- 为第一段连杆长度, 为第二段连杆长度。
- 为第一段连杆相对于基座 轴的角度。
- 为第二段连杆相对于第一段连杆方向的转角,而不是相对于基座 轴的角度。
因此,第二段连杆相对于基座 轴的绝对方向角为 。例如 、 时,第二段连杆实际指向 ,即竖直向上。

把连杆投影到两个坐标轴
平面双连杆的位置计算属于正运动学:输入连杆长度与关节角,输出肘关节和末端的位置。由于两个连杆的长度保持不变,可以先计算各自的水平与竖直分量,再做向量相加。
第一段连杆的末端就是肘关节,设其坐标为 :
第二段连杆的水平分量为 ,竖直分量为 。将其与肘关节坐标相加,得到末端坐标:
这两条公式的输出坐标属于基座坐标系,计算结果的长度单位与输入的连杆长度单位相同。
使用一组具体参数:、、、。
- 第一连杆水平投影为 ,竖直投影为 。
- 第二连杆绝对方向为 ,水平投影为 ,竖直投影为 。
- 肘关节坐标为 ,末端坐标为 。
这里的结果可以直接通过图形检查:第一段连杆向右上方倾斜,第二段竖直向上,所以末端与肘关节拥有相同的 坐标,而末端的 坐标比肘关节大 。
使用 Python 计算不同姿态
为了避免手算过程中重复出现角度换算错误,可以将正运动学公式封装为一个函数。Python 标准库 math 的三角函数接收弧度,因此需要使用 math.radians() 将输入的角度转换成弧度。
下面的示例适用于 Python 3.8 及以上版本,不依赖第三方库。将代码保存为 two_link_fk.py,执行 python two_link_fk.py 即可运行。
import math
def forward_kinematics(l1, l2, theta1_deg, theta2_deg):
# 关节角以度数输入,第二关节角相对于第一连杆
a1 = math.radians(theta1_deg)
a12 = a1 + math.radians(theta2_deg)
elbow = (l1 * math.cos(a1), l1 * math.sin(a1))
tip = (
elbow[0] + l2 * math.cos(a12),
elbow[1] + l2 * math.sin(a12),
)
return elbow, tip
if __name__ == "__main__":
# 用特殊姿态与一般姿态共同核对计算结果
cases = [(0, 0), (90, 0), (0, 90), (0, 180), (30, 60)]
for q1, q2 in cases:
elbow, tip = forward_kinematics(2.0, 1.0, q1, q2)
print(
f"q=({q1},{q2}) "
f"elbow=({elbow[0]:.3f},{elbow[1]:.3f}) "
f"tip=({tip[0]:.3f},{tip[1]:.3f})"
)预期输出:
q=(0,0) elbow=(2.000,0.000) tip=(3.000,0.000)
q=(90,0) elbow=(0.000,2.000) tip=(0.000,3.000)
q=(0,90) elbow=(2.000,0.000) tip=(2.000,1.000)
q=(0,180) elbow=(2.000,0.000) tip=(1.000,0.000)
q=(30,60) elbow=(1.732,1.000) tip=(1.732,2.000)代码先计算肘关节位置,再根据第二段连杆的绝对方向求末端位置。返回两个坐标的好处是能够分别检查第一段连杆和第二段连杆的长度,而不只是观察最终结果。
输出统一保留三位小数。浮点计算可能产生接近零的极小值,例如 在计算机中通常不是严格的零;因此不应使用浮点数直接相等作为几何正确性的唯一判断依据。
用特殊角度和长度约束核验结果
仅凭一组一般角度的输出,难以确认角度正方向和参考坐标系是否正确。特殊角度可以提供不依赖复杂三角计算的几何答案。
| 关节角 | 几何形态 | 预期末端坐标 |
|---|---|---|
| 两段连杆沿正 轴伸直 | ||
| 两段连杆沿正 轴伸直 | ||
| 第一段水平,第二段向上 | ||
| 第二段向后折回 |
除了特殊姿态,还可以使用连杆长度构造与关节角无关的几何检查。对于任意姿态,都应该满足:
- 基座到肘关节的距离为 。
- 肘关节到末端的距离为 。
- 基座到末端的直线距离 满足 。
对于前面的 姿态,末端坐标约为 ,所以 ,符合可达距离范围。第二连杆长度也可通过肘关节和末端的坐标差验证:。
发现计算错误时检查哪些地方
如果程序输出与几何形态不一致,优先检查下面三类问题。
- 把度数直接传给三角函数。
math.cos(90)计算的是 弧度的余弦,不是 的余弦。应先调用math.radians(90)。 - 把第二关节相对角当成绝对角。 当 、 时,如果错误地直接使用 和 计算第二连杆,末端会被算成约 ,而正确结果是 。
- 混用坐标正方向。 如果某个显示界面的纵轴向下为正,而运动学采用向上为正,直接使用计算结果绘图可能出现上下镜像。应在显示坐标转换时处理方向差异,不要暗中修改正运动学公式。
可以再做一次独立练习:保持 、,把角度改为 、。此时第二连杆的绝对方向为 ,预期末端坐标约为 。将这个结果与手绘的两段连杆方向对照,并检查肘关节到末端的距离是否仍为 。
完成这些核验后,双连杆正运动学的输入约定、位置公式与数值实现就形成了可以重复检查的基准。后续使用齐次变换矩阵计算同一机械臂时,也可以用这些特殊姿态作为独立的对照结果。