Lines in 3D and Plücker Coordinates
0 简述
视觉伺服中的线特征是定义在Plücker Coordinates下的一种形式。
本文是参照了2017版本的 Robot Vision and Control
其附录C Geometry中的C1.2.2一节的内容,由于书中省去了很多的推导和证明过程,看的时候不好理解,并且还存在一些问题,因此对其再进行推理理解和纠正
1 直线表达形式
通过定义两个不重合的三维点:p、q,可以在三维空间中定义一条空间直线。
定义两个点的坐标:
q
(
q
x
,
q
y
,
q
z
)
、
p
(
p
x
,
p
y
,
p
z
)
q(q_{x},q_{y},q_{z})、p(p_{x},p_{y},p_{z})
q(qx,qy,qz)、p(px,py,pz)
所以可以理解为这条直线由六个参数构成:
ℓ
=
(
q
x
,
q
y
,
q
z
,
p
x
,
p
y
,
p
z
)
\ell=(q_{x},q_{y},q_{z},p_{x},p_{y},p_{z})
ℓ=(qx,qy,qz,px,py,pz),也可以理解为一个六维向量
但是这种表示方式不是一一对应的,确定的六维向量对应一条唯一的直线,但是一条直线却对应有无数六维向量,即可以随意取直线上任意的两个不重合的点来构成这六维向量用于表示这条直线
所以采用两个三维向量来表示这个直线,分别是这个直线的方向向量和原点到这个直线的垂线方向
ℓ
=
(
ω
×
q
,
p
−
q
)
=
(
v
,
ω
)
∈
R
6
(1.1)
\ell=(\boldsymbol{\omega} \times \boldsymbol{q}, \boldsymbol{p}-\boldsymbol{q})=(\boldsymbol{v}, \boldsymbol{\omega}) \in \mathbb{R}^{6} \tag{1.1}
ℓ=(ω×q,p−q)=(v,ω)∈R6(1.1)
上式,
ω
=
p
−
q
\boldsymbol{\omega}=\boldsymbol{p}-\boldsymbol{q}
ω=p−q,是直线的方向向量;
v
=
ω
×
q
\boldsymbol{v}=\boldsymbol{\omega} \times \boldsymbol{q}
v=ω×q,这个可以将其中的
ω
\boldsymbol{\omega}
ω拆开更好理解:
v
=
ω
×
q
=
(
p
−
q
)
×
q
=
p
×
q
−
q
×
q
=
p
×
q
\boldsymbol{v}=\boldsymbol{\omega} \times \boldsymbol{q}=(\boldsymbol{p}-\boldsymbol{q})\times \boldsymbol{q}=\boldsymbol{p} \times \boldsymbol{q}-\boldsymbol{q} \times \boldsymbol{q}=\boldsymbol{p} \times \boldsymbol{q}
v=ω×q=(p−q)×q=p×q−q×q=p×q 即两个点向量的叉乘,这个向量的模等于原点与这两个点形成的三角形面积的两倍,这个后续会再说明。
书中说 v \boldsymbol{v} v 是 “a vector from the origin to a point on the line and normal to the line.”即从原点到这个直线的垂线方向,但其实这个解释应该是有问题的,实际上 v \boldsymbol{v} v是这个三角形面(原点和直线形成面)的一个法向量,后续还需要构造一次才会形成直线的位移向量。这个 v \boldsymbol{v} v实际上被称为moment vector 力矩矢量
可以发现,在直线上选择不同的两个点构造的 v \boldsymbol{v} v和 ω \boldsymbol{\omega} ω好像也是不同的六个参数。但是实际上这两个向量是相互垂直的,即 v ⋅ ω = 0 \boldsymbol{v} \cdot \boldsymbol{\omega}=0 v⋅ω=0,并且通过后续的构造,会将 v \boldsymbol{v} v转变为原点指向直线的垂线向量,这一向量对应于直线就是唯一的,而直线的方向向量 ω \boldsymbol{\omega} ω,也可以单位化,单位化后这个向量也是对应于直线是唯一的(正负性视为一致,即乘以-1系数)。所以这个形式更好。
另外还有一个问题是直线的自由度是多少?实际上直线的自由度是4,Plücker Coordinates位于六维空间中的四维流形上(我也不知道啥意思)。可以这样理解,直线穿过两个不重合的平行的平面,那么直线在空间中的所有姿态情况都能够在这两个平面中找到对应的两个点来表示(是一一对应的),每个点在平面上用两个坐标表示:即有两个自由度,两个点对应四个自由度,并且这四个参数是相互独立的,所以这个直线自由度是4!
也可以这样理解:已知空间中的刚体有六个自由度,三个平移自由度和三个转动自由度。它可以沿x轴、y轴和z轴平移,也可以绕其中一个或其他三个正交方向旋转。然而,空间中的直线只有四个自由度,因为当它作为轴围绕自身旋转或在自己的方向上平移时是不变的。
2 生成直线的系列点
直线上的系列点集可以由下式得到
p ( λ ) = v × ω ω ⋅ ω + λ ω , λ ∈ R (2.1) \boldsymbol{p}(\lambda)=\frac{\boldsymbol{v} \times \boldsymbol{\omega}}{\boldsymbol{\omega} \cdot \boldsymbol{\omega}}+\lambda \boldsymbol{\omega}, \quad \lambda \in \mathbb{R} \tag{2.1} p(λ)=ω⋅ωv×ω+λω,λ∈R(2.1)
式子右侧左边 v × ω ω ⋅ ω \frac{\boldsymbol{v} \times \boldsymbol{\omega}}{\boldsymbol{\omega} \cdot \boldsymbol{\omega}} ω⋅ωv×ω就是原点到直线的垂线向量,右侧的右边 λ ω \lambda \boldsymbol{\omega} λω即点在直线向量上的位置。可以这样理解,如果没有垂线向量,右侧的 λ ω \lambda \boldsymbol{\omega} λω构成一条通过原点的空间直线,加上垂线向量后即平移到了所求的直线上去了。
然后就是为什么 v × ω ω ⋅ ω \frac{\boldsymbol{v} \times \boldsymbol{\omega}}{\boldsymbol{\omega} \cdot \boldsymbol{\omega}} ω⋅ωv×ω是原点到直线的垂线向量呢?
1、首先根据叉乘的基本性质可以得到这个关系 v × ω ⊥ ω \boldsymbol{v} \times \boldsymbol{\omega} \perp\boldsymbol{\omega} v×ω⊥ω,说明这个向量必定是垂直于直线的,接下来只要证明这个向量的模是垂线长就可以了
2、取这个向量的模,根据定义
v
=
ω
×
q
\boldsymbol{v}=\boldsymbol{\omega} \times \boldsymbol{q}
v=ω×q 可以得到这个关系:
v
⊥
ω
\boldsymbol{v} \perp\boldsymbol{\omega}
v⊥ω,说明
∣
v
×
ω
∣
=
∣
v
∣
⋅
∣
ω
∣
⋅
sin
(
9
0
°
)
=
∣
v
∣
⋅
∣
ω
∣
|\boldsymbol{v} \times\boldsymbol{\omega}|=|\boldsymbol{v}|\cdot|\boldsymbol{\omega}|\cdot \sin(90^{\degree})=|\boldsymbol{v}|\cdot|\boldsymbol{\omega}|
∣v×ω∣=∣v∣⋅∣ω∣⋅sin(90°)=∣v∣⋅∣ω∣,就有如下等式推导
∣
v
×
ω
ω
⋅
ω
∣
=
∣
v
∣
⋅
∣
ω
∣
∣
ω
∣
2
=
∣
v
∣
∣
ω
∣
|\frac{\boldsymbol{v} \times \boldsymbol{\omega}}{\boldsymbol{\omega} \cdot \boldsymbol{\omega}}|=\frac{|\boldsymbol{v} | \cdot |\boldsymbol{\omega}|}{|\boldsymbol{\omega}|^{2}}=\frac{|\boldsymbol{v}|}{|\boldsymbol{\omega}|}
∣ω⋅ωv×ω∣=∣ω∣2∣v∣⋅∣ω∣=∣ω∣∣v∣
这个
∣
v
∣
|\boldsymbol{v}|
∣v∣实际上就是原点与
p
q
p q
pq两点形成的三角形的两倍面积(这一点与向量叉乘的性质有关,比较简单,可以自行推理),而
∣
ω
∣
|\boldsymbol{\omega}|
∣ω∣就是这个三角形的一条边长,所以这个结果就是这个三角形的高,即从原点到底边直线的垂线长。可以参看下图的二维投影图像的例子
由此可以说明这个式子的正确性质了。
2017版本的书中相应部分有一些错误,对应在596页码的式子下方给出的matlab代码,这个结果是错误的, λ = 0 \lambda=0 λ=0时,对应的应该是[0.5714 0.1429 -0.2857], λ = 1 \lambda=1 λ=1时,对应的应该是[-0.4286 -1.8571 -3.2857],错误原因是式子中 v × ω \boldsymbol{v} \times \boldsymbol{\omega} v×ω变成了 ω × v \boldsymbol{\omega} \times \boldsymbol{v} ω×v
此外如果运行其相应的matlab源代码又会发现
λ
=
1
\lambda=1
λ=1时,对应的结果变成了[0.3042 -0.3917 -1.0875],这个地方其实没有错误,主要原因是代码在这个地方使用的公式中将
ω
\boldsymbol{\omega}
ω单位化了,变成了
p
(
λ
)
=
v
×
ω
ω
⋅
ω
+
λ
ω
∣
ω
∣
,
λ
∈
R
\boldsymbol{p}(\lambda)=\frac{\boldsymbol{v} \times \boldsymbol{\omega}}{\boldsymbol{\omega} \cdot \boldsymbol{\omega}}+\frac{\lambda \boldsymbol{\omega}}{|\boldsymbol{\omega}|}, \quad \lambda \in \mathbb{R}
p(λ)=ω⋅ωv×ω+∣ω∣λω,λ∈R
2.1 点到直线的距离
求取点到直线的距离,即求得直线上距离这一点的最近的点,这个点就是垂足,这个垂线方向一定与直线方向垂直,这两个向量内积为0,假设空间中的任意一点
x
=
(
x
1
,
x
2
,
x
3
)
\boldsymbol{x}=(x_{1},x_{2},x_{3})
x=(x1,x2,x3),可列出方程求解
λ
x
\lambda_{x}
λx
(
x
−
p
(
λ
x
)
)
⋅
ω
=
0
(
x
−
v
×
ω
ω
⋅
ω
−
λ
x
ω
∣
ω
∣
)
⋅
ω
=
0
括
号
中
间
项
与
ω
垂
直
,
内
积
为
0
x
⋅
ω
−
λ
x
ω
∣
ω
∣
⋅
ω
=
0
λ
x
ω
∣
ω
∣
⋅
ω
=
x
⋅
ω
λ
x
=
x
⋅
ω
∣
ω
∣
(2.2)
\begin{aligned} (\boldsymbol{x}-\boldsymbol{p}(\lambda_{x}))\cdot \boldsymbol{\omega}&=0\\ (\boldsymbol{x}-\frac{\boldsymbol{v} \times \boldsymbol{\omega}}{\boldsymbol{\omega} \cdot \boldsymbol{\omega}}-\frac{\lambda_{x} \boldsymbol{\omega}}{|\boldsymbol{\omega}|})\cdot \boldsymbol{\omega}&=0\quad 括号中间项与\boldsymbol{\omega}垂直,内积为0\\ \boldsymbol{x}\cdot \boldsymbol{\omega}-\frac{\lambda_{x} \boldsymbol{\omega}}{|\boldsymbol{\omega}|}\cdot \boldsymbol{\omega}&=0\\ \frac{\lambda_{x} \boldsymbol{\omega}}{|\boldsymbol{\omega}|}\cdot \boldsymbol{\omega}&=\boldsymbol{x}\cdot \boldsymbol{\omega}\\ \lambda_{x} &=\frac{\boldsymbol{x}\cdot \boldsymbol{\omega}}{|\boldsymbol{\omega}|} \tag{2.2} \end{aligned}
(x−p(λx))⋅ω(x−ω⋅ωv×ω−∣ω∣λxω)⋅ωx⋅ω−∣ω∣λxω⋅ω∣ω∣λxω⋅ωλx=0=0括号中间项与ω垂直,内积为0=0=x⋅ω=∣ω∣x⋅ω(2.2)
得到的
λ
x
\lambda_{x}
λx对应的直线上的点就是
x
\boldsymbol{x}
x到直线的垂足
这个地方和书上的内容差别很大,但是其结果和此书给出的matlab代码得到的结果是一致的,反而书中的结果是对不上的,这里也给出书上的结果形式
λ
=
(
x
−
q
)
⋅
ω
ω
⋅
ω
\lambda=\frac{(\boldsymbol{x}-\boldsymbol{q}) \cdot \boldsymbol{\omega}}{\boldsymbol{\omega} \cdot \boldsymbol{\omega}}
λ=ω⋅ω(x−q)⋅ω
对应的书上的结果可以对照计算,得到的点并不在直线上
3 直线与面的交点
首先给出一些向量内积、叉积的转换等式.
a
\boldsymbol{a}
a,
b
\boldsymbol{b}
b,
c
\boldsymbol{c}
c是三个三维向量
a
⋅
b
=
b
⋅
a
(
3.1
)
a
×
b
=
−
b
×
a
,
(
3.2
)
a
⋅
(
b
×
c
)
=
(
a
×
b
)
⋅
c
,
(
3.3
)
a
×
(
b
×
c
)
=
(
a
⋅
c
)
b
−
(
a
⋅
b
)
c
(
3.4
)
\begin{aligned} \boldsymbol{a} \cdot \boldsymbol{b} &=\boldsymbol{b} \cdot \boldsymbol{a} \quad \color{green}{(3.1)}\\ \boldsymbol{a} \times \boldsymbol{b} &=-\boldsymbol{b} \times \boldsymbol{a}, \quad\color{green}{(3.2)}\\ \boldsymbol{a} \cdot(\boldsymbol{b} \times \boldsymbol{c}) &=(\boldsymbol{a} \times \boldsymbol{b}) \cdot \boldsymbol{c}, \quad \color{green}{(3.3)}\\ \boldsymbol{a} \times(\boldsymbol{b} \times \boldsymbol{c}) &=(\boldsymbol{a} \cdot \boldsymbol{c}) \boldsymbol{b}-(\boldsymbol{a} \cdot \boldsymbol{b}) \boldsymbol{c} \quad \color{green}{(3.4)} \end{aligned}
a⋅ba×ba⋅(b×c)a×(b×c)=b⋅a(3.1)=−b×a,(3.2)=(a×b)⋅c,(3.3)=(a⋅c)b−(a⋅b)c(3.4)
其中第四个等式的形式是需要注意的,后续也会用到,如果在公式3.4左侧再右叉积一个三维向量
d
\boldsymbol{d}
d,则有
(
a
×
b
)
×
(
c
×
d
)
=
(
(
a
×
b
)
⋅
d
)
c
−
(
(
a
×
b
)
⋅
c
)
d
(3.5)
(\boldsymbol{a} \times \boldsymbol{b}) \times(\boldsymbol{c} \times \boldsymbol{d})=((\boldsymbol{a} \times \boldsymbol{b}) \cdot \boldsymbol{d}) \boldsymbol{c}-((\boldsymbol{a} \times \boldsymbol{b}) \cdot \boldsymbol{c}) \boldsymbol{d} \tag{3.5}
(a×b)×(c×d)=((a×b)⋅d)c−((a×b)⋅c)d(3.5)
然后给出平面的表达形式:
n
T
x
+
d
=
0
\boldsymbol{n}^{T} \boldsymbol{x}+d=0
nTx+d=0
n
\boldsymbol{n}
n是平面的法向量,
d
d
d 可以理解为平面向法向量方向的位移(实际上是
−
d
-d
−d,方向相反),
n
T
x
\boldsymbol{n}^{T} \boldsymbol{x}
nTx可以写作内积形式
n
⋅
x
\boldsymbol{n}\cdot \boldsymbol{x}
n⋅x,所以也可写作
n
⋅
x
+
d
=
0
\boldsymbol{n} \cdot \boldsymbol{x}+d=0
n⋅x+d=0
x
\boldsymbol{x}
x既是平面上的点,又是直线上的点,于是可以用式子2.1的右侧代替
x
\boldsymbol{x}
x代入其中
n
⋅
(
v
×
ω
ω
⋅
ω
+
λ
ω
)
+
d
=
0
n
⋅
v
×
ω
ω
⋅
ω
+
λ
n
⋅
ω
+
d
=
0
λ
(
n
⋅
ω
)
=
−
n
⋅
v
×
ω
ω
⋅
ω
−
d
\begin{aligned} \boldsymbol{n} \cdot (\frac{\boldsymbol{v} \times \boldsymbol{\omega}}{\boldsymbol{\omega} \cdot \boldsymbol{\omega}}+\lambda \boldsymbol{\omega})+d&=0 \\ \boldsymbol{n} \cdot \frac{\boldsymbol{v} \times \boldsymbol{\omega}}{\boldsymbol{\omega} \cdot \boldsymbol{\omega}}+\lambda \boldsymbol{n} \cdot\boldsymbol{\omega}+d&=0 \\ \lambda (\boldsymbol{n} \cdot\boldsymbol{\omega})&=-\boldsymbol{n} \cdot \frac{\boldsymbol{v} \times \boldsymbol{\omega}}{\boldsymbol{\omega} \cdot \boldsymbol{\omega}}-d \end{aligned}
n⋅(ω⋅ωv×ω+λω)+dn⋅ω⋅ωv×ω+λn⋅ω+dλ(n⋅ω)=0=0=−n⋅ω⋅ωv×ω−d
内积
(
n
⋅
ω
)
(\boldsymbol{n} \cdot\boldsymbol{\omega})
(n⋅ω)的值是一个标量,如果这个值为0,说明这两个向量是垂直的,说明直线与平面平行,这里可以先将其移至右侧得到
λ
\lambda
λ,如果平行,可以理解为交点在无限远处
λ
=
−
n
⋅
v
×
ω
(
ω
⋅
ω
)
(
n
⋅
ω
)
−
d
n
⋅
ω
\begin{aligned} \lambda &=-\boldsymbol{n} \cdot \frac{\boldsymbol{v} \times \boldsymbol{\omega}}{(\boldsymbol{\omega} \cdot \boldsymbol{\omega}) (\boldsymbol{n} \cdot\boldsymbol{\omega})}-\frac{d}{\boldsymbol{n} \cdot\boldsymbol{\omega}} \end{aligned}
λ=−n⋅(ω⋅ω)(n⋅ω)v×ω−n⋅ωd
再把这个式子代入公式2.1
p
(
λ
)
=
x
=
v
×
ω
ω
⋅
ω
+
λ
ω
x
=
v
×
ω
ω
⋅
ω
+
(
−
n
⋅
v
×
ω
(
ω
⋅
ω
)
(
n
⋅
ω
)
−
d
n
⋅
ω
)
ω
=
v
×
ω
ω
⋅
ω
−
(
n
⋅
(
v
×
ω
)
)
ω
(
ω
⋅
ω
)
(
n
⋅
ω
)
−
d
ω
n
⋅
ω
=
(
n
⋅
ω
)
(
v
×
ω
)
(
n
⋅
ω
)
(
ω
⋅
ω
)
−
(
n
⋅
(
v
×
ω
)
)
ω
(
ω
⋅
ω
)
(
n
⋅
ω
)
−
d
ω
n
⋅
ω
=
(
n
⋅
ω
)
(
v
×
ω
)
−
(
n
⋅
(
v
×
ω
)
)
ω
(
ω
⋅
ω
)
(
n
⋅
ω
)
−
d
ω
n
⋅
ω
绿
色
部
分
刚
好
与
3.4
对
应
上
=
n
×
(
(
v
×
ω
)
×
ω
)
(
ω
⋅
ω
)
(
n
⋅
ω
)
−
d
ω
n
⋅
ω
\begin{aligned} \boldsymbol{p}(\lambda)=\boldsymbol{x}&=\frac{\boldsymbol{v} \times \boldsymbol{\omega}}{\boldsymbol{\omega} \cdot \boldsymbol{\omega}}+\lambda \boldsymbol{\omega}\\ \boldsymbol{x}&=\frac{\boldsymbol{v} \times \boldsymbol{\omega}}{\boldsymbol{\omega} \cdot \boldsymbol{\omega}}+(-\boldsymbol{n} \cdot \frac{\boldsymbol{v} \times \boldsymbol{\omega}}{(\boldsymbol{\omega} \cdot \boldsymbol{\omega}) (\boldsymbol{n} \cdot\boldsymbol{\omega})}-\frac{d}{\boldsymbol{n} \cdot\boldsymbol{\omega}} ) \boldsymbol{\omega}\\ &=\frac{\boldsymbol{v} \times \boldsymbol{\omega}}{\boldsymbol{\omega} \cdot \boldsymbol{\omega}}- \frac{(\boldsymbol{n} \cdot(\boldsymbol{v} \times \boldsymbol{\omega}))\boldsymbol{\omega}}{(\boldsymbol{\omega} \cdot \boldsymbol{\omega}) (\boldsymbol{n} \cdot\boldsymbol{\omega})}-\frac{d\boldsymbol{\omega}}{\boldsymbol{n} \cdot\boldsymbol{\omega}}\\ &=\frac{{\color{red}{(\boldsymbol{n} \cdot\boldsymbol{\omega})}} (\boldsymbol{v} \times \boldsymbol{\omega})}{{\color{red}{(\boldsymbol{n} \cdot\boldsymbol{\omega})}}(\boldsymbol{\omega} \cdot \boldsymbol{\omega})}- \frac{(\boldsymbol{n} \cdot(\boldsymbol{v} \times \boldsymbol{\omega}))\boldsymbol{\omega}}{(\boldsymbol{\omega} \cdot \boldsymbol{\omega}) (\boldsymbol{n} \cdot\boldsymbol{\omega})}-\frac{d\boldsymbol{\omega}}{\boldsymbol{n} \cdot\boldsymbol{\omega}}\\ &=\frac{{\color{green}{(\boldsymbol{n} \cdot\boldsymbol{\omega})} (\boldsymbol{v} \times \boldsymbol{\omega})-(\boldsymbol{n} \cdot(\boldsymbol{v} \times \boldsymbol{\omega}))\boldsymbol{\omega}}}{(\boldsymbol{\omega} \cdot \boldsymbol{\omega}) (\boldsymbol{n} \cdot\boldsymbol{\omega})}-\frac{d\boldsymbol{\omega}}{\boldsymbol{n} \cdot\boldsymbol{\omega}} \quad 绿色部分刚好与3.4对应上\\ &=\frac{{\color{green}{\boldsymbol{n} \times((\boldsymbol{v} \times \boldsymbol{\omega}) \times\boldsymbol{\omega})}}}{(\boldsymbol{\omega} \cdot \boldsymbol{\omega}) (\boldsymbol{n} \cdot\boldsymbol{\omega})}-\frac{d\boldsymbol{\omega}}{\boldsymbol{n} \cdot\boldsymbol{\omega}} \end{aligned}
p(λ)=xx=ω⋅ωv×ω+λω=ω⋅ωv×ω+(−n⋅(ω⋅ω)(n⋅ω)v×ω−n⋅ωd)ω=ω⋅ωv×ω−(ω⋅ω)(n⋅ω)(n⋅(v×ω))ω−n⋅ωdω=(n⋅ω)(ω⋅ω)(n⋅ω)(v×ω)−(ω⋅ω)(n⋅ω)(n⋅(v×ω))ω−n⋅ωdω=(ω⋅ω)(n⋅ω)(n⋅ω)(v×ω)−(n⋅(v×ω))ω−n⋅ωdω绿色部分刚好与3.4对应上=(ω⋅ω)(n⋅ω)n×((v×ω)×ω)−n⋅ωdω
这里主要需要看绿色公式部分的右侧括号内容,
(
v
×
ω
)
×
ω
(\boldsymbol{v} \times \boldsymbol{\omega}) \times\boldsymbol{\omega}
(v×ω)×ω,明显这两个向量垂直,
(
v
×
ω
)
⊥
ω
(\boldsymbol{v} \times \boldsymbol{\omega}) \perp\boldsymbol{\omega}
(v×ω)⊥ω,并且由于
v
⊥
ω
\boldsymbol{v} \perp \boldsymbol{\omega}
v⊥ω,所以叉积之后,这三个向量是互相垂直,容易验证,最后得到的方向应该是
−
v
-\boldsymbol{v}
−v方向,长度为
∣
v
∣
ω
∣
2
|\boldsymbol{v}|\boldsymbol{\omega}|^{2}
∣v∣ω∣2,所以上式可以写为:
x
=
n
×
(
−
∣
v
∣
∣
ω
∣
2
v
∣
v
∣
)
(
ω
⋅
ω
)
(
n
⋅
ω
)
−
d
ω
n
⋅
ω
=
n
×
(
−
∣
ω
∣
2
v
)
(
ω
⋅
ω
)
(
n
⋅
ω
)
−
d
ω
n
⋅
ω
=
n
×
(
−
v
)
n
⋅
ω
−
d
ω
n
⋅
ω
根
据
公
式
3.2
,
叉
积
交
换
顺
序
后
取
负
=
v
×
n
n
⋅
ω
−
d
ω
n
⋅
ω
=
v
×
n
−
d
ω
n
⋅
ω
\begin{aligned} \boldsymbol{x}&=\frac{{\color{green}{\boldsymbol{n} \times(-|\boldsymbol{v}||\boldsymbol{\omega}|^{2} \frac{\boldsymbol{v}}{|\boldsymbol{v}|})}}}{(\boldsymbol{\omega} \cdot \boldsymbol{\omega}) (\boldsymbol{n} \cdot\boldsymbol{\omega})}-\frac{d\boldsymbol{\omega}}{\boldsymbol{n} \cdot\boldsymbol{\omega}}\\ &=\frac{{\color{green}{\boldsymbol{n} \times(-|\boldsymbol{\omega}|^{2} \boldsymbol{v}})}}{(\boldsymbol{\omega} \cdot \boldsymbol{\omega}) (\boldsymbol{n} \cdot\boldsymbol{\omega})}-\frac{d\boldsymbol{\omega}}{\boldsymbol{n} \cdot\boldsymbol{\omega}}\\ &=\frac{{\color{green}{\boldsymbol{n} \times(-\boldsymbol{v}})}}{\boldsymbol{n} \cdot\boldsymbol{\omega}}-\frac{d\boldsymbol{\omega}}{\boldsymbol{n} \cdot\boldsymbol{\omega}} \quad 根据公式3.2,叉积交换顺序后取负\\ &=\frac{{\color{green}{\boldsymbol{v} \times\boldsymbol{n}}}}{\boldsymbol{n} \cdot\boldsymbol{\omega}}-\frac{d\boldsymbol{\omega}}{\boldsymbol{n} \cdot\boldsymbol{\omega}} \\ &=\frac{\boldsymbol{v} \times\boldsymbol{n}-d\boldsymbol{\omega}}{\boldsymbol{n} \cdot\boldsymbol{\omega}} \\ \end{aligned}
x=(ω⋅ω)(n⋅ω)n×(−∣v∣∣ω∣2∣v∣v)−n⋅ωdω=(ω⋅ω)(n⋅ω)n×(−∣ω∣2v)−n⋅ωdω=n⋅ωn×(−v)−n⋅ωdω根据公式3.2,叉积交换顺序后取负=n⋅ωv×n−n⋅ωdω=n⋅ωv×n−dω
就可以到得到点与平面相交点的公式
x
=
v
×
n
−
d
ω
n
⋅
ω
(3.6)
\boldsymbol{x}=\frac{\boldsymbol{v} \times\boldsymbol{n}-d\boldsymbol{\omega}}{\boldsymbol{n} \cdot\boldsymbol{\omega}} \tag{3.6}
x=n⋅ωv×n−dω(3.6)
4 两直线关系
两条空间直线的关系有平行parallel、相交intersect、异面skew lines,这里主要用Plücker Coordinates来解释这直线关系
从比较一般的关系,skew lines开始讲起,计算两条异面直线之间的距离。上图中一些标注如下
ℓ
1
,
ℓ
2
\ell_{1},\ell_{2}
ℓ1,ℓ2是两条已知的异面直线,相应的方向
ω
1
,
ω
2
\boldsymbol{\omega}_{1},\boldsymbol{\omega}_{2}
ω1,ω2和对应于原点的力矩矢量
v
1
,
v
2
\boldsymbol{v}_{1},\boldsymbol{v}_{2}
v1,v2都是已知的
ℓ
⊥
\ell_{\perp}
ℓ⊥是两条直线的公垂线
p
1
,
p
2
p_{1},p_{2}
p1,p2分别是直线上的两个对应于公垂线的垂足
α
\alpha
α是两个方向向量
ω
1
,
ω
2
\boldsymbol{\omega}_{1},\boldsymbol{\omega}_{2}
ω1,ω2的夹角
v
3
\boldsymbol{v}_{3}
v3是公垂线关于
p
1
p_{1}
p1点力矩矢量 moment vector
1 构造对应于点
p
1
p_{1}
p1的公垂线
ℓ
⊥
\ell_{\perp}
ℓ⊥的力矩矢量moment vector,按照定义为
v
3
=
ω
2
×
(
p
2
−
p
1
)
v
3
=
ω
2
×
p
2
−
ω
2
×
p
1
v
3
=
v
2
−
ω
2
×
p
1
(4.1)
\begin{aligned} \boldsymbol{v}_{3}&=\boldsymbol{\omega_{2}}\times(\boldsymbol{p}_{2}-\boldsymbol{p}_{1}) \\ \boldsymbol{v}_{3}&=\boldsymbol{\omega_{2}}\times\boldsymbol{p}_{2}-\boldsymbol{\omega_{2}} \times\boldsymbol{p}_{1}\\ \boldsymbol{v}_{3}&=\boldsymbol{v}_{2}-\boldsymbol{\omega_{2}} \times\boldsymbol{p}_{1} \tag{4.1} \end{aligned}
v3v3v3=ω2×(p2−p1)=ω2×p2−ω2×p1=v2−ω2×p1(4.1)
这个构造中,
ω
2
×
p
2
=
v
2
=
ω
2
×
q
2
\boldsymbol{\omega_{2}}\times\boldsymbol{p}_{2}=\boldsymbol{v}_{2}=\boldsymbol{\omega_{2}}\times\boldsymbol{q}_{2}
ω2×p2=v2=ω2×q2,其实只要这个点在直线上,这个叉积得到的力矩矢量都是一致的,可以从两个方面判断
a 构造出的向量始终垂直于同一平面,且方向一致
b 其叉积力矩矢量的模,可以参照第一个图所示,只要力矩原点距离直线的距离不变,这个三角形的面积就不会改变,代表这个叉积力矩矢量的长度不变
所以方向一致、长度相同,两个力矩矢量就是相同的
2 在图上以
p
2
\boldsymbol{p}_{2}
p2为原点画出公垂线的力矩矢量,此时就定义出了这个力矩向量和直线
ℓ
2
\ell_{2}
ℓ2所在的平面,这个平面是与公垂线
ℓ
⊥
\ell_{\perp}
ℓ⊥垂直的,所以这个平面与直线
ℓ
1
\ell_{1}
ℓ1平行,我们将直线
ℓ
1
\ell_{1}
ℓ1的方向向量
ω
1
\boldsymbol{\omega}_{1}
ω1也移动到这个平面上来,此时可以得到如下的关系
v
3
⋅
ω
1
=
∣
v
3
∣
∣
ω
1
∣
cos
(
α
+
π
2
)
v
3
⋅
ω
1
=
∣
v
3
∣
∣
ω
1
∣
(
−
sin
α
)
v
3
⋅
ω
1
=
−
∣
v
3
∣
∣
ω
1
∣
sin
α
∣
v
3
∣
=
v
3
⋅
ω
1
∣
ω
1
∣
∣
sin
α
∣
(4.2)
\begin{aligned} \boldsymbol{v}_{3}\cdot\boldsymbol{\omega}_{1}&=|\boldsymbol{v}_{3}||\boldsymbol{\omega}_{1}|\cos(\alpha+\frac{\pi}{2})\\ \boldsymbol{v}_{3}\cdot\boldsymbol{\omega}_{1}&=|\boldsymbol{v}_{3}||\boldsymbol{\omega}_{1}|(-\sin\alpha)\\ \boldsymbol{v}_{3}\cdot\boldsymbol{\omega}_{1}&=-|\boldsymbol{v}_{3}||\boldsymbol{\omega}_{1}|\sin\alpha\\ |\boldsymbol{v}_{3}|&=\frac{\boldsymbol{v}_{3}\cdot\boldsymbol{\omega}_{1}}{|\boldsymbol{\omega}_{1}||\sin\alpha|} \tag{4.2}\\ \end{aligned}
v3⋅ω1v3⋅ω1v3⋅ω1∣v3∣=∣v3∣∣ω1∣cos(α+2π)=∣v3∣∣ω1∣(−sinα)=−∣v3∣∣ω1∣sinα=∣ω1∣∣sinα∣v3⋅ω1(4.2)
由于cos函数是关于y轴对称的,所以
cos
(
α
+
π
2
)
=
cos
(
α
−
π
2
)
=
−
sin
α
\cos(\alpha+\frac{\pi}{2})=\cos(\alpha-\frac{\pi}{2})=-\sin\alpha
cos(α+2π)=cos(α−2π)=−sinα
3 这里得到的
v
3
\boldsymbol{v}_{3}
v3长度是多少呢?可以很容易地发现,这个向量的长度也是一个三角形的两倍面积,这个三角形的底长为
∣
ω
2
∣
|\boldsymbol{\omega_{2}}|
∣ω2∣,顶点是力矩原点
p
1
\boldsymbol{p}_{1}
p1,所以这个三角形相应底边的高就是
∣
p
1
p
2
∣
|\boldsymbol{p}_{1}\boldsymbol{p}_{2}|
∣p1p2∣,即为两直线之间的距离
d
=
∣
v
3
∣
∣
ω
2
∣
代
入
4.2
的
结
论
,
得
到
=
∣
v
3
⋅
ω
1
∣
∣
ω
1
∣
∣
ω
2
∣
∣
sin
α
∣
代
入
4.1
的
结
论
,
得
到
=
∣
(
v
2
−
ω
2
×
p
1
)
⋅
ω
1
∣
∣
ω
1
∣
∣
ω
2
∣
∣
sin
α
∣
=
∣
v
2
⋅
ω
1
−
(
ω
2
×
p
1
)
⋅
ω
1
∣
∣
ω
1
×
ω
2
∣
根
据
转
换
等
式
3.3
可
以
变
换
红
色
项
=
∣
v
2
⋅
ω
1
−
ω
2
⋅
(
p
1
×
ω
1
)
∣
∣
ω
1
×
ω
2
∣
再
利
用
转
换
式
3.2
,
更
换
叉
乘
方
向
=
∣
v
2
⋅
ω
1
+
ω
2
⋅
(
ω
1
×
p
1
)
∣
∣
ω
1
×
ω
2
∣
=
∣
v
2
⋅
ω
1
+
ω
2
⋅
v
1
∣
∣
ω
1
×
ω
2
∣
(
4.3
)
\begin{aligned} d&=\frac{|\boldsymbol{v}_{3}|}{|\boldsymbol{\omega}_{2}|} \quad 代入4.2的结论,得到\\ &=\frac{|\boldsymbol{v}_{3}\cdot\boldsymbol{\omega}_{1}|}{|\boldsymbol{\omega}_{1}||\boldsymbol{\omega}_{2}||\sin\alpha|} \quad 代入4.1的结论,得到\\ &=\frac{|(\boldsymbol{v}_{2}-\boldsymbol{\omega_{2}} \times\boldsymbol{p}_{1} )\cdot\boldsymbol{\omega}_{1}|}{|\boldsymbol{\omega}_{1}||\boldsymbol{\omega}_{2}||\sin\alpha|}\\ &=\frac{|\boldsymbol{v}_{2} \cdot \boldsymbol{\omega}_{1}-{\color{red}{(\boldsymbol{\omega_{2}} \times\boldsymbol{p}_{1})\cdot\boldsymbol{\omega}_{1}}}|}{|\boldsymbol{\omega}_{1}\times\boldsymbol{\omega}_{2}|}\quad 根据转换等式3.3可以变换红色项\\ &=\frac{|\boldsymbol{v}_{2} \cdot \boldsymbol{\omega}_{1}-{\color{red}{\boldsymbol{\omega_{2}} \cdot(\boldsymbol{p}_{1}\times\boldsymbol{\omega}_{1})}}|}{|\boldsymbol{\omega}_{1}\times\boldsymbol{\omega}_{2}|}\quad 再利用转换式3.2,更换叉乘方向\\ &=\frac{|\boldsymbol{v}_{2} \cdot \boldsymbol{\omega}_{1}+{\color{red}{\boldsymbol{\omega_{2}} \cdot(\boldsymbol{\omega}_{1}\times\boldsymbol{p}_{1})}}|}{|\boldsymbol{\omega}_{1}\times\boldsymbol{\omega}_{2}|}\\ &=\frac{|\boldsymbol{v}_{2} \cdot \boldsymbol{\omega}_{1}+\boldsymbol{\omega_{2}} \cdot\boldsymbol{v}_{1}|}{|\boldsymbol{\omega}_{1}\times\boldsymbol{\omega}_{2}|} \quad (4.3) \end{aligned}
d=∣ω2∣∣v3∣代入4.2的结论,得到=∣ω1∣∣ω2∣∣sinα∣∣v3⋅ω1∣代入4.1的结论,得到=∣ω1∣∣ω2∣∣sinα∣∣(v2−ω2×p1)⋅ω1∣=∣ω1×ω2∣∣v2⋅ω1−(ω2×p1)⋅ω1∣根据转换等式3.3可以变换红色项=∣ω1×ω2∣∣v2⋅ω1−ω2⋅(p1×ω1)∣再利用转换式3.2,更换叉乘方向=∣ω1×ω2∣∣v2⋅ω1+ω2⋅(ω1×p1)∣=∣ω1×ω2∣∣v2⋅ω1+ω2⋅v1∣(4.3)
这里需要讨论一下,如果
∣
ω
1
×
ω
2
∣
=
0
|\boldsymbol{\omega}_{1}\times\boldsymbol{\omega}_{2}|=0
∣ω1×ω2∣=0,即两直线平行的情况下,很容易得到距离公式
d
=
∣
v
1
×
ω
1
ω
1
2
−
v
2
×
ω
2
ω
2
2
∣
(4.4)
d=|\frac{\boldsymbol{v}_{1}\times\boldsymbol{\omega}_{1}}{\boldsymbol{\omega}_{1}^{2}}-\frac{\boldsymbol{v}_{2}\times\boldsymbol{\omega}_{2}}{\boldsymbol{\omega}_{2}^{2}}| \tag{4.4}
d=∣ω12v1×ω1−ω22v2×ω2∣(4.4)
此外如果d=0,说明两直线相交
关于如何计算具体的垂足点和公垂线的方法这里不再讨论
5 Side Operator
Side Operator 这个算子属于二次算子,用于快速表征两直线的相互关系
side
(
ℓ
1
,
ℓ
2
)
=
ℓ
1
1
ℓ
5
2
+
ℓ
2
1
ℓ
6
2
+
ℓ
3
1
ℓ
4
2
+
ℓ
4
1
ℓ
3
2
+
ℓ
5
1
ℓ
1
2
+
ℓ
6
1
ℓ
2
2
(5.1)
\operatorname{side}\left(\ell^{1}, \ell^{2}\right)=\ell_{1}^{1} \ell_{5}^{2}+\ell_{2}^{1} \ell_{6}^{2}+\ell_{3}^{1} \ell_{4}^{2}+\ell_{4}^{1} \ell_{3}^{2}+\ell_{5}^{1} \ell_{1}^{2}+\ell_{6}^{1} \ell_{2}^{2} \tag{5.1}
side(ℓ1,ℓ2)=ℓ11ℓ52+ℓ21ℓ62+ℓ31ℓ42+ℓ41ℓ32+ℓ51ℓ12+ℓ61ℓ22(5.1)
这个其实与上一节的结果是一致的,计算的是
v
2
⋅
ω
1
+
ω
2
⋅
v
1
\boldsymbol{v}_{2} \cdot \boldsymbol{\omega}_{1} +\boldsymbol{\omega_{2}}\cdot \boldsymbol{v}_{1}
v2⋅ω1+ω2⋅v1
实际上
ℓ
\ell
ℓ的参数是这样的
ℓ
=
[
p
x
q
y
−
q
x
p
y
,
p
x
q
z
−
q
x
p
z
,
p
x
−
q
x
,
p
y
q
z
−
q
y
p
z
,
p
z
−
q
z
,
p
y
−
q
y
]
\ell=[\quad \boldsymbol{p_{x}q_{y}}-\boldsymbol{q_{x}p_{y}}, \quad \boldsymbol{p_{x}q_{z}}-\boldsymbol{q_{x}p_{z}}, \quad \boldsymbol{p_{x}}-\boldsymbol{q_{x}}, \quad \boldsymbol{p_{y}q_{z}}-\boldsymbol{q_{y}p_{z}}, \quad \boldsymbol{p_{z}}-\boldsymbol{q_{z}}, \quad \boldsymbol{p_{y}}-\boldsymbol{q_{y}} \quad]
ℓ=[pxqy−qxpy,pxqz−qxpz,px−qx,pyqz−qypz,pz−qz,py−qy]
为什么写成这个形式呢,这是由于Plücker Coordinates的定义是基于以下这个矩阵的2x2子矩阵定义的,
p
q
\boldsymbol{pq}
pq是空间中的任意两个点,齐次坐标为
p
(
p
x
,
p
y
,
p
z
,
1
)
q
(
q
x
,
q
y
,
q
z
,
1
)
\boldsymbol{p}(\boldsymbol{p_{x}},\boldsymbol{p_{y}},\boldsymbol{p_{z}},1)\quad \boldsymbol{q}(\boldsymbol{q_{x}},\boldsymbol{q_{y}},\boldsymbol{q_{z}},1)
p(px,py,pz,1)q(qx,qy,qz,1)
[
p
x
p
y
p
z
1
q
x
q
y
q
z
1
]
\begin{bmatrix} &\boldsymbol{p_{x}} &\boldsymbol{p_{y}} &\boldsymbol{p_{z}} &1 \quad\\ &\boldsymbol{q_{x}} &\boldsymbol{q_{y}} &\boldsymbol{q_{z}} &1 \quad \end{bmatrix}
[pxqxpyqypzqz11]
分别做置换积,
12
→
13
→
14
→
23
→
24
→
34
12 \to 13 \to 14 \to 23 \to 24 \to 34
12→13→14→23→24→34 所以是这个顺序
当算子 结果为0,说明两条直线平行或者相交,反之则反