[發(fā)明專利]一種改進型三次樣條插值曲線擬合路徑點方法

在審

• 申請?zhí)枺?/td>: 201911073011.1
• 申請日: 2019-11-05
• 公開（公告）號: CN110823238A
• 公開（公告）日: 2020-02-21
• 發(fā)明（設計）人: 徐彪;楊帆;邊有鋼;謝國濤;秦曉輝;王曉偉;秦兆博;胡滿江;丁榮軍
• 申請（專利權(quán)）人: 湖南大學
• 主分類號: G01C21/34
• 分類號: G01C21/34
• 代理公司: 廣州容大專利代理事務所(普通合伙) 44326
• 代理人: 劉新年

本發(fā)明公開了一種改進型三次樣條插值曲線擬合路徑點方法，包括：步驟一，通過圖搜索算法得到離散路徑點組成的路徑；步驟二，對相鄰兩個路徑點使用改進型三次樣條插值曲線方法進行插值，得到高密度的連續(xù)路徑點；步驟三，將由各段三次樣條插值曲線得到的插值形成的高密度的路徑點依次連接，生成路徑。本發(fā)明的改進型三次樣條插值曲線擬合路徑點方法，通過步驟一至三的設置，便可有效的利用樣條插值的方式實現(xiàn)擬合路徑點的效果了。

技術(shù)領(lǐng)域

本發(fā)明涉及智能汽車路徑規(guī)劃技術(shù)領(lǐng)域，更具體的說是涉及一種改進型三次樣條插值曲線擬合路徑點方法。

背景技術(shù)

智能汽車由于其具有提高道路交通的安全性、經(jīng)濟性以及提高出行便利性的潛能，越來越受到人們的關(guān)注。智能汽車的路徑規(guī)劃是決策鄰域研究的重點，負責為智能汽車規(guī)劃出一條安全可行的行駛路徑。智能汽車在滿足非完整約束的條件下，在包含障礙物信息及可行駛空間的構(gòu)型空間中，通過路徑規(guī)劃算法可以得到一系列離散的路徑點，這些路徑點滿足車輛運動學約束、無碰撞的要求。如何通過路徑點擬合方法將各離散路徑點通過曲線擬合得到滿足車輛非完整約束、不與空間中的障礙物發(fā)生碰撞的連續(xù)路徑是路徑規(guī)劃的關(guān)鍵問題。

目前常用的路徑擬合方法中，主要用到三次貝塞爾曲線擬合方法和B樣條曲線擬合方法。在專利文件CN 106651987 A，2017.05.10中，使用三次貝塞爾曲線方法在每個路徑點前后的同一直線上插入兩個路徑點為路徑曲線提供方向信息，直線的斜率代表該路徑點的方向，使得生成的曲線路徑在各路徑點上滿足方向要求。在專利文件CN109176504 A，2019.01.11中，給定一組路徑控制點的坐標可擬合得到一條曲線路徑，曲線路徑的形狀由給定的路徑點坐標控制。此外也可利用傳統(tǒng)三次樣條插值曲線擬合方法，利用各連續(xù)路徑點的坐標信息可求得各相鄰路徑點之間三次樣條插值曲線表達式，再通過對橫坐標等距離離散得到插值路徑點。

上述路徑擬合方法雖然能根據(jù)給定路徑點擬合出連續(xù)的路徑曲線，但仍然存在很多局限性。對于B樣條曲線擬合方法生成的曲線路徑雖然能保證曲線平滑性，但各路徑點不包含角度信息，不能保證滿足車輛運動學的最大轉(zhuǎn)向角約束；對于三次貝塞爾曲線雖然可通過預先計算出路徑點前后插入的路徑點的位置以滿足路徑點角度要求，但對每個路徑點前后進行路徑點插入計算量大；對于傳統(tǒng)三次樣條插值曲線方法，當出現(xiàn)兩個路徑點在橫軸或縱軸方向上靠的過近、自變量反方向變化的情況，傳統(tǒng)的三次樣條曲線擬合出的曲線路徑將會出現(xiàn)任意大的震蕩畸變(也稱為“龍格現(xiàn)象”)、相鄰兩段曲線在銜接處一階導數(shù)不連續(xù)的現(xiàn)象，導致生成的曲線路徑不滿足車輛的可行駛性。

發(fā)明內(nèi)容

針對現(xiàn)有技術(shù)存在的不足，本發(fā)明的目的在于提供一種改進型三次樣條插值曲線路徑擬合方法，用于生成滿足車輛運動學約束、無震蕩畸變、一階導數(shù)連續(xù)的曲線路徑。

為實現(xiàn)上述目的，本發(fā)明提供了如下技術(shù)方案：一種改進型三次樣條插值曲線擬合路徑點方法，包括：

步驟S1，通過圖搜索算法得到離散路徑點組成的路徑；

步驟S2，對相鄰兩個路徑點使用改進型三次樣條插值曲線方法進行插值，得到高密度的連續(xù)路徑點；

步驟S3，將由各段三次樣條插值曲線得到的插值形成的高密度的連續(xù)路徑點依次連接，生成連續(xù)路徑。

作為本發(fā)明的進一步改進，所述步驟S1中各路徑點除了包含車輛位于該路徑點的坐標信息外還包含位于該點的車輛橫擺角信息。

作為本發(fā)明的進一步改進，所述步驟S2中相鄰兩個路徑點間的三次樣條插值曲線及插值點的計算步驟如下：

步驟S21，從起始路徑點開始，分別根據(jù)原坐標系下相鄰兩個路徑點的橫擺角信息計算出坐標系旋轉(zhuǎn)角度(對于各相鄰路徑點都需要依次進行坐標軸旋轉(zhuǎn))；

步驟S22，將原坐標系以步驟S21計算得到的旋轉(zhuǎn)角度進行旋轉(zhuǎn)(逆時針為旋轉(zhuǎn)正方向)，分別計算坐標系旋轉(zhuǎn)后各相鄰兩路徑點新的坐標和橫擺角；