ΠΡΠΎΠ³ΡΠ°ΠΌΠΌΠΈΡΠΎΠ²Π°Π½ΠΈΠ΅ raspberry pi Ρ ΠΈΡΠΏΠΎΠ»ΡΠ·ΠΎΠ²Π°Π½ΠΈΠ΅ΠΌ ΠΌΠ°ΡΠΈΠ½Π½ΠΎΠ³ΠΎ Π·ΡΠ΅Π½ΠΈΡ. ΠΠΎΡΠ°Π±ΠΎΡΠΊΠ° ΡΡΡΠ΅ΡΡΠ²ΡΡΡΠ΅Π³ΠΎ ΠΏΡΠΎΠ΄ΡΠΊΡΠ°. ΠΠΎΠΆΠ΅Π»Π°Π½ΠΈΡ ΠΈ ΠΎΡΠΎΠ±Π΅Π½Π½ΠΎΡΡΠΈ: ΠΡΠΏΡΠ°Π²ΠΈΠ» Π² deepseek Π·Π°ΠΏΡΠΎΡ Π½Π° ΡΡΠ΅Π±ΡΠ΅ΠΌΡΡ ΠΏΡΠΎΠ³ΡΠ°ΠΌΠΌΡ, Π·Π°ΠΏΡΠΎΡ ΠΈ ΡΠ΅Π·ΡΠ»ΡΡΠ°Ρ ΠΏΡΠΈΠΊΡΠ΅ΠΏΠ»Ρ, Π² ΠΎΠ±ΡΠ΅ΠΌ ΡΠ°ΠΌ Π²ΡΠ΅ ΠΏΠΎΠ½ΡΡΠ½ΠΎ ΡΡΠΎ Ρ Ρ
ΠΎΡΡ ΡΠ΄Π΅Π»Π°ΡΡ, Π·Π° ΠΏΠΎΠ΄ΡΠΎΠ±Π½ΡΠΌ ΡΠ· ΠΎΠ±ΡΠ°ΡΠ°ΠΉΡΠ΅ΡΡ, Π²ΡΠ΅ ΡΠ°ΡΠΏΠΈΡΡ. ΠΠ°ΠΏΡΠΎΡΡ Π² deepseek: Π’ΡΠ΅Π±ΡΠ΅ΡΡΡ ΡΠ΄Π΅Π»Π°ΡΡ ΡΠ΄Π΅ΡΠΆΠ°Π½ΠΈΠ΅ ΠΏΠΎΠ·ΠΈΡΠΈΠΈ Π΄ΡΠΎΠ½Π° Π±Π΅Π· gps Ρ ΠΈΡΠΏΠΎΠ»ΡΠ·ΠΎΠ²Π°Π½ΠΈΠ΅ΠΌ ΠΎΠ΄Π½ΠΎΠΏΠ»Π°ΡΠ½ΠΎΠ³ΠΎ ΠΊΠΎΠΌΠΏΡΡΡΠ΅ΡΠ° ΠΈ ΠΊΠ°ΠΌΠ΅ΡΡ ΡΠ°ΡΠΏΠΎΠ»ΠΎΠΆΠ΅Π½Π½ΠΎΠΉ Π² Π΄Π½ΠΈΡΠ΅ Π΄ΡΠΎΠ½Π°, ΠΊΠΎΡΠΎΡΠ°Ρ ΡΡΠ°Π±ΠΈΠ»ΠΈΠ·ΠΈΡΡΠ΅ΡΡΡ ΡΡΡΠΎΠ³ΠΎ Π²Π½ΠΈΠ· ΠΠΎΡ ΠΌΠΎΠ΄Π΅ΡΠ½ΠΈΠ·ΠΈΡΠΎΠ²Π°Π½Π½ΠΎΠ΅ ΡΠ΅ΡΠ΅Π½ΠΈΠ΅, ΠΊΠΎΡΠΎΡΠΎΠ΅ Π΄ΠΈΠ½Π°ΠΌΠΈΡΠ΅ΡΠΊΠΈ Π°Π΄Π°ΠΏΡΠΈΡΡΠ΅ΡΡΡ ΠΊ ΠΈΠ·ΠΌΠ΅Π½ΡΡΡΠ΅ΠΉΡΡ Π²ΡΡΠΎΡΠ΅ ΠΈ ΡΠΎΡ
ΡΠ°Π½ΡΠ΅Ρ ΡΠΎΡΠ½ΠΎΡΡΡ ΠΏΠΎΠ·ΠΈΡΠΈΠΎΠ½ΠΈΡΠΎΠ²Π°Π½ΠΈΡ Π±Π΅Π· GPS. ΠΠ»ΡΡΠ΅Π²ΡΠ΅ ΡΠ»ΡΡΡΠ΅Π½ΠΈΡ Π²ΠΊΠ»ΡΡΠ°ΡΡ Π°Π²ΡΠΎΠΌΠ°ΡΠΈΡΠ΅ΡΠΊΠΈΠΉ ΡΠ°ΡΡΠ΅Ρ ΠΌΠ°ΡΡΡΠ°Π±Π° ΠΈΠ·ΠΎΠ±ΡΠ°ΠΆΠ΅Π½ΠΈΡ ΠΈ ΠΌΠ½ΠΎΠ³ΠΎΡΡΠΎΠ²Π½Π΅Π²ΡΡ ΡΠΈΠ»ΡΡΡΠ°ΡΠΈΡ Π΄Π°Π½Π½ΡΡ
. --- ### 1. ΠΠ»Π³ΠΎΡΠΈΡΠΌ Ρ Π΄ΠΈΠ½Π°ΠΌΠΈΡΠ΅ΡΠΊΠΎΠΉ Π²ΡΡΠΎΡΠΎΠΉ import cv2 import numpy as np from pymavlink import mavutil import time # ΠΠΎΠ½ΡΠΈΠ³ΡΡΠ°ΡΠΈΡ INITIAL_ALTITUDE = 100 # Π‘ΡΠ°ΡΡΠΎΠ²Π°Ρ Π²ΡΡΠΎΡΠ° (ΠΌ) PIX_PER_METER_AT_100M = 64 # 1ΠΌ = 64 ΠΏΠΈΠΊΡΠ΅Π»Ρ Π½Π° 100ΠΌ Π²ΡΡΠΎΡΠ΅ RESET_INTERVAL = 30 # Π‘Π±ΡΠΎΡ ΡΡΠ΅ΠΊΠΈΠ½Π³Π° ΠΊΠ°ΠΆΠ΄ΡΠ΅ 30 ΡΠ΅ΠΊ # ΠΠ½ΠΈΡΠΈΠ°Π»ΠΈΠ·Π°ΡΠΈΡ cap = cv2.VideoCapture(0) cap.set(cv2.CAP_PROP_FRAME_WIDTH, 640) cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 480) mav = mavutil.mavlink_connection('udpout:192.168.1.2:14550') # Π’ΡΠ΅ΠΊΠ΅Ρ ΠΈ ΡΠΈΠ»ΡΡΡΡ orb = cv2.ORB_create(scoreType=cv2.ORB_FAST_SCORE) bf = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=True) ref_kp, ref_des = None, None current_altitude = INITIAL_ALTITUDE def get_altitude(): “““ΠΠΎΠ»ΡΡΠ΅Π½ΠΈΠ΅ ΡΠ΅ΠΊΡΡΠ΅ΠΉ Π²ΡΡΠΎΡΡ ΠΎΡ Π±Π°ΡΠΎΠΌΠ΅ΡΡΠ° ΡΠ΅ΡΠ΅Π· MAVLink“““ msg = mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True, timeout=0.1) return msg.alt / 1000 if msg else current_altitude # ΠΌΠΌ → ΠΌΠ΅ΡΡΡ def calculate_scale(altitude): “““ΠΠΈΠ½Π°ΠΌΠΈΡΠ΅ΡΠΊΠΈΠΉ ΡΠ°ΡΡΠ΅Ρ ΠΌΠ°ΡΡΡΠ°Π±Π° ΠΏΠΈΠΊΡΠ΅Π»Ρ/ΠΌΠ΅ΡΡ“““ return PIX_PER_METER_AT_100M * (100 / altitude) def reset_reference(): global ref_kp, ref_des, current_altitude current_altitude = get_altitude() ret, frame = cap.read() if not ret: return False gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) ref_kp, ref_des = orb.detectAndCompute(gray, None) return True def get_displacement(): if ref_des is None: return 0, 0 ret, frame = cap.read() if not ret: return 0, 0 # ΠΠ΅ΡΠ΅ΠΊΡΠΈΡ ΠΈ ΡΠΎΠΏΠΎΡΡΠ°Π²Π»Π΅Π½ΠΈΠ΅ gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) kp, des = orb.detectAndCompute(gray, None) if des is not None and len(des) > 20: matches = bf.match(ref_des, des) matches = sorted(matches, key=lambda x: x.distance)[:20] if len(matches) > 10: src_pts = np.float32([ref_kp[m.queryIdx].pt for m in matches]) dst_pts = np.float32([kp[m.trainIdx].pt for m in matches]) M, _ = cv2.estimateAffinePartial2D(src_pts, dst_pts) if M is not None: scale = calculate_scale(get_altitude()) return M[0, 2]/scale, M[1, 2]/scale return 0, 0 # ΠΡΠ½ΠΎΠ²Π½ΠΎΠΉ ΡΠΈΠΊΠ» reset_reference() last_reset = time.time() while True: # ΠΠ²ΡΠΎΠΌΠ°ΡΠΈΡΠ΅ΡΠΊΠΈΠΉ ΡΠ±ΡΠΎΡ ΠΏΠΎ ΡΠ°ΠΉΠΌΠ΅ΡΡ if time.time() - last_reset > RESET_INTERVAL: reset_reference() last_reset = time.time() dx, dy = get_displacement() # ΠΡΠΏΡΠ°Π²ΠΊΠ° Π² ArduPilot Ρ ΡΡΠ΅ΡΠΎΠΌ Π²ΡΡΠΎΡΡ mav.mav.vision_position_estimate_send( int(time.time() * 1e6), dx, dy, 0, 0, 0, 0, # ΠΡΠΈΠ΅Π½ΡΠ°ΡΠΈΡ int(current_altitude * 1000), # ΠΡΡΠΎΡΠ° Π² ΠΌΠΌ 0 ) time.sleep(0.1) --- ### 2. ΠΡΠΈΡΠΈΡΠ΅ΡΠΊΠΈΠ΅ ΡΠ»ΡΡΡΠ΅Π½ΠΈΡ #### 1. ΠΠΈΠ½Π°ΠΌΠΈΡΠ΅ΡΠΊΠΈΠΉ ΠΌΠ°ΡΡΡΠ°Π± - Π€ΠΎΡΠΌΡΠ»Π°: scale = (PIX_PER_METER_AT_100M) * (100 / current_altitude) ΠΡΠΈΠΌΠ΅Ρ: - ΠΠ° 100 ΠΌ: 64 px/ΠΌ - ΠΠ° 150 ΠΌ: 43 px/ΠΌ - ΠΠ° 50 ΠΌ: 128 px/ΠΌ #### 2. ΠΠ²ΡΠΎΠΌΠ°ΡΠΈΡΠ΅ΡΠΊΠ°Ρ ΠΊΠΎΡΡΠ΅ΠΊΡΠΈΡ Π²ΡΡΠΎΡΡ - Π§ΡΠ΅Π½ΠΈΠ΅ Π΄Π°Π½Π½ΡΡ
Π±Π°ΡΠΎΠΌΠ΅ΡΡΠ° ΡΠ΅ΡΠ΅Π· MAVLink: msg = mav.recv_match(type='GLOBAL_POSITION_INT') current_altitude = msg.alt / 1000 # ΠΠ΅ΡΠ΅Π²ΠΎΠ΄ΠΈΠΌ ΠΌΠΌ Π² ΠΌΠ΅ΡΡΡ #### 3. ΠΠΈΠ±ΡΠΈΠ΄Π½ΡΠΉ ΡΡΠ΅ΠΊΠΈΠ½Π³ - ΠΠΎΠΌΠ±ΠΈΠ½Π°ΡΠΈΡ ORB + RANSAC Π΄Π»Ρ ΡΠΈΠ»ΡΡΡΠ°ΡΠΈΠΈ Π²ΡΠ±ΡΠΎΡΠΎΠ²: M, inliers = cv2.estimateAffinePartial2D( src_pts, dst_pts, method=cv2.RANSAC, ransacReprojThreshold=3.0 ) --- ### 3. ΠΠ°ΡΡΡΠΎΠΉΠΊΠΈ ArduPilot EK3_SRC1_POSZ = 0 # ΠΠ°ΡΠΎΠΌΠ΅ΡΡ ΠΊΠ°ΠΊ ΠΎΡΠ½ΠΎΠ²Π½ΠΎΠΉ ΠΈΡΡΠΎΡΠ½ΠΈΠΊ Π²ΡΡΠΎΡΡ EK3_ALT_SOURCE = 1 # Π€ΠΎΠ»Π±ΡΠΊ Π½Π° Π±Π°ΡΠΎΠΌΠ΅ΡΡ ΠΏΡΠΈ ΠΏΠΎΡΠ΅ΡΠ΅ Vision VIS_POS_UNCERT = 3 # ΠΠΎΠ³ΡΠ΅ΡΠ½ΠΎΡΡΡ 3 ΠΌΠ΅ΡΡΠ° EK3_FLOW_USE = 1 # Π Π°Π·ΡΠ΅ΡΠΈΡΡ ΠΊΠΎΡΡΠ΅ΠΊΡΠΈΡ Π²ΡΡΠΎΡΡ --- ### 4. ΠΡΠ΅Π½ΠΊΠ° ΡΠΎΡΠ½ΠΎΡΡΠΈ ΠΏΡΠΈ ΠΈΠ·ΠΌΠ΅Π½ΡΡΡΠ΅ΠΉΡΡ Π²ΡΡΠΎΡΠ΅ | ΠΡΡΠΎΡΠ° | ΠΡΠΈΠ±ΠΊΠ° ΠΏΠΎΠ·ΠΈΡΠΈΠΎΠ½ΠΈΡΠΎΠ²Π°Π½ΠΈΡ (Π² ΡΠ°Ρ) | |--------|-------------------------------| | 50 ΠΌ | ≤5 ΠΌ | | 100 ΠΌ | ≤8 ΠΌ | | 150 ΠΌ | ≤12 ΠΌ |.