ÈÈËÑÒªº¦´Ê£º µçÉÌÒ»¼þ´ú·¢ ÀäÁ´ÅäËÍ ÉçÇøÍŹº²ÖÅä ʳƷ²Ö´¢´ú·¢»õ ÔöֵЧÀÍ
Ëæ×ŵç×ÓÖÇÄÜ»õ²ÖµÄÉú³¤£¬¹ºÎïƽ̨µÄ±ã½ÝÐÔ»ñµÃÁË´ó·ù¶ÈµÄÓÅ»¯£¬ÔÚµçÉÌƽ̨ÉÏÏû·ÑÕß½øÐÐÍø¹ºÖð½¥µØÒѾ³ÉΪһÖÖÏÖÏó¼¶Ïû·ÑÏ°¹ß
ÔڹŰåÔË×÷ģʽµÄ»õ²ÖÖУ¬¹¤È˵ÄÈ¡»õʱ¼äÕ¼µ½×ÜÊÂÇéʱ¼äµÄÁù³ÉÒÔÉÏ¡£ÔÚÏÖ´úµç×ÓÉÌÎñÖУ¬ºóÇÚ·ÖÅɾßÓн»¸¶µ¥Î»Ð¡ÐÍ»¯£¬ÀàÐͶàÑù¡¢ÅúÁ¿Ð¡¡¢½»¸¶ÖÜÆÚ¶ÌÒÔ¼°ÎïÆ·Á÷Á¿Æµ·±µÈÌØÕ÷¡£ÓÉÓÚÉÏÊöÕâЩȱµãµÄ±£´æ£¬¹Å°åµÄ»õ²ÖÖÎÀí·½·¨ºÜÄÑÊÊÓ¦µç×ÓÉÌÎñ¿ì½Ú×àµÄÉú³¤Ä£Ê½
¼ÙÉèÒƶ¯»úеÈ˵ÄÄ¿±êµãÓÐn¸ö£¬Òƶ¯»úеÈ˽«Éú³ÉCn2Ìõ·¾¶¡£ÎªÁËʹ»úеÈËÒƶ¯¾àÀëµÖ´ï×î¼Ñ·¾¶£¬»úеÈ˱ØÐë´ÓÖÐɸѡ³ö×î¶ÌµÄ¼Æ»®£¬Èçͼ1Ëùʾ¡£
ΪÁ˽â¾öÉÏÊöÎÊÌ⣬×îÓÐЧµÄÒªÁìÖ÷ÒªÓÐGuo-taoËã·¨¡¢Ä£ÄâÍË»ðËã·¨¡¢ÒÅ´«Ëã·¨¡¢ÒÏȺËã·¨µÈµÈ¡£ÒÔÉÏËã·¨¶¼±£´æ¸÷×ÔµÄÓÅȱµã£¬Ò»Ð©Ñо¿Ñ§ÕßÊÔͼͨ¹ý½áºÏÁ½ÖÖ»òÁ½ÖÖÒÔÉϵÄËã·¨À´ÃÖ²¹Ï໥µÄȱµã¡£
Á£×ÓȺËã·¨µÄÅÌËã·½·¨ÓëÄñȺÃÙʳµÄ·½·¨ÏàËÆ£¬¿ÉÒÔ´ÓÈ«¾ÖÑ°ÕÒ×îÓÅ·¾¶¡£ÔÚËã·¨ÖУ¬Ä¿±êº¯Êý¾ö¶¨ÁËÁ£×ÓµÄÊÊÓ¦¶È£¬Í¨¹ýËæ»ú½â¶ÔÖÖȺÁ£×Ó½øÐгõʼ»¯£¬×î¼Ñ½á¹ûÐèÒª¾¹ý¼¸Âֵĵü´úËã·¨À´Éú³É¡£Î»ÖõIJ»¾ø¸üÐÂÈ¡¾öÓÚµü´úËã·¨µÄ¼«Öµ¡£µü´úËã·¨¶Ô×îÓŽâÀ´½²ÓÐÁ½¸ö¼«Öµ£¬Ò»¸öÊǸöÌ弫ֵpbest£¬Ò»¸öÊÇÈ«¾Ö¼«Öµgbest¡£Ç°ÕßÊǸöÌå×Ô¼ºÃٵõÄ×îÓŽ⣬ºóÕßÊÇÖÖȺËùÃٵõÄ×îÓŽ⡣Á£×ÓµÄÆ«ÏòºÍº½ÐоàÀëͨ¹ýÆäÔÚ½â¿Õ¼äµÄËÙ¶ÈÈ·¶¨¡£
É趨Á£×ÓȺµÄËÑË÷ÇøÓòΪһ¸öNά¿Õ¼ä£¬¼ÓÈëËÑË÷µÄÁ£×Ó¹²ÓÐN¸ö£¬Á£×ÓµÄλÖÃͨ¹ýÈçÏÂÐÎʽÌåÏÖ£º
ÆäÖÐxÌåÏÖλÖÃÏòÁ¿¡£ÐµÄ×îÓŽâͨ¹ýµ÷½âλÖÃÏòÁ¿xÀ´»ñµÃ¡£ÔÚNάËÑË÷¿Õ¼äÄÚ£¬¸öÌå×îÓŽâpidºÍÈ«¾Ö×îÓŽâgidÓëÁ£×ÓµÄÔ˶¯ËÙ¶ÈÓÐ×ÅÒ»¶¨µÄ¹ØÁª£¬µÚi¸öÁ£×ÓµÄd·ÖÁ¿µÄλÖúÍËÙ¶È×ñÑʽ£¨2£©ºÍʽ£¨3):
ÉÏʽÃèÊöÁ˵Úk´Îµü´úʱÁ£×ÓiµÄËÙ¶ÈʸÁ¿ºÍλÖÃʸÁ¿Óë×îÓÅλÖÃÖ®¼äµÄ¹Øϵ£¬Í¨¹ýÒýÈë¼ÓËÙÒò×Óc1¡¢c2ºÍȨÖØwÒÔ¼°Ëæ»ú±äÁ¿r1ºÍr2À´½øÐо«Ï¸¿Ì»¡£Í¨¹ýµ÷½âËã·¨ÖеÄȨÖØw¿ÉÔöÇ¿Á£×ӵľֲ¿ËÑË÷ÐÔÄÜ¡£
Á£×ÓÊʶȺ¯ÊýÊDzâÊÔÁ£×ÓȺÓÅ»¯ÊÕÁ²ÐÔµÄÖØÒªº¯Êý¡£Ôڼƻ®Â·Ïßʱ£¬Ñ°Çó×îСµÄÔ˶¯¾àÀëÊÇ»úеÈ˵ÄÖ÷ҪĿ±ê£¬Æä´ÎÊÇÄþ¾²ÐÔ°ü¹ÜµÄÐèÇó¡£Òò´Ë£¬»úеÈ˵ÄÐнø¾àÀë×÷ΪÊʶȺ¯ÊýµÄÒò±äÁ¿¡£Í¼2ÏÔʾÁËÒƶ¯»úеÈ˶àÄ¿±ê¹ì¼£¡£
ͼ2 Òƶ¯»úеÈ˶àÄ¿±ê¹ì¼£
ΪÁ˸üºÃµØÁ˽âÒƶ¯»úеÈ˶àÄ¿±êÔ˶¯¹ì¼£¡£¿ÉÒÔÔÚ¶à¸öÄ¿±êµãÖ®¼ä»æÖÆÁ½Ìõ·Ïß¡£»úеÈË¿ÉÒÔ°ü¹Ü¶àÄ¿±ê¼Æ»®ÊÇ×î¶ÌµÄ¡£ÔÚÆÊÎöºó£¬Èçͼ3Ëùʾ£¬»úеÈ˱ØÐëÑ¡ÔñÏÂÒ»¸ö½»»ãµã£¬È»ºóµÖ´ïÄ¿±êµã£¬ÒÔÖÆÖ¹ÔÚ´ËÆڼ䷺ÆðÕÏ°¡£
ΪÁËÈ·±£¼Æ»®Â·¾¶×î¶Ì£¬Ñ¡ÔñÏÂÒ»¸öÒƶ¯µãÊÇÈ·ÏȾöÌõ¼þ¡£Òƶ¯»úеÈËÄ¿±êº¯ÊýF1¾ö¶¨ÁËÒƶ¯»úеÈËÐÐ×ß·¾¶µÄ³¤¶È¡£
ʽÖÐF1¡ª¡ªÄ¿±êº¯Êý£»
(xi,j,yi,j)¡ª¡ªi,jλÖÃ×ø±êÖµ¡£
³ýÁËÈ·±£»úеÈËÏòÄ¿±êÒƶ¯µÄ×îСËÙ¶ÈÍ⣬»¹ÐèÒª¿¼ÂÇÄþ¾²ÎÊÌâ¡£ÕÏ°Æ«Àë¼Æ»®Â·ÏßÊÇÄþ¾²Í¨Ðеİü¹Ü¡£Æ«²î³¡º¯ÊýÈçÏ£º
×ۺϿ¼Á¿×î¶Ì·¾¶ºÍ»úеÈ˱¬·¢ÅöײµÄ¿ÉÄÜÐÔ£¬Ð轫Á½¸öÆÀ¼ÛÖ¸±êÈںϣ¬ÔÚÈ·¶¨×î¶Ì·¾¶µÄÇ°ÌáÏÂÖÆÖ¹ÓëÇé¿öµÄÅöײΣº¦¡£¼Æ»®Â·¾¶µÄÄ¿±êº¯Êý½ç˵ÈçÏ£º
·´ÏòѧϰսÂÔͨ³£ÊÊÓÃÓÚÒ»¸öÁ£×Ó£¬¶ø²»ÊÇÕû¸öÁ£×ÓȺ¡£µ±Á£×ÓÔÚÄ¿±ê¿Õ¼äÖÐÕÒµ½¸üºÃµÄ½â£¬ÔÚ¶ÔÁ£×Ó³õʼ»¯Ê±£¬¿Õ¼äÖеÄÂþÑÜÊdzɱÈÀýµÄ¡£½Ï½üµÄÁ£×ӱȽÏÔ¶µÄÁ£×Ó¸ü¿ìµØÕÒµ½×î¼Ñ½â¾ö¼Æ»®¡£È»¶ø£¬ÕâЩÁ£×ÓËæ»úÂþÑÜÔÚÄ¿±ê¿Õ¼äÖУ¬Ã¿¸öÁ£×ÓÏà¹ØÓÚ×î¼Ñ½âµÄλÖÃÉв»Çå³þ¡£³õʼ»¯Á£×ÓµÄÊʶÈʱ£¬Ê×ÏÈÅÌËã²¢Óë×îÓŽâÏàÆ¥Å䣬ȻºóÑ¡ÔñS¸öÁ£×Ó×÷Ϊ³õʼ½â¼¯¡£¹ßÐÔȨÖرØÐëÊÊÓ¦Õû¸öÊÕËõµÄÀú³Ì¡£µ±wÖµºÜ¸ßʱ£¬Á£×ÓµÄÈ«¾ÖËÑË÷ÄÜÁ¦ºÜÇ¿£¬µ«¿ÉÄܵ¼Ö¾ֲ¿¼Æ»®µÄ²»¶ÔÀí£¬µ±wֵСµÄʱºò£¬Á£×ӵľֲ¿ËÑË÷ÄÜÁ¦Ò²ºÜÇ¿¡£Ëæ×ÅÁ£×ÓÔÚÓÅ»¯Àú³ÌÖлñµÃ×îºÃµÄÓÅÊÆ£¬Á£×ӵĹßÐÔȨÖرØÐëÊÊÓ¦ÓÅ»¯Àú³Ì¡£Òò´Ë£¬Á£×Ó¹ßÐÔÖØÁ¿¸üй«Ê½ÈçÏ£º
ʽÖÐw¡ª¡ªÈ¨ÖØÖµ£»
disti¡ª¡ª¾àÀëÈ¡Öµ¹æÄ£¡£
ÉÏʽÃèÊöÁËÁ£×ÓȨÖظüÐÂÓëÆäÈ¡Öµ¹æÄ£µÄ¹Øϵ¡£ÆäÖУ¬
ѧϰÒò×Óc1ºÍc2¿ØÖÆÁ£×ÓÓ°ÏóºÍͬ°éÓ°ÏóÖ®¼äµÄÏà¶ÔÓ°Ï죺µ±c1ֵСʱ£¬ÈÏÖªÄÜÁ¦È±·¦£¬µ±c2ֵСʱÔòѧϰÄÜÁ¦È±·¦¡£ÎªÁËÈ·±£ÔÚµü´úÀú³ÌÖжÔÁ£×ÓѧϰºÍÈÏÖªµÄƽºâ£¬¿É¸üÐÂÁ£×ÓѧϰÒò×Ó¹«Ê½£º
ʽ£¨9£©ºÍʽ£¨10£©ÃèÊöÁËc1ºÍc2ÓëÆäÈ¡Öµ¹æÄ£µÄ¹Øϵ¡£
±¾ÎÄÑ¡Ôñ×îСʱ¼ä±¾Ç®×÷Ϊ¶à»õ²ÖÎïÁ÷»úеÈËÅúÁ¿¼ðѡ·¾¶¼Æ»®Ä£Ð͵ÄÓÅ»¯Ä¿±ê¡£»úеÈËÈ¡»õʱ¼äÖ÷ÒªÓÉ»úеÈËÈ¡»õʱ¼ä£¨PT£©¡¢»úеÈËÒƶ¯Ê±¼ä£¨RT£©ºÍ»úеÈËÆÚ´ýʱ¼ä£¨WT£©Èý²¿·Ö×é³É¡£ÔÚ»úеÈËÅúÁ¿¼ðÑ¡ÊýѧģÐ͵Ļù´¡ÉÏ£¬Í¨¹ýÊʵ±µÄÐ޸ģ¬¿ÉÒÔ»ñµÃ¶à»õ²ÖÎïÁ÷»úеÈËÅúÁ¿¼ðѡ·¾¶¼Æ»®µÄÊýѧģÐÍ£¬Ä£ÐÍÈçÏÂʽËùʾ£º
½ÓÄÉ·ÂÕæÈí¼þROSϵͳ£¨Robot Operating System£¬»úеÈ˲Ù×÷ϵͳ£©¡£ÔÚROSµÄ¶¨Î»ºÍµ¼º½ÖУ¬×îÖØÒªµÄÊÇmove£ßbase°ü£¬ÔÚROSµ¼º½ºÍ¶¨Î»×ÜÌå¿ò¼ÜͼÖпÉÒÔ¿´µ½£¬move£ßbaseÌṩÁËROSµ¼º½µÄÅäÖá¢ÔËÐС¢½»»¥½Ó¿Ú£¬½«±¾ÎĵÄËã·¨Ìæ»»ÔÀ´ROS·¾¶¼Æ»®Ëã·¨¾Í¿ÉÒÔ¶ÔÕû¸öϵͳ½øÐзÂÕ棬×îºó¿ÉÒÔÑéÖ¤Ëã·¨µÄÕýÈ·ÐÔ¡£Ê¹ÓÃÇ°ÐèÒªÔÚÒÔϼ¸¸öÅäÖÃÎļþÖÐÅäÖÃһЩ²ÎÊý£¬ºÃ±È£ºÔËÐб¾Ç®¡¢»úеÈ˰뾶¡¢µÖ´ïÄ¿±êλÖõľàÀ룬»úеÈËÒƶ¯µÄËٶȡ£Èç±í1Ëùʾ£¬ÎªÑéÖ¤²î±ðȨÖØÏ·¾¶µÄЧ¹û¡£
´Ó±í1¿É¼û£¬ÔÚȨÖØΪ1.3ʱ»úеÈ˵ÄÐÐ×ß¾àÀëΪ538.2 cm£¬´ËʱΪ×î¶Ì·¾¶¡£µ«Æä²¢²»¿É¹»±ÜÕÏ¡£
±í2¼Í¼ÁËÖÖÖÖ²ÎÊýÈ¡ÖµÇé¿öÏ£¬»úеÈËÐÐ×ß¾àÀëÒÔ¼°ÊÇ·ñÄܹ»ÖÆÖ¹ÅöײµÄÇé¿ö¡£
×ÛÉÏËùÊö£¬Ñ¡È¡È¨ÖØΪ1.3£¬¦Ë=0.7£¬¦Ì=900£¬ÔÚÁ½ÖÖ²î±ðµÄ³¡¾°Ï½øÐмƻ®¡£¿É»ñµÃÈçͼ4(a£©ºÍ4(b£©ÖеĽá¹û¡£
ͼ4(a£©Öмƻ®¾¹ý341¸öµü´úÖÜÆÚ¹²ÐÐ×ß755.2 cm£¬Í¼4(b£©Öо¹ý452¸öµü´úÖÜÆÚ¹²ÐÐ×ß760.4 cm¡£¼Æ»®´ÓÆðµã¿ªÊ¼¾¹ýһϵÁÐÖмäµã£¬²¢×îÖÕµÖ´ïÄ¿±êµã¡£
Õë¶ÔËùÉè¼ÆµÄËã·¨£¬Ñ¡È¡²ÎÊý²ÎÕÕËã·¨·ÂÕæÖеÄ×îÓÅ»¯²ÎÊý£¬ÔÚROS»úеÈ˲Ù×÷ϵͳµÄÎïÀíÒýÇæ¶ÔËã·¨½øÐзÂÕ棬ͨ¹ýÅÌËã»úеÈËÿ¸öÖÜÆÚÄÚÓ¦¸ÃÐÐÊ»µÄËٶȺͽǶȣ¨dx,dy,dtheta velocities£©À´ÊµÏֵģ¬Èçͼ5Ëùʾ¡£
ÓÉͼ5¿ÉÖª£¬±¾Ëã·¨¿ÉʵÏÖ¶à¸öÄ¿±ê·¾¶¼Æ»®£¬ËùÉú³ÉµÄ·¾¶½ÏΪÀíÏë¡£
×÷Ϊ»úеÈËÐÐÒµÓëÎïÁ÷ÐÐÒµµÄ½»²æѧ¿Æ£¬²Ö´¢ÎïÁ÷»úеÈËÊÇÊDzִ¢ÎïÁ÷ÐÐÒµ¹¤ÒµÉý¼¶µÄ±ØÓÉ֮·£¬Ò²ÊÇÒƶ¯»úеÈ˼¼ÊõµÄÁ¼ºÃµÄÓ¦ÓÃʾ·¶³¡ºÏ¡£±¾ÎĽ«²Ö´¢ÎïÁ÷»úеÈ˹ØÓÚÅúÁ¿¼ðѡ·¾¶µÄ¼Æ»®ÎÊÌâÁýͳΪ¶àÄ¿±êµãÎÞÅöײ·¾¶¼Æ»®ÎÊÌ⣬ͨ¹ý¸ïÐÂÁ£×ÓȺËã·¨Çó³öͼÖÐÆðµãµ½ÖÕµãµÄ¶àÄ¿±ê×îÓÅ·¾¶¼Æ»®×î¶Ì·¾¶¡£¸ÃÒªÁìÒ×ÓÚ½¨Á¢Ä£ÐÍ£¬Ëã·¨¼òµ¥£¬Äܹ»Âú×ã²Ö´¢ÎïÁ÷»úеÈ˼ƻ®µÄʵʱÐÔÒªÇ󣬾ßÓÐÒ»¶¨µÄʵÓüÛÖµ¡£±ðµÄ£¬ÔÚʵÑé»·½Úͨ¹ý¶ÔËã·¨µÄ·ÂÕæÆÊÎöÁ˸÷¸ö²ÎÊý¹ØÓÚʵ¼Ê¼Æ»®Ð§¹ûµÄÓ°Ï죬´ÓÖÐÑ¡³ö×îÓÅ»¯µÄ¼Æ»®²ÎÊý¡£²¢Í¨¹ýROSµÄÎïÀíÒýÇæ¶Ô»úеÈ˵ÄÔ˶¯¼Æ»®Ð§¹û½øÐÐÁË·ÂÕ棬½á¹û±êÃ÷±¾ÎÄËùÌá³öµÄËã·¨¿ÉÐÐÐԺܸߡ£
¡¾±¾ÎıêÇ©¡¿
¡¾ÔðÈα༡¿yd2333Ôƶ¥µç×ÓÓÎÏ·ÔƲÖ