机器人手臂运动学与轨迹规划全解析

### 机器人手臂运动学与轨迹规划全解析 #### 1. 机器人手臂运动学基础 在机器人领域，运动学是理解和控制机器人动作的关键。我们可以运用之前介绍的各种方法，如正向运动学。以`irb140`机器人为例： ```python >>> irb140.fkine(irb140.qr).printline("rpy/xyz") t = 0.005, 0, 0.332; rpy/xyz = 0°, -90°, -90° ``` 还能创建面条图： ```python >>> irb140.plot(irb140.qr); ``` 或者使用滑块进行交互式控制： ```python >>> irb140.teach() ``` 用Denavit - Hartenberg符号定义的机器人，也能转换为ETS格式： ```python >>> irb140.ets() ``` #### 2. 逆运动学问题 逆运动学是一个具有实际意义的问题，即已知末端执行器的期望位姿，求所需的关节坐标。用函数形式表示为： \[q = K^{-1}(\xi_E)\] 一般来说，逆运动学的解不是唯一的，即一个特定的末端执行器位姿可以由多种关节配置实现。确定逆运动学有两种方法： - **封闭形式或解析解**：使用几何或代数技术求解，但随着机器人关节数量的增加，难度会增大，有些串联机器人甚至不存在封闭形式的解。 - **迭代数值解**：通过迭代的方式逐步逼近解。 ##### 2.1 二维（平面）机器人手臂逆运动学 以二维两关节机器人为例，我们用代数封闭形式和数值两种方法求解逆运动学。 ###### 2.1.1 封闭形式解 首先，使用SymPy符号计算正向运动学。定义机器人长度的符号常量： ```python >>> import sympy >>> a1, a2 = sympy.symbols("a1 a2") >>> e = ET2.R() * ET2.tx(a1) * ET2.R() * ET2.tx(a2); ``` 接着，定义表示关节角度的符号变量： ```python >>> q0, q1 = sympy.symbols("q0 q1") ``` 计算正向运动学，得到SE(2)矩阵： ```python >>> TE = e.fkine([q0, q1]) cos(q0 + q1) -sin(q0 + q1) a1*cos(q0) + a2*cos(q0 + q1) sin(q0 + q1) cos(q0 + q1) a1*sin(q0) + a2*sin(q0 + q1) 0 0 1 ``` 只考虑末端执行器的位置： ```python >>> x_fk, y_fk = TE.t; ``` 定义表示期望末端执行器位置的符号变量： ```python >>> x, y = sympy.symbols("x y") ``` 得到两个方程： \[x_{fk} = x; y_{fk} = y\] SymPy不能直接求解这类三角方程，我们对两个方程平方相加并化简： ```python >>> eq1 = (x_fk**2 + y_fk**2 - x**2 - y**2).trigsimp() a1**2 + 2*a1*a2*cos(q1) + a2**2 - x**2 - y**2 ``` 求解`q1`： ```python >>> q1_sol = sympy.solve(eq1, q1) [-acos(-(a1**2 + a2**2 - x**2 - y**2)/(2*a1*a2)) + 2*pi, acos((-a1**2 - a2**2 + x**2 + y**2)/(2*a1*a2))] ``` 求解`q0`，先展开方程： ```python >>> eq0 = tuple(map(sympy.expand_trig, [x_fk - x, y_fk - y])) (a1*cos(q0) + a2*(-sin(q0)*sin(q1) + cos(q0)*cos(q1)) - x, a1*sin(q0) + a2*(sin(q0)*cos(q1) + sin(q1)*cos(q0)) - y) ``` 求解`sin(q0)`和`cos(q0)`： ```python >>> q0_sol = sympy.solve(eq0, [sympy.sin(q0), sympy.cos(q0)]); ``` 计算`tan(q0)`并化简： ```python >>> sympy.atan2(q0_sol[sympy.sin(q0)], ... q0_sol[sympy.cos(q0)]).simplify() atan2((a1*y - a2*x*sin(q1) + a2*y*cos(q1))/(a1**2 + 2*a1*a2*cos(q1) + a2**2), (a1*x + a2*x*cos(q1) + a2*y*sin(q1))/(a1**2 + 2*a1*a2*cos(q1) + a2**2)) ``` ###### 2.1.2 数值解 逆运动学问题可以看作是调整关节坐标，使正向运动学结果与期望位姿匹配的优化问题： \[q^* = \arg \min_{q} \|K(q) - \xi_E^*\| \] 以二维两关节机器人为例： ```python >>> e = ET2.R() * ET2.tx(1) * ET2.R() * ET2.tx(1); >>> pstar = np.array([0.6, 0.7]); # desired position >>> E = lambda q: np.linalg.norm(e.fkine(q).t - pstar); >>> sol = optimize.minimize(E, [0, 0]); ``` 得到使误差最小的关节角度： ```python >>> sol.x array([ -0.2295, 2.183]) ``` 验证解的正确性： ```python >>> e.fkine(sol.x).printline() t = 0.6, 0.7; 112° ``` ##### 2.2 三维机器人手臂逆运动学 对于常见的六轴工业机器人，很多可以用封闭形式解来求解逆运动学，但前提是具有球形手腕机制。以经典的PUMA 560机器人为例： ```python >>> puma = models.DH.Puma560(); >>> puma.qn array([ 0, 0.7854, 3.142, 0, 0.7854, 0]) >>> T = puma.fkine(puma.qn); >>> T.printline() t = 0.596, -0.15, 0.657; rpy/zyx = 0°, 90°, 0° ``` 使用解析封闭形式解计算逆运动学： ```python >>> sol = puma.ikine_a(T) IKSolution: q=[2.649, 2.356, 0.09396, -0.609, -0.9743, -2.768], success=True ``` 验证解的正确性： ```p ```