机器人 / 系统课时

推导平面二连杆的解析逆解

推导平面二连杆的解析逆解

给定两段连杆的长度和末端目标坐标,可以直接算出平面双旋转关节机械臂的关节角。本篇从正运动学建立余弦约束,求出肘上、肘下两组解析解,再通过数值回代检查可达性和解的正确性。

学习目标

  • 从末端坐标推导第二关节角的余弦约束。
  • 用 atan2 求出并区分两种肘部构型。
  • 判断不可达目标、边界退化和关节限位,并用正运动学验证结果。

从末端坐标建立约束

设基座位于原点 O=(0,0)O=(0,0),世界 XX 轴向右、YY 轴向上,逆时针为正。两段连杆长分别为 L1>0L_1>0、L2>0L_2>0;θ1\theta_1 是第一段相对世界 XX 轴的角度,θ2\theta_2 是第二段相对第一段的关节角。

末端位置 (x,y)(x,y) 的正运动学关系为 x=L1cos⁡θ1+L2cos⁡(θ1+θ2)x=L_1\cos\theta_1+L_2\cos(\theta_1+\theta_2),y=L1sin⁡θ1+L2sin⁡(θ1+θ2)y=L_1\sin\theta_1+L_2\sin(\theta_1+\theta_2)。将两式平方相加,消去第一关节角,可得:

c2=cos⁡θ2=x2+y2−L12−L222L1L2c_2=\cos\theta_2=\frac{x^2+y^2-L_1^2-L_2^2}{2L_1L_2}

这是余弦定理的代数形式。记末端到基座的距离为 r=x2+y2r=\sqrt{x^2+y^2},只有 ∣L1−L2∣≤r≤L1+L2|L_1-L_2|\le r\le L_1+L_2 时目标才在工作空间内。等价地,计算出的 c2c_2 必须位于 [−1,1][-1,1]。

求出肘上与肘下两组角度

当 −1<c2<1-1<c_2<1 时,第二关节角有正负两支。用 atan2 同时保留正弦符号和余弦值,避免单独使用反余弦时丢失分支:

θ2(±)=atan2⁡(±1−c22,c2)\theta_2^{(\pm)}=\operatorname{atan2}(\pm\sqrt{1-c_2^2},c_2)

然后分别代入下式求第一关节角:

θ1=atan2⁡(y,x)−atan2⁡(L2sin⁡θ2,L1+L2cos⁡θ2)\theta_1=\operatorname{atan2}(y,x)-\operatorname{atan2}(L_2\sin\theta_2,L_1+L_2\cos\theta_2)

例如 L1=2L_1=2、L2=1L_2=1、目标 P=(2,1)P=(2,1),有 c2=(5−4−1)/4=0c_2=(5-4-1)/4=0。取正支得到 (θ1,θ2)=(0∘,90∘)(\theta_1,\theta_2)=(0^\circ,90^\circ),肘点 J=(2,0)J=(2,0);取负支得到约 (53.13∘,−90∘)(53.13^\circ,-90^\circ),肘点 J=(1.2,1.6)J=(1.2,1.6)。两者都到达同一个末端点。

同一末端目标的平面二连杆肘下与肘上解析解

这里的“肘上、肘下”以肘点位于基座至目标连线的哪一侧命名;不同设备的软件也可能采用相反命名,工程接口应以 θ2\theta_2 的符号为准。

用正运动学验证逆解

下面的 Python 3 示例计算两支解,再将角度代回正运动学。距离误差小于 10−910^{-9} 才判定通过;长度单位只需前后一致。

python
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}')

预期输出:

text
肘下: θ1=0.00°, θ2=90.00°, 正运动学校验=通过
肘上: θ1=53.13°, θ2=-90.00°, 正运动学校验=通过

若要在设备上使用结果,还须将角度规范化到控制器采用的范围,并检查实际关节零位、转向和限位;数学上可达不代表机械结构一定能够执行。

识别不可达与退化边界

对于 L1=2L_1=2、L2=1L_2=1,工作空间是半径从 11 到 33 的圆环。例如目标 (0,0)(0,0) 位于内孔,目标 (4,0)(4,0) 超过最大伸展半径,两者都不能用二连杆到达,不应将明显越界的 c2c_2 强行截断。

在外边界 r=3r=3 时,θ2=0\theta_2=0,两杆完全伸直;在内边界 r=1r=1 时,θ2=±π\theta_2=\pm\pi,两杆折叠。此时正负分支合并为同一种物理构型,雅可比行列式 det⁡J=L1L2sin⁡θ2=0\det J=L_1L_2\sin\theta_2=0,末端某些瞬时速度方向无法实现。若两杆等长且目标恰在原点,还会出现 θ2=π\theta_2=\pi、θ1\theta_1 任意的无限多组退化解,应单独处理。

最后按机械限位筛选候选解:假设 −20∘≤θ1≤40∘-20^\circ\le\theta_1\le40^\circ、−120∘≤θ2≤120∘-120^\circ\le\theta_2\le120^\circ,上例仅 (0∘,90∘)(0^\circ,90^\circ) 合法,另一支的第一关节角 53.13∘53.13^\circ 超限。实际选解再结合避障和当前位置选择运动代价较小的那一支。

按自己的节奏,确认真正理解。

目录与阅读设置

阅读设置

17

记录一个想法

推导平面二连杆的解析逆解

0 / 10000 · 笔记保存到账号。未提交的修改会在关闭、离开或刷新后丢失。

登录哪都通