KVKV-X

2D 2次元座標計算FUN

概略

KEYENCE KV-X・KVシリーズで利用できる2次元座標計算用のファンクションブロック(FB)を紹介します。
距離計算、角度計算、直線の交点計算、座標回転、円の中心座標計算など、画像処理や位置決め、ロボット制御で利用できる幾何演算を簡単に実装できます。

利用例

画像センサで検出した2点間距離の計算
2本のエッジ線から交点を求める
ワーク位置の座標変換
ロボットハンドの回転補正
3点測定による円中心検出

一覧

２点の距離

２直線の交点と角度

原点を中心に回転

３点を通る円の中心

２点間の距離と角度 (LinearPtoP)

KV-X

入力

pointA: 点1 [x,y]
pointB: 点2 [x,y]

出力

distance: 距離
angle: 角度

【例】
pointA = [-6.0, -3.0]
pointB = [5.0, 4.0]

distance = 13.038
angle = 32.471

【計算式】

\[ \begin{aligned} distance &= \sqrt{(p2_x-p1_x)^{2}+(p2_y-p1_y)^{2}} \\ angle &= atan2(p2_y-p1_y, p2_x-p1_x) \end{aligned} \] 計算式ではp1[0]を\(p1_x\)、p1[1]を\(p1_y\)と記述しています

２直線の交点と角度 (Intersection)

KV-X

入力

Line1: 線1 [x1, y1, x2, y2]
Line2: 線2 [x1, y1, x2, y2]

出力

point: 交点 [x, y]
angle: 角度

【例】
Line1 始点X [0,0] = -10.0
Line1 始点Y [0,1] = -7.0
Line1 終点X [1,0] = 10.0
Line1 終点X [1,1] = 3.0
Line2 始点X [0,0] = -10.0
Line2 始点Y [0,1] = 13.4
Line2 終点X [1,0] = 10.0
Line2 終点X [1,1] = -10.6

point [2, -1]
angle = -76.759

【計算式】

L1を一次関数(ax+b)に変換 \[ \begin{align} P1=&(L1[0,0],L1[0,1]) \\ P2=&(L1[1,0],L1[1,1]) \\ a1=&\frac{(P2_y-P1_y)}{(P2_x-P1_x)} \\ b1=&\frac{(P2_x*P1_y-P1_x*P2_y)}{(P2_x-P1_x)} \end{align} \] L2も同様に一次関数(ax+b)に変換して\(a2,b2\)を算出

L1,L2から \[ \begin{align} point_x = &\frac{(b2-b1)}{(a1-a2)} \\ point_y = &a1*point_x+b1 \\ angle=&tan^{-1}\left(\frac{a2-a1}{1+a1*a2} \right) \end{align} \]

原点を中心に回転 (PointRotate)

KV-X

入力

point: 点 [x,y]
angle: 回転角度

出力

out_point: 移動後の座標

【例】
point [4, 4]
angle = 90

out_point [-4, 4]

【計算式】

\[ \begin{align} \theta =& angle\frac{\pi}{180} \\ outp_x =& p_x*cos\theta-p_y*sin\theta \\ outp_y =& p_x*sin\theta+p_y*cos\theta \\ \end{align} \] 計算式ではp[0]を\(p_x\)、p[1]を\(p_y\)と記述しています

３点を通る円の中心 (GetCenter2D)

KV-X

入力

pointA: 点A [x,y]
pointB: 点B [x,y]
pointC: 点C [x,y]

出力

point0: 中心 [x,y]
radius: 半径

【例】
pointA [-4, 2]
pointB [3, -1]
pointC [1, 6]

point0 [0.128, 1.965]
radius 4.128

【計算式】

\[ \begin{align} a =& \begin{bmatrix} A_x & A_y & 1 \\ B_x & B_y & 1 \\ C_x & C_y & 1 \end{bmatrix} \\ bx =& \begin{bmatrix} A_x^{2}+A_y^{2} & A_y & 1 \\ B_x^{2}+B_y^{2} & B_y & 1 \\ C_x^{2}+C_y^{2} & C_y & 1 \end{bmatrix} \\ by =& \begin{bmatrix} A_x^{2}+A_y^{2} & A_x & 1 \\ B_x^{2}+B_y^{2} & B_x & 1 \\ C_x^{2}+C_y^{2} & C_x & 1 \end{bmatrix} \\ p0_x=&-\left(\frac{bx}{2a}\right) \\ p0_y=&-\left(\frac{by}{2a}\right) \\ r=&\sqrt{(A_x-p0_x)^2+(A_y-p0_y)^2} \end{align} \] 計算式ではA[0]を\(A_x\)、A[1]を\(A_y\)と記述しています