一種差速驅(qū)動小車曲線行走方法

依據(jù)小車實際運動狀況，在具體擬合時，提出如下擬合要求：
①擬合圓弧必須過列表點；
②列表點兩側(cè)圓弧在列表點處光滑相切，且切線斜率與理論曲線在列表點處相等；
③兩相鄰列表點之間的兩段圓弧在圓弧與圓弧交接點處相切；
④曲線段內(nèi)過列表點的兩側(cè)圓弧半徑應(yīng)盡量與理論曲線在列表點處的曲率半徑接近。
在給定曲線上取列表點A，B，C，…，記它們的坐標(biāo)分別為(xA，yA)，(xB，yB)，(zC，yC)，…，有：

根據(jù)式(7)可以求出各列表點處的法線斜率角θA，θB，θC，…，根據(jù)式(6)可以求出曲率半徑ρA，ρB，ρC，…，由這兩組數(shù)據(jù)可以求出理論曲線過列表點的各曲率中心的坐標(biāo)值。
對于曲線段AB，用兩段圓弧去擬合曲線段AB，圓弧的圓心分別在O1和O2，則半徑分別為O1A和O2B。O1和O2的坐標(biāo)分別為(X1，y1)和(x2，y2)，為滿足之前提到的第①個和第②個要求，應(yīng)滿足關(guān)系式：

為滿足上述第③個要求，應(yīng)滿足以下關(guān)系式：

聯(lián)立式(8)、式(9)，含有4個未知數(shù)x1，y1，x2，y2，但只有3個方程，故不能求解。為滿足上述第④個要求，以x1為優(yōu)化變量，以(|ρA|-O1A)2+(|ρB|-O2B)2為目標(biāo)函數(shù)，于是雙圓弧擬合問題就轉(zhuǎn)化為以這3個方程為等約束條件，使目標(biāo)函數(shù)(|ρA|-O1A)2+(|ρB|-O2B)2趨于最小的優(yōu)化問題。具體過程是：以理論曲線過A點的曲率中心的x坐標(biāo)作為x1的初始值，應(yīng)用優(yōu)化算法，優(yōu)化x1，使得目標(biāo)函數(shù)趨于最小，最終得到的(x1，y1)和(x2，y2)就是用來擬合曲線段的最佳兩個圓弧的圓心坐標(biāo)。