zenloger/autopilot
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix: reduce error

Browse Source
This commit is contained in:
russian_proger
2025-10-06 17:14:22 +03:00
parent 15a013d85e
commit 520a333812
6 changed files with 177 additions and 68 deletions
Download Patch File Download Diff File Expand all files Collapse all files

58
autopilot.py
View File

@@ -27,8 +27,6 @@ class PilotCommand:
self.stop = stop
self.velocity = velocity
MOVE_KOF: float = 1 / 1.2
class AutoPilot(Pilot):
# Состояние одометрии
@@ -194,7 +192,7 @@ class AutoPilot(Pilot):
a21, a22 = H[1, 0], H[1, 1]
# Смещение (уже отцентрировано)
tx, ty = -H[0, 2] * MOVE_KOF, -H[1, 2] * MOVE_KOF
tx, ty = -H[0, 2], -H[1, 2]
print(" [Pilot] translate:", tx, ty)
@@ -260,6 +258,50 @@ class AutoPilot(Pilot):
'angle_degrees': math.degrees(self.angle),
'frame_count': self.frame_count
}
def get_position_by_chunk(self) -> tuple[float, float, float] | None:
# Пытаемся найти ориентир на картинке:
current_image = self.prev_image
landmark_image = cv2.imread(Path('chunks') / f'chunk_{self.target_idx}.png', cv2.IMREAD_COLOR_RGB)
src_pts, dst_pts, matches, kp1, kp2 = self.detect_and_match_keypoints(current_image, landmark_image)
if src_pts is not None and dst_pts is not None and self.vis_manager:
self.vis_manager.update_chunk_matches(current_image, landmark_image, kp1, kp2, matches)
if src_pts is not None and dst_pts is not None:
# Оцениваем матрицу трансформации
landmark_transform = self.estimate_transformation_matrix(src_pts, dst_pts)
# Если ориентир достоверно найден — скорректируем глобальные координаты и угол
if landmark_transform is not None:
ok_scale = (self.min_scale <= landmark_transform['scale'] <= self.max_scale)
ok_inliers = (landmark_transform.get('inliers', 0) >= self.min_inliers)
ratio = landmark_transform.get('inliers_ratio', 0.0)
ok_ratio = (ratio >= self.min_inlier_ratio)
rmse = landmark_transform.get('rmse', None)
ok_rmse = (rmse is not None and rmse <= self.max_reproj_rmse)
if ok_scale and ok_inliers and ok_ratio and ok_rmse:
print("[PILOT]", rmse, ratio, ok_rmse)
# if False:
# Считаем абсолютную позу относительно координат ориентира
landmark_world_x, landmark_world_y = self.points[self.target_idx]
tx, ty = landmark_transform['translation']
# Смещение в системе БПЛА: (dx_meters, dy_meters)
dx_meters = ty
dy_meters = tx
# Используем оценённый абсолютный угол из гомографии относительно карты (север-вверх)
absolute_angle = landmark_transform['rotation']
cos_a = math.cos(absolute_angle)
sin_a = math.sin(absolute_angle)
# Переход в глобальные координаты карты
dx_global = dx_meters * cos_a - dy_meters * sin_a
dy_global = dx_meters * sin_a + dy_meters * cos_a
x = landmark_world_x + dx_global
y = landmark_world_y + dy_global
angle = math.atan2(math.sin(absolute_angle), math.cos(absolute_angle))
print(f" [Pilot] Landmark correction applied (inliers={landmark_transform['inliers']}, ratio={ratio:.2f}, rmse={rmse:.2f})")
return (x, y, angle)
return None
def handle(self, image: Image) -> PilotCommand:
self.frame_count += 1
@@ -330,11 +372,11 @@ class AutoPilot(Pilot):
grid_step=85)
# Пытаемся найти ориентир на картинке:
# landmark_image = cv2.imread(Path('chunks') / f'chunk_{self.target_idx}.png', cv2.IMREAD_COLOR_RGB)
# src_pts, dst_pts, matches, kp1, kp2 = self.detect_and_match_keypoints(current_image, landmark_image)
landmark_image = cv2.imread(Path('chunks') / f'chunk_{self.target_idx}.png', cv2.IMREAD_COLOR_RGB)
src_pts, dst_pts, matches, kp1, kp2 = self.detect_and_match_keypoints(current_image, landmark_image)
# if src_pts is not None and dst_pts is not None and self.vis_manager:
# self.vis_manager.update_chunk_matches(current_image, landmark_image, kp1, kp2, matches)
if src_pts is not None and dst_pts is not None and self.vis_manager:
self.vis_manager.update_chunk_matches(current_image, landmark_image, kp1, kp2, matches)
# if src_pts is not None and dst_pts is not None:
# # Оцениваем матрицу трансформации
@@ -391,7 +433,7 @@ class AutoPilot(Pilot):
angle_diff -= 2 * math.pi
self.prev_image = current_image
return PilotCommand(max(min(0.1, angle_diff), -0.1), min(50., distance_to_target / 2))
return PilotCommand(max(min(0.6, angle_diff), -0.6), min(50., distance_to_target / 2))
def reset_position(self, x: float = 0.0, y: float = 0.0, angle: float = 0.0):
"""Сбрасывает позицию и угол БПЛА"""

33
main.py
View File

@@ -13,7 +13,7 @@ import autopilot
def make_global_photo(filename):
yandexMap = YandexMap()
yandexMap.save_photo()
yandexMap.save_photo(filename)
yandexMap.destroy()
def get_trajectory_points(bg_img: str) -> list[(float, float)]:
@@ -28,28 +28,28 @@ def main():
# make_global_photo('map.jpg')
# Получаем траекторию от пользователя
# points = get_trajectory_points('map.jpg')
# print(points)
points = get_trajectory_points('map.jpg')
print(points)
# points = [np.float64(0.5443937502226799), np.float64(0.4030424838774785)], [np.float64(0.18517133490120316), np.float64(0.4586935604608052)], [np.float64(0.1641887171838272), np.float64(0.5586383510594329)], [np.float64(0.587198290366127),
# np.float64(0.5699957136274587)]
points = [[np.float64(0.481445897070552), np.float64(0.49958006570569835)], [np.float64(0.5485902737661552), np.float64(0.4325716265543457)], [np.float64(0.5376793125531197), np.float64(0.6052035375883389)], [np.float64(0.34044270600978543), np.float64(0.6052035375883389)], [np.float64(0.34044270600978543), np.float64(0.3689703961734008)], [np.float64(0.6333600493443542), np.float64(0.3224052096444948)], [np.float64(0.6686108471095458), np.float64(0.5518239335186174)]]
# points = [[np.float64(0.4931961629922826), np.float64(0.4439289891223716)], [np.float64(0.5099822571661834), np.float64(0.44165751660876645)], [np.float64(0.5083036477487933), np.float64(0.4564220879472001)], [np.float64(0.525089741922694), np.float64(0.4586935604608052)]]
# Для каждой точки сделаем приближенный снимок
yandexMap = YandexMap()
keypoints: list[(any, any)] = []
# plt.ion()
# for i in range(len(points)):
# point = points[i]
# yandexMap.scroll(point[0], point[1], 5, True)
# sleep(1)
# img = yandexMap.make_screenshot(point[0], point[1], 0.2, 0.2)
# Path('chunks').mkdir(exist_ok=True)
# cv2.imwrite(Path('.') / 'chunks' / f'chunk_{i}.png', img)
# plt.subplot(1, len(points), i+1)
# plt.imshow(img)
# plt.pause(0.25)
# yandexMap.scroll(point[0], point[1], 5, False)
plt.ion()
for i in range(len(points)):
point = points[i]
yandexMap.scroll(point[0], point[1], 5, True)
sleep(1)
img = yandexMap.make_screenshot(point[0], point[1], 0.2, 0.2)
Path('chunks').mkdir(exist_ok=True)
cv2.imwrite(Path('.') / 'chunks' / f'chunk_{i}.png', img)
plt.subplot(1, len(points), i+1)
plt.imshow(img)
plt.pause(0.25)
yandexMap.scroll(point[0], point[1], 5, False)
plt.tight_layout()
@@ -87,6 +87,7 @@ def main():
vis_manager = VisualizationManager()
width, height = yandexMap.get_size()
print(width, height)
points_coords = np.array(list(map(lambda p: [
(p[0] - points[0][0]) * width, (points[0][1] - p[1]) * height
], points)))

BIN
map.jpg
View File

Binary file not shown.
Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 1.8 MiB

After

Width:  |  Height:  |  Size: 3.4 MiB

136
test_autopilot.ipynb
View File

@@ -2,7 +2,7 @@
"cells": [
{
"cell_type": "code",
"execution_count": 2,
"execution_count": 1,
"metadata": {},
"outputs": [
{
@@ -27,29 +27,27 @@
},
{
"cell_type": "code",
"execution_count": 3,
"execution_count": 2,
"metadata": {},
"outputs": [
{
"data": {
"text/plain": [
"<autopilot.PilotCommand at 0x2696e2decd0>"
]
},
"execution_count": 3,
"metadata": {},
"output_type": "execute_result"
}
],
"outputs": [],
"source": [
"autopilot = AutoPilot([(0, 0), (1500, 1000)], [], vzm)\n",
"points = [[np.float64(0.48700765111972333), np.float64(0.49793590752304234)], [np.float64(0.4983137576734965), np.float64(0.5021469113219258)], [np.float64(0.4966178416904305), np.float64(0.5189909265174597)], [np.float64(0.4813545978428368), np.float64(0.5105689189196927)]]\n",
"width, height = 1920, 1031\n",
"points_coords = np.array(list(map(lambda p: [\n",
" (p[0] - points[0][0]) * width, (points[0][1] - p[1]) * height\n",
" ], points)))\n",
"points_coords *= 2 ** 4\n",
"\n",
"vzm.set_target_points(points_coords)\n",
"autopilot = AutoPilot(points_coords, [], vzm)\n",
"imgs = [Image.open(Path('images') / f'photo_{i}.png') for i in range(10)]\n",
"autopilot.handle(imgs[0])\n"
"autopilot.handle(imgs[0])\n",
"vzm.update_drone_trajectory(autopilot.x, autopilot.y)\n"
]
},
{
"cell_type": "code",
"execution_count": 4,
"execution_count": 3,
"metadata": {},
"outputs": [
{
@@ -57,38 +55,104 @@
"output_type": "stream",
"text": [
"\n",
" [Pilot] translate: -0.28939364297583925 8.879763334079307\n",
" [Pilot] Drone Position: (8.87, 0.58)\n",
" [Pilot] Angle: 5.6°\n",
" [Pilot] translate: -1.5380446518826545 49.21369409309772\n",
" [Pilot] Drone Position: (47.97, -11.12)\n",
" [Pilot] Angle: -11.3°\n",
" [Pilot] Target Index: 0\n",
" [Pilot] Target Position: (0, 0)\n",
" [Pilot] Target Position: [0. 0.]\n",
" [Pilot] Distance: 0.0\n",
"0.1 50.0\n",
"0.004129499336505804 50.0\n",
"\n",
" [Pilot] translate: -4.194654791266826 51.7432078133843\n",
" [Pilot] Drone Position: (60.43, 6.60)\n",
" [Pilot] Angle: 11.3°\n",
" [Pilot] translate: -0.773373251191613 49.85522586610471\n",
" [Pilot] Drone Position: (96.75, -21.44)\n",
" [Pilot] Angle: -11.1°\n",
" [Pilot] Target Index: 1\n",
" [Pilot] Target Position: (1500, 1000)\n",
" [Pilot] Distance: 1795.0821692652123\n",
"0.1 50.0\n",
" [Pilot] Target Position: [347.32359333 -69.46471867]\n",
" [Pilot] Distance: 304.9907527982671\n",
"0.003525142621744054 50.0\n",
"\n",
" [Pilot] translate: -4.752188299516379 51.16691704857423\n",
" [Pilot] Drone Position: (110.73, 17.12)\n",
" [Pilot] Angle: 17.1°\n",
" [Pilot] translate: -0.4238729464016157 49.716078726704744\n",
" [Pilot] Drone Position: (145.50, -31.21)\n",
" [Pilot] Angle: -10.8°\n",
" [Pilot] Target Index: 1\n",
" [Pilot] Target Position: (1500, 1000)\n",
" [Pilot] Distance: 1749.0606665752869\n",
"0.1 50.0\n"
" [Pilot] Target Position: [347.32359333 -69.46471867]\n",
" [Pilot] Distance: 255.13708614259195\n",
"0.0019957432639431116 50.0\n",
"\n",
" [Pilot] translate: -0.5072011337859063 49.476309464169745\n",
" [Pilot] Drone Position: (194.03, -40.83)\n",
" [Pilot] Angle: -10.6°\n",
" [Pilot] Target Index: 1\n",
" [Pilot] Target Position: [347.32359333 -69.46471867]\n",
" [Pilot] Distance: 205.42141613269357\n",
"0.0008527889497844943 50.0\n",
"\n",
" [Pilot] translate: 0.11201474583194077 50.52107985317721\n",
" [Pilot] Drone Position: (243.70, -50.06)\n",
" [Pilot] Angle: -10.6°\n",
" [Pilot] Target Index: 1\n",
" [Pilot] Target Position: [347.32359333 -69.46471867]\n",
" [Pilot] Distance: 155.9448225243591\n",
"0.0006948810462804877 50.0\n",
"\n",
" [Pilot] translate: 0.0398933070509686 50.472489376060885\n",
" [Pilot] Drone Position: (293.34, -59.18)\n",
" [Pilot] Angle: -10.5°\n",
" [Pilot] Target Index: 1\n",
" [Pilot] Target Position: [347.32359333 -69.46471867]\n",
" [Pilot] Distance: 105.42365806674246\n",
"-0.005757684837339738 50.0\n",
"\n",
" [Pilot] translate: -0.5779730654942618 49.87019784134602\n",
" [Pilot] Drone Position: (342.21, -69.12)\n",
" [Pilot] Angle: -10.8°\n",
" [Pilot] Target Index: 1\n",
" [Pilot] Target Position: [347.32359333 -69.46471867]\n",
" [Pilot] Distance: 54.951716943628604\n",
"-0.6 50.0\n",
"\n",
" [Pilot] translate: -0.3671006590822984 49.56271477490151\n",
" [Pilot] Drone Position: (376.83, -104.60)\n",
" [Pilot] Angle: -45.3°\n",
" [Pilot] Target Index: 2\n",
" [Pilot] Target Position: [ 295.22505433 -347.32359333]\n",
" [Pilot] Distance: 282.1393363134888\n",
"-0.6 50.0\n",
"\n",
" [Pilot] translate: 0.814116178363097 49.83161557706345\n",
" [Pilot] Drone Position: (386.60, -153.47)\n",
" [Pilot] Angle: -79.6°\n",
" [Pilot] Target Index: 2\n",
" [Pilot] Target Position: [ 295.22505433 -347.32359333]\n",
" [Pilot] Distance: 256.07341344997275\n",
"-0.6 50.0\n",
"\n"
]
},
{
"ename": "IndexError",
"evalue": "list index out of range",
"output_type": "error",
"traceback": [
"\u001b[31m---------------------------------------------------------------------------\u001b[39m",
"\u001b[31mIndexError\u001b[39m Traceback (most recent call last)",
"\u001b[36mCell\u001b[39m\u001b[36m \u001b[39m\u001b[32mIn[3]\u001b[39m\u001b[32m, line 3\u001b[39m\n\u001b[32m 1\u001b[39m \u001b[38;5;28;01mfor\u001b[39;00m i \u001b[38;5;129;01min\u001b[39;00m \u001b[38;5;28mrange\u001b[39m(\u001b[32m1\u001b[39m, \u001b[32m20\u001b[39m):\n\u001b[32m 2\u001b[39m \u001b[38;5;28mprint\u001b[39m()\n\u001b[32m----> \u001b[39m\u001b[32m3\u001b[39m command = autopilot.handle(\u001b[43mimgs\u001b[49m\u001b[43m[\u001b[49m\u001b[43mi\u001b[49m\u001b[43m]\u001b[49m)\n\u001b[32m 4\u001b[39m \u001b[38;5;28mprint\u001b[39m(command.dangle, command.velocity)\n\u001b[32m 5\u001b[39m vzm.set_target_index(autopilot.target_idx)\n",
"\u001b[31mIndexError\u001b[39m: list index out of range"
]
}
],
"source": [
"for i in range(1, 4):\n",
"for i in range(1, 20):\n",
" print()\n",
" command = autopilot.handle(imgs[i])\n",
" print(command.dangle, command.velocity)\n",
" vzm.pause(0.5)\n"
" vzm.set_target_index(autopilot.target_idx)\n",
" vzm.update_drone_trajectory(autopilot.x, autopilot.y)\n",
" pos = autopilot.get_position_by_chunk()\n",
" if pos is not None:\n",
" print(pos)\n",
" vzm.update_global_map(pos[0], pos[1])\n",
" vzm.pause(2.5)\n"
]
},
{

17
todo.md
View File

@@ -1,12 +1,7 @@
[+] График межкадрового смещения
| [+] График межкадровых смещениях по версии матрицы гомографии
| [+] Уменьшение погрешность за счёт удаления выбросов
| [+] Исследовать причину погрешности при развороте
[+] Устранение большой погрешности при повороте
[!] Отображение bounding box на графике сопоставления точек кадра с ориентиром
[!] Проверка корректности выявления ориентира на кадре
[!] Исправление коррекции координат на основе сопоставления с ориентиром
[-] Отображение bounding box на графике сопоставления точек кадра с ориентиром
[-] Проверка корректности выявления ориентира на кадре
[-] Исправление коррекции координат на основе сопоставления с ориентиром
[-] FPS счетчик
| [-] Оптимизация детекции точек
[-] Оформление статистики при тестовых запусках
@@ -17,3 +12,9 @@
[?] Изменение масштаба во время полёта, обработка этой трансформации
[?] Поворот ориентиров
[?] Ограничение выбора точек при построении маршрута, чтобы ориентиры полностью попадали в кадр
[+] График межкадрового смещения
| [+] График межкадровых смещениях по версии матрицы гомографии
| [+] Уменьшение погрешность за счёт удаления выбросов
| [+] Исследовать причину погрешности при развороте
[+] Устранение большой погрешности при повороте

1
yandex_map.py
View File

@@ -21,6 +21,7 @@ class YandexMap:
# options.add_experimental_option("detach", True)
self.driver = webdriver.Chrome(options)
self.driver.get(generateURL())
self.driver.maximize_window()
sleep(2)
action = ActionChains(self.driver)