文章目录
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 x_i xi轴与 z i − 1 z_{i-1} zi−1轴垂直并指向 z i + 1 z_{i+1} zi+1轴
z i z_i zi轴方向与第 i + 1 i+1 i+1个关节轴线重合
O i O_i Oi为 z i z_i zi轴与 z i − 1 z_{i-1} zi−1轴交点或与两者公垂线交点
参数定义
关节角 θ \theta θ：关节旋转角度（关节旋转变量）， θ i \theta_i θi为 x i x_i xi轴与 x i − 1 x_{i-1} xi−1轴夹角
连杆长度 a a a：两相邻关节轴的公垂线长度（固定参数）， a i − 1 a_{i-1} ai−1为 z i z_i zi轴与 z i − 1 z_{i-1} zi−1轴公垂线长度
连杆偏置 d d d：两相邻x轴的距离（关节平移变量）， d i d_i di为 x i x_i xi轴与 x i − 1 x_{i-1} xi−1轴距离
连杆扭转角 α \alpha α：连杆两端关节扭转角度（固定参数）， α i − 1 \alpha_{i-1} αi−1为 z i z_i zi轴与 z i − 1 z_{i-1} zi−1轴夹角
坐标变换
两个关节空间坐标系的平移旋转变换写为矩阵形式：UR5为六*度机械臂(即5连杆6关节)，则末端机械爪关节坐标系与基坐标系变换关系：
即为位姿变换基本矩阵
Jacobian矩阵
在向量微积分中，雅可比矩阵是一阶偏导数以一定方式排列成的矩阵，其行列式称为雅可比行列式。
上图中端点位置坐标KaTeX parse error: \tag works only in display equations
雅各比矩阵 J = [ ∂ X ∂ θ 1 ∂ X ∂ θ 2 ∂ Y ∂ θ 1 ∂ Y ∂ θ 2 ] J = \begin{bmatrix} \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{bmatrix} J=[∂θ1∂X∂θ1∂Y∂θ2∂X∂θ2∂Y]是函数 X , Y X,Y X,Y对于 θ 1 , θ 2 \theta_1,\theta_2 θ1,θ2的偏导数矩阵
则有微分形式 d V = [ d X d Y ] = J [ d θ 1 d θ 2 ] dV = \begin{bmatrix} dX \\ dY \end{bmatrix} = J \begin{bmatrix} d\theta_1 \\ d\theta_2 \end{bmatrix} dV=[dXdY]=J[dθ1dθ2]，简写为 d V = J d θ dV = J d\theta dV=Jdθ
三维空间端点广义位姿矩阵 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 = [ ∂ p x ∂ θ 1 . . . ∂ p x ∂ θ 6 ∂ p y ∂ θ 1 . . . ∂ p y ∂ θ 6 ∂ p z ∂ θ 1 . . . ∂ p z ∂ θ 6 ∂ α x ∂ θ 1 . . . ∂ α x ∂ θ 6 ∂ α y ∂ θ 1 . . . ∂ α y ∂ θ 6 ∂ α z ∂ θ 1 . . . ∂ α z ∂ θ 6 ] J = \begin{bmatrix} \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{bmatrix} J=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎡∂θ1∂px∂θ1∂py∂θ1∂pz∂θ1∂αx∂θ1∂αy∂θ1∂αz..................∂θ6∂px∂θ6∂py∂θ6∂pz∂θ6∂αx∂θ6∂αy∂θ6∂αz⎦⎥⎥⎥⎥⎥⎥⎥⎥⎤
通过求解 J − 1 J^{-1} J−1即可根据端点的运动得到关节的运动 d θ = J − 1 d V d\theta = J^{-1} dV dθ=J−1dV
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下载