[發(fā)明專利]一種基于改進(jìn)人工勢(shì)場(chǎng)的足式機(jī)器人牽引控制方法及系統(tǒng)

有效

• 申請(qǐng)?zhí)枺?/td>: 201710258142.1
• 申請(qǐng)日: 2017-04-19
• 公開（公告）號(hào): CN108153298B
• 公開（公告）日: 2022-08-09
• 發(fā)明（設(shè)計(jì)）人: 許鵬;蘇波;姚其昌;黨睿娜;江磊;許威;蔣云峰;慕林棟;降晨星;鄧秦丹;康祖銘;楊超寧
• 申請(qǐng)（專利權(quán)）人: 中國(guó)北方車輛研究所
• 主分類號(hào): G05D1/02
• 分類號(hào): G05D1/02;G05D1/08;B62D57/032
• 代理公司: 中國(guó)兵器工業(yè)集團(tuán)公司專利中心 11011
• 代理人: 周恒

1.一種基于改進(jìn)人工勢(shì)場(chǎng)的足式機(jī)器人牽引控制方法，其特征在于：

構(gòu)建機(jī)器人目標(biāo)引力勢(shì)能場(chǎng)和障礙物斥力勢(shì)能場(chǎng)函數(shù)；

根據(jù)勢(shì)能場(chǎng)計(jì)算牽引控制的目標(biāo)和障礙物虛擬力，并計(jì)算環(huán)境產(chǎn)生的虛擬合力；

根據(jù)環(huán)境產(chǎn)生的虛擬合力，計(jì)算機(jī)器人的運(yùn)動(dòng)控制信號(hào)；

根據(jù)機(jī)器人的運(yùn)動(dòng)控制信號(hào)，計(jì)算機(jī)身位姿控制器的輸入；

其中，在構(gòu)建機(jī)器人目標(biāo)引力勢(shì)能場(chǎng)和障礙物斥力勢(shì)能場(chǎng)函數(shù)之前，還包括提取機(jī)器人作業(yè)環(huán)境信息；所述作業(yè)環(huán)境信息通過(guò)機(jī)器人搭載的感知系統(tǒng)實(shí)時(shí)對(duì)環(huán)境進(jìn)行識(shí)別，包括障礙物的識(shí)別和目標(biāo)的識(shí)別；

其中，所述構(gòu)建機(jī)器人目標(biāo)引力勢(shì)能場(chǎng)函數(shù)，包括：

實(shí)時(shí)估計(jì)機(jī)器人在世界坐標(biāo)系下的位置；

依靠感知系統(tǒng)實(shí)時(shí)獲取目標(biāo)的位置信息；

實(shí)時(shí)計(jì)算機(jī)器人與目標(biāo)距離，并引入機(jī)器人距離目標(biāo)的安全距離閾值及引力場(chǎng)比例因子；

依據(jù)機(jī)器人與目標(biāo)實(shí)時(shí)距離和安全距離的關(guān)系，構(gòu)建機(jī)器人目標(biāo)勢(shì)能場(chǎng)函數(shù)；

采用機(jī)載慣性測(cè)量單元實(shí)時(shí)獲取機(jī)器人在世界坐標(biāo)系下的平動(dòng)加速度，利用加速度二次積分的方法實(shí)時(shí)估計(jì)機(jī)器人在世界坐標(biāo)系下的位置；

依靠機(jī)載感知系統(tǒng)對(duì)目標(biāo)進(jìn)行識(shí)別，提取目標(biāo)特征信息和獲取目標(biāo)的深度信息，從而獲得目標(biāo)相對(duì)感知系統(tǒng)坐標(biāo)系下的位置p_c，根據(jù)機(jī)器人在世界坐標(biāo)系下的位置p，以及感知系統(tǒng)相對(duì)于機(jī)器人坐標(biāo)系的位置p_rc，進(jìn)而得到目標(biāo)在世界坐標(biāo)系下的位置p^*，p^*＝p_c+p+p_rc；

所述機(jī)器人目標(biāo)勢(shì)能場(chǎng)函數(shù)為：

其中：p為機(jī)器人在世界坐標(biāo)系下的位置，p^*為目標(biāo)的位置信息，d(p,p^*)為機(jī)器人與目標(biāo)的歐式距離d(p,p^*)＝||p-p^*||，d^*為機(jī)器人距離目標(biāo)的安全距離閾值，ζ為引力場(chǎng)比例因子，U_a(p)為機(jī)器人目標(biāo)勢(shì)能場(chǎng)函數(shù)；

其中，所述構(gòu)建機(jī)器人障礙物斥力勢(shì)能場(chǎng)函數(shù)，包括：

依靠感知系統(tǒng)實(shí)時(shí)獲取障礙物的位置信息；

實(shí)時(shí)計(jì)算機(jī)器人與障礙物的距離，并引入機(jī)器人距離障礙物的斥力有效閾值及斥力場(chǎng)比例因子；

依據(jù)機(jī)器人與障礙物的實(shí)時(shí)距離和斥力有效閾值的關(guān)系，構(gòu)建機(jī)器人障礙物勢(shì)能場(chǎng)函數(shù)；

所述機(jī)器人障礙物勢(shì)能場(chǎng)函數(shù)為：

其中：p_i為實(shí)時(shí)獲取障礙物的位置信息，d_r(p,p_i)為計(jì)算機(jī)器人與障礙物的距離，所述距離為歐式距離d_r(p,p_i)＝||p-p_i||，Q^*為機(jī)器人距離障礙物的斥力有效閾值，η為斥力場(chǎng)比例因子，U_ri(p)為障礙物勢(shì)能場(chǎng)函數(shù)；

其中，所述根據(jù)勢(shì)能場(chǎng)計(jì)算牽引控制的目標(biāo)和障礙物虛擬力，包括：

求取引力勢(shì)能場(chǎng)的梯度并將引力勢(shì)能場(chǎng)的負(fù)梯度方向作為目標(biāo)對(duì)機(jī)器人的引力F_a(p)；

求取斥力勢(shì)能場(chǎng)的梯度并將斥力勢(shì)能場(chǎng)的負(fù)梯度方向作為障礙物對(duì)機(jī)器人的斥力F_ri(p)；

所述引力勢(shì)能場(chǎng)的梯度為：

所述斥力勢(shì)能場(chǎng)的梯度為：

其中：為引力勢(shì)能場(chǎng)的梯度，p為機(jī)器人在世界坐標(biāo)系下的位置，p^*為目標(biāo)的位置信息，d(p,p^*)為機(jī)器人與目標(biāo)距離，d^*為機(jī)器人距離目標(biāo)的安全距離閾值，ζ為引力場(chǎng)比例因子，U_a(p)為機(jī)器人目標(biāo)勢(shì)能場(chǎng)函數(shù)；為斥力勢(shì)能場(chǎng)的梯度，p_i為實(shí)時(shí)獲取障礙物的位置信息，d_r(p,p_i)為計(jì)算機(jī)器人與障礙物的距離，Q^*為機(jī)器人距離障礙物的斥力有效閾值，η為斥力場(chǎng)比例因子，U_ri(p)為障礙物勢(shì)能場(chǎng)函數(shù)；F_ri(p)為障礙物對(duì)機(jī)器人的斥力；i表示障礙物的序號(hào)，表示第i個(gè)障礙物；

其中，所述環(huán)境產(chǎn)生的虛擬合力為：

F＝F_r(p)+F_a(p)

其中：F_a(p)為目標(biāo)對(duì)機(jī)器人的引力，F(xiàn)_r(p)為所有障礙物對(duì)機(jī)器人產(chǎn)生的合力，且F_ri(p)表示第i個(gè)障礙物對(duì)機(jī)器人產(chǎn)生的斥力；

所述計(jì)算機(jī)器人的運(yùn)動(dòng)控制信號(hào)包括計(jì)算機(jī)身縱向運(yùn)動(dòng)控制信號(hào)和機(jī)身轉(zhuǎn)向速度控制信號(hào)；

所述機(jī)身縱向運(yùn)動(dòng)信號(hào)為：

其中：為機(jī)身縱向運(yùn)動(dòng)信號(hào)，k_f為增益因子，F(xiàn)_a(p)為目標(biāo)對(duì)機(jī)器人的引力，F(xiàn)_r(p)為所有障礙物對(duì)機(jī)器人產(chǎn)生的合力；

所述計(jì)算機(jī)身運(yùn)動(dòng)方向控制信號(hào)，包括：

根據(jù)環(huán)境產(chǎn)生的合力分力，求取合力對(duì)機(jī)器人作用的方向；

根據(jù)合力對(duì)機(jī)器人作用的方向設(shè)計(jì)機(jī)身轉(zhuǎn)向速度控制信號(hào)；

合力對(duì)機(jī)器人作用的方向?yàn)椋?/p>

θ_goal＝atan2(F_y,F_x)

機(jī)身轉(zhuǎn)向速度控制信號(hào)為：

其中：atan2(y,x)為反三角函數(shù)，該函數(shù)返回點(diǎn)(x,y)和原點(diǎn)(0,0)之間直線的傾斜角，θ_goal為合力對(duì)機(jī)器人作用的方向，(F_x,F_y)為世界坐標(biāo)系下的合力，F(xiàn)_x,F_y分別表示x軸的分力和y軸的分力，k_θ為增益因子，θ_d為機(jī)器人實(shí)時(shí)的期望偏航姿態(tài)，為機(jī)身運(yùn)動(dòng)方向速度控制信號(hào)；

其中，所述計(jì)算機(jī)身位姿控制器輸入，包括：

根據(jù)產(chǎn)生的機(jī)身縱向運(yùn)動(dòng)速度控制信號(hào)，并根據(jù)機(jī)身實(shí)時(shí)的位置信息，引入機(jī)身縱向運(yùn)動(dòng)控制器比例和微分因子，設(shè)計(jì)機(jī)身縱向運(yùn)動(dòng)控制器輸入；

根據(jù)產(chǎn)生的機(jī)身運(yùn)動(dòng)方向速度控制信號(hào)，并根據(jù)機(jī)身實(shí)時(shí)的方向信息，引入機(jī)身運(yùn)動(dòng)方向控制器比例和微分因子，設(shè)計(jì)機(jī)身運(yùn)動(dòng)方向控制器輸入；

機(jī)身縱向運(yùn)動(dòng)控制器輸入為：

機(jī)身運(yùn)動(dòng)方向控制器輸入為：

其中：為期望的機(jī)身縱向運(yùn)動(dòng)速度，x為實(shí)際的機(jī)身縱向位置，為實(shí)際的機(jī)身縱向速度；k_px為機(jī)身縱向運(yùn)動(dòng)控制器比例因子、k_vx為機(jī)身縱向運(yùn)動(dòng)控制器微分因子，μ_x(t)為機(jī)身縱向運(yùn)動(dòng)控制器輸入；為期望的機(jī)身運(yùn)動(dòng)方向變化速度，θ為實(shí)際的機(jī)身運(yùn)動(dòng)方向，為實(shí)際的機(jī)身運(yùn)動(dòng)方向變化速度；k_pθ為機(jī)身運(yùn)動(dòng)方向控制器比例因子，k_vθ為機(jī)身運(yùn)動(dòng)方向的微分因子，μ_θ(t)為機(jī)身運(yùn)動(dòng)方向控制器輸入。