目錄
背景
算法思路
代碼實現
驗證
背景
????????在城市交通巡檢中如何進行車輛違停判斷很重要，一個方法是通過精確坐標判斷車輛中心是否位于違停框中，我們假設無人機坐標已知，并且無人機云臺鏡頭垂直地面朝下，可根據圖像分辨率、無人機參數、無人機坐標、識別目標位置等信息，解算出識別目標的具體坐標。
無人機采用大疆機場 2 自帶的 Matrice3TD 無人機，參數如下：
水平視場角：84 °
圖像分辨率：800 X 600
無人機自帶 RTK ，實時測定無人機當前坐標信息，包括經度、緯度、相對高度以及航向角
算法思路
????????通過水平視場角和高度計算出影像實際長度，再由實際長度和影像分辨率計算單個柵格長度，再由yolo 目標識別矩形框計算中心柵格坐標，并計算與無人機地面投影點的柵格坐標差，從而計算實際矢量坐標差，再由航向角計算出目標中心最終坐標。
簡要解算：
注意，本算法僅考慮無人機鏡頭垂直朝下的情況！！！
代碼實現
參數定義：
# 參數
Horizontal_view = 84 # 無人機水平視場角
Resolution_X = 800 # X 軸柵格數
Resolution_Y = 600 # Y 軸柵格數
R = 6371000 # 地球半徑（米）獲取單個像素長度：
def calculate_pixel_length(h):"""計算像素長度:param h: 無人機相對高度（米）:return: 像素長度（米）"""# 計算水平視場角的一半half_horizontal_view = Horizontal_view / 2.0# 圖像長度l = 2.0 * h * math.tan(math.radians(half_horizontal_view))distance = l / Resolution_Xreturn distance可利用水平視場角、分辨率和高度 h 計算
解算目標坐標：
def calculate_target_coordinates(x1, y1, x2, y2, heading_angle, longitude, latitude, h):"""計算目標坐標，僅考慮目標位于北半球、東半球:param x1: 目標左上角 X 軸像素位置:param y1: 目標左上角 Y 軸像素位置:param x2: 目標右下角 X 軸像素位置:param y2: 目標右下角 Y 軸像素位置:param heading_angle: 無人機航向角（度）:param longitude: 無人機經度（度）:param latitude: 無人機緯度（度）:param h: 無人機相對高度（米）:return: 目標坐標 (x, y)"""pixel_length = calculate_pixel_length(h)# 目標中心點像素坐標target_x = (x1 + x2) / 2.0target_y = (y1 + y2) / 2.0# 無人機像素中心坐標drone_x = Resolution_X / 2.0drone_y = Resolution_Y / 2.0# 計算相對于中心點的像素位移，yolo 識別輸出圖像的坐標軸向下為 y 軸增加，需反轉，不然后面坐標系無法轉換raster_dx = target_x - drone_xraster_dy = drone_y - target_y# 實際距離vector_dx = raster_dx * pixel_lengthvector_dy = raster_dy * pixel_length# 圖像坐標系轉為地理坐標系，計算地理坐標系的分量，可理解為將圖像坐標系逆時針旋轉到與地理坐標系重合dx = vector_dx * math.cos(math.radians(heading_angle)) - vector_dy * math.sin(math.radians(heading_angle))dy = vector_dx * math.sin(math.radians(heading_angle)) + vector_dy * math.cos(math.radians(heading_angle))# 解算坐標，一緯度約為111km，一經度則還需乘cos(緯度)target_lon = longitude + (180 / math.pi) * (dx / (R * math.cos(latitude)))target_lat = latitude + (180 / math.pi) * (dy / R)# 返回目標坐標return target_lon, target_lat????????計算出目標相對于中心點的位移后，需要將 y 軸反轉，因為圖像 y 軸正方向是朝下的，與地理坐標系相反，然后再將圖像坐標系作坐標系的旋轉，旋轉到地理坐標系上，從而能計算出目標點位移對于地理坐標系的分量，從而解算目標點坐標。
驗證
if __name__ == '__main__':x, y = calculate_target_coordinates(10, 20, 20, 30, 20, 115.89, 28.68, 5)print(f"Target Coordinates: Longitude={x}, Latitude={y}")輸出結果：
大家可以自行驗證下，理論上是沒有問題的。
至于調用，在 yolo 識別后，會輸出 bbox 等參數，然后對每個幀循環目標再調用此算法即可。
感謝觀看！！！
)
