Python x Lane Keeping — ก้าวแรกสู่ Self-Driving Car!

Source: my.chevrolet.com

กราบบบสวัสดีทุกๆท่านอีกครั้ง รอบนี้มาแปลกนิดนึง 😂 พอดีดูรีวิวรถเยอะจนสงสัยพวกระบบ Assist System ต่างๆ มีการทำงานยังไง เลยเกิดการสงสัยจนต้องทำ POC (Proof Of Concept) เพื่อให้ความสงสัยนั้นหายไป 😝 ก่อนจะเริ่ม ผมขอฝาก FB Page นิดนึงครับ … “พ่อบ้าน Coding” มาเริ่มเข้าเรื่องกันดีกว่าเนอะ

Lane Keeping Assist คืออะไร ??

Lane Keeping Assist (LKAS) คือระบบที่ช่วยให้รถวิ่งอยู่ในเลนโดยอัตโนมัติด้วยการตรวจจับเส้นแบ่งเลนและควบคุมพวงมาลัยนั่นเองครับ 🚗 🚙 🛻

Source: tesla.com

หลักการทำงานของ Lane Keeping

ระบบ Lane Keeping ใช้เซ็นเซอร์ต่างๆ เช่น กล้อง, LiDAR, Radar, GPS, และ HD Map เพื่อตรวจจับเส้นแบ่งเลนและวิเคราะห์สภาพแวดล้อม โดยใช้เทคนิค Computer Vision หรือ Deep Learning (CNN) ในการประมวลผลภาพและตรวจจับเส้นเลน ก่อนควบคุมพวงมาลัยโดยอัลกอริธึมต่างๆ เช่น PID Control หรือ Model Predictive Control (MPC) เพื่อให้รถวิ่งอยู่ในเลนอย่างแม่นยำ

👆 อาจจะ งง นิดๆเนอะ แต่ใน POC (Proof Of Concept) จะใช้ CV (Computer Vision) มาทำการประมวลผลเท่านั้น เพื่อจับเลนถนนและรถคันหน้า เพื่อแจ้งให้รถทำตาม logic ที่เราต้องการ ฟังอาจจะดูภาพไม่ออกเท่าไรไม่เป็นครับ เดี๋ยวเราไปพร้อมๆกัน

Source: motor-works.com

จากรูปข้างบนจะเห็นที่วงกลมสีส้มนั่น คือ กล้อง สำหรับประมวลผล Lane Keeping ส่วนใหญ่เลยจำเป็นต้องมีกล้อง 📸 ที่ติดมาจากโรงงานเรียบร้อยแล้วนั่นเอง

Tech Stack

เดี๋ยวเรามาเริ่มเลย Tech Stack ที่เราใช้หลักๆเลยเราจะใช้ Python เพราะการใช้ ไลบรารี OpenCV ซึ่ง Python ง่ายสุดและโค้ดง่ายสุดอ่านง่าย Math สามารถเข้าใจง่าย เดี๋ยวผมจะอธิบาย Stack ยกเว้น Python เนอะเริ่มเลย 🦊

1. OpenCV (Open Source Computer Vision Library) คือ ไลบรารีโอเพนซอร์สที่ใช้สำหรับการประมวลผลภาพและคอมพิวเตอร์วิชัน (Computer Vision) โดยเฉพาะ ซึ่งมีฟังก์ชันที่ช่วยในการทำงานเกี่ยวกับการรับรู้และประมวลผลข้อมูลภาพ เช่น การตรวจจับวัตถุ, การแยกสี, การติดตามการเคลื่อนไหว, การปรับภาพ, การทำ segmentation, และการจดจำใบหน้า เป็นต้น
2. NumPy (Numerical Python) คือ ไลบรารีที่ใช้ในการคำนวณเชิงตัวเลขและการจัดการข้อมูลในรูปแบบอาเรย์ (Arrays) ที่มีประสิทธิภาพสูงใน Python โดยเฉพาะในการทำงานกับข้อมูลที่มีมิติหลายมิติ เช่น เมทริกซ์หรืออาเรย์ที่มีมากกว่าหนึ่งมิติ

หลักๆเราใช้แค่ 2 ตัวนี้แต่หัวใจหลักของเราจริงๆ OpenCV ครับ ซึ่งจะทำพวกการตรวจกับเส้นเลนถนนนั่นเอง เจ๋งๆเนอะ 🤩

Detect Lane and Detect Lane Center

เดี๋ยวเรามาลงโค้ดกันเลยดีกว่าเพราะเนื้อหาแอบเยอะนิดนึงครับ แต่ถ้าอินก็จะรู้สึกสนุกเลย มาเริ่มกันดีกว่า

def preprocess_image(frame):
    """ Preprocess the image to detect edges """
    gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
    blur = cv2.GaussianBlur(gray, (5, 5), 0)
    edges = cv2.Canny(blur, 50, 150)
    return edges

ส่วนแรกฟังก์ชั่น preprocess_image

ฟังก์ชันนี้ทำการเตรียมภาพ (Preprocessing) เพื่อให้การตรวจจับขอบ (Edge Detection) ทำได้ง่ายขึ้น

• cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY): แปลงภาพจากรูปแบบสี (BGR) เป็นขาว-ดำ (Grayscale) ซึ่งจะช่วยลดความซับซ้อนของภาพและทำให้ไม่ต้องใช้ข้อมูลสีในการตรวจจับขอบ
• cv2.GaussianBlur(gray, (5, 5), 0): ใช้ Gaussian Blur เพื่อลดสัญญาณรบกวนในภาพ ขนาดเคอร์เนล (5, 5) ควบคุมการเบลอ
• cv2.Canny(blur, 50, 150): ใช้การตรวจจับขอบแบบ Canny โดยใช้เกณฑ์ (Threshold) 50 และ 150 เพื่อหาขอบในภาพที่เบลอแล้ว

def detect_lane(frame):
    global last_lane_event_time, last_lane_message
    """ Detect lane lines, differentiate solid and dashed lines, calculate the center point, and alert for lane departure """
    edges = preprocess_image(frame)
    lines = cv2.HoughLinesP(edges, 1, np.pi / 180, 50, minLineLength=50, maxLineGap=100)

    left_lane_x, right_lane_x = [], []

    if lines is not None:
        for line in lines:
            x1, y1, x2, y2 = line[0]
            slope = (y2 - y1) / (x2 - x1) if (x2 - x1) != 0 else 0  # Calculate the slope of the line

            if slope < 0:  # left lane
                left_lane_x.append((x1 + x2) // 2)
                color = (0, 255, 0)  # Green for left lane
            else:  # right lane
                right_lane_x.append((x1 + x2) // 2)
                color = (0, 255, 0)  # Green for right lane

            cv2.line(frame, (x1, y1), (x2, y2), color, 3)

    if left_lane_x and right_lane_x:
        left_lane_avg = int(np.mean(left_lane_x))
        right_lane_avg = int(np.mean(right_lane_x))
        lane_center = (left_lane_avg + right_lane_avg) // 2
    else:
        lane_center = frame.shape[1] // 2

    car_center = frame.shape[1] // 2

    cv2.line(frame, (lane_center, frame.shape[0]), (lane_center, int(frame.shape[0] * 0.6)), (0, 0, 255), 2)

    if lane_center < car_center - 50:
        message = "Move Right ->"
        color = (0, 0, 255)  # Red for moving right
    elif lane_center > car_center + 50:
        message = "<- Move Left"
        color = (0, 0, 255)  # Red for moving left
    else:
        message = "Centered"
        color = (0, 255, 0)  # Green for centered

    current_time = time.time()
    if message != last_lane_message:
        last_lane_event_time = current_time
        last_lane_message = message
    elif current_time - last_lane_event_time >= 1.5:
        cv2.putText(frame, last_lane_message, (50, 50), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2)
    else:
        cv2.putText(frame, "Centered", (50, 50), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2)

    return frame

ส่วนต่อมาเป็นฟังก์ชั่นหลักเลยก็ว่าได้ detect_lane

ฟังก์ชันนี้ตรวจจับเส้นเลน (Lane) ทั้งที่เป็นเส้นทึบและเส้นประ คำนวณจุดศูนย์กลางของเลน, และการบังคับให้รถอยู่กลางเลน

• edges = preprocess_image(frame): เรียกใช้ฟังก์ชัน preprocess_image เพื่อหาขอบของภาพ
• lines = cv2.HoughLinesP(edges, 1, np.pi / 180, 50, minLineLength=50, maxLineGap=100): ใช้ Hough Line Transform เพื่อตรวจจับเส้นในภาพที่มีขอบ โดยมีพารามิเตอร์ต่าง ๆ เช่น ความยาวขั้นต่ำของเส้น (minLineLength) และช่องว่างสูงสุดระหว่างเส้น (maxLineGap)

การจำแนกเส้นที่พบเป็นเส้นเลนซ้ายหรือเลนขวา:

สูตรสำหรับคำนวณความชัน (slope) ของเส้น

• slope = (y2 — y1) / (x2 — x1): คำนวณความชันของเส้น
• slope < 0 หมายถึงเส้นนี้เป็นของเลนซ้าย
• slope > 0 หมายถึงเส้นนี้เป็นของเลนขวา

สำหรับแต่ละเส้น จุดกึ่งกลางของเส้น (พิกัด x) จะถูกบันทึกลงใน left_lane_x หรือ right_lane_x

• cv2.line(frame, (x1, y1), (x2, y2), color, 3): วาดเส้นที่ตรวจจับได้บนภาพ โดยใช้สีเขียวสำหรับทั้งเลนซ้ายและขวา

หลังจากตรวจจับเส้นเลนแล้ว:

• left_lane_avg และ right_lane_avg จะคำนวณค่าเฉลี่ยของพิกัด x ของเส้นเลนซ้ายและขวา
• lane_center จะถูกคำนวณเป็นจุดกึ่งกลางระหว่างค่าเฉลี่ยของเส้นเลนซ้ายและขวา ถ้าไม่พบเส้นเลนใดๆ ก็จะตั้งค่าเป็นกลางของภาพ

ตำแหน่งศูนย์กลางของรถคือจุดกึ่งกลางของความกว้างของภาพ car_center: วาดเส้นสีแดงแนวตั้งที่ตำแหน่งศูนย์กลางของเลน

cv2.line(frame, (lane_center, frame.shape[0]), (lane_center, int(frame.shape[0] * 0.6)), (0, 0, 255), 2)

def detect_front_vehicle(frame):
    """ Detect the front vehicle using Haar Cascade """
    gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
    cars = car_cascade.detectMultiScale(gray, 1.1, 3)

    for (x, y, w, h) in cars:
        cv2.rectangle(frame, (x, y), (x + w, y + h), (255, 0, 0), 2)
        car_center = (x + w // 2, y + h // 2)
        frame_center = (frame.shape[1] // 2, frame.shape[0] // 2)
        distance = int(np.sqrt((car_center[0] - frame_center[0]) ** 2 + (car_center[1] - frame_center[1]) ** 2))
        cv2.putText(frame, f"Car : {distance} px", (x, y - 10), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 2)

        if distance < SAFE_DISTANCE:  # distance brake
            cv2.putText(frame, "BRAKE!", (frame.shape[1] // 2 - 50, 50), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2)

    return frame

ส่วนต่อมาเป็นฟังก์ชั่นสำหรับตรวจจับรถคันหน้า detect_front_vehicle

แปลงภาพเป็นขาว-ดำ (Grayscale): แปลงภาพที่รับเข้ามา (ในรูปแบบ BGR) เป็นภาพขาว-ดำ (Grayscale) เพื่อให้การตรวจจับวัตถุทำได้ง่ายขึ้น (Haar Cascade ใช้ภาพขาว-ดำในการตรวจจับ)

gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)

ใช้ Haar Cascade เพื่อตรวจจับรถ: car_cascade.detectMultiScale เป็นการเรียกใช้โมเดล Haar Cascade ที่ใช้ตรวจจับรถในภาพ โดยใช้ภาพขาว-ดำเป็นอินพุต

cars = car_cascade.detectMultiScale(gray, 1.1, 3)

วนลูปเพื่อตรวจจับแต่ละรถในภาพ: ถ้ามีการตรวจพบรถหลายคันในภาพ cars จะเก็บข้อมูลพิกัด (x, y) ของจุดมุมซ้ายบนและขนาด (w, h) ของกล่องที่ล้อมรอบรถ (bounding box)

for (x, y, w, h) in cars:

วาดกรอบรอบรถ: วาดสี่เหลี่ยม (rectangle) รอบรถที่ตรวจพบ โดยใช้พิกัดที่ได้จาก x, y และขนาด w, h และใช้สี (255, 0, 0) ซึ่งเป็นสีน้ำเงิน

cv2.rectangle(frame, (x, y), (x + w, y + h), (255, 0, 0), 2)

คำนวณระยะห่างระหว่างรถกับศูนย์กลางของภาพ: คำนวณพิกัดของจุดกึ่งกลางของรถ (car_center) และพิกัดของศูนย์กลางของภาพ (frame_center) ใช้สูตรคำนวณระยะห่าง Euclidean distance เพื่อหาความห่างระหว่างรถกับศูนย์กลางของภาพ

car_center = (x + w // 2, y + h // 2)
frame_center = (frame.shape[1] // 2, frame.shape[0] // 2)
distance = int(np.sqrt((car_center[0] - frame_center[0]) ** 2 + (car_center[1] - frame_center[1]) ** 2))

แสดงระยะห่างของรถ: แสดงข้อความระยะห่างจากรถถึงศูนย์กลางในพิกัด (x, y - 10) บนภาพโดยใช้ฟอนต์ FONT_HERSHEY_SIMPLEX และใช้สีขาว (255, 255, 255)

cv2.putText(frame, f"Car : {distance} px", (x, y - 10), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 2)

ตรวจจับระยะห่างที่ปลอดภัย: หากระยะห่างระหว่างรถกับศูนย์กลางน้อยกว่า SAFE_DISTANCE (ซึ่งเป็นค่าที่กำหนดไว้ล่วงหน้า) จะถือว่าเป็นการใกล้เกินไปและแสดงข้อความ “BRAKE!” บนภาพเพื่อเตือนให้เบรก

ข้อความ “BRAKE!” จะถูกแสดงที่ตำแหน่ง (frame.shape[1] // 2 - 50, 50) โดยใช้ฟอนต์ FONT_HERSHEY_SIMPLEX และสีแดง (0, 0, 255)

if distance < SAFE_DISTANCE:  # distance brake
    cv2.putText(frame, "BRAKE!", (frame.shape[1] // 2 - 50, 50), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2)

ฟังก์ชั้นหลักๆที่เราใช้ก็หมดแล้วครับ เยอะนิดนึงแต่พยายามอธิบายโค้ดให้ละเอียดเพราะมีการใช้สูตรคำนวณ อยู่หลายที่ครับผม อาจจะรู้สึกมึนๆ ผมก็มึนครับ 5555 🤣

Demo

ถ้าดูจาก Demo จะเห็นได้ OpenCV ช่วย Detect เส้นของเลนทั้งหมด และถ้าสังเกตจะมีเส้นแดงกลางจอ สำหรับไว้เช็คว่ารถยังอยู่กลางเลนหรือไม่ ในคลิปมันมีการสั่นเนอะ ถ้าโดยปกติรถก็จะนิ่งกว่านี้ จะทำให้ยึดจุดได้แม่นกว่าเนอะเยอะมาก ผมว่าถือว่าเป็นเรื่องสนุกของผมพอควรเลยเดี๋ยวจะลอง POC ท่าอื่นไปเรื่อยๆครับ หรือถ้ามีอะไรน่าสนใจหรือน่าเล่นมาแชร์อีกนะครับ ผมใส่ reference กับ example ไว้ให้แล้วข้างล่างเรียบร้อยครับ ….. “ไม่ว่าม้าจะเร็วสักแค่ไหน สุดท้ายก็ต้องแพ้ลา” ลาก่อน วันนี้ที่จะมาแชร์ก็มีเพียงเท่านี้ จริงๆเดี๋ยวจะพยายามมาแชร์เรื่อยๆไม่ว่าจะเป็น tech อื่นๆที่นอกจาก mobile กราบบบขอบคุณสำหรับทุกๆท่านครับ 🤟🏼👋🏼 ผมขอฝาก FB Page นิดนึงครับ … “พ่อบ้าน Coding”

Reference

https://www.mdpi.com/1424-8220/22/24/9889

Example Project:

https://github.com/ongyoo/blog_unit_5_car_lane_keeping