คำนวณตำแหน่งของหุ่นยนต์ขับเคลื่อนต่าง

14

คุณคำนวณหรืออัพเดทตำแหน่งของหุ่นยนต์ขับเคลื่อนที่แตกต่างด้วยเซ็นเซอร์แบบเพิ่มหน่วยได้อย่างไร

มีเซ็นเซอร์เพิ่มความละเอียดหนึ่งอันติดอยู่กับล้อเฟืองท้ายสองอัน เซ็นเซอร์ทั้งสองเป็นตัวกำหนดระยะทางหายใจ ล้อของพวกเขาได้รีดในช่วงเวลาที่รู้จักกันT $\Delta left$ $\Delta right$ $\Delta t$

ก่อนอื่นสมมติว่าศูนย์กลางระหว่างล้อทั้งสองทำเครื่องหมายตำแหน่งของหุ่นยนต์ ในกรณีนี้เราสามารถคำนวณตำแหน่งเป็น:

x = \frac{x_{l e f t} + x_{r i g h t}}{2} y = \frac{y_{l e f t} + y_{r i g h t}}{2}

$x = \frac{x_{left}+x_{right}}{2} \\ y = \frac{y_{left}+y_{right}}{2}$

"การรับ" สมการเหล่านั้นภายใต้สมมติฐานที่ว่าล้อทั้งสองหมุนเป็นเส้นตรง (ซึ่งควรจะถูกต้องสำหรับระยะทางเล็กน้อย) ฉันได้รับ:

\frac{Δ x}{Δ t} = \frac{1}{2} (\frac{Δ l e f t}{Δ t} + \frac{Δ r i g h t}{Δ t}) c o s (θ) \frac{Δ y}{Δ t} = \frac{1}{2} (\frac{Δ l e f t}{Δ t} + \frac{Δ r i g h t}{Δ t}) s i n (θ)

$\frac{\Delta x}{\Delta t} = \frac{1}{2}\left( \frac{\Delta left}{\Delta t} + \frac{\Delta right}{\Delta t}\right)cos(\theta) \\ \frac{\Delta y}{\Delta t} = \frac{1}{2}\left( \frac{\Delta left}{\Delta t} + \frac{\Delta right}{\Delta t}\right)sin(\theta)$

โดยที่ $\theta$ คือมุมของการวางแนวของหุ่นยนต์ สำหรับการเปลี่ยนแปลงของมุมนี้ฉันพบสมการ

\frac{Δ θ}{Δ t} = \frac{1}{w} (\frac{Δ l e f t}{Δ t} - \frac{Δ r i g h t}{Δ t})

$\frac{\Delta \theta}{\Delta t} = \frac{1}{w} \left( \frac{\Delta left}{\Delta t} - \frac{\Delta right}{\Delta t}\right)$

โดยที่ $w$ คือระยะห่างระหว่างล้อทั้งสอง

เนื่องจากและขึ้นอยู่กับฉันสงสัยว่าฉันควรคำนวณใหม่ด้วยการเพิ่มหรือถ้าฉันควรจะใช้ "old" ? มีเหตุผลที่จะใช้อย่างใดอย่างหนึ่งมากกว่าที่อื่น ๆ ? $\Delta x$ $\Delta y$ $\theta$ $\theta$ $\Delta \theta$ $\theta$

จากนั้นให้สมมติว่าศูนย์กลางระหว่างล้อทั้งสองไม่ได้ทำเครื่องหมายตำแหน่งของหุ่นยนต์ แต่ฉันต้องการใช้จุดที่ทำเครื่องหมายศูนย์กลางทางเรขาคณิตของกล่องขอบเขตของหุ่นยนต์ จากนั้นและเปลี่ยนเป็น: $x$ $y$

x = \frac{x_{l e f t} + x_{r i g h t}}{2} + l c o s (θ) y = \frac{y_{l e f t} + y_{r i g h t}}{2} + l s i n (θ)

$x = \frac{x_{left}+x_{right}}{2} + l\, cos(\theta)\\ y = \frac{y_{left}+y_{right}}{2} + l\, sin(\theta)$

"Deriving" สิ่งแรกที่ให้:

\frac{Δ x}{Δ t} = \frac{1}{2} (\frac{Δ l e f t}{Δ t} + \frac{Δ r i g h t}{Δ t}) c o s (θ) - l s i n (θ) \frac{Δ θ}{Δ t}

$\frac{\Delta x}{\Delta t} = \frac{1}{2}\left( \frac{\Delta left}{\Delta t} + \frac{\Delta right}{\Delta t}\right)cos(\theta) - l\,sin(\theta)\,\frac{\Delta \theta}{\Delta t}$

ขณะนี้มี Dependance ใน\ นี่เป็นเหตุผลที่ใช้ "ใหม่"หรือไม่ $\Delta \theta$ $\theta$

มีวิธีใดที่ดีไปกว่านี้ในการปรับปรุงตำแหน่งและทิศทาง อาจใช้ตัวเลขที่ซับซ้อน (เช่นเดียวกับ quaternions ในแบบ 3 มิติ?) หรือพิกัดที่เป็นเนื้อเดียวกัน?

— Daniel Jour
แหล่งที่มา

8

เพื่อตอบคำถามแรกของคุณ: หากคุณต้องการหาสมการจลนศาสตร์ที่แท้จริงสำหรับไดรฟ์อนุพันธ์ฉันจะไม่เริ่มประมาณโดยสมมติว่าแต่ละล้อเคลื่อนที่เป็นเส้นตรง หารัศมีวงเลี้ยวคำนวณจุดศูนย์กลางของส่วนโค้งจากนั้นคำนวณจุดถัดไปของหุ่นยนต์ รัศมีการเลี้ยวจะไม่สิ้นสุดหากหุ่นยนต์เคลื่อนที่ตรง แต่ในกรณีตรงคณิตศาสตร์นั้นง่าย

ดังนั้นลองจินตนาการว่าในแต่ละขั้นตอนหรือในแต่ละครั้งที่คุณคำนวณการเปลี่ยนแปลงในเซ็นเซอร์ที่เพิ่มขึ้นหุ่นยนต์จะเคลื่อนที่จากจุด A ไปยังจุด B บนอาร์คเช่นนี้: ป้อนคำอธิบายรูปภาพที่นี่ นี่คือตัวอย่างโค้ดบางส่วนที่มีคณิตศาสตร์ง่ายขึ้น:

// leftDelta and rightDelta = distance that the left and right wheel have moved along
//  the ground

if (fabs(leftDelta - rightDelta) < 1.0e-6) { // basically going straight
    new_x = x + leftDelta * cos(heading);
    new_y = y + rightDelta * sin(heading);
    new_heading = heading;
} else {
    float R = unitsAxisWidth * (leftDelta + rightDelta) / (2 * (rightDelta - leftDelta)),
          wd = (rightDelta - leftDelta) / unitsAxisWidth;

    new_x = x + R * sin(wd + heading) - R * sin(heading);
    new_y = y - R * cos(wd + heading) + R * cos(heading);
    new_heading = boundAngle(heading + wd);
}

ฉันใช้คณิตศาสตร์ที่คล้ายกันในเครื่องจำลองเพื่อสาธิตวิธีการบังคับเลี้ยวที่แตกต่างกัน: http://www.cs.utexas.edu/~rjnevels/RobotSimulator4/demos/SteeringDemo/

— Robz
แหล่งที่มา

1

สมการที่ใช้ในข้อมูลโค้ดข้างต้นได้มาที่นี่: rossum.sourceforge.net/papers/DiffSteer

— kamek

คำอธิบายที่ดี! ลิงก์จำลองเสียหาย

— smirkingman

2

$\Delta\theta$ $\theta$ $\Delta{x}, \Delta{y}$

$\Delta\theta$ $\Delta{x}, \Delta{y}$ $\theta$

$\Delta{t}$ $0$

— เอียน
แหล่งที่มา

การค้นหา "จลนศาสตร์ล่วงหน้าของยานพาหนะขับเคลื่อนที่แตกต่างกัน" ควรให้บทความจำนวนมากด้วยวิธีการทางคณิตศาสตร์ที่มากขึ้นสำหรับคำถามนี้

— เอียน