レーザーシステムのレーザーの射出角度の設定方法の備忘録

ソフトウェアとハードウェアでカメラに写ったターゲットを自動追尾するレーザーシステムを作るので

ハードウェア側でレーザーで、下の2点を考えてレーザーの射出角度の設定方法を2つ考えた。

・なるべく正確にターゲットに照射する
・設定にコストがかからないようにする

解説を入れるとややこししいので図とコードで設定方法をざっとまとめいくただの個人的な備忘録。

目次
共通パラメーター
方法1. 視差(disparity)を使った4stepの設定方法
方法2. レーザーの射出角度とカメラのピクセル範囲をマッチングするように調整する3stepの設定方法(視差なし)

共通パラメーター

・画像サイズ：Height(縦)=720[pixel] Width(横)=1080[pixel]
・レーザー照射範囲(物体稼働領域) : 120[cm]
・物体までの距離(distance):130[cm]
・レーザー照射角度範囲のminとmax(θ):0〜180

方法1. 視差(disparity)を使った4stepの設定方法

Step
1.カメラとレーザーの位置はなるべく揃える
2.レーザー射出範囲の距離(cm)とそのピクセル数を図る
3.物体までの距離(dist)を計測する
4.公式から照射角度(ε)を求める

disparityから物体までの距離(z)を求めて、座標からレーザーの角度を求める

1.カメラとレーザーの位置はなるべく揃える

カメラとレーザー光がほぼ水平になるようにできる限り近づける。

2.レーザー射出範囲の距離(cm)とそのピクセル数を図る

レーザーで照射したい範囲の実寸とそのpixel数を図る。単位はcmが一般的で扱いやすいのでおすすめ。

3.物体までの距離(dist)を計測する

disparityを計算したら、カメラの画像素子と焦点距離を使った公式から物体までの距離を計算。

def distance_formula(disparity):
    T=2.6
    f = 0.315
    img_element = 0.00028
    K = int(T*f/img_element) #2925
    return K/disparity

4.公式から照射角度(ε)を求める
物体の移動距離(pixel)の単位をcmに直して、物体までの距離(z)からアークジェントを使って求める。

ここでは移動距離は5[pixel]とした。

def pixel2cm(pix):
    object_size = 120
    pixel = 900
    cm = pix * (object_size/pixel)
    return cm *100

	
def calcurate_Xangle(pix, dist):
    x = pixel2cm(pix)
    sita = np.arctan(x/dist)
    return sita
 

dist = disranse_formula(disparity)
#dist= 130
sita = calcurate_Xangle(pix=5, dist=130)
# sita(angle) is 0.47

コード全体

import os
import numpy as np

def pixel2cm(pix):
    object_size = 120
    pixel = 900
    cm = pix * (object_size/pixel)
    return cm *100

# 3
def distance_formula(disparity):
    T=2.6
    f = 0.315
    img_element = 0.00028
    K = int(T*f/img_element) #2925
    return K/disparity
	
# 4
def calcurate_Xangle(pix, dist):
    x = pixel2cm(pix)
    sita = np.arctan(x/dist)
    return sita
	

dist = disranse_formula(disparity)
#dist= 130
sita = calcurate_Xangle(pix=5, dist=130)
# sita(angle) is 0.47
# this is the angle for laser

方法2. レーザーの射出角度とカメラのピクセル範囲をマッチングするように調整する3stepの設定方法(視差なし)

備考
・カメラとレーザーの位置を揃えなくていい
・レーザー照射範囲の長さを計測しなくていい

Step
1.照射範囲に対してレーザーの照射角度(θ)を設定する
2.レーザーの照射角度と画像内のpixel範囲(ε)を合わせる
3.物体の位置座標と公式から照射範囲内のレーザー照射角度(ε =θ)を求める

1.照射範囲に対してレーザーの照射角度(θ)を設定する

pan_tilt.pyとかを使ってレーザーを実際に動かして照射範囲内に必要な角度を設定する。

2.レーザーの照射角度と画像内のpixel範囲(ε)を合わせる
1で設定した角度に画像内のpixel範囲を合わせる。

ちなみにpixelを角度に変換する公式は下のやつ。

class LaserAngleRange(Enum):
	XMin=80
	XMax=130
	YMin=70
	YMax=120
class AngleFormula(Enum):
    ServoMax = 180
    WidthMax = 1080
    HightMax = 720
	
def Xangle2duty(Xcoordinate):
    deg = Xcoordinate / (WidthMax/ServoMax)
    return deg

3.物体の位置座標と公式から照射範囲内のレーザー照射角度(ε =θ)を求める

レーザーの設定角度と画像内の角度が同じになるように、レーザーの設定角度範囲内で画像角度を求める。

def restricted_formula(degX):
	if degX>XMin or degX<XMax:
		#Yduty = int(float(degY)*2,17 + 102)
		Xdeg = degX
	else:
		pass
    return Xdeg
	
degX = Xangle2duty(Xcoord=700)
Xdeg = restricted_formula(degX)
# Xdeg is 116.66°

コード全体

class LaserAngleRange(Enum):
    XMin=80
    XMax=130
    YMin=70
    YMax=120
class AngleFormula(Enum):
    ServoMax = 180
    WidthMax = 1080
    HightMax = 720
	
def Xangle2duty(Xcoordinate):
    deg = Xcoordinate / (WidthMax/ServoMax)
    return deg

def restricted_formula(degX):
	if degX>XMin or degX<XMax:
		#Yduty = int(float(degY)*2,17 + 102)
		Xdeg = degX
	else:
		pass
    return Xdeg
	
degX = Xangle2duty(Xcoord=700)
Xdeg = restricted_formula(degX)
# Xdeg is 116.66° . This is laser angle