- KVKV-X
3D 3次元座標計算FUN
概略
3次元座標の計算に使用できるFUN・FBをまとめています。ロボット制御、位置補正、画像処理、搬送装置などで使用される距離計算、回転行列、オイラー角、クォータニオン、螺旋軌跡の生成などをKVシリーズ・KV-Xシリーズ向けに実装しています。
各FUNにはラダー例、計算式、グラフ表示を掲載しています。 そのままコピーして利用できるサンプルとして活用してください。
利用例
- 3次元空間の2点間距離を求める
- 3点を通る円の中心座標を求める
- X・Y・Z軸周りの回転計算を行う
- オイラー角による姿勢変換を行う
- クォータニオンによる回転を行う
- 螺旋軌跡を生成する
一覧
２点の距離
３点を通る円の中心
XYZ軸で回転
オイラー角で回転
クォータニオンで回転
螺旋
２点の距離 (Distance3D)
3次元空間上の2点間距離を求めます。
ロボットアームの移動量計算、画像処理による位置測定、レーザー変位計や3Dセンサの測定結果解析などに利用できます。
ワーク間の距離判定や、目標位置への到達確認にも使用できます。
KV
KV-X
入力
- pointA
- 点A [x,y,z]
- pointB
- 点B [x,y,z]
出力
- distance
- 2点間の距離
【例】
pointA [8, 2, 2]
pointB [2, 7, 9]
distance 10.488
【 計算式 】
\[
distance = \sqrt{(B_x-A_x)^2+(B_y-A_y)^2+(B_z-A_z)^2}
\]
計算式ではA[0]を\(A_x\)、A[1]を\(A_y\)、A[2]を\(A_z\)と記述しています
３点を通る円の中心 (GetCenter3D)
3点から円の中心座標を求めます。
パイプやシャフトなどの円弧形状の中心推定、位置補正、アライメント計算に利用できます。
画像処理で取得した3点から円中心を算出したり、ロボットの円弧補間の基準点生成にも使用できます。
KV
KV-X
入力
- pointA
- 点A [x,y,z]
- pointB
- 点B [x,y,z]
- pointC
- 点C [x,y,z]
出力
- point0
- 中心 [x,y,z]
【例】
pointA [0, 0, 2]
pointB [1, 0, 0]
pointC [0, 1, 2]
point0 [0.5, 0.5, 1.0]
【 計算式 】
\begin{align}
\overrightarrow{AB}=&
\begin{pmatrix}
B_x-A_x \\
B_y-A_y \\
B_z-A_z
\end{pmatrix} \\
\overrightarrow{AC}=&
\begin{pmatrix}
C_x-A_x \\
C_y-A_y \\
C_z-A_z
\end{pmatrix} \\
\overrightarrow{ABAC}=&\overrightarrow{AB}\times \overrightarrow{AC} \\
\end{align}
3元連立方程式を解きます
\[
\begin{bmatrix}
2AB_x & 2AB_y & 2AB_z \\
2AC_x & 2AC_y & 2AC_z \\
ABAC_x & ABAC_y & ABAC_z
\end{bmatrix}
\begin{bmatrix}
x \\
y \\
z
\end{bmatrix}
=
\begin{bmatrix}
B^2-A^2 \\
C^2-A^2 \\
ABAC\dot A
\end{bmatrix}
\]
\(\times \) は外積（クロス積）
X Y Z 軸で回転 (matRotateXYZ)
座標をX軸・Y軸・Z軸周りに回転させます。
センサ座標系と装置座標系の変換、ロボットの姿勢変更、治具の傾き補正などに利用できます。
3D CADやビジョンシステムとの座標合わせにも使用できます。
KV
KV-X
入力
- point
- 回転前の座標 [x,y,z]
- axis
- 回転軸 0=X 1=Y 2=Z
- angle
- 回転角度
出力
- out
- 移動後の座標 [x,y,z]
【例】
point [1.2, 0.8, 0.5]
axis 0(X軸)
angle 270.0
out [1.2, 0.5, -0.8]
【 計算式 】
X軸周り
\[
out=
\begin{bmatrix}
1 & 0 & 0 \\
0 & cos{\theta} & -sin{\theta} \\
0 & sin{\theta} & cos{\theta}
\end{bmatrix}
\begin{bmatrix}
x \\
y \\
z \\
\end{bmatrix}
\]
Y軸周り
\[
out=
\begin{bmatrix}
cos{\theta} & 0 & sin{\theta} \\
0 & 1 & 0 \\
-sin{\theta} & 0 & cos{\theta}
\end{bmatrix}
\begin{bmatrix}
x \\
y \\
z \\
\end{bmatrix}
\]
Z軸周り
\[
out=
\begin{bmatrix}
cos{\theta} & -sin{\theta} & 0 \\
sin{\theta} & cos{\theta} & 0 \\
0 & 0 & 1
\end{bmatrix}
\begin{bmatrix}
x \\
y \\
z \\
\end{bmatrix}
\]
オイラー角で回転 (matRotateEuler)
ロール・ピッチ・ヨーによる姿勢表現を使用して座標変換を行います。
産業用ロボット、6軸ステージ、3Dカメラなどで広く使用される回転表現です。
設備の傾き補正や、ロボットのツール姿勢計算に利用できます。
EulerToMatrix (オイラー角から行列に変換)
入力
- eulerAngle
- オイラー角 [roll, pitch, yaw]
- order
- 回転順
(0:XYZ, 1:XZY, 2:YXZ,
3:YZX, 4:ZXY, 5:ZYX)
出力
- out
- 回転行列
matRotateEuler (オイラー角で回転)
入力
- point
- 回転前の座標 [x,y,z]
- mat
- 回転行列
出力
- out
- 移動後の座標 [x,y,z]
【例】
eulerAngle [90, 60, 120]
order 0 (XYZ)
point [2, 1, 1]
out [-0.067, -2.12, 1.23]
【 計算式 】
\[
out=
\begin{bmatrix}
m00 & m01 & m02 \\
m10 & m11 & m12 \\
m20 & m21 & m22
\end{bmatrix}
\begin{bmatrix}
x \\
y \\
z \\
\end{bmatrix}
\]
クォータニオンで回転 (qtRotate)
任意軸周りの回転をクォータニオンで計算します。
オイラー角で発生するジンバルロックを回避できるため、高精度な3次元姿勢制御に適しています。
ロボット制御、AGV・AMRの姿勢推定、ドローン制御、3Dビジョンシステムなどで利用されています。
KV
KV-X
入力
- vector
- 回転軸の方向ベクトル [x,y,z]
- point
- 回転前の座標 [x,y,z]
- angle
- 回転角度
出力
- outPoint
- 移動後の座標 [x,y,z]
【例】
vector [-0.2, 1.0, 1.0]
point [1.2, 1.0, 0.5]
angle 270.0
out [0.227, -0.293, 1.598]
【 計算式 】
元のベクトル
\[
Q1=
\begin{bmatrix}
q_w \\
q_x \\
q_y \\
q_z \\
\end{bmatrix}
=
\begin{bmatrix}
0 \\
x \\
y \\
z \\
\end{bmatrix}
=
\begin{bmatrix}
0 \\
point.x \\
point.y \\
point.z \\
\end{bmatrix}
\]
回転軸（単位ベクトル）
\[
T=
\begin{bmatrix}
vector.x \\
vector.y \\
vector.z \\
\end{bmatrix}
\]
回転を表すクォータニオン
\[
R=
\begin{bmatrix}
q_w \\
q_x \\
q_y \\
q_z \\
\end{bmatrix}
=
\begin{bmatrix}
cos\frac{\theta}{2} \\
sin\frac{\theta}{2} T_x \\
sin\frac{\theta}{2} T_y \\
sin\frac{\theta}{2} T_z \\
\end{bmatrix}
\]
回転の計算
\[
out = R \times Q1 \times \bar{R}
\]
螺旋 (Spiral)
螺旋軌跡を生成します。
ねじ締め機、ディスペンサ、レーザー加工、研磨装置などの軌跡生成に利用できます。
一定ピッチのコイル形状や、外側へ広がるスパイラル軌跡を簡単に生成できます。
KV
KV-X
入力
- angle
- 回転角度
- dx
- 1周で増加するX軸方向の距離
- dz
- 1周で増加するZ軸方向の距離
- stype
- 種類
出力
- out
- 移動後の座標 [x,y,z]
【 計算式 】
\begin{align}
\theta = \frac{\pi}{180} \times Angle \\
r= \frac{dx}{360} \\
h= \frac{dz}{360}
\end{align}
コイル
\begin{align}
x= r \times \cos\theta \\
y= r \times \sin\theta \\
z= h \times Angle
\end{align}
等間隔
\begin{align}
x= r\theta \times cos\theta \\
y= r\theta \times sin\theta \\
z = h \times Angle
\end{align}
平方根
\begin{align}
x= r \sqrt{\theta} \times cos\theta \\
y= r \sqrt{\theta} \times sin\theta \\
z = h \times Angle
\end{align}
指数関数
\begin{align}
a= 1.1 \\
x= r \times a^{\theta} \times cos\theta \\
y= r \times a^{\theta} \times sin\theta \\
z = h \times Angle
\end{align}
ダウンロード
このサイトが役に立ったと感じたら、ご支援いただけると嬉しいです
