開源機器人庫orocos KDL 學習筆記二：Geometric

開源機器人庫orocos KDL 學習筆記二：Geometric

如果按照上一篇搭建的VS2015 orocos KDL環境，會出現3個例程：geometry, chainiksolverpos_lma_demo, trajectory_example. 這一篇具體看一下Geometric有哪些定義。這些Geometric的概念都是機器人學的基礎，本文只是解析一下KDL裏面是如何實現的，具體幾何上的含義可以參考【1】【2】兩本機器人學書籍。

在frames.hpp中，定義了這些基礎的幾何類型:
- KDL::Vector 向量
- KDL::Rotation 旋轉矩陣
- KDL::Frame 座標變換矩陣
- KDL::Twsit 平移和旋轉速度向量
- KDL::Wrench 力和力矩向量
- KDL::Vector2 二維向量
- KDL::Rotation2 二維旋轉矩陣
- KDL::Frame2 二維座標變換矩陣

這些類可以用來描述機器人的位置、姿態、座標系等。geometry例程主要是介紹這些類的用法。這裏主要介紹這些類的功能和可以實現的操作。

KDL::Vector

Vector是一個三維向量，包括X-Y-Z的座標值。表示一個點相對於參考座標系的三維座標，KDL::Vector=[x y z]T$KDL::Vector=[xyz{]}^T$ 。

1. Vector初始化有三種：

inline Vector() {data[0]=data[1]=data[2] = 0.0;}
inline Vector(double x,double y, double z);
inline Vector(const Vector& arg);

Vector v1; //The default constructor, X-Y-Z are initialized to zero
Vector v2(x,y,z); //X-Y-Z are initialized with the given values 
Vector v3(v2); //The copy constructor
Vector v4 = Vector::Zero(); //All values are set to zero

分別表示：v1表示使用默認初始化函數初始化一個零向量；v2表示用x,y,z初始化；v3表示將v2複製初始化v3；v4表示用零向量初始化；

2. Get/Set 元素

有兩種方式get/set單個元素：使用索引[ ]和( )操作；使用x()，y()，z()；

v1[0]=v2[1];//copy y value of v2 to x value of v1 
v2(1)=v3(3);//copy z value of v3 to y value of v2
v3.x( v4.y() );//copy y value of v4 to x value of v3

[ ]和( )操作索引從 0-2，DEBUG/NDEBUG的定義確定是否使用索引檢查。

inline double x() const;
inline double y() const;
inline double z() const;
inline void x(double);
inline void y(double);
inline void z(double);

x，y，z可以單獨取值和賦值；

3. 標量乘除

v2=2*v1;
v3=v1/2;

Vector的每個元素都與標量進行乘除；向量放在乘號左邊或右邊都行；

inline friend Vector operator*(const Vector& lhs,double rhs);
inline friend Vector operator*(double lhs,const Vector& rhs);

4. 向量之間的加減

v2+=v1;
v3-=v1;
v4=v1+v2;
v5=v2-v3;

5. 叉乘和點乘

v3=v1*v2; //Cross product
double a=dot(v1,v2)//Scalar product

符合向量的叉乘和點乘規則；

6. Reset一個向量

SetToZero(v1);

7. 向量之間的比較

v1==v2;
v2!=v3;
Equal(v3,v4,eps);//with accuracy eps

逐個元素進行比較，可以自定義精度eps；如果不使用自定義精度，那麼精度就是默認定義在文件utility.cxx的KDL::epsilon中；

namespace KDL {

int STREAMBUFFERSIZE=10000;
int MAXLENFILENAME = 255;
const double PI=       3.1415926535897932384626433832795;
const double deg2rad = 0.01745329251994329576923690768488;
const double rad2deg = 57.2957795130823208767981548141052;
double epsilon = 0.000001;
}

8. 將Vector裏的每個元素取反

v1.ReverseSign();
v2 = -v1;

inline void ReverseSign();
inline friend Vector operator-(const Vector& arg);

9. 向量歸一化

double n1 = v1.Normalize();
double n2 = v2.Norm();

/*
* Normalizes this vector and returns it norm
* makes v a unitvector and returns the norm of v.
* if v is smaller than eps, Vector(1,0,0) is returned with norm 0.
* if this is not good, check the return value of this method.
*/
double Normalize(double eps=epsilon);

//!    @return the norm of the vector
double Norm() const;

Normalize()函數將向量歸一化，並返回它的範數，讓其成爲單位向量。如果範數小於精度eps則返回向量爲：(1,0,0)，範數爲0。求範數的方法是：norm=x2+y2+z2−−−−−−−−−−√$norm=\sqrt{x^2+y^2+z^2}$ ，求歸一化向量的方法是：(x/norm,y/norm,z/norm)$(x/norm,y/norm,z/norm)$ ；
Norm()函數返回向量的範數，不歸一化；

10. 二維向量轉成三維向量

v1.Set2DXY(v2);//v1.x=v2.x, v1.y=v2.y, v1.z=0

這裏v2表示XY平面的二維向量，v1表示三維向量；其他平面二維向量也是同樣；

inline void Set2DXY(const Vector2& v);
inline void Set2DYZ(const Vector2& v);
inline void Set2DZX(const Vector2& v);

inline void Set2DPlane(const Frame& F_someframe_XY,const Vector2& v_XY);

Set2DPlane函數表示將二維XY平面的向量轉成三維，並在某一座標系中表示。其中F_someframe_XY表示一個座標變換T，T由3*3旋轉矩陣R和平移向量p組成，那麼：

v1.Set2DPlane(T,v2);

表示先進行：v1.Set2DXY(v2)$v1.Set2DXY(v2)$ ，再進行：R∗v1+p$R\ast v1+p$ ；

KDL::Rotation

Rotation是一個3x3矩陣，表示物體相對於一個座標系的三維旋轉；

⎡⎣⎢XxXyXzYxYyYzZxZyZz⎤⎦⎥$$\left[\begin{array}{ccc}Xx& Yx& Zx\\ Xy& Yy& Zy\\ Xz& Yz& Zz\end{array}\right]$$

用數組表示就是：

⎡⎣⎢data[0]data[3]data[6]data[1]data[4]data[7]data[2]data[5]data[8]⎤⎦⎥$$\left[\begin{array}{ccc}data[0]& data[1]& data[2]\\ data[3]& data[4]& data[5]\\ data[6]& data[7]& data[8]\end{array}\right]$$

1. 創建旋轉矩陣

創建旋轉矩陣有兩種方式，安全的方式和不安全的方式；
以下的方式爲安全的方式創建旋轉矩陣：
所謂安全，是指通過下面的方式創建的旋轉矩陣是一致的，即矩陣是單位正交陣。

Rotation r1; //The default constructor, initializes to an 3x3 identity matrix
Rotation r2 = Rotation::Identity();//Identity Rotation = zero rotation
Rotation r3 = Rotation::RPY(roll,pitch,yaw); //Rotation built from Roll-Pitch-Yaw angles
Rotation r4 = Rotation::EulerZYZ(alpha,beta,gamma); //Rotation built from Euler Z-Y-Z angles
Rotation r5 = Rotation::EulerZYX(alpha,beta,gamma); //Rotation built from Euler Z-Y-X angles
Rotation r6 = Rotation::Rot(vector,angle); //Rotation built from an equivalent axis(vector) and an angle.

r1調用默認構造函數，初始化爲單位矩陣；r2表示用單位矩陣初始化；r3使用RPY角初始化；r4使用ZYZ歐拉角初始化；r5使用ZYX歐拉角初始化；r6採用繞任意軸vector旋轉角度angle的方式初始化；
這裏提到的概念：RPY角、ZYZ歐拉角、ZYX歐拉角、繞任意軸旋轉，都是表示兩個座標系之間旋轉關係的方式，也可以說是表示物體姿態的方式，具體見參考文獻；

以下創建旋轉矩陣的方式爲不安全的方式：
不安全的方式意味着旋轉矩陣不一定是單位正交陣，程序也不會去檢查是否是單位正交陣；

Rotation r6( Xx,Yx,Zx,Xy,Yy,Zy,Xz,Yz,Zz);//Give each individual element (Column-Major)
Rotation r7(vectorX,vectorY,vectorZ);//Give each individual column

2. 取值

取旋轉矩陣中單個元素的值：

double Zx = r1(0,2);

Zx表示旋轉矩陣中第一行第三列的值；索引從0到2；
也可以獲取ZYZ歐拉角、ZYX歐拉角、RPY角以及任意旋轉軸和角度：

r1.GetEulerZYZ(alpha,beta,gamma);
r1.GetEulerZYX(alpha,beta,gamma);
r1.GetRPY(roll,pitch,yaw);
axis = r1.GetRot();//gives only rotation axis
angle = r1.GetRotAngle(axis);//gives both angle and rotation axis

除此之外，還可以獲取單位向量的值：

vecX=r1.UnitX();//or
r1.UnitX(vecX);
vecY=r1.UnitY();//or
r1.UnitY(vecY);
vecZ=r1.UnitZ();//or
r1.UnitZ(vecZ);

3. 旋轉矩陣的逆/轉置

旋轉矩陣是正交陣，逆與轉置相同；

r1.SetInverse();//r1 is inverted and overwritten

將r1取逆，並將r1覆蓋；

r2=r1.Inverse();//r2 is the inverse rotation of r1

r2等於r1的逆，r1沒有被覆蓋；
另外三種方式：

v2 = r1.Inverse(v1);
inline Vector Inverse(const Vector& v) const;
w2 = r2.Inverse(w1);
inline Wrench Inverse(const Wrench& arg) const;
t2 = r3.Inverse(t1);
inline Twist Inverse(const Twist& arg) const;

v2相當於v2=r1.Inverse()∗v1$v2=r1.Inverse()\ast v1$ ，更有效率的一種寫法，矩陣的逆與向量相乘；
同理，w2=r2.Inverse()∗w1$w2=r2.Inverse()\ast w1$ ，t2=r3.Inverse()∗t1$t2=r3.Inverse()\ast t1$ 。
這裏的Wrench是6*1的力和力矩向量，那3*3的旋轉矩陣怎麼能和6*1的向量相乘呢？其實是將力和力矩分成兩個3*1的向量：f和t，在分別進行R1.Inverse()∗f$R1.Inverse()\ast f$ 和 R1.Inverse()∗t$R1.Inverse()\ast t$ 最後再拼成一個6*1向量返回；

4.構造旋轉矩陣

可以將兩個旋轉矩陣構造一個新旋轉矩陣，旋轉順序很重要：

r3=r1*r2;

構造圍繞X-Y-Z旋轉的旋轉矩陣：

r1.DoRotX(angle);
r2.DoRotY(angle);
r3.DoRotZ(angle);

如果執行r1.DoRotX(angle);$r1.DoRotX(angle);$ 相當於是將當前旋轉矩陣沿x軸旋轉角度angle，注意當前矩陣會變成旋轉後的矩陣，也相當於執行： r1*RotX(angle)；
另一種複雜的寫法是：

r1 = r1*Rotation::RotX(angle)

繞座標軸的旋轉矩陣：

r1 = RotX(angle)；
r2 = RotY(angle)；
r3 = RotZ(angle)；

r1返回沿x軸旋轉角度angle的旋轉矩陣：⎡⎣⎢1000cssn0−sncs⎤⎦⎥$\left[\begin{array}{ccc}1& 0& 0\\ 0& cs& -sn\\ 0& sn& cs\end{array}\right]$ ；
r2返回沿y軸旋轉角度angle的旋轉矩陣：⎡⎣⎢cs0−sn010sn0cs⎤⎦⎥$\left[\begin{array}{ccc}cs& 0& sn\\ 0& 1& 0\\ -sn& 0& cs\end{array}\right]$ ；
r3返回沿z軸旋轉角度angle的旋轉矩陣：⎡⎣⎢cssn0−sncs0001⎤⎦⎥$\left[\begin{array}{ccc}cs& -sn& 0\\ sn& cs& 0\\ 0& 0& 1\end{array}\right]$ ；

static Rotation Rot(const Vector& rotvec,double angle);
static Rotation Rot2(const Vector& rotvec,double angle);

Rot和Rot2函數都是沿任意向量旋轉角度angle後返回旋轉矩陣，區別是Rot不要求向量歸一化，而Rot2要求向量歸一化。在Rot中，如果向量的範數太小，則返回單位矩陣，首先會將向量歸一化然後調用Rot2。在Rot2中具體實現是：
設向量v=[vx vy vz]T$v=[v_x{v}_y{v}_z{]}^T$ ，ct=cos(angle),st=sin(angle)$c_t=cos(angle),s_t=sin(angle)$ ，則繞向量v旋轉角度angle的矩陣爲：

⎡⎣⎢ct+(1−ct)v2xvzst+(1−ct)vxvy−vyst+(1−ct)vxvz−vzst+(1−ct)vxvyct+(1−ct)v2yvxst+(1−ct)vyvzvyst+(1−ct)vxvz−vxst+(1−ct)vyvzct+(1−ct)v2z⎤⎦⎥$$\left[\begin{array}{ccc}{c}_t+(1-c_t)v_x^2& -v_z{s}_t+(1-c_t)v_x{v}_y& v_y{s}_t+(1-c_t)v_x{v}_z\\ v_z{s}_t+(1-c_t)v_x{v}_y& c_t+(1-c_t)v_y^2& -v_x{s}_t+(1-c_t)v_y{v}_z\\ -v_y{s}_t+(1-c_t)v_x{v}_z& v_x{s}_t+(1-c_t)v_y{v}_z& c_t+(1-c_t)v_z^2\end{array}\right]$$

5. 向量的旋轉

旋轉矩陣與向量相乘：

 v2=r1*v1;

6. 旋轉矩陣的比較

根據用戶定義或者默認精度逐個元素進行對比：

r1==r2;
r1!=r2;
Equal(r1,r2,eps);

參考文獻

[1] Introduction to Robotics Mechanics and Control, 機器人學導論（美）HLHN J.CRAIG著 貟超等譯；
[2] Robot Modeling and Control, 機器人建模和控制（美）馬克 W. 斯龐；