1 冗余机器人运动学与动力学概述

冗余机器人是一个相对的概念，并不是一个机器人自由度越多就是冗余机器人。当机器人执行任务的维度小于机器人本身的自由度时，可以认为机器人是冗余的。即有更多任务完成选择，机器人因而存在任务的零空间。最典型的冗余机器人比如七自由度机械臂，或者蛇形机器人等。冗余机器人的任务$x=f(q)$,其中$x=[p_x,p_y,p_z,\alpha,\beta,\gamma]^T$ ,$(x,R^M),(q,R^N)$

当$M<N$时，其为运动学冗余机械臂 。对于相同的机械臂末端轨迹，关节运动情况无法唯一确定。对于冗余机械臂，对应于相同的末端位置和姿态，机械臂存在无数种构型。所以当末端位姿确定时，其关节仍运动的现象叫做机械臂的“自运动”，这是冗余机械臂在实际运动过程中与非冗余机械臂最大的区别，也是冗余机械臂冗余度的具体体现。

冗余机器人的动力学建模与其他非冗余机器人的动力学建模原理和过程完全一致。可以采用包括牛顿-欧拉方程、拉格朗日方程、空间矢量以及空间算子代数的方法建立机器人的动态模型。

考虑到机械臂的七个关节角度，$q=[{q}_1...{q}_7]^T$ ,运动学通过机器人的齐次变换矩阵来求解。

$_{7}^{0}T(q)=_{1}^{0}T(q_1)_{2}^{1}T(q_2)_{3}^{2}T(q_3)_{4}^{3}T(q_4)_{5}^{4}T(q_5)_{6}^{5}T(q_6)_{7}^{6}T(q_7)$

冗余机器人的正向运动学与其他非冗余机器人的完全一致。均是通过DH表格以及T变换矩阵求解。

• 速度级运动学 $\dot x=J(q)\dot q$
• 加速度级运动学 $\ddot x=J(q)\ddot q+\dot J(q,\dot q)\dot q$
• 速度级逆向运动学 $\dot q=J^+(q)\dot x+(I-J^+(q)J(q))\dot q_0$
• 加速度级逆向运动学 $\ddot q=J^+(q)(\ddot x-\dot J(q)\dot q)+(I-J^+(q)J(q))\ddot q_0$

冗余机器人的逆向运动学主要包含位置机械臂、速度级别以及加速度级别。就位置级别而言，冗余机器人的逆向运动学需要添加特殊扩展项才可以得到冗余解；就速度/加速度级别而言，冗余机器人的逆向运动学需要存在雅可比零空间，零空间内的运动不影响末端任务的完成。

刚体机器人的运动学和动力学相对来说较为成熟，目前有很多的开源软件可以进行直接的计算，建议在进行机器人运动学建模和动力学建模参考相关开源代码。KDL库是Orocos项目的一部分，属于开源的运动学和动力学库，提供实时可用的运动学和动力学代码，它包含用于刚体运动学计算的代码，以及运动学结构及其反向和正向运动学求解器。

git clone https://github.com/orocos/orocos_kinematics_dynamics.git

mkdir build && cd build cmake -DCMAK_BUILD_TYPE=Release .. make -j4 sudo make install

其他开源库可以参考：

Rex Gym ,Become A Software Engineer At Top Companies,Hexapod Robot Simulator ,Klein ,Hexapod ,Ik ,Ikpy ,Three.ik

Robopy ,Robot Gui ,Duik 15 ,Pybotics ,Pymanoid ,Grl ,Vrarmik ,Spinner Bindings ,Robond Kinematics Project ,Rbdl Orb,Caliko

,track IK，DART. IDtree

机械臂刚体动力学的具体表达式：

$M(q)\ddot q+C(q,\dot q)+G(q)={\tau}$

2 冗余机器人位置控制系统设计

下面以混合阻抗控制为例具体阐述机器人控制器设计。6自由度机械臂具有完整的笛卡尔位姿控制能力。笛卡尔空间某一位姿，6自由度非冗余机械臂仅具有有限组关节角度以及构型与之对应。所以对于已经非冗余机械臂的控制，其运动控制是唯一确定的。与6自由度非冗余机械臂相比，冗余机械臂可以利用冗余自由度进行运动学与动力学优化，从而达到更好的关节运动限制，避奇异，避障和优化关节驱动力矩等。

冗余机器人的位置控制核心是冗余分解，逆解的实际步骤包括：a设定关节极限，b计算雅可比矩阵，c奇异判断，d寻找最优解

以构型控制为例说明：

选择笛卡尔主任务$x_e=f(q)$, $\dot x_e=J_f(q)\dot q$, 扩展任务，$x_c=g(q)$ , $\dot x_c=J_g(q)\dot q$,则定义$x=[x_e，x_c]^T$ ,$\dot x=J(q)\dot q$

具体表示如下所示。

冗余机械臂的位置控制仿真系统包括操作空间、扩充任务空间的轨迹规划模块，逆运动学模块，关节空间计算力矩控制器，机械臂正向动力学模块以及正运动学模块。根据轨迹规划得到的运动学量不同，其可以分为跟踪目标位置的的位置控制、跟踪目标速度的位置控制或者为跟踪目标加速度的位置控制。本节采用基于跟踪目标速度的运动控制。为了更好的跟踪期望轨迹，关节层控制器采用计算力矩控制。冗余机械臂的位置控制系统如上图所示。在Matlab/Simulink 中建立机器人的数值仿真系统，具体如下所示。

为了更精确的实现运动控制，该机械臂的运动控制系统为全闭环运动控制系统，其内闭环通过反馈关节角度进行关节空间位置修正，外闭环反馈笛卡尔轨迹进行笛卡尔空间位置修正，因而其位置控制精度较高。

3 冗余机器人阻抗控制系统设计

冗余机械臂的阻抗控制主要是需要考虑到末端任务的阻抗控制以及冗余机械臂零空间的阻抗控制。末端任务的阻抗控制与非冗余机械臂是完全一致的，具体如下所示：

混合阻抗的表达式如：

$M^d(\ddot x-S{\ddot x^d}))+B^d(\dot x-S{\dot x^d}))+K^dS(x-x^d))-(I-S)F^d=-F_e$

当$S=1$,主要体现位置和姿态方向上的柔顺性，当$S=0$，主要体现在时变力的跟踪。

位置控制方向上采用笛卡尔基于位置的阻抗控制，机械臂等效为弹簧-质量-阻尼系统。该方向不进行力跟踪，但须保证柔顺性。在机械臂力控制方向上，机械臂等效为质量-阻尼系统，设定其刚度为0，从而实现精确的力控制。在Simulink中搭建七自由度空间机械臂的混合阻抗控制系统，其中包括轨迹规划，阻抗控制器，误差修正模块(ERC)，关节层控制器，机械臂正向动力学计算模块，正向运动学模块以及环境模型组成。

以镜面擦拭任务以及插拔任务为例，说明具体的控制策略以及选择矩阵情况。

而冗余机械臂零空间包含以下两种模式

• 模式1不包含关节柔顺的运动学/动力学指标零空间优化，此种情况主要是选择相应的优化指标，根据零空间的优化得出最佳解。
• 模式2 包含关节柔顺的零空间阻抗控制，此种模式下，零空间用于力的交互，需要采用关节力矩传感器。

综上，混合阻抗控制主要包括以下三方面

• 在混合阻抗控制中，通过选择矩阵，任务空间分为位置控制子空间以及力控制子空间
• 在位置控制子空间中采用阻抗控制保证该方向上柔顺性
• 在力控制方向上，通过该方向基于力控的目标阻抗跟踪期望力。该方法在机械臂接近环境以及接触环境过程中不需要切换控制律

4 冗余机器人控制实验设计

总之，冗余机械臂在面向人类生活的服务场景，面向特种环境下的操作等应用场景将发挥着更加重要的作用。冗余机器人甚至超冗余机器人的将会更加充分的发挥机器人的灵巧操作能力，更多展现机器人的灵活性与敏捷性。

【技术创作101训练营】四步重新认识冗余机器人的控制器设计

PS：推荐书籍

• Featherstone, R. (2007). Rigid Body Dynamics Algorithms. Boston: Springer.
• Siciliano, B., and Khatib, O. (eds.) (2008). Springer Handbook of Robotics. Berlin: Springer.
• Hogan, N.(1984). Impedance Control: An Approach to Manipulation, American Control Conference