机器人 / 系统课时
推导平面二连杆的解析逆解
推导平面二连杆的解析逆解
给定两段连杆的长度和末端目标坐标,可以直接算出平面双旋转关节机械臂的关节角。本篇从正运动学建立余弦约束,求出肘上、肘下两组解析解,再通过数值回代检查可达性和解的正确性。
学习目标
- 从末端坐标推导第二关节角的余弦约束。
- 用
atan2求出并区分两种肘部构型。 - 判断不可达目标、边界退化和关节限位,并用正运动学验证结果。
从末端坐标建立约束
设基座位于原点 ,世界 轴向右、 轴向上,逆时针为正。两段连杆长分别为 、; 是第一段相对世界 轴的角度, 是第二段相对第一段的关节角。
末端位置 的正运动学关系为 ,。将两式平方相加,消去第一关节角,可得:
这是余弦定理的代数形式。记末端到基座的距离为 ,只有 时目标才在工作空间内。等价地,计算出的 必须位于 。
求出肘上与肘下两组角度
当 时,第二关节角有正负两支。用 atan2 同时保留正弦符号和余弦值,避免单独使用反余弦时丢失分支:
然后分别代入下式求第一关节角:
例如 、、目标 ,有 。取正支得到 ,肘点 ;取负支得到约 ,肘点 。两者都到达同一个末端点。

这里的“肘上、肘下”以肘点位于基座至目标连线的哪一侧命名;不同设备的软件也可能采用相反命名,工程接口应以 的符号为准。
用正运动学验证逆解
下面的 Python 3 示例计算两支解,再将角度代回正运动学。距离误差小于 才判定通过;长度单位只需前后一致。
import math
l1, l2 = 2.0, 1.0
x, y = 2.0, 1.0
c2 = (x*x + y*y - l1*l1 - l2*l2) / (2*l1*l2)
# 容差只修正浮点舍入;明显越界的目标直接报错。
if not -1.0 - 1e-9 <= c2 <= 1.0 + 1e-9:
raise ValueError('目标不可达')
c2 = max(-1.0, min(1.0, c2))
for label, sign in [('肘下', 1), ('肘上', -1)]:
q2 = math.atan2(sign * math.sqrt(max(0.0, 1.0-c2*c2)), c2)
q1 = math.atan2(y, x) - math.atan2(
l2*math.sin(q2), l1+l2*math.cos(q2)
)
# 回代到末端位置,避免把角度分支算对但坐标算错。
px = l1*math.cos(q1) + l2*math.cos(q1+q2)
py = l1*math.sin(q1) + l2*math.sin(q1+q2)
passed = math.hypot(px-x, py-y) < 1e-9
status = '通过' if passed else '失败'
print(f'{label}: θ1={math.degrees(q1):.2f}°, '
f'θ2={math.degrees(q2):.2f}°, 正运动学校验={status}')预期输出:
肘下: θ1=0.00°, θ2=90.00°, 正运动学校验=通过
肘上: θ1=53.13°, θ2=-90.00°, 正运动学校验=通过若要在设备上使用结果,还须将角度规范化到控制器采用的范围,并检查实际关节零位、转向和限位;数学上可达不代表机械结构一定能够执行。
识别不可达与退化边界
对于 、,工作空间是半径从 到 的圆环。例如目标 位于内孔,目标 超过最大伸展半径,两者都不能用二连杆到达,不应将明显越界的 强行截断。
在外边界 时,,两杆完全伸直;在内边界 时,,两杆折叠。此时正负分支合并为同一种物理构型,雅可比行列式 ,末端某些瞬时速度方向无法实现。若两杆等长且目标恰在原点,还会出现 、 任意的无限多组退化解,应单独处理。
最后按机械限位筛选候选解:假设 、,上例仅 合法,另一支的第一关节角 超限。实际选解再结合避障和当前位置选择运动代价较小的那一支。