机器人运动逆解算IK

Inverse kinematics

基本概念

从任务(task)空间坐标到关节(joints)空间坐标的转换。

IK element元素：由基点base、连杆links、关节joints、顶点tip、靶target组成，如上图所示

IK group组：整体，包含至少一个IK元素，在IK组基础上定义properties；复杂情况下多个IK元素有共同关节，需要同时进行求解，必须放在相同IK组中并确定次序（如下图所示）

IK element的目标：使得Tip与Target重合并有一定约束

注意：需要确定运动学链的properties的正确性

当IK元素过度约束时，采用damped resolution method(e.g.DLS)，迭代最小化tip和target的距离，并约束使tip朝向target

范例

两个IK组的机制不共享关节，但是必须按照先解算IK1后解算IK2的顺序，否则就会造成下图的情况：

有共享关节的情况下，如果两个target可同时达到，则分两个IK组迭代反解算的效果 与在同一IK组内反解效果相同，否则需要在同一IK组内计算权重 并使tip分别指向target

数学推导

DH模型

坐标系定义

$x_i$轴与$z_{i-1}$轴垂直并指向$z_{i+1}$轴

$z_i$轴方向与第$i+1$个关节轴线重合

$O_i$为$z_i$轴与$z_{i-1}$轴交点或与两者公垂线交点

参数定义

关节角$\theta$：关节旋转角度（关节旋转变量），${\theta }_i$为$x_i$轴与$x_{i-1}$轴夹角

连杆长度$a$：两相邻关节轴的公垂线长度（固定参数），$a_{i-1}$为$z_i$轴与$z_{i-1}$轴公垂线长度

连杆偏置$d$：两相邻x轴的距离（关节平移变量），$d_i$为$x_i$轴与$x_{i-1}$轴距离

连杆扭转角$\alpha$：连杆两端关节扭转角度（固定参数），${\alpha }_{i-1}$为$z_i$轴与$z_{i-1}$轴夹角

坐标变换

两个关节空间坐标系的平移旋转变换写为矩阵形式：
UR5为六自由度机械臂(即5连杆6关节)，则末端机械爪关节坐标系与基坐标系变换关系：
即为位姿变换基本矩阵

Jacobian矩阵

在向量微积分中，雅可比矩阵是一阶偏导数以一定方式排列成的矩阵，其行列式称为雅可比行列式。

上图中端点位置坐标KaTeX parse error: \tag works only in display equations

雅各比矩阵$J=\left[\begin{array}{cc}\frac{\partial X}{\partial {\theta }_1}& \frac{\partial X}{\partial {\theta }_2}\\ \frac{\partial Y}{\partial {\theta }_1}& \frac{\partial Y}{\partial {\theta }_2}\end{array}\right]$是函数$X,Y$对于${\theta }_1,{\theta }_2$的偏导数矩阵

则有微分形式$dV=\left[\begin{array}{c}dX\\ dY\end{array}\right]=J\left[\begin{array}{c}d{\theta }_1\\ d{\theta }_2\end{array}\right]$，简写为$dV=Jd\theta$

三维空间端点广义位姿矩阵 V = { p x , p y , p z , α x , α y , α z } V = \{p_x,p_y,p_z,\alpha_x,\alpha_y,\alpha_z \} V={px​,py​,pz​,αx​,αy​,αz​}

六自由度角度矩阵 θ = { θ 1 , θ 2 , θ 3 , θ 4 , θ 5 , θ 6 } \theta = \{\theta_1,\theta_2,\theta_3,\theta_4,\theta_5,\theta_6\} θ={θ1​,θ2​,θ3​,θ4​,θ5​,θ6​}

广义雅各比矩阵$J=\left[\begin{array}{ccc}\frac{\partial p_x}{\partial {\theta }_1}& ...& \frac{\partial p_x}{\partial {\theta }_6}\\ \frac{\partial p_y}{\partial {\theta }_1}& ...& \frac{\partial p_y}{\partial {\theta }_6}\\ \frac{\partial p_z}{\partial {\theta }_1}& ...& \frac{\partial p_z}{\partial {\theta }_6}\\ \frac{\partial {\alpha }_x}{\partial {\theta }_1}& ...& \frac{\partial {\alpha }_x}{\partial {\theta }_6}\\ \frac{\partial {\alpha }_y}{\partial {\theta }_1}& ...& \frac{\partial {\alpha }_y}{\partial {\theta }_6}\\ \frac{\partial {\alpha }_z}{\partial {\theta }_1}& ...& \frac{\partial {\alpha }_z}{\partial {\theta }_6}\end{array}\right]$

通过求解$J^{-1}$即可根据端点的运动得到关节的运动$d\theta =J^{-1}dV$

CoppeliaSim

内置模块

使用CoppeliaSim内置运动学模块，通过GUI进行设置

范例

雅各比矩阵逆变换(Pseudo inverse)：

如果target位置过远或者为singular configuration，将会打破平衡，无法达成目标，如下图所示:

阻尼最小二乘法(damped least square method, DLS)：

更稳定(移动target位置，tip始终与target重叠或指向target)，但迭代次数更多

构建IK组

打开顶部导航栏Tools中的Scene Object Properties和Calculation Modules

在Scene Object Properties中将自由关节设为IK mode，选中tip将target添加为”linked dummy”并设置类型为“IK, tip-target”

在Calculation Modules中选择Kinematics选项卡，点击“add new IK group”，选择新建的IK组，点击下方“edit IK elements”，点击“add new IK element with tip”，选中tip并设置base，勾选其他Constraints section选项

API：sim.handleIkGroup

function sysCall_init()
    ikgroup=sim.getIkGroupHandle('IKGroup') --根据IK组名称更改
end
function sysCall_actuation()
    sim.handleIkGroup(ikgroup)
end

开启仿真即可进行逆解算，使tip与target重叠

外部编程

Coppelia Kinematics Routines，独立的C++例程

Coppelia运动学例程包装到一个插件simExtIK中，通过脚本函数(e.g. simIK.getConfigForTipPose)进行调用

Coppelia在程序启动时自动加载CoppeliaSimPro文件夹中的所有插件，也可在github下载