第3講:三維空間剛體運動
三維空間中剛體運動的描述方式:旋轉矩陣、變換矩陣、四元數和歐拉角
3.1 旋轉矩陣
3.1.1 點和向量,座標系
三維空間中,給定線性空間基(e1,e2,e3),則向量a,b∈R3,
a=[e1e2e3]⎣⎡a1a2a3⎦⎤=a1e1+a2e2+a3e3(3-1)
內積:
a⋅b=aTb=i=1∑3aibi=∣a∣∣b∣cos⟨a,b⟩(3-2)
內積描述向量間的投影關係。外積爲:
a×b=⎣⎡ia1b1ja2b2ka3b3⎦⎤=⎣⎡a2b3−a3b2a3b1−a1b3a1b2−a2b1⎦⎤=⎣⎡0a3−a2−a30a1a2−a10⎦⎤b≜a∧b(3-3)
外積的方向垂直於這兩個向量,大小爲∣a∣∣b∣⟨a,b⟩,表示兩個向量張成的四邊形的有向面積。符號∧表示反對稱符號,將a改寫成反對稱矩陣(skew-symmetric)形式,其作用爲將外積a×b寫成矩陣與向量的乘積a∧b,從而將外積變成線性運算。外積只對三維向量存在定義,外積可以表示向量的旋轉。
在右手法則下,用右手的四指從a轉向b,其拇指方向就是旋轉向量的方向,即a×b的方向,其大小由 a和b的夾角決定。該方式構造了從a到b的旋轉向量,該向量同樣位於三維空間中,在此座標系下,可以用三個實數描述。
3.1.2 座標系間的歐氏變換
兩個座標系間的變換關係:旋轉、平移。機器人在運動過程中,通常設定一個慣性座標系(世界座標系),認爲其固定不動,如圖中xW,yW,zW;相機或機器人則是一個移動座標系,如xC,yC,zC。相機視野中某個向量p,其座標爲pc,在世界座標系中,其座標爲pw。
剛體運動:同一個向量在各個座標系下的長度和夾角不會改變,這種變換稱爲歐氏變換。
旋轉:假設一組單位正交基(e1,e2,e3)經一次旋轉,變成(e1′,e2′,e3′),對於同一個向量a(該向量並沒有隨座標系旋轉而運動),其在兩個座標系下的座標分別爲[a1,a2,a3]T和[a1′,a2′,a3′]T,則:
[e1e2e3]⎣⎡a1a2a3⎦⎤=[e1′e2′e3′]⎣⎡a1′a2′a3′⎦⎤(3-4)
由單位正交基性質可知:
⎣⎡a1a2a3⎦⎤=⎣⎡e1Te2Te3T⎦⎤[e1′e2′e3′]⎣⎡a1′a2′a3′⎦⎤=⎣⎡e1Te1′e2Te1′e3Te1′e1Te2′e2Te2′e3Te2′e1Te3′e2Te3′e3Te3′⎦⎤⎣⎡a1′a2′a3′⎦⎤≜Ra′(3-5)
矩陣R由兩組基之間的內積組成,表示旋轉前後同一個向量的座標變換關係,即R表示旋轉,又稱爲旋轉矩陣(rotation matrix)。旋轉矩陣是一個行列式爲1的正交矩陣;反之,行列式爲1的正交矩陣也是一個旋轉矩陣。所以,旋轉矩陣的集合定義爲:
SO(n)={R∈Rn×n∣RRT=I,det(R)=1}(3-6)
SO(n)表示特殊正交羣(special orthogonal group),SO(3)表示三維空間中的旋轉。旋轉矩陣可以描述相機的旋轉。
旋轉矩陣爲正交陣,其逆(即轉置)表示方向相反的旋轉:
a′=R−1a=RTa(3-7)
假設世界座標系中的向量a,經過旋轉(R)和平移t後,得到a′,則:
a′=Ra+t(3-8)
其中,t爲平移向量。旋轉矩陣R和平移向量t共同完整地描述一個歐氏空間的座標變換關係。
3.1.3 變換矩陣與齊次座標
引入齊次座標和變換矩陣重寫方程(3-8):
[a′1]=[R0Tt1][a1]≜T[a1](3-9)
在三維向量的末尾添加1,組成四維向量,稱爲齊次座標。對於齊次向量,可以將旋轉和平移包含在一個矩陣裏,其中矩陣T稱爲變換矩陣(transform matrix)。用a~表示a的齊次座標。
在射影幾何中,通過添加最後一維,用四個實數描述一個三維向量。在齊次座標中,點x的每個分量同乘一個非零常數k後,仍然表示同一個點。因此,點的座標值並是唯一,如[1,1,1,1]T和[2,2,2,2]T表示同一個點。因此,當最後一項不爲零時,將所有座標除以最後一項,強制最後一項爲 1,從而得到一個點唯一的座標表示(即轉換成非齊次座標):
x~=[x,y,z,w]T=[x/w,y/w,z/w,1]T(3-10)
多次歐氏變換可表示爲:
b~=T1a~,c~=T2b~⇒c~=T2T1a~(3-11)
變換矩陣T的結構:左上角爲旋轉矩陣、右側爲平移向量、左下角爲零向量、右下角爲1,這種矩陣又稱爲特殊歐氏羣(special euclidean group):
SE(3)={T=[R0Tt1]∈R4×4∣R∈SO(3),t∈R3}(3-12)
變換矩陣T的逆表示反向變換:
T−1=[RT0T−RTt1](3-13)
在不引起歧義的情況下,齊次座標與普通座標符號一般不做區分,默認爲符合所用運算法則的那一種。
3.2 實踐:Eigen
#include <iostream>
#include <ctime>
using namespace std;
#include <Eigen/Core>
#include <Eigen/Dense>
#define MATRIX_SIZE 100
int main(int argc, char const *argv[])
{
Eigen::Matrix<float, 2, 3> matrix_23;
Eigen::Vector3d v_3d;
Eigen::Matrix3d matrix_33 = Eigen::Matrix3d::Zero();
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> matrix_dynamic;
Eigen::MatrixXd matrix_x;
matrix_23 << 1, 2, 3, 4, 5, 6;
cout << matrix_23 << endl;
for (int i = 0; i < 1; i++)
{
for (int j = 0; j < 2; j ++)
{
cout << matrix_23(i, j) << endl;
}
}
v_3d << 3, 2, 1;
Eigen::Matrix<double, 2, 1> result = matrix_23.cast<double>() * v_3d;
cout << result << endl;
matrix_33 = Eigen::Matrix3d::Random();
cout << matrix_33 << endl;
cout << matrix_33.transpose() << endl;
cout << matrix_33.sum() << endl;
cout << matrix_33.trace() << endl;
cout << 10 * matrix_33 << endl;
cout << matrix_33.inverse() << endl;
cout << matrix_33.determinant() << endl;
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigen_solver ( matrix_33.transpose() * matrix_33 );
cout << "eigen values = " << eigen_solver.eigenvalues() << endl;
cout << "eigen vectors = " << eigen_solver.eigenvectors() << endl;
Eigen::Matrix<double, MATRIX_SIZE, MATRIX_SIZE> matrix_NN;
matrix_NN = Eigen::MatrixXd::Random(MATRIX_SIZE, MATRIX_SIZE);
Eigen::Matrix<double, MATRIX_SIZE, 1> v_Nd;
v_Nd = Eigen::VectorXd::Random(MATRIX_SIZE);
clock_t time_stt = clock();
Eigen::Matrix<double, MATRIX_SIZE, 1> x = matrix_NN.inverse() * v_Nd;
cout << "time use in normal inverse is " << 1000 * (clock() - time_stt) / (double)CLOCKS_PER_SEC << "ms" << endl;
time_stt = clock();
x = matrix_NN.colPivHouseholderQr().solve(v_Nd);
cout << "time use in Qr compsition is " << 1000 * (clock() - time_stt) / (double)CLOCKS_PER_SEC << "ms" << endl;
return 0;
}
# CMakeLists.txt
# 聲明要求的 cmake 最低版本
cmake_minimum_required( VERSION 2.8 )
# 聲明一個 cmake 工程
project( Eigen )
# 添加頭文件
include_directories( "/usr/include/eigen3" )
# 添加一個可執行程序
# 語法:add_executable( 程序名 源代碼文件 )
add_executable( eigen_matrix eigen_matrix.cpp )
# 設置編譯模式
set( CMAKE_BUILD_TYPE "Debug" )
3.3 旋轉向量和歐拉角
3.3.1 旋轉向量
矩陣表示方式的缺點:
-
SO(3)旋轉矩陣有九個量,但一次旋轉只有三個自由度,因此這種表達方式是冗餘的。同理,變換矩陣用十六個量表達了六自由度變換;
-
旋轉矩陣自帶約束:必須爲正交矩陣,且行列式爲1。變換矩陣也是如此。約束會使估計、優化旋轉矩陣(變換矩陣)變得困難。
任意旋轉都可以用一個旋轉軸和一個旋轉角表示,因此旋轉可以用一個向量表示旋轉,其方向與旋轉軸一致、長度等於旋轉角,這種向量稱爲旋轉向量(rotation vector)(軸角,axis-angle)。同理,變換可以用一個旋轉向量和一個平移向量表示,此時,維數恰好爲6。
旋轉向量和旋轉矩陣之間的轉換:假設有一個旋轉軸爲n、角度爲θ的旋轉,其對應的旋轉向量爲θn,由羅德里格斯公式(Rodrigues’s formula):
R=cosθI+(1−cosθ)nnT+sinθn∧(3-14)
其中,∧表示向量到反對稱的轉換符。旋轉矩陣到旋轉向量的轉換:
tr(R)=cosθtr(I)+(1−cosθ)tr(nnT)+sinθtr(n∧)=1+2cosθ
因此,轉角θ爲:
θ=arccos(2tr(R)−1)(3-16)
由於轉軸向量n旋轉不變,因此
Rn=n
轉軸n是矩陣R特徵值1對應的特徵向量。
3.3.2 歐拉角
**歐拉角(Euler angle)**用三個分離的轉角把一個旋轉分解成三次繞不同軸的旋轉,提供了一種直觀方式描述旋轉。
根據不同的分解方式,歐拉角存在不同的定義方法。例如先繞X軸旋轉,再繞Y軸,最後繞Z軸,就得到了一個XYZ軸的旋轉。同理,可以定義ZYZ、ZYX等旋轉方式。此外,還需區分每次旋轉是繞固定軸,還是繞旋轉後的軸。
歐拉角常用“偏航-俯仰-滾轉”(yaw-pitch-roll)三個角度描述一個旋轉,其等價於ZYX軸旋轉。假設一個剛體的前方爲X軸,右側爲Y軸,上方爲Z軸,ZYX轉角把旋轉分解成:
-
繞物體的Z軸旋轉,得到偏航角(yaw);
-
繞旋轉之後的Y軸旋轉,得到俯仰角(pitch);
-
繞旋轉之後的X軸旋轉,得到滾轉角(roll)。
此時,使用三維向量[r,p,y]T描述任意旋轉。歐拉角的缺點是萬向鎖問題(gimbal lock):當俯仰角爲±90∘時,第一次旋轉與第三次旋轉將使用同一個軸,使得系統丟失了一個自由度(三次旋轉變成兩次旋轉),稱爲奇異性問題。理論上,只要用三個實數表示三維旋轉時,都會碰到奇異性問題。因此,歐拉角不適於插值和迭代,只能用於人機交互。
3.4 四元數
3.4.1 四元數的定義
三維旋轉是一個三維流形,若要無奇異性地表示,三個量不夠,因此,三維向量表示旋轉的無奇異性方式不存在。
四元數(quaternion):Hamilton提出的一種擴展複數,緊湊且無奇異性。一個四元數q有一個實部和三個虛部,通常實部在前(也有實部在後的):
q=q0+q1i+q2j+q3k(3-17)
其中,i,j,k爲四元數的三個虛部,三個虛部滿足:
⎩⎪⎪⎪⎨⎪⎪⎪⎧i2=j2=k2=−1ij=k,ji=−kjk=i,kj=−iki=j,ij=−j(3-18)
四元數也用一個標量和一個向量表示:
q=[s,v], s=q0∈R,v=[q1,q2,q3]T∈R3
其中,s爲四元數實部,而v爲虛部。如果一個四元數虛部爲0,稱之爲實四元數;反之,若其實部爲0,稱之爲虛四元數。
單位四元數可以表示三維空間中任意旋轉,假設一個旋轉繞單位向量n=[nx,ny,nz]T旋轉θ,則該旋轉的四元數爲:
q=[cos2θ,nxsin2θ,nysin2θ,nzsin2θ]T(3-19)
單位四元數對應旋轉軸與旋轉角分別爲:
{θ=2arccosq0[nx,ny,nz]T=sin2θ[q1,q2,q3]T(3-20)
在四元數中,任意的旋轉都可以由兩個互爲相反數的四元數表示。當θ=0時,得到一個沒有旋轉的實四元數:
q0=[±1,0,0,0]T(3-21)
3.4.2 四元數的運算
四元數包括四則運算、數乘、求逆、共軛等。
給定兩個四元數
qa=[sa,va]=sa+xai+yaj+zak
qb=[sb,vb]=sb+xbi+ybj+zbk
- 加法和減法
qa±qb=[sa±sb,va±vb](3-22)
- 乘法:qa的各項與qb的各項分別相乘,然後相加,虛部遵從方程(3-18)
qaqb=sasb−xaxb−yayb−zazb+(saxb+xasb+yazb−zayb)i+(sayb−xazb+yasb+zaxb)j+(sazb+xayb−yaxb+zasb)k=[sasb−vaTvb,sava+sbvb+va×vb](3-24)
四元數乘法通常不可交換,除非va和vb在R3中共線,此時外積項爲零。
- 共軛:四元數共軛是把虛部取成相反數
q∗=[s,−v]=s−xi−yj−zk(3-25)
四元數共軛與自身相乘,得到一個實四元數,其實部爲模長的平方:
q∗q=qq∗=[s2−vTv,0](3-26)
- 模長
∥q∥=s2+x2+y2+z2(3-27)
兩個四元數乘積的模即爲模的乘積,單位四元數相乘後仍是單位四元數。
∥qaqb∥=∥qa∥∥qb∥(3-28)
- 逆
q−1=∥q∥q∗(3-29)
則四元數與其逆的乘積爲實四元數1:
q−1q=qq−1=1(3-30)
單位四元數的逆和共軛相同。乘積的逆滿足
(qaqb)−1=qb−1qa−1(3-31)
- 數乘與點乘
四元數與標量相乘
kq=[ks,kv](3-32)
點乘:兩個四元數對應位置元素分別相乘
qa⋅qb=sasb+xaxbi+yaybj+zazbk(3-33)
3.4.3 用四元數表示旋轉
給定三維空間中點p=[x,y,z]∈R3和軸角n,θ指定的旋轉,點p經旋轉後記爲p′,則點的旋轉四元數表示爲:
- 將點p表示爲虛:
p=[0,x,y,z]=[0,v]
- 由方程(3-19)旋轉的四元數q表示爲:
q=[cos2θ,nsin2θ]
則旋轉後點p′的表示爲:
p′=qpq−1(3-34)
3.4.4 四元數到旋轉矩陣的轉換
四元數q=q0+q1i+q2j+q3k對應的旋轉矩陣R爲:
R=⎣⎡1−2q22−2q322q1q2−2q0q32q1q3+2q0q22q1q2+2q0q31−2q12−2q322q2q3−2q0q12q1q3−2q0q22q2q3+2q0q11−2q12−2q22⎦⎤(3-35)
反之,旋轉矩陣R=[mij],i,j∈{1,2,3}對應的四元數q爲:
q0=2tr(R)+1,q1=4q0m23−m32,q2=4q0m31−m13,q3=4q0m12−m21(3-36)
3.5 相似、仿射、射影變換
歐氏變換保持向量的長度和夾角不變,相當於將一個剛體進行了移動或旋轉,不改變其自身的樣子。
- 相似變換
相似變換比歐氏變換多了一個自由度,它允許物體進行均勻縮放,其矩陣表示爲:
TS=[sR0Tt1](3-37)
旋轉矩陣部分增加的縮放因子s,表示在對向量旋轉之後,還可以在x,y,z三個座標上對物體均勻縮放。例如,邊長爲1的立方體通過相似變換後,變成邊長爲10的立方體。
- 仿射變換
仿射變換的矩陣形式爲:
TA=[A0Tt1](3-38)
仿射變換要求A是一個可逆矩陣,不必是正交矩陣。仿射變換也叫正交投影。例如,立方體經過仿射變換後,變成平行六面體。
- 射影變換
射影變換形式最爲一般:
TP=[AaTt1](3-39)
其左上角爲可逆矩陣A、右上爲平移t、左下縮放aT。由於採用齊次座標,當v=0時,可以對矩陣各元素除以v得到右下角爲1的矩陣;否則,得到右下角爲0的矩陣。因此,2D的射影變換共有8個自由度,3D共有15個自由度。從真實世界到相機照片的變換是一個射影變換。如果相機的焦距爲無窮遠,這個變換則爲仿射變換。
在“不變性質”中,從上到下是包含關係。例如,歐氏變換除了保體積之外,也具有保平行、相交等性質。
實踐:Eigen幾何模塊
#include <iostream>
#include <cmath>
using namespace std;
#include <Eigen/Core>
#include <Eigen/Geometry>
int main(int argc, char const *argv[])
{
Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();
Eigen::AngleAxisd rotation_vector(M_PI / 4, Eigen::Vector3d(0, 0, 1));
cout.precision(3);
cout << "rotation matrix = \n" << rotation_vector.matrix() << endl;
rotation_matrix = rotation_vector.toRotationMatrix();
Eigen::Vector3d v(1, 0, 0);
Eigen::Vector3d v_rotated = rotation_vector * v;
cout << "(1,0,0) after rotation = " << v_rotated.transpose() << endl;
v_rotated = rotation_matrix * v;
cout << "(1,0,0) after rotation = " << v_rotated.transpose() << endl;
Eigen::Vector3d euler_angle = rotation_matrix.eulerAngles(2, 1, 0);
cout << "yaw pitch roll = " << euler_angle.transpose() << endl;
Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
T.rotate(rotation_vector);
T.pretranslate(Eigen::Vector3d(1, 3, 4));
Eigen::Vector3d v_transformed = T * v;
cout << "v tranformed = " << v_transformed.transpose() << endl;
Eigen::Quaterniond q = Eigen::Quaterniond(rotation_vector);
cout << "quaternion = \n" << q.coeffs() << endl;
q = Eigen::Quaterniond(rotation_matrix);
cout << "quaternion = \n" << q.coeffs() << endl;
v_rotated = q * v;
cout << "(1,0,0) after rotation = " << v_rotated.transpose() << endl;
return 0;
}
# CMakeLists.txt
# 聲明要求的 cmake 最低版本
cmake_minimum_required( VERSION 2.8 )
# 聲明一個 cmake 工程
project( Eigen )
# 添加頭文件
include_directories( "/usr/include/eigen3" )
# 添加一個可執行程序
# 語法:add_executable( 程序名 源代碼文件 )
add_executable( use_geometry use_geometry.cpp )
# 設置編譯模式
set( CMAKE_BUILD_TYPE "Debug" )
p.s.
scipy
部分實現了Eigen
scipy.spatial.transform.Rotation(quat, normalize=True, copy=True)
類:實現3D旋轉
-
接口:
-
四元數(quaternion)
-
旋轉矩陣(rotation matrix)
-
旋轉向量(rotation vector)
-
歐拉角(Euler angle)
-
支持:
-
Application on vectors
-
Rotation Composition
-
Rotation Inversion
-
Rotation Indexing
from scipy.spatial.transform import Rotation as R
import numpy as np
r = R.from_rotvec(np.pi / 4 * np.array([0, 0, 1]))
print(r.as_matrix())
[[ 0.70710678 -0.70710678 0. ]
[ 0.70710678 0.70710678 0. ]
[ 0. 0. 1. ]]
v = np.array([1, 0, 0])
rotation_matrix = r.as_matrix()
rotation_vector = r.as_rotvec()
v_rotated = np.matmul(rotation_matrix, v)
print("(1,0,0) after rotation = ", v_rotated.T)
v_rotated = r.apply(v)
print("(1,0,0) after rotation = ", v_rotated.T)
(1,0,0) after rotation = [0.70710678 0.70710678 0. ]
(1,0,0) after rotation = [0.70710678 0.70710678 0. ]
euler_angles = r.as_euler(seq="zyx", degrees=True)
print("yaw pitch roll = ", euler_angles)
yaw pitch roll = [45. 0. 0.]