רÀûÃû³Æ£º»ùÓÚÎÀÐǵ¼º½ºÍKalmanÂ˲¨µÄ¸ß¾«¶ÈÊÚʱϵͳ¼°·½·¨
¼¼ÊõÁìÓò£º
±¾·¢Ã÷Éæ¼°ÎÀÐǵ¼º½ÁìÓòÓëͨÐÅÁìÓò£¬¾ßÌåΪһÖÖ»ùÓÚÎÀÐǵ¼º½ºÍKalmanÂ˲¨µÄ¸ß¾«¶ÈÊÚʱϵͳ¼°·½·¨£¬ÀûÓÃKalmanÂ˲¨¼¼ÊõºÍÎÀÐǵ¼º½¶ÔÃëÐźŷ¢ÉúÆ÷µÄÏàλÎó²îÒÔ¼°ÆµÂÊÎó²î½øÐÐУÕý£¬ÊµÏָ߾«¶ÈµÄÊÚʱ¹¦ÄÜ¡£
±³¾°¼¼Êõ£º
½üÄêÀ´£¬Ëæ×ÅÎÀÐǵ¼º½ÏµÍ³µÄ·¢Õ¹£¬ÎÀÐǵ¼º½ÏµÍ³ÔÚÈÕ³£¹¤×÷ÓëÉú»îÖÐÕ¼ÓÐÁËÔ½À´Ô½ÖØÒªµÄµØÎ»¡£ÎÀÐǵ¼º½ÏµÍ³ÔÚʵÏÖ¶¨Î»ÒÔ¼°¶¨ËÙ¹¦ÄܵÄͬʱ£¬¿ÉÒÔ½«±¾µØ½ÓÊÕ»úµÄʱÖÓÎó²îÐÅÏ¢Ò»²¢½âËã³öÀ´£¬ÀûÓñ¾µØÎó²îÐÅÏ¢¿ÉÒÔ¶Ô½ÓÊÕ»úµÄÃëÐźŲúÉúµç·µÄÏàλÐÅÏ¢½øÐÐÐÞÕý£¬ÊµÏÖÊÚʱµÄ¹¦ÄÜ¡£µ«ÊÇ´«Í³µÄÊÚʱϵͳÊܵ½¶¨Î»¾«¶ÈµÄÓ°Ï죬´æÔڽϴóµÄ¶¶¶¯¡£Í¬Ê±´«Í³µÄÊÚʱ·½·¨²»¿¼ÂǾ§ÕñÎó²î¶ÔÓÚÃëÐźÅÔì³ÉµÄÓ°Ï죬½üÒ»²½½µµÍÁËÊÚʱϵͳµÄ¾«¶È¡£ÀýÈçÔÚл¸ÕµÄ¡¶GPSÔÀíÓë½ÓÊÕ»úÉè¼Æ¡·Ò»ÊéÖÐÌáµ½µÄÊÚʱ·½·¨¾Í²¢Î´Éæ¼°¶ÔÓÚ¾§ÕñÎó²îµÄ¹À¼Æ¡£
·¢Ã÷ÄÚÈÝ
Òª½â¾öµÄ¼¼ÊõÎÊÌâÕë¶ÔÖ±½ÓÀûÓÃÖÓ²îÐÅÏ¢¶ÔÃëÐźŽøÐÐУÕýÎó²î½Ï´óµÄȱµã£¬ÒÔ¼°¾§ÕñÎó²î´øÀ´µÄʱÖÓÎó²î£¬±¾·¢Ã÷Ìá³öÁËÒ»ÖÖ»ùÓÚÎÀÐǵ¼º½ºÍKalmanÂ˲¨µÄ¸ß¾«¶ÈÊÚʱϵͳ¼°·½·¨£¬¶ÔÖÓ²î½øÐÐKalmanÂ˲¨£¬½µµÍʱÖӵͶ¶¯£¬Í¬Ê±ÀûÓÃÂ˲¨µÃµ½ÃëÐÅºÅÆµÂÊÎó²î¶ÔÃëÐźŷ¢ÉúÆ÷µÄƵÂÊ¿ØÖÆ×Ö½øÐе÷Õû£¬Ìá¸ßÊÚʱÐźŵľ«¶È¡£¼¼Êõ·½°¸±¾·¢Ã÷µÄ¼¼Êõ·½°¸Îª:ËùÊö»ùÓÚÎÀÐǵ¼º½ºÍKalmanÂ˲¨µÄ¸ß¾«¶ÈÊÚʱϵͳ£¬°üÀ¨ÎÀÐǽÓÊÜÄ£¿é¡¢¶¨Î»½âËãÄ£¿é¡¢KalmanÂ˲¨Æ÷ºÍÃëÐźŷ¢ÉúÆ÷£»ËùÊöÎÀÐǽÓÊÜÄ£¿é½ÓÊÜÎÀÐÇλÖÃÐÅÏ¢Óëα¾àÐÅÏ¢£¬²¢´«µÝ¸ø¶¨Î»½âËãÄ£¿é£¬¶¨Î»½âËãÄ£¿é¸ù¾ÝÎÀÐÇλÖÃÐÅÏ¢Óëα¾àÐÅÏ¢µÃµ½ÎÀÐǽÓÊÜÄ£¿éµÄÖÓ²îÐÅÏ¢£¬²¢½«ÎÀÐǽÓÊÜÄ£¿éµÄÖÓ²îÐÅÏ¢´«µÝ¸øKalmanÂ˲¨Æ÷£¬KalmanÂ˲¨Æ÷¶ÔÎÀÐǽÓÊÜÄ£¿éµÄÖÓ²îÐÅÏ¢½øÐÐKalmanÂ˲¨£¬¸ù¾ÝµÃµ½ÖÓ²îÐÅÏ¢ÒÔ¼°Æµ²îÐÅÏ¢µÃµ½ÃëÐźŷ¢ÉúÆ÷µÄÏàλ¿ØÖÆ×ÖÒÔ¼°ÆµÂÊ¿ØÖÆ×Ö£¬KalmanÂ˲¨Æ÷½«µÃµ½µÄÏàλ¿ØÖÆ×ÖÒÔ¼°ÆµÂÊ¿ØÖÆ×Ö´«µÝ¸øÃëÐźŷ¢ÉúÆ÷£¬ÊµÏָ߾«¶ÈÊÚʱ¡£±¾·¢Ã÷Öеķ½·¨°üÀ¨¶¨Î»½âËã¡¢KalmanÂ˲¨¡¢Ê±ÖÓУÕýÈý¸ö²¿·Ö¡£¶¨Î»½âËã²ÉÓô«Í³µÄ×îС¶þ³Ë·¨¶Ô¶¨Î»½á¹û½øÐнâË㣬ͬʱµÃµ½ÎÀÐǽÓÊÕ»úÖÓ²îÐÅÏ¢£¬Í¨¹ýKalmanÂ˲¨Æ÷¶Ô¶¨Î»½âËãµÃµ½µÄÖÓ²îÐÅÏ¢½øÐÐÂ˲¨¡£ÀûÓÃÂ˲¨µÃµ½µÄ½á¹û¶ÔÃëÐźŷ¢ÉúÆ÷½øÐÐУÕý£¬µÃµ½¾«È·µÄʱÖÓÏ¢¡£ËùÊö»ùÓÚÎÀÐǵ¼º½ºÍKalmanÂ˲¨µÄ¸ß¾«¶ÈÊÚʱ·½·¨£¬²ÉÓÃÒÔϲ½Öè:²½Öè1:ÎÀÐǽÓÊÜÄ£¿é½ÓÊÜÎÀÐÇλÖÃÐÅÏ¢Óëα¾àÐÅÏ¢£¬¸ù¾ÝÎÀÐÇλÖÃÐÅÏ¢Óëα¾àÐÅÏ¢£¬²ÉÓÃ×îС¶þ³Ë·¨½âËãÎÀÐǽÓÊÜÄ£¿éµÄÖÓ²îÐÅÏ¢S tg £»
²½Öè2:¶ÔÎÀÐǽÓÊÜÄ£¿éµÄÖÓ²îÐÅÏ¢½øÐÐKalmanÂ˲¨£¬¶ÔÓÚµÚk´ÎÂ˲¨¾ßÌå²½ÖèÈçÏÂ:²½Öè2.1:KalmanÂ˲¨µÄʱ¼ä¸üйý³Ì:
ȨÀûÒªÇó
1.Ò»ÖÖ»ùÓÚÎÀÐǵ¼º½ºÍKalmanÂ˲¨µÄ¸ß¾«¶ÈÊÚʱϵͳ£¬°üÀ¨ÎÀÐǽÓÊÜÄ£¿é¡¢¶¨Î»½âËãÄ£¿é¡¢KalmanÂ˲¨Æ÷ºÍÃëÐźŷ¢ÉúÆ÷£»ËùÊöÎÀÐǽÓÊÜÄ£¿é½ÓÊÜÎÀÐÇλÖÃÐÅÏ¢Óëα¾àÐÅÏ¢£¬²¢´«µÝ¸ø¶¨Î»½âËãÄ£¿é£¬¶¨Î»½âËãÄ£¿é¸ù¾ÝÎÀÐÇλÖÃÐÅÏ¢Óëα¾àÐÅÏ¢µÃµ½ÎÀÐǽÓÊÜÄ£¿éµÄÖÓ²îÐÅÏ¢£¬²¢½«ÎÀÐǽÓÊÜÄ£¿éµÄÖÓ²îÐÅÏ¢´«µÝ¸øKalmanÂ˲¨Æ÷£¬KalmanÂ˲¨Æ÷¶ÔÎÀÐǽÓÊÜÄ£¿éµÄÖÓ²îÐÅÏ¢½øÐÐKalmanÂ˲¨£¬¸ù¾ÝµÃµ½ÖÓ²îÐÅÏ¢ÒÔ¼°Æµ²îÐÅÏ¢µÃµ½ÃëÐźŷ¢ÉúÆ÷µÄÏàλ¿ØÖÆ×ÖÒÔ¼°ÆµÂÊ¿ØÖÆ×Ö£¬KalmanÂ˲¨Æ÷½«µÃµ½µÄÏàλ¿ØÖÆ×ÖÒÔ¼°ÆµÂÊ¿ØÖÆ×Ö´«µÝ¸øÃëÐźŷ¢ÉúÆ÷£¬ÊµÏָ߾«¶ÈÊÚʱ¡£
2.Ò»ÖÖ»ùÓÚÎÀÐǵ¼º½ºÍKalmanÂ˲¨µÄ¸ß¾«¶ÈÊÚʱ·½·¨£¬²ÉÓÃÒÔϲ½Öè: ²½Öè1:ÎÀÐǽÓÊÜÄ£¿é½ÓÊÜÎÀÐÇλÖÃÐÅÏ¢Óëα¾àÐÅÏ¢£¬¸ù¾ÝÎÀÐÇλÖÃÐÅÏ¢Óëα¾àÐÅÏ¢£¬²ÉÓÃ×îС¶þ³Ë·¨½âËãÎÀÐǽÓÊÜÄ£¿éµÄÖÓ²îÐÅÏ¢¦Ä tg £» ²½Öè2:¶ÔÎÀÐǽÓÊÜÄ£¿éµÄÖÓ²îÐÅÏ¢½øÐÐKalmanÂ˲¨£¬¶ÔÓÚµÚk´ÎÂ˲¨¾ßÌå²½ÖèÈçÏÂ: ²½Öè2.1:KalmanÂ˲¨µÄʱ¼ä¸üйý³Ì: ¶Ô״̬ÏòÁ¿Xlri = [ ¦Ä V1 ¦Ä fk_J½øÐйÀ¼Æ£¬µÃµ½×´Ì¬¹À¼ÆÖµ&£¬ÆäÖÐ¦Ä V1ΪÃëÐźŷ¢ÉúÆ÷µÄÖÓ²îÐÅÏ¢¡¢SfV1ΪÃëÐźŷ¢ÉúÆ÷Ƶ²îÐÅÏ¢£¬¹À¼ÆÊ½Îª& ¡ªJ= i jµ±~ ^ ^k-1£¬ l_u I¡¹£¬k=lʱ£¬XlriȡֵΪ[O O];¸ù¾ÝʽžéµÃµ½Ð·½²îÕóPlriµÄÔ¤²â¾ØÕóK£¬ÆäÖÐQΪ¹ý³ÌÔëÉùз½²îÕó£» ²½Öè2.2 =KalmanÂ˲¨µÄÁ¿²â¸üйý³Ì: ¸ù¾ÝʽKk = >('Ø+ R) 1µÃµ½ÔöÒæ¾ØÕóKk£¬ÆäÖÐC = [I O]£¬RΪÁ¿²âÔëÉùз½²îÕ󣻸ù¾Ýʽ1=1,+4.( -C.Ö®)µÃµ½Â˲¨ºóµÄ״̬ÏòÁ¿¾ØÕóXk£¬¸ù¾ÝʽµÃµ½¸üкóµÄз½²î¾ØÕóPk£¬ÆäÖÐIΪ¶þάµ¥Î»¾ØÕó£» ²½Öè3:ÀûÓÃÿһ´ÎÂ˲¨µÃµ½µÄÃëÐźŷ¢ÉúÆ÷µÄÖÓ²îÐÅÏ¢³ËÒÔ¾§ÕñµÄƵÂÊ&µÃµ½ÃëÐźŷ¢ÉúÆ÷µÄÏàλ¿ØÖÆ×ÖNpµÄµ÷ÕûÁ¿ANp£¬ÓÃNp¼õÈ¥µ÷ÕûÁ¿ANpµÃµ½¾«È·µÄÃëÐźŷ¢ÉúÆ÷µÄÏàλ¿ØÖÆ×Ö£»ÀûÓÃÿһ´ÎÂ˲¨µÃµ½µÄÃëÐźŷ¢ÉúÆ÷µÄƵ²îÐÅÏ¢³ËÒÔ¾§ÕñµÄƵÂÊ4µÃµ½ÃëÐźŷ¢ÉúÆ÷µÄƵÂÊ¿ØÖÆ×ÖNfµÄµ÷ÕûÁ¿¦« Nf£¬ÓÃNf¼õÈ¥µ÷ÕûÁ¿¦« NfµÃµ½¾«È·µÄÃëÐźŷ¢ÉúÆ÷µÄƵÂÊ¿ØÖÆ×Ö£»¸ù¾ÝµÃµ½µÄ ¾«È·µÄÃëÐźŷ¢ÉúÆ÷µÄÏàλ¿ØÖÆ×ÖºÍÆµÂÊ¿ØÖÆ×Ö£¬ÊµÏָ߾«¶ÈÊÚʱ¡£
È«ÎÄÕªÒª
±¾·¢Ã÷Éè¼ÆÁËÒ»ÖÖ»ùÓÚÎÀÐǵ¼º½ºÍKalmanÂ˲¨µÄ¸ß¾«¶ÈÊÚʱϵͳ¼°·½·¨£¬¸Ãϵͳ°üº¬Êý¾Ý½âËãÓëÊÚʱÂ˲¨Á½²¿·Ö¡£Êý¾Ý½âË㲿·Ö²ÉÓô«Í³µÄ×îС¶þ³Ë·¨½âËã³ö±±¶·¶þ´ú½ÓÊÕ»úµÄλÖÃÐÅÏ¢ÒÔ¼°ÖÓ²îÐÅÏ¢£¬Ö®ºóÀûÓÃKalmanÂ˲¨Æ÷¶ÔÖÓ²îÐÅÏ¢½øÐÐÂ˲¨µÃµ½ÃëÐźŷ¢ÉúÆ÷µÄÏàλÎó²îÐÅÏ¢ÒÔ¼°ÆµÂÊÎó²îÐÅÏ¢£¬×îºóÀûÓÃÂ˲¨µÃµ½ÃëÐźŷ¢ÉúÆ÷µÄÏàλÎó²îÐÅÏ¢ÒÔ¼°ÆµÂÊÎó²îÐÅÏ¢¶ÔÃëÐźŷ¢ÉúÆ÷µÄ³õʼÏàλÒÔ¼°ÆµÂʼÆÊý×Ö½øÐе÷Õû£¬ÊµÏָ߾«¶ÈÊÚʱµÄ¹¦ÄÜ¡£±¾·¢Ã÷½â¾öÁËÏÖÓм¼ÊõÌõ¼þÏÂÊÚʱ¾«¶È²»¸ß£¬ÒÔ¼°ÃëÐźŷ¢ÉúÆ÷Ò×Êܾ§ÕñƵÂÊÎó²îÓ°ÏìµÄÎÊÌâ¡£
Îĵµ±àºÅG01S19/27GK103176190SQ201310069980
¹«¿ªÈÕ2013Äê6ÔÂ26ÈÕ ÉêÇëÈÕÆÚ2013Äê3ÔÂ6ÈÕ ÓÅÏÈȨÈÕ2013Äê3ÔÂ6ÈÕ
·¢Ã÷ÕߺÎΰ, Á®±£Íú, ÌÆ³É¿, À׺Æ, Á®Î°ÁÖ, ÑîÇí ÉêÇëÈË:Î÷±±¹¤Òµ´óѧ