一種人形機(jī)器人北斗導(dǎo)航自適應(yīng)規(guī)劃技術(shù)研究

李 峰，岳 云，張 巍，李 寧

（1.中原工學(xué)院信息商務(wù)學(xué)院機(jī)械工程系，河南 鄭州 450007；2.鄭州宇通集團(tuán)有限公司，河南 鄭州 450007）

1 引言

隨著社會的發(fā)展，機(jī)器人特別是人形機(jī)器人智能化程度不斷提高，人形機(jī)器人由于結(jié)構(gòu)更接近于人類，可協(xié)助人類做更多的事情，比如在車輛無人駕駛、特殊環(huán)境生產(chǎn)、航空航天方面，人形機(jī)器人的使用可以節(jié)約大量的人力、物力、財力，大大提高社會生產(chǎn)力。人類期望機(jī)器人能夠扮演更加智能的角色，能夠更加穩(wěn)定的完成任務(wù)，能夠在更廣大的范圍內(nèi)完成角色，自適應(yīng)性更高。隨著5G移動網(wǎng)絡(luò)技術(shù)的成形，可以通過網(wǎng)絡(luò)技術(shù)確定機(jī)器人的作業(yè)空間和運(yùn)動環(huán)境，協(xié)調(diào)機(jī)器人的運(yùn)動，控制機(jī)器人的工作，提高機(jī)器人的智能化程度，但提高機(jī)器人智能化程度的基礎(chǔ)是解決機(jī)器人在不同的導(dǎo)航環(huán)境下進(jìn)行路徑規(guī)劃選擇問題。機(jī)器人運(yùn)動狀態(tài)管理，機(jī)器人的運(yùn)動路線決策，是自動化控制技術(shù)、人工智能技術(shù)、衛(wèi)星導(dǎo)航技術(shù)等高科技技術(shù)的充分融合[1-3]。文獻(xiàn)[4-7]對人形機(jī)器人動作方式、結(jié)構(gòu)方面做了相關(guān)的研究，文獻(xiàn)[8-11]在機(jī)器人導(dǎo)航方面方面做了相關(guān)研究，但在人形機(jī)器人衛(wèi)星導(dǎo)航方面卻未有研究發(fā)現(xiàn)。而人形機(jī)器人的衛(wèi)星導(dǎo)航，特別是在我國北斗導(dǎo)航系統(tǒng)下的導(dǎo)航問題的研究一方面可以提高人形機(jī)器人的智能程度，一方面可以進(jìn)一步推廣普及我國自主研發(fā)的北斗導(dǎo)航系統(tǒng)。

針對人形機(jī)器人自適應(yīng)程度低，機(jī)器人行走智能化程度不高，特提出了利用自適應(yīng)控制法的來實現(xiàn)移動機(jī)器人行走路線及姿態(tài)的自適應(yīng)規(guī)劃。通過對人形機(jī)器人行走路線及位姿的優(yōu)化設(shè)計及自主決策，達(dá)到了機(jī)器人通過衛(wèi)星導(dǎo)航，自主決定運(yùn)動路線及運(yùn)動策略，實現(xiàn)了預(yù)定目標(biāo)，縮短了控制時間，提高了控制效率，增強(qiáng)了人形機(jī)器人控制的智能化程度。

2 人形機(jī)器人導(dǎo)航自適應(yīng)控制系統(tǒng)設(shè)計

2.1 人形機(jī)器人

人形機(jī)器人種類繁多，根據(jù)動力源可分為液壓驅(qū)動、電機(jī)驅(qū)動等，根據(jù)自由度也可分為多種自由度的機(jī)器人。本課題所用機(jī)器人為九自由度舵機(jī)驅(qū)動的人形機(jī)器人，如圖1所示。機(jī)器人轉(zhuǎn)角分辨率10，直線行走分辨率50mm，整體500g，采用2000ma鋰電續(xù)航，續(xù)航能力強(qiáng)。機(jī)器人結(jié)構(gòu)勻稱，更符合人形機(jī)構(gòu)。現(xiàn)在人形機(jī)器人的控制主要依靠前期對機(jī)器人進(jìn)行程序設(shè)定，將設(shè)計好的動作輸入機(jī)器人內(nèi)部存儲器，依靠固化程序?qū)崿F(xiàn)機(jī)器人完成特定的簡單動作，人形機(jī)器人自適應(yīng)程度不高，智能化程度較低。

圖1 九自由度人形機(jī)器人Fig.1 Nine Degree of Freedom Humanoid Robot

2.2 九自由度人形機(jī)器人自適應(yīng)導(dǎo)航系統(tǒng)硬件設(shè)計

為了能更好地對九自由度人形機(jī)器人進(jìn)行導(dǎo)航，提高對人形機(jī)器人導(dǎo)航的智能化程度，增強(qiáng)機(jī)器人的自適應(yīng)性，特對原九自由度人形機(jī)器人自導(dǎo)航主動控制系統(tǒng)進(jìn)行優(yōu)化改進(jìn)，增加導(dǎo)航航向路線自適應(yīng)規(guī)劃功能，原機(jī)器人導(dǎo)航系統(tǒng)雖然能夠在完成目的地坐標(biāo)輸入后自動生成導(dǎo)航軌跡，但是航向調(diào)整智能化程度不高，人性化程度較低；另外，機(jī)器人在經(jīng)過導(dǎo)航后，到達(dá)目的坐標(biāo)區(qū)域后不能很好地確定是否達(dá)到目標(biāo)，不能再做最終的位姿調(diào)整，導(dǎo)航精度低。為此，特對系統(tǒng)進(jìn)行優(yōu)化設(shè)計，以提高系統(tǒng)的自適應(yīng)性。人形機(jī)器人自適應(yīng)導(dǎo)航系統(tǒng)硬件主要包括人形移動機(jī)器人，自導(dǎo)航主動控制器，北斗導(dǎo)航儀，監(jiān)控微機(jī)。系統(tǒng)中機(jī)器人所在位置通過北斗導(dǎo)航儀獲得，通過人機(jī)界面輸入中央處理器，通過中央處理器處理后自動生成導(dǎo)航路線、軌跡，又通過中間控制器發(fā)出信號控制機(jī)器人實現(xiàn)導(dǎo)航的路線、軌跡，機(jī)器人在運(yùn)行過程中可通過視覺采集系統(tǒng)將信號通過微機(jī)直觀地展示給監(jiān)控人員，以便于對一些特殊情況進(jìn)行處理，機(jī)器人本身也可搭載障礙物、深坑信息采集裙，以便及時對特殊路徑做出規(guī)劃。本試驗在不改變原有就自由度人形機(jī)器人自導(dǎo)航主動控制試驗平臺的基礎(chǔ)上對機(jī)器人自導(dǎo)航主動控制原理進(jìn)行改進(jìn)，以提高機(jī)器人的自適應(yīng)程度。

人形機(jī)器人自適應(yīng)主動控制系統(tǒng)中，北斗定位系統(tǒng)，自適應(yīng)規(guī)劃系統(tǒng)和被導(dǎo)航機(jī)器人為三大基本組成部分，如圖2所示。北斗導(dǎo)航定位系統(tǒng)是機(jī)器人自導(dǎo)航的依據(jù)，課題使用的是ICE610冰河北斗導(dǎo)航儀，可以完成對人形機(jī)器人的北斗系統(tǒng)定位。自適應(yīng)規(guī)劃系統(tǒng)主要由人機(jī)界面，中央處理器和中間控制器組成，人機(jī)界面主要采用顯控SK—070AS 7寸工業(yè)觸摸屏，中央處理器采用比較常見的西門子S7—200PLC完成，它與工控微機(jī)和障礙物及深坑探測模塊連接，完成信息通訊。中間控制器為自己設(shè)計研發(fā)，主要完成中央處理器輸出信號和人形機(jī)器人控制信號的轉(zhuǎn)換，實現(xiàn)人形機(jī)器人的自適應(yīng)主動導(dǎo)航。人形機(jī)器人為九自由度人形機(jī)器人，通過QROBRT軟件進(jìn)行程序輸入，完成機(jī)器動作分解程序設(shè)定。

圖2 人形機(jī)器人自適應(yīng)主動控制系統(tǒng)圖Fig.2 Humanoid Robot Adaptive Active Control System Picture

3 人形機(jī)器人導(dǎo)航算法設(shè)計

3.1 九自由度人形機(jī)器人運(yùn)動學(xué)模型

為得到九自由度人形機(jī)器人的自導(dǎo)航運(yùn)動控制算法，需要分析機(jī)器人的行走移動軌跡，為此需要對機(jī)器人腳部移動狀況進(jìn)行分析研究，建立機(jī)器人移動模型，如圖3所示。

圖3 機(jī)器人移動模型Fig.3 Robot Mobile Model

圖中：α—機(jī)器人兩腳與機(jī)器人縱軸yROBERT之間的夾角，為轉(zhuǎn)向角；ψ—yROBERT與地面坐標(biāo)系yEARTH之間的夾角，為航向角；yROBERT—兩腳連線的垂直中心線方向；yEARTH—地球正北方向；（xc，yc）—機(jī)器人當(dāng)前坐標(biāo)；（xg，yg）—機(jī)器人目的地坐標(biāo)；ψh—行走過程中的機(jī)器人的航向角。由現(xiàn)在的坐標(biāo)和目的地坐標(biāo)得到：

為便于控制，令t=n*T可將△T設(shè)置為控制間隔時間T，則：

為簡化公式，令：

式中：V—機(jī)器人直線行走速度。因為人形機(jī)器人每直線行走信號行走理論值由自身結(jié)構(gòu)決定，為△S，給定機(jī)器人直線行走信號數(shù)nx，則機(jī)器人直線行走控制方程為：

因為轉(zhuǎn)向角可以控制，令轉(zhuǎn)向角：

式中：K—誤差系數(shù)，可初定為1；nz—機(jī)器人轉(zhuǎn)向信號數(shù)；△α—單個轉(zhuǎn)向信號機(jī)器人可轉(zhuǎn)角度。

式中：ψc—初始姿態(tài)；ψg—終點(diǎn)設(shè)定姿態(tài)；ψh—航向角；nz—機(jī)器人從初始姿態(tài)達(dá)到航向所生成的航向調(diào)整轉(zhuǎn)向信號數(shù)；nq—機(jī)器人從航向角達(dá)到終點(diǎn)設(shè)定姿態(tài)所生成的姿態(tài)調(diào)整轉(zhuǎn)向信號數(shù)；△α—單個轉(zhuǎn)向信號機(jī)器人可轉(zhuǎn)角度。

3.2 機(jī)器人自適應(yīng)路徑規(guī)劃控制過程設(shè)計

為了提高人形機(jī)器人的自適應(yīng)性，系統(tǒng)設(shè)計該人形機(jī)器人北斗導(dǎo)航自適應(yīng)主動控制不但能夠在輸入機(jī)器人現(xiàn)在所處地點(diǎn)北斗坐標(biāo)和目標(biāo)地點(diǎn)北斗坐標(biāo)后自動生成導(dǎo)航路線，發(fā)出指令，控制機(jī)器人自動完成任務(wù)，而且能夠自適應(yīng)選擇最佳人形機(jī)器人生成路徑的姿態(tài)，以便優(yōu)化動作程度，提高人工智能程度。機(jī)器人自適應(yīng)主動控制系統(tǒng)在輸入相關(guān)信息后，先根據(jù)所處位置和目標(biāo)位置自動生成行走路線；然后根據(jù)自身位姿和所需航向，自適應(yīng)選擇左調(diào)、右調(diào)自身姿態(tài)，達(dá)到所需航向；接著，根據(jù)航向和前期生成路線移動，完成移動軌跡；再根據(jù)最終需要姿態(tài)自適應(yīng)擬人化左調(diào)、右調(diào)姿態(tài)。而后，根據(jù)北斗導(dǎo)航信息校核距離誤差和姿態(tài)誤差，自適應(yīng)調(diào)整位置、姿態(tài)，完成任務(wù)，如圖4所示。具體步驟如下：

圖4 人形機(jī)器人北斗導(dǎo)航自適應(yīng)主動控制流程圖Fig.4 Humanoid Robot Beidou Navigation Adaptive Active Control Flowchart

（1）輸入（xc，yc）、（xg，yg）、ψc、ψg、TZ、Tx、△α、△s、K、Kx、Kq、Kz，其中：（xc，yc）—機(jī)器人初始點(diǎn)國家2000坐標(biāo)；（xg，yg）—機(jī)器人目標(biāo)點(diǎn)國家2000坐標(biāo)；ψc—機(jī)器人初始位置姿態(tài)；ψg—機(jī)器人到達(dá)目標(biāo)位置后需要確定的姿態(tài)；K—整體調(diào)整誤差系數(shù)；Kx—直線調(diào)整誤差系數(shù)；Kq—姿態(tài)調(diào)整誤差系數(shù)；Kz—航向調(diào)整誤差系數(shù)；nz—給定機(jī)器人轉(zhuǎn)向信號數(shù)；nx—給定機(jī)器人直線信號數(shù)；nq—給定機(jī)器人姿態(tài)信號數(shù)；△α—單個信號機(jī)器人調(diào)整角度；△s—單個信號機(jī)器人調(diào)整位移；TZ—轉(zhuǎn)向信號信號時間間隔；Tx—直線行走信號時間間隔。

（2）機(jī)器人行走路線主要參數(shù)nz、nx、nq規(guī)劃。根據(jù)前文中確定參數(shù)和式α=Kz×（ψg-ψc），確定人形機(jī)器人行走時所需器人行走時所需轉(zhuǎn)角α，又ɑ=Kz×（ψg-ψc）=nz×△α，根據(jù)前期測定的機(jī)器人每信號調(diào)整角度△α，得需給定機(jī)器人轉(zhuǎn)向信號數(shù)nz=α/△α，利用STL語言將公式程序化（見圖5中nz規(guī)劃程序段）；

根據(jù)前文中確定參數(shù)和S=Kx×（（yg-yc）∧2+（xg-xc）∧2）∧0.5，確定人形機(jī)器人行走時所需行走位移S，根據(jù)前期測定的機(jī)器人每信號調(diào)整位移△s，得需給定機(jī)器人行走信號數(shù)nx=S/△s，利用STL語言將公式程序化（見圖5中nx規(guī)劃程序段）；

根據(jù)前文中確定參數(shù)和ɑq=Kq×（ψg-ψc）=nq×△α，根據(jù)前期測定的機(jī)器人每信號調(diào)整姿態(tài)△α，得需給定機(jī)器人姿態(tài)調(diào)整信號數(shù)nq=α/△α，利用STL語言將公式程序化（見圖5中nq規(guī)劃程序段）；

（3）控制器發(fā)出機(jī)器人轉(zhuǎn)向信號nz次，每次間隔TZ，機(jī)器人完成航向調(diào)整，控制原理見圖5中航向調(diào)整程序段；

（4）控制器發(fā)出機(jī)器人直行信號nx次，每次間隔Tx，機(jī)器人完成位移運(yùn)動，控制原理見圖5中前行程序段；

（5）控制器發(fā)出機(jī)器人姿態(tài)調(diào)整信號nq次，每次間隔TZ，機(jī)器人完成姿態(tài)調(diào)整，控制原理見圖5中姿態(tài)調(diào)整程序段；

以上為前期人形機(jī)器人行走路線自適應(yīng)生成及動作過程，以下為人形機(jī)器人校核位置、姿態(tài)，自適應(yīng)調(diào)整位置姿態(tài)過程。

（6）根據(jù)人形機(jī)器人現(xiàn)在所在地（xc，yc）、ψc，比較距離，看是否到達(dá)目的坐標(biāo)范圍。如果達(dá)到目標(biāo)范圍，SS≤R，比較姿態(tài)，看是否到達(dá)目的坐標(biāo)姿態(tài)范圍δ，控制原理見圖5中姿態(tài)再調(diào)整程序段。

圖5 人形機(jī)器人北斗導(dǎo)航自適應(yīng)主動控制主要原理Fig.5 Humanoid Robot Beidou Navigation Adaptive Active Control Main Principle

①如果達(dá)到目標(biāo)位移范圍后姿態(tài)調(diào)整。

如果姿態(tài)達(dá)到：|ψc－ψc|≤δ，停止，退出程序；

如果姿態(tài)達(dá)到：|ψg－ψc|>δ，需調(diào)整姿態(tài)。

接下來：

如果ψg>ψc，自適應(yīng)生成給定機(jī)器人轉(zhuǎn)向信號數(shù)nq，控制器發(fā)出機(jī)器人左轉(zhuǎn)向信號nq次，每次間隔Tz，機(jī)器人完成姿態(tài)調(diào)整；

如果ψg<ψc，自適應(yīng)生成機(jī)器人轉(zhuǎn)向信號數(shù)nq，控制器發(fā)出機(jī)器人右轉(zhuǎn)向信號nq次，每次間隔Tz，機(jī)器人完成姿態(tài)調(diào)整；

②如果未達(dá)到目標(biāo)位移范圍，自適應(yīng)調(diào)整航向。

調(diào)整航向生成ɑ。根據(jù)：ɑ=Kz×（ψg-ψc）=nz×△ɑ，確定九自由度人形機(jī)器人行走時所需轉(zhuǎn)角ɑ，根據(jù)機(jī)器人內(nèi)部確定的△ɑ確定機(jī)器人轉(zhuǎn)向信號數(shù)nz；

如果ψg>ψc，求得需給定機(jī)器人轉(zhuǎn)向信號數(shù)，控制器發(fā)出機(jī)器人左轉(zhuǎn)向信號nz次，每次間隔Tz，機(jī)器人完成航向調(diào)整；如果ψg<ψc，求得需給定機(jī)器人轉(zhuǎn)向信號數(shù)nz，控制器發(fā)出機(jī)器人右轉(zhuǎn)向信號nz次，每次間隔Tz，機(jī)器人完成航向調(diào)整；

確定九自由度人形機(jī)器人行走時所需行走位移，根據(jù)機(jī)器人內(nèi)部確定的△s和公式，求得需給定機(jī)器人轉(zhuǎn)向信號數(shù)nx，控制器發(fā)出機(jī)器人轉(zhuǎn)向信號nx次，每次間隔Tx，機(jī)器人完成直線移動；

根據(jù)ɑq=Kq×（ψg-ψc）=nq×△ɑ和機(jī)器人內(nèi)部確定的△ɑ，求得需給定機(jī)器人姿態(tài)調(diào)整信號數(shù)nq。

如果ψg>ψc，求得需給定機(jī)器人轉(zhuǎn)向信號數(shù)nq，控制器發(fā)出機(jī)器人左轉(zhuǎn)向信號nq次，每次間隔Tz，機(jī)器人完成姿態(tài)調(diào)整；如果ψg<ψc求得需給定機(jī)器人轉(zhuǎn)向信號數(shù)nq，控制器發(fā)出機(jī)器人右轉(zhuǎn)向信號nq次，每次間隔Tz，機(jī)器人完成姿態(tài)調(diào)整。根據(jù)以上控制流程，人形機(jī)器人可完成行走路線自適應(yīng)規(guī)劃過程，并調(diào)整好最終姿態(tài)，達(dá)到設(shè)定任務(wù)。

3.3 主要運(yùn)動控制原理

根據(jù)人形機(jī)器人北斗導(dǎo)航自適應(yīng)主動控制原理，操作人員先將不同的數(shù)據(jù)、參數(shù)存儲到不同的地址，然后主動控制器自動生成導(dǎo)航軌跡、姿態(tài)，并將主要參數(shù)存入固定的寄存器中。接下來，人形機(jī)器人按照自動生成的軌跡先根據(jù)航向要求自適應(yīng)比較，調(diào)整航向。航向調(diào)整結(jié)束后，根據(jù)生成軌跡一步一步朝著目的地步行，在完成直線動作后，繼續(xù)按照前面生成的姿態(tài)調(diào)整要求調(diào)整姿態(tài)。這時，人形機(jī)器人自適應(yīng)主動控制系統(tǒng)根據(jù)現(xiàn)在所處位置自適應(yīng)比較距離和姿態(tài)，生成調(diào)整路線、姿態(tài)，并完成調(diào)整。利用stl語言描述了人形機(jī)器人北斗導(dǎo)航自適應(yīng)主動控制的主要控制原理，如圖5所示。根據(jù)圖5原理，結(jié)合圖2人形機(jī)器人北斗導(dǎo)航自適應(yīng)主動控制硬件系統(tǒng)，可實現(xiàn)人形機(jī)器人的自適應(yīng)導(dǎo)航控制。

4 人形機(jī)器人導(dǎo)航試驗研究

4.1 人形機(jī)器人行走路線自適應(yīng)規(guī)劃試驗簡介

根據(jù)試驗所建立的環(huán)境，依據(jù)北斗導(dǎo)航系統(tǒng)，測得試驗室機(jī)器人起點(diǎn)所在坐標(biāo)，分別設(shè)定兩個目的坐標(biāo)，每個目的坐標(biāo)設(shè)計兩個姿態(tài)，以此來進(jìn)行試驗。

試驗?zāi)康囊环矫鏅z驗人形機(jī)器人北斗導(dǎo)航能否實現(xiàn)導(dǎo)航軌跡自生成，另一方面觀察導(dǎo)航系統(tǒng)的自適應(yīng)性，其中理想軌跡坐標(biāo)為機(jī)器人自導(dǎo)航系統(tǒng)在沒有原理誤差和系統(tǒng)誤差的情況下應(yīng)該行走的軌跡，而仿真軌跡為機(jī)器人經(jīng)過機(jī)器人自適應(yīng)導(dǎo)航主動控制系統(tǒng)分析運(yùn)算，自動生成的軌跡曲線，是機(jī)器人理論上經(jīng)過自適應(yīng)導(dǎo)航主動控制系統(tǒng)控制后應(yīng)該走的路線、軌跡、航向、姿態(tài)，而實際軌跡為機(jī)器人自適應(yīng)北斗導(dǎo)航試驗人形機(jī)器人實際的軌跡及姿態(tài)。試驗時，首先，對人形機(jī)器人北斗導(dǎo)航自適應(yīng)控制系統(tǒng)進(jìn)行初始化設(shè)置，輸入相關(guān)參數(shù)，如表1、表2所示。

表1 試驗用人形機(jī)器人相關(guān)參數(shù)Tab.1 Test Humanoid Robot Related Parameters

表2 人形機(jī)器人自適應(yīng)北斗導(dǎo)航試驗初始設(shè)置相關(guān)參數(shù)Tab.2 Humanoid Robot Adaptive Test Initial Setting Parameters

然后，將機(jī)器人放入設(shè)定初始坐標(biāo)位置，并設(shè)好初始姿態(tài)。接著，啟動自適應(yīng)導(dǎo)航系統(tǒng)，人形機(jī)器人開始按照導(dǎo)航系統(tǒng)自動生成路線運(yùn)行，主要參數(shù)，如表3所示。

表3 人形機(jī)器人自適應(yīng)北斗導(dǎo)航試驗主要輸出Tab.3 Humanoid Robot Adaptive Test Main Output

試驗過程中，機(jī)器人每走一步，對其坐標(biāo)位置及姿態(tài)進(jìn)行記錄。為了驗證機(jī)器人能否人性化自動選擇左右調(diào)整姿態(tài)，設(shè)定目標(biāo)北斗坐標(biāo)（3835534419.3，1974867272.3）下兩個姿態(tài)，一個姿態(tài)為-34°，即北偏東56°，另一個姿態(tài)為-73°，即北偏東17°。因為從表4、表5 可以看出人形機(jī)器人理想狀態(tài)下中間航向為-53°，即北偏東37°，如果人形機(jī)器人能夠更人性化自動選擇左右調(diào)整姿態(tài)，則機(jī)器人在終點(diǎn)調(diào)整姿態(tài)時，終點(diǎn)姿態(tài)為-34°的會自動左轉(zhuǎn)，而終點(diǎn)姿態(tài)為-73°的會自動右轉(zhuǎn)，而不是一直左轉(zhuǎn)到設(shè)定姿態(tài)。

4.2 人形機(jī)器人自適應(yīng)北斗導(dǎo)航試驗參數(shù)、曲線及分析

人形機(jī)器人自適應(yīng)北斗導(dǎo)航試驗在目標(biāo)坐標(biāo)為北斗坐標(biāo)（3835534419.3，1974867272.3），最終目標(biāo)姿態(tài)為-34°和-73°下自動生成的航向調(diào)整、直線移動、姿態(tài)調(diào)整信號數(shù)，如表4、表5所示。是導(dǎo)航路線生成的主要階段，如果機(jī)器人在行走過程中沒有誤差及相關(guān)問題，人形機(jī)器人可以直接到達(dá)目的地并實現(xiàn)所要求姿態(tài)。但實際上由于誤差，人形機(jī)器人在完成姿態(tài)調(diào)成后實際位置和姿態(tài)與原要求還有差距，如果與設(shè)定目標(biāo)的距離及姿態(tài)小于允許誤差，則認(rèn)為機(jī)器人完成任務(wù)，如果距離或者姿態(tài)與設(shè)定的目標(biāo)位置距離、姿態(tài)相差大于允許誤差，則需要人形機(jī)器人根據(jù)現(xiàn)在所處位置、姿態(tài)，進(jìn)行位姿自適應(yīng)調(diào)整。由于本試驗后期人形機(jī)器人自適應(yīng)調(diào)整信號數(shù)據(jù)較多，如表6所示。因此整個導(dǎo)航過程分導(dǎo)航前期和后期兩個階段介紹。

表4 機(jī)器人自適應(yīng)北斗導(dǎo)航試驗坐標(biāo)、姿態(tài)參數(shù)（ψg -34°）Tab.4 Robot Adaptive Beidou Navigation Test Coordi?nates/Attitude Parameters（ψg -34°）

表4 （續(xù)）機(jī)器人自適應(yīng)北斗導(dǎo)航試驗坐標(biāo)、姿態(tài)參數(shù)（ψg -34°）Tab.4 （Cont）Robot Adaptive Beidou Navigation Test Co?ordinates/Attitude Parameters（ψg -34°）

表5 機(jī)器人自適應(yīng)北斗導(dǎo)航試驗坐標(biāo)、姿態(tài)參數(shù)（ψg -73°）Tab.5 Robot Adaptive Beidou Navigation Test Coordi?nates/Attitude Parameters（ψg -73°）

理想軌跡坐標(biāo)、姿態(tài)為理想情況下，人形機(jī)器人動作時在沒有誤差的情況下，依據(jù)自適應(yīng)導(dǎo)航發(fā)出的信號數(shù)，理想情況下應(yīng)該所處的北斗坐標(biāo)和姿態(tài)；仿真軌跡坐標(biāo)、姿態(tài)為人形機(jī)器人動作時在沒有誤差的情況下，依據(jù)自適應(yīng)導(dǎo)航發(fā)出的信號數(shù)，根據(jù)表2中參數(shù)，人形機(jī)器人應(yīng)該所處的位置和姿態(tài)，他與理想軌跡坐標(biāo)、姿態(tài)的不同是因為人形機(jī)器人本身結(jié)構(gòu)等問題，因為人形機(jī)器人每信號轉(zhuǎn)動角度、直行位移、姿態(tài)調(diào)整角度理論上都是定值，不能夠像人一樣可以根據(jù)具體情況隨意調(diào)整，因此造成人形機(jī)器人自適應(yīng)導(dǎo)航系統(tǒng)自動生成的軌跡、姿態(tài)與理想軌跡、姿態(tài)由一定的差異，這種誤差是系統(tǒng)本身的誤差，是不可避免的。實際試驗時得到的軌跡坐標(biāo)、姿態(tài)是在人形機(jī)器人動作時實時測量、記錄機(jī)器人的行走軌跡、姿態(tài)而得到的機(jī)器人的真實的行走路線、姿態(tài)。

雖然初始坐標(biāo)、目標(biāo)坐標(biāo)相同，但終點(diǎn)要求姿態(tài)不同，如表3中所示，輸出信號數(shù)經(jīng)過內(nèi)部規(guī)劃數(shù)據(jù)相同，但信號數(shù)相同不代表人形機(jī)器人動作時左、右轉(zhuǎn)選擇不同，這需要通過試驗軌跡，姿態(tài)測定。

根據(jù)表4和表5所得數(shù)據(jù)可以得到終點(diǎn)姿態(tài)-34°和-73°時人形機(jī)器人自適應(yīng)北斗導(dǎo)航軌跡、姿態(tài)圖，如圖6、圖7所示。圖中分別由三條軌跡、姿態(tài)，一條為機(jī)器人自適應(yīng)導(dǎo)航理想軌跡，一條為仿真軌跡，一條為導(dǎo)航試驗前期在終點(diǎn)位置沒有自適應(yīng)調(diào)整的曲線。

圖6 機(jī)器人自適應(yīng)北斗導(dǎo)航前期軌跡、姿態(tài)（ψg-34°）Fig.6 Robot Adaptive Beidou Navigation Early Trajectory/Attitude（ψg-34°）

圖7 機(jī)器人自適應(yīng)北斗導(dǎo)航前期軌跡、姿態(tài)（ψg -73°）Fig.7 Robot Adaptive Beidou Navigation Early Trajectory/Attitude（ψg-73°）

因為導(dǎo)航初始位置和目標(biāo)位置坐標(biāo)相同，因此人形機(jī)器人理想軌跡和仿真軌跡大致相同，但由于終點(diǎn)要求位姿不同，圖6為要求姿態(tài)-34°，要求姿態(tài)-73°，如圖7所示。兩種要求下人形機(jī)器人在終點(diǎn)位置時姿態(tài)變化不同，圖6為東偏北轉(zhuǎn)向，圖7東偏南轉(zhuǎn)向。雖然轉(zhuǎn)向信號數(shù)由于偶然相同，如表4、表5所示。

但是每轉(zhuǎn)向信號使機(jī)器人動作不同，人形機(jī)器人不再單一通過左轉(zhuǎn)或右轉(zhuǎn)實現(xiàn)要求姿態(tài)，而是左轉(zhuǎn)、右轉(zhuǎn)結(jié)合，自適應(yīng)實現(xiàn)要求姿態(tài)，這在兩圖中理論、仿真、實際軌跡、姿態(tài)中都得到了證實。因為理論、仿真軌跡、姿態(tài)中是按照理想狀況下得到的軌跡、姿態(tài)，因此不需要進(jìn)行后期自適應(yīng)校核位置、姿態(tài)進(jìn)行調(diào)整位置、姿態(tài)。

但從兩圖實際軌跡、姿態(tài)來看，人形機(jī)器人實際運(yùn)動終點(diǎn)位置、姿態(tài)與設(shè)定目標(biāo)位置、姿態(tài)還有較大差距，圖6終點(diǎn)位置坐標(biāo)為（3835534458.5，1974867406.7），姿態(tài)-20°，與設(shè)定位置（3835534419.3，1974867272.3），姿態(tài)-34°，相距140mm，姿態(tài)相差14°，大于距離允許R的50mm，姿態(tài)允許δ的10°，因此需要后期校核位置，自適應(yīng)調(diào)整姿態(tài)、位置過程；圖7 終點(diǎn)位置坐標(biāo)為（3835534440，1974867352），姿態(tài)-66°，與設(shè)定位置（3835534419.3，1974867272.3），姿態(tài)-73°，相距82mm，姿態(tài)相差7°，距離大于距離允許R的50mm，因此需要后期校核位置，自適應(yīng)調(diào)整姿態(tài)、位置過程。

從兩圖中機(jī)器人仿真軌跡、姿態(tài)和實際行走軌跡、姿態(tài)可以看出，機(jī)器人行走軌跡與理想行走軌跡重合度較高，理論上和實際上都已經(jīng)驗證了人形機(jī)器人北斗導(dǎo)航可以實現(xiàn)人形機(jī)器人的自適應(yīng)規(guī)劃控制。根據(jù)實際實驗結(jié)果，終點(diǎn)姿態(tài)-34°和-73°度機(jī)器人后期經(jīng)校核位置，自適應(yīng)調(diào)整階段的試驗坐標(biāo)、姿態(tài)參數(shù)，如表6所示。

由于人形機(jī)器人在到達(dá)位置后與原要求的位置、姿態(tài)要求的誤差大于系統(tǒng)設(shè)定的任務(wù)終結(jié)條件，因此人形機(jī)器人需要進(jìn)行后期自適應(yīng)調(diào)整位置、姿態(tài)。兩條曲線終點(diǎn)自適應(yīng)調(diào)整階段信號數(shù)不同，如表6所示。

表6 終點(diǎn)姿態(tài)-34°和-73°機(jī)器人后期自適應(yīng)調(diào)整階段試驗坐標(biāo)、姿態(tài)參數(shù)Tab.6 Test Coordinates and Attitude Parameters of Robot During Adaptive Adjustment Phase（ψg-34°and -73°）

這與誤差的大小和姿態(tài)的要求不同有關(guān)。終點(diǎn)姿態(tài)-34°自適應(yīng)調(diào)整階段導(dǎo)航系統(tǒng)規(guī)劃給出18次航向調(diào)整信號，3次直線運(yùn)動信號，16次姿態(tài)調(diào)整信號，而終點(diǎn)姿態(tài)-73°自適應(yīng)調(diào)整階段導(dǎo)航系統(tǒng)規(guī)劃給出13次航向調(diào)整信號，2次直線運(yùn)動信號，12次姿態(tài)調(diào)整信號。調(diào)整階段，人形機(jī)器人先右轉(zhuǎn)調(diào)整航向，后直線運(yùn)動，再左轉(zhuǎn)調(diào)整姿態(tài)，如圖8、圖9所示。

這說明人形機(jī)器人可以根據(jù)實際情況自適應(yīng)選擇左右轉(zhuǎn)完成相關(guān)任務(wù)，智能化程度大大提高。終點(diǎn)位置坐標(biāo)為（3835534444.6，1974867287.0），姿態(tài)-37°，與設(shè)定位置（3835534419.3，1974867272.3），姿態(tài)-34°，相距29.3mm，姿態(tài)相差3°，小于距離允許R的50mm，姿態(tài)允許δ的10°，如圖8所示。

圖8 機(jī)器人自適應(yīng)北斗導(dǎo)航整體軌跡（ψg-34°）Fig.8 Overall Trajectory of Robot Adaptive Beidou Navigation（ψg-34°）

因此完成了任務(wù)，達(dá)到了導(dǎo)航目標(biāo)；終點(diǎn)位置坐標(biāo)為（3835534440.0，1974867281.0），姿態(tài)-79°，與設(shè)定位置（3835534419.3，1974867272.3），姿態(tài)-73°，相距82mm，姿態(tài)相差6°，距離小于距離允許R的50mm，如圖9所示。

圖9 機(jī)器人自適應(yīng)北斗導(dǎo)航整體軌跡（ψg-73°）Fig.9 Overall Trajectory of Robot Adaptive Beidou Navigation（ψg-73°）

因此完成了任務(wù)，達(dá)到了導(dǎo)航目標(biāo)。從試驗結(jié)果可以看出，導(dǎo)航系統(tǒng)的自適應(yīng)性較強(qiáng)，智能化程度較高，達(dá)到了人形機(jī)器人自導(dǎo)航主動控制系統(tǒng)優(yōu)化設(shè)計的目的，自適應(yīng)性得到了加強(qiáng)。

5 結(jié)論

（1）針對人形機(jī)器人衛(wèi)星導(dǎo)航下的行走路線自適應(yīng)規(guī)劃問題，從實際應(yīng)用出發(fā)，研發(fā)了一種可以利用北斗導(dǎo)航系統(tǒng)進(jìn)行人形機(jī)器人自適應(yīng)路徑規(guī)劃控制的主動控制系統(tǒng)。（2）通過代數(shù)學(xué)方法和幾何學(xué)方法，在運(yùn)動學(xué)分析的基礎(chǔ)上建立了人形機(jī)器人自適應(yīng)主動控制模型，得到了人形機(jī)器人位置、姿態(tài)、航向和人形機(jī)器人控制信號之間的關(guān)系。（3）通過stl 語言表達(dá)了人形機(jī)器人自適應(yīng)運(yùn)動控制的控制原理，介紹了導(dǎo)航控制過程。（4）通過導(dǎo)航試驗，驗證了運(yùn)動控制的控制原理和主動控制系統(tǒng)的正確性。導(dǎo)航試驗結(jié)果表明，所研發(fā)的主動控制系統(tǒng)可以實現(xiàn)人形機(jī)器人的自適應(yīng)路徑規(guī)劃控制導(dǎo)航，為后期人形機(jī)器人導(dǎo)航控制的研究搭建了良好的實驗平臺。