约束优化二次规划与SQP
两栖球形机器人的路径规划策略
第２８卷㊀第８期２０２０年８月㊀㊀㊀㊀㊀㊀㊀㊀㊀㊀㊀㊀光学精密工程㊀O p t i c s a n dP r e c i s i o nE n g i n e e r i n g㊀㊀㊀㊀㊀㊀㊀V o l ．２８㊀N o ．８㊀A u g．２０２０㊀㊀收稿日期:２０２０Ｇ０１Ｇ０９;修订日期:２０２０Ｇ０２Ｇ１８．㊀㊀基金项目:吉林农业科技学院大学生科技创新创业训练计划资助项目(N o ．２０１９１１４３９０２７);吉林农业科技学院青年基金资助项目(N o ．２０１９０５０５)文章编号㊀１００４Ｇ９２４X (２０２０)０８Ｇ１７３３Ｇ１０两栖球形机器人的路径规划策略马宇科１,郑㊀亮１,２∗,胡高凯１,吉晓雯１,司兆怡１,刘晏彤１(１．吉林农业科技学院,吉林吉林１３２１０１;２．长春理工大学,吉林长春１３００２２)摘要:两栖机器人的水下路径最优规划是目前机器人运动控制研究领域的热点和难点.本文针对两种基于视觉伺服的广义约束优化(G C O P )和序列二次规划(S Q P )的机器人运动控制算法进行对比分析,结合视觉伺服传感器,实现了两栖机器人最佳路径的规划㊁监测动态目标标定㊁移动目标监测㊁水下障碍物识别和目标跟踪.利用球形机器人的结构对称特性及阿基米德浮力原理,并结合模糊控制算法对水舱水位进行实时控制,使球形两栖机器人在水下能实现水下多自由度运动.最后,进行了算法的仿真和水下运动实验.实验结果表明,G C O P 算法和S Q P 算法在相对障碍物的有限距离内,S Q P 算法规划的路径更加合理;而在达到目标坐标位置上,两种算法的误差为１６７．５m m ,S Q P 算法在水下路径规划上更加有效.关㊀键㊀词:两栖机器人;球形机器人;路径规划;目标识别中图分类号:T H ７３㊀㊀文献标识码:A㊀㊀d o i :１０．３７８８/O P E ．２０２０２８０８．１７３３P a t h p l a n n i n g s t r a t e g y o f a m p h i b i o u s s ph e r i c a l r o b o t MA Y u Ｇk e １,Z H E N GL i a n g １,２∗,HU G a o Ｇk a i １,J IX i a o Ｇw e n １,S I Z h a o Ｇy i １,L I U Y a n Ｇt o n g１(１．J i l i nA g r i c u l t u r a lS c i e n c e a n dT e c h n o l o g y U n i v e r s i t y ,J i l i n １３２１０１,C h i n a ;２．C h a n g c h u nU n i v e r s i t y o f S c i e n c e a n dT e c h n o l o g y ,C h a n gc h u n １３００２２,C h i n a )∗C o r r e s p o nd i n g a u t h o r ,E Ｇm a i l :s １８d ５０５＠s t u ．k a g a w a Ｇu ．a c ．j pA b s t r a c t :T h eu n d e r w a t e r p a t h p l a n n i n g o f a m p h i b i o u s s p h e r i c a l r o b o t s i s c u r r e n t l y a re s e a r c hc h a l Ｇl e n g e i n t h ef i e l d o f a m p h i b i o u s r o b o tm o t i o n c o n t r o l ．I n t h i s s t u d y ,t w o t y p e s o f r o b o tm o t i o n c o n t r o l a lg o r i th m s ,n a m e l y G e n e r a li z e dC o n s t r a i n tO p t i m i z a t i o n (G C O P )a n dS e q u e n t i a lQ u a d r a t i cP r o gr a m Ｇm i n g (S Q P )a l g o r i t h m sb a s e do nv i s u a l s e r v o ,w e r ec o m p a r e da n da n a l y z e d ．T h eo pt i m a l p a t h p l a n Ｇn i n g o f t h ea m p h i b i o u ss p h e r i c a l r o b o tw a s r e a l i z e d ,c o m b i n a t e dw i t hv i s u a l s e r v os e n s o r s ．D yn a m i c t a r g e t c a l i b r a t i o n ,m o v i n g t a r g e tm o n i t o r i n g ,u n d e r w a t e ro b s t a c l er e c o g n i t i o n ,a n dt a r g e t t r a c k i n g Ｇf u n c t i o n sw e r ea l s o d e v e l o p e d ．F u r t h e r m o r e ,t h i ss t u d y c o n s i d e r e dt h es ym m e t r i c a ls t r u c t u r eo f s p h e r i c a l r o b o t s (u s i n g A r c h i m e d e s ᶄb u o y a n c y p r i n c i p l e )a n d c o m b i n e d f u z z y c o n t r o l a l g o r i t h m s t o c o n Ｇt r o l t h ew a t e r l e v e l o f t h ew a t e r t a n ks o t h a t s p h e r i c a l a m ph i b i o u s r o b o t s c a na c h i e v em u l t i ＧD O Fu n Ｇd e r w a t e rm o t i o n ．F i n a l l y ,a l g o r i t h ms i m u l a t i o n s a n du n d e r w a t e rm o t i o ne x p e r i m e n t sw e r e p e r f o r m e d t ov e r i f y t h e f e a s i b i l i t y o f t h e p r o p o s e dm e t h o d ．T h e r e s u l t s s h o wt h a t p a t h p l a n n i n g b y t h e S Q Pa l go Ｇ. All Rights Reserved.r i t h mi sm o r e r e a s o n a b l e c o n s i d e r i n g t h e d i s t a n c e b e t w e e n t h eG C O Pa n dS Q Pa l g o r i t h m s,r e l a t i v e t o t h eo b s t a c l e．I n r e a c h i n g t h e t a r g e t c o o r d i n a t e p o s i t i o n,t h e e r r o r b e t w e e n t h e t w o a l g o r i t h m s r e a c h e s t o１６７．５m m,s h o w i n g t h a t t h e S Q P a l g o r i t h m i s s u p e r i o r i n u n d e r w a t e r p a t h p l a n n i n g t h a n t h eG C O P a l g o r i t h m．K e y w o r d s:a m p h i b i o u s r o b o t;s p h e r i c a l r o b o t;p a t h p l a n n i n g;t a r g e t r e c o g n i t i o n１㊀引㊀言㊀㊀目前,在机器人研究领域,两栖仿生机器人的研究逐步成为研究热点,通过对一种动物运动方式的长时间观察与研究,提出了相应具有运动模式的仿生机器人的设计方案.这类机器人具有较为灵活的两栖运动能力,能够在水下实现最佳的路径控制和对目标具有一定的识别与追踪能力,但是,以往提出的仿生机器人大多只适应于水下的运动环境,对于目前所需要的两栖运动环境并不合适,球形两栖机器人能够很好地解决这一问题.在一些特殊的应用环境下,例如两栖侦察㊁海底探测㊁深海探测等相关领域,一般性能的机器人无法满足要求,所以两栖机器人应运而生.由于两栖机器人特殊的灵活性㊁超强的适应能力㊁便于投放和回收的优越特性,使它可以独立在水下完成侦查㊁搜救㊁探测㊁数据收集等工作.所以两栖球形机器人很自然地成为人类延伸自己感知能力的主要工具之一.本文提出的两栖球形机器人是一种先进的执行装置,包括运动推进器,传感器,控制板和安装在球形壳体中的电源装置.作为微型球形机器人,这种机器人得到广泛应用主要依靠四个技术优势.第一个特点是球形机器人是一种可移动行走的移动机器人,可以保持先进的平衡性㊁稳定性和运动连续性.第二个特点是球形机器人具有良好的密封性,可以完全保护内部控制单元和机构,这是其他机器人无法做到的.第三个特点是球形机器人具有很强的适应性,能在无人区㊁灰尘㊁湿气㊁腐蚀性和恶劣环境下完成任务.最后是矢量推进器具有更高的稳定性和灵活性,能使机器人保持更好的水下运动性能和抗噪声干扰能力.两栖机器人控制技术发展迅速,北京理工大学仿生机器人与系统教育部重点实验室郭书祥团队研究的两栖球形机器人,是以球形为主体的机器人,整体结构分为上半球和下半球.在陆地模式时,下半球可以折叠到上半球,用４个由８个舵机组成的机械臂行走,在水下模式的时候,折叠的上半球通过二个舵机封闭下半球,由喷水电机推进行走,球体内部也安装了通信和稳定控制模块,该球基本实现了球形机器人的基本功能,但没有实现水下的自动路径规划[１Ｇ３].北京邮电大学孙汉旭团队研究的球形机器人以摩擦力为驱动力,没有被动摩擦力的球形机器人,该机器人具有运动效率高,对路面要求低,适应能力强等优点[４].哈尔滨工程大学叶秀芬团队,也对球形机器人 基于喷水推进的微小球形水下潜器 进行了深入的研究[５].天津理工大学郭健团队也对球形机器人在陆地上的路径规划进行了深入的研究[６].但目前针对水下路径规划的相关研究相对较少.机器人执行有障碍的复杂水下任务时,路径规划对水下机器人实现任务目标具有非常重要的意义.文献[７]提出了一种基于先验知识强化学习策略的最佳路径选择的新算法.针对未知空间中移动机器人的路径规划问题,Y u a n等提出了一种基于门控递归单元Ｇ递归神经网络模型的动态路径规划方法[８].B a e等提出了一种结合深度学习和卷积神经网络的多机器人路径规划算法[９].文献[１０]提出了一种非完整的三轮移动机器人的路径规划和控制方法,该机器人用于在道路跟踪和复杂环境中进行在线导航.文献[１１]开发了一种 增强轮辋跳跃 的方法,该方法不依赖于逐点定位,而是通过找到障碍物之间的多次切线来获得最短路径.尽管在两栖球形机器人的路径规划方面已有许多研究,但是多数研究是基于单一的陆地环境下进行的.本文以实现球形两栖机器人最佳路径规划为研究目标,针对两种基于视觉伺服的广义约束优化(G e n e r a l i z e dC o n s t r a i n tO p t iＧm i z a t i o n,G C O P)和序列二次规划(S e q u e n t i a l Q u a d r a t i cP r o c o n t r o l,S Q P)的机器人运动控制算法进行对比分析,使用视觉伺服传感器实现两栖４３７１㊀㊀㊀㊀㊀光学㊀精密工程㊀㊀㊀㊀㊀第２８卷㊀. All Rights Reserved.机器人的最佳路径规划,并实时对监测动态目标进行标定㊁移动目标监测㊁水下障碍物识别和目标跟踪.最后,通过水下运动测试和仿真来对比验证算法的可行性.该运动的路径规划方案提高了球形两栖机器人的运动性能,使机器人可以执行更为复杂的水下任务.２㊀两栖球形机器人设计２．１㊀机械设计新型两栖球形机器人不仅具有良好的陆地运动能力,而且能够实现水下多自由度的运动.图１是机器人陆地运动模式和水下运动模式的结构.在陆地模式下,机器人下半球壳体通过两个舵机折叠至上半球,４个喷水推进器根据相应步态调整实现陆地行走[１２].在水下模式下,下半球闭合,机器人可以利用４个水下推进器实现水下多自由度运动.相比其他两栖球形机器人,加入了稳定控制块㊁４个激光测距模块和４个视觉采集模块,用来实现路径规划的相关参数采集.图１㊀两栖球形机器人的机械结构F i g ．１㊀M e c h a n i s mo f a m p h i b i o u s s ph e r i c a l r o b o t ２．２㊀硬件构成两栖球形机器人采用模块化设计,通过各模块的合理分布与协调工作,保证球体运动的可靠性和稳定性.如图２所示,球体主要搭载一块嵌入式处理器(A R M S ３C ６４１０,２G B D D R ３,l i n u x３．１２．０),用于数据处理;G P S 模块和声纳模块用于机器人通讯.陀螺仪传感器对机器人进行姿态感应与调整,伺服电机和电机控制器控制机器人的运动和姿态,图像采集模块用于机器人的视觉识别与动态目标捕捉,５０００m A 锂电供电模块用于机器人的供电.图２㊀两栖球形机器人的硬件结构F i g ．２㊀H a r d w a r es t r u c t u r eo fa m p h i b i o u ss ph e r i c a l r o b ot图３㊀机器人上浮和下潜原理F i g ．３㊀P r i n c i p l e f o r r o b o t f l o a t i n g a n dd i v i n g３㊀两栖球形机器人上浮下潜原理㊀㊀球体分为上半球和下半球两个仓室,如图３所示,下仓室装有排水口,通过上仓室的仓室气泵将上仓室的空气压入下仓室,通过下仓室的水压调节罐控制水箱的进水量,根据阿基米德浮力原理,球体的浮力由球体排开水的体积决定.当机５３７１第８期㊀㊀㊀㊀㊀㊀㊀马宇科,等:两栖球形机器人的路径规划策略. All Rights Reserved.器人下潜时,防水舵机顺时针旋转,将放气的阀门关闭,水从球体一端的孔进入,由于压力的作用,原本在球体内部的气体从球的另一端排出.水的重量再加上球体本身防水舵机的重量使球形机器人进行下潜.上浮时,防水舵机逆时针旋转,将二氧化碳气瓶中的压缩二氧化碳气体放出,此时排气孔和进水孔都变成了排水孔,机器人上浮.４㊀运动控制和路径规划算法４．１㊀非线性６自由度算法两栖球形机器人能够在陆地与水下多自由度运动,在陆地上要具有４个自由度,在水下要具有６个自由度的运动模式.图４㊀６自由度建模坐标标定F i g ．４㊀C a l i b r a t i o no f ６ＧD O Fm o d e l i n g co o r d i n a t e ６自由度包括R P Y (q ,p ,r )和Z Y Z (v ,u ,w )参考系.为了达到精确控制的目的,分别给出了两种受控源的旋转矩阵.如图４所示,参考坐标以x 轴围绕的角度 q 旋转,并用矩阵R x (q )表示.参考坐标以Y 轴围绕的角度 p旋转,并用矩阵R y (p )表示.参考坐标以Z 轴围绕的角度 r 旋转,并用矩阵R z (r )表示[１３].R (φ)＝Rx (q )R y (p )R z (r )＝C q C p C q S p S r －S q C r C q S p C r ＋S q S r S q S p S q S p S r ＋C q C r S q S p S r ＋C q S r －S pC p S r C q C r æèçççöø÷÷÷．(１)㊀㊀假设参考坐标绕Z 轴旋转一个φ的角度,并且旋转矩阵为R z (φ),参考坐标绕Y 轴旋转一个ϑ的角度,并且旋转矩阵为R yᶄ(ϑ),参考坐标绕Z 轴旋转一个ψ的角度,并且旋转矩阵为R z ᵡ(ψ).最终坐标系的方向是通过合成相对于当前坐标系的旋转矩阵并通过右乘计算得出的,获得基本矩阵为:R (Φ)＝R z (φ)R y ᶄ(ϑ)R z ᵡ(φ)．(２)㊀㊀旋转矩阵为:R ＝r １１r １２r １３r ２１r ２２r ２３r ３１r ３２r ３３æèçççöø÷÷÷．(３)㊀㊀参数ϑ在[π,０],可以表示为:φ＝A t a n２(r ２３,r １３)ϑ＝A t a n２(r ２１３＋r ２２３,r ３３)ψ＝At a n２(r ３２,－r ３１)ìîíïïïï．(４)㊀㊀参数φ在[－π,０],可以表示为:φ＝A t a n２(－r ２３,r １３)ϑ＝A t a n２(－r ２１３＋r ２２３,r ３３)ψ＝At a n２(－r ３２,－r ３１)ìîíïïïï．(５)４．２㊀G C O P 控制算法假设物体的表面由m 个方程表示为:h i (x ),i ＝１,２, ,m ,并且它的内部方程为[１４Ｇ１６]:h １＜０ɡh ２ɡ ɡh m ＜０．(６)㊀㊀对每个h i 构造新的函数:v i ＝(h ２i ＋t ２)１/２＋h i ,(７)其中:t 是一个小的正实数,v i 是x 和t 的函数,对整个物体,构造函数V :V ＝v １＋v ２＋ ＋v m ＝ðmi ＝１v i．(８)㊀㊀验证从h i 到v i 和v i 到V 的两个变换的性质.首先,函数v i 对任意x 和t 总是正的,其次,v i 是关于h i 的递增函数,即当h i ＞０时v i 的值和当h i ＜０时v i 的值.如果t ≪１,v i 可以近似表示为:v i ＝２h i ＋O (t ２)≫t ＞０,h i ＞０v i ʈt ,h i ＝０v i ʈO (t ２),h i ＜０ìîíïïï,(９)其中:O (t ２)为一个值非常小的正数.式(９)表明,除了点在物体表面附近时,h i ＝０;当h i ＞０时,v i ＞t ;当h i ＜０时,v i ＜t .对于在物体h i 内部和边界附近的点,其他函６３７１㊀㊀㊀㊀㊀光学㊀精密工程㊀㊀㊀㊀㊀第２８卷㊀. All Rights Reserved.数值h i (j ＝１,２,．．．,m ,j ʂi )是小于零的,因此得到v t ＝O (t ２).将式(７)替换到式(９)中,得到:V ≫t ,(h １＞０)＋(h ２＞０)＋ ＋(h m ＞０)V ʈt ＋O (t ２),(h １＝０ɡh j ＜０,j ＝２,３, ,m )＋(h ２＝０ɡh j ＜０,j ＝㊀㊀１,３,４, ,m )＋ ＋(h m ＝０ɡh j ＜０,j ＝１,２, ,m －１)V ʈO (t ２),(h １＜０)ɡ(h ２＜０)ɡ ɡ(h m ＜０)ìîíïïïïï．(１０)㊀㊀从式(１０)可以看出,当所有的h i 是负,即点在物体内部时,函数V 非常小;当点在物体外部时,V ≫t ＋O (t ２);当点在物体边界附近时,V ʈt.考虑当t ң０时,则有:v i ＞０,h i ＞０v i ＝０,h i ɤ０{．(１１)㊀㊀两个正数的和还是正数,一个正数与零的和是正数,两个零的和是零.因此,如果把正数作为逻辑 １ ,把零作为逻辑 ０ ,那么式(１１)中v i 的加 操作对应了布尔 或 运算.所以得到:V ＞０,(h １＞０)ᶱ(h ２＞０)ᶱ ᶱ(h m ＞０)V ＝０,(h １ɤ０)ɡ(h ２ɤ０)ɡ ɡ(h m ɤ０){．(１２)㊀㊀在水下机器人路径规划中,不期望路径太靠近障碍物.因此,引入一个小的正数Δv 作为路径到障碍物的距离控制参数,如果x 满足以下不等式:V ＝ðviȡΔv 或Δv －ðv i ＜０．(１３)㊀㊀那么这个点一定在由式(１３)所确定的障碍物的外部.如果Δv ң０,由Δv －ðv i ɤ０确定的边界将趋近于障碍物的表面.如果一个物体的表面和外部由(h １ȡ０)ᶱ(h ２ȡ０)ᶱ ᶱ(h m ȡ０)确定,那么它的外部和表面同样可以由Δv －ðv i ɤ０确定,其中Δv ң０.x 若满足Δv －ðv i ɤ０,那么x 就落在物体的外部.４．３㊀S QP 控制算法为了在三个维度上对两栖球形机器人进行建模,必须设定６个独立变量构建环境的空间位置㊁方向㊁大小和形状,O ＝O (x ０,y ０,z ０)代表机器人的几何中心,θ＝(θ１,θ２,θ３)代表机器人的定位方向[１７Ｇ１９].x ＝x (x ０,y０,z ０,θ１,θ２,θ３,v ),(１４)y ＝y (x ０,y ０,z ０,θ１,θ２,θ３,v ),(１５)z ＝z (x ０,y０,z ０,θ１,θ２,θ３,v ),(１６)其中:x ,y ,z 是机器人边界上的一个点,用于构造没有碰撞条件的机器人,x ０,y０,z ０是空间位置,θ１,θ２,θ３是空间方向,v 是具有两个参数的向量,(t １,t ２)用于表示特定机器人的边界点,球形机器人定义如下:((x －x ０)/r x )２/s ２＋((y －y ０)/r y )２/s ２[]s ２/s １＋((z －z ０)/r z )s /s １＝１,(１７)x ＝r x c o s s １(t １)c o s s ２(t ２),(１８)y ＝r y co s s １(t １)c o s s ２(t ２),－π/２ɤt １ɤπ/２,(１９)z ＝r z c o s s １(t １),０ɤt １ɤ２π．(２０)㊀㊀基于S Q P 算法,路径规划问题转化为半无限约束优化问题.假设空间中有n 个障碍物,则j个障碍物的表面可以表示为[２０]:h １(x ,y ,z )＝１,j ＝１,２, ,n ．(２１)㊀㊀整个可用空间可表示为:１－h １(x ,y ,z )ɤ０,j ＝１,２, ,n ．(２２)㊀㊀没有碰撞的必要和充分条件是曲面上所有点都必须无碰撞,获得无碰撞的充分条件是:１－h j (x l o c ,y l o c ,z l o c )ɤ１,(２３)其中:x l o c ＝x (x ０,y０,z ０,θ１,θ２,θ３,v )y l o c ＝y (x ０,y０,z ０,θ１,θ２,θ３,v )z l o c ＝z (x ０,y０,z ０,θ１,θ２,θ３,v )．(２４)㊀㊀在路径规划中,配置变量需要一个约束,如公式(２５)所示[２１Ｇ２２]:x ０l ɤx ０ɤx ０u ,y０l ɤy ０ɤy ０u ,z ０l ɤz ０ɤz ０u ,θ１l ɤθ１ɤθ１u ,θ２l ɤθ２ɤθ２u ,θ３l ɤθ３ɤθ３u ．(２５)其中(x ０l ,y ０l ,z ０l )和(x ０u ,y ０u ,z ０u )分别是上限和下限,推导得到:１－h j (r x c o s s １(t １)c o s s ２(t ２)(c o s (θ１)c o s (θ２)c o s (θ３)－s i n (θ１)s i n (θ３))－r yc o s s １(t １)s i n s ２(t ２)(c o s (θ１)s i n (θ３)－c o s (θ２)s i n (θ２)s i n (θ３))＋r z s i n s １(t １)(c o s (θ１)s i n (θ２))＋x ０,７３７１第８期㊀㊀㊀㊀㊀㊀㊀马宇科,等:两栖球形机器人的路径规划策略. All Rights Reserved.r x c o s s １(t １)c o s s ２(t ２)(－c o s (θ１)c o s (θ２)s i n (θ３)－s i n (θ１)c o s (θ３))＋r yc o s s １(t １)s i n s ２(t ２)(s i n (θ１)c o s (θ２)s i n (θ３)－c o s (θ１)s i n (θ３))＋r z s i n s １(t １)s i n (θ２)s i n (θ３)＋y ０,r x c o s s １(t １)c o s s ２(t ２)c o s (θ１)s i n (θ２)＋r yc o s s １(t １)s i n s ２(t ２)s i n (θ１)s i n (θ２)＋r z s i n s １(t １)c o s (θ２)＋z ０ɤ０．(２６)㊀㊀在方程中,－π/２ɤt １ɤπ/２,０ɤt ２ɤ２π.在非线性规划问题中,目标函数必须具有下限的最小值.目标设置必须使全局最小值可变,二次函数的目标函数表示为:f (x ０,y ０,z ０,θ１,θ２,θ３)＝w ((x ０－x ０g )２＋(y ０－y ０g )２＋(z ０－z ０g )２)＋(１－w )((θ１－θ１g )２＋(θ２－θ２g )２＋(θ３－θ３g )２)．(２７)㊀㊀其最优化点的坐标为(x ０g ,y ０g ,z ０g ),最优化角度为(θ１g ,θ２g ,θ３g ),并且满足方程式:m i n f (x ０,y０,z ０,θ１,θ２,θ３)＝(x ０g ,y ０g ,z ０g ,θ１g ,θ２g ,θ３g )＝０．(２８)㊀㊀在前面的公式中,w 表示用于调整空间位置(x ０－x ０g )２＋(y ０－y ０g )２＋(z ０－z ０g )２和机器人空间方向(θ１－θ１g )２＋(θ２－θ２g )２＋(θ３－θ３g )２之间的相对关系的权重,当w ＝１时计算空间位置.当w ＝０．５时,空间位置和空间的权重方向必然相等.５㊀实验及结果分析５．１㊀仿真实验为验证两种基于路径规划的G C O P 和S Q P在水下两栖机器人的路径规划过程中的有效性,使用MA T L A B 设置了两种算法,从初始起始坐标O l ＝(－２００,－２００,－２００)到达目标O u ＝(６００,６００,６００)的仿真实验的场景范围.场景空间的长㊁宽和高为６００c m 的立方体形状.基于S Q P 算法的不等式O l ɤO ɤO u 通过四个参数确保生成的路径在场景内,坐标分布分别在三维平面上的O s ＝(x ０s ,y ０s ,z ０s ),θs ＝(θ１s ,θ１s ,θ１s ),O g ＝(x ０g ,y ０g ,z ０g )和θg ＝(θ１g ,θ２g ,θ３g ).GC O P 算法的初始时刻设定t ＝０,Δv ＝０．０００１,起点坐标为:(x ０s ,y０s ,z ０s ,θ１s ,θ２s ,θ３s )＝(－２００,－２００,－２００,０,０,０),(２９)(x ０g ,y ０g ,z ０g ,θ１g ,θ２g ,θ３g )＝(５００,５００,５００,０,０,０)．(３０)㊀㊀目标函数定义为:f ＝ω[(x ０－５００)２＋(y ０－５００)２＋(z ０－５００)２]＋(１－ω)(θ２１＋θ２２＋θ２３)．(３１)㊀㊀如果满足等式,则该坐标点不会与球形障碍物碰撞.在模拟实验中设置了４个球形障碍物,为确保基于两种算法的准确性.４个障碍物的坐标和方程定义为:(x －２００)２＋(y －２００)２＋(z －２００)２＝５０２,x ２＋(y －２００)２＋(z －２００)２＝３０２,(x －４００)２＋(y －２００)２＋(z －２００)２＝３０２,(x －２００)２＋y ２＋(z －２００)２＝３０２．(３２)㊀㊀球形机器人的参数为(r x ,r y ,r z )是对象的几何间隔(s １＝１,s ２＝１．５表示机器人的形状是球形):r x ＝５,r y ＝４,r z ＝３,s １＝１,s ２＝１．５．(３３)㊀㊀图５为仿真结果(彩图见期刊电子版),在起点和目标点之间有４个障碍.粉色球和灰色球表示不同算法的两栖机器人的运动轨迹.３个蓝色球体和１个红色球体分别代表障碍物.机器人从起始位置到目标位置经过４个障碍物,两种算法比较,S Q P 算法在路径规划上更加合理,并在３个采样点处(黄色圆点)离障碍８３７１㊀㊀㊀㊀㊀光学㊀精密工程㊀㊀㊀㊀㊀第２８卷㊀. All Rights Reserved.图５㊀两种路径规划算法仿真分析F i g．５㊀S i m u l a t i o na n a l y s i so f t w o p a t h p l a n n i n g a lＧg o r i t h m s物始终保持了安全距离.但G C O P算法选择的路径在坐标点(－１００,１９０,３３２．５)处离障碍物较近,如机器人发生小角度偏移,有发生碰撞的危险.所以S Q P算法比G C O P算法在安全路径规划的路径控制方面更加有效.而在达到目标坐标位置上,两种算法的误差Δd＝１６７．５m m,因此S Q P算法在水下路径规划上更具有优势.５．２㊀水下实验为了进一步验证文章所提出算法的有效性,本文设计了基于S Q P算法的水下测试实验,实验是在长１５００m m,宽为１０００m m,高度为８００m m的封闭水池环境下进行,如图６所示.图中设置了跟仿真环境相匹配的４个球形障碍物,分别设计障碍物的固定坐标位置.机器人从起点出发,激光测距模块和图像采集传感器实时采集障碍物在水中的坐标,并实时调整机器人的运动轨迹,按算法程序中设定的最佳路径移动到目标位置.图７(a)是t＝０s时刻机器人的初始位置,箭头标明了规划的机器人最优路径轨迹.图７(b)~７(i)是机器人从t＝０s移动至t＝１５s的实际运动轨迹.在实验过程中,采集６个图６㊀球形机器人避障实验环境F i g．６㊀E x p e r i m e n t a l s e t u p f o ra v o i d i n g o b s t a c l e so fr o b ot图７㊀S Q P路径规划算法的实验图片F i g．７㊀P h o t o e s o f p a t h p l a n n i n g b y S Q Pa l g o r i t h m 不同的时间点,对机器人y轴和z轴方向的位移变化曲线进行采样,从而判断机器人在最佳路径选择上的稳定性和可靠性.从图８可以看出,机器人分别在第二个和第三个采样点偏移误差较大,误差值达到２０m m,在其余４个采样点误差小于１０m m,这是因为机器人在第二个到第三个采样点主要是对路径的选择阶段,从而影响了机器人的运动形态.机器人的运动轨迹如图９所示.９３７１第８期㊀㊀㊀㊀㊀㊀㊀马宇科,等:两栖球形机器人的路径规划策略. All Rights Reserved.图８㊀球形机器人y轴和z轴运动方向误差F i g．８㊀K i n e m a t i c e r r o r s o f r o b o t i n yＧa x i s a n d zＧa x i s图９㊀球形机器人的最佳路径轨迹F i g．９㊀O p t i m a l p a t h t r a j e c t o r y o f s p h e r i c a l r o b o t ６㊀结㊀论㊀㊀本文提出了两种基于路径规划的算法(G C O P和S Q P),通过设置４个不同尺寸的球形障碍物使两栖球形机器人可以通过视觉伺服传感器实现避障功能并选择最佳路径.在实验中,设置３个采样点对两种路径规划算法进行评估,基于G C O P算法在(－１００,１９０,５００)和(－５,２００,３４５．８)处距离障碍物较近;而基于S Q P算法在相对应的采集点处坐标分别为(－１００,１９０,５００)和(－１６０,１９０,５００),相对处于离障碍物较为安全和合理的位置.在终点坐标位置,基于G C O P算法到达预计终点坐标为(５００,５００,３３２．５),偏离了预先设定的终点坐标,而基于S Q P算法到达终点的坐标为(５００,５００,５００),基本达到预先路径规划的要求.通过３个坐标点的数据显示分析,两个采样点的坐标偏差最优化是要保证距离障碍物的距离在合理范围之内,S Q P算法在两个采集点处的障碍物距离更加合理.在第三个终点采样点,两种算法的机器人运动轨迹偏差为１６７．５m m.实验表明,对于两栖水下机器人的水下运动控制,基于S Q P的路径规划算法比G C O P的路径规划算法更具有优越性.未来的研究工作会在机械设计和控制方法上持续改进,以实现多机器人的水下多机协作控制与路径规划的最优控制.参考文献:[１]㊀Z H E N G L,G U OSX,G U SX．T h ec o m m u n i c aＧt i o na n d s t a b i l i t y e v a l u a t i o no f a m p h i b i o u s s p h e r i c a l r o b o t s[J]．M i c r o s y s t e m T e c h n o l o g i e s,２０１９,２５(７):２６２５Ｇ２６３６．[２]㊀郭书祥,孙珊,郭健．新型仿生水下子母机器人系统设计[J]．控制与决策,２０１９,３４(５):１００４Ｇ１０１０．G U OS H X,S U N S H,G U OJ．D e s i g no f an o v e lb i o m i m e t ic u nde r w a t e r m o t h e rＧs o n r o b o t s y s t e m[J]．C o n t r o la n d D e c i s i o n,２０１９,３４(５):１００４Ｇ１０１０．(i nC h i n e s e)[３]㊀Z H E N GL,G U OSX,P I A O Y,e t a l．．C o l l a b o r aＧt i o na n dt a s k p l a n n i n g o ft u r t l eＧi n s p i r e d m u l t i p l ea m p h ib i o u ss p h e r ic a lr o b o t s[J]．M i c r o m a c h i n e s,２０２０,１１(１):７１．[４]㊀于涛,孙汉旭,赵伟,等．一种球形滚动机器人的路径跟踪控制器设计[J]．计算机测量与控制,２０１９,２７(３):９１Ｇ９６．Y U T,S U N H X,Z HA O W,e t a l．．D e s i g no f ap a t h f o l l o w i n g c o n t r o l l e r f o ras p h e r i c a l r o l l i n g r oＧb o t[J]．C o m p u t e r&C o n t r o l o f C o m p u t e r,２０１９,２７(３):９１Ｇ９６．(i nC h i n e s e)[５]㊀杨红彪．水下球形机器人的关键技术研究[D]．哈尔滨:哈尔滨工程大学,２０１８:１５Ｇ１９．Y A N G H B．R e s e a r c h o n t h eK e y T e c h n o l o g i e s o f０４７１㊀㊀㊀㊀㊀光学㊀精密工程㊀㊀㊀㊀㊀第２８卷㊀. All Rights Reserved.t h e U n d e r w a t e r S p h e r i c a l R o b o t[D]．H a r b i n:H a r b i n E n g i n e e r i n g U n i v e r s i t y,２０１８:１５Ｇ１９．(i nC h i n e s e)[６]㊀G U OJ,L IC Y,G U OSX．An o v e l s t e p o p t i m a l p a t h p l a n n i n g a l g o r i t h mf o r t h e s p h e r i c a lm o b i l e r oＧb o t b a s e do n f u z z yc o n t r o l[J]．I E E EA c c e s s,２０２０,８:１３９４Ｇ１４０５．[７]㊀L I U X H,Z HA N G D G,Y A N H R,e t a l．．A n e wa l g o r i t h m o f t h eb e s t p a t hs e l e c t i o nb a s e do nm a c h i n el e a r n i n g[J]．I E E E A c c e s s,２０１９,７:１２６９１３Ｇ１２６９２８．[８]㊀Y U A NJY,WA N G HJ,L I NCJ,e t a l．．An o v e lG R UＧR N N n e t w o r k m o d e l f o rd y n a m i c p a t h p l a nＧn i n g o fm o b i l er o b o t[J]．I E E E A c c e s s,２０１９,７:１５１４０Ｇ１５１５１．[９]㊀B A E H,K I M G,K I MJ,e t a l．．M u l t iＧr o b o t p a t h p l a n n i n g m e t h o du s i n g r e i n f o r c e m e n t l e a r n i n g[J]．A p p l i e dS c i e n c e s,２０１９,９(１５):３０５７．[１０]㊀A L I M A H,MA I L A H M．P a t h p l a n n i n g a n dc o n t r o l o fm o b i l e r o b o t i n r o ade n v i r o n m e n t s u s i n gs e n s o r f u s i o na n da c t i v ef o r c ec o n t r o l[J]．I E E ET r a n s a c t i o n s o nV e h i c u l a rT e c h n o l o g y,２０１９,６８(３):２１７６Ｇ２１９５．[１１]㊀Y A OZ,Z HA N G W M,S H IY L,e t a l．．R e i nＧf o r c e d R i m J u m p:t a ng e n tＧb a s e dsh o r t e s tＧp a t h p l a nＧn i n g f o r t w oＧd i m e n s i o n a lm a p s[J]．I E E E T r a n sＧa c t i o n s o nI n d u s t r i a lI n f o r m a t i c s,２０２０,１６(２):９４９Ｇ９５８．[１２]㊀Z H E N GL,P I A O Y,MA Y K,e t a l．．D e v e l o pＧm e n t a n d c o n t r o l o f a r t i c u l a t e da m p h i b i o u s s p h e r iＧc a l r o b o t[J]．M i c r o s y s t e mT e c h n o l o g i e s,２０２０,２６(５):１５５３Ｇ１５６１．[１３]㊀郑亮,朴燕,马宇科．非线性反馈和二次型调节器在两栖机器人中的应用[J]．光学精密工程,２０１９,２７(１０):２１９９Ｇ２２０６．Z H E N G L,P I A O Y,MA Y K,A p p l i c a t i o no fn o n l i n e a r f e e d b a c ka n d q u a d r a t i c r e g u l a t o r s i na mＧp h i b i o u s r o b o t s[J]．O p t．P r e c i s i o nE n g．,２０１９,２７(１０):２１９９Ｇ２２０６．(i nC h i n e s e)[１４]㊀张旭,曾祥鑫,郎博．基于控制变量参数化方法的自由漂浮空间机器人路径规划[J]．光学精密工程,２０１９,２７(２):３７２Ｇ３７８．Z HA N G X,Z E N G XX,L A N GB．P a t h p l a n n i n go f f r e eＧf l o a t i n g s p a c e r o b o t b a s e do nc o n t r o l v a r i aＧb l e p a r a m e t e r i z a t i o n m e t h o d[J]．O p t．P r ec i s i o nE n g．,２０１９,２７(２):３７２Ｇ３７８．(i nC h i n e s e) [１５]㊀Y UJZ,L I UJC,WUZX,e t a l．．D e p t h c o n t r o l o fab i o i n s p i r e dr o b o t i cd o l p h i nb a s e do ns l i d i n gＧm o d ef u z z y c o n t r o l m e t h o d[J]．I E E E T r a n s a cＧt i o n so n I n d u s t r i a l E l e c t r o n i c s,２０１８,６５(３):２４２９Ｇ２４３８．[１６]㊀Z HA N GS W,Q I A N Y,L I A OP,e t a l．．D e s i g na n d c o n t r o l o f a na g i l e r ob o t ic f i s hw i t h i n t e g r a t i v eb i o m i m e t i cm ec h a n i s m s[J]．I E E E/A S M ET r a n sＧa c t i o n s o nM e c h a t r o n i c s,２０１６,２１(４):１８４６Ｇ１８５７．[１７]㊀曾祥鑫,关英姿,晏卓,等．自由漂浮空间机器人最小基座扰动路径规划[J]．光学精密工程,２０１７,２５(１２z):６７Ｇ７３．Z E N G XX,G U A N YZ,Y A NZ H,e t a l．．P a t hp l a n n i n g f o r m i n i m i z i n g b a s ed i s t u r b a n c eo ff r e eＧf l o a t i ng s p a c er o b o t[J]．O p t．P r e c i s i o n E n g．,２０１７,２５(１２z):６７Ｇ７３．(i nC h i n e s e)[１８]㊀陈原,何淑垒,姜媛,等．轮Ｇ腿复合式移动机器人球面并联腿机构的动力学模型[J]．光学精密工程,２０１９,２７(８):１８００Ｇ１８１０．C H E N Y,H ES H L,J I A N G Y,e t a l．．D y n a m i cm o d e l o f s p h e r i c a l p a r a l l e lm e c h a n i s mf o rw h e e lＧl e gh y b r i d m o b i l er o b o t[J]．O p t．P r e c i s i o n E n g．,２０１９,２７(８):１８００Ｇ１８１０．(i nC h i n e s e)[１９]㊀WA N G W,D A IX,L IL,e t a l．．T h r e eＧd i m e nＧs i o n a lm o d e l i n g o f a f i nＧa c t u a t e dr o b o t i c f i s h w i t hm u l t i m o d a l s w i m m i n g[J]．A S M ET r a n s a c t i o n s o nM e c h a t r o n i c s,２０１８,２３(４):１６４１Ｇ１６５２．[２０]㊀WUZX,L I UJC,Y UJZ,e t a l．．D e v e l o p m e n t o f a n o v e l r o b o t i c d o l p h i n a n d i t s a p p l i c a t i o n t ow aＧt e r q u a l i t y m o n i t o r i n g[J]．A S M ET r a n s a c t i o n s o nM e c h a t r o n i c s,２０１７,２２(５):２１３０Ｇ２１４０．[２１]㊀徐彦伟,刘明明,刘洋,等．基于信息融合的机器人薄壁轴承故障智能诊断[J]．光学精密工程,２０１９,２７(７):１５７７Ｇ１５９２．X U Y W,L I U M M,L I U Y,e t a l．．I n t e l l i g e n tf a u l t d i ag n o s i s o f thi nw a l l b e a r i n g b a s e do n i n f o rＧm a t i o n f u s i o n[J]．O p t．P r e c i s i o nE n g．,２０１９,２７(７):１５７７Ｇ１５９２．(i nC h i n e s e)[２２]㊀刘涛,尹仕斌,任永杰,等．机器人工具坐标系自动校准方法[J]．光学精密工程,２０１９,２７(３):６６１Ｇ６７０．１４７１第８期㊀㊀㊀㊀㊀㊀㊀马宇科,等:两栖球形机器人的路径规划策略. All Rights Reserved.L I U T ,Y I NS H B ,R E N YJ ,e t a l ．．A u t o m a t i c c a l i b r a t i o no fr o b o tt o o lc e n t e rf r a m er o b o tt o o l c e n t e r f r a m e [J ]．O p t ．P r e c i s i o nE n g ．,２０１９,２７(３):６６１Ｇ６７０．(i nC h i n e s e)作者简介:㊀马宇科(１９９６－),男,吉林通化人,主要从事机器人机械结构设计和机器人建模相关方向的研究.E Ｇm a i l :f r a n k Ｇm a １２０８１６＠１６３．c o m通讯作者:㊀郑㊀亮(１９８２－),男,吉林吉林人,博士研究生,讲师,２００６㊁２０１０年于长春理工大学分别获得学士㊁硕士学位,２０１５年至今香川大学(日本)在读博士,主要从事机器人控制学㊁水下机器人建模与仿真系统的研究.E Ｇm a i l :s １８d ５０５＠s t u ．k a g a w a Ｇu ．a c ．j p２４７１㊀㊀㊀㊀㊀光学㊀精密工程㊀㊀㊀㊀㊀第２８卷㊀. All Rights Reserved.。
第四章-序列二次规划
x L( x, , ) 0,
i gi ( x) 0, i 1, ..., m
i 0, i 1, 2, ..., m.
m
l
其中L( x, , ) f ( x) i gi ( x) jhj ( x)为Lagrange函数.
i 1
j1
最优化方法之约束非线性规划
当解(1)得到一个迭代点xk时,为求得下一个更好的迭代点xk1时, 一种自然的想法,就是用问题(1)在xk处的二次规划模型代替 问题(1),以一系列二次规划的解逼近(1)的解,这种方法称为 序列二次规划法( SQP ).
一、搜索方向的确定
当运用SQP方法时, 在x k点处的二次规划一般形式为
最优化方法之约束非线性规划
x1 , x2 0.
取x0 (0, 0)T , 用第一种方法构造H0 ,写出求d 0的二次规划.
解.
f
(
x)
2 x1
2
x2
4
, f
(
x0
)
4
0
2
f
(
x)
2, 0,
0 2
,
2
f
(
x0
)
2, 0,
0 2
最优化方法之约束非线性规划
xk
,
k
,
k
)信息的正定阵.
最优化方法之约束非线性规划
二、步长的确定
步长确定的常用方法： (1).固定步长为1,即xk1 xk d k ; (2).可行性优先准则,即若xk d k为(1)的可行解,则取 xk1 xk d k ;
SQP算法
if(m==0) dh1=dah(ep+rho^i*de,d+rho^i*dd,mu+rho^i*du,lam,dfk,Bk,Ae,hk,Ai,gk); end if(norm(dh1)<=(1-sigma*(1-gamma*ep0)*rho^i)*norm(dh)) mk=i;break; end i=i+1; if(i==20) mk=10; end end alpha=rho^mk; if(l>0&m>0) ep=ep+alpha*de; d=d+alpha*dd; mu=mu+alpha*du; lam=lam+alpha*dl; end if(l==0) ep=ep+alpha*de; d=d+alpha*dd; lam=lam+alpha*dl; end if(m==0) ep=ep+alpha*de; d=d+alpha*dd; mu=mu+alpha*du; end k=k+1; end val=f1(d); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function p=phi(ep,a,b) p=a+b-sqrt(a^2+b^2+2*ep^2); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function dh=dah(ep,d,mu,lam,dfk,Bk,Ae,hk,Ai,gk) n=length(dfk); l=length(hk); m=length(gk); dh=zeros(n+l+m+1,1); dh(1)=ep; if(l>0&m>0) dh(2:n+1)=Bk*d-Ae'*mu-Ai'*lam+dfk; dh(n+2:n+l+1)=hk+Ae*d; for(i=1:m) dh(n+l+1+i)=phi(ep,lam(i),gk(i)+Ai(i,:)*d); end end if(l==0) dh(2:n+1)=Bk*d-Ai'*lam+dfk;
带约束的非线性优化问题解法小结
(1)带约束的非线性优化问题解法小结考虑形式如下的非线性最优化问题(NLP):min f(x)「g j (x )“ jI st 彳 g j (x)=O j L其 中， ^(x 1,x 2...x n )^ R n， f : R n > R ， g j :R n > R(j I L) ， I 二｛1,2,…m ｝， L ={m 1,m 2...m p}。
上述问题(1)是非线性约束优化问题的最一般模型，它在军事、经济、工程、管理以 及生产工程自动化等方面都有重要的作用。
非线性规划作为一个独立的学科是在上世纪 50年 代才开始形成的。
到70年代，这门学科开始处于兴旺发展时期。
在国际上，这方面的专门性 研究机构、刊物以及书籍犹如雨后春笋般地出现，国际会议召开的次数大大增加。
在我国， 随着电子计算机日益广泛地应用，非线性规划的理论和方法也逐渐地引起很多部门的重视。
关于非线性规划理论和应用方面的学术交流活动也日益频繁，我国的科学工作者在这一领域 也取得了可喜的成绩。
到目前为止，还没有特别有效的方法直接得到最优解，人们普遍采用迭代的方法求解： 首先选择一个初始点，利用当前迭代点的或已产生的迭代点的信息，产生下一个迭代点，一 步一步逼近最优解，进而得到一个迭代点列，这样便构成求解( 1)的迭代算法。
利用间接法求解最优化问题的途径一般有：一是利用目标函数和约束条件构造增广目标 函数，借此将约束最优化问题转化为无约束最优化问题，然后利用求解无约束最优化问题的 方法间接求解新目标函数的局部最优解或稳定点，如人们所熟悉的惩罚函数法和乘子法；另 一种途径是在可行域内使目标函数下降的迭代点法，如可行点法。
此外，近些年来形成的序 列二次规划算法和信赖域法也引起了人们极大的关注。
在文献［1］中，提出了很多解决非线性 规划的算法。
下面将这些算法以及近年来在此基础上改进的算法简单介绍一下。
1. 序列二次规划法序列二次规划法，简称SQ 方法.亦称约束变尺度法。
序列二次规划算法
序列二次规划法求解一般线性优化问题:12min (x)h (x)0,i E {1,...,m }s.t.(x)0,i {1,...,m }i i f g I =∈=⎧⎨≥∈=⎩ (1.1) 基本思想：在每次迭代中通过求解一个二次规划子问题来确定一个下降方向，通过减少价值函数来获取当前迭代点的移动步长，重复这些步骤直到得到原问题的解。
1.1等式约束优化问题的Lagrange-Newton 法考虑等式约束优化问题min (x)s.t.h (x)0,E {1,...,m}j f j =∈=(1.2)其中:,n f R R →:()n i h R R i E →∈都为二阶连续可微的实函数. 记1()((),...,())T m h x h x h x =. 则(1.3)的Lagrange 函数为： 1(,)()*()()*()mT i i i L x u f x u h x f x u h x ==-=-∑（1.3）其中12(,,...,)T m u u u u =为拉格朗日乘子向量。
约束函数()h x 的Jacobi 矩阵为：1()()((),...,())T T m A x h x h x h x =∇=∇∇.对（1.3）求导数，可以得到下列方程组：(,)()A()*(,)0(,)()T x u L x u f x x u L x u L x u h x ∇⎡⎤⎡⎤∇-∇===⎢⎥⎢⎥∇-⎣⎦⎣⎦（1.4）现在考虑用牛顿法求解非线性方程（1.4）.(,)L x u ∇的Jacobi 矩阵为：(,)()(,)()0T W x u A x N x u A x ⎛⎫-= ⎪-⎝⎭（1.5）其中221(,)L(,)()*()mxx iii W x u x u f x u h x ==∇=∇-∇∑是拉格朗日函数L(,)x u 关于x 的Hessen 矩阵.(,)N x u 也称为K-T 矩阵。
对于给定的点(,)k k k z x u =，牛顿法的迭代格式为：1k k k z z z +=+∆. 其中k k (d ,v )k z ∆=是线性方程组k k k k (,)()(x )A(x )u *()0(x )k k k k T T k k d W x u A x f A x v h ⎛⎫-⎛⎫-∇+⎛⎫= ⎪ ⎪ ⎪ ⎪-⎝⎭⎝⎭⎝⎭（1.6）的解。
第四章 二次规划
关于二次规划(QP )中G为半正定矩阵和C 0或G为正定矩阵,
则Lemke方法可求出二次规划的一个K T点.若G为半正定
矩阵, 则算法或终止于求出一个K T点, 或终止于半射线,
即显示二次规划有无界解.
最优化方法之约束非线性规划
二次规划
形如
min s.t .
6/102 0 1/6 -96/102 -5/6 24/102 4/6 120/102 -2/6 -11/6 1/6 24/102 1/6 -6/102 -2/6 30/102 4/6 7/6 24/102 0 -1/17 -1/6 -1/17 1 -1/6 -4/17 -4/6 14/17 14/6 17/6
8 3 4 0 -1
di 0
6 5 -1 -2
由于右端有负数,所以加一人工变量w0 , 表格改为
最优化方法之约束非线性规划
线性互补表
1
2
3
4
z1
z2
z3
z4
0
q
1 1 1 0 0 0 0 0 0
2 0 0 1 1 0 0 0 0
3 0 0 0 0 1 1 0 0
4 -1 0 -1 0 -1 0 -1 1
二次规划
(1). x quadprog( H , f , A, b) : 求解只有不等式约束的二次 规划问题,并返回极值点;
(2). x quadprog( H , f , A, b, Aeq, beq ) : 求解含有不等式和 等式约束的二次规划问题，并返回极值点; (3). x quadprog( H , f , A, b, Aeq, beq, lb, ub) : 求解标准形式 的二次规划问题,并返回极值点; (4). x quadprog( H , f , A, b, Aeq, beq, lb, ub, x0)：求解指定
序列二次规划法
n
（1-10）
其中， E 代表等式约束下的集合， I k 代表不等式约束中起作用约束的下标 集合。
此式即式 （1-8） ， 可以用同样的方法求解。 在求得式 （1-10） 的解 [ S
k 1
, k 1]T
之后，根据 k-t 条件，若解中对应原等式约束条件的乘子不全为零，对应起作用 约束条件的乘子不小于零，则 S 最优解 S * 。 综上所述，在迭代点 X 上先进行矩阵 H 的变更，在构造和求解相应的二 次规划子问题，并该子问题最优解 S * 作为下一次迭代的搜索方向 S 。然后在 该方向上对原非线性最优化问题目标函数进行约束一维搜索， 得到下一个迭代点
此问题是原约束最优化问题的近似问题，但其解不一定是原问题的可行点。 为此，令
S X Xk
将上述二次规划问题变成关于变量的 S 的问题，即
1 min f ( X ) S T 2 f ( X k ) S f ( X k )T S 2 s. t. gu ( X k )T S gu ( X k ) 0 (u 1,2,..., p) hv ( X k )T S hv ( X k ) 0
T
等于 n m 。由线性代数知，此方程要么无解，要么有惟一解。如果有解，利用 消元变换可以方便的地求出该方程的惟一解， 记作 [ S 若此解中的乘子向量
k 1
k 1
根据 k-t 条件， , k 1]T 。
不全为零， 则S 。
k 1
就是等式约束二次规划问题式 （1-8）
的最优解 S * ，即 S* S
2 序列二次规划的研究
最优化理论及方法是一个具有广泛应用背景的研究领域。 它研究诸如从众多 的方案中选出最优方案等问题，常见的各种模型如线性规划，二次规划，非线性 规划， 多目标规划等。 最优化理论及方法已经在经济计划， 工程设计， 生产管理，
序列二次规划法
min
1 T S HS C T S 2 s. t. AS B Aeq S Beq
（1-5）
求解此二次规划问题，将其最优解 S * 作为原问题的下一个搜索方向 S ， 并在该方向上进行原约束问题目标函数的约束一维搜索， 就可以得到原约束问题 的一个近似解 X
此问题是原约束最优化问题的近似问题，但其解不一定是原问题的可行点。 为此，令
S X Xk
将上述二次规划问题变成关于变量的 S 的问题，即
1 min f ( X ) S T 2 f ( X k ) S f ( X k )T S 2 s. t. gu ( X k )T S gu ( X k ) 0 (u 1,2,..., p) hv ( X k )T S hv ( X k ) 0
k
3 序列二次规划算法推导过程
序列二次规划（SQP）算法是将复杂的非线性约束最优化问题转化为比较 简单的二次规划（QP）问题求解的算法。所谓二次规划问题就是目标函数为二 次函数， 约束函数为线性函数的最优化问题。二次规划问题是最简单的非线性约 束最优化问题。
3.1 序列二次规划算法思想
非线性约束最优化问题：
令
（1-3）
(v 1,2,..., m)
H 2 f ( X k ) C f ( X k ) Aeq [h1 ( X k ), h2 ( X k ),..., hm ( X k )]T A [g1 ( X k ), g 2 ( X k ),..., g p ( X k )]T Beq [h1 ( X k ), h2 ( X k ),..., hm ( X k )]T B [ g1 ( X k ), g 2 ( X k ),..., g p ( X k )]T
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
等式约束二次规划－基本消元法(续)
找 A 的可逆子矩阵 A1，进行消元
如果 正定，解方程组
可得惟一解
等式约束二次规划－广义消元法
令 Y 和 Z 分别是 n×m 与 n×(n-m)矩阵，满足
考察方程组ATx=b: Yb是特解；通解x=Yb+ s， 其中s 是齐次线性方程组ATs=0的解
任一可行解均可表示为: x=Yb+Zy
• 近似二阶导数
⊙ 用近似矩阵B(k)代替W(k) ⊙ 用近似矩阵代替既约海森矩阵Z(k)TW(k) Z(k)
• 子问题的求解
积极集法－算法的原理(续)
◎ x(k)是当前等式约束问题的解，即s(k) =0： 设当前等式约束问题的Lagrange乘子是
⊙ 乘子中与不等式约束对应的分量非负： x(k)是原问题的KKT点，进而是全局解
⊙ 否则，存在
通常取指标 q 满足：
积极集法－算例
积极集法－算例(续)
作业中用同样的初始点和不同的初始工作集进行迭代求解
凸二次规划
技术注记：此处用线性约束规范代替LICQ! 故二次规划的任 一解x*均满足KKT条件
最优积极集！
积极集法－算法的动机(motivation)
如果提前知道 ，求解
对最优积极集进行猜测，并不断修正，直到得到正确的！ 考虑第 k 次迭代： x(k)是可行点， Wk 是工作集(由等式约束和部分或全部 积极不等式约束组成)
基本/局部逐步二次规划法(续)
假设
是等式约束问题的满足二阶充分条件的极
小点，即
这里 Z 是A*Ts=0的基础解系组成的矩阵.
则s*=0 (x*)是下列问题的惟一最优解
基本/局部逐步二次规划法(续)
算法10.3.1 基本SQP法
基本/局部逐步二次规划法(续)
例
基本/局部逐步二次规划法(续)
• 优点：局部二阶收敛
即使初始工作集相同，后面的迭代也可能不同 ⊙ 选取初试工作集的额外要求：所选约束的梯度线性无关 ⊙ 迭代次数有可能超过不等式约束的个数
逐步二次规划法
Successive Quadratic Programming Method
假设和记号
在设计和分析算法时，通常假设 f(x) , ci(x) 是连续 可微(二阶连续可微)的，且导数是李普希兹连续的！
约束优化二次规划与SQP
有价证券的组合优化(续)
⊙ 证卷组合： 证卷组合的利润： 证卷组合的期望收益和方差：
G 是半正定矩阵！ ⊙ 证卷组合优化(portfolio optimization)：
有价证券的组合优化(续)
Markowitz引入风险容许参数(risk tolerance parameter)
如果ZTGZ正定，则原问题有惟一解，解方程组
等式约束二次规划－广义消元法(续)
构造 Y 和 Z的正交分解法 对矩阵 A 进行QR分解，即
等式约束二次规划－广义消元法(续)
实用二次规划算法综述
⊙ 经典积极集法(classical active-set methods)
求解凸和非凸二次规划问题－－中小规模(几百个变量！)
等式约束问题－Lagrange-Newton法
KKT条件： 其中
等式约束问题－Lagrange-Newton法
KKT条件： 其中
设
是近似解，则其牛顿校正
满足
等式约束问题－Lagrange-Newton法(续)
令
，上述方程组即
给定初始点
，利用上面两式进行迭代
解等式约束问题的－ Lagrange-Newton法
⊙ 梯度投影法(gradient-projection methods)
界约束QP(BoxQP)!
⊙ 内点法(interior-point methods)
大规模凸二次规划！
积极集法
积极集法－问题
其中 G 是 n 阶对称方阵，ai , d是 n 维常向量 解的情况：无可行解、无界、有解
G 半正定
• 存在问题
⊙ 初始点不好时，迭代可能发散 ⊙ 子问题的解可能不存在－无界或者不可行 ⊙ 需要二阶导数－W(k)
实用逐步二次规划法
• 全局化策略：使用线搜索策略或者信赖域策略
⊙ 评价函数法 常用的是 l1 精确罚函数，迭代中需更新惩罚因子；
⊙ 滤子(Filter)法
存在问题：具有Martos效应，需要采取校正措施
积极集法－算法
算法10.2.1 求解凸二次规划的积极集法
积极集法－理论分析
定理10.2.1 设 x(k) 是等式约束二次规划子问题的最优解，
是对应的乘子. 假设约束的梯度向量
线性无关，且存在指标
使得
. 考虑
问题
设该问题的解为 s’ . 则 s’ 是第 j 个约束的可行方向，即
. 此外，如果 s’ 满足二阶充分条件，则
其中
积极集法－算法的原理
◎ x(k)不是当前等式约束问题的解，即s(k) ≠0: ⊙ x(k) +s(k)满足其它约束： ，工作集保持不变 ⊙ x(k) +s(k)不满足某些约束，找阻滞约束和步长：
称取到最小值的指标 p对应的约束为阻滞(blocking)约束
无阻滞约束时，工作 集不变；否则给工作 集添加一个阻滞约束
.
定理10.2.2 假设 s(k) 是关于增量的等式约束二次规划子问题 的最优解，且满足该问题的二阶充分条件，则 p(k) =s(k) 是 原目标函数的下降方向. 线搜索法、每个迭代点都可行
积极集法－进一步说明
⊙ 存在许多技术确定初始点－－比如人工变量法！ ⊙ 在恰当的假定下可证明－－算法有限步找到解！ ⊙ 可以推广来求解非凸二次规划 ⊙ 初试点相同，但初始工作集不同，则后面的迭代不同；
定理 假设 x*是等式约束问题的满足二阶充分条件的局
部极小点, 且rank (A*)=m ， 是惟一的Lagrange乘子.
则当
充分接近
时，Lagrange-Newton
法有定义，且由该方法产生的序列
二次
收敛到
.
基本/局部逐步二次规划法
考虑二次规划问题
的解和对应的Lagrange乘子，其中 二次规划的 KKT条件
找出“最优的”证券投资组合！
⊙ 参数
，设定值依赖于投资者的个人偏好
保守型投资者：大的参数取值 冒险性投资者：小的参数取值
等式约束二次规划 积极集法 逐步二次规划法
等式约束二次规划
等式约束二次规划
其中
假定:
线性无关
核心思想：消元法(基本、广义)
其中
，A1可逆
等式约束二次规划－基本消元法
消去 x3 代入 q(x)