ここ数週間、ORB_SLAM3(単眼）を使い、自律制御で下記のような軌道で目標物を周回することを目指してきましたが、力尽きてしまいました。
結果としては、上記軌道では、制御不能でした。技術課題は、主に下記の２つです。
- 車を転回するとSLAMマップをロストしてしまう(30度くらいでもロストする）
- 転回すると推定された自己位置、姿勢が実体とかけ離れてしまう
原因は色々と推察できますが、どうにもうまくいきません。
車を旋回させるとSLAMマップをロストしてしまい、運よくロストしなくても、推定された姿勢、自己位置が実体と違うので、車があさっての方向に進んでしまいます。
車両精度の問題もありましたが、上記２つが致命的であるため、残念ですがGiveUPしました。
補足しますと、目標物にたどり着くだけの直線的な動きであれば、問題はありません。
ですが、今回のように旋回するような軌道は達成不可能と判断しました。
失敗ですが、せっかくなので、長い時間をかけて色々と挑戦したので、備忘録を残しておこうと思います。
■ 自律制御方式
私が採用した方式をざっくりと書き留めます。
今回メインで使用するのは、Yolo v5とORB_SLAM3です。
まず、撮影画像を使い、Yolo v5で撮影画像上の目標物の位置を求めます。
次に、平面上の対象物の位置と焦点距離から、対象物のPointCloud検索に必要な画角を求めます。
求めた画角内に存在する目標物のPointCloudのみを集め、目標物の座標を求めます。（下図参照）
求めた目標物の位置と自己位置より、軌道を算出します。
今回は、目標物周辺の周回軌道を円軌道として精度は６分割としました。
理由は、あまり角度やスピードがきついとマップをロストしやすくなるのと、車の精度上、これ以上細かく制御できないと考えたからです。
周回軌道は、円周上の座標を求めるだけですので角度で精度を決めることが可能です。
後は、各通過点を通るようにPiCarを以下のように繰り返し制御すれば、スラロームできるはずです。（ここで、マップをロストしたり、自己位置、姿勢が大きくずれなければ、成功するはずだった？）
- 現在位置と目標となる軌道の点、PiCarのYawから、PiCarを進むべき方向に転回します。
- PiCarが目標物に向いたところで、目標となる軌道の点まで直進します。
コードを全文掲載しようともおもったのですが、少々デカいので主要な部分のみ掲載します。
〇 目標物の画角を求める
Yolo V5でターゲット座標は、画像幅、高さを1.0とした論理座標を返します。
下記メソッドは、入力画像正面を0度としてどの方向にターゲットがあるのか画角を求めるメソッド：landmark_degree()です。
画角は、焦点距離、、センサー中心点から求まります。単純なものですが、思いつくまでに結構時間がかかりました。
# here's the f and center. this comes from TUM1.
cam_fx_ = 733.1180416702534 # 焦点距離
cam_fy_ = 736.6720146569185
cam_cx_ = 532.0877752312018 # センサー中心点
cam_cy_ = 359.8242822174343
#
# purpose : entry to get degree of area.
# returns : the dgrees of corners.
#
def landmark_degree(self, sx, sy, ex, ey):
sx = sx * cam_cx_ * 2
ex = ex * cam_cx_ * 2
sy = sy * cam_cy_ * 2
ey = ey * cam_cy_ * 2
dex = (cam_cx_ - sx) / cam_cx_ * self.fovx_ * 3 / 4 # yaw方向を少し中心側に設定
dsx = (cam_cx_ - ex) / cam_cx_ * self.fovx_ * 3 / 4 # 同上
dey = (cam_cy_ - sy) / cam_cy_ * self.fovy_ * 3 / 4 # pitch方向を少し中心側に設定
dsy = (cam_cy_ - ey) / cam_cy_ * self.fovy_ * 3 / 4 # 同上
return dsx, dsy, dex, dey
下記のようにORB_SLAM3のPointCloud取得コールバックで、landmark_degree()を使い、下記のようにターゲット座標を（ざっくりと）求めます。
黄色の部分は、landmark_degree()で画像上のターゲット画角をもとめ、picarの姿勢（pitch、yaw)を加算して、抽出するPointCloudのSLAMマップ上の角度を求めています。
青色の部分は、黄色でもとめた画角内に収まるPointCloudをかき集めています。
最終的には、かき集めた座標の平均値をターゲット中心点とします。
#
# purpose : entry to get point cloud to get landmark.
#
def pcd_callback(self, msg):
#
# in this case, the coordinantes has already got.
#
if self.landmark_cor_ == True:
return
if self.landmark_ == True:
# this calculates the degree of object.
dsx, dsy, dex, dey = self.landmark_degree(
self.landmark_object_.sx, self.landmark_object_.sy,
self.landmark_object_.ex, self.landmark_object_.ey)
dsx = dsx + self.yaw_
dex = dex + self.yaw_
dsy = dsy + self.pitch_
dey = dey + self.pitch_
sumx = sumy = sumz = 0
near_x = near_z = 0
near_y = near_distance = 10.0
num_sample = 0
for point in point_cloud2.read_points(msg, skip_nans=True):
y = point[0]
z = point[1]
x = point[2]
if x != 0 or y != 0 or z != 0:
z = z - self.z_;
x = x - self.x_;
y = y - self.y_;
yaw = math.atan2(x, y)
if yaw >= dsx and yaw <= dex:
d = math.sqrt(x * x + y * y)
pitch = math.atan2(z, d)
if pitch >= dsy and pitch <= dey:
d = math.sqrt(x * x + y * y)
if d > car_width_ and d < near_distance:
near_distance = d
near_x = x
near_y = y
near_z = z
sumx = sumx + x
sumy = sumy + y
sumz = sumz + z
num_sample = num_sample + 1
if num_sample > 0:
self.landmark_x_ = sumx / num_sample + self.x_
self.landmark_y_ = sumy / num_sample + self.y_
self.landmark_z_ = sumz / num_sample + self.z_
self.landmark_cor_ = True
else:
# in this case, this retries to get the object again.
self.get_logger().info("can't get the point.")
self.landmark_ = False
self.landmark_cor_ = False
〇 軌道を求める
今回は１つのターゲットに対する軌道を求めるだけのものです。
ターゲット中心点をベースに周回軌道を計算するだけのものですが、参考程度に掲載します。
landmark、車体幅をベースに回転半径を決め、6角形となるように移動位置を三角関数で求めています。
# here's the size of landmark
landmark_dia_ = 0.40 # 0.5(m). # landmarkの半径
# here's the car width
car_width_ = 0.30 # 0.3(m). # 車体幅
#
# purpose : entry to generate orbital.
# reutrns : number of generated point.
#
def orbital_generate(self):
space = (landmark_dia_ + car_width_) / 2
sx = self.x_
sy = self.y_
dx = self.landmark_x_ - sx
dy = self.landmark_y_ - sy
theta = math.atan2(dx, dy) # x y has been swapped
dist = math.sqrt(dx * dx + dy * dy) - space
rad = math.pi
self.ckpoint_x_[0] = self.landmark_x_ + space*math.sin(theta + rad)
self.ckpoint_y_[0] = self.landmark_y_ + space*math.cos(theta + rad)
rad = math.pi + math.pi / 3
self.ckpoint_x_[1] = self.landmark_x_ + space*math.sin(theta + rad)
self.ckpoint_y_[1] = self.landmark_y_ + space*math.cos(theta + rad)
rad = rad + math.pi / 3
self.ckpoint_x_[2] = self.landmark_x_ + space*math.sin(theta + rad)
self.ckpoint_y_[2] = self.landmark_y_ + space*math.cos(theta + rad)
rad = rad + math.pi / 3
self.ckpoint_x_[3] = self.landmark_x_ + space*math.sin(theta + rad)
self.ckpoint_y_[3] = self.landmark_y_ + space*math.cos(theta + rad)
rad = rad + math.pi / 3
self.ckpoint_x_[4] = self.landmark_x_ + space*math.sin(theta + rad)
self.ckpoint_y_[4] = self.landmark_y_ + space*math.cos(theta + rad)
rad = rad + math.pi / 3
self.ckpoint_x_[5] = self.landmark_x_ + space*math.sin(theta + rad)
self.ckpoint_y_[5] = self.landmark_y_ + space*math.cos(theta + rad)
self.ckpoint_x_[6] = sx
self.ckpoint_y_[6] = sy
return 7
■マップロスト原因（推察）
旋回時にSLAMマップをロストしてしまったり、向きを変えた際に自己位置を見失ってしまう現象が発生しました。あくまで私の推察でしかないのですが、主な原因はカメラのシャッター方式とシャッタースピードではないかと考えます。
ORB_SLAM3単眼性能については、回転しない場合（前後する程度）であれば正確に位置はとれていましたので、主要な問題ではないと考えます。
今回、懐事情で安いカメラを使用したので、シャッター方式がグローバルシャッターではなく、ローリングシャッター方式となっています。ローリンシャッター方式は、下図のように画像の一部を順番に撮影する方式です。
この方式の欠点は、旋回中には「こんにゃく画像」や「ぶれ画像」が発生しやすくなります。実際の旋回中の画像は、下記のようになっています。このような画像では特徴点の見つけたり、マッチングさせることはほぼ不可能と思われます。この画像を見て、今の環境では無理と判断した次第です。
■ 次回
次回以降は、狩猟シーズンが近づいてきたこともあり、機材の改良、開発を行ってみようとおもいます。少し私的にバタバタしていることもあり、間があいてしまうかもしれませんが、まとまったところで投稿しようと思います。
