付根平 朱立學(xué) 張世昂 伍榮達(dá) 黃偉鋒

摘要：香蕉采摘機(jī)器人要在各采樣時(shí)刻根據(jù)果柄夾持機(jī)構(gòu)的目標(biāo)位置和姿態(tài)求得4個(gè)自由度的逆運(yùn)動(dòng)學(xué)解，其底座轉(zhuǎn)角和末端轉(zhuǎn)角可根據(jù)幾何關(guān)系求得解析解，但水平位移和垂直位移因相互耦合呈非線性關(guān)系，難以通過(guò)解析法求得逆運(yùn)動(dòng)學(xué)解，故提出一種基于牛頓迭代法的香蕉采摘機(jī)器人逆運(yùn)動(dòng)學(xué)數(shù)值求解方法。首先根據(jù)采摘機(jī)器人的機(jī)械結(jié)構(gòu)和幾何關(guān)系，建立由4個(gè)自由度表示果柄夾持機(jī)構(gòu)位置和姿態(tài)的正運(yùn)動(dòng)學(xué)模型，再據(jù)此構(gòu)造水平位移和垂直位移2個(gè)自由度滿足的方程組，并求出其雅可比矩陣的逆矩陣，然后利用牛頓迭代算法根據(jù)果柄夾持機(jī)構(gòu)的目標(biāo)位置和姿態(tài)求得水平位移和垂直位移的逆運(yùn)動(dòng)學(xué)解。試驗(yàn)結(jié)果表明：該方法求得采摘機(jī)器人底座轉(zhuǎn)角和末端轉(zhuǎn)角的逆解無(wú)誤差。而對(duì)于水平位移和垂直位移的逆解，當(dāng)果柄夾持機(jī)構(gòu)取單個(gè)任意的目標(biāo)位置和姿態(tài)，且牛頓迭代初值取固定初始位姿時(shí)，2次迭代的逆解誤差小于0.1mm，3次迭代的逆解無(wú)誤差；當(dāng)果柄夾持機(jī)構(gòu)取連續(xù)運(yùn)動(dòng)中各采樣時(shí)刻的目標(biāo)位置和姿態(tài)，且牛頓迭代初值取上一采樣時(shí)刻的逆解時(shí)，只需1次迭代即可得到高精度的逆解。

關(guān)鍵詞：香蕉采摘機(jī)器人；牛頓迭代法；逆運(yùn)動(dòng)學(xué)求解；正運(yùn)動(dòng)學(xué)建模

中圖分類號(hào)：TP242

文獻(xiàn)標(biāo)識(shí)碼：A

文章編號(hào)：20955553 （2023） 12020010

Research on solving inverse kinematics of banana picking robot

based on Newtons iteration method

Fu Genping1， Zhu Lixue2， Zhang Shiang1， Wu Rongda2， Huang Weifeng1

（1. School of Automation， Zhongkai University of Agriculture and Engineering， Guangzhou， 510225， China;

2. School of Electro-mechanical Engineering， Zhongkai University of Agriculture and Engineering，

Guangzhou， 510225， China）

Abstract：

The inverse kinematics solution of the banana picking robot with four degrees of freedom should be resolved according to the target position and attitude of the fruit handle clamping actuator at each sampling time. The two degrees of freedom of the base rotation angle and end rotation angle of the banana picking robot， its analytical solution of inverse kinematics is resolved according to the geometric relations. However， the two degrees of freedom of the horizontal displacement and vertical displacement are non-linear due to their mutual coupling， so it is difficult to resolve their inverse kinematics solutions by analytical solution methods. Therefore， a numerical solution method for inverse kinematics of the banana picking robot based on Newton iteration method is proposed. Firstly， according to the mechanical structure and geometric relations of picking robot， the forward kinematics model solving the position and attitude of the fruit handle clamping actuator by four degrees of freedom is established， then the equations satisfied by the two degrees of freedom of horizontal displacement and vertical displacement are constructed， and the inverse matrix of its Jacobian matrix is obtained. Then the Newton iterative algorithm designed in this paper is used to resolve the inverse kinematics solution of horizontal displacement and vertical displacement according to the target position and attitude of the fruit handle clamping actuator. The experimental results show that the inverse kinematics solution of the base rotation angle and end rotation angle of the picking robot solved by the method proposed in this paper has no error. For the inverse kinematics solution of horizontal displacement and vertical displacement， when the fruit handle clamping actuator takes any target position and attitude， and the initial value of Newton iteration takes a fixed initial position and attitude， the error of inverse kinematics solution with two Newton iterations is less than 0.1 mm， and has no error with three Newton iterations. When the fruit handle clamping actuator takes the target position and attitude at each sampling time in continuous motion， and the initial value of Newton iteration takes the inverse kinematics solution of previous sampling time， only one iteration is needed to obtain the high-precision inverse solution.

Keywords：

banana picking robot; Newtons iteration method; solving inverse kinematics; forward kinematics modeling

0 引言

香蕉是嶺南特色水果和經(jīng)濟(jì)作物，適收期的香蕉串質(zhì)量約30kg，果柄切割處高度超過(guò)2m。目前，香蕉串采摘以人工方式為主，由多人協(xié)作完成，勞動(dòng)強(qiáng)度大、人工成本高、采摘效率低，嚴(yán)重制約著香蕉的產(chǎn)業(yè)發(fā)展和經(jīng)濟(jì)效益，而機(jī)器人可實(shí)現(xiàn)香蕉串采摘的機(jī)械化和智能化［13］。采摘機(jī)器人先根據(jù)香蕉串果柄位姿和周圍環(huán)境規(guī)劃出末端采摘執(zhí)行器中果柄夾持機(jī)構(gòu)的運(yùn)動(dòng)軌跡，從而得到各采樣時(shí)刻果柄夾持機(jī)構(gòu)的目標(biāo)位姿，然后據(jù)此求得機(jī)器人各自由度的逆運(yùn)動(dòng)學(xué)解，輸入關(guān)節(jié)伺服控制器，進(jìn)而控制果柄夾持機(jī)構(gòu)準(zhǔn)確定位到香蕉串果柄切割處并將其夾住，再啟動(dòng)鏈鋸切斷果柄，完成香蕉串采摘。顯然，逆運(yùn)動(dòng)學(xué)求解是實(shí)現(xiàn)機(jī)器人末端果柄夾持機(jī)構(gòu)運(yùn)動(dòng)控制和精準(zhǔn)定位的前提和關(guān)鍵。

通常，機(jī)器人的逆運(yùn)動(dòng)學(xué)求解方法主要有解析解法和數(shù)值解法［48］。其中，解析解法因計(jì)算量小、精度高、實(shí)時(shí)性好而被廣泛應(yīng)用于機(jī)器人的逆運(yùn)動(dòng)學(xué)求解，但要求機(jī)器人的結(jié)構(gòu)必須滿足Pieper準(zhǔn)則［9］；而對(duì)于結(jié)構(gòu)不符合Pieper準(zhǔn)則的機(jī)器人，則可采用數(shù)值解法求得滿足給定精度的逆運(yùn)動(dòng)學(xué)解［1011］。相關(guān)研究人員采用神經(jīng)網(wǎng)絡(luò)［1215］、強(qiáng)化學(xué)習(xí)［1617］、粒子群優(yōu)化算法［1819］、遺傳算法［20］、果蠅優(yōu)化算法［2122］、差分進(jìn)化算法［23］等數(shù)值解法求機(jī)器人的逆運(yùn)動(dòng)學(xué)解。但神經(jīng)網(wǎng)絡(luò)結(jié)構(gòu)不好確定、訓(xùn)練的參數(shù)多、訓(xùn)練樣本量大且難以獲取、網(wǎng)絡(luò)訓(xùn)練時(shí)間長(zhǎng)，對(duì)機(jī)器人控制系統(tǒng)的硬件配置要求較高；強(qiáng)化學(xué)習(xí)也需要大量的數(shù)據(jù)樣本，并且訓(xùn)練過(guò)程較長(zhǎng)；粒子群優(yōu)化算法迭代優(yōu)化過(guò)程復(fù)雜，粒子總數(shù)難以確定；遺傳算法求解逆運(yùn)動(dòng)學(xué)過(guò)程中容易早熟，局部搜索能力差，不一定每次都能求得滿足給定精度的逆運(yùn)動(dòng)學(xué)解。

牛頓迭代法是一種適合求解復(fù)雜非線性方程組的經(jīng)典數(shù)值解法［24］，它的迭代過(guò)程簡(jiǎn)單、收斂速度快，也常用于機(jī)器人的逆運(yùn)動(dòng)學(xué)求解并取得了較好的效果。如陳光榮等［25］基于擴(kuò)展雅可比矩陣求解四足機(jī)器人的逆運(yùn)動(dòng)學(xué)；吉陽(yáng)珍等［26］先通過(guò)改進(jìn)的鯨魚(yú)優(yōu)化算法求得逆運(yùn)動(dòng)學(xué)解作為初值，再利用牛頓迭代法求出滿足精度要求的逆運(yùn)動(dòng)學(xué)解；韓磊等［27］結(jié)合非偏置型機(jī)器人封閉逆解和改進(jìn)牛頓迭代法求解手腕偏置型6自由度機(jī)器人的逆運(yùn)動(dòng)學(xué)解。

由于香蕉采摘機(jī)器人的運(yùn)動(dòng)機(jī)構(gòu)為串聯(lián)和并聯(lián)混合結(jié)構(gòu)［2829］，具有2個(gè)轉(zhuǎn)動(dòng)自由度和2個(gè)相互耦合的平動(dòng)自由度，其結(jié)構(gòu)不滿足Pieper準(zhǔn)則，不能用代數(shù)法求得4個(gè)自由度的逆運(yùn)動(dòng)學(xué)解析解。然而，根據(jù)香蕉采摘機(jī)器人特殊的機(jī)械結(jié)構(gòu)可由幾何關(guān)系求得2個(gè)轉(zhuǎn)動(dòng)自由度的逆運(yùn)動(dòng)學(xué)解析解；但2個(gè)平動(dòng)自由度因相互耦合而呈復(fù)雜的非線性關(guān)系，難以求得逆運(yùn)動(dòng)學(xué)解析解，因此，考慮采用數(shù)值解法。由于香蕉采摘機(jī)器人各自由度運(yùn)動(dòng)軌跡平滑、局部極值點(diǎn)少，且只需計(jì)算關(guān)于2個(gè)平動(dòng)自由度的雅可比矩陣，計(jì)算量較小，適合牛頓迭代法求解，基于此，本文提出一種基于牛頓迭代法的香蕉采摘機(jī)器人逆運(yùn)動(dòng)學(xué)數(shù)值求解方法。先根據(jù)采摘機(jī)器人的機(jī)械結(jié)構(gòu)由幾何關(guān)系建立采摘執(zhí)行器中果柄夾持機(jī)構(gòu)的位姿與各自由度的正運(yùn)動(dòng)學(xué)模型，并據(jù)此構(gòu)造水平位移和垂直位移2個(gè)平動(dòng)自由度滿足的方程組，然后求其雅可比矩陣的逆矩陣；再基于牛頓迭代算法根據(jù)果柄夾持機(jī)構(gòu)的目標(biāo)位置和姿態(tài)求得滿足設(shè)定精度的逆運(yùn)動(dòng)學(xué)解。

1 香蕉采摘機(jī)器人的正運(yùn)動(dòng)學(xué)建模

1.1 采摘機(jī)器人的運(yùn)動(dòng)結(jié)構(gòu)分析

根據(jù)適收期的香蕉串質(zhì)量大和高位生長(zhǎng)的特點(diǎn)，要求采摘機(jī)器人具備較強(qiáng)的負(fù)載能力和較大的工作空間，設(shè)計(jì)如圖1所示的集香蕉串夾持和切割于一體的混聯(lián)式采摘機(jī)器人，安裝在履帶底盤(pán)上，可在崎嶇不平的香蕉園機(jī)耕道運(yùn)動(dòng)。采摘機(jī)器人具有4個(gè)自由度，能夠較好地調(diào)節(jié)末端采摘執(zhí)行器的位姿，同時(shí)主臂和副臂較長(zhǎng)，使得末端采摘執(zhí)行器的作業(yè)空間大，滿足高位生長(zhǎng)的香蕉串采摘需求。此外，各自由度的驅(qū)動(dòng)電機(jī)功率較大，且主臂和副臂為剛度大的并聯(lián)機(jī)構(gòu)，故機(jī)器人的負(fù)載能力較強(qiáng)，可達(dá)60kg，完全能夠承受香蕉串的重量。

采摘機(jī)器人的機(jī)械結(jié)構(gòu)如圖2所示，相應(yīng)的連桿參數(shù)如表1所示。該機(jī)器人共設(shè)置4個(gè)自由度，如表2所示，分別是底座轉(zhuǎn)動(dòng)自由度、滑塊C沿導(dǎo)軌的水平運(yùn)動(dòng)自由度、滑塊A沿導(dǎo)軌的垂直運(yùn)動(dòng)自由度、末端轉(zhuǎn)動(dòng)自由度。其中，底座轉(zhuǎn)動(dòng)自由度驅(qū)動(dòng)整個(gè)機(jī)器人轉(zhuǎn)動(dòng)，可大范圍調(diào)整采摘執(zhí)行器的位置和姿態(tài)；水平運(yùn)動(dòng)自由度和垂直運(yùn)動(dòng)自由度分別通過(guò)驅(qū)動(dòng)滑塊C、滑塊A連同短主臂CE、長(zhǎng)主臂AD沿導(dǎo)軌作水平運(yùn)動(dòng)、垂直運(yùn)動(dòng)，進(jìn)而改變采摘執(zhí)行器的位置；而末端轉(zhuǎn)動(dòng)自由度主要調(diào)整采摘執(zhí)行器的姿態(tài)，同時(shí)小范圍調(diào)整其位置。

香蕉串采摘執(zhí)行器位于機(jī)器人的末端，如圖3所示，主要由末端轉(zhuǎn)動(dòng)機(jī)構(gòu)、香蕉果柄夾持機(jī)構(gòu)、香蕉果柄切割機(jī)構(gòu)三部分組成。機(jī)器人采摘香蕉串時(shí)，先控制夾持機(jī)構(gòu)準(zhǔn)確定位到香蕉串果柄切割處并可靠地夾持住香蕉串果柄，再啟動(dòng)切割機(jī)構(gòu)將果柄切斷。為研究方便，將末端轉(zhuǎn)動(dòng)自由度的轉(zhuǎn)軸簡(jiǎn)化為連桿MN，香蕉果柄夾持機(jī)構(gòu)簡(jiǎn)化為連桿NH，表示果柄夾持機(jī)構(gòu)的中心H到末端轉(zhuǎn)軸MN的距離。由于平行四邊形機(jī)構(gòu)EFKJ和CEIG通過(guò)三角形構(gòu)件IEJ連接，并鉸連在水平滑塊C上，使得香蕉果柄夾持機(jī)構(gòu)等效連桿NH始終保持水平狀態(tài)。

1.2 采摘機(jī)器人的正運(yùn)動(dòng)學(xué)建模

由前文可知，通過(guò)控制機(jī)器人4個(gè)自由度的運(yùn)動(dòng)可調(diào)節(jié)采摘執(zhí)行器中果柄夾持機(jī)構(gòu)的位置和姿態(tài)，使其準(zhǔn)確定位到香蕉串果柄切割處以便完成采摘作業(yè)。因此，為了對(duì)果柄夾持機(jī)構(gòu)的位置和姿態(tài)進(jìn)行精確控制，需要先建立4個(gè)自由度變量（θ、x、z、φ）描述果柄夾持機(jī)構(gòu)位置和姿態(tài)的采摘機(jī)器人正運(yùn)動(dòng)學(xué)模型。

采摘機(jī)器人的運(yùn)動(dòng)機(jī)構(gòu)如圖4所示，短主臂CE與水平方向夾角為α（逆時(shí)針為正方向），副臂DF與水平方向夾角為β（順時(shí)針為正方向），連桿FK與LM的夾角為γ（常數(shù)）。在水平運(yùn)動(dòng)方向和垂直運(yùn)動(dòng)方向的交點(diǎn)處建立局部坐標(biāo)系o-x′y′z′，并隨底座一起轉(zhuǎn)動(dòng)；而在底座轉(zhuǎn)動(dòng)中心建立基坐標(biāo)系O-XYZ。其中，局部坐標(biāo)系o-x′y′z′在基坐標(biāo)系O-XYZ中的位置向量

p=［px，py，pz］=［-364，0，657］

考慮到采摘機(jī)器人的連桿機(jī)構(gòu)和局部坐標(biāo)系ox′y′z′隨底座一起轉(zhuǎn)動(dòng)，故水平運(yùn)動(dòng)自由度和垂直運(yùn)動(dòng)自由度只在x′oz′平面內(nèi)調(diào)整連桿機(jī)構(gòu)的位置，因此，先在x′oz′平面內(nèi)建立末端轉(zhuǎn)動(dòng)中心M的位置與水平位移x、垂直位移z的函數(shù)關(guān)系。根據(jù)圖4所示采摘機(jī)器人各連桿之間的幾何關(guān)系，可得節(jié)點(diǎn)A、C、M的位置表達(dá)式如式（1）～式（3）所示。

由式（1）～式（3）可得，末端轉(zhuǎn)動(dòng)中心M的位置表達(dá)式，如式（4）所示。

由式（4）可知，M的位置不僅與水平位移x、垂直位移z有關(guān)，還與主臂夾角α、副臂夾角β有關(guān)。由表1中采摘機(jī)器人連桿參數(shù)可知，相關(guān)連桿長(zhǎng)度不滿足M的水平運(yùn)動(dòng)和垂直運(yùn)動(dòng)解耦條件［29］：l1l4=l2l3，從而導(dǎo)致機(jī)器人的正運(yùn)動(dòng)學(xué)模型比較復(fù)雜。因此，需要進(jìn)一步探究主臂夾角α、副臂夾角β與水平位移x、垂直位移z的關(guān)系。

聯(lián)立式（1）、式（2）和主臂夾角α的半角公式，同時(shí)考慮主臂夾角α的取值范圍，可得主臂夾角α與水平位移x、垂直位移z的相關(guān)函數(shù)表達(dá)式為

同理，可得副臂夾角β與水平位移x、垂直位移z的相關(guān)函數(shù)表達(dá)式如式（9）～式（12）所示。

將式（6）、式（11）代入式（4），即可得x′oz′平面內(nèi)末端轉(zhuǎn)動(dòng)中心M的位置（xM，zM）關(guān)于水平位移x、垂直位移z的非線性函數(shù)表達(dá)式。結(jié)合局部坐標(biāo)系o-x′y′z′在基坐標(biāo)系O-XYZ中的位姿p，同時(shí)考慮底座轉(zhuǎn)角θ的作用，如圖5所示，根據(jù)幾何關(guān)系可得，末端轉(zhuǎn)動(dòng)中心M在基坐標(biāo)系O-XYZ中的位置表達(dá)式，如式（13）所示。

而對(duì)于果柄夾持機(jī)構(gòu)的位姿，還需考慮末端轉(zhuǎn)角φ的作用，如圖5所示，因此，根據(jù)機(jī)器人的連桿結(jié)構(gòu)和幾何關(guān)系，可得果柄夾持機(jī)構(gòu)相對(duì)于末端轉(zhuǎn)動(dòng)中心M在基坐標(biāo)系O-XYZ中的位置。

將M的位置表達(dá)式（13）代入式（14）可得

將式（6）、式（11）代入式（15），即得各自由度變量θ、x、z、φ描述果柄夾持機(jī)構(gòu)位置的函數(shù)關(guān)系式。

由于底座轉(zhuǎn)角θ、末端轉(zhuǎn)角φ均為偏擺自由度，故果柄夾持機(jī)構(gòu)的姿態(tài)角

Ψ=θ+φ? ?（16）

至此，得到式（15）、式（16）所示采摘機(jī)器人的正運(yùn)動(dòng)學(xué)模型，即果柄夾持機(jī)構(gòu)的位置（XH，YH，ZH）、姿態(tài)角Ψ與各自由度變量θ、x、z、φ的函數(shù)關(guān)系式。

2 采摘機(jī)器人的逆運(yùn)動(dòng)學(xué)求解

機(jī)器人采摘香蕉串過(guò)程中，通過(guò)控制各自由度變量以調(diào)節(jié)果柄夾持機(jī)構(gòu)的位姿，使其準(zhǔn)確定位到香蕉串果柄切割處。因此，需要對(duì)采摘機(jī)器人進(jìn)行逆運(yùn)動(dòng)學(xué)求解，即在各采樣時(shí)刻根據(jù)果柄夾持機(jī)構(gòu)的目標(biāo)位置（XH，YH，ZH）和姿態(tài)角Ψ在線求得4個(gè)自由度θ、x、z、φ的逆運(yùn)動(dòng)學(xué)解。

2.1 底座轉(zhuǎn)角和末端轉(zhuǎn)角的求解

如圖5所示，由于基坐標(biāo)系O-XYZ建在底座轉(zhuǎn)動(dòng)中心，故機(jī)器人連桿在XOY平面內(nèi)的投影經(jīng)過(guò)基坐標(biāo)系原點(diǎn)O，因此，先根據(jù)幾何關(guān)系由果柄夾持機(jī)構(gòu)的目標(biāo)位置（XH，YH，ZH）和姿態(tài)角Ψ求得末端轉(zhuǎn)動(dòng)中心M的位置分量XM、YM，再通過(guò)反正切函數(shù)求解底座轉(zhuǎn)角θ。聯(lián)立式（14）、式（16）可得M的位置分量XM、YM。

因?yàn)榈鬃D(zhuǎn)角θ的取值范圍為（-π，π），而反正切函數(shù)的取值范圍為（-π/2，π/2），因此，根據(jù)M的位置分量XM、YM所處象限進(jìn)行修正，可得底座轉(zhuǎn)角θ的逆解表達(dá)式如式（18）所示。

由于反正切函數(shù)中分母XM不能為0，若XM=0，則結(jié)合YM判斷底座轉(zhuǎn)角θ是π/2還是-π/2。求得底座轉(zhuǎn)角θ后，再根據(jù)末端轉(zhuǎn)角φ的取值范圍（-2π/3，2π/3），由式（16）可得末端轉(zhuǎn)角φ的逆解表達(dá)式，如式（19）所示。

2.2 基于牛頓迭代法求解水平位移和垂直位移

采摘機(jī)器人的底座轉(zhuǎn)角θ、末端轉(zhuǎn)角φ確定后，由式（6）、式（11）、式（15）可知，果柄夾持機(jī)構(gòu)的位置分量XH、YH、ZH相互耦合，是水平位移x和垂直位移z的復(fù)雜非線性函數(shù)，難以通過(guò)傳統(tǒng)的方程組解法直接進(jìn)行求解。而數(shù)值迭代法比較適合求解非線性方程組，故本文采用牛頓迭代法求解水平位移x、垂直位移z的逆運(yùn)動(dòng)學(xué)解。

選取采摘機(jī)器人正運(yùn)動(dòng)學(xué)模型的位置表達(dá)式（15）中XH、ZH分量，以及姿態(tài)角Ψ表達(dá)式（16）構(gòu)造關(guān)于水平位移x和垂直位移z的方程組f（x，z），即

將式（6）、式（11）代入式（21），再分別對(duì)x、z求偏導(dǎo)數(shù)，可得其雅可比矩陣J（x，z），如式（22）所示。

雅可比矩陣J（x，z）的逆矩陣J-1（x，z）計(jì)算表達(dá)式如式（23）所示。

將式（21）、式（23）代入式（20），即得計(jì)算采摘機(jī)器人水平位移x、垂直位移z的牛頓迭代表達(dá)式。

基于牛頓迭代法求解水平位移x、垂直位移z的流程如圖6所示，先根據(jù)果柄夾持機(jī)構(gòu)的目標(biāo)位置（XH，YH，ZH）和姿態(tài)角Ψ，由式（18）、式（19）求得底座轉(zhuǎn)角θ、末端轉(zhuǎn)角φ的逆解。設(shè)定牛頓迭代初值x0、z0，迭代次數(shù)k=0，給定誤差閾值ε、最大迭代次數(shù)η等結(jié)束條件。由式（21）～式（23）和牛頓迭代式（20）計(jì)算xk+1、zk+1，并計(jì)算與上次迭代值xk、zk的誤差ek+1，若ek+1<ε，則迭代結(jié)束，輸出x、z的逆解；若ek+1>ε且k<η，則更新迭代值xk、zk和迭代次數(shù)k，繼續(xù)迭代；如果ek+1>ε且k>η，表明在最大迭代次數(shù)η內(nèi)沒(méi)有求得滿足給定精度的逆解，可能迭代初值選擇不合理，需修改迭代初值x0、z0后重新進(jìn)行求解。

由于牛頓迭代初值對(duì)算法的收斂速度和逆解的精度影響較大，考慮到采摘機(jī)器人的位姿在短時(shí)間內(nèi)變化較小，因此，對(duì)于連續(xù)運(yùn)動(dòng)，可選擇水平位移x和垂直位移z的實(shí)時(shí)值或上一采樣時(shí)刻的逆解作為牛頓迭代初值x0、z0，通常只需要1次迭代即可求得高精度的逆解。

3 試驗(yàn)與分析

為了驗(yàn)證本文方法的效果，在MATLAB軟件平臺(tái)上按照表1所示參數(shù)建立香蕉采摘機(jī)器人的連桿模型，如圖7所示，分別對(duì)果柄夾持機(jī)構(gòu)的單個(gè)目標(biāo)位姿和連續(xù)運(yùn)動(dòng)中各采樣時(shí)刻目標(biāo)位姿的逆運(yùn)動(dòng)學(xué)求解進(jìn)行仿真試驗(yàn)。

3.1 單個(gè)位姿的逆運(yùn)動(dòng)學(xué)求解

首先討論采摘機(jī)器人的果柄夾持機(jī)構(gòu)從初始位姿到單個(gè)目標(biāo)位姿的逆解。出于一般性考慮，在表2所示采摘機(jī)器人自由度變量的取值范圍內(nèi)，任意選取3組目標(biāo)值（θd，xd，zd，φd），由采摘機(jī)器人的正運(yùn)動(dòng)學(xué)模型式（15）、式（16）求得果柄夾持機(jī)構(gòu)的目標(biāo)位置（XH，YH，ZH）和姿態(tài)角Ψ，如表3中組序1、2、3，而組序0表示采摘機(jī)器人初始狀態(tài)下各自由度的初值和果柄夾持機(jī)構(gòu)的初始位姿。

當(dāng)果柄夾持機(jī)構(gòu)分別取組序1、2、3的目標(biāo)位姿時(shí)，先采用式（17）～式（19）求得底座轉(zhuǎn)角θ、末端轉(zhuǎn)角φ的逆運(yùn)動(dòng)學(xué)解，再通過(guò)圖6所示牛頓迭代算法求出水平位移x、垂直位移z的逆運(yùn)動(dòng)學(xué)解，如表4所示。其中，牛頓迭代初值均取組序0中初始水平位移x0=360mm、垂直位移z0=-270mm。

表4中底座轉(zhuǎn)角θ、末端轉(zhuǎn)角φ的逆解與表3中目標(biāo)值θd、φd相同，表明解析解法可求得高精度逆解。水平位移和垂直位移任取的3組目標(biāo)值xd、zd（組序1、2、3）雖與牛頓迭代初值x0、z0相差較大，但經(jīng)過(guò)2次牛頓迭代后的值x2、z2誤差小于0.1mm；而經(jīng)過(guò)3次牛頓迭代后的值x3、z3無(wú)誤差。結(jié)果表明，對(duì)于任意給定的果柄夾持機(jī)構(gòu)目標(biāo)位置和姿態(tài)，本文方法都能求出采摘機(jī)器人各自由度的高精度逆運(yùn)動(dòng)學(xué)解，且牛頓迭代次數(shù)不超過(guò)3次。

3.2 連續(xù)運(yùn)動(dòng)中各采樣時(shí)刻位姿的逆運(yùn)動(dòng)學(xué)求解

為了進(jìn)一步驗(yàn)證本文方法逆運(yùn)動(dòng)學(xué)求解的精度、實(shí)時(shí)性和適用性，在果柄夾持機(jī)構(gòu)跟蹤規(guī)劃的軌跡運(yùn)動(dòng)到香蕉串果柄切割處的過(guò)程中，根據(jù)連續(xù)采樣時(shí)刻果柄夾持機(jī)構(gòu)的目標(biāo)位置和姿態(tài)求解各自由度的逆運(yùn)動(dòng)學(xué)解。

選取表3中組序0和組序3的位置和姿態(tài)角分別作為果柄夾持機(jī)構(gòu)在起始時(shí)刻（t=0.0s）、結(jié)束時(shí)刻（t=6.0s）的位姿，選取（1710.4，-1489.1，1391.6）和-16.7°作為中間過(guò)渡時(shí)刻（t=3.5s）的位置和姿態(tài)角，同時(shí)設(shè)定起始、結(jié)束時(shí)刻果柄夾持機(jī)構(gòu)位置和姿態(tài)角的一階導(dǎo)數(shù)（速度和角速度）均為0，即

根據(jù)上述約束條件，通過(guò)三次樣條插值方法［30］分別規(guī)劃出果柄夾持機(jī)構(gòu)連續(xù)運(yùn)動(dòng)的位置分量軌跡XH（t）、YH（t）、ZH（t）和姿態(tài)角軌跡Ψ（t），如圖8所示，據(jù)此可得采樣時(shí)刻ts果柄夾持機(jī)構(gòu)的目標(biāo)位置（XH（ts），YH（ts），ZH（ts））和姿態(tài)角Ψ（ts），然后采用本文方法求出各自由度的逆運(yùn)動(dòng)學(xué)解θ（ts）、x（ts）、z（ts）、φ（ts），進(jìn)而得到連續(xù)采樣時(shí)刻各自由度的逆運(yùn)動(dòng)學(xué)解軌跡如圖9所示（采樣周期Ts=25ms）。

由于在較短的采樣周期Ts內(nèi)各自由度值變化不大，因此，為了減少牛頓迭代次數(shù)，提高逆解算法的實(shí)時(shí)性，選取上一采樣時(shí)刻水平位移x和垂直位移z的逆解作為牛頓迭代初值，且只做1次牛頓迭代即輸出逆解。圖9中各自由度的逆運(yùn)動(dòng)學(xué)解軌跡均連續(xù)、平滑，沒(méi)有奇異點(diǎn)，表明本文逆運(yùn)動(dòng)學(xué)求解方法適用性好，在每個(gè)采樣時(shí)刻都能求得有效逆解。

為了便于說(shuō)明水平位移x和垂直位移z的逆運(yùn)動(dòng)學(xué)求解精度，將圖9中各自由度的逆運(yùn)動(dòng)學(xué)解代入正運(yùn)動(dòng)學(xué)模型式（15）求出果柄夾持機(jī)構(gòu)的位置，與圖8（a）中規(guī)劃位置的誤差軌跡如圖10所示，3個(gè)位置分量XH、YH、ZH的最大誤差均為10-3mm數(shù)量級(jí)，其中：XH的最大誤差emX為0.889×10-3mm；YH的最大誤差emY為-1.286×10-3mm；ZH的最大誤差emZ為-0.853×10-3mm。這表明在連續(xù)運(yùn)動(dòng)中，取上一采樣時(shí)刻的逆解作為牛頓迭代初值，只需經(jīng)過(guò)1次牛頓迭代即可求得水平位移x和垂直位移z較高精度的逆運(yùn)動(dòng)學(xué)解。此外，由于底座轉(zhuǎn)角θ、末端轉(zhuǎn)角φ可由式（17）～式（19）求得無(wú)誤差的逆運(yùn)動(dòng)學(xué)解析解，故不做贅述。

為了進(jìn)一步說(shuō)明采摘機(jī)器人連續(xù)運(yùn)動(dòng)狀態(tài)下對(duì)于不同的采樣周期Ts，只需1次牛頓迭代即可求得水平位移x和垂直位移z的高精度逆解，采樣周期Ts分別取10ms、25ms、50ms、100ms、200ms，重復(fù)前述方法求出連續(xù)采樣時(shí)刻各自由度逆運(yùn)動(dòng)學(xué)解對(duì)應(yīng)的果柄夾持機(jī)構(gòu)3個(gè)位置分量的最大誤差，如表5所示。由結(jié)果可知，隨著采樣周期Ts變大，各自由度逆運(yùn)動(dòng)學(xué)解對(duì)應(yīng)果柄夾持機(jī)構(gòu)的位置誤差也變大，但最大誤差小于0.1mm；當(dāng)Ts取200ms時(shí)，果柄夾持機(jī)構(gòu)位置分量的最大誤差為emY=-8.297×10-2mm。這表明采摘機(jī)器人在連續(xù)運(yùn)動(dòng)狀態(tài)下，基于牛頓迭代的逆運(yùn)動(dòng)學(xué)求解方法具有較高的精度和較好的實(shí)時(shí)性。

4 結(jié)論

1） 根據(jù)香蕉采摘機(jī)器人特殊的機(jī)械結(jié)構(gòu)和幾何關(guān)系，建立采摘執(zhí)行器中果柄夾持機(jī)構(gòu)的位置（XH，YH，ZH）和姿態(tài)角Ψ由4個(gè)自由度變量（θ、x、z、φ）描述的正運(yùn)動(dòng)學(xué)模型。

2） 針對(duì)采摘機(jī)器人的水平位移x和垂直位移z相互耦合呈復(fù)雜非線性關(guān)系，難以求得逆運(yùn)動(dòng)學(xué)解析解的問(wèn)題，提出了一種基于牛頓迭代法的香蕉采摘機(jī)器人逆運(yùn)動(dòng)學(xué)數(shù)值求解方法。先根據(jù)采摘機(jī)器人的正運(yùn)動(dòng)學(xué)模型和幾何關(guān)系求得底座轉(zhuǎn)角θ、末端轉(zhuǎn)角φ的逆運(yùn)動(dòng)學(xué)解析解，再由機(jī)器人的正運(yùn)動(dòng)學(xué)模型構(gòu)造水平位移x和垂直位移z滿足的方程組，并求出其雅可比矩陣的逆矩陣，然后采用牛頓迭代算法根據(jù)果柄夾持機(jī)構(gòu)的目標(biāo)位置和姿態(tài)求得滿足設(shè)定精度的水平位移x和垂直位移z的逆運(yùn)動(dòng)學(xué)解。

3） 試驗(yàn)表明，本文方法可求得香蕉采摘機(jī)器人底座轉(zhuǎn)角θ、末端轉(zhuǎn)角φ無(wú)誤差的逆運(yùn)動(dòng)學(xué)解析解。對(duì)于果柄夾持機(jī)構(gòu)單個(gè)任意目標(biāo)位置和姿態(tài)，且牛頓迭代初值取固定的初始位姿時(shí)，2次牛頓迭代后水平位移x和垂直位移z的逆解誤差小于0.1mm，而3次迭代可得到無(wú)誤差的逆解；對(duì)于果柄夾持機(jī)構(gòu)連續(xù)運(yùn)動(dòng)中各采樣時(shí)刻的目標(biāo)位置和姿態(tài)，若牛頓迭代初值取上一采樣時(shí)刻的逆解，則只需1次迭代即可求得水平位移x和垂直位移z的高精度逆解。本文方法不僅可以求出采摘機(jī)器人任意目標(biāo)位姿對(duì)應(yīng)的各自由度的高精度逆運(yùn)動(dòng)學(xué)解，而且實(shí)時(shí)性較好。

參 考 文 獻(xiàn)

［1］ Nissimov S， Goldberger J， Alchanatis V. Obstacle detection in a greenhouse environment using the Kinect sensor ［J］. Computers and Electronics in Agriculture， 2015， 113： 104-115.

［2］ 羅欣， 丁曉軍. 地面移動(dòng)作業(yè)機(jī)器人運(yùn)動(dòng)規(guī)劃與控制研究綜述［J］. 哈爾濱工業(yè)大學(xué)學(xué)報(bào)， 2021， 53（1）： 1-15.

Luo Xin， Ding Xiaojun. Research and prospective on motion planning and control of ground mobile manipulators ［J］. Journal of Harbin Institute of Technology， 2021， 53（1）： 1-15.

［3］ 趙春江. 智慧農(nóng)業(yè)發(fā)展現(xiàn)狀及戰(zhàn)略目標(biāo)研究［J］. 智慧農(nóng)業(yè)， 2019， 1（1）： 1-7.

Zhao Chunjiang. State-of-the-art and recommended developmental strategic objectives of smart agriculture ［J］. Smart Agriculture， 2019， 1（1）： 1-7.

［4］ 韓興國(guó)， 宋小輝， 殷鳴， 等. 6R焊接機(jī)器人逆解算法與焊接軌跡誤差分析［J］. 農(nóng)業(yè)機(jī)械學(xué)報(bào)， 2017， 48（8）： 384-390， 412.

Han Xingguo， Song Xiaohui， Yin Ming， et al. Solution of inverse kinematics and welding trajectory error analysis for 6R welding robot ［J］. Transactions of the Chinese Society for Agricultural Machinery， 2017， 48（8）： 384-390， 412.

［5］ 張毅， 劉芳君， 胡磊. 結(jié)合MPGA-RBFNN的一般機(jī)器人逆運(yùn)動(dòng)學(xué)求解［J］. 智能系統(tǒng)學(xué)報(bào)， 2019， 14（1）： 165-170.

Zhang Yi， Liu Junfang， Hu Lei. A general robot inverse kinematics solution based on MPGA-RBFNN ［J］. Transactions on Intelligent Systems， 2019， 14（1）： 165-170.

［6］ Deepak B， Parhi D R. Control of an automated mobile manipulator using artificial immune system ［J］. Journal of Experimental & Theoretical Artificial Intelligence， 2016， 28（1-2）： 417-439.

［7］ Hebert P， Bajracharya M， Ma J， et al. Mobile manipulation and mobility as manipulation—Design and algorithms of RoboSimian ［J］. Journal of Field Robotics， 2015， 32（2）： 255-274.

［8］ Roa M A， Berenson D， Huang W. Mobile manipulation： Toward smart manufacturing ［J］. IEEE Robotics & Automation Magazine， 2015， 22（4）： 14-15.

［9］ 周鋒. 基于迭代學(xué)習(xí)算法的六自由度機(jī)械臂運(yùn)動(dòng)學(xué)求解分析［D］. 合肥： 中國(guó)科學(xué)技術(shù)大學(xué)， 2017.

Zhou Feng. Analysis of kinematics solution for six degree of freedom robot arm based on iterative learning algorithm ［D］. Hefei： University of Science and Technology of China， 2017.

［10］ 戴厚德， 曾現(xiàn)萍， 游鴻修， 等. 基于光學(xué)運(yùn)動(dòng)跟蹤系統(tǒng)的機(jī)器人末端位姿測(cè)量與誤差補(bǔ)償［J］. 機(jī)器人， 2019， 41（2）： 206-215.

Dai Houde， Zeng Xianping， You Hongxiu， et al. Pose measurement and error compensation of the robot end-effector based on an optical tracking system ［J］. Robot， 2019， 41（2）： 206-215.

［11］ 李瑋， 章逸豐， 王鵬， 等. 一種采用迭代優(yōu)化的移動(dòng)抓取規(guī)劃方法［J］. 機(jī)器人， 2019， 41（2）： 165-184.

Li Wei， Zhang Yifeng， Wang Peng， et al. A mobile grasp planning method based on iterative optimization ［J］. Robot， 2019， 41（2）： 165-174， 184.

［12］ Alebooyeh M， Urbanic R J. Neural network model for identifying workspace， forward and inverse kinematics of the 7-DOF YuMi 14000 ABB collaborative robot ［J］. IFAC-PapersOnLine， 2019， 52（10）： 176-181.

［13］ 李光， 章曉峰， 楊加超， 等. 基于殘差BP神經(jīng)網(wǎng)絡(luò)的6自由度機(jī)器人視覺(jué)標(biāo)定［J］. 農(nóng)業(yè)機(jī)械學(xué)報(bào)， 2021， 52（4）： 366-374.

Li Guang， Zhang Xiaofeng， Yang Jiachao， et al. Vision calibration of six degree of freedom robot based on residual BP neural network ［J］. Transactions of the Chinese Society for Agricultural Machinery， 2021， 52（4）： 366-374.

［14］ 劉世平， 曹俊峰， 孫濤， 等. 基于BP神經(jīng)網(wǎng)絡(luò)的冗余機(jī)械臂逆運(yùn)動(dòng)學(xué)分析［J］. 中國(guó)機(jī)械工程， 2019， 30（24）： 2974-2977.

Liu Shiping， Cao Junfeng， Sun Tao， et al. Inverse kinematics analysis of redundant manipulators based on BP neural network ［J］. China Mechanical Engineering， 2019， 30（24）： 2974-2977.

［15］ Zubizarreta A， Larrea M， Irigoyen E， et al. Real time direct kinematic problem computation of the 3PRS robot using neural networks ［J］. Neurocomputing， 2018， 271： 104-114.

［16］ Lin G， Zhu L， Li J， et al. Collision-free path planning for a guava-harvesting robot based on recurrent deep reinforcement learning ［J］. Computers and Electronics in Agriculture， 2021， 188： 106350.

［17］ Lenz I， Lee H， Saxena A. Deep learning for detecting robotic grasps ［J］. The International Journal of Robotics Research， 2015， 34（4-5）： 705-724.

［18］ Rokbani N， Alimi A M. Inverse kinematics using particle swarm optimization， a statistical analysis ［J］. Procedia Engineering， 2013， 64： 1602-1611.

［19］ El-Sherbiny A， Elhosseini M A， Haikal A Y. A new ABC variant for solving inverse kinematics problem in 5 DOF robot arm ［J］. Applied Soft Computing， 2018， 73： 24-38.

［20］ 林陽(yáng)， 趙歡， 丁漢. 基于多種群遺傳算法的一般機(jī)器人逆運(yùn)動(dòng)學(xué)求解［J］. 機(jī)械工程學(xué)報(bào)， 2017， 53（3）： 1-8.

Lin Yang， Zhao Huan， Ding Han. Solution of inverse kinematics for general robot manipulators based on multiple population genetic algorithm ［J］. Journal of Mechanical Engineering， 2017， 53（3）： 1-8.

［21］ 石建平， 李培生， 劉國(guó)平， 等. 基于混合學(xué)習(xí)果蠅優(yōu)化算法的冗余機(jī)械臂逆運(yùn)動(dòng)學(xué)求解［J］. 農(nóng)業(yè)機(jī)械學(xué)報(bào)， 2021， 52（9）： 410-416.

Shi Jianping， Li Peisheng， Liu Guoping， et al. Inverse kinematics solution of redundant manipulator based on hybrid learning fruit fly optimization algorithm ［J］. Transactions of the Chinese Society for Agricultural Machinery， 2021， 52（9）： 410-416.

［22］ Ren Z， Wang Z， Sun L. A hybrid biogeography-based optimization method for the inverse kinematics problem of an 8-DOF redundant humanoid manipulator ［J］. Frontiers of Information Technology & Electronic Engineering， 2015， 16（7）： 607-616.

［23］ 謝習(xí)華， 范詩(shī)萌， 周烜亦， 等. 基于改進(jìn)差分進(jìn)化算法的機(jī)械臂運(yùn)動(dòng)學(xué)逆解［J］. 機(jī)器人， 2019， 41（1）： 50-57.

Xie Xihua， Fan Shimeng， Zhou Xuanyi， et al. Inverse kinematics of manipulator based on the improved differential evolution algorithm ［J］. Robot， 2019， 41（1）： 50-57.

［24］ 李松. 解非線性方程的迭代算法及其進(jìn)一步研究［D］. 合肥： 合肥工業(yè)大學(xué)， 2013.

Li Song. Iterative methods and further research for solving nonlinear equations ［D］. Hefei： Hefei University of Technology， 2013.

［25］ 陳光榮， 郭盛， 侯博文， 等.基于擴(kuò)展雅可比矩陣的冗余液壓驅(qū)動(dòng)四足機(jī)器人運(yùn)動(dòng)控制［J］. 控制理論與應(yīng)用， 2021， 38（2）： 213-223.

Chen Guangrong， Guo Sheng， Hou Bowen， et al. Motion control of a redundant hydraulic actuated quadruped robot based on extended Jacobian matrix ［J］. Control Theory & Applications， 2021， 38（2）： 213-223.

［26］ 吉陽(yáng)珍， 侯力， 羅嵐， 等. 基于組合優(yōu)化算法的6R機(jī)器人逆運(yùn)動(dòng)學(xué)求解［J］. 中國(guó)機(jī)械工程， 2021， 32（10）： 1222-1232.

Ji Yangzhen， Hou Li， Luo Lan， et al. Solution of inverse kinematics for 6R robots based on combinatorial optimization algorithm ［J］. China Mechanical Engineering， 2021， 32（10）： 1222-1232.

［27］ 韓磊， 刁燕， 張希斌， 等. 基于改進(jìn)牛頓迭代法的手腕偏置型六自由度關(guān)節(jié)機(jī)器人逆解算法［J］. 機(jī)械傳動(dòng)， 2017， 41（1）： 127-130， 150.

Han Lei， Diao Yan， Zhang Xibin， et al. Inverse kinematics algorithm for 6-DOF joint robot with offset wrist based on modified Newton iteration method ［J］. Journal of Mechanical Transmission， 2017， 41（1）： 127-130， 150.

［28］ 沈惠平， 顧曉陽(yáng)， 李菊， 等. 基于拓?fù)浣雕畹?T1R并聯(lián)機(jī)構(gòu)設(shè)計(jì)與運(yùn)動(dòng)學(xué)特性分析［J］. 農(nóng)業(yè)機(jī)械學(xué)報(bào)， 2021， 52（8）： 406-415.

Shen Huiping， Gu Xiaoyang， Li Ju，et al. Topological coupling-reducing based design of 3T1R parallel mechanism and kinematics performances analysis ［J］. Transactions of the Chinese Society for Agricultural Machinery， 2021， 52（8）： 406-415.

［29］ 張志強(qiáng)， 臧冀原， 贠超. 混聯(lián)碼垛機(jī)器人運(yùn)動(dòng)學(xué)分析及仿真［J］. 機(jī)械設(shè)計(jì)， 2010， 27（11）： 47-51.

Zhang Zhiqiang， Zang Jiyuan， Yun Chao. Kinematics analysis and simulation of series-parallel palletizing robot ［J］. Journal of Machine Design， 2010， 27（11）： 47-51.

［30］ Huang Q， Yokoi K， Kajita S， et al. Planning walking patterns for a biped robot ［J］. IEEE Transactions on Robotics and Automation， 2001， 17（3）： 280-289.