轮式里程计标定及二维激光数据去畸变

00.引言

二维激光数据结构查看rosmsg show sensor_msgs/LaserScan：

std_msgs/Header header	//　数据的消息头
  uint32 seq			//　数据的序号
  time stamp			//　数据的时间戳
  string frame_id		//　数据的坐标系
float32 angle_min		//　雷达数据的起始角度（最小角度）
float32 angle_max		//　雷达数据的终止角度（最大角度）
float32 angle_increment	//　雷达数据的角度分辨率（角度增量）
float32 time_increment	//　雷达数据每个数据点的时间间隔
float32 scan_time		//　当前帧数据与下一帧数据的时间间隔
float32 range_min		//　雷达数据的最小值
float32 range_max		//　雷达数据的最大值
float32[] ranges		//　雷达数据每个点对应的在极坐标系下的距离值
float32[] intensities	//　雷达数据每个点对应的强度值

实际数据距离：

header: 
  seq: 3568
  stamp: 
    secs: 1510031272
    nsecs: 522861940
  frame_id: "base_laser"
angle_min: -1.5708758831
angle_max: 1.5762219429
angle_increment: 0.00581718236208
time_increment: 6.17219993728e-05
scan_time: 0.0666666701436
range_min: 0.0500000007451
range_max: 25.0
ranges: [6.2769999504089355, 6.285999774932861, 6.296000003814697, 6.303999900817871, 6.315000057220459, 6.322999954223633, 6.313000202178955, 6.186999797821045, 6.116000175476074, 6.123000144958496, 6.13100004196167, 6.145999908447266, 6.1579999923706055, 6.163000106811523, 6.183000087738037, 6.193999767303467, 6.203000068664551, 6.2170000076293945, 6.234000205993652, 6.256999969482422, 6.306000232696533, 6.285999774932861, 6.290999889373779, 6.303999900817871, 6.318999767303467, 6.329999923706055, 6.3460001945495605, 6.359000205993652, 6.38100004196167, 6.394999980926514, 6.409999847412109, 6.428999900817871, 6.445000171661377, 6.460000038146973, 6.486000061035156, 6.494999885559082, 6.514999866485596, 6.5370001792907715, 6.546999931335449, 6.572999954223633, 6.590000152587891, 6.607999801635742, 6.636000156402588, 6.6529998779296875, 6.660999774932861, 10.97700023651123, 10.920000076293945, 11.164999961853027, 11.23799991607666, 0.0, 0.0, 0.0, 0.0, 14.932000160217285, 0.0, 0.0, 0.0, 13.340999603271484, 13.33899974822998, 13.338000297546387, 13.317999839782715, 13.29800033569336, 13.288000106811523, 13.262999534606934, 13.309000015258789, 0.0, 0.0, 0.0, 0.0, 0.0, 20.569000244140625, 20.45400047302246, 7.980000019073486, 7.979000091552734, 7.979000091552734, 7.986999988555908, 7.973999977111816, 7.9770002365112305, 7.9730000495910645, 7.9670000076293945, 7.960999965667725, 7.953000068664551, 7.936999797821045, 7.935999870300293, 0.0, 8.86400032043457, 0.0, 0.0, 14.166000366210938, 0.0, 0.0, 22.135000228881836, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 14.161999702453613, 2.2079999446868896, 2.2039999961853027, 2.193000078201294, 2.178999900817871, 2.1659998893737793, 2.1589999198913574, 2.1489999294281006, 2.13700008392334, 2.127000093460083, 2.1110000610351562, 2.1050000190734863, 2.0929999351501465, 2.0840001106262207, 2.072000026702881, 2.066999912261963, 2.0510001182556152, 2.055000066757202, 2.0409998893737793, 2.0299999713897705, 2.0209999084472656, 2.0250000953674316, 2.0280001163482666, 2.0390000343322754, 2.0199999809265137, 1.9900000095367432, 1.9759999513626099, 1.9630000591278076, 1.9609999656677246, 1.9570000171661377, 1.9509999752044678, 1.9420000314712524, 1.9329999685287476, 1.9299999475479126, 1.9229999780654907, 1.909000039100647, 1.9040000438690186, 1.8940000534057617, 1.8860000371932983, 1.8819999694824219, 1.8769999742507935, 1.8769999742507935, 1.8669999837875366, 1.8650000095367432, 1.8639999628067017, 1.8589999675750732, 1.8519999980926514, 1.840999960899353, 1.8569999933242798, 1.8849999904632568, 1.909000039100647, 1.9329999685287476, 1.9550000429153442, 1.9839999675750732, 2.010999917984009, 2.0329999923706055, 2.062999963760376, 2.0940001010894775, 2.121000051498413, 2.1510000228881836, 2.181999921798706, 2.2100000381469727, 2.246999979019165, 2.2799999713897705, 2.318000078201294, 2.3550000190734863, 2.4010000228881836, 2.424999952316284, 2.4630000591278076, 2.510999917984009, 2.5510001182556152, 2.5959999561309814, 2.6429998874664307, 2.687000036239624, 2.74399995803833, 2.7960000038146973, 2.8469998836517334, 2.9040000438690186, 2.996000051498413, 3.0910000801086426, 3.115999937057495, 3.130000114440918, 3.1429998874664307, 3.1659998893737793, 3.193000078201294, 9.24899959564209, 9.413999557495117, 7.545000076293945, 7.498000144958496, 7.423999786376953, 7.355000019073486, 7.2829999923706055, 7.2230000495910645, 7.1570000648498535, 7.10099983215332, 7.039999961853027, 6.9770002365112305, 6.928999900817871, 6.877999782562256, 6.883999824523926, 6.909999847412109, 6.936999797821045, 6.9679999351501465, 6.995999813079834, 7.03000020980835, 7.059999942779541, 7.0929999351501465, 7.127999782562256, 7.158999919891357, 7.198999881744385, 7.228000164031982, 7.263999938964844, 7.296000003814697, 7.336999893188477, 7.36899995803833, 7.400000095367432, 7.440999984741211, 7.480000019073486, 7.506999969482422, 11.678000450134277, 11.60200023651123, 11.538000106811523, 11.454999923706055, 11.381999969482422, 11.324999809265137, 11.255999565124512, 11.199000358581543, 11.234999656677246, 11.3149995803833, 11.388999938964844, 11.451000213623047, 11.508000373840332, 11.538000106811523, 13.14799976348877, 13.088000297546387, 13.005999565124512, 12.940999984741211, 12.871999740600586, 12.803999900817871, 12.746000289916992, 12.684000015258789, 12.619000434875488, 12.557000160217285, 12.512999534606934, 12.564000129699707, 12.654999732971191, 12.758000373840332, 12.829999923706055, 12.850000381469727, 14.444000244140625, 14.385000228881836, 14.32699966430664, 14.258000373840332, 14.199999809265137, 14.145999908447266, 14.09000015258789, 14.027000427246094, 13.968999862670898, 13.993000030517578, 14.125, 14.22700023651123, 14.29699993133545, 15.925000190734863, 15.859000205993652, 15.805999755859375, 15.73799991607666, 15.694999694824219, 15.666999816894531, 15.567000389099121, 15.541000366210938, 0.0, 0.0, 0.0, 12.972000122070312, 12.963000297546387, 12.939000129699707, 0.0, 12.852999687194824, 6.728000164031982, 6.696000099182129, 6.677000045776367, 6.659999847412109, 6.63700008392334, 6.614999771118164, 6.598999977111816, 6.578999996185303, 6.557000160217285, 6.541999816894531, 6.51800012588501, 6.506999969482422, 6.497000217437744, 6.520999908447266, 6.478000164031982, 6.433000087738037, 6.420000076293945, 6.408999919891357, 6.392000198364258, 6.376999855041504, 6.361000061035156, 6.3460001945495605, 6.336999893188477, 6.35099983215332, 6.451000213623047, 6.572000026702881, 6.681000232696533, 6.776000022888184, 6.849999904632568, 6.992000102996826, 7.046000003814697, 7.039000034332275, 7.0269999504089355, 7.0279998779296875, 7.014999866485596, 7.004000186920166, 6.998000144958496, 6.989999771118164, 6.978000164031982, 6.9670000076293945, 6.956999778747559, 3.056999921798706, 3.0450000762939453, 3.0429999828338623, 3.0450000762939453, 6.875999927520752, 6.9070000648498535, 6.8979997634887695, 6.888999938964844, 6.880000114440918, 6.874000072479248, 6.860000133514404, 6.861999988555908, 6.849999904632568, 6.8460001945495605, 6.8420000076293945, 5.090000152587891, 2.671999931335449, 2.5950000286102295, 2.4149999618530273, 2.377000093460083, 2.3910000324249268, 2.38700008392334, 2.3970000743865967, 2.4000000953674316, 2.4030001163482666, 2.4040000438690186, 2.4030001163482666, 2.4040000438690186, 2.38700008392334, 2.381999969482422, 2.3940000534057617, 2.3959999084472656, 2.3989999294281006, 2.4019999504089355, 2.3970000743865967, 2.3929998874664307, 2.4010000228881836, 2.3989999294281006, 2.3970000743865967, 2.3959999084472656, 2.3980000019073486, 2.4030001163482666, 2.4149999618530273, 2.4019999504089355, 2.4070000648498535, 2.4059998989105225, 2.4089999198913574, 2.4089999198913574, 2.4110000133514404, 2.4140000343322754, 2.4179999828338623, 2.4179999828338623, 2.4200000762939453, 2.4200000762939453, 2.424999952316284, 2.427999973297119, 2.430999994277954, 2.430999994277954, 2.437999963760376, 2.440999984741211, 2.427000045776367, 2.378999948501587, 2.3299999237060547, 2.2709999084472656, 2.2039999961853027, 2.1410000324249268, 2.0759999752044678, 2.0139999389648438, 1.940999984741211, 1.8819999694824219, 1.8669999837875366, 1.8700000047683716, 1.8739999532699585, 1.878000020980835, 1.9869999885559082, 2.4660000801086426, 2.505000114440918, 2.506999969482422, 2.510999917984009, 2.5139999389648438, 2.5220000743865967, 2.5250000953674316, 2.5320000648498535, 2.5390000343322754, 2.5409998893737793, 2.5399999618530273, 2.549999952316284, 2.555000066757202, 2.563999891281128, 2.568000078201294, 2.572999954223633, 2.5820000171661377, 2.5850000381469727, 2.5910000801086426, 2.5969998836517334, 2.6070001125335693, 2.6080000400543213, 2.619999885559082, 2.627000093460083, 2.630000114440918, 2.640000104904175, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.8279999494552612, 1.8329999446868896, 1.8229999542236328, 1.8220000267028809, 1.843999981880188, 5.202000141143799, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.2829999923706055, 2.2890000343322754, 2.2860000133514404, 2.2960000038146973, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.615000009536743, 2.61899995803833, 2.618000030517578, 2.614000082015991, 2.614000082015991, 2.6089999675750732, 2.6040000915527344, 2.5980000495910645, 2.5910000801086426, 2.565999984741211, 2.5420000553131104, 2.5360000133514404, 2.5329999923706055, 2.5329999923706055, 2.5230000019073486, 1.7480000257492065, 1.7269999980926514, 1.7230000495910645, 1.718000054359436, 1.7350000143051147, 2.063999891281128, 2.2049999237060547, 2.1989998817443848, 2.203000068664551, 2.3540000915527344, 2.506999969482422, 2.51200008392334, 2.500999927520752, 2.496000051498413, 2.489000082015991, 2.492000102996826, 2.496000051498413, 2.499000072479248, 2.493000030517578, 2.49399995803833, 2.499000072479248, 2.4860000610351562, 2.489000082015991, 2.4860000610351562, 2.496000051498413, 2.486999988555908, 2.4830000400543213, 2.489000082015991, 2.4860000610351562, 2.4809999465942383, 2.4820001125335693, 2.4830000400543213, 2.484999895095825, 2.4830000400543213, 2.486999988555908, 2.490000009536743, 2.484999895095825, 2.5, 2.5339999198913574, 2.546999931335449, 2.5429999828338623, 2.5409998893737793, 2.5399999618530273, 2.5420000553131104, 2.5420000553131104, 2.5450000762939453, 2.565999984741211, 2.617000102996826, 2.627000093460083, 2.628000020980835, 2.63100004196167, 2.631999969482422, 2.63700008392334, 2.635999917984009, 2.640000104904175, 2.6389999389648438, 2.424999952316284, 2.1110000610351562, 2.1019999980926514, 2.111999988555908, 2.1989998817443848, 2.617000102996826, 2.6589999198913574, 2.6589999198913574, 2.6659998893737793, 2.6700000762939453, 4.570000171661377, 1.6360000371932983, 1.6399999856948853, 1.6410000324249268, 1.6449999809265137]
intensities: [113.0, 116.0, 110.0, 110.0, 113.0, 116.0, 93.0, 79.0, 96.0, 106.0, 103.0, 100.0, 106.0, 103.0, 100.0, 103.0, 103.0, 96.0, 103.0, 100.0, 86.0, 96.0, 100.0, 100.0, 93.0, 100.0, 100.0, 100.0, 103.0, 100.0, 100.0, 103.0, 100.0, 100.0, 103.0, 96.0, 96.0, 96.0, 100.0, 100.0, 100.0, 93.0, 96.0, 100.0, 66.0, 39.0, 42.0, 32.0, 22.0, 0.0, 0.0, 0.0, 0.0, 12.0, 0.0, 0.0, 0.0, 19.0, 49.0, 53.0, 49.0, 53.0, 56.0, 59.0, 22.0, 0.0, 0.0, 0.0, 0.0, 0.0, 5.0, 5.0, 63.0, 83.0, 90.0, 86.0, 86.0, 90.0, 96.0, 100.0, 100.0, 100.0, 113.0, 106.0, 0.0, 32.0, 0.0, 0.0, 63.0, 0.0, 0.0, 3.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 59.0, 153.0, 160.0, 160.0, 160.0, 160.0, 167.0, 163.0, 160.0, 170.0, 167.0, 167.0, 167.0, 167.0, 163.0, 167.0, 167.0, 174.0, 170.0, 170.0, 170.0, 167.0, 143.0, 133.0, 130.0, 157.0, 167.0, 170.0, 170.0, 177.0, 177.0, 174.0, 174.0, 174.0, 174.0, 170.0, 170.0, 170.0, 170.0, 174.0, 170.0, 177.0, 177.0, 174.0, 180.0, 180.0, 180.0, 167.0, 147.0, 147.0, 150.0, 140.0, 143.0, 143.0, 137.0, 140.0, 143.0, 133.0, 137.0, 133.0, 133.0, 133.0, 130.0, 120.0, 123.0, 123.0, 133.0, 137.0, 116.0, 116.0, 110.0, 110.0, 110.0, 113.0, 106.0, 106.0, 106.0, 100.0, 96.0, 106.0, 110.0, 113.0, 110.0, 103.0, 56.0, 46.0, 59.0, 53.0, 79.0, 73.0, 79.0, 83.0, 86.0, 83.0, 86.0, 79.0, 86.0, 79.0, 90.0, 93.0, 96.0, 93.0, 93.0, 96.0, 96.0, 90.0, 93.0, 90.0, 86.0, 93.0, 93.0, 86.0, 86.0, 90.0, 86.0, 83.0, 90.0, 83.0, 42.0, 56.0, 59.0, 63.0, 63.0, 59.0, 63.0, 63.0, 66.0, 69.0, 63.0, 63.0, 63.0, 69.0, 63.0, 63.0, 56.0, 59.0, 63.0, 59.0, 63.0, 66.0, 63.0, 59.0, 66.0, 63.0, 56.0, 46.0, 56.0, 59.0, 49.0, 53.0, 53.0, 56.0, 63.0, 63.0, 59.0, 59.0, 63.0, 63.0, 53.0, 46.0, 42.0, 49.0, 46.0, 46.0, 46.0, 46.0, 46.0, 3.0, 16.0, 12.0, 0.0, 0.0, 0.0, 19.0, 42.0, 49.0, 0.0, 26.0, 63.0, 93.0, 100.0, 103.0, 100.0, 100.0, 103.0, 103.0, 100.0, 103.0, 103.0, 100.0, 100.0, 90.0, 90.0, 100.0, 103.0, 103.0, 103.0, 103.0, 106.0, 110.0, 100.0, 86.0, 66.0, 59.0, 56.0, 66.0, 63.0, 73.0, 79.0, 76.0, 76.0, 83.0, 79.0, 79.0, 86.0, 79.0, 76.0, 86.0, 83.0, 106.0, 120.0, 140.0, 123.0, 76.0, 90.0, 86.0, 90.0, 83.0, 86.0, 90.0, 90.0, 86.0, 90.0, 90.0, 90.0, 103.0, 143.0, 184.0, 190.0, 211.0, 217.0, 230.0, 230.0, 230.0, 230.0, 230.0, 230.0, 217.0, 207.0, 211.0, 204.0, 194.0, 187.0, 184.0, 177.0, 174.0, 170.0, 163.0, 167.0, 167.0, 170.0, 174.0, 167.0, 167.0, 167.0, 160.0, 167.0, 167.0, 160.0, 160.0, 167.0, 167.0, 167.0, 163.0, 163.0, 167.0, 163.0, 184.0, 194.0, 150.0, 110.0, 113.0, 106.0, 106.0, 110.0, 106.0, 113.0, 116.0, 153.0, 170.0, 174.0, 174.0, 174.0, 137.0, 150.0, 160.0, 160.0, 160.0, 157.0, 157.0, 153.0, 157.0, 157.0, 160.0, 153.0, 157.0, 157.0, 157.0, 157.0, 157.0, 153.0, 160.0, 153.0, 153.0, 157.0, 150.0, 153.0, 153.0, 147.0, 130.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 147.0, 180.0, 177.0, 167.0, 140.0, 19.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 130.0, 160.0, 160.0, 130.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 63.0, 140.0, 160.0, 160.0, 160.0, 157.0, 157.0, 157.0, 160.0, 153.0, 150.0, 160.0, 153.0, 160.0, 140.0, 133.0, 184.0, 187.0, 184.0, 174.0, 133.0, 180.0, 184.0, 177.0, 160.0, 170.0, 177.0, 174.0, 170.0, 167.0, 170.0, 174.0, 180.0, 177.0, 180.0, 184.0, 177.0, 180.0, 177.0, 184.0, 177.0, 177.0, 180.0, 180.0, 177.0, 180.0, 180.0, 177.0, 180.0, 180.0, 180.0, 177.0, 174.0, 174.0, 174.0, 174.0, 170.0, 170.0, 167.0, 163.0, 167.0, 163.0, 163.0, 163.0, 163.0, 160.0, 163.0, 163.0, 160.0, 160.0, 160.0, 110.0, 174.0, 174.0, 177.0, 153.0, 153.0, 157.0, 153.0, 160.0, 153.0, 59.0, 180.0, 190.0, 190.0, 194.0]
---

rviz显示原始数据：

01.里程计标定

这里的里程计指的是轮式里程计。轮式里程计子啊进行航迹推算时需要使用到轮间距等机器人固有属性参数，虽然出厂会给出参数，但也不是十分标准，类似相机一样需要标定一下。

标定时，使用直接线性方程法进行标定，其实最后标定的参数也不是机器人固有参数，而是得出轮式里程计位姿与"真实"位姿之间（一般通过激光匹配获取）的一个变换矩阵。

• 参考博客

• 注意两帧之间的相对位姿求解，假设机器人从第$i$时刻(轮式里程计path为$T_i$)运动至第 $j$时刻(轮式里程计path为$T_j$)，则相对位姿为：$T_i^{-1}.T_j$

• 航迹推算中的path更新为：$$\left[\begin{array}{l}x\\ y\\ \theta \end{array}\right]=\left[\begin{array}{l}x\\ y\\ \theta \end{array}\right]+\left[\begin{array}{ccc}\cos \theta & -\sin \theta & 0\\ \sin \theta & \cos \theta & 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{l}dx\\ dy\\ d\theta \end{array}\right]$$

最后一维求解出来感觉有问题：

3.95997e-316 6.95292e-310 6.95292e-310
6.95292e-310 6.95292e-310 3.20386e-317
           0 6.95292e-310         -nan

有知道原因的小伙伴帮忙指出一下。

通过激光配准的相对位姿来求解映射矩阵，然后将校正后的数据进行发布。

02.二维激光雷达畸变校正

待更。