参考资料：现代机器人学
D-H 参数表达
有关开链机器人正向运动学的 D-H 参数表达，可基于附着在每个杆上的参考坐标系之间的相对位移来描述。 若连杆坐标系按顺序分别标注为 \(\{0\}, \cdots, \{n+1\}\)，如 \(\{0\}\) 为基座坐标系 \(\{s\}\)，\(\{i\}\) 为附着在杆 \(i\) 上处于关节 \(i\) 位置的连杆坐标系， \(\{n+1\}\) 为末端工具坐标系。正向运动学方程可写成
\[
T_{0,n+1}(\theta) = T_{01}(\theta_1)T_{12}(\theta_2)\cdots T_{n-1,n}(\theta_n)T_{n,n+1}
\]
式中，\(\theta_i\) 表示第 \(i\) 个关节的关节变量，\(T_{n,n+1}\) 表示末端坐标系相对 \(\{n\}\) 的位形。 若末端坐标系 \(\{b\}\) 与 \(\{n\}\) 重合，\(T_{n,n+1}\) 项可忽略掉。
使用 D-H 法时，需要严格定义连杆坐标系。 遵照此规定，连杆坐标系 \(\{i-1\}\) 与 \(\{i\}\) 之间的齐次变换矩阵 \(T_{i-1,i}\) 可以用 4 个参数来表示（称为 D-H 参数），其中 3 个参数表示运动学结构特征，第四个参数为关节值。 这 4 个参数为描述连杆坐标系之间相对位移的最小数。
指数积公式 PoE 表达
正向运动学也可以写成指数积的形式（以空间坐标系的形式为例），即
\[
T(\theta) = e^{[S_1]\theta_1} \cdots e^{[S_n]\theta_n}M
\]
式中，\(S_i = (\omega_i, v_i)\) 为关节 \(i\) 相对基坐标系的旋量坐标，\(\theta_i\) 表示第 \(i\) 个关节的关节变量，\(M \in SE(3)\) 表示当机器人处于初始位形时，末端坐标系的位姿； 这时，无须单独定义对应各杆的连杆坐标系，只需要定义 \(M\) 和螺旋轴 \(S_1, \cdots, S_n\)。
引入依赖
#r "nuget: MathNet.Numerics"
open type System.Math
open MathNet.Numerics.LinearAlgebra
帮助方法
// n维单位矩阵
let Iof (n: int) : Matrix<float> = Matrix<float>.Build.DenseIdentity(n)
// n x m 零矩阵
let Oof (n: int) (m: int) : Matrix<float> = Matrix<float>.Build.Dense(n, m, 0.0)
// 矩阵元素四舍五入到指定小数位数
let roundMatrix (digits: int) (M: Matrix<float>) : Matrix<float> =
M.Map (fun x ->
let x = Round(x, digits)
if x = 0.0 then 0.0 else x
)
// 3维向量叉乘
let cross3 (a: Vector<float>) (b: Vector<float>) : Vector<float> =
Vector<float>.Build.DenseOfArray [|
a[1] * b[2] - a[2] * b[1]
a[2] * b[0] - a[0] * b[2]
a[0] * b[1] - a[1] * b[0]
|]
// 组合子矩阵为大矩阵
let composeSubMatrixs (subMs: Matrix<float>[][]) : Matrix<float> =
let n = subMs |> Array.sumBy (Array.head >> _.RowCount)
let m = subMs[0] |> Array.sumBy _.ColumnCount
let M = Oof n m
let mutable rowOffset = 0
for i in 0 .. subMs.Length - 1 do
let mutable colOffset = 0
for j in 0 .. subMs[i].Length - 1 do
let subM = subMs[i][j]
M.SetSubMatrix(rowOffset, subM.RowCount, colOffset, subM.ColumnCount, subM)
colOffset <- colOffset + subM.ColumnCount
rowOffset <- rowOffset + (subMs[i][0]).RowCount
M
test
composeSubMatrixs [|
[| Iof 2; Oof 2 1; Iof 2 |]
[| Oof 3 2; Oof 3 1; Oof 3 2 |]
[| Iof 2; Oof 2 1; Iof 2 |]
|]
|> printfn "%A"
output
DenseMatrix 7x5-Double
1 0 0 1 0
0 1 0 0 1
0 0 0 0 0
0 0 0 0 0
0 0 0 0 0
1 0 0 1 0
0 1 0 0 1
指数积公式 PoE 相关计算
// 3维向量的反对称矩阵
let ``⟦v⟧ of`` (v: Vector<float>) : Matrix<float> =
Matrix<float>.Build.DenseOfRowArrays [|
[| 0.0; -v[2]; v[1] |]
[| v[2]; 0.0; -v[0] |]
[|-v[1]; v[0]; 0.0 |]
|]
// 构造单位螺旋轴 S 的矩阵表示 [S]
let ``[S] of`` (``⟦w⟧``: Matrix<float>) (v: Vector<float>) : Matrix<float> =
composeSubMatrixs [|
[| ``⟦w⟧``; Matrix.Build.DenseOfColumnVectors v |]
[| Oof 1 3; Iof 1 |]
|]
// 罗德里格斯公式实现
let ``e^⟦w⟧ɵ calc`` (``[w]``: Matrix<float>) (ɵ: float) : Matrix<float> =
let I = Matrix<float>.Build.DenseIdentity(3)
I + sin ɵ * ``[w]`` + (1.0 - cos ɵ) * ``[w]`` * ``[w]``
// T = e^[S]ɵ 计算
let ``e^[S]ɵ calc`` (w: Vector<float>) (v: Vector<float>) (ɵ: float) : Matrix<float> =
let I = Matrix<float>.Build.DenseIdentity(3)
let ``⟦w⟧`` = ``⟦v⟧ of`` w
let ``e^⟦w⟧ɵ`` = ``e^⟦w⟧ɵ calc`` ``⟦w⟧`` ɵ
let ``*`` = (I * ɵ + (1.0 - cos ɵ) * ``⟦w⟧`` + (ɵ - sin ɵ) * ``⟦w⟧`` * ``⟦w⟧``) * v
composeSubMatrixs [|
[| ``e^⟦w⟧ɵ``; Matrix.Build.DenseOfColumnVectors ``*`` |]
[| Oof 1 3; Iof 1 |]
|]
// 刚体运动的齐次坐标表示。
// T of (R, p) 的矩阵表示，其中 R 为旋转矩阵，p 为平移向量。
let Tof (R: Matrix<float>) (p: Vector<float>) : Matrix<float> =
composeSubMatrixs [|
[| R; Matrix.Build.DenseOfColumnVectors p |]
[| Oof 1 3; Iof 1 |]
|]
// T 的伴随矩阵
let Adof (T: Matrix<float>) : Matrix<float> =
let R = T.SubMatrix(0, 3, 0, 3)
let p = T.SubMatrix(0, 3, 3, 1).Column(0)
let ``[p]`` = ``⟦v⟧ of`` p
composeSubMatrixs [|
[| R; Oof 3 3 |]
[| ``[p]`` * R; R |]
|]
// calculate e^[S]ɵ based on w and point and theta
let multiplyTof (wpɵs: float[][]) : Matrix<float> =
let mutable T = Iof 4
for i, wpɵ in Seq.indexed wpɵs do
let w = Vector<float>.Build.DenseOfArray [| wpɵ[0]; wpɵ[1]; wpɵ[2] |]
let p = Vector<float>.Build.DenseOfArray [| wpɵ[3]; wpɵ[4]; wpɵ[5] |]
let ɵ = wpɵ[6]
let v = - cross3 w p
let Ti = ``e^[S]ɵ calc`` w v ɵ
T <- T * Ti
// printfn "T%d\n%A" (i + 1) (roundMatrix 3 Ti)
// printfn "T\n%A" (roundMatrix 3 T)
// printfn "---------------------------"
T
正向运动学计算
// 给定末端的初始位形M，末端坐标系下的关节旋量（由旋转轴和位置坐标表示）以及关节值，计算末端坐标系
let fKinBody (M: Matrix<float>) (wpɵs: float[][]) = M * multiplyTof wpɵs
// 给定末端的初始位形M，空间坐标系下的关节旋量（由旋转轴和位置坐标表示）以及关节值，计算末端坐标系
let fKinSpace (M: Matrix<float>) (wpɵs: float[][]) = multiplyTof wpɵs * M
test
let W1 = 0.109
let W2 = 0.082
let L1 = 0.425
let L2 = 0.392
let H1 = 0.089
let H2 = 0.095
let M = Matrix<float>.Build.DenseOfRowArrays [|
[|-1.0; 0.0; 0.0; L1 + L2 |]
[| 0.0; 0.0; 1.0; W1 + W2 |]
[| 0.0; 1.0; 0.0; H1 - H2 |]
[| 0.0; 0.0; 0.0; 1.0 |]
|]
fKinSpace M [|
[| 0; 0; 1; 0; 0; 0; 0 |]
[| 0; 1; 0; 0; 0; H1; -PI / 2.0 |]
[| 0; 1; 0; L1; W1; H1; 0 |]
[| 0; 1; 0; L1+L2;0; H1; 0 |]
[| 0; 0;-1; L1+L2;W1; H1; PI / 2.0 |]
[| 0; 1; 0; L1+L2;W1+W2;H1-H2; 0 |]
|]
|> roundMatrix 3
|> printfn "%A"
output
DenseMatrix 4x4-Double
0 -1 0 0.095
1 0 0 0.109
0 0 1 0.988
0 0 0 1