關鍵概念
T_{0,n+1}(\theta)=T_{01}(\theta_1)\ldots T_{n-1,n}(\theta_n)T_{n,n+1}
這裏 \theta_i 表示關節 i 變量， T_{n,n+1} 表明末端座標系在座標系{n}中的構型。如果末端座標系{b}選擇與座標系{n}重合，我們可以省掉座標系{n+1}.
T(\theta)=e^{[\mathcal S_1]\theta_1 }\ldots e^{[\mathcal S_n]\theta_n}M,
這裏 \mathcal S_i=(\omega_i,v_i) 是沿關節 i 正向運動的螺旋軸在固定系座標{s}中表達， \theta_i 是關節 i 的變量， M\in SE(3) 表示機器人末端座標系{b}的零位(初始位置)。這不需要分別定義每個連桿座標系，只需要定義 M 和螺旋軸 \mathcal S_1,\dots, \mathcal S_n 。
T(\theta)=Me^{[\mathcal B_1] \theta_1}\ldots e^{[\mathcal B_2]\theta_n},
這裏 \mathcal B_i=[Ad_{M^{-1}}]S_i, i=1, \dots, n; \mathcal B_i=(\omega_i,v_i) 是關節 i 的螺旋軸在初始狀態在座標系{b}中的表達。
指數積公式的演算法實作
該部份主要包括兩個公式的實作，
實作程式碼用到了上一篇部落格的剛體運動演算法，程式碼較長，就不放在這裏了。下面只列出兩個公式的實作程式碼。程式碼不斷更新叠代，完整的最新源碼放在github--連結
/**
* @brief Description: Algorithm module of robotics, according to the
* book[modern robotics : mechanics, planning, and control].
* @file: RobotAlgorithmModule.c
* @author: LiBing
* @date: 2019/03/01 12:23
* Copyright(c) 2019 LiBing. All rights reserved.
* https://blog.csdn.net/libing403
* Contact [email protected]
* @note:
* @warning:
*/
#include
"RobotAlgorithmModule.h"
#include
<math.h>
#include
<string.h>
#include
<stdlib.h>
#include
<stdio.h>
/**
*@brief Description: Computes the end-effector frame given the zero position of the end-effector M,
* the list of joint screws Slist expressed in the fixed-space frame, and the list of joint values thetalist.
*@param[in] M the zero position of the end-effector expressed in the fixed-space frame.
*@param[in] Joint Num the number of joints.
*@param[in] Slist the list of joint screws Slist expressed in the fixed-space frame.
* in the format of a matrix with the screw axes as the column.
*@param[in] thetalist the list of joint values.
*@param[out] T the end-effector frame expressed in the fixed-space frame.
*@return No return value.
*@note: when Slist is a matrix ,make sure that columns number of Slist is equal to JointNum,
* rows number of Slist 6 .The function call should be written as
* FKinBody(...,(double *)Slist,...).
*@waring:
*/
void
FKinSpace
(
double
M
[
4
][
4
],
int
JointNum
,
double
*
Slist
,
double
*
thetalist
,
double
T
[
4
][
4
])
{
int
i
,
j
,
k
,
l
;
double
se3mat
[
4
][
4
];
double
T2
[
4
][
4
];
double
exp6
[
4
][
4
];
double
V
[
6
];
Matrix4Equal
(
M
,
T
);
for
(
i
=
JointNum
-
1
;
i
>=
0
;
i
--
)
{
for
(
l
=
0
;
l
<
6
;
l
++
)
{
V
[
l
]
=
Slist
[
l
*
JointNum
+
i
];
//get each column of Slist.
}
VecTose3
(
V
,
se3mat
);
for
(
j
=
0
;
j
<
4
;
j
++
)
{
for
(
k
=
0
;
k
<
4
;
k
++
)
{
se3mat
[
j
][
k
]
=
se3mat
[
j
][
k
]
*
thetalist
[
i
];
}
}
MatrixExp6
(
se3mat
,
exp6
);
Matrix4Mult
(
exp6
,
T
,
T2
);
Matrix4Equal
(
T2
,
T
);
}
return
;
}
/**
*@brief Description:Computes the end-effector frame given the zero position of the end-effector M,
* the list of joint screws Blist expressed in the end-effector frame, and the list of joint values thetalist.
*@param[in] M the zero position of the end-effector expressed in the end-effector frame.
*@param[in] JointNum the number of joints.
*@param[in] Blist the list of joint screws Slist expressed in the end-effector frame.
* in the format of a matrix with the screw axes as the column.
*@param[in] thetalist the list of joint values.
*@param[out] T the end-effector frame expressed in the end-effector frame.
*@return No return value.
*@note: when Blist is a matrix ,make sure that columns number of Slist is equal to JointNum,
* rows number of Slist 6 .The function call should be written as
* FKinBody(...,(double *)Blist,...).
*@waring:
*/
void
FKinBody
(
double
M
[
4
][
4
],
int
JointNum
,
double
*
Blist
,
double
thetalist
[],
double
T
[
4
][
4
])
{
int
i
,
j
,
k
,
l
;
double
se3mat
[
4
][
4
];
double
T2
[
4
][
4
];
double
exp6
[
4
][
4
];
double
V
[
6
];
Matrix4Equal
(
M
,
T
);
for
(
i
=
0
;
i
<
JointNum
;
i
++
)
{
for
(
l
=
0
;
l
<
6
;
l
++
)
{
V
[
l
]
=
Blist
[
l
*
JointNum
+
i
];
//get each column of Slist.
}
VecTose3
(
V
,
se3mat
);
for
(
j
=
0
;
j
<
4
;
j
++
)
{
for
(
k
=
0
;
k
<
4
;
k
++
)
{
se3mat
[
j
][
k
]
=
se3mat
[
j
][
k
]
*
thetalist
[
i
];
}
}
MatrixExp6
(
se3mat
,
exp6
);
Matrix4Mult
(
T
,
exp6
,
T2
);
Matrix4Equal
(
T2
,
T
);
}
return
;
}
參考文獻
Kenvin M. Lynch , Frank C. Park, Modern Robotics Mechanics,Planning , and Control. May 3, 2017
